turtle_ROS/solution.cpp at master · tarang98/turtle_ROS · 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
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

const double PI = 3.14159265359;


// Initialize the node


int main(int argc, char **argv)
{
    ros::init(argc, argv, "move_turtle");


  ros::Rate rate(3);
//void move(double distance, int turn);

  move(1.0, 1);
  move(1.0, 3);
  move(2.0, 3);
  move(6.0, 1);
  move(1.0, 2);
  move(1.0, 3);
  move(1.0, 3);
  move(1.0, 1);
  move(1.0, 1);
  move(1.0, 2);
  move(1.0, 3);
  move(1.0, 3);
  move(1.0, 1);
  move(0.5, 3);
  move(1.0, 1);
  move(3.0, 2);
  move(2.0, 2);
  move(0.5, 3);
  move(2.0, 3);
  move(1.0, 1);
  move(2.0, 1);
  move(1.0, 3);
  move(2.0, 3);
  move(1.0, 1);
  move(3.0, 1);
  move(1.0, 2);
  move(1.0, 1);
  move(2.0, 3);
  move(1.0, 1);
  move(2.0, 1);
  move(2.0, 2);
  move(1.0, 1);
  move(2.0, 1);
  move(2.0, 3);
  move(2.0, 3);
  move(1.0, 3);
  move(2.0, 3);
  move(2.0, 2);
  move(2.0, 1);
  move(2.0, 1);
  move(1.0, 3);
  move(2.0, 3);
  move(2.0, 2);
  move(2.0, 3);
  move(1.0, 3);
  move(1.0, 3);
  move(1.0, 1);
  move(1.0, 1);

  return 0;

/*
  while (ros::ok()) {

    for (int i

    ROS_INFO("INFO");
    pub.publish(msg);
    rate.sleep();


}
*/

}

void move(double distance, int turn) {
  // A publisher for the movement data
ros::NodeHandle nh;


    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 10);
    // Drive forward at a given speed.  The robot points up the x-axis.
    // The default constructor will set all commands to 0
    geometry_msgs::Twist msg;
    if (ros::ok()) {
        switch(turn) {
            case 1:
                msg.angular.z = PI/2;
		break;
            case 2:
                msg.angular.z = PI;
		break;
            case 3:
                msg.angular.z = 3*PI/2;
		break;
	}
	msg.linear.x = 0.6 * 11.1 / 4.4;
        ROS_INFO("INFO");
	pub.publish(msg);
	rate.sleep();
}
}