【ROS2】演示:用假机器人(dummy robot)进行实验

最新推荐文章于 2025-08-24 23:05:07 发布

原创最新推荐文章于 2025-08-24 23:05:07 发布 · 779 阅读

9 ·

CC 4.0 BY-SA版权

文章标签：

#机器人

在这个演示中，我们展示了一个简单的演示机器人，包含从发布关节状态到发布虚假激光数据再到在 RViz 中可视化机器人模型的所有组件。

启动演示

我们假设您的 ROS 2 安装目录为 ~/ros2_ws 。请根据您的平台更改目录。

要启动演示，我们执行演示启动文件，我们将在下一节中详细解释。

源码构建:

mkdir -p ~/ros2_ws/src 
cd ~/ros2_ws/src 
git clone -b ${ROS_DISTRO} https://github.com/ros2/demos 
cd .. && colcon build –packages-up-to dummy_robot_bringup 
source ~/ros2_ws/install/setup.bash 
ros2 launch dummy_robot_bringup dummy_robot_bringup_launch.py

cxy@cxy-Ubuntu2404:~/second_ros2_ws$  ros2 launch dummy_robot_bringup dummy_robot_bringup_launch.py
[INFO] [launch]: All log files can be found below /home/cxy/.ros/log/2024-07-25-14-00-29-377480-cxy-Ubuntu2404-194696
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [dummy_map_server-1]: process started with pid [194699]
[INFO] [robot_state_publisher-2]: process started with pid [194700]
[INFO] [dummy_joint_states-3]: process started with pid [194701]
[INFO] [dummy_laser-4]: process started with pid [194702]
[dummy_laser-4] [INFO] [1721887229.677420145] [dummy_laser]: angle inc:  0.004363
[dummy_laser-4] [INFO] [1721887229.677495680] [dummy_laser]: scan size:  1081
[dummy_laser-4] [INFO] [1721887229.677505147] [dummy_laser]: scan time increment:   0.000000
[robot_state_publisher-2] [INFO] [1721887230.335038334] [robot_state_publisher]: Robot initialized

dummy_robot_bringup_launch.py

import os  # 导入操作系统模块


from launch import LaunchDescription  # 从launch模块导入LaunchDescription类
from launch.substitutions import FileContent  # 从launch.substitutions模块导入FileContent类
from launch_ros.actions import Node  # 从launch_ros.actions模块导入Node类
from launch_ros.substitutions import FindPackageShare  # 从launch_ros.substitutions模块导入FindPackageShare类


def generate_launch_description():  # 定义生成启动描述的函数
    pkg_share = FindPackageShare('dummy_robot_bringup').find('dummy_robot_bringup')  
    # 查找'dummy_robot_bringup'包的共享路径
    urdf_file = os.path.join(pkg_share, 'launch', 'single_rrbot.urdf')  
    # 构建URDF文件的路径
    robot_desc = FileContent(urdf_file)  
    # 读取URDF文件的内容
    rsp_params = {'robot_description': robot_desc}  
    # 创建包含机器人描述的参数字典


    return LaunchDescription([  # 返回一个LaunchDescription对象，包含以下节点
        Node(package='dummy_map_server', executable='dummy_map_server', output='screen'),  
        # 创建一个dummy_map_server节点，输出到屏幕
        Node(package='robot_state_publisher', executable='robot_state_publisher',
             output='screen', parameters=[rsp_params]),  
        # 创建一个robot_state_publisher节点，输出到屏幕，并传递机器人描述参数
        Node(package='dummy_sensors', executable='dummy_joint_states', output='screen'),  
        # 创建一个dummy_joint_states节点，输出到屏幕
        Node(package='dummy_sensors', executable='dummy_laser', output='screen')  
        # 创建一个dummy_laser节点，输出到屏幕
    ])

如果您现在在一个新的终端中打开 RViz2，您将看到您的机器人。🎉

$ source ~/ros2_ws/install/setup.bash
$ rviz2

这将打开 RViz2。假设你仍然启动了 dummy_robot_bringup，现在你可以添加 TF 显示插件并将全局框架配置为 world 。完成后，你应该会看到类似的图片：

发生了什么？

如果你仔细查看启动文件，我们会同时启动几个节点。

dummy_map_server 虚拟地图服务器
dummy_laser 虚拟激光
dummy_joint_states 虚拟关节状态
robot_state_publisher 机器人状态发布器

前两个包相对简单。 dummy_map_server 不断发布一个带有周期性更新的空地图。 dummy_laser 基本上做同样的事情；发布虚假的激光扫描。

dummy_joint_states 节点正在发布假的关节状态数据。由于我们正在发布一个只有两个关节的简单 RRbot，因此该节点发布这两个关节的关节状态值。

robot_state_publisher 正在做实际有趣的工作。它解析给定的 URDF 文件，提取机器人模型并监听传入的关节状态。通过这些信息，它为我们的机器人发布 TF 值，我们在 RViz 中可视化这些值。

附录

cxy@cxy-Ubuntu2404:~/second_ros2_ws/src/demos/dummy_robot$ tree
.
├── dummy_map_server
│   ├── CHANGELOG.rst
│   ├── CMakeLists.txt
│   ├── img
│   │   └── occupancy_grid.png
│   ├── package.xml
│   ├── README.md
│   └── src
│       └── dummy_map_server.cpp
├── dummy_robot_bringup
│   ├── CHANGELOG.rst
│   ├── CMakeLists.txt
│   ├── config
│   │   └── dummy_robot.rviz
│   ├── launch
│   │   ├── dummy_robot_bringup_launch.py
│   │   ├── dummy_robot_bringup_launch.xml
│   │   ├── dummy_robot_bringup_launch.yaml
│   │   └── single_rrbot.urdf
│   └── package.xml
└── dummy_sensors
    ├── CHANGELOG.rst
    ├── CMakeLists.txt
    ├── package.xml
    ├── README.md
    └── src
        ├── dummy_joint_states.cpp
        └── dummy_laser.cpp


9 directories, 20 files
cxy@cxy-Ubuntu2404:~/seco

这些代码片段展示了如何在ROS 2中创建和发布不同类型的消息，包括占用栅格、关节状态和激光扫描数据。以下是每个代码片段的总结：

占用栅格消息发布：

初始化ROS 2节点并创建一个名为dummy_map_server的节点。
创建一个发布nav_msgs::msg::OccupancyGrid消息的发布者。
设置占用栅格的分辨率、尺寸和原点位置。
初始化栅格数据为未知状态。
在循环中更新栅格数据并发布消息。

关节状态消息发布：

初始化ROS 2节点并创建一个名为dummy_joint_states的节点。
创建一个发布sensor_msgs::msg::JointState消息的发布者。
设置关节名称和初始位置。
在循环中计算关节位置的正弦值并更新消息。
发布关节状态消息。

激光扫描消息发布：

初始化ROS 2节点并创建一个名为dummy_laser的节点。
创建一个发布sensor_msgs::msg::LaserScan消息的发布者。
设置激光扫描的角度分辨率、起始角度、终止角度和扫描频率。
初始化扫描数据并设置相关参数。
在循环中计算距离值并更新消息。
发布激光扫描消息。

这些代码展示了如何使用ROS 2的节点、发布者和消息类型来模拟机器人传感器数据的发布过程。每个代码片段都包含了初始化、消息设置和循环发布的基本步骤。

dummy_map_server.cpp

#include <chrono>  // 导入时间相关的库
#include <iostream>  // 导入输入输出流库
#include <memory>  // 导入智能指针库


#include "rclcpp/clock.hpp"  // 导入ROS 2的时钟库
#include "rclcpp/rclcpp.hpp"  // 导入ROS 2的核心库
#include "rcl/rcl.h"  // 导入ROS 2的C API库
#include "rclcpp/time_source.hpp"  // 导入ROS 2的时间源库


#include "nav_msgs/msg/occupancy_grid.hpp"  // 导入占用栅格消息类型


int main(int argc, char * argv[])  // 主函数入口
{
  rclcpp::init(argc, argv);  // 初始化ROS 2


  auto node = rclcpp::Node::make_shared("dummy_map_server");  // 创建一个名为"dummy_map_server"的节点


  auto map_pub = node->create_publisher<nav_msgs::msg::OccupancyGrid>(  // 创建一个占用栅格消息的发布者
    "map",  // 话题名称为"map"
    rclcpp::QoS(rclcpp::KeepLast(1)).transient_local());  // 使用QoS设置，保留最后一条消息并设置为瞬态本地


  rclcpp::WallRate loop_rate(1);  // 设置循环频率为1Hz


  nav_msgs::msg::OccupancyGrid msg;  // 创建一个占用栅格消息对象
  msg.header.frame_id = "world";  // 设置消息的坐标系为"world"


  msg.info.resolution = 0.1f;  // 设置栅格分辨率为0.1米
  msg.info.width = 100;  // 设置栅格宽度为100
  msg.info.height = 100;  // 设置栅格高度为100
  msg.info.origin.position.x = -(msg.info.width * msg.info.resolution) / 2;  // 设置栅格原点的x坐标
  msg.info.origin.position.y = -(msg.info.width * msg.info.resolution) / 2;  // 设置栅格原点的y坐标
  msg.info.origin.position.z = 0;  // 设置栅格原点的z坐标
  msg.info.origin.orientation.x = 0;  // 设置栅格原点的四元数x分量
  msg.info.origin.orientation.y = 0;  // 设置栅格原点的四元数y分量
  msg.info.origin.orientation.z = 0;  // 设置栅格原点的四元数z分量
  msg.info.origin.orientation.w = 1;  // 设置栅格原点的四元数w分量


  for (size_t i = 0; i < msg.info.width * msg.info.height; ++i) {  // 初始化栅格数据
    msg.data.push_back(-1);  // 将所有栅格设置为未知状态
  }


  rclcpp::TimeSource ts(node);  // 创建一个时间源对象并附加到节点
  rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);  // 创建一个ROS时间的时钟对象
  ts.attachClock(clock);  // 将时钟附加到时间源


  int lhs = 0;  // 初始化左侧索引
  int center = 1;  // 初始化中心索引
  int rhs = 2;  // 初始化右侧索引
  while (rclcpp::ok()) {  // 循环直到ROS关闭
    msg.data[(lhs) % (msg.info.width * msg.info.height)] = -1;  // 设置左侧栅格为未知状态
    msg.data[(center) % (msg.info.width * msg.info.height)] = -1;  // 设置中心栅格为未知状态
    msg.data[(rhs) % (msg.info.width * msg.info.height)] = -1;  // 设置右侧栅格为未知状态
    msg.data[(++lhs) % (msg.info.width * msg.info.height)] = 0;  // 设置新的左侧栅格为空闲状态
    msg.data[(++center) % (msg.info.width * msg.info.height)] = 100;  // 设置新的中心栅格为占用状态
    msg.data[(++rhs) % (msg.info.width * msg.info.height)] = 0;  // 设置新的右侧栅格为空闲状态


    msg.header.stamp = clock->now();  // 更新消息的时间戳


    map_pub->publish(msg);  // 发布占用栅格消息
    rclcpp::spin_some(node);  // 处理节点的回调函数
    loop_rate.sleep();  // 休眠以保持循环频率
  }


  rclcpp::shutdown();  // 关闭ROS


  return 0;  // 返回0表示程序正常结束
}

dummy_joint_states.cpp

#include <chrono>  // 导入时间相关的库
#include <cmath>  // 导入数学库
#include <iostream>  // 导入输入输出流库
#include <memory>  // 导入智能指针库


#include "rclcpp/clock.hpp"  // 导入ROS 2的时钟库
#include "rclcpp/rclcpp.hpp"  // 导入ROS 2的核心库
#include "rclcpp/time_source.hpp"  // 导入ROS 2的时间源库


#include "sensor_msgs/msg/joint_state.hpp"  // 导入关节状态消息类型


int main(int argc, char * argv[])  // 主函数入口
{
  rclcpp::init(argc, argv);  // 初始化ROS 2


  auto node = rclcpp::Node::make_shared("dummy_joint_states");  // 创建一个名为"dummy_joint_states"的节点


  auto joint_state_pub = node->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);  
  // 创建一个关节状态消息的发布者，话题名称为"joint_states"，队列大小为10


  rclcpp::WallRate loop_rate(50);  // 设置循环频率为50Hz


  sensor_msgs::msg::JointState msg;  // 创建一个关节状态消息对象
  msg.name.push_back("single_rrbot_joint1");  // 添加关节名称"single_rrbot_joint1"
  msg.name.push_back("single_rrbot_joint2");  // 添加关节名称"single_rrbot_joint2"
  msg.position.push_back(0.0);  // 初始化关节1的位置为0.0
  msg.position.push_back(0.0);  // 初始化关节2的位置为0.0


  rclcpp::TimeSource ts(node);  // 创建一个时间源对象并附加到节点
  rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);  // 创建一个ROS时间的时钟对象
  ts.attachClock(clock);  // 将时钟附加到时间源


  auto counter = 0.0;  // 初始化计数器
  auto joint_value = 0.0;  // 初始化关节值
  while (rclcpp::ok()) {  // 循环直到ROS关闭
    counter += 0.1;  // 增加计数器
    joint_value = std::sin(counter);  // 计算关节值为正弦函数


    for (size_t i = 0; i < msg.name.size(); ++i) {  // 更新所有关节的位置
      msg.position[i] = joint_value;  // 设置关节位置为计算的关节值
    }


    msg.header.stamp = clock->now();  // 更新消息的时间戳


    joint_state_pub->publish(msg);  // 发布关节状态消息
    rclcpp::spin_some(node);  // 处理节点的回调函数
    loop_rate.sleep();  // 休眠以保持循环频率
  }


  rclcpp::shutdown();  // 关闭ROS


  return 0;  // 返回0表示程序正常结束
}

dummy_laser.cpp

#ifdef _MSC_VER  // 如果使用的是Microsoft编译器
#ifndef _USE_MATH_DEFINES  // 如果没有定义_USE_MATH_DEFINES
#define _USE_MATH_DEFINES  // 定义_USE_MATH_DEFINES以便使用数学常量
#endif
#endif


#include <math.h>  // 导入数学库


#include <chrono>  // 导入时间相关的库
#include <iostream>  // 导入输入输出流库
#include <memory>  // 导入智能指针库


#include "rclcpp/clock.hpp"  // 导入ROS 2的时钟库
#include "rclcpp/rclcpp.hpp"  // 导入ROS 2的核心库
#include "rclcpp/time_source.hpp"  // 导入ROS 2的时间源库


#include "sensor_msgs/msg/laser_scan.hpp"  // 导入激光扫描消息类型


#define DEG2RAD M_PI / 180.0  // 定义角度到弧度的转换常量


int main(int argc, char * argv[])  // 主函数入口
{
  rclcpp::init(argc, argv);  // 初始化ROS 2


  auto node = rclcpp::Node::make_shared("dummy_laser");  // 创建一个名为"dummy_laser"的节点


  auto laser_pub = node->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);  
  // 创建一个激光扫描消息的发布者，话题名称为"scan"，队列大小为10


  rclcpp::WallRate loop_rate(30);  // 设置循环频率为30Hz


  sensor_msgs::msg::LaserScan msg;  // 创建一个激光扫描消息对象
  msg.header.frame_id = "single_rrbot_hokuyo_link";  // 设置消息的坐标系为"single_rrbot_hokuyo_link"


  double angle_resolution = 2500;  // 设置角度分辨率
  double start_angle = -450000;  // 设置起始角度
  double stop_angle = 2250000;  // 设置终止角度
  double scan_frequency = 2500;  // 设置扫描频率


  double angle_range = stop_angle - start_angle;  // 计算角度范围
  double num_values = angle_range / angle_resolution;  // 计算扫描值的数量
  if (static_cast<int>(angle_range) % static_cast<int>(angle_resolution) == 0) {
    // 如果角度范围是角度分辨率的整数倍，包含终点
    ++num_values;
  }
  msg.ranges.resize(static_cast<int>(num_values));  // 调整ranges数组的大小


  // 设置time_increment为0以避免在rviz2中显示不稳定状态
  msg.time_increment = 0.0f;
  msg.angle_increment = static_cast<float>(angle_resolution / 10000.0 * DEG2RAD);  // 设置角度增量
  msg.angle_min = static_cast<float>(start_angle / 10000.0 * DEG2RAD - M_PI / 2);  // 设置最小角度
  msg.angle_max = static_cast<float>(stop_angle / 10000.0 * DEG2RAD - M_PI / 2);  // 设置最大角度
  msg.scan_time = static_cast<float>(100.0 / scan_frequency);  // 设置扫描时间
  msg.range_min = 0.0f;  // 设置最小范围
  msg.range_max = 10.0f;  // 设置最大范围


  RCLCPP_INFO(node->get_logger(), "angle inc:\t%f", msg.angle_increment);  // 输出角度增量信息
  RCLCPP_INFO(node->get_logger(), "scan size:\t%zu", msg.ranges.size());  // 输出扫描大小信息
  RCLCPP_INFO(node->get_logger(), "scan time increment: \t%f", msg.time_increment);  // 输出扫描时间增量信息


  rclcpp::TimeSource ts(node);  // 创建一个时间源对象并附加到节点
  rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);  // 创建一个ROS时间的时钟对象
  ts.attachClock(clock);  // 将时钟附加到时间源


  auto counter = 0.0;  // 初始化计数器
  auto amplitude = 1;  // 设置振幅
  auto distance = 0.0f;  // 初始化距离
  while (rclcpp::ok()) {  // 循环直到ROS关闭
    counter += 0.1;  // 增加计数器
    distance = static_cast<float>(std::abs(amplitude * std::sin(counter)));  // 计算距离为正弦函数的绝对值


    for (size_t i = 0; i < msg.ranges.size(); ++i) {  // 更新所有扫描值
      msg.ranges[i] = distance;  // 设置扫描值为计算的距离
    }


    msg.header.stamp = clock->now();  // 更新消息的时间戳


    laser_pub->publish(msg);  // 发布激光扫描消息
    rclcpp::spin_some(node);  // 处理节点的回调函数
    loop_rate.sleep();  // 休眠以保持循环频率
  }


  rclcpp::shutdown();  // 关闭ROS


  return 0;  // 返回0表示程序正常结束
}