remove the scan_mirror_fix and change mujoco plugin to handle the 180 roll

bkanator · bkanator · commit 1fcd6d988ba6 · 2026-03-19T10:47:46.000-04:00
diff --git a/src/hangar_sim/CMakeLists.txt b/src/hangar_sim/CMakeLists.txt
@@ -25,7 +25,6 @@ pluginlib_export_plugin_description_file(
 install(PROGRAMS
   script/odometry_joint_state_publisher.py
   script/odom_qos_relay.py
-  script/scan_mirror_fix.py
   DESTINATION lib/${PROJECT_NAME}
 )
 
diff --git a/src/hangar_sim/description/ur5e_ridgeback.xml b/src/hangar_sim/description/ur5e_ridgeback.xml
@@ -211,8 +211,12 @@
       <body name="base_platform" pos="0 0 0" euler="0 0 0">
         <body name="ridgeback_base_link">
           <!-- Front SICK TIM571: mounted at x=+0.45m (front bumper), z=0.15m.
-               91 beams x 3 deg = 270 deg FOV, sweeping CCW from beam-0 at -135 deg.
-               quat="0.6533 0.2706 -0.6533 0.2706": beam-0 +Z fires at -135 deg from +X. -->
+               91 beams x 3 deg = 270 deg FOV, sweeping CCW from beam-0 at -135 deg in robot frame.
+               Quat rule: site Z axis = beam-0 firing direction. Site X axis must point in world -Z
+               so the plugin's Rot(-90,Y) produces a Z-up _ROS frame.
+               Formula: R_base = Rot_Z(phi) * Rot_Y(+90 deg), where phi = beam-0 robot-frame angle.
+               Front: phi=-135 deg -> R_base = Rot_Z(-135)*Rot_Y(90) -> quat=(0.2706,0.6533,0.2706,-0.6533).
+               angle_min=0 in rangefinder user params so the plugin sets _ROS frame X = beam-0 direction. -->
           <body name="lidar_front_mount" pos="0.45 0 0.15">
             <inertial
               mass="0.25"
@@ -224,7 +228,7 @@
                 name="lidar_front"
                 size="0.01"
                 pos="0 0 0"
-                quat="0.6533 0.2706 -0.6533 0.2706"
+                quat="0.2706 0.6533 0.2706 -0.6533"
               />
             </replicate>
             <!-- TIM571 body: dark cylinder, visual-only -->
@@ -252,8 +256,9 @@
           </body>
 
           <!-- Rear SICK TIM571: mounted at x=-0.45m (rear bumper), z=0.15m.
-               91 beams x 3 deg = 270 deg FOV, sweeping CCW from beam-0 at +45 deg.
-               quat="0.2706 -0.6533 -0.2706 -0.6533": beam-0 +Z fires at +45 deg from +X. -->
+               91 beams x 3 deg = 270 deg FOV, sweeping CCW from beam-0 at +45 deg in robot frame.
+               Rear: phi=+45 deg -> R_base = Rot_Z(45)*Rot_Y(90) -> quat=(0.6533,-0.2706,0.6533,0.2706).
+               angle_min=0 in rangefinder user params so the plugin sets _ROS frame X = beam-0 direction. -->
           <body name="lidar_rear_mount" pos="-0.45 0 0.15">
             <inertial
               mass="0.25"
@@ -265,7 +270,7 @@
                 name="lidar_rear"
                 size="0.01"
                 pos="0 0 0"
-                quat="0.2706 -0.6533 -0.2706 -0.6533"
+                quat="0.6533 -0.2706 0.6533 0.2706"
               />
             </replicate>
             <!-- TIM571 body: dark cylinder, visual-only -->
@@ -669,15 +674,12 @@
     <framequat objtype="site" objname="imu_site" />
     <gyro site="imu_site" />
     <accelerometer site="imu_site" />
-    <!-- Front TIM571: 270 deg FOV, 3 deg resolution (91 beams), 0.05-25m range -->
-    <rangefinder
-      site="lidar_front"
-      user="0.05 -2.35619 2.35619 0.05236 0.05 25.0"
-    />
-    <!-- Rear TIM571: same specs, mounted facing backward for 360 deg coverage -->
-    <rangefinder
-      site="lidar_rear"
-      user="0.05 -2.35619 2.35619 0.05236 0.05 25.0"
-    />
+    <!-- Front TIM571: 270 deg FOV, 3 deg resolution (91 beams), 0.05-25m range.
+         angle_min=0 so the _ROS frame X axis points at beam-0 (per plugin convention).
+         The sweep is 0 to 4.7124 rad (270 deg) in the sensor frame, which maps to
+         beam-0 at -135 deg and beam-90 at +135 deg in the robot frame. -->
+    <rangefinder site="lidar_front" user="0.05 0.0 4.7124 0.05236 0.05 25.0" />
+    <!-- Rear TIM571: same convention — angle_min=0, frame X points at beam-0 (+45 deg robot frame). -->
+    <rangefinder site="lidar_rear" user="0.05 0.0 4.7124 0.05236 0.05 25.0" />
   </sensor>
 </mujoco>
diff --git a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
@@ -286,17 +286,8 @@ def generate_launch_description():
 
     hangar_sim_pkg = FindPackageShare("hangar_sim")
 
-    # Reverses the ranges array in /scan to correct the Y-axis flip caused by the
-    # MuJoCo lidar plugin's 180-deg roll in the lidar_*_ROS TF frames.
-    scan_mirror_fix = Node(
-        package="hangar_sim",
-        executable="scan_mirror_fix.py",
-        name="scan_mirror_fix",
-        output="log",
-    )
-
-    # Angular bounds filter: clips chassis self-hitting beams (±93° to ±135°) from
-    # /scan_mirrored, publishes clean scan to /scan_filtered for Nav2 costmap.
+    # Angular bounds filter: clips chassis self-hitting beams (±93° to ±135°),
+    # publishes clean scan to /scan_filtered for Nav2 costmap.
     laser_filter_node = Node(
         package="laser_filters",
         executable="scan_to_scan_filter_chain",
@@ -306,11 +297,7 @@ def generate_launch_description():
                 [hangar_sim_pkg, "params", "laser_filter_params.yaml"]
             ),
             {"use_sim_time": use_sim_time},
-            {"qos_overrides./scan_mirrored.subscription.reliability": "best_effort"},
-        ],
-        remappings=[
-            ("scan", "/scan_mirrored"),
-            ("scan_filtered", "/scan_filtered"),
+            {"qos_overrides./scan.subscription.reliability": "best_effort"},
         ],
         output="log",
     )
@@ -356,7 +343,6 @@ def generate_launch_description():
     ld.add_action(static_tf_map_to_odom)
     ld.add_action(odom_to_joint_state_repub)
     ld.add_action(odom_qos_relay)
-    ld.add_action(scan_mirror_fix)
     ld.add_action(laser_filter_node)
     ld.add_action(fuse_state_estimator)
 
diff --git a/src/hangar_sim/params/laser_filter_params.yaml b/src/hangar_sim/params/laser_filter_params.yaml
@@ -1,17 +1,20 @@
 # Angular bounds filter for the dual SICK TIM571 lidars.
 #
-# Both lidars (lidar_front_ROS and lidar_rear_ROS) publish to /scan.
-# Each covers -135 deg to +135 deg. Beams from +/-93 deg to +/-135 deg
-# point backward past the chassis edges and produce self-hits.
+# Both lidars publish to /scan with angle_min=0 and angle_max=4.7124 (270 deg sweep).
+# The lidar _ROS frame X axis points at beam-0, so scan angle alpha maps to robot frame
+# angle (alpha + frame_yaw):
+#   front lidar frame yaw = -135 deg: robot angle = scan_angle - 135 deg
+#   rear  lidar frame yaw =  +45 deg: robot angle = scan_angle + 45 deg
 #
-# This filter keeps only beams within +/-93 deg (-1.6231 to 1.6231 rad),
-# removing the self-hitting beams at the edges. Combined, both lidars
-# give 360 deg coverage.
+# Chassis self-hits occur when |robot_frame_angle| > 90 deg, i.e. beams looking more
+# than 90 deg backward. This maps to scan angles 0-45 deg and 225-270 deg for both lidars.
+# Keeping scan angles 45-225 deg (0.7854 to 3.9270 rad) eliminates all self-hits and
+# gives each lidar a clean 180 deg arc; combined they cover 360 deg.
 laser_angular_filter:
   ros__parameters:
     filter1:
       name: angular_bounds
       type: laser_filters/LaserScanAngularBoundsFilter
       params:
-        lower_angle: -1.6231
-        upper_angle: 1.6231
+        lower_angle: 0.7854
+        upper_angle: 3.9270
diff --git a/src/hangar_sim/params/nav2_params.yaml b/src/hangar_sim/params/nav2_params.yaml
@@ -235,8 +235,8 @@ controller_server:
 local_costmap:
   local_costmap:
     ros__parameters:
-      update_frequency: 5.0
-      publish_frequency: 2.0
+      update_frequency: 10.0
+      publish_frequency: 5.0
       global_frame: odom
       robot_base_frame: ridgeback_base_link
       use_sim_time: True
diff --git a/src/hangar_sim/script/scan_mirror_fix.py b/src/hangar_sim/script/scan_mirror_fix.py

Original file line number	Diff line number	Diff line change
`@@ -25,7 +25,6 @@ pluginlib_export_plugin_description_file(`
`25`	`25`	`install(PROGRAMS`
`26`	`26`	`script/odometry_joint_state_publisher.py`
`27`	`27`	`script/odom_qos_relay.py`
`28`		`- script/scan_mirror_fix.py`
`29`	`28`	`DESTINATION lib/${PROJECT_NAME}`
`30`	`29`	`)`
`31`	`30`