Ajout des zed launch files

rove.repos

@@ -11,3 +11,12 @@ repositories:
     type: git
     url: https://github.com/dawonn/vectornav.git
     version: ros2
+  zed_wrapper:
+    type: git
+    url: https://github.com/stereolabs/zed-ros2-wrapper.git
+    version: master
+  zed_wrapper/zed-ros2-interfaces:
+    type: git
+    url: https://github.com/stereolabs/zed-ros2-interfaces.git
+    version: main
+

src/rove_bringup/config/VLP16_velodyne_params.yaml

@@ -0,0 +1,30 @@
+# from https://github.com/ros-drivers/velodyne/tree/ros2
+velodyne_driver_node:
+    ros__parameters:
+        device_ip: 192.168.84.201 # be sure that your ip adress is 192.168.84.150/24 or look destination in the velodyne settings
+        gps_time: false
+        time_offset: 0.0
+        enabled: true
+        read_once: false
+        read_fast: false
+        repeat_delay: 0.0
+        frame_id: velodyne
+        model: VLP16
+        rpm: 600.0
+        port: 2368
+        timestamp_first_packet: false
+
+velodyne_laserscan_node:
+    ros__parameters:
+        ring: -1
+        resolution: 0.007
+
+velodyne_transform_node:
+    ros__parameters:
+        model: VLP16
+        min_range: 0.9
+        max_range: 130.0
+        view_direction: 0.0
+        fixed_frame: ""
+        target_frame: ""
+        organize_cloud: true

src/rove_bringup/config/VLP16db.yaml

@@ -0,0 +1,53 @@
+# callibration of the Velodyne lidar
+# from https://github.com/ros-drivers/velodyne/tree/ros2
+lasers:
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, rot_correction: 0.0,
+  vert_correction: -0.2617993877991494, vert_offset_correction: 0.0112}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, rot_correction: 0.0,
+  vert_correction: 0.017453292519943295, vert_offset_correction: -0.0007}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, rot_correction: 0.0,
+  vert_correction: -0.22689280275926285, vert_offset_correction: 0.0097}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, rot_correction: 0.0,
+  vert_correction: 0.05235987755982989, vert_offset_correction: -0.0022}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, rot_correction: 0.0,
+  vert_correction: -0.19198621771937624, vert_offset_correction: 0.0081}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, rot_correction: 0.0,
+  vert_correction: 0.08726646259971647, vert_offset_correction: -0.0037}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, rot_correction: 0.0,
+  vert_correction: -0.15707963267948966, vert_offset_correction: 0.0066}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, rot_correction: 0.0,
+  vert_correction: 0.12217304763960307, vert_offset_correction: -0.0051}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, rot_correction: 0.0,
+  vert_correction: -0.12217304763960307, vert_offset_correction: 0.0051}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, rot_correction: 0.0,
+  vert_correction: 0.15707963267948966, vert_offset_correction: -0.0066}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, rot_correction: 0.0,
+  vert_correction: -0.08726646259971647, vert_offset_correction: 0.0037}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, rot_correction: 0.0,
+  vert_correction: 0.19198621771937624, vert_offset_correction: -0.0081}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, rot_correction: 0.0,
+  vert_correction: -0.05235987755982989, vert_offset_correction: 0.0022}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, rot_correction: 0.0,
+  vert_correction: 0.22689280275926285, vert_offset_correction: -0.0097}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, rot_correction: 0.0,
+  vert_correction: -0.017453292519943295, vert_offset_correction: 0.0007}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, rot_correction: 0.0,
+  vert_correction: 0.2617993877991494, vert_offset_correction: -0.0112}
+num_lasers: 16
+distance_resolution: 0.002

src/rove_bringup/launch/common.launch.py

@@ -8,9 +8,12 @@
 from launch.substitutions import Command
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
+from launch.substitutions import LaunchConfiguration
 
 
 def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time')
+
     # Get the launch directory
     pkg_rove_bringup = get_package_share_directory('rove_bringup')
     pkg_rove_description = get_package_share_directory('rove_description')
@@ -30,7 +33,7 @@ def generate_launch_description():
         output='both',
         parameters=[
             {'robot_description': robot_desc},
-            {"use_sim_time": True, }
+            {"use_sim_time": use_sim_time, }
         ]
     )
 
@@ -50,7 +53,7 @@ def generate_launch_description():
         name='ekf_filter_node_local',
         output='screen',
         parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
-                    {'use_sim_time': True}],
+                    {'use_sim_time': use_sim_time}],
         remappings=[('odometry/filtered', 'odometry/local')])
 
 
@@ -60,7 +63,7 @@ def generate_launch_description():
         name='ekf_filter_node_global',
         output='screen',
         parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
-                    {'use_sim_time': True}],
+                    {'use_sim_time': use_sim_time}],
         remappings=[('odometry/filtered', 'odometry/global')])
 
 
@@ -70,7 +73,7 @@ def generate_launch_description():
         name="navsat_transform",
         output="screen",
         parameters=[os.path.join(pkg_rove_description, 'config/navsat_transform.yaml'),
-                   {'use_sim_time': True}],
+                   {'use_sim_time': use_sim_time}],
         remappings=[
             # Subscribed Topics
                     ("imu/data", "imu"),
@@ -90,7 +93,7 @@ def generate_launch_description():
             os.path.join(pkg_rove_bringup, "launch", "autonomy.launch.py"),
         ),
         launch_arguments={
-            "use_sim_time": "true",
+            'use_sim_time': use_sim_time,
             "deskewing": "true",
             "use_slam3d": "false",
         }.items(),

src/rove_bringup/launch/real.launch.py

@@ -18,6 +18,9 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             os.path.join(pkg_rove_bringup, "launch", "common.launch.py"),
         ),
+        launch_arguments={
+            "use_sim_time": "false",
+        }.items(),
     )
 
 
@@ -28,7 +31,14 @@ def generate_launch_description():
         ),
     )
 
+    velodyne = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_bringup, "launch", "velodyne.launch.py"),
+        ),
+    )
+
     return LaunchDescription([
             common,
             vectornav,
+            velodyne,
             ])

src/rove_bringup/launch/sim.launch.py

@@ -19,6 +19,8 @@ def generate_launch_description():
     # Get the URDF file
     urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro')
     robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str)
+
+    # Get simulation file
     world_file_name = 'worlds/base_world.world'
     world = os.path.join(pkg_rove_description, world_file_name)
 
@@ -72,6 +74,9 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             os.path.join(pkg_rove_bringup, "launch", "common.launch.py"),
         ),
+        launch_arguments={
+            "use_sim_time": "true",
+        }.items(),
     )
 
     return LaunchDescription([

src/rove_bringup/launch/test.launch.py

@@ -0,0 +1,108 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    # Get the launch directory
+    pkg_rove_bringup = get_package_share_directory('rove_bringup')
+    pkg_rove_description = get_package_share_directory('rove_description')
+    pkg_rove_slam = get_package_share_directory('rove_slam')
+    bringup_pkg_path = get_package_share_directory('rove_bringup')
+
+    # Get the URDF file
+    urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro')
+    robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str)
+
+    # Takes the description and joint angles as inputs and publishes
+    # the 3D poses of the robot links
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='both',
+        parameters=[
+            {'robot_description': robot_desc},
+            # {"use_sim_time": True, }
+        ]
+    )
+
+    # Visualize in RViz
+    rviz = Node(
+       package='rviz2',
+       executable='rviz2',
+       arguments=[
+        '-f', 'velodyne',
+        # '-d', os.path.join(pkg_rove_description, 'config','basic.rviz')
+        ],
+    )
+
+    # used tutorial: https://navigation.ros.org/tutorials/docs/navigation2_with_gps.html
+
+    robot_localization_node_local = Node(
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_filter_node_local',
+        output='screen',
+        parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
+                    # {'use_sim_time': True}
+                    ],
+        remappings=[('odometry/filtered', 'odometry/local')])
+
+
+    robot_localization_node_global = Node(
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_filter_node_global',
+        output='screen',
+        parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'),
+                    # {'use_sim_time': True}
+                    ],
+        remappings=[('odometry/filtered', 'odometry/global')])
+
+
+    navsat_transform = Node(
+        package="robot_localization",
+        executable="navsat_transform_node",
+        name="navsat_transform",
+        output="screen",
+        parameters=[os.path.join(pkg_rove_description, 'config/navsat_transform.yaml'),
+                    # {'use_sim_time': True}
+                   ],
+        remappings=[
+            # Subscribed Topics
+                    ("imu/data", "imu"),
+                    ("gps/fix", "gps"),
+                    ('odometry/filtered', 'odometry/global'),
+                ],
+    )
+
+    ###### Sensor ######
+    vectornav = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_bringup, "launch", "vectornav.launch.py"),
+        ),
+    )
+
+    velodyne = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_bringup, "launch", "velodyne.launch.py"),
+        ),
+    )
+
+    return LaunchDescription([
+            # robot_state_publisher,
+            robot_localization_node_local,
+            robot_localization_node_global,
+            navsat_transform,
+            vectornav,
+            velodyne,
+            rviz,
+            ])

src/rove_bringup/launch/velodyne.launch.py

@@ -0,0 +1,50 @@
+import os
+import launch
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+import yaml
+
+# from https://github.com/ros-drivers/velodyne/tree/ros2
+def generate_launch_description():
+    package_name = 'rove_bringup'
+    dir = get_package_share_directory(package_name)
+    # Get params files
+    params = os.path.join(dir, 'config', 'VLP16_velodyne_params.yaml')
+    with open(params, 'r') as f:
+        transform_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters']
+    transform_params['calibration'] = os.path.join(dir, 'config', 'VLP16db.yaml')
+
+    driver_node = Node(package='velodyne_driver',
+        executable='velodyne_driver_node',
+        output='both',
+        parameters=[params])
+
+    driver_exit_event = launch.actions.RegisterEventHandler(
+        event_handler=launch.event_handlers.OnProcessExit(
+            target_action=driver_node,
+            on_exit=[launch.actions.EmitEvent(
+                event=launch.events.Shutdown())],
+        ))
+
+    # laserscan
+    laserscan_node = Node(package='velodyne_laserscan',
+        executable='velodyne_laserscan_node',
+        name='scan',
+        output='both',
+        parameters=[params])
+
+    # pointcloud
+    transform_node = Node(package='velodyne_pointcloud',
+        executable='velodyne_transform_node',
+        name='velodyne_points',
+        output='both',
+        parameters=[transform_params])
+
+    return LaunchDescription([
+        driver_node,
+        driver_exit_event,
+        laserscan_node,
+        transform_node,
+        ])