Class Polygon

Inheritance Relationships

Derived Types

Class Documentation

class Polygon

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

Subclassed by nav2_collision_monitor::Circle, nav2_collision_monitor::VelocityPolygon

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 getMinPoints() const

Obtains polygon minimum points to enter inside polygon causing the action.

Returns:

Minimum number of data readings within a zone to trigger the action

double getSlowdownRatio() const

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

Returns:

Speed slowdown ratio

double getLinearLimit() const

Obtains speed linear limit for current polygon. Applicable for LIMIT model.

Returns:

Speed linear limit

double getAngularLimit() const

Obtains speed angular z limit for current polygon. Applicable for LIMIT model.

Returns:

Speed angular limit

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)

std::vector<std::string> getSourcesNames() const

Obtains the name of the observation sources for current polygon.

Returns:

Names of the observation sources

virtual bool isShapeSet()

Returns true if polygon points were set. Otherwise, prints a warning and returns false.

virtual void updatePolygon(const Velocity&)

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.

virtual int getPointsInside(const std::unordered_map<std::string, std::vector<Point>> &sources_collision_points_map) const

Gets number of points inside given polygon.

Parameters:

sources_collision_points_map – Map containing source name as key, and input array of source’s points to be checked as value

Returns:

Number of points inside polygon, for sources in map that are associated with current polygon. If there are no points, returns zero value.

double getCollisionTime(const std::unordered_map<std::string, std::vector<Point>> &sources_collision_points_map, const Velocity &velocity) const

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

Parameters:

  • sources_collision_points_map – Map containing source name as key, and input array of source’s 2D obstacle points as value

  • velocity – Simulated robot velocity

Returns:

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

void publish()

Publishes polygon message into a its own topic.

Protected Functions

bool getCommonParameters(std::string &polygon_sub_topic, std::string &polygon_pub_topic, std::string &footprint_topic, bool use_dynamic_sub = false)

Supporting routine obtaining ROS-parameters common for all shapes.

Parameters:

  • polygon_pub_topic – Output name of polygon or radius subscription topic. Empty, if no polygon subscription.

  • polygon_sub_topic – Output name of polygon publishing topic

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

  • use_dynamic_sub – If false, the parameter polygon_sub_topic or footprint_topic will not be declared

Returns:

True if all parameters were obtained or false in failure case

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

Supporting routine obtaining polygon-specific ROS-parameters.

Parameters:

  • polygon_sub_topic – Output name of polygon or radius subscription topic. Empty, if no polygon subscription.

  • 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

virtual void createSubscription(std::string &polygon_sub_topic)

Creates polygon or radius topic subscription.

Parameters:

polygon_sub_topic – Output name of polygon or radius subscription topic. Empty, if no polygon subscription.

void updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg)

Updates polygon from geometry_msgs::msg::PolygonStamped message.

Parameters:

msg – Message to update polygon from

void polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg)

Dynamic polygon callback.

Parameters:

msg – Shared pointer to the polygon message

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

Callback executed when a parameter change is detected.

Parameters:

event – ParameterEvent message

bool isPointInside(const Point &point) const

Checks if point is inside polygon.

Parameters:

point – Given point to check

Returns:

True if given point is inside polygon, otherwise false

bool getPolygonFromString(std::string &poly_string, std::vector<Point> &polygon)

Extracts Polygon points from a string with of the form [[x1,y1],[x2,y2],[x3,y3]…].

Parameters:

  • poly_string – Input String containing the verteceis of the polygon

  • polygon – Output Point vector with all the vertices of the polygon

Returns:

True if all parameters were obtained or false in failure case

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 min_points_

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

double slowdown_ratio_

Robot slowdown (share of its actual speed)

double linear_limit_

Robot linear limit.

double angular_limit_

Robot angular limit.

double time_before_collision_

Time before collision in seconds.

double simulation_time_step_

Time step for robot movement simulation.

bool enabled_

Whether polygon is enabled.

bool polygon_subscribe_transient_local_

Whether the subscription to polygon topic has transient local QoS durability.

rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_sub_

Polygon subscription.

std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_

Footprint subscriber.

std::vector<std::string> sources_names_

Name of the observation sources to check for polygon.

std::shared_ptr<tf2_ros::Buffer> tf_buffer_

TF buffer.

std::string base_frame_id_

Base frame ID.

tf2::Duration transform_tolerance_

Transform tolerance.

rclcpp::Clock::SharedPtr node_clock_

Collision monitor node’s clock.

bool visualize_

Whether to publish the polygon.

geometry_msgs::msg::PolygonStamped polygon_

Polygon, used for: 1. visualization; 2. storing latest dynamic polygon message.

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

Polygon publisher for visualization purposes.

std::vector<Point> poly_

Polygon points (vertices) in a base_frame_id_.