ROS代碼

寫在前面：

環境：ubuntu 16.04

IDE：roboware studio

１、收發消息(topic)

(1). publisher

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

int main(int argc, char ** argv){
    //解析函數，命名結點
    ros::init(argc, argv, "publisher");
    //創建句柄，用於初始化node
    ros::NodeHandle nh;
    //自定義msg的內容
    roscpp_topic_demo::gps msg;
    msg.x = 1.0;
    msg.y = 1.2;
    msg.state = "working";
    //創建publisher
    ros::Publisher pub = nh.advertise<roscpp_topic_demo::gps>("gps_info",1);
    //定義發佈的頻率
    ros::Rate loop_rate(1.0);
    while(ros::ok()){
        ROS_INFO("Talker:GPS: x = %f, y = %f", msg.x, msg.y);
        msg.x = msg.x * 1.03;
        msg.y = msg.y * 1.02;
        pub.publish(msg);
        loop_rate.sleep();
    }
    return 0;
}

2.subscriber
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <roscpp_topic_demo/gps.h>

void gpsCallback(const roscpp_topic_demo::gps::ConstPtr &msg){
    //計算距離原點的距離
    std_msgs::Float32 distance;
    //注意這裏使用的ros自帶的float32類型，distance是一個結構體，具體數值保存在data中
    distance.data = sqrt(pow(msg->x,2)+pow(msg->y,2));
    ROS_INFO("Listener:Distance to origin = %f, state: %s", distance.data,msg->state.c_str());

}

int main(int argc, char ** argv){
    //解析函數，命名結點
    ros::init(argc, argv, "subscriber");
    //創建句柄，初始化結點
    ros::NodeHandle nh;
    //創建subscriber,創建回調函數
    ros::Subscriber sub = nh.subscribe("gps_info",1,gpsCallback);
    //堵塞函數，使得其一直處於監聽狀態
    ros::spin();
    return 0;
}

(3)gps.msg
string state
float32 x
float32 y

２、請求和響應服務(service)

(1).client
#include "ros/ros.h"
#include "roscpp_service_demo/Greetings.h"

int main(int argc, char ** argv){
    //解析參數，命名結點
    ros::init(argc, argv, "client");
    //創建句柄，創建client
    ros::NodeHandle nh;
    //創建客戶機
    ros::ServiceClient client = nh.serviceClient<roscpp_service_demo::Greetings>("greetings_info");
    //實例化服務對象，向服務機發出請求
    roscpp_service_demo::Greetings srv;
    //srv中有兩部分，一部分是客戶機的request，另一部分是服務機的response,這裏需要的客戶機的request
    srv.request.name = "Han";
    srv.request.age = 20;
    //客戶機發送請求,call的返回值是由服務機的回調函數返回的
    if(client.call(srv)){
        //服務機返回了響應，並響應的內容打印下來
        ROS_INFO("Request from server: %s", srv.response.feedback.c_str());
    }else{
        //服務機沒有返回響應，說明出現了錯誤
        ROS_ERROR("Failed to call service");
        return 1;
    }
    return 0;

}

(2)server
# include "ros/ros.h"
# include "roscpp_service_demo/Greetings.h"
# include "string"


bool handle_function(roscpp_service_demo::Greetings::Request &req, roscpp_service_demo::Greetings::Response &res){
    //直接將服務輸出打印
    ROS_INFO("Request from %s with age %d", req.name.c_str(),req.name);
    //重置反饋的內容
    res.feedback = "Hi" + req.name + ".i'm server!";
    return true;

}

int main(int argc, char ** argv){
    //解析參數，命名結點
    ros::init(argc, argv, "server");
    //創建句柄，用於創建service
    ros::NodeHandle nh;
    //創建server的實例對象
    ros::ServiceServer server = nh.advertiseService("greetings_info", handle_function);
    ros::spin();
    return 0;
}

(3)Greetings.srv
string name
int32 age
---
string feedback