ros-hackathon/go_three_second.cpp at master · HackInstitute/ros-hackathon · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
#include<ros/ros.h>
#include<geometry_msgs/Twist.h>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "go_three_second");
  ros::NodeHandle nh;

  ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("RosAria/cmd_vel", 1000);
  geometry_msgs::Twist msg;

  double BASE_SPEED = 0.2, MOVE_TIME = 2, CLOCK_SPEED = 0.5;
  int count = 0;
  ros::Rate rate(CLOCK_SPEED);

  // Make the robot stop (robot perhaps has a speed already)
  msg.linear.x = 0;
  msg.linear.y = 0;
  msg.linear.z = 0;

  pub.publish(msg);

  while(ros::ok() && count<MOVE_TIME/CLOCK_SPEED + 1)
    {
      if (count == 0 || count == 1)
	{
	    msg.linear.x = BASE_SPEED;
	    pub.publish(msg);
	}
      ROS_INFO_STREAM("The robot is now moving forward!");
      count++;
      ros::spinOnce();
      rate.sleep();
   }

  // make the robot stop
  for (int i = 0; i < 2; i++)
    {

      msg.linear.x = 0;
      msg.linear.y = 0;
      msg.linear.z = 0;

      pub.publish(msg);

    }
    ROS_INFO_STREAM("The robot finished moving forward three seconds!");

    // Guard, make sure the robot stops.
    rate.sleep();
    msg.linear.x = 0;
    msg.linear.y = 0;
    msg.linear.z = 0;

    pub.publish(msg);

}