1. 程式人生 > >機器人作業系統入門:二(中科大&&重德智慧)

機器人作業系統入門:二(中科大&&重德智慧)

目錄

六  roscpp

1.Client library 是提供ROS程式設計的庫

2. 一個簡單的topic Demo

3. 一個簡單的ServiceDemo

4. 一個簡單的Param Demo


一  roscpp

1.Client library 是提供ROS程式設計的庫

CL語言包有兩種:c++和py。roscpp 執行效率較高,rospy 開發效率較高。

 

roscpp 是和topic   service  param  timer  互動的一個介面。

roscpp包含的API簡介:

http://docs.ros.org/api/roscpp/html/

 

 

 

 

 

  

2. 一個簡單的topic Demo

兩個node,一個釋出模擬的GPS訊息,一個接收處理。

步驟:

  1. 建立package
  2. 自定義的訊息格式 msg
  3. 釋出者的原始碼 talker.cpp
  4. 接收者的原始碼  listener.cpp
  5. 修改CMakeLists  和  xml檔案 

1.建立package

cd ~/catkin_ws/src
catkin_create_pkg topic_demo roscpp rospy std_msgs

2.自定義訊息格式msg

cd topic_demo/ && mkdir msg
cd msg && vim gps.msg

 gps.msg 檔案內容格式如下:

float32 y

float32 x

string state

經過編譯過後會出現對應的標頭檔案  ~/catkin_ws/devel/include/topic_demo/gps.h

 

3.釋出者的原始碼 talker.cpp

#include <ros/ros.h>
#include <topic_demo/gps.h>

int main(int argc,char** argv)
{
    ros::init(argc,argv,"talker");   //解析引數,命名節點(talker是節點名稱)
    ros::NodeHandle nh;     //建立控制代碼,例項化node

    topic_demo::gps msg;  //建立msg訊息
    msg.x=1.0;
    mag.y=1.0;
    msg.state="working";


/********建立publisher,advertise 模板函式的第一個引數gps_info是釋出的topic的名稱,第二個引數是先發布到一個佇列中(類似快取?),隨發隨收,所以佇列的長度設為1。********/    
    ros::Publisher pub= nh.advertise<topic_demo::gps>("gps_info",1);   
  

/*********接下來要週期性迴圈釋出訊息*************/
    ros::Rate loop_rate(1.0);   //定義迴圈釋出的頻率,1代表1hz
    while(ros::ok())
    {
        msg.x=1.03* msg.x;
        msg.y=1.03* msg.y;
        ROS_INFO( "GPS:x=%f,y=%f",msg.x,msg.y);  //列印輸出當前msg
        pub.publish(msg);     //釋出訊息
        loop_rate.sleep();    
    }

    return 0;

}

4.接收者的原始碼 listener.cpp 

#include <ros/ros.h>
#incldue <topic_demo/gps.h>
#include <std_msgs/Float32.h>
 
void gpsCallback(const topic_demo::gps::ConstPtr & msg)  //常指標
{
    std_msgs::Float32 distance;   //ros自帶的Float32型別,為結構體
    distance.data=sqrt(pow(msg->x,2),pow(msg->y,2));
    ROS_INFO("Distance=%f,state-=%s",distance.data,msg->state.c_str());  //列印輸出

}




int main(int argc,char** argv)
{
    ros::init(argc,argv,"listener");  //解析引數,命名節點
    ros::NodeHandle n;
    

/**********建立subscriber************/
/********第一個引數是監聽的topic的名稱,第二個引數是訊息佇列的長度,第三個是回撥函式(指標),用來處理監聽到的資訊*******/
    ros::Subscriber sub=n.subscribe("gps_info",1,gpsCallback);    
    ros::spin();   //反覆檢視佇列是否有訊息,然後呼叫當前可觸發的回撥函式,系統呈阻塞狀態

    return 0;
}

 

5.改動CMakeLists 和 xml

改動 CMakeLists

 

 

改動 xml 

 6.編譯執行

程式碼工作均已完成,接下來 編譯 catkin_make  執行  rosrun

 

3. 一個簡單的ServiceDemo

兩個node,一個釋出請求(格式自定義,本例中是姓名和年齡),一個接收處理並返回資訊(本例中是一個字串)。

步驟:

  1. 建立package
  2. 定義srv
  3. 響應者的原始碼 server.cpp
  4. 請求者的原始碼 client.cpp
  5. 修改CMakeLists  和  package.xml檔案 

 1.建立package

cd ~/catkin_ws/src
catkin_create_pkg service_demo roscpp rospy std_msgs

2.定義srv

cd service_demo/
mkdir srv
vim Greeting.srv

 

編譯後會在指定檔案下出現標頭檔案,具體如下

 

 

3.響應者的原始碼 server.cpp

#include <ros/ros.h>
#include <service_demo/Greeting.h>

//返回值為布林型,表示函式是否被正確執行

bool handle_function(service::demo::Greeting::Request & req, service::demo::Greating::Response &res)
{
    ROS_INFO("Request from %s with age %d", req.name.c_str(),req.age); //列印請求資訊
    res.feedback=“Hi” + req.name +“I am server!”;  //處理請求,結果寫入reponse
    return ture; //返回 true ,正確處理請求
}


int main()
{
    ros::init(argc,argv, "greetings_server");   //解析引數,命名節點
    ros::NodeHandle nh;   //建立控制代碼,例項化node

/********提供服務,第一個引數是服務名稱,第二個引數是處理函式(函式指標)*********/
    ros::ServcieServer service = nh.advertiseService(“greetings”, handle_function)  
    
    ros::spin();
    return 0;


}

 

4.請求者的原始碼

#include <ros/ros.h>
#include <service_demo/Greeting.h>

int main(int argc, char** argv)
{
    ros::init(argc,argv,"greetings_server");
    ros::NodeHandle nh;

    /*******這步是建立client,傳送的請求的型別是servcie_demo::Greeting,函式的引數“greetings”就是要傳送到的sevice********/
    ros::ServiceClient client= nh.serviceClient<servcie_demo::Greeting>("greetings");

    
    service_demo::Greeting srv;
    srv.request.name = "HAN" ;
    srv.request.age = 20;

    
    /*********client.call返回的布林值變數也是handle_function返回的結果*************/
    if(client.call(srv))
    {
        ROS_INFO("Feedback from server:%s",srv.response.feedback);

    }
    else
    {
        ROS_ERROR("Failed to call service greeting.");
        return 1;
    }

    return 0;
}

 

5.修改 CMakeList 和  xml

類比之前的。。。

 

4. 一個簡單的Param Demo

 同一個功能兩套API:ros::param  和 ros::NodeHandle

 

param_demo.cpp ()在原始碼中設定引數

#incldue <ros/ros.h>

int main(int argc, char** argv)
{
    ros::init(argc,argv,"greetings_server");
    ros::NodeHandle nh;
    
    int parameter1,parameter2,parameter3,parameter4,parameter5;
    
    //獲取引數的三種方法, param 是namespace
    ros::param::get("param1",parameter1);  //把第一個引數(key)的值賦給第二個引數
    nh.getParam("param2",parameter2);    //把第一個引數(key)的值賦給第二個引數
    nh.param("param3",parameter3,123);  //如果沒有找到param3,就賦給預設值123
    
    //設定引數
    ros::param::set("param4",parameter4)
    nh.setParam("param5",parameter5)
    
    //檢查引數是否存在
    ros::param::has("param5")
    nh.hasParam("param6");
    
    //刪除引數
    ros::param::del("param5");
    ros::deleteParam("param6");

    return 0;

}

另一種設定引數的方法  launch檔案法

param_demo_cpp.launch

<launch>
    <!--Param標籤設定單個引數-->
    <param name="param1" value="1" />
    <param name="param2" value="2" />
    <param name="robot_description" command="$(find xacro)/xacro.py $(find demo)/urdf/robot.urdf" />


    <!--rosparam標籤設定多個引數-->
    <rosparam>
        param3:3
        param4:4
        param10:helloworld!
    </rosparam>
    <rosparam file= "$(find param_demo)/config/myparam.yaml" command= "load" />
    
    <node pkg= "param_demo" type="param_demo" name="param_demo" output= "screen" />

</launch>