Added first version of Sensorsim

2023-11-29 23:09:05 +01:00
parent 784dca286b
commit 97443604b7
15 changed files with 1028 additions and 0 deletions
--- a/launch/launch_sim.py
+++ b/launch/launch_sim.py
@ -0,0 +1,103 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import Node
+from launch.substitutions import Command
+
+
+def generate_launch_description():
+
+    # Check if we're told to use sim time
+    lidar_vertical_fov_upper = LaunchConfiguration('lidar_vertical_fov_upper')
+    lidar_vertical_fov_lower = LaunchConfiguration('lidar_vertical_fov_lower')
+    lidar_horizontal_fov_upper = LaunchConfiguration('lidar_horizontal_fov_upper')
+    lidar_horizontal_fov_lower = LaunchConfiguration('lidar_horizontal_fov_lower')
+
+
+    # Process the URDF file
+    package_name='sensor_sim' #<--- CHANGE ME
+    pkg_path = os.path.join(get_package_share_directory(package_name))
+    xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
+    robot_description_config = Command(['xacro ', xacro_file, ' lidar_vertical_fov_upper:=', lidar_vertical_fov_upper, ' lidar_vertical_fov_lower:=', lidar_vertical_fov_lower, ' lidar_horizontal_fov_upper:=', lidar_horizontal_fov_upper, ' lidar_horizontal_fov_lower:=', lidar_horizontal_fov_lower])
+    world_path = os.path.join(pkg_path,'worlds')
+
+    # Create a robot_state_publisher node
+    params = {'robot_description': robot_description_config, 'use_sim_time': True, 'publish_frequency': 0.0001, 'use_tf_static': False}
+    node_robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        parameters=[params]
+    )
+
+    node_joint_state_publisher = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        parameters=[{'use_sim_time': True}]
+    )
+
+    gazebo = IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([os.path.join(
+                    get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')]),
+                    launch_arguments=[('gz_args', [f" -r -v 0 {world_path}/test_world.sdf"])],
+             )
+
+    # 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='ros_gz_sim', executable='create',
+        arguments=[
+            '-topic', 'robot_description',
+            '-name', 'spawn_robot',
+            '-z', '5',
+            '-x', '0',
+            ],
+        output='screen'
+    )
+
+    # Bridge
+    bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=[
+            '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
+            '/lidar/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked',
+        ],
+        output='screen'
+    )
+
+    lidar_visualizer = Node(
+        package='sensor_sim',
+        executable='lidar_visualizer.py',
+        name='lidar_visualizer',
+        output='screen'
+    )
+
+    # Launch!
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'lidar_vertical_fov_upper',
+            default_value='45',
+            description='Upper limit of FOV of lidar'),
+        DeclareLaunchArgument(
+            'lidar_vertical_fov_lower',
+            default_value='-45',
+            description='Lower limit of FOV of lidar'),
+        DeclareLaunchArgument(
+            'lidar_horizontal_fov_upper',
+            default_value='45',
+            description='Upper limit of FOV of lidar'),
+        DeclareLaunchArgument(
+            'lidar_horizontal_fov_lower',
+            default_value='-45',
+            description='Lower limit of FOV of lidar'),
+        node_robot_state_publisher,
+        node_joint_state_publisher,
+        gazebo,
+        spawn_entity,
+        bridge,
+        lidar_visualizer
+    ])