Class CollisionMonitor

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Class Documentation

class CollisionMonitor : public nav2_route::RouteOperation

A route operation to process a costmap to determine if a route is blocked requiring immediate rerouting. If using the local costmap topic, then it will check in the local time horizon only. if using the global, it may check the full route for continued validity. It is however recommended to specify a maximum collision distance for evaluation to prevent necessarily long-term evaluation of collision information which may not be representative of the conditions in that area by the time the robot gets there.

Public Functions

CollisionMonitor() = default

Constructor.

virtual ~CollisionMonitor() = default

destructor

virtual void configure(const rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_subscriber, const std::string &name) override

Configure.

inline virtual std::string getName() override

Get name of the plugin for parameter scope mapping.

Returns:

Name

inline virtual RouteOperationType processType() override

Indication that the adjust speed limit route operation is performed on all state changes.

Returns:

The type of operation (on graph call, on status changes, or constantly)

virtual OperationResult perform(NodePtr, EdgePtr curr_edge, EdgePtr, const Route &route, const geometry_msgs::msg::PoseStamped &curr_pose, const Metadata*) override

The main speed limit operation to adjust the maximum speed of the vehicle.

Parameters:

  • mdata – Metadata corresponding to the operation in the navigation graph. If metadata is invalid or irrelevant, a nullptr is given

  • node_achieved – Node achieved, for additional context

  • edge_entered – Edge entered by node achievement, for additional context

  • edge_exited – Edge exited by node achievement, for additional context

  • route – Current route being tracked in full, for additional context

  • curr_pose – Current robot pose in the route frame, for additional context

Returns:

Whether to perform rerouting and report blocked edges in that case

Protected Functions

Coordinates backoutValidEndPoint(const Coordinates &start, const Coordinates &end, const float dist)

Backs out the end coordinate along the line segment start-end to length dist.

Parameters:

  • start – Start of line segment

  • end – End of line segment

  • dist – Distance along line segment to find the new end point along

Returns:

Coordinates of the new end point dist away from the start along the line

bool backoutValidEndPoint(const Coordinates &start, LineSegment &line)

Backs out the line end coordinates of the start-end line segment where costmap transforms are possible.

Parameters:

  • start – Start of line segment

  • line – LineSegment object to replace the x1/y1 values for along segment until invalid

Returns:

If any part s of the segment requested is valid

bool lineToMap(const Coordinates &start, const Coordinates &end, LineSegment &line)

Converts a line segment start-end into a LineSegment struct in costmap frame.

Parameters:

  • start – Start of line segment

  • end – End of line segment

  • line – LineSegment object to populate

Returns:

If line segment is valid (e.g. start and end both in costmap transforms)

bool isInCollision(const LineSegment &line)

Checks a line segment in costmap frame for validity.

Parameters:

line – LineSegment object to collision check in costmap set

Returns:

If any part of the line segment is in collision

void getCostmap()

Gets the latest costmap from the costmap subscriber.

Protected Attributes

std::string name_
std::string topic_
std::atomic_bool reroute_
rclcpp::Logger logger_ = {rclcpp::get_logger("CollisionMonitor")}
rclcpp::Clock::SharedPtr clock_
rclcpp::Time last_check_time_
rclcpp::Duration checking_duration_ = {0, 0}
float max_collision_dist_
float max_cost_
bool reroute_on_collision_
unsigned int check_resolution_ = {1u}
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_subscriber_
std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ = {nullptr}
struct LineSegment

Public Members

unsigned int x0
unsigned int y0
unsigned int x1
unsigned int y1