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