ROS通信(海龟模拟器)
一、发布海龟速度指令,让海龟圆周运动
创建一个Publisher,发布名为turtle1/cmd_vel的topic,其消息类型为geometry_msgs::Twist,通过控制cmd_vel中linear下的x和angular下的z。
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
参考代码
#include <sstream>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
int main(int argc, char **argv)
{
// ROS节点初始化
ros::init(argc, argv, "turtle_publisher");
// 创建节点句柄
ros::NodeHandle n;
// 创建一个Publisher,发布名为turtle1/cmd_vel的topic,消息类型为geometry_msgs::Twist,传输空间大小1000
ros::Publisher chatter_pub = n.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
// 设置循环的频率
ros::Rate loop_rate(10);
int count = 0;
while (ros::ok())
{
// 初始化geometry_msgs::Twist类型的消息
geometry_msgs::Twist Twist_msg;
float linear_x, linear_y, angular_z;
linear_x = 2;
linear_y = 0;
angular_z = 1;
Twist_msg.linear.x = linear_x;
Twist_msg.linear.y = linear_y;
Twist_msg.angular.z = angular_z;
// 发布消息
ROS_INFO("I heard cmd_vel linear.x:[%f] linear.y:[%f] angular.z:[%f]",linear_x, linear_y, angular_z);
chatter_pub.publish(Twist_msg);
// 按照循环频率延时
loop_rate.sleep();
++count;
}
return 0;
}
二、定阅海龟的位置,并在终端中周期打印输出
创建一个Subscriber,订阅名为turtle1/pose的topic,其消息类型为turtlesim::Pose,注册回调函数chatterCallback_pose,而输出turtle1/pose中x, y,theta就是海龟turtle1的位置。
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity
参考代码:
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "turtlesim/Pose.h"
// 接收到订阅的消息后,会进入消息回调函数
void chatterCallback_pose(const turtlesim::Pose::ConstPtr& Pose_msg)
{
// 将接收到的消息打印出来
float x, y, theta;
x=Pose_msg->x;
y=Pose_msg->y;
theta=Pose_msg->theta;
ROS_INFO("I heard pose x:[%f] y:[%f] z:[%f]",x, y, theta);
}
int main(int argc, char **argv)
{
// 初始化ROS节点
ros::init(argc, argv, "turtle_subscriber");
// 创建节点句柄
ros::NodeHandle n;
// 创建一个Subscriber,订阅名为turtle1/pose的topic,注册回调函数chatterCallback_pose
ros::Subscriber sub = n.subscribe("turtle1/pose", 1000, chatterCallback_pose);
// 循环等待回调函数
ros::spin();
return 0;
}
三、创建新海龟的服务,在仿真器中产生一只新海龟
创建一个client,service消息类型是turtlesim::Spawn,向服务端传入设置好的海龟位置和名称(x y theta name)。
Node: /turtlesim
URI: rosrpc://10.0.2.15:60487
Type: turtlesim/Spawn
Args: x y theta name
参考代码:
#include "ros/ros.h"
#include "std_srvs/SetBool.h"
#include "turtlesim/Spawn.h"
#include <stdlib.h> /* srand, rand */
#include <time.h> /* time */
int main(int argc, char **argv)
{
// ROS节点初始化
ros::init(argc, argv, "turtle_client");
// set argv[1] not null
if(argv[1] == NULL){
argv[1] = "";
}
ROS_INFO("argv: %s", argv[1]);
// 创建节点句柄
ros::NodeHandle n;
// 创建一个client,service消息类型是turtlesim::Spawn turtlesim/Spawn
ros::service::waitForService("/spawn");
ros::ServiceClient client = n.serviceClient<turtlesim::Spawn>("/spawn");
// 创建turtlesim::Spawn类型的service消息
turtlesim::Spawn srv;
srand (time(NULL));
srv.request.x = rand()%10;
srv.request.y = rand()%10;
srv.request.theta = rand()%10*0.5;
srv.request.name = argv[1];
// 发布service请求,等待应答结果
if (client.call(srv))
{
ROS_INFO("Response : ok");
}
else
{
ROS_ERROR("Failed to call service spawn");
return 1;
}
return 0;
}
四、控制特定海龟的速度大小
这里和1中的实现方式相似,可以在终端输入控制的参数argv,然后由这些参数控制海龟状态。
ps:有时候在编译时会出现warning: Clock skew detected. Your build may be incomplete.这样的警告。这是时钟同步问题,可以
find . -type f | xargs -n 5 touch