ROS通信——C++实现
一、普通话题通信
创建功能包：
catkin_create_pkg package roscpp rospy std_msgs创建发布者：
# include "ros/ros.h"
# include "std_msgs/String.h"
using namespace std;
int main (int argc, char** argv)
{
//处理乱码问题
setlocale(LC_ALL, "");
//初始化ROS节点
ros::init(argc,argv,"pub");
//创建句柄
ros::NodeHandle nh;
//创建发布者
ros::Publisher pub = nh.advertise("topic",100);
std_msgs::String msg;
//设置10HZ的循环周期
ros::Rate rate(10);
//先休眠3秒，防止出现前几条数据丢失的现象
ros::Duration(3).sleep();
//只要程序正常，ros::ok就返回true
while (ros::ok)
{
//msg中只有一个成员data，用来储存字符串
msg.data = "Hello ROS";
//发布消息，括号里面的要和上面的消息类型一致
pub.publish(msg);
//打印日志，后面要加上c_str()否则会乱码
ROS_INFO("%s",msg.data.c_str());
//休眠，配合10HZ的循环周期
rate.sleep();
}
return 0;
} 创建订阅者：
# include "ros/ros.h"
# include "std_msgs/String.h"
void callback(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO("%s", msg->data.c_str());
}
int main (int argc, char** argv)
{
setlocale(LC_ALL, "");
ros::init(argc, argv, "sub");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("topic", 100, callback);
//循环等待回调函数
ros::spin();
return 0;
}编译配置：在Cmakelist中添加：
add_executable(test1 src/test1.cpp)
target_link_libraries(test1 ${catkin_LIBRARIES})二、自定义话题通信
Vscode这部分实现代码补全的配置：
1. 自定义的消息配置完成后打开devel下面的include目录，在终端中打开，然后输入pwd找到自定义信息的路径
2. 复制路径到c_cpp_properties.json文件的include下面：
实现发布者发布两个整数，接收者实现求和
创建功能包：
catkin_create_pkg package roscpp rospy std_msgs创建自定义消息：message.msg文件
注意：这里不要写为int
int32 num1
int32 num2创建发布者：
# include "ros/ros.h"
# include "package/message.h"//package为功能包名，message为自己定义的message.msg名
# include
using namespace std;
int main (int argc, char** argv)
{
ros::init(argc,argv,"pub");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise("topic",100);
package::message msg;
while (ros::ok)
{
int a,b;
cin >> a >> b;
msg.num1 = a;
msg.num2 = b;
pub.publish(msg);
cout<<"num1="<# include "ros/ros.h"
# include "package/message.h"
# include
void callback(const package::message::ConstPtr& msg)
{
int sum;
sum = msg->num1 + msg->num2;
ROS_INFO("sum = %d", sum);
}
int main (int argc, char** argv)
{
ros::init(argc, argv, "sub");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("topic", 100, callback);
//循环等待回调函数
ros::spin();
return 0;
}
环境配置：
在package.xml中加入：
message_generation
message_runtime 在Cmakelist中加入：
(1) find_package(catkin REQUIRED COMPONENTS message_generation)
(2) add_message_files(
FILES
message.msg//自己创建的msg中的文件
)
(3) generate_messages(
DEPENDENCIES
std_msgs
)
(4) catkin_package(
CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
)
(5)
add_executable(pub src/pub.cpp)
target_link_libraries(pub ${catkin_LIBRARIES})
add_dependencies(pub ${PROGECT_NAME}_gencpp)
add_executable(sub src/sub.cpp)
target_link_libraries(sub ${catkin_LIBRARIES})
add_dependencies(sub ${PROGECT_NAME}_gencpp)三、服务通信
实现客户端输入两个整数，服务端实现相加
创建功能包：
catkin_create_pkg package roscpp rospy std_msgs 创建自定义消息：
#客户端
int32 num1
int32 num2
---
#服务端
int32 sum创建client：
#include
#include "ros/ros.h"
#include "package/message1.h"
int main(int argc, char **argv)
{
// ROS节点初始化
ros::init(argc, argv, "add_two_ints_client");
// 从终端命令行获取两个加数，第一个指令是路径(默认)，第二、三个是输入的两个参数
if (argc != 3)
{
ROS_INFO("usage: add_two_ints_client X Y");
return 1;
}
// 创建节点句柄
ros::NodeHandle n;
// 创建一个client，请求add_two_int service，service消息类型是learning_communication::AddTwoInts
ros::ServiceClient client = n.serviceClient("add_two_ints");
//等待服务器启动，也就是先启动客户端的解决办法
//方法1
//ros::service::waitForService("add_two_ints");
//方法2
//client.waitForExistence();
// 创建learning_communication::AddTwoInts类型的service消息
package::message1 srv;
srv.request.a = atoll(argv[1]);
srv.request.b = atoll(argv[2]);
// 发布service请求，等待加法运算的应答结果
if (client.call(srv))//client.call(srv)发送服务请求，得到的回应自动放在srv.sum中
{
ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service add_two_ints");
return 1;
}
return 0;
} 创建server：
#include "ros/ros.h"
#include "package/message1.h"
// service回调函数，输入参数req，输出参数res
bool add(package::message1::Request &req,
package::message1::Response &res)
{
// 将输入参数中的请求数据相加，结果放到应答变量中
res.sum = req.a + req.b;
ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
ROS_INFO("sending back response: [%ld]", (long int)res.sum);
return true;
}
int main(int argc, char **argv)
{
// ROS节点初始化
ros::init(argc, argv, "add_two_ints_server");
// 创建节点句柄
ros::NodeHandle n;
// 创建一个名为add_two_ints的server，注册回调函数add()
ros::ServiceServer service = n.advertiseService("add_two_ints", add);
// 循环等待回调函数
ROS_INFO("Ready to add two ints.");
ros::spin();
return 0;
}环境配置：
在package.xml中加入：
message_generation
message_runtime 在Cmakelist中加入：
(1) find_package(catkin REQUIRED COMPONENTS message_generation)
(2) add_service_files(
FILES
message.msg//自己创建的srv中的文件
)
(3) generate_messages(
DEPENDENCIES
std_msgs
)
(4) catkin_package(
CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
)
(5)
add_executable(client src/client.cpp)
target_link_libraries(client ${catkin_LIBRARIES})
add_dependencies(client ${PROGECT_NAME}_gencpp)
add_executable(server src/server.cpp)
target_link_libraries(server ${catkin_LIBRARIES})
add_dependencies(server ${PROGECT_NAME}_gencpp)四、注意
自定义话题通信和服务通信不能一起使用
五、参数服务器
创建功能包：
catkin_create_pkg roscpp rospy std_msgs# include "ros/ros.h"
using namespace std;
int main (int argc, char* argv[])
{
setlocale(LC_ALL,"");
ros::init(argc, argv, "param");
ros::NodeHandle n;
//参数新增
//方案1：n
n.setParam("type", "car");
n.setParam("num", 100);
//方案2：ros::param
ros::param::set("type_param", "bicycle");
ros::param::set("num_param", 500);
//参数修改
//方案1：n
n.setParam("type", "big_car");
//方案2：ros::param
ros::param::set("type_param", "big_bicycle");
//参数查询
//方案1：param,99为默认值，如果没找到就默认为99
int radius = n.param("num", 99);
ROS_INFO("num=%d",radius);
//方案2：getParam
int radius1 = 0;
bool result = n.getParam("num", radius1);
if (result)
{
ROS_INFO("num=%d",radius1);
}
//方案3：getParamCached
int radius2 = 0;
bool result2 = n.getParamCached("num", radius2);
if (result2)
{
ROS_INFO("num=%d",radius2);
}
//参数删除
//方案1：n.deleteParam
bool result5 = n.deleteParam("num");
if (result5)
{
ROS_INFO("删除成功");
}
//方案2：ros::param::del
bool result6 = ros::param::del("num");
if (result6)
{
ROS_INFO("删除成功");
}
else
{
ROS_INFO("删除失败");
}
return 0;
}配置环境：
#配置参数服务器
add_executable(canshu src/canshu.cpp)
target_link_libraries(canshu ${catkin_LIBRARIES})六、动作通信
创建功能包：
catkin_create_pkg package roscpp rospy std_msgs actionlib actionlib_msgs创建服务端：
# include "ros/ros.h"
# include "actionlib/server/simple_action_server.h"
# include "package/dataAction.h"
typedef actionlib::SimpleActionServer Server;
void callback(const package::dataGoalConstPtr& goal,Server* server)
{
//解析提交的目标值
int goal_num = goal->num;
ROS_INFO("客户提交的目标值是%d",goal_num);
//产生连续反馈
ros::Rate rate(10);
int result = 0;
ROS_INFO("开始连续反馈...");
for (size_t i = 1; i <= goal_num; i++)
{
//累加
result+=i;
//休眠
rate.sleep();
//产生连续反馈
package::dataFeedback fb;
fb.progress=i/(double)goal_num;
server->publishFeedback(fb);
}
//最终结果
package::dataResult result1;
result1.result=result;
server->setSucceeded(result1);
}
int main (int argc, char* argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"action_server");
ros::NodeHandle n;
//创建server服务对象
//SimpleActionServer(ros::NodeHandle n,
// std::__cxx11::string name,
// boost::function execute_callback,
// bool auto_start)
//第一个参数：NodeHandle；
//第二个参数：话题名称；
//第三个参数：回调函数；
//第四个参数：是否自动执行
Server server(n,"action",boost::bind(&callback,_1,&server),false);
server.start();//如果第四个参数为false则需要调用该函数启动服务
ros::spin();
return 0;
} 创建客户端：
# include "ros/ros.h"
# include "actionlib/client/simple_action_client.h"
# include "package/dataAction.h"
//激活回调
void active_cb()
{
ROS_INFO("客户端服务连接建立...");
}
//联系反馈回调
void feedback_cb(const package::dataFeedbackConstPtr &feedback)
{
ROS_INFO("当前进度是:%.2f",feedback->progress);
}
//相应成功时回调
void done_cb(const actionlib::SimpleClientGoalState &state, const package::dataResultConstPtr &result)
{
if (state.state_==state.SUCCEEDED)
{
ROS_INFO("相应成功，最终结果是：%d",result->result);
}
else
{
ROS_INFO("请求失败");
}
}
int main (int argc, char* argv[])
{
setlocale(LC_ALL,"");
ros::init(argc, argv, "action_client");
ros::NodeHandle n;
//创建action客户端对象
actionlib::SimpleActionClient client(n,"action");
//发送请求
//注意：先等待服务器启动
ROS_INFO("等待服务器启动.....");
client.waitForServer();
//建立连接---回调函数
//处理连续反馈---回调函数
//处理最终反馈---回调函数
/*
void sendGoal(const package::dataGoal &goal,
boost::function done_cb = actionlib::SimpleActionClient::SimpleDoneCallback(),
boost::function active_cb ,
boost::function feedback_cb)
*/
package::dataGoal goal;
goal.num=100;
client.sendGoal(goal,&done_cb,&active_cb,&feedback_cb);
ros::spin();
return 0;
}
[点击并拖拽以移动]
配置环境：
在CMakelist中：
#配置动作通信
add_executable(action_server src/action_server.cpp)
target_link_libraries(action_server ${catkin_LIBRARIES})
add_dependencies(action_server ${PROJECT_NAME}_gencpp)
#配置动作通信
add_executable(action_client src/action_client.cpp)
target_link_libraries(action_client ${catkin_LIBRARIES})
add_dependencies(action_client ${PROJECT_NAME}_gencpp)
七、动态参数
创建功能包：
catkin_make_pkg package roscpp rospy std_msgs dynamic_reconfigure创建服务端：
# include "ros/ros.h"
# include "dynamic_reconfigure/server.h"
# include "package/drConfig.h"
void cd(package::drConfig &config, uint32_t level)
{
ROS_INFO("修改后的数据是：%d",config.int_param);
}
int main(int argc, char *argv[])
{
ros::init(argc,argv,"dr_server");
ros::NodeHandle n;
//创建服务端对象
dynamic_reconfigure::Server server;
//回调函数解析修改后的参数
server.setCallback(boost::bind(&cd,_1,_2));
ros::spin();
return 0;
} 创建客户端：
新建cfg文件夹，再建立**.cfg文件（python文件）
#! /usr/bin/env python
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("int_param",int_t,0,"int_param_int",10,1,100)
exit(gen.generate("package","dr_client","dr"))配置环境：
CMakelist中：添加
#配置动态参数
generate_dynamic_reconfigure_options(
cfg/dr.cfg
)
#配置动态参数
add_executable(dr01_server src/dr01_server.cpp)
add_dependencies(dr01_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(dr01_server ${catkin_LIBRARIES})