第一题:wpr机器人走椭圆轨迹 按键控制机器人启停
1. 需求分析
椭圆轨迹
要让wpr机器人走椭圆轨迹,我们知道wpr机器人的速度是订阅话题\cmd_vel
获得的,要想让机器人走椭圆轨迹,就要用话题\cmd_vel
发布特定的速度信息。
而通过速度控制机器人走椭圆有两种思路:
1.前进方向线速度不变,改变航向角角速度
2.航向角角速度不变,改变前进方向线速度
在此我们选择第二种思路,角速度不变,改变线速度。
按键启停
想要实现按键启停首先要能够读取按键输入,读取到特定输入之后进行相应动作,将速度置零或恢复运行速度。
2. 具体实现
第一步:环境搭建
-
创建工作空间,创建软件包并添加所需依赖。
-
将wpr_simulation软件包导入。
-
在软件包内创建新节点,并修改
CMakeLists.txt
文件使节点被包含。
第二步:代码实现
节点代码如下:
#include "ros/ros.h"
#include <geometry_msgs/Twist.h> //运动速度结构体类型 geometry_msgs::Twist的定义文件
#include <iostream>
#include <sys/ioctl.h>
#include <termios.h>
using namespace std;
bool kbhit(){
termios term;
tcgetattr(0, &term);
termios term2 = term;
term2.c_lflag &= ~ICANON;
tcsetattr(0, TCSANOW, &term2);
int byteswaiting;
ioctl(0, FIONREAD, &byteswaiting);
tcsetattr(0, TCSANOW, &term);
return byteswaiting > 0;
}
int main(int argc, char *argv[]){
ros::init(argc, argv, "vel_ctrl");
ros::NodeHandle n;
ros::Publisher Publisher = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
ros::Rate rate(2);
ROS_INFO("draw_circle start...");
int i;
int c;
bool flag = 0;
geometry_msgs::Twist cmd;
while (true){
if (kbhit()){
c = fgetc(stdin);
cout << endl;
switch (c){
case 'w':
flag = 1;
break;
case 's':
flag = 0;
break;
default:
break;
}
}
if(flag == 1){
cmd.angular.z = 1.09;
cmd.linear.x = 2;
i++;
if(((i >= 3)&&(i <= 6)) || ((i >= 9)&&(i <= 12))){
cmd.linear.x = cmd.linear.x + 0.9;
}
else{
cmd.linear.x = cmd.linear.x - 0.9;
}
if(i == 12){
i = 0;
}
}
else if(flag == 0){
cmd.angular.z = 0;
cmd.linear.x = 0;
}
ROS_INFO("flag: %d\n", flag);
Publisher.publish(cmd);
rate.sleep();
}
return 0;
}
第二题:通过Gmapping或Cartographer完成机器人在特定地图下(自选)的导航
1. 需求分析
Gmapping算法和Cartographer算法都是常见的SLAM实现算法,SLAM全称为Simultaneous Localization and Mapping,是指即时定位与地图构建,这里采用Gmapping算法来实现SLAM。
2. 具体实现
第一步:环境搭建
1. 创建工作空间,新建软件包
软件报名slam_gazebo_robot
,软件包依赖为:
- gazebo_plugins
- gazebo_ros
- gazebo_ros_control
- urdf
- xacro
第二步:机器人模型搭建
1. 使用Xacro构建机器人模型(也可以使用URDF构建,Xacro相较于URDF更精简,且效率更高)
机器人模型由车体、两个驱动轮、两个从动轮、一个激光雷达以及一个摄像头组成。以下为机器人模型文件结构:
- my_base_camera_laser.urdf.xacro
- my_base.urdf.xacro
- my_camera.urdf.xacro
- my_head.urdf.xacro
- my_laser.urdf.xacro
将这些文件放在slam_gazebo_robot\urdf\xacro
目录下,其中my_base_camera_laser.urdf.xacro
是集成机器人各部分的文件,my_base.urdf.xacro
是机器人主体模型文件,my_camera.urdf.xacro
是摄像头模型文件,my_head.urdf.xacro
是机器人头部模型文件,my_laser.urdf.xacro
是激光雷达模型文件。
my_base_camera_laser.urdf.xacro
集成文件代码如下
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:include filename="my_head.urdf.xacro" />
<xacro:include filename="my_base.urdf.xacro" />
<xacro:include filename="my_camera.urdf.xacro" />
<xacro:include filename="my_laser.urdf.xacro" />
</robot>
2. 给模型添加控制器
上一步实现了机器人模型的搭建,要想让机器人动起来还需要添加机器人各关节配置并添加控制器。这里采用ros_control软件包来实现机器人控制。
ros_control:是一组机器人控制软件包,它包含了控制器接口,控制器管理器,传输和硬件接口。ros_control 是一套机器人控制的中间件,是一套规范,不同的机器人平台只要按照这套规范实现,那么就可以保证 与ROS 程序兼容,通过这套规范,实现了一种可插拔的架构设计,大大提高了程序设计的效率与灵活性。
在slam_gazebo_robot\urdf\xacro
目录下创建文件move.urdf.xacro
作为机器人控制配置文件。并将这个文件集成到my_base_camera_laser.urdf.xacro
中。
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:include filename="my_head.urdf.xacro" />
<xacro:include filename="my_base.urdf.xacro" />
<xacro:include filename="my_camera.urdf.xacro" />
<xacro:include filename="my_laser.urdf.xacro" />
<xacro:include filename="move.urdf.xacro" />
</robot>
3. 配置传感器
同样编写xacro文件为机器人添加激光雷达以及摄像头的传感器配置。
在slam_gazebo_robot\urdf\xacro
目录下创建文件my_sensors_laser.urdf.xacro
作为激光雷达配置文件,创建文件my_sensors_camara.urdf.xacro
作为摄像头配置文件。并将这两个文件集成到my_base_camera_laser.urdf.xacro
中。
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:include filename="my_head.urdf.xacro" />
<xacro:include filename="my_base.urdf.xacro" />
<xacro:include filename="my_camera.urdf.xacro" />
<xacro:include filename="my_laser.urdf.xacro" />
<xacro:include filename="move.urdf.xacro" />
<xacro:include filename="my_sensors_laser.urdf.xacro" />
<xacro:include filename="my_sensors_camara.urdf.xacro" />
</robot>
至此,机器人模型已经全部完成,slam_gazebo_robot\urdf\xacro
目录文件结构如下
- my_base_camera_laser.urdf.xacro
- my_base.urdf.xacro
- my_camera.urdf.xacro
- my_head.urdf.xacro
- my_laser.urdf.xacro
- move.urdf.xacro
- my_sensors_laser.urdf.xacro
- my_sensors_camara.urdf.xacro
第三步:机器人仿真环境搭建
1. 创建地图
在Gazebo中可以通过添加组件的方式在空世界中创建自定义地图,这里我找到了一个super_mall
地图作为仿真地图
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-PQ0iyQX5-1687780448550)(D:\Daily documents\Study\作业\ROS\期末\Picture\gazebo_robot.png)]
将地图文件super_mall.world
保存在slam_gazebo_robot\worlds
目录下。
2. 编写仿真环境启动launch文件
在slam_gazebo_robot\launch
目录下创建文件gazebo_robot.launch
<launch>
<!-- 将 Urdf 文件的内容加载到参数服务器 -->
<param name="robot_description" command="$(find xacro)/xacro $(find slam_gazebo_robot)/urdf/xacro/my_base_camera_laser.urdf.xacro" />
<!-- 启动 gazebo -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find slam_gazebo_robot)/worlds/super_mall.world" />
</include>
<!-- 在 gazebo 中显示机器人模型 -->
<node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description" />
</launch>
3. 传感器信息显示
经过上面的步骤已经可以将创建好的机器人模型放到特定的仿真环境中并进行控制,接下来需要通过Rviz将机器人感知到的环境显示出来为之后的导航作准备。
首先编写Rviz的启动文件rviz.launch
并将其保存在slam_gazebo_robot\launch
目录下
<launch>
<!-- 启动 rviz -->
<node pkg="rviz" type="rviz" name="rviz" />
<!-- 关节以及机器人状态发布节点 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
</launch>
第四步:导航实现
1. SLAM建图
安装gmapping
sudo apt-get install ros-melodic-gmapping
编写gmapping启动文件gmapping.launch
并保存在slam_gazebo_robot\launch
目录下
<launch>
<param name="use_sim_time" value="true"/>
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<remap from="scan" to="scan"/>
<param name="base_frame" value="base_footprint"/>
......
......
</node>
</launch>
这一步完成之后已经可以手动控制机器人在地图中运动建图了,但此时机器人还没有定位能力,无法完成导航。
2. 定位
导航 = 建图 + 定位,上一步的建图中其实也包含定位的功能,但建图中的定位是用来构建全局地图的,而现在的定位需要应用于导航过程中,通过定位判断机器人的轨迹是否符合预期,这里我采用acml功能包用于机器人的定位。
AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统,它实现了自适应(或KLD采样)蒙特卡洛定位方法,可以根据已有地图使用粒子滤波器推算机器人位置。它已经被集成到了navigation功能包。
安装navigation
sudo apt-get install ros-melodic-navigation
编写acml节点相关的启动文件acml.launch
并保存在slam_gazebo_robot\launch
目录下。(acml包中的示例文件已经给出了launch文件,只需要稍作修改就可以使用)
<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="diff"/> <!-- 里程计模式为差分 -->
......
......
</node>
</launch>
3. 路径规划
有了地图和定位,接下来就需要机器人知道该如何移动才能到达目标点来实现导航,navigation
中提供了move_base
功能包来实现路径规划。
move_base 功能包提供了基于动作(action)的路径规划实现,move_base 可以根据给定的目标点,控制机器人底盘运动至目标位置,并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。它主要由全局路径规划与本地路径规划组成。
在路径规划中有一个很重要的概念:代价地图。它是由多层地图叠加得到的,有以下层级:
- Static Map Layer:静态地图层,SLAM构建的静态地图。
- Obstacle Map Layer:障碍地图层,传感器感知的障碍物信息。
- Inflation Layer:膨胀层,在以上两层地图上进行膨胀(向外扩张),以避免机器人的外壳会撞上障碍物。
- Other Layers:自定义costmap。
由于实际环境会不断变化,因此只使用SLAM构建的静态地图并不能完成导航,必须要在静态地图的基础上感知周围环境,将新的信息与静态地图叠加起来才能得到准确的信息。
路径规划中使用的代价地图有两个:全局代价地图global_costmap和局部代价地图local_costmap。分别用于全局路径规划和局部路径规划,全局路径规划用于寻找目标点与机器人所在点的大致路径,局部路径规划用于寻找机器人所在点与全局路径的机器人运动路径。
配置参数。在slam_gazebo_robot\param
目录下新建四个文件
- costmap_common_params.yaml:配置机器人的尺寸信息、距离障碍物的安全距离、传感器信息
- global_costmap_params.yaml:配置全局代价地图参数
- local_costmap_params.yaml:配置局部代价地图参数
- base_local_planner_params:配置机器人的速度及加速度参数
编写move_base节点启动文件move_base.launch
并保存在slam_gazebo_robot\launch
目录下
<launch>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
<rosparam file="$(find slam_gazebo_robot)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find slam_gazebo_robot)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find slam_gazebo_robot)/param/local_costmap_params.yaml" command="load" />
<rosparam file="$(find slam_gazebo_robot)/param/global_costmap_params.yaml" command="load" />
<rosparam file="$(find slam_gazebo_robot)/param/base_local_planner_params.yaml" command="load" />
</node>
</launch>
4. 导航
导航所需的全部功能已经实现,接下来对这些功能进行整合,编写一个启动文件将所有节点一起启动。
在slam_gazebo_robot\launch
目录下新建文件navigation.launch
<launch>
<!-- 启动仿真环境 -->
<include file="$(find slam_gazebo_robot)/launch/gazebo_robot.launch" />
<!-- 启动AMCL节点 -->
<include file="$(find slam_gazebo_robot)/launch/amcl.launch" />
<!-- 启动move_base节点 -->
<node pkg="tf" type="static_transform_publisher" name="link_name" args="0 0 0 0 0 0 map odom 0" />
<include file="$(find slam_gazebo_robot)/launch/move_base.launch" />
<!-- 运行SLAM -->
<include file="$(find slam_gazebo_robot)/launch/gmapping.launch" />
<!-- 运行rviz -->
<include file="$(find slam_gazebo_robot)/launch/rviz.launch" />
</launch>
运行navigation.launch
文件
roslaunch slam_gazebo_robot navigation.launch
rviz和gazebo开始运行
在rviz程序中点击左下角Add添加以下几个组件:
- Map:Topic选择/map
- RobotModel
- Odometry:Topic选择/odom,取消勾选Covariance
- LaserScan:Topic选择/scan
- PoseArray::Topic选择/particlecloud
- Map:全局代价地图,Topic选择/move_base/global_costmap/costmap
- Map:局部代价地图,Topic选择/move_base/local_costmap/costmap
- Path:全局规划路径,Topic选择/move_base/TrajectoryPlannerROS/global_plan
- Path:局部规划路径,Topic选择/move_base/TrajectoryPlannerROS/local_plan,颜色修改为红色
点击左上角File/Save Config
把配置文件命名为planning.rviz
保存在slam_gazebo_robot\rviz
目录下
修改文件rviz.launch
<launch>
<!-- 启动 rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_gazebo_robot)/rviz/planning.rviz"/>
<!-- 关节以及机器人状态发布节点 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
</launch>
这样运行rviz启动文件时就会自动按照配置运行。
重新运行navigation.launch
启动文件,在rviz界面中使用上方的2D Nav Goal
可以设定目标点,机器人会沿着路径往目标点移动。
5. 自主探索建图
可以通过不停的设置目标点让机器人遍历整个地图来扫描并创建地图,但需要人工操作,为了避免手动设置目标点,采用explore_lite功能包
explore-lite提供了贪婪的基于边界的探索。当节点运行时,机器人会贪婪地探索它的环境,直到找不到边界,它的移动命令会发送至move-base节点。
安装explore-lite
sudo apt-get install ros-melodic-explore-lite
explore-lite提供了自主探索配置的启动文件,直接在navigation.launch
文件中集成即可
<launch>
<!-- 启动仿真环境 -->
<include file="$(find slam_gazebo_robot)/launch/gazebo_robot.launch" />
<!-- 启动AMCL节点 -->
<include file="$(find slam_gazebo_robot)/launch/amcl.launch" />
<!-- 启动move_base节点 -->
<node pkg="tf" type="static_transform_publisher" name="link_name" args="0 0 0 0 0 0 map odom 0" />
<include file="$(find slam_gazebo_robot)/launch/move_base.launch" />
<!-- 运行SLAM -->
<include file="$(find slam_gazebo_robot)/launch/gmapping.launch" />
<!-- 运行rviz -->
<include file="$(find slam_gazebo_robot)/launch/rviz.launch" />
<!-- 运行explore-lite -->
<include file="$(find explore_lite)/launch/explore.launch" />
</launch>
6. 地图保存
地图扫描完成后需要将其保存,map_serve功能包可以实现该功能。
map_server功能包中提供了两个节点: map_saver 和 map_server,前者用于将栅格地图保存到磁盘,后者读取磁盘的栅格地图并以服务的方式提供出去。
安装map_server
sudo apt-get install ros-melodic-map-server
在slam_gazebo_robot
目录下新建map
文件夹用于存放地图。
编写map_saver启动文件map_saver.launch
并将其保存在slam_gazebo_robot\launch
目录下
<launch>
<arg name="filename" value="$(find slam_gazebo_robot)/map/map" />
<node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>
运行navigation.launch
文件
roslaunch slam_gazebo_robot navigation.launch
机器人已经自动开始探索扫描地图,等待机器人将地图探索完毕后,运行map_saver.launch
文件
roslaunch slam_gazebo_robot map_saver.launch
完整地图已经保存在slam_gazebo_robot\map
目录下
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-8nWSSDon-1687780448553)(D:\Daily documents\Study\作业\ROS\期末\Picture\map_folder.png)]
其中.pgm是图片文件,可以直接打开查看,.yaml文件是地图的元数据信息。
所建地图效果如下:
参考资料:
https://blog.csdn.net/qq_22701545/