diff --git a/tuw_gazebo_models/launch/spawn_robot_v1.launch.py b/tuw_gazebo_models/launch/spawn_robot_v1.launch.py new file mode 100644 index 0000000..5c08ca4 --- /dev/null +++ b/tuw_gazebo_models/launch/spawn_robot_v1.launch.py @@ -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"), + ]) diff --git a/tuw_gazebo_models/launch/spawn_robot_v2.launch.py b/tuw_gazebo_models/launch/spawn_robot_v2.launch.py new file mode 100644 index 0000000..5c08ca4 --- /dev/null +++ b/tuw_gazebo_models/launch/spawn_robot_v2.launch.py @@ -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"), + ]) diff --git a/tuw_gazebo_models/launch/spawn_robot.launch.py b/tuw_gazebo_models/launch/spawn_robot_v3.launch.py similarity index 94% rename from tuw_gazebo_models/launch/spawn_robot.launch.py rename to tuw_gazebo_models/launch/spawn_robot_v3.launch.py index 3381a12..2bf89de 100644 --- a/tuw_gazebo_models/launch/spawn_robot.launch.py +++ b/tuw_gazebo_models/launch/spawn_robot_v3.launch.py @@ -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) @@ -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')}], diff --git a/tuw_gazebo_models/launch/robot_description.launch.py b/tuw_gazebo_models/launch/spawn_robot_v4.launch.py similarity index 69% rename from tuw_gazebo_models/launch/robot_description.launch.py rename to tuw_gazebo_models/launch/spawn_robot_v4.launch.py index 84be764..2bf89de 100644 --- a/tuw_gazebo_models/launch/robot_description.launch.py +++ b/tuw_gazebo_models/launch/spawn_robot_v4.launch.py @@ -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) @@ -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')}], diff --git a/tuw_gazebo_models/models/hokuyo/main_v1.xacro b/tuw_gazebo_models/models/hokuyo/main_v1.xacro new file mode 100644 index 0000000..0e9afb4 --- /dev/null +++ b/tuw_gazebo_models/models/hokuyo/main_v1.xacro @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 0 0 0 0 0 + 30 + ${visualize} + true + + + + 683 + 1.0 + -2.35619449019 + 2.35619449019 + + + + 0.05 + 10 + + + gaussian + 0.0 + 0.01 + + + + + ${namespace} + ~/out:=${topic_name} + + sensor_msgs/LaserScan + ${namespace}/${frame_name} + + + + + + + + + diff --git a/tuw_gazebo_models/models/hokuyo/main.xacro b/tuw_gazebo_models/models/hokuyo/main_v2.xacro similarity index 100% rename from tuw_gazebo_models/models/hokuyo/main.xacro rename to tuw_gazebo_models/models/hokuyo/main_v2.xacro diff --git a/tuw_gazebo_models/models/pioneer3dx/main.xacro b/tuw_gazebo_models/models/pioneer3dx/main.xacro index 380493e..bbdb83b 100644 --- a/tuw_gazebo_models/models/pioneer3dx/main.xacro +++ b/tuw_gazebo_models/models/pioneer3dx/main.xacro @@ -9,11 +9,13 @@ + + @@ -23,11 +25,11 @@ - + - + diff --git a/tuw_gazebo_models/models/pioneer3dx/plugins_v1.xacro b/tuw_gazebo_models/models/pioneer3dx/plugins_v1.xacro new file mode 100644 index 0000000..663a2f4 --- /dev/null +++ b/tuw_gazebo_models/models/pioneer3dx/plugins_v1.xacro @@ -0,0 +1,49 @@ + + + + + + + + ${namespace} + cmd_vel:=cmd_vel + odom:=odom + + + left_wheel_joint + right_wheel_joint + 0.3 + 0.18 + 10 + 1.5 + + true + true + false + 0 + true + + ${namespace}/odom + ${namespace}/base_link + + + + + + + + + + ${namespace} + joint_states:=joint_states + + 10 + left_wheel_joint + right_wheel_joint + swivel_joint + swivel_wheel_joint + + + + diff --git a/tuw_gazebo_models/models/pioneer3dx/plugins.xacro b/tuw_gazebo_models/models/pioneer3dx/plugins_v2.xacro similarity index 97% rename from tuw_gazebo_models/models/pioneer3dx/plugins.xacro rename to tuw_gazebo_models/models/pioneer3dx/plugins_v2.xacro index cfafce4..e64b0c4 100644 --- a/tuw_gazebo_models/models/pioneer3dx/plugins.xacro +++ b/tuw_gazebo_models/models/pioneer3dx/plugins_v2.xacro @@ -8,6 +8,7 @@ ${namespace} cmd_vel:=cmd_vel odom:=odom + /tf:=tf left_wheel_joint @@ -46,4 +47,4 @@ - \ No newline at end of file +