Configurable static tf pub

src/base_autonomy/local_planner/launch/local_planner.launch.py

@@ -2,6 +2,7 @@
 import yaml
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -12,7 +13,7 @@ def generate_launch_description():
     local_planner_share = get_package_share_directory('local_planner')
 
     # Get robot config from environment variable or use default
-    robot_config_env = os.environ.get('ROBOT_CONFIG_PATH', 'unitree/unitree_g1')
+    robot_config_env = os.environ.get('ROBOT_CONFIG_PATH', 'mechanum_drive')
 
     # Declare launch arguments
     config_arg = DeclareLaunchArgument(
@@ -52,6 +53,12 @@ def generate_launch_description():
         default_value='0.0'
     )
 
+    static_tf_pub_arg = DeclareLaunchArgument(
+        'static_tf_pub',
+        default_value='true',
+        description='If false, do not publish vehicleTrans and sensorTrans static TFs'
+    )
+
     # Read sensor offsets from robot config YAML
     sensor_offsets = {
         'sensorOffsetX': 0.0,
@@ -212,7 +219,8 @@ def generate_launch_description():
             '0', '0', '0',
             'sensor',
             'base_link'
-        ]
+        ],
+        condition=IfCondition(LaunchConfiguration('static_tf_pub'))
     )
 
     sensorTransPublisher_node = Node(
@@ -227,7 +235,8 @@ def generate_launch_description():
             str(camera_offsets['cameraOffsetPitch']),
             str(camera_offsets['cameraOffsetYaw']),
             'sensor', camera_link
-        ]
+        ],
+        condition=IfCondition(LaunchConfiguration('static_tf_pub'))
     )
 
     return LaunchDescription([
@@ -238,6 +247,7 @@ def generate_launch_description():
         joyToSpeedDelay_arg,
         goalX_arg,
         goalY_arg,
+        static_tf_pub_arg,
         localPlanner_node,
         pathFollower_node,
         vehicleTransPublisher_node,

src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py

@@ -15,6 +15,7 @@ def generate_launch_description():
   vehicleX = LaunchConfiguration('vehicleX')
   vehicleY = LaunchConfiguration('vehicleY')
   checkTerrainConn = LaunchConfiguration('checkTerrainConn')
+  static_tf_pub = LaunchConfiguration('static_tf_pub')
 
   declare_world_name = DeclareLaunchArgument('world_name', default_value='real_world', description='')
   declare_sensorOffsetX = DeclareLaunchArgument('sensorOffsetX', default_value='0.05', description='')
@@ -23,6 +24,7 @@ def generate_launch_description():
   declare_vehicleX = DeclareLaunchArgument('vehicleX', default_value='0.0', description='')
   declare_vehicleY = DeclareLaunchArgument('vehicleY', default_value='0.0', description='')
   declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='')
+  declare_static_tf_pub = DeclareLaunchArgument('static_tf_pub', default_value='true', description='If false, do not publish vehicleTrans and sensorTrans static TFs')
 
   start_local_planner = IncludeLaunchDescription(
     PythonLaunchDescriptionSource(os.path.join(
@@ -35,6 +37,7 @@ def generate_launch_description():
       'cameraOffsetZ': cameraOffsetZ,
       'goalX': vehicleX,
       'goalY': vehicleY,
+      'static_tf_pub': static_tf_pub,
     }.items()
   )
 
@@ -102,6 +105,7 @@ def generate_launch_description():
   ld.add_action(declare_vehicleX)
   ld.add_action(declare_vehicleY)
   ld.add_action(declare_checkTerrainConn)
+  ld.add_action(declare_static_tf_pub)
 
   ld.add_action(start_local_planner)
   ld.add_action(start_terrain_analysis)

src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py

@@ -16,6 +16,7 @@ def generate_launch_description():
   vehicleX = LaunchConfiguration('vehicleX')
   vehicleY = LaunchConfiguration('vehicleY')
   checkTerrainConn = LaunchConfiguration('checkTerrainConn')
+  static_tf_pub = LaunchConfiguration('static_tf_pub')
 
   declare_exploration_planner_config = DeclareLaunchArgument('exploration_planner_config', default_value='indoor_small', description='')
   declare_world_name = DeclareLaunchArgument('world_name', default_value='real_world', description='')
@@ -25,6 +26,7 @@ def generate_launch_description():
   declare_vehicleX = DeclareLaunchArgument('vehicleX', default_value='0.0', description='')
   declare_vehicleY = DeclareLaunchArgument('vehicleY', default_value='0.0', description='')
   declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='')
+  declare_static_tf_pub = DeclareLaunchArgument('static_tf_pub', default_value='true', description='If false, do not publish vehicleTrans and sensorTrans static TFs')
 
   start_local_planner = IncludeLaunchDescription(
     PythonLaunchDescriptionSource(os.path.join(
@@ -37,6 +39,7 @@ def generate_launch_description():
       'cameraOffsetZ': cameraOffsetZ,
       'goalX': vehicleX,
       'goalY': vehicleY,
+      'static_tf_pub': static_tf_pub,
     }.items()
   )
 
@@ -113,6 +116,7 @@ def generate_launch_description():
   ld.add_action(declare_vehicleX)
   ld.add_action(declare_vehicleY)
   ld.add_action(declare_checkTerrainConn)
+  ld.add_action(declare_static_tf_pub)
 
   ld.add_action(start_local_planner)
   ld.add_action(start_terrain_analysis)

src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py

@@ -19,6 +19,7 @@ def generate_launch_description():
   vehicleX = LaunchConfiguration('vehicleX')
   vehicleY = LaunchConfiguration('vehicleY')
   checkTerrainConn = LaunchConfiguration('checkTerrainConn')
+  static_tf_pub = LaunchConfiguration('static_tf_pub')
 
   declare_use_pct_planner = DeclareLaunchArgument(
     'use_pct_planner',
@@ -33,6 +34,7 @@ def generate_launch_description():
   declare_vehicleX = DeclareLaunchArgument('vehicleX', default_value='0.0', description='')
   declare_vehicleY = DeclareLaunchArgument('vehicleY', default_value='0.0', description='')
   declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='')
+  declare_static_tf_pub = DeclareLaunchArgument('static_tf_pub', default_value='true', description='If false, do not publish vehicleTrans and sensorTrans static TFs')
 
   start_local_planner = IncludeLaunchDescription(
     PythonLaunchDescriptionSource(os.path.join(
@@ -45,6 +47,7 @@ def generate_launch_description():
       'cameraOffsetZ': cameraOffsetZ,
       'goalX': vehicleX,
       'goalY': vehicleY,
+      'static_tf_pub': static_tf_pub,
     }.items()
   )
 
@@ -132,6 +135,7 @@ def generate_launch_description():
   ld.add_action(declare_vehicleX)
   ld.add_action(declare_vehicleY)
   ld.add_action(declare_checkTerrainConn)
+  ld.add_action(declare_static_tf_pub)
 
   ld.add_action(start_local_planner)
   ld.add_action(start_terrain_analysis)

src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch.py

@@ -15,6 +15,7 @@ def generate_launch_description():
   vehicleX = LaunchConfiguration('vehicleX')
   vehicleY = LaunchConfiguration('vehicleY')
   checkTerrainConn = LaunchConfiguration('checkTerrainConn')
+  static_tf_pub = LaunchConfiguration('static_tf_pub')
 
   declare_world_name = DeclareLaunchArgument('world_name', default_value='real_world', description='')
   declare_sensorOffsetX = DeclareLaunchArgument('sensorOffsetX', default_value='0.05', description='')
@@ -23,6 +24,7 @@ def generate_launch_description():
   declare_vehicleX = DeclareLaunchArgument('vehicleX', default_value='0.0', description='')
   declare_vehicleY = DeclareLaunchArgument('vehicleY', default_value='0.0', description='')
   declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='')
+  declare_static_tf_pub = DeclareLaunchArgument('static_tf_pub', default_value='true', description='If false, do not publish vehicleTrans and sensorTrans static TFs')
 
   start_local_planner = IncludeLaunchDescription(
     PythonLaunchDescriptionSource(os.path.join(
@@ -35,6 +37,7 @@ def generate_launch_description():
       'cameraOffsetZ': cameraOffsetZ,
       'goalX': vehicleX,
       'goalY': vehicleY,
+      'static_tf_pub': static_tf_pub,
     }.items()
   )
 
@@ -107,6 +110,7 @@ def generate_launch_description():
   ld.add_action(declare_vehicleX)
   ld.add_action(declare_vehicleY)
   ld.add_action(declare_checkTerrainConn)
+  ld.add_action(declare_static_tf_pub)
 
   ld.add_action(start_local_planner)
   ld.add_action(start_terrain_analysis)

src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch.py

@@ -16,6 +16,7 @@ def generate_launch_description():
   vehicleX = LaunchConfiguration('vehicleX')
   vehicleY = LaunchConfiguration('vehicleY')
   checkTerrainConn = LaunchConfiguration('checkTerrainConn')
+  static_tf_pub = LaunchConfiguration('static_tf_pub')
 
   declare_exploration_planner_config = DeclareLaunchArgument('exploration_planner_config', default_value='indoor_small', description='')
   declare_world_name = DeclareLaunchArgument('world_name', default_value='real_world', description='')
@@ -25,6 +26,7 @@ def generate_launch_description():
   declare_vehicleX = DeclareLaunchArgument('vehicleX', default_value='0.0', description='')
   declare_vehicleY = DeclareLaunchArgument('vehicleY', default_value='0.0', description='')
   declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='')
+  declare_static_tf_pub = DeclareLaunchArgument('static_tf_pub', default_value='true', description='If false, do not publish vehicleTrans and sensorTrans static TFs')
 
   start_local_planner = IncludeLaunchDescription(
     PythonLaunchDescriptionSource(os.path.join(
@@ -37,6 +39,7 @@ def generate_launch_description():
       'cameraOffsetZ': cameraOffsetZ,
       'goalX': vehicleX,
       'goalY': vehicleY,
+      'static_tf_pub': static_tf_pub,
     }.items()
   )
 
@@ -118,6 +121,7 @@ def generate_launch_description():
   ld.add_action(declare_vehicleX)
   ld.add_action(declare_vehicleY)
   ld.add_action(declare_checkTerrainConn)
+  ld.add_action(declare_static_tf_pub)
 
   ld.add_action(start_local_planner)
   ld.add_action(start_terrain_analysis)

src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch.py

@@ -19,6 +19,7 @@ def generate_launch_description():
   vehicleX = LaunchConfiguration('vehicleX')
   vehicleY = LaunchConfiguration('vehicleY')
   checkTerrainConn = LaunchConfiguration('checkTerrainConn')
+  static_tf_pub = LaunchConfiguration('static_tf_pub')
 
   declare_use_pct_planner = DeclareLaunchArgument(
     'use_pct_planner',
@@ -33,6 +34,7 @@ def generate_launch_description():
   declare_vehicleX = DeclareLaunchArgument('vehicleX', default_value='0.0', description='')
   declare_vehicleY = DeclareLaunchArgument('vehicleY', default_value='0.0', description='')
   declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='')
+  declare_static_tf_pub = DeclareLaunchArgument('static_tf_pub', default_value='true', description='If false, do not publish vehicleTrans and sensorTrans static TFs')
 
   start_local_planner = IncludeLaunchDescription(
     PythonLaunchDescriptionSource(os.path.join(
@@ -45,6 +47,7 @@ def generate_launch_description():
       'cameraOffsetZ': cameraOffsetZ,
       'goalX': vehicleX,
       'goalY': vehicleY,
+      'static_tf_pub': static_tf_pub,
     }.items()
   )
 
@@ -137,6 +140,7 @@ def generate_launch_description():
   ld.add_action(declare_vehicleX)
   ld.add_action(declare_vehicleY)
   ld.add_action(declare_checkTerrainConn)
+  ld.add_action(declare_static_tf_pub)
 
   ld.add_action(start_local_planner)
   ld.add_action(start_terrain_analysis)

src/base_autonomy/vehicle_simulator/launch/system_simulation.launch.py

@@ -16,6 +16,7 @@ def generate_launch_description():
   terrainZ = LaunchConfiguration('terrainZ')
   vehicleYaw = LaunchConfiguration('vehicleYaw')
   checkTerrainConn = LaunchConfiguration('checkTerrainConn')
+  static_tf_pub = LaunchConfiguration('static_tf_pub')
 
   declare_world_name = DeclareLaunchArgument('world_name', default_value='unity', description='')
   declare_vehicleHeight = DeclareLaunchArgument('vehicleHeight', default_value='0.75', description='')
@@ -25,6 +26,7 @@ def generate_launch_description():
   declare_terrainZ = DeclareLaunchArgument('terrainZ', default_value='0.0', description='')
   declare_vehicleYaw = DeclareLaunchArgument('vehicleYaw', default_value='0.0', description='')
   declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='')
+  declare_static_tf_pub = DeclareLaunchArgument('static_tf_pub', default_value='true', description='If false, do not publish vehicleTrans and sensorTrans static TFs')
 
   start_local_planner = IncludeLaunchDescription(
     PythonLaunchDescriptionSource(os.path.join(
@@ -35,6 +37,7 @@ def generate_launch_description():
       'cameraOffsetZ': cameraOffsetZ,
       'goalX': vehicleX,
       'goalY': vehicleY,
+      'static_tf_pub': static_tf_pub,
     }.items()
   )
 
@@ -136,6 +139,7 @@ def generate_launch_description():
   ld.add_action(declare_terrainZ)
   ld.add_action(declare_vehicleYaw)
   ld.add_action(declare_checkTerrainConn)
+  ld.add_action(declare_static_tf_pub)
 
   ld.add_action(start_local_planner)
   ld.add_action(start_terrain_analysis)

src/base_autonomy/vehicle_simulator/launch/system_simulation_with_exploration_planner.launch.py

@@ -17,6 +17,7 @@ def generate_launch_description():
   terrainZ = LaunchConfiguration('terrainZ')
   vehicleYaw = LaunchConfiguration('vehicleYaw')
   checkTerrainConn = LaunchConfiguration('checkTerrainConn')
+  static_tf_pub = LaunchConfiguration('static_tf_pub')
 
   declare_exploration_planner_config = DeclareLaunchArgument('exploration_planner_config', default_value='indoor_small', description='')
   declare_world_name = DeclareLaunchArgument('world_name', default_value='unity', description='')
@@ -27,6 +28,7 @@ def generate_launch_description():
   declare_terrainZ = DeclareLaunchArgument('terrainZ', default_value='0.0', description='')
   declare_vehicleYaw = DeclareLaunchArgument('vehicleYaw', default_value='0.0', description='')
   declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='')
+  declare_static_tf_pub = DeclareLaunchArgument('static_tf_pub', default_value='true', description='If false, do not publish vehicleTrans and sensorTrans static TFs')
 
   start_local_planner = IncludeLaunchDescription(
     PythonLaunchDescriptionSource(os.path.join(
@@ -37,6 +39,7 @@ def generate_launch_description():
       'cameraOffsetZ': cameraOffsetZ,
       'goalX': vehicleX,
       'goalY': vehicleY,
+      'static_tf_pub': static_tf_pub,
     }.items()
   )
 
@@ -147,6 +150,7 @@ def generate_launch_description():
   ld.add_action(declare_terrainZ)
   ld.add_action(declare_vehicleYaw)
   ld.add_action(declare_checkTerrainConn)
+  ld.add_action(declare_static_tf_pub)
 
   ld.add_action(start_local_planner)
   ld.add_action(start_terrain_analysis)

src/base_autonomy/vehicle_simulator/launch/system_simulation_with_route_planner.launch.py

@@ -20,6 +20,7 @@ def generate_launch_description():
   terrainZ = LaunchConfiguration('terrainZ')
   vehicleYaw = LaunchConfiguration('vehicleYaw')
   checkTerrainConn = LaunchConfiguration('checkTerrainConn')
+  static_tf_pub = LaunchConfiguration('static_tf_pub')
 
   declare_use_pct_planner = DeclareLaunchArgument(
     'use_pct_planner',
@@ -35,6 +36,7 @@ def generate_launch_description():
   declare_terrainZ = DeclareLaunchArgument('terrainZ', default_value='0.0', description='')
   declare_vehicleYaw = DeclareLaunchArgument('vehicleYaw', default_value='0.0', description='')
   declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='')
+  declare_static_tf_pub = DeclareLaunchArgument('static_tf_pub', default_value='true', description='If false, do not publish vehicleTrans and sensorTrans static TFs')
 
   start_local_planner = IncludeLaunchDescription(
     PythonLaunchDescriptionSource(os.path.join(
@@ -45,6 +47,7 @@ def generate_launch_description():
       'cameraOffsetZ': cameraOffsetZ,
       'goalX': vehicleX,
       'goalY': vehicleY,
+      'static_tf_pub': static_tf_pub,
     }.items()
   )
 
@@ -160,6 +163,7 @@ def generate_launch_description():
   ld.add_action(declare_terrainZ)
   ld.add_action(declare_vehicleYaw)
   ld.add_action(declare_checkTerrainConn)
+  ld.add_action(declare_static_tf_pub)
 
   ld.add_action(start_local_planner)
   ld.add_action(start_terrain_analysis)

src/utilities/livox_ros_driver2/config/MID360_config.json

@@ -11,21 +11,21 @@
       "log_data_port": 56500
     },
     "host_net_info" : {
-      "cmd_data_ip" : "192.168.1.5",
+      "cmd_data_ip" : "192.168.123.5",
       "cmd_data_port": 56101,
-      "push_msg_ip": "192.168.1.5",
+      "push_msg_ip": "192.168.123.5",
       "push_msg_port": 56201,
-      "point_data_ip": "192.168.1.5",
+      "point_data_ip": "192.168.123.5",
       "point_data_port": 56301,
-      "imu_data_ip" : "192.168.1.5",
+      "imu_data_ip" : "192.168.123.5",
       "imu_data_port": 56401,
       "log_data_ip" : "",
       "log_data_port": 56501
     }
   },
   "lidar_configs" : [
     {
-      "ip" : "192.168.1.131",
+      "ip" : "192.168.123.120",
       "pcl_data_type" : 1,
       "pattern_mode" : 0,
       "extrinsic_parameter" : {