概述:
上一篇博客:ROS2进阶第一章 – 从头开始构建一个可视化的差速轮式机器人模型 – 学习URDF机器人建模与xacro优化,我们使用urdf和xacro搞了一个差速轮式机器人的外形建模,并使用 rviz 可视化查看。
本文是此系列的第二章,我们将使用gazebo仿真环境创建一个世界,并将上一节制作的机器人加载到仿真环境中,在通过键盘控制节点来控制小车移动,并通过rviz实时察看 camera,kinect和lidar三种传感器的仿真效果。
参考资料
- ROS2入门21讲——学习笔记(二)常用工具部分15-21讲中的第18讲
- ROS2高效进阶 – ROS2高级组件之gazebo11学习 – 模型库和编辑模型
- Gazebo官方教程
- ros2-control介绍
- 欢迎阅读ros2_control文档!
- ROS高效进阶第三章 – 以差速轮式机器人为例,使用Gazebo构建机器人仿真平台
1. 绘制 my_house空间模型
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python mbot_gazebo --dependencies urdf xacro gazebo_plugins gazebo_ros gazebo_ros_control geometry_msgs rclcpp rclpy
cd mbot_gazebo
mkdir -p config launch meshes scripts urdf/sensor worlds rviz
(2)启动gazebo,绘制my_house空间模型,并保存 my_house.sdf文件。绘制模型的过程可以参考:ROS2高效进阶 – ROS2高级组件之gazebo11学习 – 模型库和编辑模型
查看保存好的模型:在模型的保存路径下打开终端,运行下面的命令
gazebo --verbose <world_file>
gazebo --verbose my_house.sdf
下面是我刚刚绘制的世界
2 为机器人模型添加物理属性和差速控制器插件
(1)在 mbot_gazebo 的 urdf 目录内,创建机器人模型文件
cd ~/ros2_ws/src/mbot_gazebo
touch mbot_base_gazebo.xacro mbot_gazebo.xacro
touch sensors/camera_gazebo.xacro sensors/kinect_gazebo.xacro sensors/lidar_gazebo.xacro
(2)转动惯量和惯性矩阵:
转动惯量:Moment of Inertia,在物理学中,用于描述物体绕特定轴线旋转时的惯性大小,也就是物体抵抗旋转运动的能力。具体而言,对于某个轴线,离该轴线越远的物体质量对转动惯量的贡献越大。
惯性矩阵:描述物体关于某一点的转动惯量的3x3的对称矩阵。在ROS中,这个矩阵的形式如下:
| ixx ixy ixz |
| ixy iyy iyz |
| ixz iyz izz |
对角线上的元素(ixx, iyy, izz)分别表示物体绕x轴、y轴和z轴的主转动惯量。非对角元素(ixy, ixz, iyz)表示物体关于不同轴之间的耦合作用,也就是说他们表示当物体同时绕多个轴旋转时存在的影响,这些被称为非对角转动惯量。
对于均匀的几何体(如长方体、圆柱体、球体等),它们的主轴通常与物体的对称轴相沿。在这种情况下,如果我们选择这些对称轴作为参考系(坐标系),那么非对角元素(也就是耦合项)将会消失,所以设为0。如果,物体旋转轴并不完全沿着这些主轴,那么就需要计算和考虑非对角元素了。
(3)圆柱体的惯性矩阵:m是物体质量,r是圆柱半径,h是圆柱高度
ixx:圆柱体绕x轴的转动惯量,I = m * (3 * r^2 + h^2) / 12
iyy:圆柱体绕y轴的转动惯量,与绕x轴相同
izz:圆柱体绕z轴的转动惯量,I = m * r^2 / 2
ixy, ixz, iyz均为0,表示非对角转动惯量为0。
(4)球体的惯性矩阵:m是物体质量,r是圆柱半径
ixx/iyy/izz:球体绕x,y,z轴的转动惯量公式:I = 2 * m * r^2 / 5
ixy, ixz, iyz均为0,表示非对角转动惯量为0。
(5)惯性矩阵(inertial matrix)和碰撞参数(collision properties)
在 ROS2进阶第一章 – 从头开始构建一个可视化的差速轮式机器人模型 – 学习URDF机器人建模与xacro优化 中,我们讲了一个刚体 link,有外观和物理两大属性,其中物理属性又包括:惯性矩阵(inertial matrix)和碰撞参数(collision properties)。
以差速轮式机器人主导轮为例,其惯性矩阵为:
<xacro:macro name="cylinder_inertial_matrix" params="m r h">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
izz="${m*r*r/2}" />
</inertial>
</xacro:macro>
以差速轮式机器人万向轮为例,其惯性矩阵为:
<xacro:macro name="sphere_inertial_matrix" params="m r">
<inertial>
<mass value="${m}" />
<inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
iyy="${2*m*r*r/5}" iyz="0"
izz="${2*m*r*r/5}" />
</inertial>
</xacro:macro>
至于刚体的碰撞参数,一般与外观一致,以底盘base_link为例,其外观,惯性矩阵和碰撞参数为:
<link name="base_link">
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${base_length}" radius="${base_radius}"/>
</geometry>
<material name="yellow" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${base_length}" radius="${base_radius}"/>
</geometry>
</collision>
<xacro:cylinder_inertial_matrix m="${base_mass}" r="${base_radius}" h="${base_length}" />
</link>
(6)刚体link如果要在gazebo上显示,必须使用gazebo标签,可以添加颜色,或者关闭重力:
<gazebo reference="${prefix}_wheel_link">
<material>Gazebo/White</material>
</gazebo>
// base_footprint是影子link,无重力
<gazebo reference="base_footprint">
<turnGravityOff>false</turnGravityOff>
</gazebo>
(7)ros2_control框架
一个机器人实现运动,无论是自主运动还是被动控制运动,在应用程序与底层执行机构之间,需要一个控制中间件,其能对各种底层硬件,无论是真实的还是仿真的,进行抽象,并提供多种控制器,而这就是ros提供的ros_control。ros_control以插件的形式,提供了多种控制器,本文的差速轮式机器人使用的是差速控制器插件。
ros2_control框架的架构如下图所示:
上半部分是控制者(控制器插件):不直接接触硬件,从抽象层请求资源
下半部分是机器人的硬件:管理硬件资源,处理硬件冲突
(8)差速控制器
为差速轮式机器人的两个主动轮,添加传动装置,这里添加的是两个速度控制接口。
<transmission name="${prefix}_wheel_joint_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}_wheel_joint" >
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="${prefix}_wheel_joint_motor">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
然后为差速轮式机器人添加差速控制器插件:
<gazebo>
<plugin name="differential_drive_controller"
filename="libgazebo_ros_diff_drive.so">
<update_rate>30</update_rate>
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>${wheel_joint_y*2}</wheel_separation>
<wheel_diameter>${2*wheel_radius}</wheel_diameter>
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>
<command_topic>cmd_vel</command_topic>
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
<odometry_topic>odom</odometry_topic>
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_footprint</robot_base_frame>
<odometry_source>1</odometry_source>
</plugin>
</gazebo>
(9)完整内容如下:mbot_base_gazebo.xacro
<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- PROPERTY LIST -->
<xacro:property name="M_PI" value="3.1415926"/>
<xacro:property name="base_mass" value="1" />
<xacro:property name="base_radius" value="0.20"/>
<xacro:property name="base_length" value="0.16"/>
<xacro:property name="pillar_length" value="0.22"/>
<xacro:property name="pillar_radius" value="0.03"/>
<xacro:property name="wheel_mass" value="0.2" />
<xacro:property name="wheel_radius" value="0.06"/>
<xacro:property name="wheel_length" value="0.025"/>
<xacro:property name="wheel_joint_y" value="0.19"/>
<xacro:property name="wheel_joint_z" value="0.05"/>
<xacro:property name="caster_mass" value="0.2" />
<xacro:property name="caster_radius" value="0.015"/> <!-- wheel_radius - ( base_length/2 - wheel_joint_z) -->
<xacro:property name="caster_joint_x" value="0.18"/>
<!-- Defining the colors used in this robot -->
<material name="yellow">
<color rgba="1 0.4 0 1"/>
</material>
<material name="black">
<color rgba="0 0 0 0.95"/>
</material>
<material name="gray">
<color rgba="0.75 0.75 0.75 1"/>
</material>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
<!-- Macro for inertia matrix -->
<xacro:macro name="sphere_inertial_matrix" params="m r">
<inertial>
<mass value="${m}" />
<inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
iyy="${2*m*r*r/5}" iyz="0"
izz="${2*m*r*r/5}" />
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertial_matrix" params="m r h">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
izz="${m*r*r/2}" />
</inertial>
</xacro:macro>
<!-- Macro for robot wheel -->
<xacro:macro name="wheel" params="prefix reflect">
<joint name="${prefix}_wheel_joint" type="continuous">
<origin xyz="0 ${reflect*wheel_joint_y} ${-wheel_joint_z}" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="${prefix}_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="${prefix}_wheel_link">
<visual>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
</geometry>
<material name="gray" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
</geometry>
</collision>
<xacro:cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
</link>
<gazebo reference="${prefix}_wheel_link">
<material>Gazebo/Gray</material>
<mu1>100000.0</mu1>
<mu2>100000.0</mu2>
</gazebo>
<!-- Transmission is important to link the joints and the controller -->
<transmission name="${prefix}_wheel_joint_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}_wheel_joint" >
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="${prefix}_wheel_joint_motor">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<!-- Macro for robot caster -->
<xacro:macro name="caster" params="prefix reflect">
<joint name="${prefix}_caster_joint" type="fixed">
<origin xyz="${reflect*caster_joint_x} 0 ${-(base_length/2 + caster_radius)}" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="${prefix}_caster_link"/>
</joint>
<link name="${prefix}_caster_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${caster_radius}" />
</geometry>
<material name="black" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${caster_radius}" />
</geometry>
</collision>
<xacro:sphere_inertial_matrix m="${caster_mass}" r="${caster_radius}" />
</link>
<gazebo reference="${prefix}_caster_link">
<material>Gazebo/Black</material>
</gazebo>
</xacro:macro>
<xacro:macro name="mbot_base_gazebo">
<link name="base_footprint">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
</link>
<gazebo reference="base_footprint">
<turnGravityOff>false</turnGravityOff>
</gazebo>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 ${base_length/2 + caster_radius*2}" rpy="0 0 0" />
<parent link="base_footprint"/>
<child link="base_link" />
</joint>
<link name="base_link">
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${base_length}" radius="${base_radius}"/>
</geometry>
<material name="yellow" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${base_length}" radius="${base_radius}"/>
</geometry>
</collision>
<xacro:cylinder_inertial_matrix m="${base_mass}" r="${base_radius}" h="${base_length}" />
</link>
<link name="base_link_upper">
<visual>
<origin xyz=" 0 0 ${base_length}" rpy="0 0 0" />
<geometry>
<cylinder length="0.05" radius="${base_radius}" />
</geometry>
<material name="red" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.05" radius="${base_radius}"/>
</geometry>
</collision>
<xacro:cylinder_inertial_matrix m="${base_mass}" r="${base_radius}" h="${base_length}" />
</link>
<!-- Attach base_link_upper to base_link -->
<joint name="base_link_to_upper" type="fixed">
<origin xyz="0 0 ${base_length}" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="base_link_upper"/>
</joint>
<!-- Macro for pillar -->
<xacro:macro name="pillar" params="x y">
<link name="pillar_${x}_${y}">
<visual>
<origin xyz="${x} ${y} ${base_length/2}" rpy="0 0 0"/>
<geometry>
<cylinder radius="${pillar_radius/2}" length="${pillar_length}" />
</geometry>
<material name="black" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${pillar_radius/2}" radius="${pillar_length}"/>
</geometry>
</collision>
<!-- <xacro:cylinder_inertial_matrix m="${base_length}" r="${pillar_radius/2}}" h="${pillar_length}" /> -->
</link>
<joint name="pillar_${x}_${y}_joint" type="fixed">
<origin xyz="${x} ${y} ${base_length/1.5}" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="pillar_${x}_${y}"/>
</joint>
</xacro:macro>
<!-- Pillars connecting upper and lower base -->
<xacro:pillar x="0.05" y="0.05"/>
<xacro:pillar x="-0.05" y="0.05"/>
<xacro:pillar x="-0.05" y="-0.05"/>
<xacro:pillar x="0.05" y="-0.05"/>
<gazebo reference="base_link">
<material>Gazebo/Blue</material>
</gazebo>
<xacro:wheel prefix="left" reflect="1"/>
<xacro:wheel prefix="right" reflect="-1"/>
<xacro:caster prefix="front" reflect="-1"/>
<xacro:caster prefix="back" reflect="1"/>
<!-- controller -->
<gazebo>
<plugin name="differential_drive_controller"
filename="libgazebo_ros_diff_drive.so">
<update_rate>30</update_rate>
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>${wheel_joint_y*2}</wheel_separation>
<wheel_diameter>${2*wheel_radius}</wheel_diameter>
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>
<command_topic>cmd_vel</command_topic>
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
<odometry_topic>odom</odometry_topic>
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_footprint</robot_base_frame>
<odometry_source>1</odometry_source>
</plugin>
</gazebo>
</xacro:macro>
</robot>
在mbot_gazebo.xacro中复制下面的代码
这样引用可以使代码更整洁,且后期添加部件也很方便
<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find mbot_gazebo)/urdf/mbot_base_gazebo.xacro" />
<xacro:mbot_base_gazebo/>
</robot>
3 在gazebo中显示模型,并使用rviz查看三种传感器仿真效果
(1)创建launch文件
cd ~/ros2_ws/src/mbot_gazebo
touch launch/mbot.launch.py launch/load_urdf_into_gazebo.launch.py
(2)mbot.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
import xacro
def generate_launch_description():
# Check if we're told to use sim time
use_sim_time = LaunchConfiguration('use_sim_time')
# Process the URDF file
pkg_path = os.path.join(get_package_share_directory('mbot_gazebo'))
xacro_file = os.path.join(pkg_path,'urdf','mbot_gazebo.xacro')
robot_description_config = xacro.process_file(xacro_file)
# Create a robot_state_publisher node
params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
node_robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params]
)
# Launch!
return LaunchDescription([
DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use sim time if true'),
node_robot_state_publisher
])
(3) load_urdf_into_gazebo.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
def generate_launch_description():
# Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
# !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!!
package_name='mbot_gazebo' #<--- CHANGE ME
world_file_path = 'worlds/my_house.sdf'
pkg_path = os.path.join(get_package_share_directory(package_name))
world_path = os.path.join(pkg_path, world_file_path)
# Pose where we want to spawn the robot
spawn_x_val = '0.0'
spawn_y_val = '0.0'
spawn_z_val = '0.0'
spawn_yaw_val = '0.0'
mbot = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory(package_name),'launch','mbot.launch.py'
)]), launch_arguments={'use_sim_time': 'true', 'world':world_path}.items()
)
# Include the Gazebo launch file, provided by the gazebo_ros package
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
)
# Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
arguments=['-topic', 'robot_description',
'-entity', 'mbot',
'-x', spawn_x_val,
'-y', spawn_y_val,
'-z', spawn_z_val,
'-Y', spawn_yaw_val],
output='screen')
# Launch them all!
return LaunchDescription([
mbot,
gazebo,
spawn_entity,
])
(4) 修改setup.py文件
from setuptools import find_packages, setup
import os
from glob import glob
package_name = 'mbot_gazebo'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
(os.path.join('share', package_name, 'urdf'), glob(os.path.join('urdf', '*.*'))),
(os.path.join('share', package_name, 'meshes'), glob(os.path.join('meshes', '*.*'))),
(os.path.join('share', package_name, 'urdf/sensors'), glob(os.path.join('urdf/sensors', '*.*'))),
(os.path.join('share', package_name, 'worlds'), glob(os.path.join('worlds', '*.*'))),
(os.path.join('share', package_name, 'rviz'), glob(os.path.join('rviz', '*.rviz'))),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='lll',
maintainer_email='lll@todo.todo',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
],
},
)
(5) 编译并运行launch程序
cd ~/ros2_ws/
colcon build --packages-select mbot_gazebo
source install/setup.bash
ros2 launch mbot_gazebo load_urdf_into_gazebo.launch.py
可以看到我们已经将模型加载了进来,但此时还没有相机和雷达,接下面我们将相机和雷达也添加进来
(6)添加相机和雷达
在camera_gazebo.xacro中添加代码:
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="camera">
<xacro:macro name="usb_camera" params="prefix:=camera">
<!-- Create laser reference frame -->
<link name="${prefix}_link">
<inertial>
<mass value="0.1" />
<origin xyz="0 0 0" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0"
iyy="0.01" iyz="0.0"
izz="0.01" />
</inertial>
<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<box size="0.01 0.04 0.04" />
</geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<box size="0.01 0.04 0.04" />
</geometry>
</collision>
</link>
<gazebo reference="${prefix}_link">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="${prefix}_link">
<sensor type="camera" name="camera_node">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="gazebo_camera" filename="libgazebo_ros_camera.so">
<ros>
<!-- <namespace>stereo</namespace> -->
<remapping>~/image_raw:=image_raw</remapping>
<remapping>~/camera_info:=camera_info</remapping>
</ros>
<camera_name>${prefix}</camera_name>
<frame_name>${prefix}_link</frame_name>
<hack_baseline>0.2</hack_baseline>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>
在kinect_gazebo.xacro中添加下面的代码:
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="kinect_camera">
<xacro:macro name="kinect_camera" params="prefix:=camera">
<!-- Create kinect reference frame -->
<!-- Add mesh for kinect -->
<link name="${prefix}_link">
<origin xyz="0 0 0" rpy="0 0 0"/>
<visual>
<origin xyz="0 0 0" rpy="0 0 ${M_PI/2}"/>
<geometry>
<box size="0.15 0.04 0.04" />
</geometry>
</visual>
<collision>
<geometry>
<box size="0.07 0.3 0.09"/>
</geometry>
</collision>
</link>
<joint name="${prefix}_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
<parent link="${prefix}_link"/>
<child link="${prefix}_frame_optical"/>
</joint>
<link name="${prefix}_frame_optical"/>
<gazebo reference="${prefix}_link">
<sensor type="depth" name="${prefix}">
<always_on>true</always_on>
<update_rate>15.0</update_rate>
<pose>0 0 0 0 0 0</pose>
<camera name="kinect">
<horizontal_fov>${60.0*M_PI/180.0}</horizontal_fov>
<image>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>8.0</far>
</clip>
</camera>
<plugin name="${prefix}_controller" filename="libgazebo_ros_camera.so">
<ros>
<!-- <namespace>${prefix}</namespace> -->
<remapping>${prefix}/image_raw:=rgb/image_raw</remapping>
<remapping>${prefix}/image_depth:=depth/image_raw</remapping>
<remapping>${prefix}/camera_info:=rgb/camera_info</remapping>
<remapping>${prefix}/camera_info_depth:=depth/camera_info</remapping>
<remapping>${prefix}/points:=depth/points</remapping>
</ros>
<camera_name>${prefix}</camera_name>
<frame_name>${prefix}_frame_optical</frame_name>
<hack_baseline>0.07</hack_baseline>
<min_depth>0.001</min_depth>
<max_depth>300.0</max_depth>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>
在lidar_gazebo.xacro中添加下面的代码
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="laser">
<xacro:macro name="rplidar" params="prefix:=laser">
<!-- Create laser reference frame -->
<link name="${prefix}_link">
<inertial>
<mass value="0.1" />
<origin xyz="0 0 0" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0"
iyy="0.01" iyz="0.0"
izz="0.01" />
</inertial>
<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<cylinder length="0.05" radius="0.05"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<cylinder length="0.06" radius="0.05"/>
</geometry>
</collision>
</link>
<gazebo reference="${prefix}_link">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="${prefix}_link">
<sensor type="ray" name="rplidar">
<update_rate>20</update_rate>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1</resolution>
<min_angle>-3</min_angle>
<max_angle>3</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_rplidar" filename="libgazebo_ros_ray_sensor.so">
<ros>
<namespace>/</namespace>
<remapping>~/out:=scan</remapping>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>
在rgbd_gazebo.xacro中添加下面的代码
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rgbd_camera">
<xacro:macro name="rgbd_camera" params="prefix:=camera">
<!-- Create rgbd reference frame -->
<!-- Add mesh for rgbd -->
<link name="${prefix}_link">
<origin xyz="0 0 0" rpy="0 0 0"/>
<visual>
<origin xyz="0 0 0" rpy="0 0 ${M_PI/2}"/>
<geometry>
<box size="0.15 0.04 0.04" />
</geometry>
</visual>
<collision>
<geometry>
<box size="0.07 0.3 0.09"/>
</geometry>
</collision>
</link>
<joint name="${prefix}_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
<parent link="${prefix}_link"/>
<child link="${prefix}_frame_optical"/>
</joint>
<link name="${prefix}_frame_optical"/>
<gazebo reference="${prefix}_link">
<sensor type="depth" name="${prefix}">
<always_on>true</always_on>
<update_rate>15.0</update_rate>
<pose>0 0 0 0 0 0</pose>
<camera name="rgbd">
<horizontal_fov>${60.0*M_PI/180.0}</horizontal_fov>
<image>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>8.0</far>
</clip>
</camera>
<plugin name="${prefix}_controller" filename="libgazebo_ros_camera.so">
<ros>
<!-- <namespace>${prefix}</namespace> -->
<remapping>${prefix}/image_raw:=rgb/image_raw</remapping>
<remapping>${prefix}/image_depth:=depth/image_raw</remapping>
<remapping>${prefix}/camera_info:=rgb/camera_info</remapping>
<remapping>${prefix}/camera_info_depth:=depth/camera_info</remapping>
<remapping>${prefix}/points:=depth/points</remapping>
</ros>
<camera_name>${prefix}</camera_name>
<frame_name>${prefix}_frame_optical</frame_name>
<hack_baseline>0.07</hack_baseline>
<min_depth>0.001</min_depth>
<max_depth>300.0</max_depth>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>
新建mbot_with_kinect_laser_gazebo.xacro文件并在其中添加下面的代码
<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find mbot_gazebo)/urdf/my_mbot_base_gazebo.xacro" />
<xacro:include filename="$(find mbot_gazebo)/urdf/sensors/kinect_gazebo.xacro" />
<xacro:include filename="$(find mbot_gazebo)/urdf/sensors/lidar_gazebo.xacro" />
<xacro:include filename="$(find mbot_gazebo)/urdf/sensors/camera_gazebo.xacro" />
<xacro:property name="camera_offset_x" value="0.17" />
<xacro:property name="camera_offset_y" value="0" />
<xacro:property name="camera_offset_z" value="0.38" />
<xacro:property name="lidar_offset_x" value="0" />
<xacro:property name="lidar_offset_y" value="0" />
<xacro:property name="lidar_offset_z" value="0.105" />
<xacro:property name="kinect_offset_x" value="0.15" />
<xacro:property name="kinect_offset_y" value="0" />
<xacro:property name="kinect_offset_z" value="0.11" />
<!-- kinect -->
<joint name="kinect_joint" type="fixed">
<origin xyz="${kinect_offset_x} ${kinect_offset_y} ${kinect_offset_z}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="kinect_link"/>
</joint>
<xacro:kinect_camera prefix="kinect"/>
<joint name="lidar_joint" type="fixed">
<origin xyz="${lidar_offset_x} ${lidar_offset_y} ${lidar_offset_z}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="laser_link"/>
</joint>
<!-- Camera -->
<joint name="camera_joint" type="fixed">
<origin xyz="${camera_offset_x} ${camera_offset_y} ${camera_offset_z}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="camera_link"/>
</joint>
<xacro:usb_camera prefix="camera"/>
<xacro:rplidar prefix="laser"/>
<xacro:mbot_base_gazebo/>
</robot>
在mbot.launch.py中的第20行改为
xacro_file = os.path.join(pkg_path,'urdf','mbot_with_kinect_laser_gazebo.xacro')
然后编译并运行
cd ~/ros2_ws/
colcon build --packages-select mbot_gazebo
source install/setup.bash
ros2 launch mbot_gazebo load_urdf_into_gazebo.launch.py
运行结果如图所示
然后我们运行rviz2
ros2 run rviz2 rviz2
启动成功后,在左侧Displays窗口中点击“Add”,找到Image显示项,OK确认后就可以加入显示列表啦,然后配置好该显示项订阅的图像话题,就可以顺利看到机器人的摄像头图像啦。
同样的流程,点击Add,添加PointCloud2,设置订阅的点云话题,还要配置Rviz的参考系是odom,就可以看到点云数据啦,每一个点都是由xyz位置和rgb颜色组成。
点击Add,选择Laserscan,然后配置订阅的话题名,rviz的固定坐标系依然是odom,此时就可以看到激光点啦。
topic列表:
4 编写机器人运动控制程序,遥控机器人运动
下载键盘控制节点teleop_twist_keyboard
cd ~/ros2_ws/
colcon build --packages-select teleop_twist_keyboard
source install/setup.bash
ros2 run teleop_twist_keyboard teleop_twist_keyboard
运行结果如下图,已经可以通过键盘控制自己的小车移动了。
5 总结
至此我们有了一个完整的差速轮式机器人,可以使用键盘控制它,在rviz实时查看它,但是它自己没有感知规划能力,后续我们将添加语音控制的节点和视觉目标检测的功能。