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. More...

#include <nav2_route/include/nav2_route/plugins/route_operations/collision_monitor.hpp>

Inheritance diagram for nav2_route::CollisionMonitor:

[legend]

Collaboration diagram for nav2_route::CollisionMonitor:

[legend]

Classes
struct	LineSegment

Public Member Functions
	CollisionMonitor ()=default
	Constructor.

virtual	~CollisionMonitor ()=default
	destructor

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

std::string	getName () override
	Get name of the plugin for parameter scope mapping. More...

RouteOperationType	processType () override
	Indication that the adjust speed limit route operation is performed on all state changes. More...

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. More...

Public Member Functions inherited from nav2_route::RouteOperation
	RouteOperation ()=default
	Constructor.

virtual	~RouteOperation ()=default
	Destructor.

Protected Member 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. More...

bool	backoutValidEndPoint (const Coordinates &start, LineSegment &line)
	Backs out the line end coordinates of the start-end line segment where costmap transforms are possible. More...

bool	lineToMap (const Coordinates &start, const Coordinates &end, LineSegment &line)
	Converts a line segment start-end into a LineSegment struct in costmap frame. More...

bool	isInCollision (const LineSegment &line)
	Checks a line segment in costmap frame for validity. More...

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}

Additional Inherited Members
Public Types inherited from nav2_route::RouteOperation
using	Ptr = std::shared_ptr< nav2_route::RouteOperation >

Detailed Description

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.

Definition at line 43 of file collision_monitor.hpp.

Member Function Documentation

◆ backoutValidEndPoint() [1/2]

Coordinates nav2_route::CollisionMonitor::backoutValidEndPoint	(	const Coordinates &	start,
		const Coordinates &	end,
		const float	dist
	)

protected

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

Definition at line 168 of file collision_monitor.cpp.

Referenced by perform().

Here is the caller graph for this function:

◆ backoutValidEndPoint() [2/2]

bool nav2_route::CollisionMonitor::backoutValidEndPoint	(	const Coordinates &	start,
		LineSegment &	line
	)

protected

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

Definition at line 183 of file collision_monitor.cpp.

References nav2_util::LineIterator::advance(), nav2_util::LineIterator::getX(), nav2_util::LineIterator::getY(), and nav2_util::LineIterator::isValid().

Here is the call graph for this function:

◆ getName()

std::string nav2_route::CollisionMonitor::getName ( )

inlineoverridevirtual

Get name of the plugin for parameter scope mapping.

Returns: Name

Implements nav2_route::RouteOperation.

Definition at line 73 of file collision_monitor.hpp.

Referenced by configure().

Here is the caller graph for this function:

◆ isInCollision()

bool nav2_route::CollisionMonitor::isInCollision ( const LineSegment & line )

protected

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

Definition at line 221 of file collision_monitor.cpp.

References nav2_util::LineIterator::advance(), nav2_util::LineIterator::getX(), nav2_util::LineIterator::getY(), and nav2_util::LineIterator::isValid().

Referenced by perform().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ lineToMap()

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

protected

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)

Definition at line 210 of file collision_monitor.cpp.

Referenced by perform().

Here is the caller graph for this function:

◆ perform()

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

overridevirtual

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

Implements nav2_route::RouteOperation.

Definition at line 90 of file collision_monitor.cpp.

References backoutValidEndPoint(), getCostmap(), isInCollision(), and lineToMap().

Here is the call graph for this function:

◆ processType()

RouteOperationType nav2_route::CollisionMonitor::processType ( )

inlineoverridevirtual

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)

Reimplemented from nav2_route::RouteOperation.

Definition at line 80 of file collision_monitor.hpp.

The documentation for this class was generated from the following files:

nav2_route/include/nav2_route/plugins/route_operations/collision_monitor.hpp
nav2_route/src/plugins/route_operations/collision_monitor.cpp

Classes

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Function Documentation

◆ backoutValidEndPoint() [1/2]

◆ backoutValidEndPoint() [2/2]

◆ getName()

◆ isInCollision()

◆ lineToMap()

◆ perform()

◆ processType()