Class Polygon

Inheritance Relationships

Derived Type

Class Documentation

class Polygon

Basic polygon shape class. For STOP/SLOWDOWN model it represents zone around the robot while for APPROACH model it represents robot footprint.

Subclassed by nav2_collision_monitor::Circle

Public Functions

Polygon(const nav2_util::LifecycleNode::WeakPtr &node, const std::string &polygon_name, const std::shared_ptr<tf2_ros::Buffer> tf_buffer, const std::string &base_frame_id, const tf2::Duration &transform_tolerance)

Polygon constructor.

Parameters:

  • node – Collision Monitor node pointer

  • polygon_name – Name of polygon

  • tf_buffer – Shared pointer to a TF buffer

  • base_frame_id – Robot base frame ID

  • transform_tolerance – Transform tolerance

virtual ~Polygon()

Polygon destructor.

bool configure()

Shape configuration routine. Obtains ROS-parameters related to shape object and creates polygon lifecycle publisher.

Returns:

True in case of everything is configured correctly, or false otherwise

void activate()

Activates polygon lifecycle publisher.

void deactivate()

Deactivates polygon lifecycle publisher.

std::string getName() const

Returns the name of polygon.

Returns:

Polygon name

ActionType getActionType() const

Obtains polygon action type.

Returns:

Action type for current polygon

bool getEnabled() const

Obtains polygon enabled state.

Returns:

Whether polygon is enabled

int getMaxPoints() const

Obtains polygon maximum points to enter inside polygon causing no action.

Returns:

Maximum points to enter to current polygon and take no action

double getSlowdownRatio() const

Obtains speed slowdown ratio for current polygon. Applicable for SLOWDOWN model.

Returns:

Speed slowdown ratio

double getTimeBeforeCollision() const

Obtains required time before collision for current polygon. Applicable for APPROACH model.

Returns:

Time before collision in seconds

virtual void getPolygon(std::vector<Point> &poly) const

Gets polygon points.

Parameters:

poly – Output polygon points (vertices)

void updatePolygon()

Updates polygon from footprint subscriber (if any)

virtual int getPointsInside(const std::vector<Point> &points) const

Gets number of points inside given polygon.

Parameters:

points – Input array of points to be checked

Returns:

Number of points inside polygon. If there are no points, returns zero value.

double getCollisionTime(const std::vector<Point> &collision_points, const Velocity &velocity) const

Obtains estimated (simulated) time before a collision. Applicable for APPROACH model.

Parameters:

  • collision_points – Array of 2D obstacle points

  • velocity – Simulated robot velocity

Returns:

Estimated time before a collision. If there is no collision, return value will be negative.

void publish() const

Publishes polygon message into a its own topic.

Protected Functions

bool getCommonParameters(std::string &polygon_pub_topic)

Supporting routine obtaining ROS-parameters common for all shapes.

Parameters:

polygon_pub_topic – Output name of polygon publishing topic

Returns:

True if all parameters were obtained or false in failure case

virtual bool getParameters(std::string &polygon_pub_topic, std::string &footprint_topic)

Supporting routine obtaining polygon-specific ROS-parameters.

Parameters:

  • polygon_pub_topic – Output name of polygon publishing topic

  • footprint_topic – Output name of footprint topic. Empty, if no footprint subscription

Returns:

True if all parameters were obtained or false in failure case

rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)

Checks if point is inside polygon.

Callback executed when a parameter change is detected

Parameters:

  • point – Given point to check

  • event – ParameterEvent message

Returns:

True if given point is inside polygon, otherwise false

bool isPointInside(const Point &point) const

Protected Attributes

nav2_util::LifecycleNode::WeakPtr node_

Collision Monitor node.

rclcpp::Logger logger_ = {rclcpp::get_logger("collision_monitor")}

Collision monitor node logger stored for further usage.

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_

Dynamic parameters handler.

std::string polygon_name_

Name of polygon.

ActionType action_type_

Action type for the polygon.

int max_points_

Maximum number of data readings within a zone to not trigger the action.

double slowdown_ratio_

Robot slowdown (share of its actual speed)

double time_before_collision_

Time before collision in seconds.

double simulation_time_step_

Time step for robot movement simulation.

std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_

Footprint subscriber.

bool enabled_

Whether polygon is enabled.

std::shared_ptr<tf2_ros::Buffer> tf_buffer_

TF buffer.

std::string base_frame_id_

Base frame ID.

tf2::Duration transform_tolerance_

Transform tolerance.

bool visualize_

Whether to publish the polygon.

geometry_msgs::msg::Polygon polygon_

Polygon points stored for later publishing.

rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_pub_

Polygon publisher for visualization purposes.

std::vector<Point> poly_

Polygon points (vertices)