参考引用
- 机器人工匠阿杰
- wpr_simulation
1. 机器人运动控制
1.1 测试环境安装
- wpr_simulation 安装
$ mkdir -p catkin_ws/src $ cd catkin_ws/src $ git clone https://github.com/6-robot/wpr_simulation.git $ cd wpr_simulation/scripts/ $ ./install_for_melodic.sh # 自动下载和安装编译需要的依赖项 $ cd ~/catkin_ws $ catkin_make
1.2 控制量纲与消息包
- 矢量运动速度量纲为 m/s,旋转运动速度量纲为 rad/s,如:3.14 代表 1s 转半圈 180°
- x、y、z 方向通过右手定则确定:食指指向正前方(+x),中指指向正左方(+y),大拇指指向正上方(+z)
-
速度控制消息包:geometry_msgs/Twist
-
消息包具体定义:linear & angular
1.3 控制 x 方向速度 /cmd_vel
- 构建一个新的功能包 vel_pkg,并在 vel_pkg 功能包的 src 下新建 vel_node.cpp 文件
$ cd catkin_ws/src $ catkin_create_pkg vel_pkg roscpp rospy geometry_msgs
#include <ros/ros.h> #include <geometry_msgs/Twist.h> int main(int argc, char *argv[]) { ros::init(argc, argv, "vel_node"); ros::NodeHandle n; // 向 ROS 大管家 NodeHandle 申请发布话题 /cmd_vel 并拿到发布对象 vel_pub ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10); // 构建一个 geometry_msgs/Twist 类型的消息包 vel_msg,用来承载要发送的速度值 geometry_msgs::Twist vel_msg; vel_msg.linear.x = 0.1; vel_msg.linear.y = 0.0; vel_msg.linear.z = 0.0; vel_msg.angular.x = 0; vel_msg.angular.y = 0; vel_msg.angular.z = 0; // 开启一个 while 循环,不停的使用 vel_pub 对象发送速度消息包 vel_msg ros::Rate r(30); while (ros::ok()) { vel_pub.publish(vel_msg); r.sleep(); } return 0; } // 修改 CMakeLists.txt 文件此处略 // 最后到 catkin_ws 目录下编译
- 测试运行
$ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc $ roslaunch wpr_simulation wpb_simple.launch $ rosrun vel_pkg vel_node
2. 使用 Rviz 观测传感器数据
- Gazebo 是模拟真实机器人发出传感器数据的工具(虚拟的环境),现实中不存在并被真实的实体机器人和真实环境所代替
- Rviz 是接收传感器数据并进行显示的工具(真实的环境),只有需要观察某些数据实时变化的时候才用到
$ cd ~/catkin_ws/ $ roslaunch wpr_simulation wpb_simple.launch $ roslaunch wpr_simulation wpb_rviz.launch
3. 激光雷达 /scan
3.1 消息包类型:sensor_msgs/LaserScan
# 1 / scan_time = LiDAR 扫描频率
# --noarr 防止数组消息刷屏
$ rostopic echo /scan --noarr
上图中超过雷达测距范围的点数值变成 INF
3.2 获取激光雷达数据节点
- 构建一个新的功能包,包名叫做 lidar_pkg,并在软件包中新建一个节点,节点名叫做 lidar_node
$ cd catkin_ws/src $ catkin_create_pkg lidar_pkg roscpp rospy sensor_msgs
#include <ros/ros.h> #include <sensor_msgs/LaserScan.h> // 构建回调函数 LidarCallback(),用来接收和处理雷达数据 void LidarCallback(const sensor_msgs::LaserScan msg) { float fMidDist = msg.ranges[180]; // 调用 ROS_INFO() 打印雷达检测到的前方障碍物距离 ROS_INFO("qianfangceju ranges[180] = %f m", fMidDist); } int main(int argc, char *argv[]) { setlocale(LC_ALL, ""); ros::init(argc, argv, "lidar_node"); // 向 ROS 大管家 NodeHandle 申请订阅话题 /scan,并设置回调函数为 LidarCallback() ros::NodeHandle n; ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback); ros::spin(); return 0; } // 修改 CMakeLists.txt 文件此处略 // 最后到 catkin_ws 目录下编译
3.3 激光雷达避障节点
// 在 3.2 小节基础上修改
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>
ros::Publisher vel_pub; // 全局变量,保证回调函数也能使用
int nCount = 0;
void LidarCallback(const sensor_msgs::LaserScan msg) {
float fMidDist = msg.ranges[180];
ROS_INFO("qianfangceju ranges[180] = %f m", fMidDist);
if(nCount > 0) {
nCount--;
return;
}
// 构建速度控制消息包 vel_cmd
geometry_msgs::Twist vel_cmd;
// 根据激光雷达的测距数值,实时调整机器人运动速度,从而避开障碍物
if (fMidDist < 1.5) {
vel_cmd.angular.z = 0.3;
nCount = 40; // 确保机器人旋转的角度足够避开障碍物
} else {
vel_cmd.linear.x = 0.1;
}
vel_pub.publish(vel_cmd);
}
int main(int argc, char *argv[]) {
setlocale(LC_ALL, "");
ros::init(argc, argv, "lidar_node");
// 让大管家 NodeHandle 发布速度控制话题 /cmd_vel
ros::NodeHandle n;
ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);
vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10); // 发布速度控制话题 /cmd_vel
ros::spin();
return 0;
}
4. IMU
4.1 消息包类型 sensor_msgs/Imu
4.2 获取 IMU 数据节点
-
ROS 官方 IMU 话题种类标准
-
构建一个新的软件包叫做 imu_pkg,在软件包中新建一个节点叫做 imu_node.cpp
$ cd catkin_ws/src $ catkin_create_pkg imu_pkg roscpp rospy sensor_msgs
#include <ros/ros.h> #include <sensor_msgs/Imu.h> #include <tf/tf.h> void IMUCallback(sensor_msgs::Imu msg) { // 先对四元数 orientation 的协方差矩阵第一个数值进行判断,如果小于0 // 说明四元数的值不存在,具体查看sensor_msgs/Imu Message说明 if(msg.orientation_covariance[0] < 0) { return; } // 使用 tf 工具将四元数转换成 tf 四元数对象 tf::Quaternion quaternion ( msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w ); double roll, pitch, yaw; // 用来装载转换后的欧拉角结果 // 先将 quaternion 转换成一个 tf 的 3×3 矩阵对象,然后调用 getRPY 转换成欧拉角 tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw); // 将弧度值转换为角度值 roll = roll * 180 / M_PI; pitch = pitch * 180 / M_PI; yaw = yaw * 180 / M_PI; // 调用 ROS_INFO() 显示转换后的欧拉角数值 ROS_INFO("roll = %.0f pitch = %.0f yaw = %.0f", roll, pitch, yaw); } int main(int argc, char *argv[]) { setlocale(LC_ALL, ""); ros::init(argc, argv, "imu_node"); // 在节点中向 ROS 大管家 NodeHandle 申请订阅话题 /imu/data,并设置回调函数为 IMUCallback() ros::NodeHandle n; ros::Subscriber imu_sub = n.subscribe("/imu/data", 10, IMUCallback); ros::spin(); return 0; } // 修改 CMakeLists.txt 文件此处略 // 到 catkin_ws 目录下编译
4.3 IMU 航向锁定节点
// 在 4.2 小节基础上修改
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"
#include "geometry_msgs/Twist.h"
// 速度消息发布对象(全局变量)
ros::Publisher vel_pub;
// IMU 回调函数
void IMUCallback(const sensor_msgs::Imu msg) {
// 检测消息包中四元数数据是否存在
if(msg.orientation_covariance[0] < 0)
return;
// 四元数转成欧拉角
tf::Quaternion quaternion(
msg.orientation.x,
msg.orientation.y,
msg.orientation.z,
msg.orientation.w
);
double roll, pitch, yaw;
tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
// 弧度换算成角度
roll = roll*180/M_PI;
pitch = pitch*180/M_PI;
yaw = yaw*180/M_PI;
ROS_INFO("滚转 = %.0f 俯仰 = %.0f 朝向 = %.0f", roll, pitch, yaw);
// 速度消息包
geometry_msgs::Twist vel_cmd;
// 设定一个目标朝向角,当姿态信息中的朝向角和目标朝向角不一致时,控制机器人转向目标朝向角
double target_yaw = 90; // 目标朝向角
// 计算速度
double diff_angle = target_yaw - yaw;
vel_cmd.angular.z = diff_angle * 0.01;
vel_cmd.linear.x = 0.1;
vel_pub.publish(vel_cmd);
}
int main(int argc, char **argv) {
setlocale(LC_ALL, "");
ros::init(argc, argv, "imu_node");
ros::NodeHandle n;
// 订阅 IMU 的数据话题
ros::Subscriber sub = n.subscribe("imu/data", 100, IMUCallback);
// 让 ROS 大管家 NodeHandle 发布速度控制话题 /cmd_vel
vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);
ros::spin();
return 0;
}
5. ROS 常用消息包
5.1 标准消息包 std_msgs
5.2 几何消息包 geometry_msgs
其中一些消息包带了 stamped 关键词,这些消息包都是多了一个 Header,也就是多了时间戳信息和坐标系(frame) ID,将空间量和时间量进行了绑定,多用于一些预测和滤波算法中
5.3 传感器消息包 sensor_msgs
6. 占据栅格地图
导航所使用的地图数据,就是 ROS 导航软件包里的 map_server 节点在话题 /map 中发布的消息数据,消息类型是 nav_msgs 消息包的 OccupancyGrid(占据栅格)地图,就是一种正方形小格子组成的地图,每个格子里填入一个数值,表示障碍物占据情况
6.1 占据栅格地图形象解释
-
在地面上划分出一个个大小一样的正方形栅格,然后在栅格中填充不同的颜色表示占据情况,没有障碍物的栅格为白色,被占据的栅格为黑色(除去中间的墙面,剩下的黑白方格便是栅格地图)
-
栅格尺寸大小可变,越小则黑色的区域越精细,也就越接近障碍物的轮廓,但同时地图的数据量就越大,处理的时候计算量就越大,所以一般会给栅格设置一个适当的尺寸
-
栅格尺寸:指的是栅格的单边长度,体现了地图的精细程度,常被用来表示栅格地图的分辨率,ROS 中栅格地图的默认分辨率为 0.05m,也就是每个栅格的边长为5cm
-
数字表示栅格
-
将栅格一行一行拼接起来变成一个数组,有了这个数组,再加上栅格的行、列数等信息,就能通过具体的数值将这个栅格地图描述清楚了
6.2 占据栅格地图消息类型
6.3 占据栅格地图发布节点
- 构建一个软件包 map_pkg,依赖项里加上 nav_msgs
$ cd ~/catkin_ws/src $ catkin_create_pkg map_pkg roscpp rospy nav_msgs $ cd .. $ code .
- 在 map_pkg 里创建一个节点 map_pub_node
#include <ros/ros.h> #include <nav_msgs/OccupancyGrid.h> int main(int argc, char *argv[]) { ros::init(argc, argv, "map_pub_node"); // 通过大管家发布话题 /map,消息类型为 nav_msgs::OccupancyGrid ros::NodeHandle n; ros::Publisher pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 10); ros::Rate r(1); while(ros::ok()) { // 构建一个 nav_msgs::OccupancyGrid 地图消息包,并对其进行赋值 nav_msgs::OccupancyGrid msg; msg.header.frame_id = "map"; msg.header.stamp = ros::Time::now(); msg.info.origin.position.x = 0; msg.info.origin.position.y = 0; msg.info.resolution = 1.0; msg.info.width = 4; msg.info.height = 2; msg.data.resize(4 * 2); msg.data[0] = 100; msg.data[1] = 100; msg.data[2] = 0; msg.data[3] = -1; pub.publish(msg); r.sleep(); } return 0; } // 修改 CMakeLists.txt 文件此处略 // 到 catkin_ws 目录下编译
- 启动 Rviz,订阅话题 /map,显示地图
$ roscore $ rosrun map_pkg map_pub_node $ rviz