Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 9 additions & 5 deletions core_ws/src/golem_slam/config/example.nav2.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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
6 changes: 3 additions & 3 deletions core_ws/src/golem_slam/config/g1/nav2.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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
6 changes: 3 additions & 3 deletions core_ws/src/golem_slam/config/h12/nav2.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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
42 changes: 36 additions & 6 deletions core_ws/src/golem_slam/golem_slam/dynamic_footprint.py
Original file line number Diff line number Diff line change
Expand Up @@ -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).

Expand All @@ -34,14 +39,16 @@
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

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
Expand Down Expand Up @@ -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."""

Expand All @@ -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)

Expand All @@ -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:
Expand Down Expand Up @@ -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:
Expand Down
14 changes: 12 additions & 2 deletions core_ws/src/golem_slam/test/test_dynamic_footprint.py
Original file line number Diff line number Diff line change
@@ -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

Expand Down Expand Up @@ -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
Loading