[古月ros入门21讲] 3.编程基础

最新推荐文章于 2024-09-01 09:33:44 发布

Deephome

最新推荐文章于 2024-09-01 09:33:44 发布

阅读量630

点赞数

分类专栏： ROS

本文链接：https://blog.csdn.net/muyiyushan/article/details/114368747

版权

ROS 专栏收录该内容

8 篇文章 0 订阅

订阅专栏

9 创建工作空间与功能包

工作空间Workspace是一个存放工程开发相关文件的文件夹。

src : 代码空间，source space，放置功能包
build : 编译空间，build space，编译中间文件
devel : 开发空间，development space，编译生成的头文件、可执行文件、库等
install : 安装空间，install space ，安装位置
（ros2中没有devel）

命令：
catkin_init_workspace （在src中）
catkin_make (在workspace中)

#创建工作空间
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
# 初始化工作空间
$ catkin_init_workspace
# 编译工作空间 （在workspace中）
$ cd ~/catkin_ws/
$ catkin_make
#设置环境变量
$ source devel/setup.bash
# 检查环境变量
$ echo $ROS_PACKAGE_PATH

创建功能包（package）:
catkin_create_pkg <package_name> [depend1] [depend2] [depend3]

#创建功能包 （src中）
$ cd ~/catkin_ws/src
$ catkin_create_pkg test_pkg std_msgs rospy roscpp
# 编译功能包
$ cd ~/catkin_ws
$ catkin_make
$ source ~/catkin_ws/devel/setup.bash

注：同一个工作空间下，不允许存在同名功能包。不同工作空间下，允许存在同名功能包。

总结：
catkin_init_workspace创建工作空间，catkin_create_pkg创建功能包，功能包里组织代码，工作空间里编译运行。

10 发布者Publisher的编程实现

话题模型如下图，publisher (Turtle Velocity)通过topic 发布message给subscriber (turtlesim),控制turtle的运动。
在这里插入图片描述

10.1创建功能包

$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim

10.2 创建publisher的代码（C++）

在learning_topic/src中新建velocity_publisher.cpp文件，代码如下：

/**
 * 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist
 */
 
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

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步：1）初始化节点，2）创建publisher，3）创建消息，4）发布消息

10.3 配置编译规则

打开功能包leaning_topic中的CMakeLists.txt，在build模块最后加入：

# 生成可执行文件
add_executable(velocity_publisher src/velocity_publisher.cpp)
# 添加链接库
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})

10.4 编译运行

# worksapce中编译
$ cd ~/catkin_ws
$ catkin_make
# 环境变量
$ source devel/setup.bash
#运行
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_topic velocity_publisher

为了避免每次编译后运行source命令，可修改.bashrc。

打开home目录，按“Ctrl+h”显示隐藏文件，打开.bashrc，最后一行添加“source /home/<your_name>/catkin_ws/devel/setup.bash ”

11 订阅者Subscriber编程实现

发布者是turtlesim，通过话题(/turtle1/pose)发布消息(turtlesim::Pose)，订阅者是pose listener。
在这里插入图片描述

11.1 创建订阅者代码

在功能包learning_topic/scr中新建pose_subscriber.cpp文件：

/**
 * 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose
 */
 
#include <ros/ros.h>
#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;
}

callback函数：接受消息，并处理
main函数：1）初始节点，2）创建subscriber，3）循环等待回调函数

11.2 配置编译规则

在CMakeLists.txt中加入：

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

11.3 编译运行

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_topic velocity_publisher
$ rosrun learning_topic pose_subscriber

注：运行python文件，首先设置为可执行文件，然后rosrun learning_topic pose_subscriber.py。

12 话题消息定义和使用

publisher发送person info给subscriber。
在这里插入图片描述

12.1 定义和编译话题消息

定义msg文件
在功能包learning_topic中建立文件夹msg，在msg中新建文件Person.msg。

string name
uint8 sex
uint8 age
uint8 unknown = 0
uint8 male= 1
uint8 female = 2

在package.xml中添加功能包依赖

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

在CMakeLists.txt添加编译选项

find_package( ...... message_generation)
add_message_files(FILES Person.msg)
generate_messages(DEPENDENCIES std_msgs)
catkin_package(...... message_runtime)

编译生成语言相关文件
在devel/include/learning_topic中得到Person.h

12.2 使用自定义的消息

12.2.1 创建publisher/subscriber

在src中新建person_publisher.cpp

/**
 * 该例程将发布/person_info话题，自定义消息类型learning_topic::Person
 */
 
#include <ros/ros.h>
#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;
}

person_subscriber.cpp:

/**
 * 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person
 */
 
#include <ros/ros.h>
#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;
}

12.2.2 配置编译规则

在CMakeLists.txt中添加：

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)

注：使用add_dependencies是为了让可执行文件动态地与自定义的头文件Person.h产生连接

12.2.3 编译运行publisher/ subscriber

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun learning_topic person_subscriber
$ rosrun learning_topic person_publisher

13 客户端Client的编程实现

Client (turtle_spawn) request service (/spawn), server (turtlesim) response to the service request.
在这里插入图片描述

13.1 创建功能包

src中创建learning_service功能包

$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs
turtlesim

13.2 编写客户端代码（C++）

src/turtle_spawn.cpp

/**
 * 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn
 */

#include <ros/ros.h>
#include <turtlesim/Spawn.h>

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); // call请求并等待回复

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

	return 0;
};

1）初始化节点，2）创建Client实例，3）发布请求数据，4）等待Server处理之后的应答结果

客户端add_turtle通过call函数请求服务/spawn, 服务数据是类型为turtlesim::Spawn的srv。

13.3 配置编译规则

在CMakeLists.txt中设置可执行文件和链接库：

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

13.4 编译运行客户端

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_service turtle_spawn

14 服务器Server的编程实现

客户端Client是命令行终端Terminal，发送std_srv::Trigger类型请求数据，请求（call）服务/turtle_command,服务器收到请求后，进行处理和响应。
在这里插入图片描述

14.1 创建服务器代码

src/turtle_command_server.cpp

/**
 * 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger
 */
 
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Trigger.h>

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

1)初始化节点，2）创建Server实例，3）循环等待服务请求，进入回调函数，4）在回调函数中完成服务功能的处理，反馈应答数据

注：

节点还包括一个publisher，该publisher通过/turtle/cmd_vel话题向turtlesim发送运动信息(Twist)。在循环中，当服务器接收到客户端的请求后，会通过callback函数改变pubCommand状态，通过判断pubCommand，决定是否向turtlesim发送运动信息。
这个服务器接收来自命令行终端的请求（类型为std_srvs/Trigger，request可空），在命令行终端中请求服务方式如下：rosservice call /turtle_command “{}”

14.2 编译配置

CMakeLists.txt中添加：

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

14.3 编译运行服务器

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_service turtle_command_server
$ rosservice call /turtle_command "{}"

15 服务数据的定义与使用

12讲中的publisher/subscriber模型是publisher不断发送数据，然后subscriber接收；而此讲介绍Client/Server模型，Client请求Service之后，Server作出响应。这里采用自定义的服务数据类型learnin_service::Person。
在这里插入图片描述

15.1 自定义服务数据

15.1.1 定义srv文件

在learning_service/srv中新建Person.srv:

string name
uint8 age
uint8 sex

uint8 unknown = 0
uint8 male = 1
uint8 female = 2
---
string result

(—以上是request数据，下面是response数据)

15.1.2 package.xml中添加功能包依赖

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

15.1.3 CMakeLists.txt中添加编译选项

- find_package (... message_generation)
# Declare ROS messages, services and actions
- add_service_files(FILES Person.srv)
- generate_messages(DEPENDENCIES std_msgs)
# catkin specific configuration
- catkin_package( ... message_runtime)

15.1.4 编译得到头文件

在catkin_ws中运行catkin_make，在devel/include/learning_service中生成对应的头文件Person.h等。
Person.h部分代码：

// Generated by gencpp from file learning_service/Person.msg
// DO NOT EDIT!


#ifndef LEARNING_SERVICE_MESSAGE_PERSON_H
#define LEARNING_SERVICE_MESSAGE_PERSON_H

#include <ros/service_traits.h>


#include <learning_service/PersonRequest.h>
#include <learning_service/PersonResponse.h>


namespace learning_service
{

struct Person
{

typedef PersonRequest Request;
typedef PersonResponse Response;
Request request;
Response response;

typedef Request RequestType;
typedef Response ResponseType;

}; // struct Person
} // namespace learning_service

15.2 使用自定义服务数据

15.2.1 创建服务器/客户端代码

服务器 person_server.cpp

/**
 * 该例程将执行/show_person服务，服务数据类型learning_service::Person
 */
 
#include <ros/ros.h>
#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;
}

客户端person_client.cpp代码：

/**
 * 该例程将请求/show_person服务，服务数据类型learning_service::Person
 */

#include <ros/ros.h>
#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;

    // 客户端请求服务（call service）
	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;
};

15.2.2 配置编译规则

可执行文件，连接库，依赖项

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)

注：add_dependencies在依赖库也是编译生成时使用，在编译上层target时，自动检查下层依赖库是否已经生成，若没有，则先要编译下层依赖库，再编译上层target，最后link depend target。

15.2.3 编译运行

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash

$ roscore
$ rosrun learning_service person_server
$ rosrun learning_service person_client

16 参数的使用与编程方法

ROS Master包含一个Parameter Server，储存全局变量字典，可供其他节点查询使用。

在这里插入图片描述

16.1 创建功能包

$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_parameter roscpp rospy std_srvs

16.2 参数文件和命令行

yaml参数文件：
在这里插入图片描述
命令行：rosparam

rosparam list # 列出当前参数
rosparam get param_key # 显示某个参数值
rosparam set param_key param_value # 设置某个参数值
rosparam dump file_name # 保存参数到文件
rosparam load file_name # 从文件读取参数
rosparam delete param_key # 删除参数

16.3 编程方法

在src/learning_param中新建parameter_config.cpp:

/**
 * 该例程设置/读取海龟例程中的参数
 */
#include <string>
#include <ros/ros.h>
#include <std_srvs/Empty.h>

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

	// 调用服务，刷新背景颜色(blue->white)
	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;
}

16.4 配置编译规则

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

16.5 编译运行

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_parameter parameter_config