ROS 入门教程笔记

ROS 入门教程笔记

前言：参看视频：【古月居】古月·ROS入门21讲 | 一学就会的ROS机器人入门教程
如果不是虚拟机的话，可以将文件拷贝到U盘或移动硬盘里，在UNBUNTU 里是可以直接识别的。记得现在windows 系统里先解压，ubuntu 解压会乱码，原因暂时没有深入了解，现以课程学习为主。

1.Linux 系统基础操作

快捷键：

• 快速打开终端 ctrl + alt + t
• 关闭终端 ctrl + d

命令行

• 查看当前路径：pwd
• 切换路径：cd 返回到上一路径cd …
• 创建文件夹：mkdir + 文件夹名
• 查看当前路径的所有文件夹：ls
• 创建文件：touch + 文件名
• 剪切文件作用 ：mv + 文件名 + 路径
• 拷贝文件：cp
• 删除文件：rm 删除目录加 -r
• 提高权限：sudo
• 更新软件源列表： sudo apt -get update
• 命令的使用说明：–help

2.C++&Python 极简基础

区别：

底层：C++
应用层：python

安装

C++ 编译器

sudo apt-get install g++

文件编译

g++ 文件名 -o 编译的文件名

执行文件

./编译文件名

python 解析器

sudo apt-get install python

无需编译，直接执行

python py文件名

3.ROS 是什么

通信机制+开发工具+应用功能+生态系统

通信机制：松耦合分布式通信

开发工具：

• 命令行&编译器
• Rviz
• QT工具箱
• Gazebo
• TF坐标变换

应用功能：

• Navigation
• SLAM
• moveit，（初步思想就是搭建积木）。

生态系统：

• Distribution
• Repository：开源代码包，不同机构共享软件
• wiki：ROS主要论坛
• ROS Answers：咨询问题的网站
• Blog：相关新闻、图片、视频(http://www.ros.rog/news)

作用：提高机器人研发中的软件服用率

4.ROS 的核心概念

通信机制

Node & ROS Master

节点 Node – 执行单元

• 完成某个功能的独立运行的文件;
• 不同节点可以用不同的编程语言，分布式工作;
• 节点名称必须是唯一的。

节点管理器 ROS Master --控制中心

• 为节点提供命名和注册服务
• 辅助节点相互查找、建立链接
• 提供参数服务器，节点使用服务器存储和检索运行时的参数。

话题通信

话题（topic）–异步通信机制（数据管道）

• 传输数据的总线
• 单向：发布者——> 订阅者
• 多对多，适用数据传输

消息（Message) – 话题数据（数据内容）

• 数据类型和数据结构
• 使用.msg 文件定义

服务通信

服务（Service）–同步通信机制（有反馈）

• 客户端/服务器（C/S）模型，发送/应答
• 使用 .srv 文件定义请求和应答数据结构
• 一对多，适用逻辑处理

参数

参数（Parameter） – 全局共享字典

• 可通过网络访问共享的、多变量的字典（RPC框架）
• 节点用次服务器来存储和检索运行时的参数
• 适合静态的，非二进制的，不频繁改变的数据，不合适存储动态配置的数据。

文件系统

功能包

• 基本单元：节点、消息、服务

功能包清单

• 记录功能包的基本信息，包含于功能包里面。

元功能包

• 组织多个用于同一个目的的功能包

5. ROS 命令行工具

常用命令

• rostopic
• rosservice
• rosnode
• rosparam
• rosmsg
• rossrv

小海龟示例

• roscore：启动ROS Master，所有节点的管理器
• rosrun turtlesim turtlesim_node：运行节点
• rosrun turtlesim turtle_teleop_key：按键控制
  工具介绍：
• rqt_graph ：看到系统的全貌，显示不同节点的关系
• rosnode
  – rosnode list:显示节点，rosout 默认节点，用于显示
  – rosnode info /节点名：显示节点信息
• rostopic：话题相关
  – rostopic list：显示活跃的topic
  – rostopic pub：给节点发送指令
• rosmsg ：显示消息的具体信息
  – rosmsg show：显示指令的据i定义
• rosservice：服务相关
  – rosservice call：如创建新对象
• rosbag :话题记录和复现
  – rosbag record：话题记录
  – rosbag play:话题复现

创建工作空间与功能包

工作空间（Workspace）

存放工程相关的文件夹。

• src：代码空间
• build：编译空间。中间文件
• devel：开发空间（ROS1 有，ROS2 无）
• install：安装空间，编译生成的可执行文件

创建工作空间

• 创建工作空间

mkdir -p ~/catkin_ws/src  
cd ~/catkin_ws/src
catkin_init_workspace   //初始变成一个工作空间

• 编译工作空间

cd .. //回到上一空间
catkin_make //编译生成工作空间
catkin_make install  //产生 install 安装空间

• 生成功能包
  同个工作空间是不存在多个同名的功能包的

catkin_create_pkg test_pkg std_msgs rospy roscpp //catkin_create_pkg <包名> [依赖的包...]，在src文件夹下
cd .. //回到上一级路径
catkin_make  //编译工作空间，将功能包编译

• 设置环境变量
  要想使用功能包，要设置环境变量。让系统找到工作空间以及对应的功能包

source devel/setup.bash  //devel 文件里面有 setup.up 文件才能找到工作空间和功能包

为了避免每次重复操作，可在主目录下 按快捷键 ctrl + h，显示隐藏文件。 打开 .bashrc 文件
在最后加上 source 路径/devel/setup.bash，就直接加入了环境变量。如source /home/whj/catkin_ws/devel/setup.bash。重新打开终端生效。

• 检查环境变量

echo $ROS_PACKAGE_PATH    //找到所有功能包的路径

src文件的作用

package.xml 文件：

• xml语言描述功能包相关信息 ：名字 、版本号、维护信息 、 许可证等等;
• 编译和运行所需要的库和包。

CMakeLists.txt：

• 描述功能包的编译规则;
• Cmake语法 ，Cmake是基于gc的编译器，通过语言设置编译规则。

6.发布者Publisher的编程实现

topic 需要publisher 和subsciber : 发布和订阅

创建功能包：

catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim

部分代码注释：Velocity_publisher.cpp

• 节点初始化
• ROS Master 注册节点信息：创建publisher对象，包含topic名称和发送的数据类型
• 创建发布数据
• 按照一定频率循环发布消息。

/**
 * 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist
 */

#include <ros/ros.h>  //ROS 函数定义
#include <geometry_msgs/Twist.h>  //线速度和角速度定义

int main(int argc, char **argv)
{
        // ROS节点初始化
        ros::init(argc, argv, "velocity_publisher");  //ros::init(argc, argv, "节点唯一名称");  argc argv 是属性设置，一般不用

        // 创建节点句柄
        ros::NodeHandle n; //管理节点资源

        // 初始化创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，
        //队列长度10，publisher 发布数据时，底层可能来不及快速响应发布的频率，就先放在队列里面等着。
        //比如无人机发送的频率很快，但是以太网发布的数据很慢，就慢慢发送。
        //注意：底层数据发送能力太弱，就可能出现数据一直发送不出去。此时队列就只会保存最新的一组数据，抛弃原有的 
        //数据，就会出现掉帧的情况
        ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

        // 设置循环的频率，设置pub的频率
        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;

            // 发布消息，通过以太网TCP 的通信机制，将封装好的数据发送出去
                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;
}

修改功能包下的Cmake文件

#编译 + 链接
add_executable(velocity_publisher src/velocity_publisher.cpp)
# 将后面的文件src/veclocity_publisher.cpp 编译为可执行文件 velocity_publisher
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})
# 将可执行文件 velocity_publisher 和ROS的库做链接，c++接口 py接口 等等

退回到src上一级文件夹，进行编译ctakin_make
编译生成的publisher 在/home/whj/catkin_ws/devel/lib/learning_topic 里可看到 velocity_publisher

测试

分别在三个终端运行三个命令

roscore
rosrun turtlesim turtlesim_node 
rosrun learning_topic velocity_publisher

python的使用

同上，在learning_topic文件下创建 scripts 文件夹，将py文件拷贝过来/自己创建，注意查看权限是否可作为可执行文件（属性查看）。

7.订阅者Subsciber的编程实现

部分代码注释

• 节点初始化
• ROS Master 注册节点信息：创建Subscriber对象，订阅需要的话题；
• 循环等待话题，接收到数据进入回调函数
• 回调函数处理数据（注意回调函数处理时间不能太长，高效处理，类似中断函数）

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

修改功能包下的Cmake文件

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

退回到src上一级文件夹，进行编译ctakin_make

测试

分别在三个终端运行三个命令

roscore
rosrun turtlesim turtlesim_node 
rosrun learning_topic velocity_publisher
rosrun learning_topic pose_subscriber

python的使用

同上，在learning_topic文件下创建 scripts 文件夹，将py文件拷贝过来/自己创建，注意查看权限是否可作为可执行文件（属性查看）。

8. 话题消息的定义和使用

重点时掌握自定义消息（数据信息）的方法和步骤，调用和之前一致

自定义话题信息

• 定义 msg 文件

  • 1.在功能包里创建 msg 文件夹，存储信息定义信息
  • 2.在该文件下打开终端，创建文件touch Person.msg
  • 3.插入相关代码

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

• 在package.xml 中添加功能包依赖
  编译依赖：动态产生message的功能包
  运行依赖：runtime 运行时的依赖

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

• 在CmakeList 添加编译选项

find_package( …… message_generation)#添加包
add_message_files(FILES Person.msg)# Person.msg  作为定义的消息接口，针对其做编译
generate_messages(DEPENDENCIES std_msgs)# 上一步编译的时候需要依赖 std_msgs 

CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs turtlesim message_runtime
# 添加需要的依赖

• 编译生成语言相关文件
  devel/include/learning_topic 里可以看到编译生成的 .h文件

调用话题信息

整体使用和前面一样

• 发布信息

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

• 订阅

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

修改 CmakeList文件

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)

编译执行发布订阅

catkin_make
roscore
rosrun learning_topic person_subscriber
rosrun learning_topic person_publisher

注意：roscore 开启 ROS Master，刚开始起到婚介所的作用，后面建立连接后关闭不影响话题的发布和订阅。

python实现

同上，见代码

9.客户端 client 的实现

创建功能包

catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim

代码注释 turtle_spawn.cpp

• 初始节点
• 创建client 实例
• 发布服务请求数据
• 等待Server处理之后的应答结果

/**
 * 该例程将请求/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"); //等待，阻塞型的api，直到确认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;
};

修改Cmake文件

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

然后退回到上一级编译catkin_make

编译运行

roscore
rosrun turtlesim turtlesim_node 
rosrun learning_service turtle_spawn

python 实现

流程基本一致

10 服务端Server的编程实现

案例中用的trigger request没有定义，只定义了response。

创建服务器

• 初始化节点
• 创建Server实例
• 循环等待服务请求，进入回调函数
• 回调函数完成服务功能请求，反馈数据

/**
 * 该例程将执行/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; // 标志是停止还是运动 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;
}

修改 Cmake

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

编译后测试

catkin_make
roscore
rosrun turtlesim turtlesim_node 
rosrun learning_service turtle_command_server //此时等待数据
rosservice call /turtlertle_command "{}"   //发送请求，服务器收到请求之后开始控制海龟的运动。

11. 服务数据的定义和使用

服务数据的定义

定义srv 文件

string name
uint8  age
uint8  sex

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

---
string result

修改xml 和Cmake文件

操作见pdf

编译

编译之后，文件夹 /home/whj/catkin_ws/devel/include/learning_service 里面有三个 .h 文件

Person.h  //整合
PersonRequest.h   //请求
PersonResponse.h //回复

服务数据的使用

服务端代码

收到数据回复 result ，并打印收到的数据

/**
 * 该例程将执行/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;
}

客户端代码

发送perosn信息请求，收到response 并打印。

/**
 * 该例程将请求/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;

    // 请求服务调用
	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);

	// 显示服务调用结果

配置Cmake 文件

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)

编译运行

rescore
rosrun learning_service person_client
rosrun learning_service person_server

12. 参数的使用和编程

参数模型

全局变量，通过ROS Master 可以访问一些全局信息。

创建功能包

catkin_create_pkg learning_parameter roscpp rospy std_srvs

参数命令行使用

• 列出当前参数 rosparam list
• 显示某个参数值 rosparam get param_key
• 设置参数值rosparam set param_key param_value，后面需要发送请求，然后服务器会查询相关信息，如rosservice call /clear "{}"
• 保存参数到文件 rosparam dump param.yaml（文件名）
• 从文件读取参数rosparam load param.yaml（文件名）
• 删除参数rosparam delete param_key

代码注释 parameter_config.cpp

• 初始节点
• get函数获取参数
• set函数设置参数
  注意参数的路径要做出修改才有作用。如

	ros::param::get("/turtlesim//background_r", red);
	ros::param::get("/turtlesim//background_g", green);
	ros::param::get("/turtlesim//background_b", blue);

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

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

配置 CmakeList 文件

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

编译运行

catkin_make
roscore
rosrun turtlesim turtlesim_node
rosrun learning_parameter parameter_config

13. ROS 中的坐标系管理系统

机器人中的坐标变换

参考《机器人学导论》

TF 功能包（transform）

• 10秒内 坐标系之间的关系是怎样的
• 物体和中心坐标系
• 中心坐标系和全局坐标系
• 等等

TF坐标变换如何实现

• 广播TF变换
• 监听TF变换

示例

sudo apt-get install ros-noetic-turtle-tf
roslaunch turtle_tf turtle_tf_demo.launch 
rosrun turtlesim turtle_teleop_key 
rosrun tf view_frames  //生成pdf文件显示对应关系
rosrun tf tf_echo turtle1 turtle2  //显示坐标，角度（四元数 、弧度、角度）
rosrun rviz rviz -d 'rospack find turtle_tf'/rviz/turtle_rviz.rviz  //可视化工具的使用

注意事项：

• 1.注意 ROS 版本
• 2.运行launch文件出错时，更新pythonsudo apt install python-is-python3
• 3.rosrun tf view_frames出现问题

File "/opt/ros/noetic/lib/tf/view_frames", line 89, in generate
    m = r.search(vstr)
TypeError: cannot use a string pattern on a bytes-like object

修改办法

sudo vim /opt/ros/noetic/lib/tf/view_frames

第89行代码修改

#m = r.search(vstr)
m = r.search(vstr.decode('utf-8'))

14. TF 坐标系广播和监听的编程实现

实现 launch文件同样的功能

创建功能包

创建广播代码

• 定义TF 广播器
• 创建坐标变换值
• 发布坐标变换

/**
 * 该例程产生tf数据，并计算、发布turtle2的速度指令
 */

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

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 监听器

• 定义 TF 监听器
• 查找坐标变换

/**
 * 该例程监听tf数据，并计算、发布turtle2的速度指令
 */

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

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

设置Cmake

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

编译运行

catkin_make
roscore
rosrun turtlesim turtlesim_node
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1  // 节点名 + 坐标系名
rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
rosrun learning_tf turtle_tf_listener
rosrun turtlesim turtle_teleop_key 

15. launch 文件的使用方法

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

launch文件语法

• l标签：launch文件中的根元素
• 标签:
  • pkg：功能包名称
  • type：节点可执行文件名称
  • name：节点运行时的名称
  • 其他可选属性：output 、respawn 、ns等等
• :设置参数
• ：加载参数
• : 局部变量
• ：重映射
• ：包含其他launch文件。类似C头文件包含

示例

启动两个节点

将信息output到屏幕上

<launch>
    <node pkg="learning_topic" type="person_subscriber" name="talker" output="screen" />
    <node pkg="learning_topic" type="person_publisher" name="listener" output="screen" /> 
</launch>

参数设置及加载

在节点里面写参数会加上前缀，写在外面就不用。区分

<launch>

	<param name="/turtle_number"   value="2"/>

    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
		<param name="turtle_name1"   value="Tom"/>
		<param name="turtle_name2"   value="Jerry"/>

		<rosparam file="$(find learning_launch)/config/param.yaml" command="load"/>
	</node>

    <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"/>

</launch>

TF 广播和监听的 launch 文件

 <launch>

    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />

    <node pkg="learning_tf" type="turtle_tf_listener" name="listener" />

  </launch>

remap 重映射

话题remap

<launch>

	<include file="$(find learning_launch)/launch/simple.launch" />

    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
		<remap from="/turtle1/cmd_vel" to="/cmd_vel"/>
	</node>

</launch>

16 常用可视化工具的使用

Qt 工具箱

• rqt ：综合的工具，包括下面所说的的工具。
• rqt_console: 日志输出工具，可以对信息进行删选处理
• rqt_graph:计算图可视化工具
• rqt_plot：数据绘图工具,显示动态变化的数据
• rqt_image_view:图像渲染工具

Rviz

三维可视化工具，后面请熟悉使用方法，后面会经常使用。

Gazebo

三维物理仿真平台

• 物理引擎
• 图形渲染
• 方便的编程和图形接口
• 开源免费

应用场景

• 算法测试
• 机器人的设计
• 现实情景下的回溯测试。

17 课程总结和进阶攻略

实现功能

• 机器人控制和仿真
• 及时定位与地图建模
• 运动规划 （move group）
• 等等

理论基础

• 机器人学 – 斯坦福大学公开课

资料整理

follow 国内外的一些先进成果

注意事项

• roscore 每次运行之前都要关闭，然后重新打开。不然程序运行很多的话，某些参数名一样的话，会出现很多奇怪的问题。