Skip to content

multi robot simulation in Gazebo #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: ros2-multi-robot
Choose a base branch
from
Open

multi robot simulation in Gazebo #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
85 changes: 85 additions & 0 deletions tuw_gazebo_models/launch/spawn_robot_v1.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
#!/usr/bin/python3
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch_ros.actions import Node
from launch.actions import OpaqueFunction
from launch.actions import SetLaunchConfiguration
import xml.dom.minidom
from launch.conditions import IfCondition

import xacro

def generate_launch_description():

use_sim_time = LaunchConfiguration('use_sim_time', default='true')
namespace_arg = DeclareLaunchArgument('namespace', default_value=TextSubstitution(text=''))
model_name_arg = DeclareLaunchArgument('model_name', default_value=TextSubstitution(text='robot0'))
robot_arg = DeclareLaunchArgument('robot', default_value=TextSubstitution(text='pioneer3dx'))
X_launch_arg = DeclareLaunchArgument('X', default_value=TextSubstitution(text='0.0'))
Y_launch_arg = DeclareLaunchArgument('Y', default_value=TextSubstitution(text='0.0'))
Theta_launch_arg = DeclareLaunchArgument('Theta', default_value=TextSubstitution(text='0.0'))

models_dir = get_package_share_directory('tuw_gazebo_models') + '/models'



def create_robot_description(context):
xacro_file = os.path.join(get_package_share_directory('tuw_gazebo_models'), 'models', context.launch_configurations['robot'], 'main.xacro')
assert os.path.exists(xacro_file), "The main.xacro doesnt exist in "+str(xacro_file)
robot_description_config = xacro.process_file(xacro_file,
mappings={ "namespace": context.launch_configurations['namespace'],
"plugins_version": "v1",
"models_dir": models_dir})
robot_desc = robot_description_config.toxml()
dom = xml.dom.minidom.parseString(robot_desc)
f = open("/tmp/robot_description.xml", "w") ## for debugging only
f.write(dom.toprettyxml())
f.close()
return [SetLaunchConfiguration('robot_desc', robot_desc)]

def create_prefix(context):
namespace = context.launch_configurations['namespace']
prefix = namespace + '/' if namespace != '' else ''
return [SetLaunchConfiguration('prefix', prefix)]

create_robot_description_arg = OpaqueFunction(function=create_robot_description)
create_prefix_action = OpaqueFunction(function=create_prefix)

return LaunchDescription([
namespace_arg,
robot_arg,
X_launch_arg,
Y_launch_arg,
Theta_launch_arg,
create_robot_description_arg,
create_prefix_action,
model_name_arg,
Node(
condition=IfCondition(use_sim_time),
package='tuw_gazebo_models',
name="publisher_robot",
executable='spawn_robot.py',
arguments=[LaunchConfiguration('robot_desc')],
parameters=[{
"X": LaunchConfiguration('X'),
"Y": LaunchConfiguration('Y'),
"Theta": LaunchConfiguration('Theta'),
"model_name": LaunchConfiguration('model_name'),
"namespace": LaunchConfiguration('namespace')}],
output='screen'),
Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
arguments=[LaunchConfiguration('robot_desc')],
namespace=[LaunchConfiguration('namespace')],

parameters=[{
"frame_prefix": LaunchConfiguration('prefix'),
"use_sim_time": use_sim_time,
"robot_description": LaunchConfiguration('robot_desc')}],
output="screen"),
])
85 changes: 85 additions & 0 deletions tuw_gazebo_models/launch/spawn_robot_v2.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
#!/usr/bin/python3
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch_ros.actions import Node
from launch.actions import OpaqueFunction
from launch.actions import SetLaunchConfiguration
import xml.dom.minidom
from launch.conditions import IfCondition

import xacro

def generate_launch_description():

use_sim_time = LaunchConfiguration('use_sim_time', default='true')
namespace_arg = DeclareLaunchArgument('namespace', default_value=TextSubstitution(text=''))
model_name_arg = DeclareLaunchArgument('model_name', default_value=TextSubstitution(text='robot0'))
robot_arg = DeclareLaunchArgument('robot', default_value=TextSubstitution(text='pioneer3dx'))
X_launch_arg = DeclareLaunchArgument('X', default_value=TextSubstitution(text='0.0'))
Y_launch_arg = DeclareLaunchArgument('Y', default_value=TextSubstitution(text='0.0'))
Theta_launch_arg = DeclareLaunchArgument('Theta', default_value=TextSubstitution(text='0.0'))

models_dir = get_package_share_directory('tuw_gazebo_models') + '/models'



def create_robot_description(context):
xacro_file = os.path.join(get_package_share_directory('tuw_gazebo_models'), 'models', context.launch_configurations['robot'], 'main.xacro')
assert os.path.exists(xacro_file), "The main.xacro doesnt exist in "+str(xacro_file)
robot_description_config = xacro.process_file(xacro_file,
mappings={ "namespace": context.launch_configurations['namespace'],
"plugins_version": "v1",
"models_dir": models_dir})
robot_desc = robot_description_config.toxml()
dom = xml.dom.minidom.parseString(robot_desc)
f = open("/tmp/robot_description.xml", "w") ## for debugging only
f.write(dom.toprettyxml())
f.close()
return [SetLaunchConfiguration('robot_desc', robot_desc)]

def create_prefix(context):
namespace = context.launch_configurations['namespace']
prefix = namespace + '/' if namespace != '' else ''
return [SetLaunchConfiguration('prefix', prefix)]

create_robot_description_arg = OpaqueFunction(function=create_robot_description)
create_prefix_action = OpaqueFunction(function=create_prefix)

return LaunchDescription([
namespace_arg,
robot_arg,
X_launch_arg,
Y_launch_arg,
Theta_launch_arg,
create_robot_description_arg,
create_prefix_action,
model_name_arg,
Node(
condition=IfCondition(use_sim_time),
package='tuw_gazebo_models',
name="publisher_robot",
executable='spawn_robot.py',
arguments=[LaunchConfiguration('robot_desc')],
parameters=[{
"X": LaunchConfiguration('X'),
"Y": LaunchConfiguration('Y'),
"Theta": LaunchConfiguration('Theta'),
"model_name": LaunchConfiguration('model_name'),
"namespace": LaunchConfiguration('namespace')}],
output='screen'),
Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
arguments=[LaunchConfiguration('robot_desc')],
namespace=[LaunchConfiguration('namespace')],

parameters=[{
"frame_prefix": LaunchConfiguration('prefix'),
"use_sim_time": use_sim_time,
"robot_description": LaunchConfiguration('robot_desc')}],
output="screen"),
])
9 changes: 4 additions & 5 deletions ...azebo_models/launch/spawn_robot.launch.py → ...bo_models/launch/spawn_robot_v3.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ def create_robot_description(context):
assert os.path.exists(xacro_file), "The main.xacro doesnt exist in "+str(xacro_file)
robot_description_config = xacro.process_file(xacro_file,
mappings={ "namespace": context.launch_configurations['namespace'],
"version": "v2",
"models_dir": models_dir})
robot_desc = robot_description_config.toxml()
dom = xml.dom.minidom.parseString(robot_desc)
Expand Down Expand Up @@ -68,11 +69,9 @@ def create_robot_description(context):
name="robot_state_publisher",
arguments=[LaunchConfiguration('robot_desc')],
namespace=[LaunchConfiguration('namespace')],
# this works but I will not use it at the moment
# remappings=[
# ("/tf", "/r0/tf"),
# ("/tf_static", "/r0/tf_static")
#],

remappings=[('/tf', 'tf'),
('/tf_static', 'tf_static')],
parameters=[{
"use_sim_time": use_sim_time,
"robot_description": LaunchConfiguration('robot_desc')}],
Expand Down
30 changes: 25 additions & 5 deletions ...models/launch/robot_description.launch.py → ...bo_models/launch/spawn_robot_v4.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,14 +18,20 @@ def generate_launch_description():
namespace_arg = DeclareLaunchArgument('namespace', default_value=TextSubstitution(text=''))
model_name_arg = DeclareLaunchArgument('model_name', default_value=TextSubstitution(text='robot0'))
robot_arg = DeclareLaunchArgument('robot', default_value=TextSubstitution(text='pioneer3dx'))
X_launch_arg = DeclareLaunchArgument('X', default_value=TextSubstitution(text='0.0'))
Y_launch_arg = DeclareLaunchArgument('Y', default_value=TextSubstitution(text='0.0'))
Theta_launch_arg = DeclareLaunchArgument('Theta', default_value=TextSubstitution(text='0.0'))

models_dir = get_package_share_directory('tuw_gazebo_models') + '/models'



def create_robot_description(context):
xacro_file = os.path.join(get_package_share_directory('tuw_gazebo_models'), 'models', context.launch_configurations['robot'], 'main.xacro')
assert os.path.exists(xacro_file), "The main.xacro doesnt exist in "+str(xacro_file)
robot_description_config = xacro.process_file(xacro_file,
mappings={ "namespace": context.launch_configurations['namespace'],
"version": "v2",
"models_dir": models_dir})
robot_desc = robot_description_config.toxml()
dom = xml.dom.minidom.parseString(robot_desc)
Expand All @@ -39,19 +45,33 @@ def create_robot_description(context):
return LaunchDescription([
namespace_arg,
robot_arg,
X_launch_arg,
Y_launch_arg,
Theta_launch_arg,
create_robot_description_arg,
model_name_arg,
Node(
condition=IfCondition(use_sim_time),
package='tuw_gazebo_models',
name="publisher_robot",
executable='spawn_robot.py',
arguments=[LaunchConfiguration('robot_desc')],
parameters=[{
"X": LaunchConfiguration('X'),
"Y": LaunchConfiguration('Y'),
"Theta": LaunchConfiguration('Theta'),
"model_name": LaunchConfiguration('model_name'),
"namespace": LaunchConfiguration('namespace')}],
output='screen'),
Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
arguments=[LaunchConfiguration('robot_desc')],
namespace=[LaunchConfiguration('namespace')],
# this works but I will not use it at the moment
# remappings=[
# ("/tf", "/r0/tf"),
# ("/tf_static", "/r0/tf_static")
#],

remappings=[('/tf', 'tf'),
('/tf_static', 'tf_static')],
parameters=[{
"use_sim_time": use_sim_time,
"robot_description": LaunchConfiguration('robot_desc')}],
Expand Down
85 changes: 85 additions & 0 deletions tuw_gazebo_models/models/hokuyo/main_v1.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="hokuyo" >


<!-- hokuyo Laser -->
<xacro:macro name="laser" params="name parent namespace topic_name frame_name xyz rpy driver:=cpu visualize:=true models_dir:=^" >

<xacro:if value="${driver == 'gpu'}">
<xacro:property name="sensor_type" value="gpu_ray"/>
</xacro:if>
<xacro:if value="${driver == 'cpu'}">
<xacro:property name="sensor_type" value="ray"/>
</xacro:if>

<joint name="${parent}_${name}_joint" type="fixed">
<axis xyz="0 0 1" />
<origin xyz="${xyz}" rpy="${rpy}"/>
<parent link="${parent}"/>
<child link="${name}"/>
</joint>

<link name="${name}">
<collision>
<origin xyz="0 0 -0.025" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 -0.025" rpy="0 0 0"/>
<geometry>
<mesh filename="file://${models_dir}/hokuyo/meshes/hokuyo.dae"/>
</geometry>
</visual>
<inertial>
<mass value="1e-2" />
<origin xyz="0 0 -0.025" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>


</link>

<!-- ray sensor -->
<gazebo reference="${name}">
<sensor name="${name}_sensor" type="${sensor_type}">
<pose>0 0 0 0 0 0</pose>
<update_rate>30</update_rate>
<visualize>${visualize}</visualize>
<always_on>true</always_on>
<ray>
<scan>
<horizontal>
<samples>683</samples>
<resolution>1.0</resolution>
<min_angle>-2.35619449019</min_angle>
<max_angle>2.35619449019</max_angle>
</horizontal>
</scan>
<range>
<min>0.05</min>
<max>10</max>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="${name}_laserscan" filename="libgazebo_ros_ray_sensor.so">
<ros>
<namespace>${namespace}</namespace>
<remapping>~/out:=${topic_name}</remapping>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>${namespace}/${frame_name}</frame_name>
</plugin>
</sensor>
</gazebo>


</xacro:macro>


</robot>
0 tuw_gazebo_models/models/hokuyo/main.xacro → ...gazebo_models/models/hokuyo/main_v2.xacro
View file
Open in desktop
File renamed without changes.
6 changes: 4 additions & 2 deletions tuw_gazebo_models/models/pioneer3dx/main.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,13 @@
<xacro:arg name="laser0_topic" default="scan_raw"/>
<xacro:arg name="laser0_visualize" default="true"/>
<xacro:arg name="laser0_driver" default="cpu"/>
<xacro:arg name="version" default="v1"/>

<xacro:property name="models_dir" value="$(arg models_dir)"/>
<xacro:property name="namespace" value="$(arg namespace)"/>
<xacro:property name="robot_model" value="$(arg robot_model)"/>
<xacro:property name="laser_model" value="$(arg laser_model)"/>
<xacro:property name="version" value="$(arg version)"/>

<!-- Import Rviz colors -->
<xacro:include filename="${models_dir}/materials.xacro" />
Expand All @@ -23,11 +25,11 @@
<xacro:vehicle_body name="body" parent="base_link"/>

<!-- differential drive and joint stage plugin -->
<xacro:include filename="${models_dir}/${robot_model}/plugins.xacro"/>
<xacro:include filename="${models_dir}/${robot_model}/plugins_${version}.xacro"/>
<xacro:differential_drive name="$(arg robot_name)" namespace="${namespace}" />
<xacro:joint_state_publisher name="$(arg robot_name)" namespace="${namespace}" />

<!-- Laser -->
<xacro:include filename="${models_dir}/${laser_model}/main.xacro"/>
<xacro:include filename="${models_dir}/${laser_model}/main_${version}.xacro"/>
<xacro:laser name="laser0" parent="top" namespace="${namespace}" topic_name="$(arg laser0_topic)" frame_name="laser0" xyz="0.15 0.0 0.06" rpy="0 0 0" driver="$(arg laser0_driver)" visualize="$(arg laser0_visualize)"/>
</robot>
Loading