Learn ROS

一、消息

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 data.c_str());

}

int main(int argc, char **argv)

{

ros::init(argc, argv, "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, char **argv)

{

ros::init(argc, argv, "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, char **argv)

{

ros::init(argc, argv, "example3_b");

ros::NodeHandle n;

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

ros::Subscriber sub = n.subscribe("chapter2_tutorials/message", 100, 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, char **argv)

{

ros::init(argc, argv, "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, char **argv)

{

ros::init(argc, argv, "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. 发布行动 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;// 标志

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]);

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

{

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

{

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

as_.setPreempted();

success = false;

break;

}

// 产生后一个数

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

as_.publishFeedback(feedback_);// 发布

r.sleep();

}

if(success)

{

// 最终结果

result_.sequence = feedback_.sequence;

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

as_.setSucceeded(result_);

}

}

};

int main(int argc, char** argv)

{

ros::init(argc, argv, "fibonacci server");

FibonacciAction fibonacci("fibonacci");

ros::spin();

return 0;

}

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

#include

#include // action 客户端

#include // action 状态

#include // 自定义行动类型

int main (int argc, char **argv)

{

ros::init(argc, argv, "fibonacci client");

// action 客户端 =====

actionlib::SimpleActionClient ac("fibonacci", true);

ROS_INFO("Waiting for action server to start.");

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===========

// 参数改变后 的回调函数，parameter_server_Config 为参数头

void callback(parameter_server_tutorials::parameter_server_Config &config, uint32_t level)

{

ROS_INFO("Reconfigure Request: %s %d %f %s %d",

config.BOOL_PARAM?"True":"False",

config.INT_PARAM,

config.DOUBLE_PARAM,

config.STR_PARAM.c_str(),

config.SIZE);

}

int main(int argc, char **argv)

{

ros::init(argc, argv, "parameter_server_tutorials");

dynamic_reconfigure::Server server;// 参数服务器

dynamic_reconfigure::Server::CallbackType f;// 参数改变 回调类型

// 绑定回调函数

f = boost::bind(&callback, _1, _2);

// 参数服务器设置 回调器

server.setCallback(f);

ROS_INFO("Spinning");

ros::spin();// 启动

return 0;

}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)

project(parameter_server_tutorials)

# add_compile_options(-std=c++11)

# 找到包

find_package(catkin REQUIRED COMPONENTS

roscpp

std_msgs

message_generation

dynamic_reconfigure

)

# 动态参数配置文件

generate_dynamic_reconfigure_options(

cfg/parameter_server_tutorials.cfg

)

# 依赖

catkin_package(

CATKIN_DEPENDS message_runtime

)

# 包含

include_directories(

include

${catkin_INCLUDE_DIRS}

)

# 生成可执行文件

add_executable(parameter_server_tutorials src/parameter_server_tutorials.cpp)

add_dependencies(parameter_server_tutorials parameter_server_tutorials_gencfg)

target_link_libraries(parameter_server_tutorials ${catkin_LIBRARIES})

4. 坐标变换发布 tf_broadcaster

#include

#include // 坐标变换发布/广播

#include // 小乌龟位置类型

std::string turtle_name;

// 小乌龟 位姿 话题 回调函数 =======

void poseCallback(const turtlesim::PoseConstPtr& msg)

{

static tf::TransformBroadcaster br;// 坐标变换广播

tf::Transform transform;// 坐标变换

transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );// 坐标位置

tf::Quaternion q;// 位姿四元素

q.setRPY(0, 0, msg->theta);// 按照 rpy 姿态向量形式设置 平面上只有 绕Z轴的旋转 偏航角

transform.setRotation(q);// 姿态

// 广播位姿变换消息=====

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

}

int main(int argc, char** argv)

{

ros::init(argc, argv, "tf_broadcaster");

if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};

turtle_name = argv[1];

ros::NodeHandle node;

// 订阅小乌龟 位姿 话题数据 绑定回调函数 poseCallback

ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

ros::spin();

return 0;

}

5. 坐标变换监听 tf_listener

#include

#include // 坐标变换监听

#include // 消息类型

#include // 生成一个小乌龟

int main(int argc, char** argv)

{

ros::init(argc, argv, "tf_listener");

ros::NodeHandle node;

ros::service::waitForService("spawn");// 等待 生成小乌龟的服务到来

ros::ServiceClient add_turtle =

node.serviceClient("spawn"); // 服务客户端

turtlesim::Spawn srv;

add_turtle.call(srv); // 调用服务

// 发布小乌龟运动指令=====

ros::Publisher turtle_vel =

node.advertise("turtle2/cmd_vel", 10);

// 左边变换监听

tf::TransformListener listener;

ros::Rate rate(10.0);

while (node.ok())

{

tf::StampedTransform transform; // 得到的坐标变换消息

try

{

// 两个小乌龟坐标变换消息 之差 左边变换??

// 有两个 坐标变换发布器 一个发布 /turtle1 一个发布 /turtle2

listener.lookupTransform("/turtle2", "/turtle1",

ros::Time(0), transform);

}

catch (tf::TransformException &ex)

{

ROS_ERROR("%s",ex.what());

ros::Duration(1.0).sleep();

continue;

}

// 根据位姿差，发布 命令 让 小乌龟2 追赶上 小乌龟1

geometry_msgs::Twist vel_msg;

// 位置差值 计算角度

vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),

transform.getOrigin().x());

// 位置直线距离，关联到速度

vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +

pow(transform.getOrigin().y(), 2));

// 发布速度命令

turtle_vel.publish(vel_msg);

rate.sleep();

}

return 0;

}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)

project(tf_tutorials)

find_package(catkin REQUIRED COMPONENTS

roscpp

rospy

tf

turtlesim

)

catkin_package()

include_directories(

# include

${catkin_INCLUDE_DIRS}

)

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)

target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)

target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

start_demo.launch