【ROS】ROS1编程速览

【ROS】郭老二博文之：ROS目录

1、简述

很多项目已经转向ROS2，本人作为ROS小白从ROS1开始学起，但是不会深入学习ROS1，只一带而过。
下面只了解一些ROS1中的概念和基本编程接口。

ROS1中有两种通信模式：话题模式和服务模式，区别如下

2、话题模式

2.1 查看话题和消息

1）列出所有话题：rostopic list

~/workspace/ros$ rostopic list 
/rosout
/rosout_agg
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose

2）查看话题详细信息：rostopic info
和下面发布编程相关的话题：

~/workspace/ros$ rostopic info /turtle1/cmd_vel 
Type: geometry_msgs/Twist

Subscribers: 
 * /turtlesim (http://laoer:43127/)

和下面订阅编程相关的话题：

$ rostopic info /turtle1/pose 
Type: turtlesim/Pose

Publishers: 
 * /turtlesim (http://laoer:43127/)

3）查看消息详细信息：rosmsg show
和下面发布编程相关的消息：

~/workspace/ros$ rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

对应程序中的代码：

ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
geometry_msgs::Twist vel_msg;
vel_msg.linear.x = 0.5;
vel_msg.angular.z = 0.2;
turtle_vel_pub.publish(vel_msg);

和下面订阅编程相关的消息：

~/workspace/ros$ rosmsg show turtlesim/Pose 
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);
}
ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);

2.2、发布者

1）进入ROS1的工程目录

cd ~/workspace/ros/src/

2）创建功能包

~/workspace/ros/src$ catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim

输出信息如下：

Created file learning_topic/CMakeLists.txt
Created file learning_topic/package.xml
Created folder learning_topic/include/learning_topic
Created folder learning_topic/src
Successfully created files in /home/laoer/workspace/ros/src/learning_topic. Please adjust the values in package.xml.

3）编辑源码

cd ~/workspace/ros
vi src/learning_topic/src/velocity_publisher.cpp

源码如下：

#include 
#include 

int main(int argc, char **argv)
{
	// ROS节点初始化
	ros::init(argc, argv, "velocity_publisher");

	// 创建节点句柄
	ros::NodeHandle n;

	// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
	ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

	// 设置循环的频率
	ros::Rate loop_rate(10);

	int count = 0;
	while (ros::ok())
	{
	    // 初始化geometry_msgs::Twist类型的消息
		geometry_msgs::Twist vel_msg;
		vel_msg.linear.x = 0.5;
		vel_msg.angular.z = 0.2;

	    // 发布消息
		turtle_vel_pub.publish(vel_msg);
		ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
				vel_msg.linear.x, vel_msg.angular.z);

	    // 按照循环频率延时
	    loop_rate.sleep();
	}

	return 0;
}

4）修改CMakeLists.txt

~/workspace/ros$ vi src/learning_topic/CMakeLists.txt

添加需要编译生成的可执行文件规则和连接库
在## Build ##下添加

add_executable(velocity_publisher src/velocity_publisher.cpp)
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})

5）编译

catkin_make

编译输出：

Base path: /home/laoer/workspace/ros
……
-- +++ processing catkin package: 'learning_topic'
-- ==> add_subdirectory(learning_topic)
……
[100%] Built target velocity_publisher

6）运行
在终端1中启动节点管理器：

roscore

在终端2中启动小乌龟节点：

rosrun turtlesim turtlesim_node

在终端3中先配置环境变量：

~/workspace/ros$ source devel/setup.bash

再启动发布者

~/workspace/ros$ rosrun learning_topic velocity_publisher 

输出信息如下：

[ INFO] [1684324440.780789684]: Publsh turtle velocity command[0.50 m/s, 0.20 rad/s]
[ INFO] [1684324440.880933087]: Publsh turtle velocity command[0.50 m/s, 0.20 rad/s]
[ INFO] [1684324440.980945586]: Publsh turtle velocity command[0.50 m/s, 0.20 rad/s]

小乌龟将做圆形运动

2.3、订阅者

1）进入ROS1的工程目录

cd ~/workspace/ros/src/

2）创建功能包
已经创建过learning_topic，不需要再创建

~/workspace/ros/src$ catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim

如果重复创建，将会输出如下信息，提示文件已存在：

catkin_create_pkg: error: File exists: /home/laoer/workspace/ros/src/learning_topic/CMakeLists.txt

3）编辑源码

cd ~/workspace/ros
vi src/learning_topic/src/pose_subscriber.cpp

#include 
#include "turtlesim/Pose.h"

// 接收到订阅的消息后，会进入消息回调函数
void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "pose_subscriber");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

4）修改CMakeLists.txt

~/workspace/ros$ vi src/learning_topic/CMakeLists.txt

添加需要编译生成的可执行文件规则和连接库
在## Build ##下添加

add_executable(pose_subscriber src/pose_subscriber.cpp)
target_link_libraries(pose_subscriber ${catkin_LIBRARIES})

5）编译

catkin_make

编译输出：

Base path: /home/laoer/workspace/ros
……
[100%] Built target pose_subscriber

6）运行
在终端1中启动节点管理器：

roscore

在终端2中启动小乌龟节点：

rosrun turtlesim turtlesim_node

在终端3中先配置环境变量：

~/workspace/ros$ source devel/setup.bash

再启动订阅者

~/workspace/ros$ rosrun learning_topic pose_subscriber 

输出信息如下：

[ INFO] [1684373591.045027979]: Turtle pose: x:7.060666, y:10.029119
[ INFO] [1684373591.061111496]: Turtle pose: x:7.060666, y:10.029119

此时位置信息没有变化，可以运行2.1中发布者来改变位置信息
在终端4中先配置环境变量：

~/workspace/ros$ source devel/setup.bash

再启动发布者

~/workspace/ros$ rosrun learning_topic velocity_publisher 

在终端3中可以看到位置信息已改变

[ INFO] [1684373649.013267506]: Turtle pose: x:3.651149, y:9.681684
[ INFO] [1684373649.028763094]: Turtle pose: x:3.645919, y:9.675630

2.4 节点结构

可以使用rqt_graph来打印当前的节点结构

2.5、话题消息

上面发布者和订阅者的示例中，都使用的是已经定义好的信息，如：

~/workspace/ros$ rosmsg show turtlesim/Pose 
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

下面演示如何自定义话题信息
1）定义msg文件
进入话题工程目录

~/workspace/ros$ cd src/learning_topic/

创建保存消息文件的文件夹

~/workspace/ros/src/learning_topic$ mkdir msg

创建消息文件Person.msg

~/workspace/ros/src/learning_topic$ vi Person.msg 

内容如下：

string name
uint8  age
uint8  sex

uint8 unknown = 0
uint8 male    = 1
uint8 female  = 2

2）在package.xml中添加功能包依赖
在编译自定义消息时，需要依赖消息生成的依赖包：message_generation
在运行自定义消息时，需要依赖运行的依赖包：message_runtime
修改package.xml，将关于message_generation和message_runtime注释打开即可

message_generation
message_runtime

3）C++代码
订阅者相关代码

~/workspace/ros$ vi src/learning_topic/src/person_subscriber.cpp 

#include 
#include "learning_topic/Person.h"

// 接收到订阅的消息后，会进入消息回调函数
void personInfoCallback(const learning_topic::Person::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Subcribe Person Info: name:%s  age:%d  sex:%d", 
			 msg->name.c_str(), msg->age, msg->sex);
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "person_subscriber");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallback
    ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

发布者相关代码：

~/workspace/ros$ vi src/learning_topic/src/person_publisher.cpp 

#include 
#include "learning_topic/Person.h"

int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "person_publisher");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10
    ros::Publisher person_info_pub = n.advertise<learning_topic::Person>("/person_info", 10);

    // 设置循环的频率
    ros::Rate loop_rate(1);

    int count = 0;
    while (ros::ok())
    {
        // 初始化learning_topic::Person类型的消息
    	learning_topic::Person person_msg;
		person_msg.name = "Tom";
		person_msg.age  = 18;
		person_msg.sex  = learning_topic::Person::male;

        // 发布消息
		person_info_pub.publish(person_msg);

       	ROS_INFO("Publish Person Info: name:%s  age:%d  sex:%d", 
				  person_msg.name.c_str(), person_msg.age, person_msg.sex);

        // 按照循环频率延时
        loop_rate.sleep();
    }

    return 0;
}

4）在CMakeLists.txt中添加编译选项

find_package(catkin REQUIRED COMPONENTS
……
  message_generation
)
add_message_files(
  FILES
  Person.msg
)

generate_messages(
  DEPENDENCIES
  std_msgs
)
catkin_package(
   CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs turtlesim message_runtime
)

add_executable(person_publisher src/person_publisher.cpp)
target_link_libraries(person_publisher ${catkin_LIBRARIES})
add_dependencies(person_publisher ${PROJECT_NAME}_generate_messages_cpp)

add_executable(person_subscriber src/person_subscriber.cpp)
target_link_libraries(person_subscriber ${catkin_LIBRARIES})
add_dependencies(person_subscriber ${PROJECT_NAME}_generate_messages_cpp)

5）编译

~/workspace/ros$ catkin_make

……
[ 46%] Generating Javascript code from learning_topic/Person.msg
[ 93%] Built target person_publisher
[100%] Built target person_subscriber

6）运行
在终端1中启动节点管理器：

~/workspace/ros$ roscore

在终端2中启动订阅者：

~/workspace/ros$ source devel/setup.bash
~/workspace/ros$ rosrun learning_topic person_subscriber 

在终端3中启动发布者：

~/workspace/ros$ source devel/setup.bash
~/workspace/ros$ rosrun learning_topic person_publisher 

终端2打印订阅者收到的信息：

[ INFO] [1684377584.196850906]: Subcribe Person Info: name:Tom  age:18  sex:1
[ INFO] [1684377585.196714423]: Subcribe Person Info: name:Tom  age:18  sex:1

终端3打印发布者发送的信息：

[ INFO] [1684377583.196183979]: Publish Person Info: name:Tom  age:18  sex:1
[ INFO] [1684377584.196358219]: Publish Person Info: name:Tom  age:18  sex:1

7）节点图很简单，如下：

3、服务模式

3.1 查看服务和数据

1）列出所有的服务

~/workspace/ros$ rosservice list
/clear
……
/spawn
……

2）查看服务（spawn产卵，即在界面中生成一个新的小乌龟）

~/workspace/ros$ rosservice info /spawn 
Node: /turtlesim
URI: rosrpc://lesen-System-Product-Name:54203
Type: turtlesim/Spawn
Args: x y theta name

3）调用服务命令 rosservice call
下面的客户端示例的功能和这个命令类似，调用服务多产生几个小乌龟

~/workspace/ros$ rosservice call /spawn 3 7 8 haha

3.2、客户端

~/workspace/ros$ rosservice call /spawn 3 3 4 hah
1）进入ROS1的工程目录

cd ~/workspace/ros/src/

2）创建功能包

~/workspace/ros/src$ catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim

3）编辑源码

~/workspace/ros/src/learning_service/src$ vi turtle_spawn.cpp 

#include 
#include 

int main(int argc, char** argv)
{
    // 初始化ROS节点
	ros::init(argc, argv, "turtle_spawn");

    // 创建节点句柄
	ros::NodeHandle node;

    // 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
	ros::service::waitForService("/spawn");
	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");

    // 初始化turtlesim::Spawn的请求数据
	turtlesim::Spawn srv;
	srv.request.x = 2.0;
	srv.request.y = 2.0;
	srv.request.name = "turtle2";

    // 请求服务调用
	ROS_INFO("Call service to spwan turtle[x:%0.6f, y:%0.6f, name:%s]", 
			 srv.request.x, srv.request.y, srv.request.name.c_str());

	add_turtle.call(srv);

	// 显示服务调用结果
	ROS_INFO("Spwan turtle successfully [name:%s]", srv.response.name.c_str());

	return 0;
};

4）修改CMakeLists.txt
添加需要编译生成的可执行文件规则和连接库
在## Build ##下添加

add_executable(turtle_spawn src/turtle_spawn.cpp)
target_link_libraries(turtle_spawn ${catkin_LIBRARIES})

5）编译

~/workspace/ros$ catkin_make

编译输出信息

[ 100%] Built target turtle_spawn

6）运行
在终端1中启动节点管理器：

~/workspace/ros$ roscore

在终端2中启动小乌龟：

 rosrun turtlesim turtlesim_node 

在终端3中启动客户端

~/workspace/ros$ source devel/setup.bash
~/workspace/ros$ rosrun learning_service turtle_spawn 

输出信息如下：

[ INFO] [1684380205.381761081]: Call service to spwan turtle[x:2.000000, y:2.000000, name:turtle2]
[ INFO] [1684380205.401406453]: Spwan turtle successfully [name:turtle2]

在小乌龟界面将会出现两个小乌龟

3.3、服务端

1）进入ROS1的工程目录

cd ~/workspace/ros/src/

2）创建功能包
学习服务模式的功能包已经创建，这里不需要再运行

~/workspace/ros/src$ catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim

3）编辑源码

~/workspace/ros/src/learning_service/src$ vi turtle_command_server.cpp

#include 
#include 
#include 

ros::Publisher turtle_vel_pub;
bool pubCommand = false;

// service回调函数，输入参数req，输出参数res
bool commandCallback(std_srvs::Trigger::Request  &req,
         			std_srvs::Trigger::Response &res)
{
	pubCommand = !pubCommand;

    // 显示请求数据
    ROS_INFO("Publish turtle velocity command [%s]", pubCommand==true?"Yes":"No");

	// 设置反馈数据
	res.success = true;
	res.message = "Change turtle command state!"

    return true;
}

int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "turtle_command_server");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个名为/turtle_command的server，注册回调函数commandCallback
    ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);

	// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
	turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

    // 循环等待回调函数
    ROS_INFO("Ready to receive turtle command.");

	// 设置循环的频率
	ros::Rate loop_rate(10);

	while(ros::ok())
	{
		// 查看一次回调函数队列
    	ros::spinOnce();
		
		// 如果标志为true，则发布速度指令
		if(pubCommand)
		{
			geometry_msgs::Twist vel_msg;
			vel_msg.linear.x = 0.5;
			vel_msg.angular.z = 0.2;
			turtle_vel_pub.publish(vel_msg);
		}

		//按照循环频率延时
	    loop_rate.sleep();
	}

    return 0;
}

4）修改CMakeLists.txt
添加需要编译生成的可执行文件规则和连接库
在## Build ##下添加

add_executable(turtle_command_server src/turtle_command_server.cpp)
target_link_libraries(turtle_command_server ${catkin_LIBRARIES})

5）编译

~/workspace/ros$ catkin_make

编译输出信息

……
[ 100%] Built target turtle_command_server

6）运行
在终端1中启动节点管理器：

~/workspace/ros$ roscore

在终端2中启动小乌龟：

 rosrun turtlesim turtlesim_node 

在终端3中启动服务器

~/workspace/ros$ source devel/setup.bash
~/workspace/ros$ rosrun learning_service turtle_spawn

在终端4中使用rosservice call来调用服务

rosservice call /turtle_command

终端3中的打印信息

[ INFO] [1684395192.110521550]: Ready to receive turtle command.
[ INFO] [1684395203.911182038]: Publish turtle velocity command [Yes]
[ INFO] [1684395245.311201634]: Publish turtle velocity command [No]

流程说明：
执行命令后将调用服务turtle_command，然后执行对应的回调函数，回调函数只控制pubCommand的真假值，主循环会根据pubCommand的真假来决定是否发布消息

注：小乌龟的运动是最终还是通过发布者Publisher，发布名为/turtle1/cmd_vel的主题topic，消息类型为控制小乌龟运动的消息geometry_msgs::Twist

节点关系如下：

3.4、自定义服务数据

3.4.1 定义服务数据

创建描述服务数据的srv文件
1）进入包目录
进入~/workspace/ros/src/learning_service中

cd ~/workspace/ros/src/learning_service

2）创建保存服务数据文件的目录srv

~/workspace/ros/src/learning_service$  mkdir srv

3）编辑服务数据文件Person.srv

~/workspace/ros/src/learning_service$ vi srv/Person.srv 

内容如下，注意“—”不是省略号，是数据文件格式的一部分

string name
uint8  age
uint8  sex

uint8 unknown = 0
uint8 male    = 1
uint8 female  = 2

---
string result

4）在package.xml中添加功能包依赖
（和话题模式的自定义消息类似）

 message_generation
message_runtime

5）在CMakeLists.txt中添加编译选项
（和话题模式的自定义消息类似）

find_package(catkin REQUIRED COMPONENTS
……
  message_generation
)
add_service_files(
  FILES
  Person.srv
)

generate_messages(
  DEPENDENCIES
  std_msgs
)

catkin_package(
   CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs turtlesim message_runtime
)

3.4.2 服务端

编辑服务端代码

~/workspace/ros/src/learning_service$ vi src/person_server.cpp

#include 
#include "learning_service/Person.h"

// service回调函数，输入参数req，输出参数res
bool personCallback(learning_service::Person::Request  &req,
         			learning_service::Person::Response &res)
{
    // 显示请求数据
    ROS_INFO("Person: name:%s  age:%d  sex:%d", req.name.c_str(), req.age, req.sex);

	// 设置反馈数据
	res.result = "OK";

    return true;
}

int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "person_server");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个名为/show_person的server，注册回调函数personCallback
    ros::ServiceServer person_service = n.advertiseService("/show_person", personCallback);

    // 循环等待回调函数
    ROS_INFO("Ready to show person informtion.");
    ros::spin();

    return 0;
}

3.4.3 客户端

编辑客户端代码

~/workspace/ros/src/learning_service$ vi src/person_client.cpp

#include 
#include "learning_service/Person.h"

int main(int argc, char** argv)
{
    // 初始化ROS节点
	ros::init(argc, argv, "person_client");

    // 创建节点句柄
	ros::NodeHandle node;

    // 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
	ros::service::waitForService("/show_person");
	ros::ServiceClient person_client = node.serviceClient<learning_service::Person>("/show_person");

    // 初始化learning_service::Person的请求数据
	learning_service::Person srv;
	srv.request.name = "Tom";
	srv.request.age  = 20;
	srv.request.sex  = learning_service::Person::Request::male;

    // 请求服务调用
	ROS_INFO("Call service to show person[name:%s, age:%d, sex:%d]", 
			 srv.request.name.c_str(), srv.request.age, srv.request.sex);

	person_client.call(srv);

	// 显示服务调用结果
	ROS_INFO("Show person result : %s", srv.response.result.c_str());

	return 0;
};

3.4.4 编译

1）在CMakeLists.txt中添加服务端、客户端相关的编译规则

add_executable(person_server src/person_server.cpp)
target_link_libraries(person_server ${catkin_LIBRARIES})
add_dependencies(person_server ${PROJECT_NAME}_gencpp)

add_executable(person_client src/person_client.cpp)
target_link_libraries(person_client ${catkin_LIBRARIES})
add_dependencies(person_client ${PROJECT_NAME}_gencpp)

2）编译

~/workspace/ros$ catkin_make

编译输出

[ 80%] Built target person_server
[100%] Built target person_client

3.4.5 运行

在终端1中启动服务器：

~/workspace/ros$ rosrun learning_service person_server

在终端2中多次执行客户端，命令及打印信息如下：

~/workspace/ros$ rosrun learning_service person_client 
[ INFO] [1684397359.830399671]: Call service to show person[name:Tom, age:20, sex:1]
[ INFO] [1684397359.831655045]: Show person result : OK
~/workspace/ros$ rosrun learning_service person_client 
[ INFO] [1684397360.439275903]: Call service to show person[name:Tom, age:20, sex:1]
[ INFO] [1684397360.440764700]: Show person result : OK

终端1收到的信息如下：

[ INFO] [1684397334.962962627]: Ready to show person informtion.
[ INFO] [1684397351.231455028]: Person: name:Tom  age:20  sex:1
[ INFO] [1684397358.320771056]: Person: name:Tom  age:20  sex:1
[ INFO] [1684397359.144605775]: Person: name:Tom  age:20  sex:1

4、参数

ROS 参 数( parameters )机制用于获取任何节点保存在参数服务器中的信息，类似全局变量。
方法是使用集中参数服务器( parameter server )维护一个变量集的值,包括整数、浮点数、字符串以及其他数据类型,每一个变量用一个较短的字符串标识 。由于允许节点主动查询其感兴趣的参数的值,它们适用于配置那些不会随时间频繁变更的信息。

4.1 rosparam

操作参数的命令

• rosparam set，设置参数
• rosparam get，获取参数
• rosparam load，从文件中加载参数
• rosparam dump，将参数写入文件
• rosparam delete，删除参数
• rosparam list，列出所有的参数

示例1：列出所有的参数

~/workspace/ros$ rosparam list

输出：

/rosdistro
/roslaunch/uris/host_lesen_system_product_name__46275
/rosversion
/run_id
/turtlesim/background_b
/turtlesim/background_g
/turtlesim/background_r

示例2：查看某个参数的值

~/workspace/ros$ rosparam get /turtlesim/background_b

输出：

255

4.2 自定义参数文件

1）进入ROS1的工程目录

cd ~/workspace/ros/src/

2）创建功能包

~/workspace/ros/src$ catkin_create_pkg learning_parameter roscpp rospy std_msgs geometry_msgs turtlesim

3）进入功能包目录

cd ~/workspace/ros/src/learning_parameter

4）创建保存参数文件的目录

mkdir config

5）编辑参数文件

~/workspace/ros/src/learning_parameter$ vi  config/turtle_param.yaml 

background_b: 255
background_g: 86
background_r: 69
rosdistro: 'melodic'
roslaunch:
  uris: {host_hcx_vpc__43763: 'http://hcx-vpc:43763/'}
rosversion: '1.14.3'
run_id: 077058de-a38b-11e9-818b-000c29d22e4d

4.3 参数操作示例

1）编辑源码

~/workspace/ros/src/learning_parameter$ vi src/parameter_config.cpp 

注意：古月居的示例源码中background_r需要改为/turtlesim/background_r否则小乌龟不会改变背景色

 ros::param::get("/turtlesim/background_r", red);

#include 
#include 
#include 

int main(int argc, char **argv)
{
	int red, green, blue;

    // ROS节点初始化
    ros::init(argc, argv, "parameter_config");

    // 创建节点句柄
    ros::NodeHandle node;

    // 读取背景颜色参数
	ros::param::get("/turtlesim/background_r", red);
	ros::param::get("/turtlesim/background_g", green);
	ros::param::get("/turtlesim/background_b", blue);

	ROS_INFO("Get Backgroud Color[%d, %d, %d]", red, green, blue);

	// 设置背景颜色参数
	ros::param::set("/turtlesim/background_r", 255);
	ros::param::set("/turtlesim/background_g", 255);
	ros::param::set("/turtlesim/background_b", 255);

	ROS_INFO("Set Backgroud Color[255, 255, 255]");

    // 读取背景颜色参数
	ros::param::get("/turtlesim/background_r", red);
	ros::param::get("/turtlesim/background_g", green);
	ros::param::get("/turtlesim/background_b", blue);

	ROS_INFO("Re-get Backgroud Color[%d, %d, %d]", red, green, blue);

	// 调用服务，刷新背景颜色
	ros::service::waitForService("/clear");
	ros::ServiceClient clear_background = node.serviceClient<std_srvs::Empty>("/clear");
	std_srvs::Empty srv;
	clear_background.call(srv);
	
	sleep(1);

    return 0;
}

2）编译

~/workspace/ros$ catkin_make

输出：

[  100%] Built target parameter_config

3） 运行
在终端1中启动节点管理器

roscore

在终端2中启动小乌龟

rosrun turtlesim turtlesim_node

在终端3中启动参数测试程序

~/workspace/ros$ source devel/setup.bash
~/workspace/ros$ rosrun learning_parameter parameter_config

打印输出：

[ INFO] [1684400198.204756094]: Get Backgroud Color[0, 14, -2147483648]
[ INFO] [1684400198.206004077]: Set Backgroud Color[255, 255, 255]
[ INFO] [1684400198.206633529]: Re-get Backgroud Color[255, 255, 255]
[ INFO] [1684400198.206846322]: waitForService: Service [/clear] has not been advertised, waiting...

5、坐标

5.1 命令示例

1）安装ros-melodic-turtle-tf
以小乌龟的坐标变换为例，需要先安装一个软件

$ sudo apt install ros-melodic-turtle-tf

2）终端1运行节点管理器

$ roscore

3）终端2运行两个小乌龟自动跟随的demo

$ roslaunch turtle_tf turtle_tf_demo.launch 

4）终端3运行键盘控制节点

$ rosrun turtlesim turtle_teleop_key

5）坐标树查看

$ rosrun tf view_frames 

输出信息

Listening to /tf for 5.0 seconds
Done Listening
dot - graphviz version 2.40.1 (20161225.0304)

Detected dot version 2.40
frames.pdf generated

在当前目录下生成文件frames.pdf

6）坐标变换

$ rosrun tf tf_echo turtle1 turtle2

输出

At time 1684405017.779
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.979, 0.205]
            in RPY (radian) [-0.000, -0.000, 2.728]
            in RPY (degree) [-0.000, -0.000, 156.306]
At time 1684405018.514
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.979, 0.205]
            in RPY (radian) [-0.000, -0.000, 2.728]
            in RPY (degree) [-0.000, -0.000, 156.306]
……

7）rviz可视化工具查看坐标

$ rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz

5.2 代码示例

1）进入ROS1的工程目录

cd ~/workspace/ros/src/

2）创建功能包

~/workspace/ros/src$ catkin_create_pkg learning_tf roscpp rospy tf turtlesim

3）编辑源码
tf广播器：

~/workspace/ros/src/learning_tf/src$ vi turtle_tf_broadcaster.cpp

#include 
#include 
#include 

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
	// 创建tf的广播器
	static tf::TransformBroadcaster br;

	// 初始化tf数据
	tf::Transform transform;
	transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
	tf::Quaternion q;
	q.setRPY(0, 0, msg->theta);
	transform.setRotation(q);

	// 广播world与海龟坐标系之间的tf数据
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv)
{
    // 初始化ROS节点
	ros::init(argc, argv, "my_tf_broadcaster");

	// 输入参数作为海龟的名字
	if (argc != 2)
	{
		ROS_ERROR("need turtle name as argument"); 
		return -1;
	}

	turtle_name = argv[1];

	// 订阅海龟的位姿话题
	ros::NodeHandle node;
	ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

    // 循环等待回调函数
	ros::spin();

	return 0;
};

tf监听器：

~/workspace/ros/src/learning_tf/src$ cat turtle_tf_listener.cpp

#include 
#include 
#include 
#include 

int main(int argc, char** argv)
{
	// 初始化ROS节点
	ros::init(argc, argv, "my_tf_listener");

    // 创建节点句柄
	ros::NodeHandle node;

	// 请求产生turtle2
	ros::service::waitForService("/spawn");
	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
	turtlesim::Spawn srv;
	add_turtle.call(srv);

	// 创建发布turtle2速度控制指令的发布者
	ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);

	// 创建tf的监听器
	tf::TransformListener listener;

	ros::Rate rate(10.0);
	while (node.ok())
	{
		// 获取turtle1与turtle2坐标系之间的tf数据
		tf::StampedTransform transform;
		try
		{
			listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
		}
		catch (tf::TransformException &ex) 
		{
			ROS_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}

		// 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
		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;
};

4）配置CMakeLists.txt

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})

5）编译

cd ~/workspace/ros
~/workspace/ros$ catkin_make

6）运行
终端1运行节点管理器

$ roscore

终端2运行小乌龟

$ rosrun turtlesim turtlesim_node

终端3运行广播1

$ rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1

终端4运行广播2

$ rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2

终端5运行监听

$ rosrun learning_tf turtle_tf_listener

终端6运行键盘控制

$ rosrun turtlesim turtle_teleop_key

7）效果
效果和命令示例一样

6、launch批量启动节点

6.1 说明

roslaunch可以实现自动启动ROS Master、通过XML文件实现多个节点的配置和启动

6.2 创建launch功能包

1）进入ROS1的工程目录

cd ~/workspace/ros/src/

2）创建功能包

~/workspace/ros/src$	catkin_create_pkg learning_launch

6.3 launch文件格式

roslaunch需要加载XML文件来读取各个节点，格式如下：

    "learning_topic" type="person_subscriber" name="talker" output="screen" />
    "learning_topic" type="person_publisher" name="listener" output="screen" /> 

1）launch：根元素
2）node：节点
pkg：节点所在的功能包名称
type：可执行文件名称
name：执行程序运行时的名称
output：打印输出
3）include：launch文件嵌套

6.4 执行命令

~/workspace/ros$ roslaunch learning_launch simple.launch 

输出：

……
NODES
/
listener (learning_topic/person_publisher)
talker (learning_topic/person_subscriber)
ROS_MASTER_URI=http://localhost:11311
process[talker-1]: started with pid [6649]
process[listener-2]: started with pid [6655]
[ INFO] [1684408582.094373243]: Publish Person Info: name:Tom  age:18  sex:1
[ INFO] [1684408583.094561611]: Publish Person Info: name:Tom  age:18  sex:1
[ INFO] [1684408583.095011334]: Subcribe Person Info: name:Tom  age:18  sex:1
[ INFO] [1684408584.094552704]: Publish Person Info: name:Tom  age:18  sex:1
[ INFO] [1684408584.094849697]: Subcribe Person Info: name:Tom  age:18  sex:1

7、可视化工具

7.1 rqt：rqt系列工具集

7.1 rqt_console：日志输出工具

7.2 rqt_graph：节点图

7.3 rqt_plot：数据绘制

7.4 rqt_image_view：图像渲染工具

不知道怎么安装

7.5 rqt_bag

7.6 Rviz：三维可视化工具

7.7 Gazebo：三维物理仿真平台

7.8 rqt_shell终端

rqt_shell

7.9 rqt_multiplot：可视化多个2D图中的数值

7.10 rqt_logger_level：配置 ROS 节点的日志级别

7.11 rqt_paramedit：编辑参数服务

7.12 rqt_py_trees：可视化py_trees行为树

行为树：用来让机器人实现复杂任务

7.13 rqt_dep：查看ROS包依赖