目标:用障碍物避让节点扩展机器人模拟。
教程级别:高级
时间:20 分钟
目录
-
背景
-
先决条件
-
任务
-
1 更新
my_robot.urdf
-
2 创建一个 ROS 节点以避免障碍物
-
3 更新附加文件
-
4 测试避障代码
-
-
摘要
-
下一步
背景
在本教程中,您将扩展教程第一部分中创建的软件包:设置机器人模拟(基础)。目的是实现一个使用机器人距离传感器避开障碍物的 ROS 2 节点 。本教程重点介绍使用具有 webots_ros2_driver
接口的机器人设备。
先决条件
这是教程第一部分的延续:设置机器人模拟(基础)。必须从第一部分开始设置自定义包和必要的文件。
本教程兼容 webots_ros2
版本 2023.1.0 和 Webots R2023b 以及即将发布的版本。
任务
1 更新 my_robot.urdf
如设置机器人模拟(基础)中所述, webots_ros2_driver
包含可将大多数 Webots 设备直接与 ROS 2 连接的插件。这些插件可以使用机器人的 URDF 文件中的 <device>
标签加载 。 reference
属性应与 Webots 设备的 name
参数匹配 。所有现有接口及相应参数的列表可以在设备参考页面 https://github.com/cyberbotics/webots_ros2/wiki/References-Devices 上找到。对于未在 URDF 文件中配置的可用设备,接口将自动创建,并且 ROS 参数将使用默认值(例如 update rate
、 topic name
和 frame name
)。
在 my_robot.urdf
中替换整个内容为:
Python:
xml
<?xml version="1.0" ?>
<robot name="My robot">
<!-- 定义机器人的名称为 "My robot" -->
<webots>
<!-- Webots 机器人仿真器相关配置 -->
<device reference="ds0" type="DistanceSensor">
<!-- 定义一个名为 ds0 的距离传感器设备 -->
<ros>
<!-- ROS 相关配置 -->
<topicName>/left_sensor</topicName>
<!-- 定义 ROS 话题名称为 /left_sensor -->
<alwaysOn>true</alwaysOn>
<!-- 设置传感器设备始终开启 -->
</ros>
</device>
<device reference="ds1" type="DistanceSensor">
<!-- 定义另一个名为 ds1 的距离传感器设备 -->
<ros>
<!-- ROS 相关配置 -->
<topicName>/right_sensor</topicName>
<!-- 定义 ROS 话题名称为 /right_sensor -->
<alwaysOn>true</alwaysOn>
<!-- 设置传感器设备始终开启 -->
</ros>
</device>
<plugin type="my_package.my_robot_driver.MyRobotDriver" />
<!-- 加载自定义插件 my_package.my_robot_driver.MyRobotDriver -->
</webots>
</robot>
C++:
xml
<?xml version="1.0" ?>
<robot name="My robot">
<webots>
<device reference="ds0" type="DistanceSensor">
<ros>
<topicName>/left_sensor</topicName>
<alwaysOn>true</alwaysOn>
</ros>
</device>
<device reference="ds1" type="DistanceSensor">
<ros>
<topicName>/right_sensor</topicName>
<alwaysOn>true</alwaysOn>
</ros>
</device>
<plugin type="my_robot_driver::MyRobotDriver" />
</webots>
</robot>
除了您的自定义插件,webots_ros2_driver
将解析指向 DistanceSensor 节点的 <device>
标签 ,并使用 <ros>
标签中的标准参数来启用传感器并命名它们的主题。
2 创建一个 ROS 节点以避免障碍物
Python: 定义了一个名为ObstacleAvoider
的类,继承自Node
类。该类初始化时创建了两个订阅者,分别订阅left_sensor
和right_sensor
话题,并定义了相应的回调函数。在回调函数中,根据传感器的距离值,发布控制机器人运动的Twist
消息。主函数中,初始化rclpy,创建ObstacleAvoider
节点对象,并进入循环等待消息。
机器人将使用标准的 ROS 节点来检测墙壁并发送电机命令以避开它 。在 my_package/my_package/
文件夹中,创建一个名为 obstacle_avoider.py
的文件,并使用以下代码:
ruby
import rclpy # 导入rclpy库
from rclpy.node import Node # 从rclpy.node模块导入Node类
from sensor_msgs.msg import Range # 从sensor_msgs.msg模块导入Range消息类型
from geometry_msgs.msg import Twist # 从geometry_msgs.msg模块导入Twist消息类型
MAX_RANGE = 0.15 # 定义最大范围常量为0.15米
class ObstacleAvoider(Node): # 定义一个名为ObstacleAvoider的类,继承自Node类
def __init__(self): # 初始化方法
super().__init__('obstacle_avoider') # 调用父类的初始化方法,并命名节点为'obstacle_avoider'
self.__publisher = self.create_publisher(Twist, 'cmd_vel', 1) # 创建一个发布者,发布Twist消息到'cmd_vel'话题
self.create_subscription(Range, 'left_sensor', self.__left_sensor_callback, 1) # 创建一个订阅者,订阅'left_sensor'话题,并指定回调函数
self.create_subscription(Range, 'right_sensor', self.__right_sensor_callback, 1) # 创建一个订阅者,订阅'right_sensor'话题,并指定回调函数
def __left_sensor_callback(self, message): # 定义左传感器的回调函数
self.__left_sensor_value = message.range # 获取左传感器的距离值
def __right_sensor_callback(self, message): # 定义右传感器的回调函数
self.__right_sensor_value = message.range # 获取右传感器的距离值
command_message = Twist() # 创建一个Twist消息对象
command_message.linear.x = 0.1 # 设置线速度为0.1米/秒
if self.__left_sensor_value < 0.9 * MAX_RANGE or self.__right_sensor_value < 0.9 * MAX_RANGE: # 如果任一传感器的距离值小于最大范围的90%
command_message.angular.z = -2.0 # 设置角速度为-2.0弧度/秒
self.__publisher.publish(command_message) # 发布Twist消息
def main(args=None): # 定义主函数
rclpy.init(args=args) # 初始化rclpy
avoider = ObstacleAvoider() # 创建ObstacleAvoider节点对象
rclpy.spin(avoider) # 让节点进入循环,等待消息
avoider.destroy_node() # 显式销毁节点(可选)
rclpy.shutdown() # 关闭rclpy
if __name__ == '__main__': # 如果脚本是直接执行的
main() # 调用主函数
此节点将在此处为命令创建一个发布者并订阅传感器主题
php
self.__publisher = self.create_publisher(Twist, 'cmd_vel', 1)
self.create_subscription(Range, 'left_sensor', self.__left_sensor_callback, 1)
self.create_subscription(Range, 'right_sensor', self.__right_sensor_callback, 1)
当从左传感器接收到测量值时,它将被复制到一个成员字段中
ruby
def __left_sensor_callback(self, message):
self.__left_sensor_value = message.range
最后,当收到来自右侧传感器的测量值时,将向 /cmd_vel
主题发送消息。 command_message
将在 linear.x
中至少注册一个前进速度 ,以便在没有检测到障碍物时使机器人移动。如果两个传感器中的任何一个检测到障碍物, command_message
还将在 angular.z
中注册一个旋转速度,以便使机器人向右转。
ruby
def __right_sensor_callback(self, message):
self.__right_sensor_value = message.range
command_message = Twist()
command_message.linear.x = 0.1
if self.__left_sensor_value < 0.9 * MAX_RANGE or self.__right_sensor_value < 0.9 * MAX_RANGE:
command_message.angular.z = -2.0
self.__publisher.publish(command_message)
C++:
机器人将使用标准的 ROS 节点来检测墙壁并发送电机命令以避开它。在 my_package/include/my_package
文件夹中,创建一个名为 ObstacleAvoider.hpp
的头文件,并使用以下代码:
cpp
#include <memory>
#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/range.hpp"
class ObstacleAvoider : public rclcpp::Node {
public:
explicit ObstacleAvoider();
// 显式构造函数,初始化节点
private:
void leftSensorCallback(const sensor_msgs::msg::Range::SharedPtr msg);
// 左传感器回调函数,处理传感器数据
void rightSensorCallback(const sensor_msgs::msg::Range::SharedPtr msg);
// 右传感器回调函数,处理传感器数据
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
// 发布器,用于发布Twist消息
rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr left_sensor_sub_;
// 订阅器,用于订阅左传感器的Range消息
rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr right_sensor_sub_;
// 订阅器,用于订阅右传感器的Range消息
double left_sensor_value{0.0};
// 左传感器的测量值,初始值为0.0
double right_sensor_value{0.0};
// 右传感器的测量值,初始值为0.0
};
在 my_package/src
文件夹中,创建一个名为 ObstacleAvoider.cpp
的源文件,并使用以下代码:
php
#include "my_package/ObstacleAvoider.hpp"
#define MAX_RANGE 0.15
// 定义最大距离为0.15米
ObstacleAvoider::ObstacleAvoider() : Node("obstacle_avoider") {
publisher_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1);
// 创建一个发布器,发布Twist消息到"/cmd_vel"话题
left_sensor_sub_ = create_subscription<sensor_msgs::msg::Range>(
"/left_sensor", 1,
[this](const sensor_msgs::msg::Range::SharedPtr msg){
return this->leftSensorCallback(msg);
}
);
// 创建一个订阅器,订阅"/left_sensor"话题,并将回调函数设为leftSensorCallback
right_sensor_sub_ = create_subscription<sensor_msgs::msg::Range>(
"/right_sensor", 1,
[this](const sensor_msgs::msg::Range::SharedPtr msg){
return this->rightSensorCallback(msg);
}
);
// 创建一个订阅器,订阅"/right_sensor"话题,并将回调函数设为rightSensorCallback
}
void ObstacleAvoider::leftSensorCallback(
const sensor_msgs::msg::Range::SharedPtr msg) {
left_sensor_value = msg->range;
// 更新左传感器的测量值
}
void ObstacleAvoider::rightSensorCallback(
const sensor_msgs::msg::Range::SharedPtr msg) {
right_sensor_value = msg->range;
// 更新右传感器的测量值
auto command_message = std::make_unique<geometry_msgs::msg::Twist>();
// 创建一个Twist消息对象,用于存储速度命令
command_message->linear.x = 0.1;
// 设置线速度为0.1米/秒
if (left_sensor_value < 0.9 * MAX_RANGE ||
right_sensor_value < 0.9 * MAX_RANGE) {
command_message->angular.z = -2.0;
// 如果任一传感器的值小于最大距离的90%,则设置角速度为-2.0弧度/秒
}
publisher_->publish(std::move(command_message));
// 发布速度命令消息
}
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
// 初始化rclcpp库
auto avoider = std::make_shared<ObstacleAvoider>();
// 创建ObstacleAvoider节点对象
rclcpp::spin(avoider);
// 让节点进入循环,处理回调函数
rclcpp::shutdown();
// 关闭rclcpp库
return 0;
// 返回0,表示程序正常结束
}
此节点将在此处为命令创建一个发布者并订阅传感器主题
php
publisher_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1);
left_sensor_sub_ = create_subscription<sensor_msgs::msg::Range>(
"/left_sensor", 1,
[this](const sensor_msgs::msg::Range::SharedPtr msg){
return this->leftSensorCallback(msg);
}
);
right_sensor_sub_ = create_subscription<sensor_msgs::msg::Range>(
"/right_sensor", 1,
[this](const sensor_msgs::msg::Range::SharedPtr msg){
return this->rightSensorCallback(msg);
}
);
当从左传感器接收到测量值时,它将被复制到一个成员字段中
php
void ObstacleAvoider::leftSensorCallback(
const sensor_msgs::msg::Range::SharedPtr msg) {
left_sensor_value = msg->range;
}
最后,当收到来自右侧传感器的测量值时,将向 /cmd_vel
主题发送消息。 command_message
将在 linear.x
中至少注册一个前进速度,以便在没有检测到障碍物时使机器人移动。如果两个传感器中的任何一个检测到障碍物, command_message
还将在 angular.z
中注册一个旋转速度,以便使机器人向右转。
php
void ObstacleAvoider::rightSensorCallback(
const sensor_msgs::msg::Range::SharedPtr msg) {
right_sensor_value = msg->range;
auto command_message = std::make_unique<geometry_msgs::msg::Twist>();
command_message->linear.x = 0.1;
if (left_sensor_value < 0.9 * MAX_RANGE ||
right_sensor_value < 0.9 * MAX_RANGE) {
command_message->angular.z = -2.0;
}
publisher_->publish(std::move(command_message));
}
3 更新附加文件
你必须修改这两个其他文件以启动你的新节点。
Python:
编辑 setup.py
并将 'console_scripts'
替换为:
javascript
'console_scripts': [
'my_robot_driver = my_package.my_robot_driver:main',
'obstacle_avoider = my_package.obstacle_avoider:main'
],
这将为 obstacle_avoider
节点添加一个入口点。
转到文件 robot_launch.py
并将其替换为:
python
import os # 导入os模块,用于处理操作系统相关的功能
import launch # 导入launch模块,用于创建Launch描述符
from launch_ros.actions import Node # 从launch_ros.actions模块导入Node类,用于创建ROS 2节点
from launch import LaunchDescription # 从launch模块导入LaunchDescription类,用于定义Launch描述符
from ament_index_python.packages import get_package_share_directory # 从ament_index_python.packages模块导入get_package_share_directory函数,用于获取包共享目录
from webots_ros2_driver.webots_launcher import WebotsLauncher # 从webots_ros2_driver.webots_launcher模块导入WebotsLauncher类,用于启动Webots仿真器
from webots_ros2_driver.webots_controller import WebotsController # 从webots_ros2_driver.webots_controller模块导入WebotsController类,用于控制Webots中的机器人
def generate_launch_description():
package_dir = get_package_share_directory('my_package') # 获取my_package包的共享目录
robot_description_path = os.path.join(package_dir, 'resource', 'my_robot.urdf') # 生成机器人描述文件的路径
webots = WebotsLauncher(
world=os.path.join(package_dir, 'worlds', 'my_world.wbt') # 创建WebotsLauncher实例,加载指定的世界文件my_world.wbt
)
my_robot_driver = WebotsController(
robot_name='my_robot', # 指定机器人名称为my_robot
parameters=[
{'robot_description': robot_description_path}, # 设置机器人描述参数
]
)
obstacle_avoider = Node(
package='my_package', # 指定节点所属的包为my_package
executable='obstacle_avoider', # 指定可执行文件名称为obstacle_avoider
)
return LaunchDescription([
webots, # 启动Webots仿真器
my_robot_driver, # 启动Webots中的机器人控制器
obstacle_avoider, # 启动障碍物回避节点
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=webots, # 目标动作是Webots仿真器
on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], # 当Webots仿真器进程退出时,触发关闭事件
)
)
])
这将创建一个 obstacle_avoider
节点,该节点将包含在 LaunchDescription
中。
C++:
编辑 CMakeLists.txt
并添加 obstacle_avoider
的编译和安装:
sql
cmake_minimum_required(VERSION 3.5) # 设置CMake的最低版本要求为3.5
project(my_package) # 定义项目名称为my_package
if(NOT CMAKE_CXX_STANDARD) # 如果没有设置C++标准
set(CMAKE_CXX_STANDARD 14) # 设置C++标准为14
endif()
# 除了包特定的依赖项,我们还需要`pluginlib`和`webots_ros2_driver`
find_package(ament_cmake REQUIRED) # 查找ament_cmake包
find_package(rclcpp REQUIRED) # 查找rclcpp包
find_package(std_msgs REQUIRED) # 查找std_msgs包
find_package(geometry_msgs REQUIRED) # 查找geometry_msgs包
find_package(pluginlib REQUIRED) # 查找pluginlib包
find_package(webots_ros2_driver REQUIRED) # 查找webots_ros2_driver包
# 导出插件配置文件
pluginlib_export_plugin_description_file(webots_ros2_driver my_robot_driver.xml)
# 障碍物避让器
include_directories(
include # 包含头文件目录
)
add_executable(obstacle_avoider
src/ObstacleAvoider.cpp # 添加可执行文件obstacle_avoider,源文件为src/ObstacleAvoider.cpp
)
ament_target_dependencies(obstacle_avoider
rclcpp # 设置obstacle_avoider的依赖项为rclcpp
geometry_msgs # 设置obstacle_avoider的依赖项为geometry_msgs
sensor_msgs # 设置obstacle_avoider的依赖项为sensor_msgs
)
install(TARGETS
obstacle_avoider # 安装目标obstacle_avoider
DESTINATION lib/${PROJECT_NAME} # 安装路径为lib/${PROJECT_NAME}
)
install(
DIRECTORY include/ # 安装include目录
DESTINATION include # 安装路径为include
)
# MyRobotDriver库
add_library(# 添加共享库,源文件为src/MyRobotDriver.cpp
${PROJECT_NAME} # 添加库,名称为项目名称
SHARED # 设置为共享库
src/MyRobotDriver.cpp # 源文件为src/MyRobotDriver.cpp
)
target_include_directories( # 指定包含目录
${PROJECT_NAME} # 设置库的名称为项目名称
PRIVATE # 设置为私有
include # 包含头文件目录
)
ament_target_dependencies(# 指定目标依赖项
${PROJECT_NAME} # 设置库的名称为项目名称
pluginlib # 设置库的依赖项为pluginlib
rclcpp # 设置库的依赖项为rclcpp
webots_ros2_driver # 设置库的依赖项为webots_ros2_driver
)
install(TARGETS
${PROJECT_NAME} # 安装目标为项目名称
ARCHIVE DESTINATION lib # 安装路径为lib
LIBRARY DESTINATION lib # 安装路径为lib
RUNTIME DESTINATION bin # 安装路径为bin
)
# 安装其他目录
install(DIRECTORY
launch # 安装launch目录
resource # 安装resource目录
worlds # 安装worlds目录
DESTINATION share/${PROJECT_NAME}/ # 安装路径为share/${PROJECT_NAME}/
)
ament_export_include_directories(
include # 导出include目录
)
ament_export_libraries(
${PROJECT_NAME} # 导出库,名称为项目名称
)
ament_package() # 定义ament包
转到文件 robot_launch.py
并将其替换为:
python
import os # 导入os模块,用于处理操作系统相关的功能
import launch # 导入launch模块,用于创建Launch描述符
from launch_ros.actions import Node # 从launch_ros.actions模块导入Node类,用于创建ROS 2节点
from launch import LaunchDescription # 从launch模块导入LaunchDescription类,用于定义Launch描述符
from ament_index_python.packages import get_package_share_directory # 从ament_index_python.packages模块导入get_package_share_directory函数,用于获取包共享目录
from webots_ros2_driver.webots_launcher import WebotsLauncher # 从webots_ros2_driver.webots_launcher模块导入WebotsLauncher类,用于启动Webots仿真器
from webots_ros2_driver.webots_controller import WebotsController # 从webots_ros2_driver.webots_controller模块导入WebotsController类,用于控制Webots中的机器人
def generate_launch_description():
package_dir = get_package_share_directory('my_package') # 获取my_package包的共享目录
robot_description_path = os.path.join(package_dir, 'resource', 'my_robot.urdf') # 生成机器人描述文件的路径
webots = WebotsLauncher(
world=os.path.join(package_dir, 'worlds', 'my_world.wbt') # 创建WebotsLauncher实例,加载指定的世界文件my_world.wbt
)
my_robot_driver = WebotsController(
robot_name='my_robot', # 指定机器人名称为my_robot
parameters=[
{'robot_description': robot_description_path}, # 设置机器人描述参数
]
)
obstacle_avoider = Node(
package='my_package', # 指定节点所属的包为my_package
executable='obstacle_avoider', # 指定可执行文件名称为obstacle_avoider
)
return LaunchDescription([
webots, # 启动Webots仿真器
my_robot_driver, # 启动Webots中的机器人控制器
obstacle_avoider, # 启动障碍物回避节点
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=webots, # 目标动作是Webots仿真器
on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], # 当Webots仿真器进程退出时,触发关闭事件
)
)
])
这将创建一个 obstacle_avoider
节点,该节点将包含在 LaunchDescription
中。
4 测试避障代码
从你的 ROS 2 工作区的终端启动模拟:
Linux: 在你的 ROS 2 工作区的终端中运行:
properties
colcon build --packages-select my_package
source install/local_setup.bash
ros2 launch my_package robot_launch.py
您的机器人应该向前移动,在撞到墙之前应顺时针转弯。您可以在 Webots 中按 Ctrl+F10
或转到 View
菜单, Optional Rendering
和 Show DistanceSensor Rays
以显示机器人的距离传感器范围。
cs
cxy@ubuntu2404-cxy:~/ros2_ws$ colcon build --packages-select my_package
Starting >>> my_package
Finished <<< my_package [10.7s]
Summary: 1 package finished [21.7s]
cxy@ubuntu2404-cxy:~/ros2_ws$ source install/local_setup.bash
cxy@ubuntu2404-cxy:~/ros2_ws$ ros2 launch my_package robot_launch.py
[INFO] [launch]: All log files can be found below /home/cxy/.ros/log/2024-07-18-10-06-49-551812-ubuntu2404-cxy-9720
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [webots-1]: process started with pid [9731]
[INFO] [webots_controller_my_robot-2]: process started with pid [9732]
[INFO] [obstacle_avoider-3]: process started with pid [9733]
[webots_controller_my_robot-2] The specified robot is not in the list of robots with <extern> controllers, retrying for another 50 seconds...
[webots_controller_my_robot-2] The Webots simulation world is not yet ready, pending until loading is done...
[webots_controller_my_robot-2] [INFO] [1721268424.071231410] [my_robot]: Controller successfully connected to robot in Webots simulation.
摘要
在本教程中,您扩展了基本仿真,添加了一个障碍物回避器 ROS 2 节点,该节点根据机器人距离传感器的值发布速度命令。
下一步
您可能需要改进插件或创建新节点以更改机器人的行为。您还可以实现一个重置处理程序,以便在从 Webots 界面重置仿真时自动重新启动您的 ROS 节点。