第二讲: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、话题编程
话题编程流程:
- 创建发布者
- 创建订阅者
- 添加编译选项
- 运行可执行程序
如何实现一个发布者:
- 初始化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是1,那么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、定义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 运动规划器