1. 發(fā)布海龜速度指令橄抹，讓海龜圓周運(yùn)動(dòng)
創(chuàng)建一個(gè)Publisher，發(fā)布名為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節(jié)點(diǎn)初始化
ros::init(argc, argv, "turtle_publisher");
// 創(chuàng)建節(jié)點(diǎn)句柄
ros::NodeHandle n;
// 創(chuàng)建一個(gè)Publisher，發(fā)布名為turtle1/cmd_vel的topic液斜，消息類型為geometry_msgs::Twist累贤，傳輸空間大小1000
ros::Publisher chatter_pub = n.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
// 設(shè)置循環(huán)的頻率
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;
// 發(fā)布消息
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);
// 按照循環(huán)頻率延時(shí)
loop_rate.sleep();
++count;
}
return 0;
}
2. 定閱海龜?shù)奈恢茫⒃诮K端中周期打印輸出
創(chuàng)建一個(gè)Subscriber少漆，訂閱名為turtle1/pose的topic臼膏，其消息類型為turtlesim::Pose，注冊回調(diào)函數(shù)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"
// 接收到訂閱的消息后夺溢，會(huì)進(jìn)入消息回調(diào)函數(shù)
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節(jié)點(diǎn)
ros::init(argc, argv, "turtle_subscriber");
// 創(chuàng)建節(jié)點(diǎn)句柄
ros::NodeHandle n;
// 創(chuàng)建一個(gè)Subscriber，訂閱名為turtle1/pose的topic烛谊，注冊回調(diào)函數(shù)chatterCallback_pose
ros::Subscriber sub = n.subscribe("turtle1/pose", 1000, chatterCallback_pose);
// 循環(huán)等待回調(diào)函數(shù)
ros::spin();
return 0;
}
3. 創(chuàng)建新海龜?shù)姆?wù)风响，在仿真器中產(chǎn)生一只新海龜
創(chuàng)建一個(gè)client，service消息類型是turtlesim::Spawn丹禀，向服務(wù)端傳入設(shè)置好的海龜位置和名稱(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節(jié)點(diǎn)初始化
ros::init(argc, argv, "turtle_client");
// set argv[1] not null
if(argv[1] == NULL){
argv[1] = "";
}
ROS_INFO("argv: %s", argv[1]);
// 創(chuàng)建節(jié)點(diǎn)句柄
ros::NodeHandle n;
// 創(chuàng)建一個(gè)client鞋怀，service消息類型是turtlesim::Spawn turtlesim/Spawn
ros::service::waitForService("/spawn");
ros::ServiceClient client = n.serviceClient<turtlesim::Spawn>("/spawn");
// 創(chuàng)建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];
// 發(fā)布service請求，等待應(yīng)答結(jié)果
if (client.call(srv))
{
ROS_INFO("Response : ok");
}
else
{
ROS_ERROR("Failed to call service spawn");
return 1;
}
return 0;
}
4. 控制特定海龜?shù)乃俣却笮?/h4>
這里和1中的實(shí)現(xiàn)方式相似持搜，可以在終端輸入控制的參數(shù)argv密似，然后由這些參數(shù)控制海龜狀態(tài)。
ps:有時(shí)候在編譯時(shí)會(huì)出現(xiàn)warning: Clock skew detected. Your build may be incomplete.這樣的警告葫盼。這是時(shí)鐘同步問題残腌，可以
find . -type f | xargs -n 5 touch
