From a211d5253fefa55eac871fa849f0d8ad671bb1c5 Mon Sep 17 00:00:00 2001 From: humanoid desktop Date: Wed, 18 Nov 2026 14:23:26 -0700 Subject: [PATCH 1/2] golem_slam: inflate the local costmap past dynamic_footprint's inscribed radius The fixed footprint's rear edge sits at x = -0.20, so its inscribed radius equals the 0.2 m inflation exactly. dynamic_footprint adds the arm links' padded boxes, and arms hanging behind the pelvis push the rear edge back (~0.246 m in the H1-2 sim), so Nav2's local costmap logged "inflation radius smaller than the inscribed radius" on every footprint update, about once a second. Both robots' local costmaps, and the MPPI ObstaclesCritic that reads them, now inflate by 0.3 m. The global costmaps stay at 0.2 m, so the planner still plans through the same narrow kitchen gaps. Co-Authored-By: Claude Opus 5.5 --- core_ws/src/golem_slam/config/example.nav2.yaml | 9 ++++++--- core_ws/src/golem_slam/config/g1/nav2.yaml | 4 ++-- core_ws/src/golem_slam/config/h12/nav2.yaml | 4 ++-- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/core_ws/src/golem_slam/config/example.nav2.yaml b/core_ws/src/golem_slam/config/example.nav2.yaml index 5ac9878..823b2e8 100644 --- a/core_ws/src/golem_slam/config/example.nav2.yaml +++ b/core_ws/src/golem_slam/config/example.nav2.yaml @@ -126,8 +126,8 @@ controller_server: collision_margin_distance: 0.10 # m from the goal inside which the repulsion is turned off. near_goal_distance: 0.5 - # m, equal to the costmaps' inflation_radius. - inflation_radius: 0.2 + # m, equal to the local costmap's inflation_radius (the critic reads the local costmap). + inflation_radius: 0.3 # Equal to the costmaps' inflation layer cost_scaling_factor. cost_scaling_factor: 3.0 GoalCritic: @@ -348,6 +348,9 @@ local_costmap: raytrace_min_range: 0.0 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" + # Steepness of the cost's fall-off with distance from an obstacle. cost_scaling_factor: 1.0 - inflation_radius: 0.2 + # m around obstacles that carries cost; at least the largest inscribed + # radius dynamic_footprint publishes, or Nav2 logs an error per footprint. + inflation_radius: 0.3 always_send_full_costmap: True diff --git a/core_ws/src/golem_slam/config/g1/nav2.yaml b/core_ws/src/golem_slam/config/g1/nav2.yaml index 5d8c68e..53ef2e9 100644 --- a/core_ws/src/golem_slam/config/g1/nav2.yaml +++ b/core_ws/src/golem_slam/config/g1/nav2.yaml @@ -79,7 +79,7 @@ controller_server: collision_cost: 10000.0 collision_margin_distance: 0.10 near_goal_distance: 0.5 - inflation_radius: 0.2 # = the costmaps' inflation_radius + inflation_radius: 0.3 # = the local costmap's inflation_radius cost_scaling_factor: 3.0 GoalCritic: enabled: true @@ -246,5 +246,5 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 1.0 - inflation_radius: 0.2 + inflation_radius: 0.3 # >= dynamic_footprint's inscribed radius (the arms hanging behind the pelvis push it past 0.2 m) always_send_full_costmap: True diff --git a/core_ws/src/golem_slam/config/h12/nav2.yaml b/core_ws/src/golem_slam/config/h12/nav2.yaml index 3301e9b..cdc30d9 100644 --- a/core_ws/src/golem_slam/config/h12/nav2.yaml +++ b/core_ws/src/golem_slam/config/h12/nav2.yaml @@ -84,7 +84,7 @@ controller_server: collision_cost: 10000.0 collision_margin_distance: 0.10 near_goal_distance: 0.5 - inflation_radius: 0.2 # = the costmaps' inflation_radius + inflation_radius: 0.3 # = the local costmap's inflation_radius cost_scaling_factor: 3.0 GoalCritic: enabled: true @@ -255,5 +255,5 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 1.0 - inflation_radius: 0.2 + inflation_radius: 0.3 # >= dynamic_footprint's inscribed radius (the arms hanging behind the pelvis: up to ~0.25 m seen in the sim) always_send_full_costmap: True From 85cae5a9e56fcc33514f130b79544940882f95df Mon Sep 17 00:00:00 2001 From: Max Date: Thu, 19 Nov 2026 15:21:11 -0700 Subject: [PATCH 2/2] golem_slam: drop the arms from the footprint until the arm controller is up; global inflation past Nav2's padding dynamic_footprint publishes the body polygon alone until the first message on arms_ready_topic (default /left_ee_pose, which frame_task_server publishes once its start-up moves are done): those moves swing the arms behind the pelvis (an inscribed radius of 0.33-0.42 m on the H1-2), past the local costmap's 0.3 m, while the robot cannot drive yet. Both robots' global costmaps inflate by 0.22 m: Nav2 pads the fixed body footprint by its 0.01 m footprint_padding, so its inscribed radius is 0.21 m, and 0.2 m logged the same error once at start. On the Mac (H1-2 and G1 navigation stacks at 1.00x): no inflation error over start-up, named_config moves and Nav2 goals, from 13 (H1-2) with the 0.3 m local inflation alone. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_015EG7pDNJLWR31eQmyKaWhr --- .../src/golem_slam/config/example.nav2.yaml | 5 ++- core_ws/src/golem_slam/config/g1/nav2.yaml | 2 +- core_ws/src/golem_slam/config/h12/nav2.yaml | 2 +- .../golem_slam/dynamic_footprint.py | 42 ++++++++++++++++--- .../golem_slam/test/test_dynamic_footprint.py | 14 ++++++- 5 files changed, 53 insertions(+), 12 deletions(-) diff --git a/core_ws/src/golem_slam/config/example.nav2.yaml b/core_ws/src/golem_slam/config/example.nav2.yaml index 823b2e8..e66d6db 100644 --- a/core_ws/src/golem_slam/config/example.nav2.yaml +++ b/core_ws/src/golem_slam/config/example.nav2.yaml @@ -296,8 +296,9 @@ global_costmap: plugin: "nav2_costmap_2d::InflationLayer" # Steepness of the cost's fall-off with distance from an obstacle. cost_scaling_factor: 3.0 - # m around obstacles that carries cost. - inflation_radius: 0.2 + # m around obstacles that carries cost; more than the footprint's inscribed + # radius plus Nav2's footprint_padding (0.01 m), or Nav2 logs an error. + inflation_radius: 0.22 local_costmap: local_costmap: diff --git a/core_ws/src/golem_slam/config/g1/nav2.yaml b/core_ws/src/golem_slam/config/g1/nav2.yaml index 53ef2e9..16e5f4f 100644 --- a/core_ws/src/golem_slam/config/g1/nav2.yaml +++ b/core_ws/src/golem_slam/config/g1/nav2.yaml @@ -198,7 +198,7 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.2 + inflation_radius: 0.22 # > the body footprint's inscribed radius with Nav2's 0.01 m footprint_padding (0.21 m) local_costmap: local_costmap: ros__parameters: diff --git a/core_ws/src/golem_slam/config/h12/nav2.yaml b/core_ws/src/golem_slam/config/h12/nav2.yaml index cdc30d9..ef64077 100644 --- a/core_ws/src/golem_slam/config/h12/nav2.yaml +++ b/core_ws/src/golem_slam/config/h12/nav2.yaml @@ -206,7 +206,7 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.2 + inflation_radius: 0.22 # > the body footprint's inscribed radius with Nav2's 0.01 m footprint_padding (0.21 m) local_costmap: local_costmap: ros__parameters: diff --git a/core_ws/src/golem_slam/golem_slam/dynamic_footprint.py b/core_ws/src/golem_slam/golem_slam/dynamic_footprint.py index 604daa3..24fb1b6 100644 --- a/core_ws/src/golem_slam/golem_slam/dynamic_footprint.py +++ b/core_ws/src/golem_slam/golem_slam/dynamic_footprint.py @@ -12,15 +12,20 @@ executing or cancelling), the footprint is the body polygon alone: a hand reaching for a handle or a counter must not read as the robot colliding with it, or Nav2 would stop the base during a grasp. The same body polygon goes out -while any arm link has no TF within ``max_tf_age``. +until the first message on ``arms_ready_topic``, which the arm controller +publishes once its start-up moves are done (frame_task_server's +``/left_ee_pose``): those moves swing the arms behind the pelvis, past the +local costmap's inflation radius, while the robot cannot drive yet. It also goes +out while any arm link has no TF within ``max_tf_age``. Nav2's costmap reads a polygon on its ``footprint`` topic in place of its ``footprint`` parameter (nav2_costmap_2d's Costmap2DROS); the launch remaps ``footprint`` to the local costmap's. The global costmap keeps the fixed polygon, so the planner's paths do not change with the arms. -Subscribes ``/robot_description`` (std_msgs/String, transient local) and each -``arm_action_status`` topic (action_msgs/GoalStatusArray); publishes +Subscribes ``/robot_description`` (std_msgs/String, transient local), each +``arm_action_status`` topic (action_msgs/GoalStatusArray) and +``arms_ready_topic`` (geometry_msgs/PoseStamped); publishes ``footprint`` (geometry_msgs/Polygon, in ``base_frame``: Humble's costmap takes a Polygon with no frame and reads it in its robot_base_frame). @@ -34,6 +39,8 @@ moves by more than this, and at least once a second max_tf_age s; an arm link whose latest TF is older drops the arms arm_action_status action status topics whose live goals drop the arms + arms_ready_topic the arms count from its first message on (empty: from the + start) """ import yaml @@ -41,7 +48,7 @@ import numpy as np import rclpy from action_msgs.msg import GoalStatus, GoalStatusArray -from geometry_msgs.msg import Point32, Polygon +from geometry_msgs.msg import Point32, Polygon, PoseStamped from rclpy.duration import Duration from rclpy.node import Node from rclpy.qos import DurabilityPolicy, QoSProfile @@ -116,6 +123,20 @@ def arm_goal_live(status_arrays): for array in status_arrays for s in array.status_list) +def body_only_reason(arms_ready, status_arrays, ready_topic): + """ + Return why the footprint is the body alone, or None when the arms count. + + arms_ready is whether ready_topic has had a message; status_arrays are the arm + actions' latest GoalStatusArrays. + """ + if not arms_ready: + return f'the arm controller is starting up (no {ready_topic} yet)' + if arm_goal_live(status_arrays): + return 'an arm goal is in flight' + return None + + class DynamicFootprint(Node): """Publish the local costmap's footprint widened by the arms.""" @@ -133,12 +154,14 @@ def __init__(self): topics = [t for t in self.declare_parameter( 'arm_action_status', ['/frame_task/_action/status', '/named_config/_action/status']).value if t] + self.ready_topic = self.declare_parameter('arms_ready_topic', '/left_ee_pose').value self.boxes = None self.status = {} self.published = None self.last_publish = None self.reason = None + self.arms_ready = not self.ready_topic self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) @@ -151,12 +174,18 @@ def __init__(self): self.create_subscription( GoalStatusArray, topic, lambda msg, topic=topic: self.status.__setitem__(topic, msg), status_qos) + if self.ready_topic: + self.create_subscription(PoseStamped, self.ready_topic, self._on_ready, 1) self.pub = self.create_publisher(Polygon, 'footprint', 10) self.create_timer(1.0 / rate, self._tick) self.get_logger().info( f'footprint from {len(self.arm_links)} arm links, body ' f'{self.body.tolist()}, dropping the arms during goals on {topics}') + def _on_ready(self, _msg): + """Count the arms from now on: the arm controller's start-up moves are done.""" + self.arms_ready = True + def _on_description(self, msg): """Build the arm links' boxes from the URDF.""" try: @@ -189,8 +218,9 @@ def _arm_points(self): def _tick(self): """Recompute the footprint and publish it when it moved, or once a second.""" - if arm_goal_live(self.status.values()): - arms, reason = np.empty((0, 3)), 'an arm goal is in flight' + reason = body_only_reason(self.arms_ready, self.status.values(), self.ready_topic) + if reason: + arms = np.empty((0, 3)) else: arms, reason = self._arm_points() if arms is None: diff --git a/core_ws/src/golem_slam/test/test_dynamic_footprint.py b/core_ws/src/golem_slam/test/test_dynamic_footprint.py index 000990f..2ca1ed6 100644 --- a/core_ws/src/golem_slam/test/test_dynamic_footprint.py +++ b/core_ws/src/golem_slam/test/test_dynamic_footprint.py @@ -1,7 +1,7 @@ """Test the footprint geometry and the arm-goal check of golem_slam.dynamic_footprint.""" from action_msgs.msg import GoalStatus, GoalStatusArray -from golem_slam.dynamic_footprint import (arm_goal_live, box_corners, convex_hull, - footprint, moved, parse_polygon) +from golem_slam.dynamic_footprint import (arm_goal_live, body_only_reason, box_corners, + convex_hull, footprint, moved, parse_polygon) import numpy as np import pytest @@ -88,3 +88,13 @@ def test_live_goals_drop_the_arms(): GoalStatus.STATUS_CANCELING): assert arm_goal_live([status_array(GoalStatus.STATUS_SUCCEEDED), status_array(state)]) + + +def test_the_arms_count_only_after_the_controller_starts_up(): + """Before the ready topic's first message, or during a live arm goal, the body is alone.""" + done = [status_array(GoalStatus.STATUS_SUCCEEDED)] + live = [status_array(GoalStatus.STATUS_EXECUTING)] + assert 'starting up' in body_only_reason(False, done, '/left_ee_pose') + assert '/left_ee_pose' in body_only_reason(False, done, '/left_ee_pose') + assert body_only_reason(True, live, '/left_ee_pose') == 'an arm goal is in flight' + assert body_only_reason(True, done, '/left_ee_pose') is None