ROS實踐手冊（五）RoboWare實現learning_service編程實驗

筆者根據 古月居 · ROS入門21講 學習整理，並參考《ROS機器人開發實踐》一書。
相關課件及源碼可參考 Github/huchunxu/ros_21_tutorials

編程實現"spawn"服務請求

• 右鍵點擊 ROS 工作區下的 “catkin_ws/src” ，選擇 “新建ROS包” ，輸入包名稱及其依賴包的名稱 learning_service roscpp rospy std_msgs geometry_msgs turtlesim ，回車後，會創建名爲 “learning_service” 、以 “roscpp” 、“rospy”、“std_msgs”、“geometry_msgs”和“turtlesim” 爲依賴的 ROS 包。
• 右鍵點擊 catkin_ws/src/learning_service/src 文件夾，點擊 “新建CPP源文件” 輸入文件名turtle_spawn ，點擊回車鍵彈出列表，選擇 “加入到新的可執行文件中” ，則會創建一個與cpp文件同名的可執行文件（ROS節點）。
• 編輯 turtle_spawn.cpp 文件如下。

/**
 * 該例程將請求/spawn服務，服務數據類型turtlesim::Spawn
 */

#include <ros/ros.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
    // 初始化ROS節點
    ros::init(argc, argv, "turtle_spawn");
    
    // 創建節點句柄
    ros::NodeHandle node;

    // 發現/spawn服務後，創建一個服務客戶端，連接名爲/spawn的service
    ros::service::waitForService("/spawn");
    ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
    
    // 初始化turtlesim::Spawn的請求數據
    turtlesim::Spawn srv;
    srv.request.x = 2.0;
    srv.request.y = 2.0;
    srv.request.name = "turtle2";

    // 請求服務調用
    ROS_INFO("Call service to spwan turtle[x:%0.6f, y:%0.6f, name:%s]", 
             srv.request.x, srv.request.y, srv.request.name.c_str());
    add_turtle.call(srv);

    // 顯示服務調用結果
    ROS_INFO("Spwan turtle successfully [name:%s]", srv.response.name.c_str());

    return 0;
}

• 點擊“文件-首選項-設置”，選擇“ROS”，修改構件工具爲“catkin_make”，並關閉保存。
• 在主界面左上角，修改資源管理器旁的構建方式爲“Debug”，並點擊小錘子進行構建。
• 在主界面左下角“ROS節點-learning_topic-velocity_publisher”，即可進行調試、運行、配置參數等。
• 點擊主界面上方“ROS-運行roscore”
• 在主界面右側下方點擊“+”創建新終端，輸入 $ rosrun turtlesim turtlesim_node 啓動小海龜仿真器。
• 在主界面左下角“ROS節點-learning_topic-velocity_publisher”，點擊“運行文件”，創建新海龜“turtle2”。
  

編程實現"command"服務端

• 右鍵點擊 catkin_ws/src/learning_service/src 文件夾，點擊 “新建CPP源文件” 輸入文件名turtle_command_server ，點擊回車鍵彈出列表，選擇 “加入到新的可執行文件中” 。
• 編輯 turtle_command_server.cpp 文件如下。

/**
 * 該例程將執行/turtle_command服務，服務數據類型std_srvs/Trigger
 */

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Trigger.h>

ros::Publisher turtle_vel_pub;
bool pubCommand = false;

// service回調函數，輸入參數req，輸出參數res
bool commandCallback(std_srvs::Trigger::Request  &req,
                     std_srvs::Trigger::Response &res)
{
    pubCommand = !pubCommand;
    
    // 顯示請求數據
    ROS_INFO("Publish turtle velocity command [%s]", pubCommand==true?"Yes":"No");

    // 設置反饋數據
    res.success = true;
    res.message = "Change turtle command state!";
    
    return true;
}

int main(int argc, char **argv)
{
    // ROS節點初始化
    ros::init(argc, argv, "turtle_command_server");
    
    // 創建節點句柄
    ros::NodeHandle n;

    // 創建一個名爲/turtle_command的server，註冊回調函數commandCallback
    ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);

    // 創建一個Publisher，發佈名爲/turtle1/cmd_vel的topic，消息類型爲geometry_msgs::Twist，隊列長度10
    turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

    // 循環等待回調函數
    ROS_INFO("Ready to receive turtle command.");

    // 設置循環的頻率
    ros::Rate loop_rate(10);

    while(ros::ok())
    {
        // 查看一次回調函數隊列
        ros::spinOnce();

        // 如果標誌爲true，則發佈速度指令
        if(pubCommand)
        {
            geometry_msgs::Twist vel_msg;
            vel_msg.linear.x = 0.5;
            vel_msg.angular.z = 0.2;
            turtle_vel_pub.publish(vel_msg);
        }
        
        //按照循環頻率延時
        loop_rate.sleep();
    }
    return 0;
}

• 在主界面左上角，修改資源管理器旁的構建方式爲“Debug”，並點擊小錘子進行構建。
• 點擊主界面上方“ROS-運行roscore”
• 在主界面右側下方點擊“+”創建新終端，輸入 $ source devel/setup.bash
• 在主界面右側下方點擊“+”創建新終端，輸入 $ rosrun turtlesim turtlesim_node 啓動小海龜仿真器。
• 在主界面右側下方點擊“+”創建新終端，輸入 $ rosrun learning_service turtle_command_server 啓動command服務端。
• 在主界面右側下方點擊“+”創建新終端，輸入 $ rosservice call /turtle_command 並雙擊“TAB”鍵，發送服務請求。單次使得海龜行動，雙次使得海龜停止。