cmd_vel_mux + base_link frame name + odometry_transformer (#6)

* fixed path bug

* Refactor launch scripts to source workspace setup from a consistent directory structure and include cmd_vel_mux in all relevant launch files.

* Update frame_id references from 'vehicle' to 'base_link' in launch files and source code for consistency

* Implemented new odom transformer node. Remove sensor offset parameters from configuration and source code; update odometry topic references to use '/odom_base_link' instead of '/state_estimation' across launch files and implementations.

* bug fix for building

* rviz bug

* wrong url fixed

Author: alexlin2

README.md

@@ -194,7 +194,7 @@ ros2 run pct_planner pcd_to_tomogram.py /home/user/maps/warehouse/map.pcd -o /ho
 
 The stack supports Unity-based simulation environments. Download a Unity environment model and unzip to:
 
-Download a [Unity environment model for the Mecanum wheel platform](https://drive.google.com/drive/folders/1G1JYkccvoSlxyySuTlPfvmrWoJUO8oSs?usp=sharing) and unzip the files to the 'src/base_autonomy/vehicle_simulator/mesh/unity' folder. The environment model files should look like below. For computers without a powerful GPU, please try the 'without_360_camera' version for a higher rendering rate.
+Download a [Unity environment model for the Mecanum wheel platform](https://drive.google.com/drive/folders/1GNz386h6wiiFuQQdaY2_HbNRyd7nKA1N) and unzip the files to the 'src/base_autonomy/vehicle_simulator/mesh/unity' folder. The environment model files should look like below. For computers without a powerful GPU, please try the 'without_360_camera' version for a higher rendering rate.
 
 mesh/<br>
 &nbsp;&nbsp;&nbsp;&nbsp;unity/<br>

src/base_autonomy/local_planner/config/unitree/unitree_g1.yaml

@@ -26,10 +26,6 @@ localPlanner:
     # obstacle height threshold
     obstacleHeightThre: 0.1 # 0.1 for flat terrain
 
-    # Sensor offset
-    sensorOffsetX: 0.0
-    sensorOffsetY: 0.0
-  
     # Vehicle dimensions
     vehicleLength: 0.5
     vehicleWidth: 0.5

src/base_autonomy/local_planner/launch/local_planner.launch

@@ -78,8 +78,8 @@
   </node>
 
   <!-- Static transform using sensor offsets from launch arguments (should match robot_config file values) -->
-  <node pkg="tf2_ros" exec="static_transform_publisher" name="vehicleTransPublisher" args="$(var sensorOffsetX) $(var sensorOffsetY) $(var sensorOffsetZ) 0 0 0 /sensor /vehicle"/>
+  <node pkg="tf2_ros" exec="static_transform_publisher" name="vehicleTransPublisher" args="$(var sensorOffsetX) $(var sensorOffsetY) $(var sensorOffsetZ) 0 0 0 sensor base_link"/>
 
-  <node pkg="tf2_ros" exec="static_transform_publisher" name="sensorTransPublisher" args="0 0 $(var cameraOffsetZ) -1.5707963 0 -1.5707963 /sensor /camera"/>
+  <node pkg="tf2_ros" exec="static_transform_publisher" name="sensorTransPublisher" args="0 0 $(var cameraOffsetZ) -1.5707963 0 -1.5707963 sensor camera"/>
 
 </launch>

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

@@ -185,6 +185,21 @@ def generate_launch_description():
         ]
     )
 
+    # Odom transformer: re-expresses /state_estimation (sensor frame) as
+    # odom_base_link (base_link frame) using the sensor->base_link static TF.
+    odomTransformer_node = Node(
+        package='odom_transformer',
+        executable='odom_transformer_node',
+        name='odom_transformer',
+        output='screen',
+        parameters=[{
+            'source_frame': 'sensor',
+            'target_frame': 'base_link',
+            'odom_topic_in': '/state_estimation',
+            'odom_topic_out': '/odom_base_link',
+        }]
+    )
+
     # Static transform publishers with sensor offsets from config
     vehicleTransPublisher_node = Node(
         package='tf2_ros',
@@ -195,8 +210,8 @@ def generate_launch_description():
             str(-sensor_offsets['sensorOffsetY']),
             str(-sensor_offsets['sensorOffsetZ']),
             '0', '0', '0',
-            '/sensor',
-            '/vehicle'
+            'sensor',
+            'base_link'
         ]
     )
 
@@ -211,7 +226,7 @@ def generate_launch_description():
             str(camera_offsets['cameraOffsetRoll']),
             str(camera_offsets['cameraOffsetPitch']),
             str(camera_offsets['cameraOffsetYaw']),
-            '/sensor', camera_link
+            'sensor', camera_link
         ]
     )
 
@@ -227,4 +242,5 @@ def generate_launch_description():
         pathFollower_node,
         vehicleTransPublisher_node,
         sensorTransPublisher_node,
+        odomTransformer_node,
     ])

src/base_autonomy/local_planner/src/localPlanner.cpp

@@ -52,8 +52,6 @@ double normalizeAngle(double angle) {
 string pathFolder;
 double vehicleLength = 0.6;
 double vehicleWidth = 0.6;
-double sensorOffsetX = 0;
-double sensorOffsetY = 0;
 bool twoWayDrive = true;
 double laserVoxelSize = 0.05;
 double terrainVoxelSize = 0.2;
@@ -169,8 +167,8 @@ void odometryHandler(const nav_msgs::msg::Odometry::ConstSharedPtr odom)
   vehicleRoll = roll;
   vehiclePitch = pitch;
   vehicleYaw = yaw;
-  vehicleX = odom->pose.pose.position.x - cos(yaw) * sensorOffsetX + sin(yaw) * sensorOffsetY;
-  vehicleY = odom->pose.pose.position.y - sin(yaw) * sensorOffsetX - cos(yaw) * sensorOffsetY;
+  vehicleX = odom->pose.pose.position.x;
+  vehicleY = odom->pose.pose.position.y;
   vehicleZ = odom->pose.pose.position.z;
 }
 
@@ -562,8 +560,6 @@ int main(int argc, char** argv)
   nh->declare_parameter<std::string>("pathFolder", pathFolder);
   nh->declare_parameter<double>("vehicleLength", vehicleLength);
   nh->declare_parameter<double>("vehicleWidth", vehicleWidth);
-  nh->declare_parameter<double>("sensorOffsetX", sensorOffsetX);
-  nh->declare_parameter<double>("sensorOffsetY", sensorOffsetY);
   nh->declare_parameter<bool>("twoWayDrive", twoWayDrive);
   nh->declare_parameter<double>("laserVoxelSize", laserVoxelSize);
   nh->declare_parameter<double>("terrainVoxelSize", terrainVoxelSize);
@@ -610,8 +606,6 @@ int main(int argc, char** argv)
   nh->get_parameter("pathFolder", pathFolder);
   nh->get_parameter("vehicleLength", vehicleLength);
   nh->get_parameter("vehicleWidth", vehicleWidth);
-  nh->get_parameter("sensorOffsetX", sensorOffsetX);
-  nh->get_parameter("sensorOffsetY", sensorOffsetY);
   nh->get_parameter("twoWayDrive", twoWayDrive);
   nh->get_parameter("laserVoxelSize", laserVoxelSize);
   nh->get_parameter("terrainVoxelSize", terrainVoxelSize);
@@ -655,7 +649,7 @@ int main(int argc, char** argv)
   nh->get_parameter("goalX", goalX);
   nh->get_parameter("goalY", goalY);
 
-  auto subOdometry = nh->create_subscription<nav_msgs::msg::Odometry>("/state_estimation", 5, odometryHandler);
+  auto subOdometry = nh->create_subscription<nav_msgs::msg::Odometry>("/odom_base_link", 5, odometryHandler);
 
   auto subLaserCloud = nh->create_subscription<sensor_msgs::msg::PointCloud2>("/registered_scan", 5, laserCloudHandler);
 
@@ -1061,7 +1055,7 @@ int main(int argc, char** argv)
           }
 
           path.header.stamp = rclcpp::Time(static_cast<uint64_t>(odomTime * 1e9));
-          path.header.frame_id = "vehicle";
+          path.header.frame_id = "base_link";
           pubPath->publish(path);
 
           #if PLOTPATHSET == 1
@@ -1107,7 +1101,7 @@ int main(int argc, char** argv)
           sensor_msgs::msg::PointCloud2 freePaths2;
           pcl::toROSMsg(*freePaths, freePaths2);
           freePaths2.header.stamp = rclcpp::Time(static_cast<uint64_t>(odomTime * 1e9));
-          freePaths2.header.frame_id = "vehicle";
+          freePaths2.header.frame_id = "base_link";
           pubFreePaths->publish(freePaths2);
           #endif
         }
@@ -1137,23 +1131,23 @@ int main(int argc, char** argv)
         path.poses[0].pose.orientation.w = 0;
 
         path.header.stamp = rclcpp::Time(static_cast<uint64_t>(odomTime * 1e9));
-        path.header.frame_id = "vehicle";
+        path.header.frame_id = "base_link";
         pubPath->publish(path);
 
         #if PLOTPATHSET == 1
         freePaths->clear();
         sensor_msgs::msg::PointCloud2 freePaths2;
         pcl::toROSMsg(*freePaths, freePaths2);
         freePaths2.header.stamp = rclcpp::Time(static_cast<uint64_t>(odomTime * 1e9));
-        freePaths2.header.frame_id = "vehicle";
+        freePaths2.header.frame_id = "base_link";
         pubFreePaths->publish(freePaths2);
         #endif
       }
 
       /*sensor_msgs::msg::PointCloud2 plannerCloud2;
       pcl::toROSMsg(*plannerCloudCrop, plannerCloud2);
       plannerCloud2.header.stamp = rclcpp::Time(static_cast<uint64_t>(odomTime * 1e9));
-      plannerCloud2.header.frame_id = "vehicle";
+      plannerCloud2.header.frame_id = "base_link";
       pubLaserCloud->publish(plannerCloud2);*/
     }
 

src/base_autonomy/local_planner/src/pathFollower.cpp

@@ -46,8 +46,6 @@ double normalizeAngle(double angle) {
 bool realRobot = false;
 string serialPort = "/dev/ttyACM0";
 int baudrate = 115200;
-double sensorOffsetX = 0;
-double sensorOffsetY = 0;
 int pubSkipNum = 1;
 int pubSkipCount = 0;
 bool twoWayDrive = true;
@@ -133,8 +131,8 @@ void odomHandler(const nav_msgs::msg::Odometry::ConstSharedPtr odomIn)
   vehicleRoll = roll;
   vehiclePitch = pitch;
   vehicleYaw = yaw;
-  vehicleX = odomIn->pose.pose.position.x - cos(yaw) * sensorOffsetX + sin(yaw) * sensorOffsetY;
-  vehicleY = odomIn->pose.pose.position.y - sin(yaw) * sensorOffsetX - cos(yaw) * sensorOffsetY;
+  vehicleX = odomIn->pose.pose.position.x;
+  vehicleY = odomIn->pose.pose.position.y;
   vehicleZ = odomIn->pose.pose.position.z;
 
   if ((fabs(roll) > inclThre * PI / 180.0 || fabs(pitch) > inclThre * PI / 180.0) && useInclToStop) {
@@ -246,8 +244,6 @@ int main(int argc, char** argv)
   nh->declare_parameter<bool>("realRobot", realRobot);
   nh->declare_parameter<string>("serialPort", serialPort);
   nh->declare_parameter<int>("baudrate", baudrate);
-  nh->declare_parameter<double>("sensorOffsetX", sensorOffsetX);
-  nh->declare_parameter<double>("sensorOffsetY", sensorOffsetY);
   nh->declare_parameter<int>("pubSkipNum", pubSkipNum);
   nh->declare_parameter<bool>("twoWayDrive", twoWayDrive);
   nh->declare_parameter<double>("lookAheadDis", lookAheadDis);
@@ -282,8 +278,6 @@ int main(int argc, char** argv)
   nh->get_parameter("realRobot", realRobot);
   nh->get_parameter("serialPort", serialPort);
   nh->get_parameter("baudrate", baudrate);
-  nh->get_parameter("sensorOffsetX", sensorOffsetX);
-  nh->get_parameter("sensorOffsetY", sensorOffsetY);
   nh->get_parameter("pubSkipNum", pubSkipNum);
   nh->get_parameter("twoWayDrive", twoWayDrive);
   nh->get_parameter("lookAheadDis", lookAheadDis);
@@ -315,7 +309,7 @@ int main(int argc, char** argv)
   nh->get_parameter("joyToSpeedDelay", joyToSpeedDelay);
   nh->get_parameter("goalYawGain", goalYawGain);
 
-  auto subOdom = nh->create_subscription<nav_msgs::msg::Odometry>("/state_estimation", 5, odomHandler);
+  auto subOdom = nh->create_subscription<nav_msgs::msg::Odometry>("/odom_base_link", 5, odomHandler);
 
   auto subPath = nh->create_subscription<nav_msgs::msg::Path>("/path", 5, pathHandler);
 
@@ -327,9 +321,9 @@ int main(int argc, char** argv)
 
   auto subSlowDown = nh->create_subscription<std_msgs::msg::Int8>("/slow_down", 5, slowDownHandler);
 
-  auto pubSpeed = nh->create_publisher<geometry_msgs::msg::TwistStamped>("/cmd_vel", 5);
+  auto pubSpeed = nh->create_publisher<geometry_msgs::msg::TwistStamped>("/navigation_cmd_vel", 5);
   geometry_msgs::msg::TwistStamped cmd_vel;
-  cmd_vel.header.frame_id = "vehicle";
+  cmd_vel.header.frame_id = "base_link";
 
   if (autonomyMode) {
     joySpeed = autonomySpeed / maxSpeed;

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

@@ -86,6 +86,12 @@ def generate_launch_description():
   		}]
   )
 
+  start_cmd_vel_mux = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('cmd_vel_mux'), 'launch', 'cmd_vel_mux-launch.py')
+    )
+  )
+
   ld = LaunchDescription()
 
   # Add the actions
@@ -105,4 +111,6 @@ def generate_launch_description():
   ld.add_action(start_visualization_tools)
   ld.add_action(start_joy)
 
+  ld.add_action(start_cmd_vel_mux)
+
   return ld

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

@@ -96,6 +96,12 @@ def generate_launch_description():
     }.items()
   )
 
+  start_cmd_vel_mux = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('cmd_vel_mux'), 'launch', 'cmd_vel_mux-launch.py')
+    )
+  )
+
   ld = LaunchDescription()
 
   # Add the actions
@@ -117,4 +123,6 @@ def generate_launch_description():
   ld.add_action(start_joy)
   ld.add_action(start_tare_planner)
 
+  ld.add_action(start_cmd_vel_mux)
+
   return ld

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

@@ -114,6 +114,12 @@ def generate_launch_description():
     condition=IfCondition(use_pct_planner)
   )
 
+  start_cmd_vel_mux = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('cmd_vel_mux'), 'launch', 'cmd_vel_mux-launch.py')
+    )
+  )
+
   ld = LaunchDescription()
 
   # Add the actions
@@ -137,4 +143,6 @@ def generate_launch_description():
   ld.add_action(start_far_planner)
   ld.add_action(start_pct_planner)
 
+  ld.add_action(start_cmd_vel_mux)
+
   return ld

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

@@ -91,6 +91,12 @@ def generate_launch_description():
       [get_package_share_directory('livox_ros_driver2'), '/launch_ROS2/msg_MID360_launch.py']),
   )
 
+  start_cmd_vel_mux = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('cmd_vel_mux'), 'launch', 'cmd_vel_mux-launch.py')
+    )
+  )
+
   ld = LaunchDescription()
 
   # Add the actions
@@ -111,4 +117,6 @@ def generate_launch_description():
   ld.add_action(start_joy)
   ld.add_action(start_mid360)
 
+  ld.add_action(start_cmd_vel_mux)
+
   return ld