add lidar sensor and use obstacle_layer in costmap params

bkanator · bkanator · commit b1ca37e5f027 · 2026-03-17T14:02:20.000-04:00
diff --git a/src/hangar_sim/CMakeLists.txt b/src/hangar_sim/CMakeLists.txt
@@ -25,6 +25,7 @@ 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/config/config.yaml b/src/hangar_sim/config/config.yaml
@@ -20,6 +20,7 @@ hardware:
           package: "hangar_sim"
           path: "config/moveit/joint_limits.yaml"
       - mujoco_model: "description/hangar_scene.xml"
+      - mujoco_viewer: "false"
 
   # Specify any additional launch files for running the robot in simulation mode.
   # Used when hardware.simulated is True.
diff --git a/src/hangar_sim/description/hangar_scene.xml b/src/hangar_sim/description/hangar_scene.xml
@@ -14,17 +14,16 @@
   </visual>
 
   <worldbody>
-    <!-- Scene Camera -->
-    <site
+<site
       name="scene_camera_optical_frame"
       pos="-14.4207 -4.39686 9.04094"
-      quat="0.415847 -0.883719 0.194298 -0.09143"
+      quat="0 0.7071 -0.7071 0"
     />
     <camera
       name="scene_camera"
       pos="-14.4207 -4.39686 9.04094"
-      quat="0.883719 0.415847 -0.09143 -0.194298"
-      fovy="45"
+      quat="0.7071 0 0 -0.7071"
+      fovy="75"
       mode="fixed"
       resolution="1280 720"
     />
diff --git a/src/hangar_sim/description/ur5e_ridgeback.xml b/src/hangar_sim/description/ur5e_ridgeback.xml
@@ -3,6 +3,8 @@
 
   <option integrator="implicitfast" />
 
+  <size nuser_sensor="6" />
+
   <default>
     <default class="ur5e">
       <material specular="0.5" shininess="0.25" />
@@ -208,6 +210,87 @@
       <joint name="rotational_yaw_joint" type="hinge" axis="0 0 1" />
       <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.
+               31 beams × 9 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. -->
+          <body name="lidar_front_mount" pos="0.45 0 0.15">
+            <inertial
+              mass="0.25"
+              pos="0 0 0"
+              diaginertia="0.0001 0.0001 0.0001"
+            />
+            <replicate count="31" sep="-" offset="0 0 0" euler="0 0 0.15708">
+              <site
+                name="lidar_front"
+                size="0.01"
+                pos="0 0 0"
+                quat="0.6533 0.2706 -0.6533 0.2706"
+              />
+            </replicate>
+            <!-- TIM571 body: dark cylinder, visual-only -->
+            <geom
+              name="lidar_front_body"
+              type="cylinder"
+              size="0.030 0.042"
+              pos="0 0 0"
+              rgba="0.2 0.2 0.2 1"
+              contype="0"
+              conaffinity="0"
+              group="2"
+            />
+            <!-- Scan plane: cyan disk showing sweep area -->
+            <geom
+              name="lidar_front_scan_plane"
+              type="cylinder"
+              size="0.30 0.002"
+              pos="0 0 0"
+              rgba="0 1 1 0.3"
+              contype="0"
+              conaffinity="0"
+              group="2"
+            />
+          </body>
+
+          <!-- Rear SICK TIM571: mounted at x=-0.45m (rear bumper), z=0.15m.
+               31 beams × 9 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. -->
+          <body name="lidar_rear_mount" pos="-0.45 0 0.15">
+            <inertial
+              mass="0.25"
+              pos="0 0 0"
+              diaginertia="0.0001 0.0001 0.0001"
+            />
+            <replicate count="31" sep="-" offset="0 0 0" euler="0 0 0.15708">
+              <site
+                name="lidar_rear"
+                size="0.01"
+                pos="0 0 0"
+                quat="0.2706 -0.6533 -0.2706 -0.6533"
+              />
+            </replicate>
+            <!-- TIM571 body: dark cylinder, visual-only -->
+            <geom
+              name="lidar_rear_body"
+              type="cylinder"
+              size="0.030 0.042"
+              pos="0 0 0"
+              rgba="0.2 0.2 0.2 1"
+              contype="0"
+              conaffinity="0"
+              group="2"
+            />
+            <!-- Scan plane: magenta disk showing sweep area -->
+            <geom
+              name="lidar_rear_scan_plane"
+              type="cylinder"
+              size="0.30 0.002"
+              pos="0 0 0"
+              rgba="1 0 1 0.3"
+              contype="0"
+              conaffinity="0"
+              group="2"
+            />
+          </body>
           <geom
             name="chassis_link"
             mesh="chassis_link"
@@ -586,5 +669,15 @@
     <framequat objtype="site" objname="imu_site" />
     <gyro site="imu_site" />
     <accelerometer site="imu_site" />
+    <!-- Front TIM571: 270 deg FOV, 9 deg resolution (31 beams), 0.05-25m range -->
+    <rangefinder
+      site="lidar_front"
+      user="0.05 -2.35619 2.35619 0.15708 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.15708 0.05 25.0"
+    />
   </sensor>
 </mujoco>
diff --git a/src/hangar_sim/launch/sim/localization_launch.py b/src/hangar_sim/launch/sim/localization_launch.py
@@ -56,7 +56,7 @@ def generate_launch_description():
     use_respawn = LaunchConfiguration("use_respawn")
     log_level = LaunchConfiguration("log_level")
 
-    lifecycle_nodes = ["map_server", "amcl"]
+    lifecycle_nodes = ["map_server"]
 
     # Map fully qualified names to relative ones so the node's namespace can be prepended.
     # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
@@ -181,13 +181,6 @@ def generate_launch_description():
                 parameters=[configured_params],
                 remappings=remappings,
             ),
-            ComposableNode(
-                package="nav2_amcl",
-                plugin="nav2_amcl::AmclNode",
-                name="amcl",
-                parameters=[configured_params],
-                remappings=remappings,
-            ),
             ComposableNode(
                 package="nav2_lifecycle_manager",
                 plugin="nav2_lifecycle_manager::LifecycleManager",
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
@@ -284,8 +284,38 @@ def generate_launch_description():
         output="log",
     )
 
-    # Fuse state estimator for mobile base localization
     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.
+    laser_filter_node = Node(
+        package="laser_filters",
+        executable="scan_to_scan_filter_chain",
+        name="laser_angular_filter",
+        parameters=[
+            PathJoinSubstitution(
+                [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"),
+        ],
+        output="log",
+    )
+
+    # Fuse state estimator for mobile base localization
     fuse_state_estimator = Node(
         package="fuse_optimizers",
         executable="fixed_lag_smoother_node",
@@ -326,6 +356,8 @@ 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)
 
     return ld
diff --git a/src/hangar_sim/package.xml b/src/hangar_sim/package.xml
@@ -19,6 +19,7 @@
   <exec_depend>moveit_ros_perception</exec_depend>
   <exec_depend>moveit_studio_agent</exec_depend>
   <exec_depend>moveit_pro_behavior</exec_depend>
+  <exec_depend>laser_filters</exec_depend>
   <exec_depend>nav2_bringup</exec_depend>
   <exec_depend>picknik_accessories</exec_depend>
   <exec_depend>picknik_mujoco_ros</exec_depend>
diff --git a/src/hangar_sim/params/laser_filter_params.yaml b/src/hangar_sim/params/laser_filter_params.yaml
@@ -0,0 +1,17 @@
+# 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.
+#
+# 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.
+laser_angular_filter:
+  ros__parameters:
+    filter1:
+      name: angular_bounds
+      type: laser_filters/LaserScanAngularBoundsFilter
+      params:
+        lower_angle: -1.6231
+        upper_angle: 1.6231
diff --git a/src/hangar_sim/params/nav2_params.yaml b/src/hangar_sim/params/nav2_params.yaml
@@ -126,7 +126,7 @@ controller_server:
     progress_checker:
       plugin: "nav2_controller::SimpleProgressChecker"
       required_movement_radius: 0.5
-      movement_time_allowance: 10.0
+      movement_time_allowance: 20.0
     # Goal checker parameters
     #precise_goal_checker:
     #  plugin: "nav2_controller::SimpleGoalChecker"
@@ -142,7 +142,7 @@ controller_server:
       plugin: "nav2_mppi_controller::MPPIController"
       time_steps: 56
       model_dt: 0.05
-      batch_size: 2000
+      batch_size: 500
       vx_std: 0.2
       vy_std: 0.2
       wz_std: 0.4
@@ -241,41 +241,36 @@ local_costmap:
       robot_base_frame: ridgeback_base_link
       use_sim_time: True
       rolling_window: true
-      width: 3
-      height: 3
+      width: 10
+      height: 10
       resolution: 0.05
       footprint: "[[0.45, 0.375], [0.45, -0.375], [-0.45, -0.375], [-0.45, 0.375]]"
       footprint_padding: 0.025
-      plugins: ["voxel_layer", "inflation_layer"]
+      plugins: ["obstacle_layer", "inflation_layer"]
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 1.50
-        inflation_radius: 2.2
-      voxel_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.75
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True
-        publish_voxel_map: True
-        origin_z: 0.0
-        z_resolution: 0.1
-        z_voxels: 16
-        max_obstacle_height: 3.0
-        mark_threshold: 1
-        observation_sources: pointcloud
-        pointcloud:
-          topic: /left_camera/points
-          max_obstacle_height: 3.0
+        observation_sources: scan
+        scan:
+          topic: /scan_filtered
+          max_obstacle_height: 2.0
           clearing: True
           marking: True
-          raytrace_max_range: 3.0
+          data_type: "LaserScan"
+          raytrace_max_range: 10.0
           raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
-          data_type: "PointCloud2"
-      static_layer:
-        plugin: "nav2_costmap_2d::StaticLayer"
-        map_subscribe_transient_local: True
+          obstacle_max_range: 8.0
+          inf_is_valid: false
+          qos_overriding_options:
+            policy_kinds: [reliability]
+            reliability: best_effort
       always_send_full_costmap: True
 
+
 global_costmap:
   global_costmap:
     ros__parameters:
@@ -288,34 +283,32 @@ global_costmap:
       footprint_padding: 0.025
       resolution: 0.05
       track_unknown_space: true
-      plugins: ["static_layer", "voxel_layer", "inflation_layer"]
-      voxel_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True
-        publish_voxel_map: True
-        origin_z: 0.1
-        z_resolution: 0.0
-        z_voxels: 16
-        max_obstacle_height: 3.0
-        mark_threshold: 2
-        observation_sources: pointcloud
-        pointcloud:
-          topic: /left_camera/points
-          max_obstacle_height: 3.0
+        observation_sources: scan
+        scan:
+          topic: /scan_filtered
+          max_obstacle_height: 2.0
           clearing: True
           marking: True
+          data_type: "LaserScan"
           raytrace_max_range: 3.0
           raytrace_min_range: 0.0
           obstacle_max_range: 2.5
           obstacle_min_range: 0.0
-          data_type: "PointCloud2"
-      static_layer:
-        plugin: "nav2_costmap_2d::StaticLayer"
-        map_subscribe_transient_local: True
+          inf_is_valid: false
+          qos_overriding_options:
+            policy_kinds: [reliability]
+            reliability: best_effort
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 1.50
-        inflation_radius: 2.20
+        cost_scaling_factor: 4.0
+        inflation_radius: 0.7
       always_send_full_costmap: True
 
 map_server:
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,6 +25,7 @@ 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`
`28`	`29`	`DESTINATION lib/${PROJECT_NAME}`
`29`	`30`	`)`
`30`	`31`