WIP: added sherpa

src/sherpa/launch/simulator.launch.py

@@ -0,0 +1,108 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+from launch.substitutions import Command
+
+def generate_launch_description():
+
+    ld = LaunchDescription()  
+
+    pkg_name = 'sherpa'
+    pkg_share = get_package_share_directory(pkg_name)
+    pkg_prefix = os.path.dirname(pkg_share) 
+
+    world_path = os.path.join(pkg_share, 'worlds', 'empty.world')
+    xacro_file = os.path.join(pkg_share, 'urdf', 'sherpa.xacro')
+
+    # create nodes
+
+    gz_env = SetEnvironmentVariable(
+        name='GZ_SIM_RESOURCE_PATH',
+        value=[pkg_prefix]
+    )
+
+    gz = ExecuteProcess(
+        cmd=[
+            'ros2', 'launch', 'ros_gz_sim', 'gz_sim.launch.py',
+            f'gz_args:=-r {world_path}'
+        ],
+        output='screen'
+    )
+
+    state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[{
+            'robot_description': Command(['xacro ', xacro_file])
+        }]
+    )
+
+    joint_pub = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
+        output='screen'
+    )
+
+    static_tf = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='static_tf_base_to_lidar',
+        arguments=[
+            '0', '0', '0',
+            '0', '0', '0',
+            'lidar_link',
+            'camera_init',
+        ]          
+    )
+
+    robot = ExecuteProcess(
+        cmd=[
+            'ros2', 'run', 'ros_gz_sim', 'create',
+            '-name', 'sherpa',
+            '-topic', 'robot_description',
+            '-x', '0', '-y', '0', '-z', '0.5'
+        ],
+        output='screen'
+    )
+
+    gz_bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=[
+            '/velodyne_points/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked',
+            '/model/sherpa/joint_states@gz.msgs.Model_JointState@ros2/sensor_msgs/msg/JointState',
+        ],
+        remappings=[
+            ('/velodyne_points/points', '/velodyne_points'),
+            ('/model/sherpa/joint_states', '/joint_states'),
+        ],
+        output='screen'
+    )
+
+
+    rviz2 = Node(
+        package='rviz2',
+        executable='rviz2',
+        arguments=['-d', f"{os.path.join(get_package_share_directory('sherpa'), 'config', 'sherpa.rviz')}"]
+    )
+
+    ld.add_action(gz_env)
+    ld.add_action(gz)
+    ld.add_action(state_publisher)
+    ld.add_action(joint_pub)
+    ld.add_action(static_tf)
+    ld.add_action(robot)
+    ld.add_action(gz_bridge)
+    ld.add_action(rviz2)
+
+    return ld