diff --git a/core_ws/src/golem_slam/config/example.nav2.yaml b/core_ws/src/golem_slam/config/example.nav2.yaml index 5ac9878..e66d6db 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: @@ -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: @@ -348,6 +349,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..16e5f4f 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 @@ -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: @@ -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..ef64077 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 @@ -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: @@ -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 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