第二讲：ROS基础

第二讲：ROS基础

一. 创建工作空间

1、什么是工作空间

​ 工作空间（workspace）是一个存放开发相关文件的文件夹。

​ **src：**代码空间（Source Space）

​ **build：**编译空间（Build Space）

​ **devel：**开发空间（Development Space）

​ **install：**安装空间（Install Space）

2、创建工作空间

2、创建工作空间

​ 创建工作空间：

$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ catkin_init_workspace

​ 编译工作空间：

$ cd ~/catkin_ws/
$ catkin_make

​ 设置环境变量：

$ source devel/setup.bash

​ 检查环境变量：

$ echo $ROS_PACKAGE_PATH

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-31wROikx-1595044670409)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200711160005334.png)]

3、创建功能包

$ catkin_create_package <package_name> [depend1] [depend2] [depend3]

​ 创建功能包：

$ cd ~/catkin_ws/src
$ catkin_create_package learning_communication std_msgs roscpp rospy

​ 编译功能包：

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

二、ROS通信编程

1、话题编程

​ 话题编程流程：

1. 创建发布者
2. 创建订阅者
3. 添加编译选项
4. 运行可执行程序

---

如何实现一个发布者：

• 初始化ROS节点
• 向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型
• 按照一定频率循环发布信息

#include <sstream>
#include "ros/ros.h"
#include "std_msgs/String.h"

int main(int argc,char **argv)
{
	//ROS节点初始化,"talker"定义整个节点运行起来的名称
	ros::init(argc, argv, "talker");

	//创建节点句柄，发布者、订阅者和话题都是通过句柄来管理的
	ros::NodeHandle n;
	
	//创建一个Publisher叫做chatter_pub，发布名为chatter的topic，消息类型为std_msgs::String，1000是指发布者的队列长度，当数据发布很快超过队列长度的时候，会把时间戳最早的数据删掉，可能会断帧
	ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter",1000);

	//设置循环的频率，定义具体的循环周期，10hz=100ms为周期，把消息发布出去
	ros::Rate loop_rate(10);

	int count = 0;

	while(ros::ok())
	{
		//初始化std_msgs::String类型的消息
		std_msgs::String msg;
		std::stringstream ss;
		//该语句实现了string型的数据 hello world和int型变量 count的拼接，形成一个新的string。即如果count是１，那么hello world1会作为string被存放在ss当中。
		ss<<"hello world"<<count;
		msg.data = ss.str();
		
		//发布消息
        //c_str()：生成一个const char*指针，指向以空字符终止的数组。
		ROS_INFO("%s",msg.data.c_str());
		chatter_pub.publish(msg);

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

		//按照循环频率延时，节点进入休眠状态，100ms后继续工作
		loop_rate.sleep();
		++count;
	}

	return 0;
}

---

如何实现一个订阅者：

• 初始化ROS节点
• 订阅需要的话题
• 循环等待话题消息，接收到消息后进入回调函数
• 在回调函数中完成消息处理

#include "ros/ros.h"
#include "std_msgs/String.h"

//接收到订阅的消息后，会进入消息回调函数
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
	//将接收到的消息打印出来
	ROS_INFO("I heard:[%s]",msg->data.c_str());
}

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

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

	//创建一个Subscriber，订阅名为chatter的话题，注册回调函数chatterCallback
	ros::Subscriber sub = n.subscribe("chatter",1000,chatterCallback);

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

	return 0;
}	

---

如何编译代码：

• 设置需要编译的代码和生成的可执行文件
• 设置链接库
• 设置依赖

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

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

---

如何运行可执行文件：

$ rosrun learning_communication talker
$ rosrun learning_communication listener

用launch文件运行：

<launch>
	<node pkg="learning_communication" name="talker" type="talker" output="screen"/>
	<node pkg="learning_communication" name="listener" type="listener" output="screen"/>
</launch>

运行效果如图：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-EUmn1cRe-1595044670413)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200711164601518.png)]

---

如何自定义话题消息：

1、定义msg文件

string name
uint8 sex
uint8 age

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

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

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

3、在CMakeLists.txt中添加编译选项

find_package(catkin REQUIRED COMPONENTS message_generation)

catkin_package(CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs message_runtime)

add_message_files(FILES Person.msg)
generate_messages(DEPENDENCIES std_msgs)

person_publisher.cpp

/**
 * 该例程将发布/person_info话题，learning_communication::Person
 */
 
#include <ros/ros.h>
#include "learning_communication/Person.h"

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

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

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

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

    int count = 0;
    while (ros::ok())
    {
        // learning_communication::Person类型的消息
    	learning_communication::Person person_msg;
		person_msg.name = "Tom";
		person_msg.age  = 18;
		person_msg.sex  = learning_communication::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_communication::Person
 */
 
#include <ros/ros.h>
#include "learning_communication/Person.h"

// 接收到订阅的消息后，会进入消息回调函数
void personInfoCallback(const learning_communication::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;
}

2、服务编程

​ 服务编程流程：

1. 创建服务器
2. 创建客户端
3. 添加编译选项
4. 运行可执行程序

---

如何自定义服务请求与应答：

1、定义srv文件

上面是request–服务的请求数据，下面是response–服务的应答数据

int64 a
int64 b
---
int64 sum

---

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

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

3、在CMakeLists.txt中添加编译选项

find_package(catkin REQUIRED COMPONENTS message_generation)

catkin_package(CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs message_runtime)

add_message_files(FILES AddTwoInts.srv)

---

如何实现一个服务器：

• 初始化ROS节点
• 创建Server实例
• 循环等待服务请求，进入回调函数
• 在回调函数中完成服务功能的处理，并反馈应答函数

#include "ros/ros.h"
#include "learning_communication/AddTwoInts.h"

//service回调函数，输入参数req，输出参数res
bool add(learning_communication::AddTwoInts::Request &req,
         learning_communication::AddTwoInts::Response &res)
{
    //将输入参数中的请求数据相加，结果放到应答变量中
    res.sum = req.a + req.b;
    ROS_INFO("request:x=%ld,y=%ld",(long int)req.a,(long int)req.b);
    ROS_INFO("sending back response:[%ld]",(long int)res.sum);

    return true;
}

int main(int argc, char **argv)
{
    //ros节点初始化
    ros::init(argc,argv,"add_two_ints_server");
    
    //创建节点句柄
    ros::NodeHandle n;

    //创建一个名为add_two_ints的server，注册回调函数add（）
    ros::ServiceServer service = n.advertiseService("add_two_ints",add);

    //循环等待回调函数
    ROS_INFO("Ready to add two ints.");
    ros::spin();

    return 0;
}

---

如何实现一个客户端：

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

#include <cstdlib>
#include "ros/ros.h"
#include "learning_communication/AddTwoInts.h"

//argc是命令行总的参数个数,argv[]是argc个参数，其中第0个参数是程序的全名，以后的参数是命令行后面跟的用户输入的参数
int main(int argc ,char ** argv)
{
    //ROS节点初始化
    ros::init(argc,argv,"add_two_ints_client");

    //从终端命令获取两个加数
    if(argc != 3)
    {
        ROS_INFO("usage:add_two_ints_client X Y");
        //return 0代表函数正常终止，return 1代表函数非正常终止
        return 1;
    }

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

    //创建一个client，请求add_two_int service
    //service消息类型是learning_communication::AddTwoInts
    ros::ServiceClient client = n.serviceClient<learning_communication::AddTwoInts>("add_two_ints");

    //创建learning_communication::AddTwoInts类型的service消息
    //atoll把字符串转换成长长整型数（64位）
    learning_communication::AddTwoInts srv;
    srv.request.a = atoll(argv[1]);
    srv.request.b = atoll(argv[2]);

    //发布service请求，等待加法运算的应答结果
    //call是阻塞命令，如果一直没有response就一直卡在这个地方
    if(client.call(srv))
    {
        ROS_INFO("Sum:%ld",(long int)srv.response.sum);
    }
    else
    {
        ROS_ERROR("Failed to call service add_two_ints");
        return 1;
    }
    
    return 0;
}

---

如何编译代码：

• 设置需要编译的代码和生成的可执行文件
• 设置链接库
• 设置依赖

add_executable(server src/server.cpp)
target_link_libraries(server ${catkin_LIBRARIES})
add_dependencies(server ${PROJECT_NAME}generate_messages_cpp)

add_executable(client src/client.cpp)
target_link_libraries(client ${catkin_LIBRARIES})
add_dependencies(client ${PROJECT_NAME}generate_messages_cpp)

---

如何运行可执行文件：

$ rosrun learning_communication server
$ rosrun learning_communication client

---

程序运行结果：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-VJmFVYfF-1595044670414)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200711180330019.png)]

3、动作编程

什么是动作：

• 一种问答通信机制
• 带有连续反馈
• 可以在任务过程中中止运行
• 基于ROS的消息机制实现

---

Action的接口：

• goal：发布任务目标
• cancel：请求取消任务
• status：通知客户端当前的状态
• feedback：周期反馈任务运行的监控数据
• result：向客户端发送任务的执行结果，只发布一次

---

如何自定义动作消息：

1、定义action文件

# 定义目标信息
uint32 dishwasher_id
# Specify which dishwasher we wa
nt to use
---
# 定义结果信息
uint32 total_dishes_cleaned
---
# 定义周期反馈的消息
float32 percent_complete

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

  <build_depend>actionlib</build_depend>
  <build_depend>actionlib_msgs</build_depend>
  <exec_depend>actionlib</exec_depend>
  <exec_depend>actionlib_msgs</exec_depend>

3、在CMakeLists.txt中添加编译选项

find_package(catkin REQUIRED COMPONENTS actionlib_msgs actionlib)

add_action_files(DIRECTORY action FILES DoDishes.action)

generate_messages(DEPENDENCIES std_msgs actionlib_msgs)

---

如何实现一个动作服务器：

• 初始化ROS节点
• 创建动作服务器实例
• 启动服务器，等待动作请求
• 在回调函数中完成动作服务功能的处理，并反馈进度信息
• 动作完成，发送结束信息

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "learning_communication/DoDishesAction.h"

typedef actionlib::SimpleActionServer<learning_communication::DoDishesAction> Server;

// 收到action的goal后调用该回调函数
void execute(const learning_communication::DoDishesGoalConstPtr& goal, Server* as)
{
    ros::Rate r(1);
    learning_communication::DoDishesFeedback feedback;

    ROS_INFO("Dishwasher %d is working.", goal->dishwasher_id);

    // 假设洗盘子的进度，并且按照1hz的频率发布进度feedback
    for(int i=1; i<=10; i++)
    {
        feedback.percent_complete = i * 10;
        as->publishFeedback(feedback);
        r.sleep();
    }

    // 当action完成后，向客户端返回结果
    ROS_INFO("Dishwasher %d finish working.", goal->dishwasher_id);
    as->setSucceeded();
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "do_dishes_server");
    ros::NodeHandle n;

    // 定义一个服务器
    Server server(n, "do_dishes", boost::bind(&execute, _1, &server), false);
    
    // 服务器开始运行
    server.start();

    ros::spin();

    return 0;
}

---

如何实现一个动作服务端：

• 初始化ROS节点
• 创建动作客户端实例
• 连接动作服务端
• 发送动作目标
• 根据不同类型的服务端反馈处理回调函数

#include <actionlib/client/simple_action_client.h>
#include "learning_communication/DoDishesAction.h"

typedef actionlib::SimpleActionClient<learning_communication::DoDishesAction> Client;

// 当action完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,
        const learning_communication::DoDishesResultConstPtr& result)
{
    ROS_INFO("Yay! The dishes are now clean");
    ros::shutdown();
}

// 当action激活后会调用该回调函数一次
void activeCb()
{
    ROS_INFO("Goal just went active");
}

// 收到feedback后调用该回调函数
void feedbackCb(const learning_communication::DoDishesFeedbackConstPtr& feedback)
{
    ROS_INFO(" percent_complete : %f ", feedback->percent_complete);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "do_dishes_client");

    // 定义一个客户端
    Client client("do_dishes", true);

    // 等待服务器端
    ROS_INFO("Waiting for action server to start.");
    //client.waitForServer(),客户端等待服务器函数，可以传递一个ros::Duration作为最大等待值，程序进入到这个函数开始挂起等待，服务器就位或者达到等待最大时间退出，前者返回true,后者返回false.
    client.waitForServer();
    ROS_INFO("Action server started, sending goal.");

    // 创建一个action的goal
    learning_communication::DoDishesGoal goal;
    goal.dishwasher_id = 1;

    // 发送action的goal给服务器端，并且设置回调函数
    //client.sendGoal(),客户端发送目标函数，本函数一共需要四个参数。第一个必须，另三个可选。
    //第一个参数就是本次交互的目标，第二个参数是服务端运行结束时调用的函数，第三个是服务器刚刚收到参数激活时调用的函数，第四个是服务器在进程中不停回传反馈时调用的函数。
    client.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);

    ros::spin();

    return 0;
}

---

如何编译代码：

• 设置需要编译的代码和生成的可执行文件
• 设置链接库
• 设置依赖

add_executable(DoDishes_client src/DoDishes_client.cpp)
target_link_libraries( DoDishes_client ${catkin_LIBRARIES})
add_dependencies(DoDishes_client ${${PROJECT_NAME}_EXPORTED_TARGETS})

add_executable(DoDishes_server src/DoDishes_server.cpp)
target_link_libraries( DoDishes_server ${catkin_LIBRARIES})
add_dependencies(DoDishes_server ${${PROJECT_NAME}_EXPORTED_TARGETS})

---

如何运行可执行文件：

$ rosrun learning_communication DoDishes_client
$ rosrun learning_communication DoDishes_server

用launch文件运行:

<launch>
	<node pkg="learning_communication" name="DoDishes_client" type="DoDishes_client" output="screen"/>
	<node pkg="learning_communication" name="DoDishes_server" type="DoDishes_server" output="screen"/>
</launch>

---

程序运行结果：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-6KxmmVWS-1595044670416)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200711204525843.png)]

三、分布式通信

四、ROS中的关键组件

1、Launch文件

launch文件：通过XML文件实现多节点的配置和启动（可自动启动ROS MASTER）

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-dUGqtFlq-1595044670418)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200711205036313.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-UfwfLqU0-1595044670419)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200711210654420.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-YdBcxKFO-1595044670420)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200711210727471.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-RNaYxvsR-1595044670421)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200711210749153.png)]

2、TF坐标变换

小海龟跟随实验：

$ sudo apt-get install ros-kinetic-turtle-tf
$ roslaunch turtle_tf turtle_tf_demo.launch
$ rosrun turtlesim turtle_teleop_key
$ rosrun tf view_frames
$ rosrun tf tf_echo turtle1 turtle2
$ rosrun rviz rviz -d `rospack find turtle_tf` /rviz/turtle_rviz.rviz

如何实现一个TF广播器：

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

/**
 * 该例程产生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;
    //setOrigin设置平移变换
	transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
	tf::Quaternion q;
	q.setRPY(0, 0, msg->theta);
    //setRotation设置旋转变换
	transform.setRotation(q);

	// 广播world与海龟坐标系之间的tf数据
    //这里发布的TF消息类型是tf::StampedTransform，不仅包含tf::Transform类型的坐标变换、时间戳，而且需要指定坐标变换的源坐标系（parent）和目标坐标系（child）
	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监听器（TransformListener）
• 查找坐标变换（waitForTranform、lookupTransform）

/**
 * 该例程监听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;
	//缓存10秒
	ros::Rate rate(10.0);
	while (node.ok())
	{
		// 获取turtle1与turtle2坐标系之间的tf数据
		tf::StampedTransform transform;
		try
		{
            //给定源坐标系（source_frame）和目标坐标系（target_frame），等待两个坐标系之间指定时间（time）的变换关系，该函数会阻塞程序运行，所以要设置超时时间
			listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
            //给定源坐标系（source_frame）和目标坐标系（target_frame），得到两个坐标系之间指定时间（time）的坐标变换（transform），ros::Time(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;
};

---

如何编译代码：

• 设置需要编译的代码和生成的可执行文件
• 设置链接库

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

---

如何运行可执行文件：

 <launch>
    <!-- 海龟仿真器 -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!-- 键盘控制 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <!-- 两只海龟的tf广播 -->
    <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" />

    <!-- 监听tf广播，并且控制turtle2移动 -->
    <node pkg="learning_tf" type="turtle_tf_listener"
          name="listener" />

 </launch>

---

程序运行结果：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-focvNJP8-1595044670422)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200712113114228.png)]

3、Qt工具箱

• 日志输出工具——rqt_console
• 计算图可视化工具——rqt_graph
• 数据绘图工具——rqt_plot
• 参数动态配置工具——rqt_reconfigure

---

4、Rviz可视化平台

---

5、Gazebo物理仿真环境

---

五、第二章作业

1、话题与服务编程

通过代码新生一只海龟，放置在（5，5）点，命名为“turtle2”；通过代码订阅turtle2的实时位置并在终端打印；控制turtle2实现旋转运动

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

ros::Publisher turtle_vel;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    ROS_INFO("Turtle2 position:(%f,%f)",msg->x,msg->y);
}

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

    ros::NodeHandle node;
   
    //通过服务调用，产生第二只乌龟turtle2
    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");

    turtlesim::Spawn srv;
    srv.request.x = 5;
    srv.request.y = 5;
    srv.request.name = "turtle2";
    add_turtle.call(srv);

    ROS_INFO("The turtle2 has been spawn!");

    //订阅乌龟的pose信息
    ros::Subscriber sub = node.subscribe("turtle2/pose", 10, &poseCallback);
    
    //定义turtle的速度控制发布器
    turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",10);

    ros::Rate rate(10.0);

    while (node.ok())
    {
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 1;
        vel_msg.linear.x = 1;
        turtle_vel.publish(vel_msg);

        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}

第三讲：机器人系统设计

一、机器人的定义和组成

二、机器人系统构建

三、URDF机器人建模

1、什么是URDF

• Unified Robot Description Format，统一机器人描述格式
• ROS中一个非常重要的机器人模型描述格式
• 可以解析URDF文件中使用XML格式描述的机器人模型
• ROS同时也提供URDF文件的C++解析器

---

• ：描述机器人link部分的外观参数
• ：描述link的惯性参数
• ：描述link的碰撞属性

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-wklVOygn-1595044670423)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200712160748142.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-6PkoBmcw-1595044670424)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200712171555125.png)]

---

：

• 描述机器人关节的运动学和动力学属性
• 包括关节运动的位置和速度限制
• 根据关节运动形式，可以将其分为六种类型

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-YG6fGouD-1595044670425)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200712171615287.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-e56qu4op-1595044670426)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200712171629742.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Usz1Ytr8-1595044670427)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200712171706617.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-jUT2XIsC-1595044670428)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200712171806904.png)]

---

• 完整机器人模型的最顶层标签
• 和标签都必须包含在标签内

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-88S1wbVn-1595044670429)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200712172036587.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-FiI1rPyM-1595044670429)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200712172043958.png)]

2、创建一个机器人建模的功能包

$ catkin_create_pkg mbot_description urdf xacro

• urdf：存放机器人模型的URDF或xacro文件
• meshes：放置URDF中引用的模型渲染文件
• launch：保存相关启动文件
• config：保存rviz的配置文件

---

display_mbot_base_urdf.launch:

<launch>
	<param name="robot_description" textfile="$(find mbot_description)/urdf/mbot_base.urdf" />

	<!-- 设置GUI参数，显示关节控制插件 -->
	<param name="use_gui" value="true"/>
	
	<!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
	
	<!-- 运行robot_state_publisher节点，发布tf  -->
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
	
	<!-- 运行rviz可视化界面，设置好的显示的插件，加载配置文件，在config文件夹下 -->
	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_description)/config/mbot_urdf.rviz" required="true" />
</launch>

• joint_state_publisher：发布每个joint（除fixed类型）的状态，而且可以通过UI界面对joint进行控制
• robot_state_publisher：将机器人各个links、joints之间的关系，通过TF的形式，整理成三维姿态信息发布

3、第一步：使用圆柱体创建一个车体模型

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-TYFRlT0a-1595044670430)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200713105315086.png)]

<?xml version="1.0" ?>
<robot name="mbot">
	<!-- 一般都会把主体部分叫做base_link，这段代码用来描述机器人的底盘link -->
    <link name="base_link">
        <!-- 先从纯视觉上创建一个感官上的机器人模型，visual来定义底盘的外观属性 -->
        <visual>
            <!-- 设置起点坐标为界面的中心坐标，rpy是相对于三个坐标轴的旋转，单位是弧度 -->
            <origin xyz=" 0 0 0" rpy="0 0 0" />
            <geometry>
                <!-- cylinder标签定义圆柱的半径和高 -->
                <cylinder length="0.16" radius="0.20"/>
            </geometry>
            <!-- 使用material标签设置机器人底盘的颜色——黄色，其中的color便是黄色的RGBA值，a是颜色的透明度，0是完全不透明 -->
            <material name="yellow">
                <color rgba="1 0.4 0 1"/>
            </material>
        </visual>
    </link>

</robot>

4、第二步：使用圆柱体创建左侧车轮

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-0jbmK8UF-1595044670431)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200713105327377.png)]

<?xml version="1.0" ?>
<robot name="mbot">

    <!-- 在左轮模型设置中，不需要位置偏移，放置在（0，0，0）位置即可，将joint放置到安装位置 -->
    <joint name="left_wheel_joint" type="continuous">
        <origin xyz="0 0.19 -0.05" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="left_wheel_link"/>
       <!-- axis标签定义该joint的旋转轴是正y轴，轮子在运动时就会围绕y轴旋转 --> 
        <axis xyz="0 1 0"/>
    </joint>

    <link name="left_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <cylinder radius="0.06" length = "0.025"/>
            </geometry>
            <material name="white">
                <color rgba="1 1 1 0.9"/>
            </material>
        </visual>
    </link>

</robot>

5、第三步：使用圆柱体创建右侧后轮

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-F1TXMAx9-1595044670432)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200713105336464.png)]

<?xml version="1.0" ?>
<robot name="mbot">

    <joint name="right_wheel_joint" type="continuous">
        <origin xyz="0 -0.19 -0.05" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="right_wheel_link"/>
        <axis xyz="0 1 0"/>
    </joint>

    <link name="right_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <cylinder radius="0.06" length = "0.025"/>
            </geometry>
            <material name="white">
                <color rgba="1 1 1 0.9"/>
            </material>
        </visual>
    </link>

</robot>

6、第四步：使用球体创建前后支撑轮

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-wVocZZH8-1595044670433)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200713105440077.png)]

<?xml version="1.0" ?>
<robot name="mbot">

    <joint name="front_caster_joint" type="continuous">
        <origin xyz="0.18 0 -0.095" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="front_caster_link"/>
        <axis xyz="0 1 0"/>
    </joint>

    <link name="front_caster_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <sphere radius="0.015" />
            </geometry>
            <material name="black">
                <color rgba="0 0 0 0.95"/>
            </material>
        </visual>
    </link>
    
    <joint name="back_caster_joint" type="continuous">
        <origin xyz="-0.18 0 -0.095" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="back_caster_link"/>
        <axis xyz="0 1 0"/>
    </joint>

    <link name="back_caster_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <sphere radius="0.015" />
            </geometry>
            <material name="black">
                <color rgba="0 0 0 0.95"/>
            </material>
        </visual>
    </link>

</robot>

7、第五步：创建传感器——摄像头

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-BBHUmg7g-1595044670433)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200713105451735.png)]

<?xml version="1.0" ?>
<robot name="mbot">

    <link name="camera_link">
        <visual>
            <origin xyz=" 0 0 0 " rpy="0 0 0" />
            <geometry>
                <box size="0.03 0.04 0.04" />
            </geometry>
            <material name="black">
                <color rgba="0 0 0 0.95"/>
            </material>
        </visual>
    </link>

    <joint name="camera_joint" type="fixed">
        <origin xyz="0.17 0 0.10" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="camera_link"/>
    </joint>

</robot>

8、第五步：创建传感器——激光雷达

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-56aDcwvJ-1595044670434)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200713105519417.png)]

<?xml version="1.0" ?>
<robot name="mbot">

    <link name="laser_link">
		<visual>
			<origin xyz=" 0 0 0 " rpy="0 0 0" />
			<geometry>
				<cylinder length="0.05" radius="0.05"/>
			</geometry>
			<material name="black"/>
		</visual>
    </link>

    <joint name="laser_joint" type="fixed">
        <origin xyz="0 0 0.105" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="laser_link"/>
    </joint>

</robot>

9、第五步：创建传感器——Kinect

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-hVNWuWtg-1595044670435)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200713105800494.png)]

<?xml version="1.0" ?>
<robot name="mbot">

    <link name="kinect_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 1.5708"/>
            <geometry>
                <mesh filename="package://mbot_description/meshes/kinect.dae" />
            </geometry>
        </visual>
    </link>

    <joint name="laser_joint" type="fixed">
        <origin xyz="0.15 0 0.11" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="kinect_link"/>
    </joint>

</robot>

10、第六步：检查URDF模型整体结构

在URDF文件夹中打开终端：

$ urdf_to_graphiz mbot_with_kinect.urdf 

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-rfLfAaia-1595044670436)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200713111247356.png)]

第四讲：机器人仿真

一、机器人URDF模型优化

URDF模型的进化版本——xacro模型文件

xacro是URDF的升级版，模型文件的后缀名由.urdf变为.xacro，而且在模型标签中需要加入xacro的声明：

	<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">

• 精简模型代码
  • 创建宏定义
  • 文件包含
• 提供可编程接口
  • 常量
  • 变量
  • 数学计算
  • 条件语句

---

<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <!-- PROPERTY LIST -->
    <xacro:property name="M_PI" value="3.1415926"/>
    <xacro:property name="base_radius" value="0.20"/>
    <xacro:property name="base_length" value="0.16"/>

    <xacro:property name="wheel_radius" value="0.06"/>
    <xacro:property name="wheel_length" value="0.025"/>
    <xacro:property name="wheel_joint_y" value="0.19"/>
    <xacro:property name="wheel_joint_z" value="0.05"/>

    <xacro:property name="caster_radius" value="0.015"/> <!-- wheel_radius - ( base_length/2 - wheel_joint_z) -->
    <xacro:property name="caster_joint_x" value="0.18"/>

    <!-- Defining the colors used in this robot -->
    <material name="yellow">
        <color rgba="1 0.4 0 1"/>
    </material>
    <material name="black">
        <color rgba="0 0 0 0.95"/>
    </material>
    <material name="gray">
        <color rgba="0.75 0.75 0.75 1"/>
    </material>
    
    <!-- Macro for robot wheel -->
    <xacro:macro name="wheel" params="prefix reflect">
        <joint name="${prefix}_wheel_joint" type="continuous">
            <origin xyz="0 ${reflect*wheel_joint_y} ${-wheel_joint_z}" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="${prefix}_wheel_link"/>
            <axis xyz="0 1 0"/>
        </joint>

        <link name="${prefix}_wheel_link">
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
                </geometry>
                <material name="gray" />
            </visual>
        </link>
    </xacro:macro>

    <!-- Macro for robot caster -->
    <xacro:macro name="caster" params="prefix reflect">
        <joint name="${prefix}_caster_joint" type="continuous">
            <origin xyz="${reflect*caster_joint_x} 0 ${-(base_length/2 + caster_radius)}" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="${prefix}_caster_link"/>
            <axis xyz="0 1 0"/>
        </joint>

        <link name="${prefix}_caster_link">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <sphere radius="${caster_radius}" />
                </geometry>
                <material name="black" />
            </visual>
        </link>
    </xacro:macro>

    <xacro:macro name="mbot_base">
        <link name="base_footprint">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.001 0.001 0.001" />
                </geometry>
            </visual>
        </link>

        <joint name="base_footprint_joint" type="fixed">
            <origin xyz="0 0 ${base_length/2 + caster_radius*2}" rpy="0 0 0" />        
            <parent link="base_footprint"/>
            <child link="base_link" />
        </joint>

        <link name="base_link">
            <visual>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_length}" radius="${base_radius}"/>
                </geometry>
                <material name="yellow" />
            </visual>
        </link>

        <wheel prefix="left" reflect="-1"/>
        <wheel prefix="right" reflect="1"/>

        <caster prefix="front" reflect="-1"/>
        <caster prefix="back" reflect="1"/>
    </xacro:macro>
</robot>

1、常量定义

常量定义：

    <xacro:property name="M_PI" value="3.1415926"/>

常量使用：

    <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />

---

2、数学计算

在**"${}"**语句中，不仅可以调用常量，还可以使用一些常用的数字运算，包括加、减、乘、除、负号、括号等，例如：

	<origin xyz="0 ${(motor_length+wheel_length)/2} 0" rpy="0 0 0"/>

注意：所有数学运算都会转换成浮点数进行，以保证运算精度

---

3、宏定义

宏定义：

<xacro:macro name="name" params="A B C">
	......
</xacro:macro>

<!-- Macro for robot wheel -->
    <xacro:macro name="wheel" params="prefix reflect">
        <joint name="${prefix}_wheel_joint" type="continuous">
            <!-- reflect为-1或1决定了是左轮还是右轮 -->
            <origin xyz="0 ${reflect*wheel_joint_y} ${-wheel_joint_z}" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="${prefix}_wheel_link"/>
            <axis xyz="0 1 0"/>
        </joint>

        <link name="${prefix}_wheel_link">
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
                </geometry>
                <material name="gray" />
            </visual>
        </link>
    </xacro:macro>

宏调用：

<name A="A_value" B="B_value" C="C_value"/>

        <wheel prefix="left" reflect="-1"/>
        <wheel prefix="right" reflect="1"/>

---

4、文件包含

	<xacro:include filename="$(find mbot_description)/urdf/xacro/mbot_base.xacro" />

---

5、模型显示

方法一：在xacro文件夹中打开终端，输入以下命令，转换成URDF文件后显示

$ rosrun xacro xacro.py mbot.xacro > mbot.urdf

---

方法二：直接调用xacro文件解析器

在launch文件中添加以下代码：

	<arg name="model" default="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/mbot.xacro'" />

	<param name="robot_description" command="$(arg model)" />

然后运行指令：

$ roslaunch mbot_description display_mbot_base_xacro.launch

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Wf6ZTfyx-1595044670437)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200713220055810.png)]

---

二、ArbotiX+rviz功能仿真

ArbotiX是什么？

• 一款控制电机、舵机的硬件控制板
• 提供了相应的ROS功能包
• 提供了一个差速控制器，通过接收速度控制指令，更新机器人的里程计状态

---

1、安装ArbotiX

$ catkin_make

$ sudo apt-get install ros-indigo-arbotix-*

Kinetic版本：（功能包放在src文件夹下）

$ git clone https://github.com/vanadiumlabs/arbotix_ros.git
$ catkin_make

---

2、配置Arbotix控制器

第一步：创建launch文件

	<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
        	<rosparam file="$(find mbot_description)/config/fake_mbot_arbotix.yaml" command="load" />
        	<param name="sim" value="true"/>
	</node>

第二步：创建配置文件

arbotix_drivercontrollers: {
    
    
   <!-- 控制器命名为base_controller，可以定义很多个controller -->
   base_controller: {
       <!-- 类型是diff_controller，也就是差速控制器，刚好可以控制机器人模型的双轮差速运动 -->
       type: diff_controller, 
       <!-- 车体坐标系，使用的是base_footprint -->
       base_frame_id: base_footprint, 
       <!-- 两个轮子之间的间距 -->
       base_width: 0.26, 
       <!-- 设置控制的频率 -->
       ticks_meter: 4100, 
       Kp: 12, 
       Kd: 12, 
       Ki: 0, 
       Ko: 50, 
       accel_limit: 1.0 
    }
}

第三步：启动仿真器

$ roslaunch mbot_description arbotix_mbot_with_camera_xacro.launch 

第四步：启动键盘控制

$ roslaunch mbot_teleop mbot_teleop.launch 

可先用list命令查看当前话题列表

$ rostopic list 

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-kmka9lQu-1595044670438)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200714121618575.png)]

三、Gazebo物理仿真环境搭建

1、ros_control

ros_control是什么？

• ROS给开发者提供的机器人控制中间件
• 包含一系列控制器接口、传动装置接口、硬件接口、控制器工具箱等等
• 可以帮助机器人应用功能包快速落地，提高开发效率

---

ros_control的总体框架

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-x0iWR4Tj-1595044670439)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200716095341216.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-j78SR43g-1595044670440)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200716100220286.png)]

• 控制器管理器：提供一种通用的接口来管理不同的控制器

• 控制器：读取硬件状态，发布控制命令，完成每个joint的控制

• 硬件资源：为上下两层提供硬件资源的接口

• 机器人硬件抽象：机器人硬件抽象之间和硬件资源打交道，通过write和read方法完成硬件操作

• 真实机器人：执行接收到的命令

---

控制器（Controllers）：

• joint_effort_controller
• joint_state_controller
• joint_position_controller
• joint_velocity_controller

---

2、仿真步骤

2.1 配置物理仿真模型

第一步：为link添加惯性参数和碰撞属性

<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <!-- PROPERTY LIST -->
    <xacro:property name="M_PI" value="3.1415926"/>
    <xacro:property name="base_mass"   value="20" /> 
    <xacro:property name="base_radius" value="0.20"/>
    <xacro:property name="base_length" value="0.16"/>

    <xacro:property name="wheel_mass"   value="2" />
    <xacro:property name="wheel_radius" value="0.06"/>
    <xacro:property name="wheel_length" value="0.025"/>
    <xacro:property name="wheel_joint_y" value="0.19"/>
    <xacro:property name="wheel_joint_z" value="0.05"/>

    <xacro:property name="caster_mass"    value="0.5" /> 
    <xacro:property name="caster_radius"  value="0.015"/> <!-- wheel_radius - ( base_length/2 - wheel_joint_z) -->
    <xacro:property name="caster_joint_x" value="0.18"/>

    <!-- Defining the colors used in this robot -->
    <material name="yellow">
        <color rgba="1 0.4 0 1"/>
    </material>
    <material name="black">
        <color rgba="0 0 0 0.95"/>
    </material>
    <material name="gray">
        <color rgba="0.75 0.75 0.75 1"/>
    </material>
    
    <!-- Macro for inertia matrix -->
    <xacro:macro name="sphere_inertial_matrix" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
                iyy="${2*m*r*r/5}" iyz="0" 
                izz="${2*m*r*r/5}" />
        </inertial>
    </xacro:macro>

    <xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" /> 
        </inertial>
    </xacro:macro>

    <!-- Macro for robot wheel -->
    <xacro:macro name="wheel" params="prefix reflect">
        <joint name="${prefix}_wheel_joint" type="continuous">
            <origin xyz="0 ${reflect*wheel_joint_y} ${-wheel_joint_z}" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="${prefix}_wheel_link"/>
            <axis xyz="0 1 0"/>
        </joint>

        <link name="${prefix}_wheel_link">
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
                </geometry>
                <material name="gray" />
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
                </geometry>
            </collision>
            <cylinder_inertial_matrix  m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
        </link>

        <gazebo reference="${prefix}_wheel_link">
            <material>Gazebo/Gray</material>
        </gazebo>

        <!-- Transmission is important to link the joints and the controller -->
        <transmission name="${prefix}_wheel_joint_trans">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="${prefix}_wheel_joint" >
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            </joint>
            <actuator name="${prefix}_wheel_joint_motor">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
                <mechanicalReduction>1</mechanicalReduction>
            </actuator>
        </transmission>
    </xacro:macro>

    <!-- Macro for robot caster -->
    <xacro:macro name="caster" params="prefix reflect">
        <joint name="${prefix}_caster_joint" type="continuous">
            <origin xyz="${reflect*caster_joint_x} 0 ${-(base_length/2 + caster_radius)}" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="${prefix}_caster_link"/>
            <axis xyz="0 1 0"/>
        </joint>

        <link name="${prefix}_caster_link">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <sphere radius="${caster_radius}" />
                </geometry>
                <material name="black" />
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <sphere radius="${caster_radius}" />
                </geometry>
            </collision>      
            <sphere_inertial_matrix  m="${caster_mass}" r="${caster_radius}" />
        </link>

        <gazebo reference="${prefix}_caster_link">
            <material>Gazebo/Black</material>
        </gazebo>
    </xacro:macro>

    <xacro:macro name="mbot_base_gazebo">
        <link name="base_footprint">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.001 0.001 0.001" />
                </geometry>
            </visual>
        </link>
        <gazebo reference="base_footprint">
            <turnGravityOff>false</turnGravityOff>
        </gazebo>

        <joint name="base_footprint_joint" type="fixed">
            <origin xyz="0 0 ${base_length/2 + caster_radius*2}" rpy="0 0 0" />        
            <parent link="base_footprint"/>
            <child link="base_link" />
        </joint>

        <link name="base_link">
            <visual>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_length}" radius="${base_radius}"/>
                </geometry>
                <material name="yellow" />
            </visual>
            <collision>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_length}" radius="${base_radius}"/>
                </geometry>
            </collision>   
            <cylinder_inertial_matrix  m="${base_mass}" r="${base_radius}" h="${base_length}" />
        </link>

        <gazebo reference="base_link">
            <material>Gazebo/Blue</material>
        </gazebo>

        <wheel prefix="left"  reflect="-1"/>
        <wheel prefix="right" reflect="1"/>

        <caster prefix="front" reflect="-1"/>
        <caster prefix="back"  reflect="1"/>

        <!-- controller -->
        <gazebo>
            <plugin name="differential_drive_controller" 
                    filename="libgazebo_ros_diff_drive.so">
                <rosDebugLevel>Debug</rosDebugLevel>
                <publishWheelTF>true</publishWheelTF>
                <robotNamespace>/</robotNamespace>
                <publishTf>1</publishTf>
                <publishWheelJointState>true</publishWheelJointState>
                <alwaysOn>true</alwaysOn>
                <updateRate>100.0</updateRate>
                <legacyMode>true</legacyMode>
                <leftJoint>left_wheel_joint</leftJoint>
                <rightJoint>right_wheel_joint</rightJoint>
                <wheelSeparation>${wheel_joint_y*2}</wheelSeparation>
                <wheelDiameter>${2*wheel_radius}</wheelDiameter>
                <broadcastTF>1</broadcastTF>
                <wheelTorque>30</wheelTorque>
                <wheelAcceleration>1.8</wheelAcceleration>
                <commandTopic>cmd_vel</commandTopic>
                <odometryFrame>odom</odometryFrame> 
                <odometryTopic>odom</odometryTopic> 
                <robotBaseFrame>base_footprint</robotBaseFrame>
            </plugin>
        </gazebo> 
    </xacro:macro>

</robot>

        <collision>
            <origin xyz=" 0 0 0" rpy="0 0 0" />
            <geometry>
                <cylinder length="${base_length}" radius="${base_radius}"/>
            </geometry>
        </collision>   
        <cylinder_inertial_matrix  m="${base_mass}" r="${base_radius}" h="${base_length}" />

---

第二步：为link添加gazebo标签

        <gazebo reference="${prefix}_caster_link">
            <material>Gazebo/Black</material>
        </gazebo>


        <gazebo reference="base_link">
            <material>Gazebo/Blue</material>
        </gazebo>


        <!-- base_footprint没有任何物理意义，所以把它的重力属性关闭了 -->
        <gazebo reference="base_footprint">
            <turnGravityOff>false</turnGravityOff>
        </gazebo>


        <gazebo reference="${prefix}_wheel_link">
            <material>Gazebo/Gray</material>
        </gazebo>   

---

第三步：为joint添加传动装置

        <!-- Transmission is important to link the joints and the controller -->
        <transmission name="${prefix}_wheel_joint_trans">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="${prefix}_wheel_joint" >
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            </joint>
            <actuator name="${prefix}_wheel_joint_motor">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
                <mechanicalReduction>1</mechanicalReduction>
            </actuator>
        </transmission>

---

第四步：添加gazebo控制器插件

        <!-- controller -->
        <gazebo>
            <plugin name="differential_drive_controller" 
                    filename="libgazebo_ros_diff_drive.so">
                <rosDebugLevel>Debug</rosDebugLevel>
                <publishWheelTF>true</publishWheelTF>
                <robotNamespace>/</robotNamespace>
                <publishTf>1</publishTf>
                <publishWheelJointState>true</publishWheelJointState>
                <alwaysOn>true</alwaysOn>
                <updateRate>100.0</updateRate>
                <legacyMode>true</legacyMode>
                <leftJoint>left_wheel_joint</leftJoint>
                <rightJoint>right_wheel_joint</rightJoint>
                <wheelSeparation>${wheel_joint_y*2}</wheelSeparation>
                <wheelDiameter>${2*wheel_radius}</wheelDiameter>
                <broadcastTF>1</broadcastTF>
                <wheelTorque>30</wheelTorque>
                <wheelAcceleration>1.8</wheelAcceleration>
                <commandTopic>cmd_vel</commandTopic>
                <odometryFrame>odom</odometryFrame> 
                <odometryTopic>odom</odometryTopic> 
                <robotBaseFrame>base_footprint</robotBaseFrame>
            </plugin>
        </gazebo> 

：机器人的命名空间

和：左右轮转动的关节joint

和：机器人模型的相关尺寸，在计算差速参数时需要用到

：控制器订阅的速度控制指令，生成全局命名时需要结合中的命名空间

：里程计数据的参考坐标系，ROS中一般都命名为odom

2.2 创建仿真环境

2.3 开始仿真

第五讲：机器人感知

一、机器视觉（图像校准、图像识别等）

1、ROS中的图像数据（二维图像）

2、摄像头标定

3、ROS+OpenCV应用实例（人脸识别、物体跟踪）

4、二维码试别

5、扩展内容：物体试别与机器学习

二、机器语音（科大讯飞SDK）

第六讲：机器人SLAM与自主导航

一、机器人必备条件

1、硬件要求

• 差分轮式机器人，可使用Twist速度指令控制
  • linear：XYZ方向上的线速度，单位是m/s
  • angular：XYZ方向上的角速度，单位是m/s
• 机器人必须安装激光雷达等测距设备，可以获取环境深度信息
• 最好使用正方形和圆形的机器人，其他外形的机器人虽然可以正常使用，但是效果可能很不佳

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ldFioniq-1595044670441)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200716180938826.png)]

---

2、深度信息

• angle_min：可检测范围的起始角度
• angle_max：可检测范围的终止角度，与angle_min组成激光雷达的可检测范围
• angle_increment：相邻数据帧之间的角度步长
• time_increment：采集到相邻数据帧之间的时间步长，当传感器处于相对运动状态时进行补偿使用
• scan_time：采集一帧数据所需要的时间
• range_min：最近可检测深度的阈值
• range_max：最远可检测深度的阈值
• ranges：一帧深度数据的存储数组
• intensities：深度信息

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-EAx4kneg-1595044670442)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200716182558310.png)]

---

3、里程计信息

• pose：机器人当前位置坐标，包括机器人的XYZ三轴位置与方向参数，以及用于校正误差的协方差矩阵
• twist：机器人当前的运动状态，包括XYZ三轴的线速度与角速度，以及用于校正误差的协方差矩阵

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-55dSfutw-1595044670442)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200716193451471.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-uAEILk6u-1595044670443)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200716193504046.png)]

---

二、SLAM功能包的应用

1、gmapping

gmapping功能包

• 基于激光雷达
• Rao-Blackllized粒子滤波算法
• 二位栅格地图
• 需要机器人提供里程计信息
• OpenSlam开源算法
• 输出地图话题:nav_msgs/OccupancyGrid

---

gmapping功能包的总体框架

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Q5J2F6RS-1595044670444)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200716202545218.png)]

---

安装gmapping

$ sudo apt-get install ros-kinetic-gmapping

gmapping功能包中的话题和服务

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-TSotgCmG-1595044670444)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200716203125999.png)]

gmapping功能包中的话题和服务

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-dcdwgKaP-1595044670445)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200716203223543.png)]

---

栅格地图取值原理：

• 致命障碍：栅格值为254，障碍物与机器人的中心重合，此时机器人必然与障碍物发生碰撞。
• 内切障碍：栅格值为253，障碍物处于机器人轮廓的内切圆内，此时机器人也必然与障碍物发生碰撞。
• 外切障碍：栅格值为252~ 128，障碍物处于机器人的轮廓的外切圆内，此时机器人与障碍物临界接触，不一定发生碰撞。
• 非自由空间：栅格值为128~0，障碍物附近区域，一旦机器人进入该区域，将有较大概率发生碰撞，属于危险警戒区,机器人应该尽量避免进入。
• 自由区域：栅格值为0，此处没有障碍物，机器人可以自由通过。
• 未知区域：栅格值为255，此处还没有探知是否有障碍物,机器人可以前往继续建图。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-csGf997E-1595044670446)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200716204000441.png)]

---

配置gmapping节点

<launch>
    <arg name="scan_topic" default="scan" />

    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">
        <param name="odom_frame" value="odom"/>
        <param name="map_update_interval" value="5.0"/>
        <!-- Set maxUrange < actual maximum range of the Laser -->
        <param name="maxRange" value="5.0"/>
        <param name="maxUrange" value="4.5"/>
        <param name="sigma" value="0.05"/>
        <param name="kernelSize" value="1"/>
        <param name="lstep" value="0.05"/>
        <param name="astep" value="0.05"/>
        <param name="iterations" value="5"/>
        <param name="lsigma" value="0.075"/>
        <param name="ogain" value="3.0"/>
        <param name="lskip" value="0"/>
        <param name="srr" value="0.01"/>
        <param name="srt" value="0.02"/>
        <param name="str" value="0.01"/>
        <param name="stt" value="0.02"/>
        <param name="linearUpdate" value="0.5"/>
        <param name="angularUpdate" value="0.436"/>
        <param name="temporalUpdate" value="-1.0"/>
        <param name="resampleThreshold" value="0.5"/>
        <param name="particles" value="80"/>
        <param name="xmin" value="-1.0"/>
        <param name="ymin" value="-1.0"/>
        <param name="xmax" value="1.0"/>
        <param name="ymax" value="1.0"/>
        <param name="delta" value="0.05"/>
        <param name="llsamplerange" value="0.01"/>
        <param name="llsamplestep" value="0.01"/>
        <param name="lasamplerange" value="0.005"/>
        <param name="lasamplestep" value="0.005"/>
        <remap from="scan" to="$(arg scan_topic)"/>
    </node>
</launch>

启动gmapping演示

$ roslaunch mbot_gazebo mbot_kinetic_nav_gazebo.launch
$ roslaunch mbot_navigation gmapping_demo.launch
$ roslaunch mbot_teleop mbot_teleop.launch

用如下命令保存当前地图:

$ rosrun map_server map_server -f cloister_gmapping

---

2、hector_slam

3、cartographer

4、ORB_SLAM

三、ROS中的导航框架

1、move_base

基于move_base的导航框架

$ sudo apt-get install ros-kinetic-navigation

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-BkrGhEhD-1595044670447)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200717181311633.png)]

---

move_base功能包：

• 全局路径规划
  • 全局最优路径规划
  • Dijkstra或A*算法
• 本地实时规划
  • 规划机器人每个周期内的线速度、角速度，使之尽量符合全局最优路径
  • 实时避障
  • Trajectory Rollout和Dynamic Window Approaches算法
  • 搜索躲避和行进的多条路劲，综合各评价标准选取最优路劲

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-OTpzYTor-1595044670447)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200717190839655.png)]

---

move_base功能包中的话题和服务

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-NVR5dGmo-1595044670449)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200717191110965.png)]

---

配置move_base节点

<launch>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <rosparam file="$(find mbot_navigation)/config/mbot/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find mbot_navigation)/config/mbot/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find mbot_navigation)/config/mbot/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find mbot_navigation)/config/mbot/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find mbot_navigation)/config/mbot/base_local_planner_params.yaml" command="load" />
  </node>
  
</launch>

---

2、amcl

• 蒙特卡罗定位方法
• 二维环境定位
• 针对已有地图使用粒子滤波器跟踪一个机器人的姿态

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-rhWFMudt-1595044670450)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200717193827821.png)]

amcl功能包中的话题和服务

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-dD5fTAl7-1595044670450)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200717194058254.png)]

---

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-sM2cSga3-1595044670451)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200717194117152.png)]

• 里程计定位：只通过里程计的数据来处理/base和/odom之间的TF转换
• amcl定位：可以估算机器人在地图坐标系/map下的位姿信息，提供/base、/odom、/map之间的TF变换

---

配置amcl节点：

<launch>
    <arg name="use_map_topic" default="false"/>
    <arg name="scan_topic" default="scan"/>

    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
        <param name="use_map_topic" value="$(arg use_map_topic)"/>
        <!-- Publish scans from best pose at a max of 10 Hz -->
        <param name="odom_model_type" value="diff"/>
        <param name="odom_alpha5" value="0.1"/>
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="60"/>
        <param name="laser_max_range" value="12.0"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="2000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="odom_alpha1" value="0.2"/>
        <param name="odom_alpha2" value="0.2"/>
        <!-- translation std dev, m -->
        <param name="odom_alpha3" value="0.2"/>
        <param name="odom_alpha4" value="0.2"/>
        <param name="laser_z_hit" value="0.5"/>
        <param name="laser_z_short" value="0.05"/>
        <param name="laser_z_max" value="0.05"/>
        <param name="laser_z_rand" value="0.5"/>
        <param name="laser_sigma_hit" value="0.2"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_model_type" value="likelihood_field"/>
        <!-- <param name="laser_model_type" value="beam"/> -->
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="update_min_d" value="0.25"/>
        <param name="update_min_a" value="0.2"/>
        <param name="odom_frame_id" value="odom"/>
        <param name="resample_interval" value="1"/>
        <!-- Increase tolerance because the computer can get quite busy -->
        <param name="transform_tolerance" value="1.0"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
        <remap from="scan" to="$(arg scan_topic)"/>
    </node>
</launch>

---

四、导航框架应用

第七讲：MoveIt！机械臂控制

一、MoveIt！系统架构

1、MoveIt！是什么

• 一个易于使用的集成化开发平台
• 由一系列移动操作的功能包组成
  • 运动规划
  • 操作控制
  • 3D感知
  • 运动学
  • 控制与导航算法
  • …
• 提供友好的GUI
• 可应用于工业、商业、研发和其他领域
• ROS社区中使用度排名前三的功能包

---

2、系统架构

2.1 运动组（move_group）

• 用户接口
  • C++：使用move_group_interface包提供的API
  • Python：使用moveit_commander包提供的API
  • GUI：使用MoveIt！的rviz插件
• ROS参数服务器
  • URDF：robot_description参数，获取机器人URDF模型的描述信息
  • SRDF：robot_description_semantic参数，获取机器人模型的配置信息
  • config：机器人的其他配置信息，例如关节限位、运动学插件、运动规划插件等
• 机器人
  • Topic和Action通信

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-VhCW178J-1595044670452)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200717213516118.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-MxmBumxa-1595044670453)(C:\Users\ymz\AppData\Roaming\Typora\typora-user-images\image-20200717213534406.png)]

2.2 运动规划器

m best pose at a max of 10 Hz -->

------

## 四、导航框架应用

### 











# 第七讲：MoveIt！机械臂控制

## 一、MoveIt！系统架构

### 1、MoveIt！是什么

- 一个易于使用的集成化开发平台
- 由一系列移动操作的功能包组成
  - 运动规划
  - 操作控制
  - 3D感知
  - 运动学
  - 控制与导航算法
  - ......
- 提供友好的GUI
- 可应用于工业、商业、研发和其他领域
- ROS社区中使用度排名前三的功能包

------

### 2、系统架构

#### 2.1	运动组（move_group）

- 用户接口
  - C++：使用move_group_interface包提供的API
  - Python：使用moveit_commander包提供的API
  - GUI：使用MoveIt！的rviz插件
- ROS参数服务器
  - URDF：robot_description参数，获取机器人URDF模型的描述信息
  - SRDF：robot_description_semantic参数，获取机器人模型的配置信息
  - config：机器人的其他配置信息，例如关节限位、运动学插件、运动规划插件等
- 机器人
  - Topic和Action通信

[外链图片转存中...(img-VhCW178J-1595044670452)]

[外链图片转存中...(img-MxmBumxa-1595044670453)]



#### 2.2	运动规划器