ros_turtlesim_module/TurtleCommander.cpp at master · iskinmike/ros_turtlesim_module · 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
111
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>

#include <beginner_tutorials/Spawn.h>
#include <beginner_tutorials/Kill.h>

#include <string>

// GLOBAL VARIABLES
ros::Rate *ploop_rate ;
ros::NodeHandle *pNode;
//

void initTurtles()
{
	// proxy args for ros::init()
	int targ1=1;
	char ch1[6]="proxy";
	char *pch1=ch1;
	char **targ2=&pch1;
	/////////////////////////////

	if (!ros::isInitialized()){
		ros::init(targ1, targ2, "TurtleCommander");
	}

	ros::NodeHandle *n = new ros::NodeHandle;
	ros::Rate *loop_rate = new ros::Rate(2);

	pNode = n;
	ploop_rate = loop_rate;
}

void clearTurtleNode()
{
	delete pNode;
	delete ploop_rate;
}


std::string spawnTurtle(double x, double y, double theta, std::string name)
{
	ros::ServiceClient turtle_spawn_int = (*pNode).serviceClient<beginner_tutorials::Spawn>("spawn",1);

	beginner_tutorials::Spawn newTurtle;

	newTurtle.request.x=x;
	newTurtle.request.y=y;
	newTurtle.request.theta=theta;
	newTurtle.request.name=name;

	turtle_spawn_int.call(newTurtle);

	return newTurtle.response.name;
}


void killTurtle(std::string name)
{
	beginner_tutorials::Kill turtle;
	turtle.request.name = name;
	ros::ServiceClient turtle_kill_int = (*pNode).serviceClient<beginner_tutorials::Kill>("kill",1);
	turtle_kill_int.call(turtle);
}


void moveTurtle(std::string name, double ForwardSpeed, double AngularSpeed)
{
	ros::Publisher chatter_pub_int = (*pNode).advertise<geometry_msgs::Twist>(name + "/cmd_vel", 1000);
	geometry_msgs::Twist MesToTurtle;

	MesToTurtle.linear.x = ForwardSpeed;
	MesToTurtle.angular.z = AngularSpeed;

	(*ploop_rate).sleep();
	chatter_pub_int.publish(MesToTurtle);

	ros::spinOnce();
	(*ploop_rate).sleep();
}
void moveTurtleForward(std::string name, double ForwardSpeed)
{
	ros::Publisher chatter_pub_int = (*pNode).advertise<geometry_msgs::Twist>(name + "/cmd_vel", 1000);

	geometry_msgs::Twist MesToTurtle;

	MesToTurtle.linear.x = ForwardSpeed;
	MesToTurtle.angular.z = 0;

	(*ploop_rate).sleep();
	chatter_pub_int.publish(MesToTurtle);

	ros::spinOnce();
	(*ploop_rate).sleep();
}
void rotateTurtle(std::string name, double AngularSpeed)
{
	ros::Publisher chatter_pub_int = (*pNode).advertise<geometry_msgs::Twist>(name + "/cmd_vel", 1000);

	geometry_msgs::Twist MesToTurtle;

	MesToTurtle.linear.x = 0;
	MesToTurtle.angular.z = AngularSpeed;

	(*ploop_rate).sleep();
	chatter_pub_int.publish(MesToTurtle);

	ros::spinOnce();
	(*ploop_rate).sleep();
}