ros2 加载多机器人
一.效果：
如图，启动了十个方块机器人。
二.环境配置
2.1文件布局
description:机器人部分
gazebo：仿真环境部分
2.2 启动的文件
ros2 launch box_bot_gazebo multi_box_bot_launch.py
1.描述文件所在位置：
分别给出box_bot_gazebo 和 box_bot_description两个文件夹的位置
2.world部分：
world启动文件的位置
3.robot部分：
robot启动文件的位置
4.返回，开始启动两部分文件
三、world和robot代码解读
3.1 world.py
start_world_launch:
#!/usr/bin/python3
# -*- coding: utf-8 -*-
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
pkg_box_bot_gazebo = get_package_share_directory('box_bot_gazebo')
# Gazebo launch
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_gazebo_ros, 'launch', 'gazebo.launch.py'),
)
)
return LaunchDescription([
DeclareLaunchArgument(
'world',
default_value=[os.path.join(pkg_box_bot_gazebo, 'worlds', 'box_bot_empty.world'), ''],
description='SDF world file'),
gazebo
])
1.启动gazebo文件：
2.加载世界：
box_bot_empty.world:
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="default">
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://ground_plane</uri>
</include>
</world>
</sdf>
3.2 robot.py
multi_spawn_robot_launch.py
#!/usr/bin/python3
# -*- coding: utf-8 -*-
import sys
import os
from ament_index_python.packages import get_package_share_directory, get_package_prefix
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, TextSubstitution
def gen_robot_list(number_of_robots):
robots = []
for i in range(number_of_robots):
robot_name = "box_bot"+str(i)
x_pos = float(i)
robots.append({'name': robot_name, 'x_pose': x_pos, 'y_pose': 0.0, 'z_pose': 0.01})
return robots
def generate_launch_description():
urdf = os.path.join(get_package_share_directory('box_bot_description'), 'robot/', 'box_bot_v2.urdf')
pkg_box_bot_description = get_package_share_directory('box_bot_description')
assert os.path.exists(urdf), "Thebox_bot.urdf doesnt exist in "+str(urdf)
# Names and poses of the robots
robots = gen_robot_list(10)
# We create the list of spawn robots commands
spawn_robots_cmds = []
for robot in robots:
spawn_robots_cmds.append(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_box_bot_description, 'launch',
'spawn_box_bot_launch.py')),
launch_arguments={
'robot_urdf': urdf,
'x': TextSubstitution(text=str(robot['x_pose'])),
'y': TextSubstitution(text=str(robot['y_pose'])),
'z': TextSubstitution(text=str(robot['z_pose'])),
'robot_name': robot['name'],
'robot_namespace': robot['name']
}.items()))
# Create the launch description and populate
ld = LaunchDescription()
for spawn_robot_cmd in spawn_robots_cmds:
ld.add_action(spawn_robot_cmd)
return ld
3-1.gen_robot_list:
生成一个机器人列表的子函数，robots是一个列表，其中每一个元素代表一个机器人，机器人属性被定义在一个字典里。
3-2.generate_launch_description
（1）.加载urdf文件
（2）.依次创建机器人
为每个机器人创造一个节点，具体的机器人创造在spawn_box_bot_launch.py文件中
（3）.创造 launch description 用于启动
3-2-1 spawn_box_bot_launch.py
该文件创造一个节点！而spawn_box_bot_v2.py代表一个节点的内容
from launch import LaunchDescription
import launch.actions
import launch_ros.actions
def generate_launch_description():
return LaunchDescription([
launch_ros.actions.Node(
package='box_bot_description',
executable='spawn_box_bot_v2.py',
output='screen',
arguments=[
'--robot_urdf', launch.substitutions.LaunchConfiguration('robot_urdf'),
'--robot_name', launch.substitutions.LaunchConfiguration('robot_name'),
'--robot_namespace', launch.substitutions.LaunchConfiguration('robot_namespace'),
'-x', launch.substitutions.LaunchConfiguration('x'),
'-y', launch.substitutions.LaunchConfiguration('y'),
'-z', launch.substitutions.LaunchConfiguration('z')]),
])
package：执行文件的文件夹位置
executable:执行文件（package中）
output:输出方式
arguments:向executable传入命令行参数，传入的参数可以从节点启动时main函数中的argc参数获取到参数个数，argv参数获取到参数内容。
launch.substitutions.LaunchConfiguration：允许在启动时，获得内部的值
3-2-1-1 spawn_box_bot_v2.py
#!/usr/bin/python3
# -*- coding: utf-8 -*-
"""Script used to spawn a robot in a generic position."""
import argparse
import os
import xml.etree.ElementTree as ET
from ament_index_python.packages import get_package_share_directory
from gazebo_msgs.srv import SpawnEntity
import rclpy
def main():
# Get input arguments from user
parser = argparse.ArgumentParser(description='Spawn Robot into Gazebo with navigation2')
parser.add_argument('-urdf', '--robot_urdf', type=str, default='dummy.urdf',
help='Name of the robot to spawn')
parser.add_argument('-n', '--robot_name', type=str, default='dummy_robot',
help='Name of the robot to spawn')
parser.add_argument('-ns', '--robot_namespace', type=str, default='dummy_robot_ns',
help='ROS namespace to apply to the tf and plugins')
parser.add_argument('-namespace', '--namespace', type=bool, default=True,
help='Whether to enable namespacing')
parser.add_argument('-x', type=float, default=0,
help='the x component of the initial position [meters]')
parser.add_argument('-y', type=float, default=0,
help='the y component of the initial position [meters]')
parser.add_argument('-z', type=float, default=0,
help='the z component of the initial position [meters]')
args, unknown = parser.parse_known_args()
# Start node
rclpy.init()
node = rclpy.create_node('entity_spawner')
node.get_logger().info(
'Creating Service client to connect to `/spawn_entity`')
client = node.create_client(SpawnEntity, '/spawn_entity')
node.get_logger().info('Connecting to `/spawn_entity` service...')
if not client.service_is_ready():
client.wait_for_service()
node.get_logger().info('...connected!')
# sdf_file_path = os.path.join(
# get_package_share_directory('amazon_robot_gazebo'), 'models',
# 'amazon_robot2', 'model.sdf')
urdf_file_path = args.robot_urdf
print(urdf_file_path)
# We need to remap the transform (/tf) topic so each robot has its own.
# We do this by adding `ROS argument entries` to the urdf file for
# each plugin broadcasting a transform. These argument entries provide the
# remapping rule, i.e. /tf -> //tf
tree = ET.parse(urdf_file_path)
root = tree.getroot()
imu_plugin = None
diff_drive_plugin = None
for plugin in root.iter('plugin'):
if 'differential_drive_controller' in plugin.attrib.values():
diff_drive_plugin = plugin
elif 'box_bot_imu_plugin' in plugin.attrib.values():
imu_plugin = plugin
# We change the namespace to the robots corresponding one
tag_diff_drive_ros_params = diff_drive_plugin.find('ros')
tag_diff_drive_ns = ET.SubElement(tag_diff_drive_ros_params, 'namespace')
tag_diff_drive_ns.text = '/' + args.robot_namespace
ros_tf_remap = ET.SubElement(tag_diff_drive_ros_params, 'remapping')
ros_tf_remap.text = '/tf:=/' + args.robot_namespace + '/tf'
# if imu_plugin is not None:
# tag_imu_ros_params = imu_plugin.find('ros')
# tag_imu_ns = ET.SubElement(tag_imu_ros_params, 'namespace')
# tag_imu_ns.text = '/' + args.robot_namespace + '/imu'
# else:
# print("ERROR>>>>>>>>>>>>>>>>>>>>> IMU NOT FOUND")
# Set data for request
request = SpawnEntity.Request()
request.name = args.robot_name
request.xml = ET.tostring(root, encoding='unicode')
request.initial_pose.position.x = float(args.x)
request.initial_pose.position.y = float(args.y)
request.initial_pose.position.z = float(args.z)
if args.namespace is True:
node.get_logger().info('spawning `{}` on namespace `{}` at {}, {}, {}'.format(
args.robot_name, args.robot_namespace, args.x, args.y, args.z))
request.robot_namespace = args.robot_namespace
print(args.robot_namespace)
else:
node.get_logger().info('spawning `{}` at {}, {}, {}'.format(
args.robot_name, args.x, args.y, args.z))
node.get_logger().info('Spawning Robot using service: `/spawn_entity`')
future = client.call_async(request)
rclpy.spin_until_future_complete(node, future)
if future.result() is not None:
print('response: %r' % future.result())
else:
raise RuntimeError(
'exception while calling service: %r' % future.exception())
node.get_logger().info('Done! Shutting down node.')
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
改变了命名空间。
这一套流程就是给节点进行重映射，因为要涉及到相同的功能，给每个机器人的功能节点都映射一个自己的名字，首先定位到urdf_file_path
3-2-1-2配置cmake文件
cmake_minimum_required(VERSION 3.5)
project(box_bot_description)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake REQUIRED)
find_package(urdf REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
install(
DIRECTORY
launch
robot
DESTINATION
share/${PROJECT_NAME}/
)
install(
PROGRAMS
launch/spawn_box_bot.py
launch/spawn_box_bot_v2.py
DESTINATION lib/${PROJECT_NAME}
)
ament_package()
multi_spawn_robot_launch.py 启动多机器人的节点的文件
install(
PROGRAMS
launch/spawn_box_bot.py
launch/spawn_box_bot_v2.py
DESTINATION lib/${PROJECT_NAME}
)
用于放启动launch文件所包含的py文件！而不是launch.py！
spawn_box_bot_launch.py 创造一个机器人的节点的文件
spawn_box_bot_v2.py 完善一个机器人信息的文件
后面两个文件，关于创造一个机器人的节点，必须将其写入cmake中的install programs！否则找不到