Class CollisionMonitor

Inheritance Relationships

Base Type

  • public nav2_util::LifecycleNode

Class Documentation

class CollisionMonitor : public nav2_util::LifecycleNode

Collision Monitor ROS2 node.

Public Functions

explicit CollisionMonitor(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())

Constructor for the nav2_collision_monitor::CollisionMonitor.

Parameters:

options – Additional options to control creation of the node.

~CollisionMonitor()

Destructor for the nav2_collision_monitor::CollisionMonitor.

Protected Functions

nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &state) override

: Initializes and obtains ROS-parameters, creates main subscribers and publishers, creates polygons and data sources objects

Parameters:

state – Lifecycle Node’s state

Returns:

Success or Failure

nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &state) override

: Activates LifecyclePublishers, polygons and main processor, creates bond connection

Parameters:

state – Lifecycle Node’s state

Returns:

Success or Failure

nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &state) override

: Deactivates LifecyclePublishers, polygons and main processor, destroys bond connection

Parameters:

state – Lifecycle Node’s state

Returns:

Success or Failure

nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &state) override

: Resets all subscribers/publishers, polygons/data sources arrays

Parameters:

state – Lifecycle Node’s state

Returns:

Success or Failure

nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &state) override

Called in shutdown state.

Parameters:

state – Lifecycle Node’s state

Returns:

Success or Failure

void cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg)

Callback for input cmd_vel.

Parameters:

msg – Input cmd_vel message

void publishVelocity(const Action &robot_action)

Publishes output cmd_vel. If robot was stopped more than stop_pub_timeout_ seconds, quit to publish 0-velocity.

Parameters:

robot_action – Robot action to publish

bool getParameters(std::string &cmd_vel_in_topic, std::string &cmd_vel_out_topic, std::string &state_topic)

Supporting routine obtaining all ROS-parameters.

Parameters:

  • cmd_vel_in_topic – Output name of cmd_vel_in topic

  • cmd_vel_out_topic – Output name of cmd_vel_out topic is required.

  • state_topic – topic name for publishing collision monitor state

Returns:

True if all parameters were obtained or false in failure case

bool configurePolygons(const std::string &base_frame_id, const tf2::Duration &transform_tolerance)

Supporting routine creating and configuring all polygons.

Parameters:

  • base_frame_id – Robot base frame ID

  • transform_tolerance – Transform tolerance

Returns:

True if all polygons were configured successfully or false in failure case

bool configureSources(const std::string &base_frame_id, const std::string &odom_frame_id, const tf2::Duration &transform_tolerance, const rclcpp::Duration &source_timeout, const bool base_shift_correction)

Supporting routine creating and configuring all data sources.

Parameters:

  • base_frame_id – Robot base frame ID

  • odom_frame_id – Odometry frame ID. Used as global frame to get source->base time interpolated transform.

  • transform_tolerance – Transform tolerance

  • source_timeout – Maximum time interval in which data is considered valid

  • base_shift_correction – Whether to correct source data towards to base frame movement, considering the difference between current time and latest source time

Returns:

True if all sources were configured successfully or false in failure case

void process(const Velocity &cmd_vel_in)

Main processing routine.

Parameters:

cmd_vel_in – Input desired robot velocity

bool processStopSlowdownLimit(const std::shared_ptr<Polygon> polygon, const std::vector<Point> &collision_points, const Velocity &velocity, Action &robot_action) const

Processes the polygon of STOP, SLOWDOWN and LIMIT action type.

Parameters:

  • polygon – Polygon to process

  • collision_points – Array of 2D obstacle points

  • velocity – Desired robot velocity

  • robot_action – Output processed robot action

Returns:

True if returned action is caused by current polygon, otherwise false

bool processApproach(const std::shared_ptr<Polygon> polygon, const std::vector<Point> &collision_points, const Velocity &velocity, Action &robot_action) const

Processes APPROACH action type.

Parameters:

  • polygon – Polygon to process

  • collision_points – Array of 2D obstacle points

  • velocity – Desired robot velocity

  • robot_action – Output processed robot action

Returns:

True if returned action is caused by current polygon, otherwise false

void notifyActionState(const Action &robot_action, const std::shared_ptr<Polygon> action_polygon) const

Log and publish current robot action and polygon.

Parameters:

  • robot_action – Robot action to notify

  • action_polygon – Pointer to a polygon causing a selected action

void publishPolygons() const

Polygons publishing routine. Made for visualization.

Protected Attributes

std::shared_ptr<tf2_ros::Buffer> tf_buffer_

TF buffer.

std::shared_ptr<tf2_ros::TransformListener> tf_listener_

TF listener.

std::vector<std::shared_ptr<Polygon>> polygons_

Polygons array.

std::vector<std::shared_ptr<Source>> sources_

Data sources array.

rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_in_sub_

@beirf Input cmd_vel subscriber

rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_out_pub_

Output cmd_vel publisher.

rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionMonitorState>::SharedPtr state_pub_

CollisionMonitor state publisher.

rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr collision_points_marker_pub_

Collision points marker publisher.

bool process_active_

Whether main routine is active.

Action robot_action_prev_

Previous robot action.

rclcpp::Time stop_stamp_

Latest timestamp when robot has 0-velocity.

rclcpp::Duration stop_pub_timeout_

Timeout after which 0-velocity ceases to be published.