C# · 12月 20, 2021

ROS 教程5 基础综合应用C++ 发布订阅 话题 服务 action 动态参数配置 坐标变换 日志 发布图像/点云/marker/雷达扫描/里程计

Learn ROS

本文github

中国大学MOOC《机器人操作系统入门》课程代码示例

ROS 1 和 ROS 2 的前世、今生、安装使用说明与资料汇总

ROS(1和2)机器人操作系统相关书籍、资料和学习路径

move_base的全局路径规划代码研究1

move_base的全局路径规划代码研究2

move_base代码学习一

octomap中3d-rrt路径规划

ROS多个master消息互通

roscpp源码阅读

ros的源码阅读

Gazebo Ros入门

ROS源代码分析、笔记和注释

ROS学习资料汇总

古月居

ros下开发工具 脚本

ros编程书籍 c++ !!!推荐

Mastering-ROS-for-Robotics-Programming-Second-Edition 代码

一、消息

1. 发布 字符串 消息

#include “ros/ros.h”

#include “std_msgs/String.h”// 字符串消息 其他 int.h

#include

int main(int argc,char **argv)

{

ros::init(argc,argv,”example1a”);// 节点初始化

ros::NodeHandle n;

ros::Publisher pub = n.advertise(“message”,100);// 发布消息到 message 话题,100个数据空间

ros::Rate loop_rate(10);// 发送频率

while (ros::ok())

{

std_msgs::String msg;

std::stringstream ss;

ss << "Hello World!"; // 生成消息

msg.data = ss.str();

pub.publish(msg);// 发布

ros::spinOnce();// 给ros控制权

loop_rate.sleep();// 时间没到,休息

}

return 0;

}

2. 订阅消息

#include “ros/ros.h”

#include “std_msgs/String.h”

// 订阅消息的回调函数

void messageCallback(const std_msgs::String::ConstPtr& msg)

{

ROS_INFO(“Thanks: [%s]”,msg->data.c_str());

}

int main(int argc,”example1b”);

ros::NodeHandle n;

// 订阅话题,消息,接收到消息就会 调用 回调函数 messageCallback

ros::Subscriber sub = n.subscribe(“message”,100,messageCallback);

ros::spin();

return 0;

}

3. 发布自定义消息 msg

#include “ros/ros.h”

#include “chapter2_tutorials/chapter2_msg.h” // 项目 msg文件下

// msg/chapter2_msg.msg 包含3个整数的消息

// int32 A

// int32 B

// int32 C

#include

int main(int argc,”example2a”);

ros::NodeHandle n;

// 发布自定义消息====

ros::Publisher pub = n.advertise(“chapter2_tutorials/message”,100);

ros::Rate loop_rate(10);

while (ros::ok())

{

chapter2_tutorials::chapter2_msg msg;

msg.A = 1;

msg.B = 2;

msg.C = 3;

pub.publish(msg);

ros::spinOnce();

loop_rate.sleep();

}

return 0;

}

4. 订阅自定义消息 msg

#include “ros/ros.h”

#include “chapter2_tutorials/chapter2_msg.h”

void messageCallback(const chapter2_tutorials::chapter2_msg::ConstPtr& msg)

{

ROS_INFO(“I have received: [%d] [%d] [%d]”,msg->A,msg->B,msg->C);

}

int main(int argc,”example3_b”);

ros::NodeHandle n;

// 订阅自定义消息===

ros::Subscriber sub = n.subscribe(“chapter2_tutorials/message”,messageCallback);

ros::spin();

return 0;

}

5. 发布自定义服务 srv

#include “ros/ros.h”

#include “chapter2_tutorials/chapter2_srv.h” // 项目 srv文件下

// chapter2_srv.srv

// int32 A 请求

// int32 B

// —

// int32 sum 响应—该服务完成求和服务

// 服务回调函数==== 服务提供方具有 服务回调函数

bool add(chapter2_tutorials::chapter2_srv::Request &req,// 请求

chapter2_tutorials::chapter2_srv::Response &res) // 回应

{

res.sum = req.A + req.B; // 求和服务

ROS_INFO(“Request: A=%d,B=%d”,(int)req.A,(int)req.B);

ROS_INFO(“Response: [%d]”,(int)res.sum);

return true;

}

int main(int argc,”adder_server”);

ros::NodeHandle n;

// 发布服务(打广告) 广而告之 街头叫卖 等待被撩.jpg

ros::ServiceServer service = n.advertiseService(“chapter2_tutorials/adder”,add);

ROS_INFO(“adder_server has started”);

ros::spin();

return 0;

}

6. 订阅服务 获取服务 强撩.jpg

#include “ros/ros.h”

#include “chapter2_tutorials/chapter2_srv.h”

#include

int main(int argc,”adder_client”);

if (argc != 3)

{

ROS_INFO(“Usage: adder_client A B “);

return 1;

}

ros::NodeHandle n;

// 服务客户端,需求端,调用服务

ros::ServiceClient client = n.serviceClient(“chapter2_tutorials/adder”);

//创建服务类型

chapter2_tutorials::chapter2_srv srv;

// 设置请求内容

srv.request.A = atoll(argv[1]);

srv.request.B = atoll(argv[2]);

// 调用服务===

if (client.call(srv))

{

// 打印服务带有的响应数据====

ROS_INFO(“Sum: %ld”,(long int)srv.response.sum);

}

else

{

ROS_ERROR(“Failed to call service adder_server”);

return 1;

}

return 0;

}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)

project(chapter2_tutorials) # 项目名称

## 依赖包===========

find_package(catkin required COMPONENTS

roscpp

std_msgs

message_generation # 生成自定义消息的头文件

dynamic_reconfigure

)

## 自定义消息文件====

add_message_files(

FILES

chapter2_msg.msg

)

## 自定义服务文件====

add_service_files(

FILES

chapter2_srv.srv

)

## 生成消息头文件

generate_messages(

DEPENDENCIES

std_msgs

)

## 依赖

catkin_package(

CATKIN_DEPENDS message_runtime

)

## 编译依赖库文件

include_directories(

include

${catkin_INCLUDE_DIRS}

)

# 创建可执行文件

add_executable(example1a src/example_1a.cpp)

add_executable(example1b src/example_1b.cpp)

add_executable(example2a src/example_2a.cpp)

add_executable(example2b src/example_2b.cpp)

add_executable(example3a src/example_3a.cpp)

add_executable(example3b src/example_3b.cpp)

## 添加依赖

add_dependencies(example1a chapter2_tutorials_generate_messages_cpp)

add_dependencies(example1b chapter2_tutorials_generate_messages_cpp)

add_dependencies(example2a chapter2_tutorials_generate_messages_cpp)

add_dependencies(example2b chapter2_tutorials_generate_messages_cpp)

add_dependencies(example3a chapter2_tutorials_generate_messages_cpp)

add_dependencies(example3b chapter2_tutorials_generate_messages_cpp)

# 动态链接库

target_link_libraries(example1a ${catkin_LIBRARIES})

target_link_libraries(example1b ${catkin_LIBRARIES})

target_link_libraries(example2a ${catkin_LIBRARIES})

target_link_libraries(example2b ${catkin_LIBRARIES})

target_link_libraries(example3a ${catkin_LIBRARIES})

target_link_libraries(example3b ${catkin_LIBRARIES})

二、行动action类型 参数服务器 坐标变换 tf可视化 安装插件 gazebo仿真

[插件1]

(https://github.com/PacktPublishing/Robot-Operating-System-Cookbook/tree/master/Chapter03/chapter3_tutorials/nodelet_hello_ros)

插件2

1. 发布行动 action

// 类似于服务,但是是应对 服务任务较长的情况,避免客户端长时间等待,

// 以及服务结果是一个序列,例如一件工作先后很多步骤完成

#include

#include // action 服务器

#include // 自定义的 action类型 产生斐波那契数列

// action/Fibonacci.action

// #goal deFinition 任务目标

// int32 order

// —

// #result deFinition 最终 结果

// int32[] sequence

// —

// #Feedback 反馈 序列 记录中间 递增 序列

// int32[] sequence

// 定义的一个类========================

class FibonacciAction

{

// 私有=============

protected:

ros::NodeHandle nh_; // 节点实例

// 节点实例必须先被创建 NodeHandle instance

actionlib::SimpleActionServer as_; // 行动服务器,输入自定义的模板类似

std::string action_name_;// 行动名称

// 行动消息,用来发布的 反馈Feedback / 结果result

actionlib_tutorials::FibonacciFeedback Feedback_;

actionlib_tutorials::FibonacciResult result_;

// 公开==================

public:

// 类构造函数=============

FibonacciAction(std::string name) :

// 行动服务器 需要绑定 行动回调函数===FibonacciAction::executeCB====

as_(nh_,name,boost::bind(&FibonacciAction::executeCB,this,_1),false),

action_name_(name)

{

as_.start();// 启动

}

// 类析构函数========

~FibonacciAction(void)

{

}

// 行动回调函数=========

void executeCB(const actionlib_tutorials::FibonacciGoalConstPtr &goal)

{

ros::Rate r(1);// 频率

bool success = true;// 标志

/* the seeds for the fibonacci sequence */

Feedback_.sequence.clear();// 结果以及反馈

Feedback_.sequence.push_back(0); // 斐波那契数列

Feedback_.sequence.push_back(1);

ROS_INFO(“%s: Executing,creating fibonacci sequence of order %i with seeds %i,%i”,action_name_.c_str(),goal->order,Feedback_.sequence[0],Feedback_.sequence[1]);

/* start executing the action */

for(int i=1; iorder; i++)// order 为序列数量

{

/* check that preempt has not been requested by the client */

if (as_.isPreemptRequested() || !ros::ok())

{

ROS_INFO(“%s: Preempted”,action_name_.c_str());

/* set the action state to preempted */

as_.setPreempted();

success = false;

break;

}

// 产生后一个数

Feedback_.sequence.push_back(Feedback_.sequence[i] + Feedback_.sequence[i-1]);

/* publish the Feedback */

as_.publishFeedback(Feedback_);// 发布

/* this sleep is not necessary,however,the sequence is computed at 1 Hz for demonstration purposes */

r.sleep();

}

if(success)

{

// 最终结果

result_.sequence = Feedback_.sequence;

ROS_INFO(“%s: Succeeded”,action_name_.c_str());

/* set the action state to succeeded */

as_.setSucceeded(result_);

}

}

};

int main(int argc,char** argv)

{

ros::init(argc,”fibonacci server”);

FibonacciAction fibonacci(“fibonacci”);

ros::spin();

return 0;

}

2. 行动客户端 类似 服务消费者

#include

#include // action 客户端

#include // action 状态

#include // 自定义行动类型

int main (int argc,”fibonacci client”);

/* create the action client

“true” causes the client to spin its own thread */

// action 客户端 =====

actionlib::SimpleActionClient ac(“fibonacci”,true);

ROS_INFO(“Waiting for action server to start.”);

/* will be waiting for infinite time */

ac.waitForServer(); // 等待 行动服务器启动

ROS_INFO(“Action server started,sending goal.”);

// 发布任务目标 产生20个数量的 斐波那契数列序列

actionlib_tutorials::FibonacciGoal goal;

goal.order = 20;

ac.sendGoal(goal);// 发给 行动服务器=====

// 等待 行动 执行结果

bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));

if (finished_before_timeout)

{

actionlib::SimpleClientGoalState state = ac.getState();// 状态

ROS_INFO(“Action finished: %s”,state.toString().c_str());

}

else

ROS_INFO(“Action doesnot finish before the time out.”);

return 0;

}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)

project(actionlib_tutorials)

# add_compile_options(-std=c++11)

# 找到包依赖

find_package(catkin required COMPONENTS

actionlib

actionlib_msgs

message_generation

roscpp

rospy

std_msgs

)

## 行动自定义文件

add_action_files(

DIRECTORY action

FILES Fibonacci.action

)

## 生成行动类型 头文件

generate_messages(

DEPENDENCIES actionlib_msgs std_msgs

)

## 包依赖

catkin_package(

INCLUDE_DIRS include

LIBRARIES actionlib_tutorials

CATKIN_DEPENDS actionlib actionlib_msgs message_generation roscpp rospy std_msgs

DEPENDS system_lib

)

## 包含

include_directories(

# include

${catkin_INCLUDE_DIRS}

)

## 编译 连接

add_executable(fibonacci_server src/fibonacci_server.cpp)

add_executable(fibonacci_client src/fibonacci_client.cpp)

target_link_libraries(fibonacci_server ${catkin_LIBRARIES})

target_link_libraries(fibonacci_client ${catkin_LIBRARIES})

add_dependencies(fibonacci_server ${actionlib_tutorials_EXPORTED_TARGETS})

add_dependencies(fibonacci_client ${actionlib_tutorials_EXPORTED_TARGETS})

3. 参数服务器 parameter_server

#include

#include // 动态参数 调整

#include // 自定义的 配置参数列表

// cfg/parameter_server_tutorials.cfg===========

/*

# coding:utf-8

#!/usr/bin/env python

PACKAGE = “parameter_server_tutorials” # 包名

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()# 参数生成器

# 参数列表 ====================

gen.add(“BOOL_PARAM”,bool_t,”A Boolean parameter”,True) # BOOL量类型参数

gen.add(“INT_PARAM”,int_t,”An Integer Parameter”,1,100) # 整形量参数

gen.add(“DOUBLE_PARAM”,double_t,”A Double Parameter”,0.01,1)# 浮点型变量参数

gen.add(“STR_PARAM”,str_t,”A String parameter”,”Dynamic Reconfigure”) # 字符串类型变量参数

# 自定义 枚举常量 类型 ==========

size_enum = gen.enum([ gen.const(“Low”,”Low : 0″),

gen.const(“Medium”,”Medium : 1″),

gen