RemoveInCollisionGoals

Looks over the input port goals and removes any waypoint that has a point or footprint cost above a certain threshold. This may be used to cull goal points passed from ComputePathThroughPoses to avoid waiting indefinitely on occupied waypoints.

Input Ports

`input_goals`

Type: nav_msgs::msg::Goals Default: N/A: A vector of goals to check if in collision.

`cost_threshold`

Type: double Default: 254.0: The cost threshold above which a waypoint is considered in collision and should be removed. If use_footprint = false, consider setting to 253 for occupied.

`use_footprint`

Type: bool Default: true: Whether to use the footprint cost or the point cost.

`consider_unknown_as_obstacle`

Type: bool Default: false: Whether to consider unknown cost (255) as obstacle.

`input_waypoint_statuses`

Type: vector<nav2_msgs::msg::WaypointStatus> Default: N/A: Original waypoint_statuses to mark waypoint status from.

`service_name`

Type: string Default: N/A: Costmap service name responsible for getting the cost. If not specified, /global_costmap/get_cost_global_costmap is used.

`server_timeout`

Type: chrono::milliseconds Default: N/A: Service server timeout (ms). If not provided, uses the BT Navigator's default_server_timeout parameter value (20 ms by default).

Output Ports

`output_goals`

Type: nav_msgs::msg::Goals Default: N/A: A vector of goals containing only those that are not in collision.

`output_waypoint_statuses`

Type: vector<nav2_msgs::msg::WaypointStatus> Default: N/A: waypoint_statuses with in-collision waypoints marked.

Example

<RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" cost_threshold="254.0"
                        use_footprint="true" service_name="/global_costmap/get_cost_global_costmap"
                        input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" />