【ROS 基础教学系列】机器人仿真运动控制

ROS 基础教学系列-机器人仿真运动控制与传感器

前言

话题通信适用于不断更新数据、少逻辑处理的传输相关的应用场景。

一、机器人运动控制

1.1 测试环境安装

wpr_simulation 安装

$ mkdir -p catkin_ws/src
$ cd catkin_ws/src
$ git clone https://gitee.com/yechen11/wpr_simulation.git
$ cd wpr_simulation/scripts/
$ ./install_for_melodic.sh  # 自动下载和安装编译需要的依赖项
$ cd ~/catkin_ws
$ catkin_make

随后还需下载以下两个功能包：

$ git clone https://gitee.com/yechen11/wpb_home.git
$ git clone https://gitee.com/yechen11/waterplus_map_tools.git

同样需要安装编译。
此时可以运行一下：

roslaunch wpr_simulation wpb_simple.launch

启动另一个终端，执行：

rosrun rqt_robot_steering rqt_robot_steering

鼠标改变速度和角度值，仿真机器人发生位姿变化。

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

我们的目标是构建一个速度控制节点，将速度消息包发送到速度控制话题/cmd_vel中去，这就是个典型的发布者节点

实现思路：

构建一个新的功能包 vel_pkg，并在 vel_pkg 功能包的 src 下新建 vel_node.cpp 文件

$ cd catkin_ws/src
$ catkin_create_pkg vel_pkg roscpp rospy geometry_msgs

2.

输入catkin_create_pkg vel_pkg roscpp rospy geometry_msgs，创建软件包，名字是vel_pkg，依赖项是roscpp rospy和geometry_msgs，这个geometry_msgs就是包含了速度消息类型的软件包

输入code . 打开VScode

在刚才创建的vel_pkg软件包的src文件夹里新建一个文件

名字叫做vel_node.cpp，这是一个节点代码文件

在vel_node.cpp节点代码文件中写入如下代码：

#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;    //x轴方向前进速度
    vel_msg.linear.y = 0.0;      //y轴方向前进速度
    vel_msg.linear.z = 0.0;      //z轴方向前进速度
    vel_msg.angular.x = 0;     //绕x轴旋转速度
    vel_msg.angular.y = 0;     //绕y轴旋转速度
    vel_msg.angular.z = 0;     //绕z轴旋转速度
    
  // 开启一个 while 循环，不停的使用 vel_pub 对象发送速度消息包 vel_msg
    ros::Rate r(30);
    while (ros::ok()) {
        vel_pub.publish(vel_msg);
        r.sleep();
    }
    
    return 0;
}

打开软件包的CMakeLists.txt文件

在文件末尾写上这三条编译规则

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

Ctrl+s保存后，Ctrl+Shift+B进行编译，编译成功

测试运行：
打开一个终端，在工作空间中输入source ./devel/setup.bash

输入roslaunch wpr_simulation wpb_simple.launch将仿真环境运行起来

再开另一个终端，输入source ./devel/setup.bash
输入rosrun vel_pkg vel_node后回车

可参考如下操作：

$ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
$ roslaunch wpr_simulation wpb_simple.launch
$ rosrun vel_pkg vel_node

查看仿真环境，发现机器人向前运动了起来
回到程序，对程序进行修改，使机器人绕Z轴进行旋转

按Ctrl+Shift+B进行编译，若出现许多项让你选择，选择如下

编译成功后，重新运行节点

机器人成功绕Z轴运动

作业：
机器人怎么画圆？

1.4 实验案例

小乌龟运动控制-1 小乌龟划圆圈
小乌龟运动控制-2 小乌龟走方形
小乌龟运动控制-3小乌龟走五角星
完成小乌龟，并移植到仿真环境中。

二. 使用 Rviz 观测传感器数据

• Gazebo 是模拟真实机器人发出传感器数据的工具（虚拟的环境），现实中不存在并被真实的实体机器人和真实环境所代替。
• Rviz 是接收传感器数据并进行显示的工具（真实的环境），只有需要观察某些数据实时变化的时候才用到。

仿真测试：

$ cd ~/catkin_ws/
$ roslaunch wpr_simulation wpb_simple.launch
$ roslaunch wpr_simulation wpb_rviz.launch

三. 激光雷达 /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

lidar_node.cpp文件内容

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