Static tf + ouster

etc/demo_static.sh

@@ -5,10 +5,16 @@ source ~/ros2_ws/install/local_setup.bash && source ~/ros2_ws/install/setup.bash
 
 echo "[INFO] Starting gps_duro_reference"
 screen -dmS gps_duro_reference bash -c 'ros2 launch lexus_bringup gps_duro_reference.launch.py'
+echo "[INFO] Starting os_64_center_a"
+screen -dmS os_64_center_a bash -c 'ros2 launch lexus_bringup os_64_center_a.launch.py'
+echo "[INFO] Starting os_32_right_a"
+screen -dmS os_32_right_a bash -c 'ros2 launch lexus_bringup os_32_right_a.launch.py'
+echo "[INFO] Starting tf_static"
+screen -dmS tf_static bash -c 'ros2 launch lexus_bringup tf_static.launch.py'
 echo "[INFO] Starting foxglove_bridge"
 screen -dmS foxglove_bridge bash -c 'ros2 launch foxglove_bridge foxglove_bridge_launch.xml'
-echo "[INFO] Starting foxglove"
-foxglove-studio "foxglove://open?ds=rosbridge-websocket&ds.url=ws://localhost:8765"
+#echo "[INFO] Starting foxglove"
+#foxglove-studio "foxglove://open?ds=rosbridge-websocket&ds.url=ws://localhost:8765"
 
 
 

launch/drivers/os_32_right_a.launch.py

@@ -35,6 +35,7 @@ def generate_launch_description():
     share_dir = get_package_share_directory('lexus_bringup')
     parameter_file = LaunchConfiguration('params_file')
     node_name = 'ouster_driver'
+    node_id = 'os_right'
 
     # Acquire the driver param file
     params_declare = DeclareLaunchArgument('params_file',
@@ -50,6 +51,7 @@ def generate_launch_description():
                                 parameters=[parameter_file],
                                 arguments=['--ros-args', '--log-level', 'INFO'],
                                 namespace='/',
+                                remappings=[('/points', node_id + '/points')],
                                 )
 
     configure_event = EmitEvent(

launch/drivers/os_32_right_a_driver_config.yaml

@@ -6,14 +6,14 @@ ouster_driver:
     lidar_mode: "1024x10"
     imu_port: 7503
     lidar_port: 7502
-    sensor_frame: laser_sensor_frame
-    laser_frame: laser_data_frame
-    imu_frame: imu_data_frame
+    sensor_frame: os_right_a
+    laser_frame: os_right_a_laser_data_frame
+    imu_frame: os_right_a_imu_data_frame
 
     # if False, data are published with sensor data QoS. This is preferrable
     # for production but default QoS is needed for rosbag.
     # See: https://github.com/ros2/rosbag2/issues/125
-    use_system_default_qos: False
+    use_system_default_qos: True
 
     # Set the method used to timestamp measurements.
     # Valid modes are:
@@ -32,7 +32,7 @@ ouster_driver:
     # activated upon startup of the driver. This will determine the topics
     # that are available for client applications to consume. The defacto
     # reference for these values are defined in:
-    # `include/ros2_ouster/processors/processor_factories.hpp`
+    # `include/ros2_ouster/procecssors/processor_factories.hpp`
     #
     # For convenience, the available data processors are:
     #

launch/drivers/os_64_center_a.launch.py

@@ -35,6 +35,7 @@ def generate_launch_description():
     share_dir = get_package_share_directory('lexus_bringup')
     parameter_file = LaunchConfiguration('params_file')
     node_name = 'ouster_driver'
+    node_id = 'os_center'
 
     # Acquire the driver param file
     params_declare = DeclareLaunchArgument('params_file',
@@ -44,12 +45,13 @@ def generate_launch_description():
 
     driver_node = LifecycleNode(package='ros2_ouster',
                                 executable='ouster_driver',
-                                name=node_name,
+                                name=node_name, ## TODO
                                 output='screen',
                                 emulate_tty=True,
                                 parameters=[parameter_file],
                                 arguments=['--ros-args', '--log-level', 'INFO'],
                                 namespace='/',
+                                remappings=[('/points', node_id + '/points')],
                                 )
 
     configure_event = EmitEvent(

launch/drivers/os_64_center_a_driver_config.yaml

@@ -6,14 +6,14 @@ ouster_driver:
     lidar_mode: "1024x10"
     imu_port: 7503
     lidar_port: 7502
-    sensor_frame: laser_sensor_frame
-    laser_frame: laser_data_frame
-    imu_frame: imu_data_frame
+    sensor_frame: os_center_a
+    laser_frame: os_center_a_laser_data_frame
+    imu_frame: os_center_a_imu_data_frame
 
     # if False, data are published with sensor data QoS. This is preferrable
     # for production but default QoS is needed for rosbag.
     # See: https://github.com/ros2/rosbag2/issues/125
-    use_system_default_qos: False
+    use_system_default_qos: True
 
     # Set the method used to timestamp measurements.
     # Valid modes are:

launch/tf_static.launch.py

@@ -0,0 +1,86 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+
+def generate_launch_description():
+
+    #pkg_name = 'lexus_bringup'
+    #pkg_dir = os.popen('/bin/bash -c "source /usr/share/colcon_cd/function/colcon_cd.sh && colcon_cd %s && pwd"' % pkg_name).read().strip()
+
+
+    return LaunchDescription([
+        Node(
+            package='tf2_ros',
+            #namespace='lexus3',
+            executable='static_transform_publisher',
+            name='gyor0_tf_publisher',
+            output='screen',
+            ##  Old-style arguments are deprecated, parameters should be used, but this does not work TODO
+            arguments=['697237.0', '5285644.0', '0.0','0', '0', '0', '1','map','map_gyor_0'],
+            #parameters=[{'translation.x': 697237.0, 'translation.y': 5285644.0, 'translation.z': 0.0, 'rotation.x': 0.0, 'rotation.y': 0.0, 'rotation.z': 0.0, 'rotation.w': 1.0, 'frame_id': 'map', 'child_frame_id': 'map_gyor_0'}]
+        ),
+        Node(
+            package='tf2_ros',
+            #namespace='lexus3',
+            executable='static_transform_publisher',
+            name='zala0_tf_publisher',
+            output='screen',
+            arguments=['639770.0', '5195040.0', '0.0','0', '0', '0', '1','map','map_zala_0'],
+        ),
+        Node(
+            package='tf2_ros',
+            #namespace='lexus3',
+            executable='static_transform_publisher',
+            name='duro_gps_tf_publisher',
+            output='screen',
+            arguments=['1.6', '0.0', '0.2','0', '0', '0', '1','base_link','duro_gps'],
+        ),
+        Node(
+            package='tf2_ros',
+            #namespace='lexus3',
+            executable='static_transform_publisher',
+            name='zed_camera_front_tf_publisher',
+            output='screen',
+            arguments=['1.6', '0.0', '1.286','0', '0', '0', '1','base_link','zed_camera_front'],
+        ),
+        Node(
+            package='tf2_ros',
+            #namespace='lexus3',
+            executable='static_transform_publisher',
+            name='duro_gps_imu_tf_publisher',
+            output='screen',
+            arguments=['0.0', '0.0', '0.2','0', '0', '0', '1','base_link','duro_gps_imu'],
+        ),
+        Node(
+            package='tf2_ros',
+            #namespace='lexus3',
+            executable='static_transform_publisher',
+            name='base_link_ground_link_publisher',
+            output='screen',
+            arguments=['0.0', '0.0', '-0.37','0', '0', '0', '1','base_link','ground_link'],
+        ),
+        Node(
+            package='tf2_ros',
+            #namespace='lexus3',
+            executable='static_transform_publisher',
+            name='left1_os_front_tf_publisher',
+            output='screen',
+            arguments=['1.6', '0.5', '1.3','0', '0', '0', '1','base_link','os_left_a'],
+        ),
+        Node(
+            package='tf2_ros',
+            #namespace='lexus3',
+            executable='static_transform_publisher',
+            name='right1_os_front_tf_publisher',
+            output='screen',
+            arguments=['1.53', '-0.5', '1.41','0', '0', '0', '1','base_link','os_right_a'],
+        ),
+        Node(
+            package='tf2_ros',
+            #namespace='lexus3',
+            executable='static_transform_publisher',
+            name='center1_os_front_tf_publisher',
+            output='screen',
+            arguments=['0.75', '0.0', '1.91','0', '0', '0', '1','base_link','os_center_a'],
+        )
+    ])