ROS通信(海龟模拟器)

1. 发布海龟速度指令，让海龟圆周运动

创建一个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 
#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("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;
}

2. 定阅海龟的位置，并在终端中周期打印输出

创建一个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;
}

3. 创建新海龟的服务，在仿真器中产生一只新海龟

创建一个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              /* srand, rand */
#include                /* 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("/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;
}

4. 控制特定海龟的速度大小

这里和1中的实现方式相似，可以在终端输入控制的参数argv，然后由这些参数控制海龟状态。

ps:有时候在编译时会出现warning: Clock skew detected. Your build may be incomplete.这样的警告。这是时钟同步问题，可以

find . -type f | xargs -n 5 touch