Skip to content

Commit

Permalink
Add velocity based polygon (#3708)
Browse files Browse the repository at this point in the history
* add velocity based polygon

* fix header, copyright and variable name change

* optimise polygon update

* optimise duplicated code with setPolygonShape

* add warning log for uncovered speed

* update feedback

* rename polygon velocity to velocity polygon

* cleanup

* fix typo

* add dynamic support for velocity polygon

* wrap try catch for getting parameters

* update naming and linting

* use switch case

* Revert "use switch case"

This reverts commit 1230ede.

* fix proper return for invalid parameters

* remove topic parameter for velocity polygon

* fix formatting manually

* continue if points are not defined

* rewrite velocity polygon with polygon base class

Signed-off-by: nelson <kaichie.lee@gmail.com>

* update review comments and description

Signed-off-by: nelson <kaichie.lee@gmail.com>

* add VelocityPolygon to detector node

Signed-off-by: nelson <kaichie.lee@gmail.com>

* review update

Signed-off-by: nelson <kaichie.lee@gmail.com>

* fix cpplint

Signed-off-by: nelson <kaichie.lee@gmail.com>

* Update nav2_collision_monitor/src/velocity_polygon.cpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: nelson <kaichie.lee@gmail.com>

* add velocity polygon tests

Signed-off-by: nelson <kaichie.lee@gmail.com>

* fix cpplint

Signed-off-by: nelson <kaichie.lee@gmail.com>

* add in-line comment

Signed-off-by: nelson <kaichie.lee@gmail.com>

* fix push back

Signed-off-by: nelson <kaichie.lee@gmail.com>

* minor change and update README

Signed-off-by: nelson <kaichie.lee@gmail.com>

* update README

Signed-off-by: nelson <kaichie.lee@gmail.com>

---------

Signed-off-by: nelson <kaichie.lee@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
  • Loading branch information
kaichie and SteveMacenski committed Apr 4, 2024
1 parent 4c3174e commit a3ed745
Show file tree
Hide file tree
Showing 16 changed files with 1,092 additions and 10 deletions.
2 changes: 2 additions & 0 deletions nav2_collision_monitor/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ set(detector_library_name ${detector_executable_name}_core)
add_library(${monitor_library_name} SHARED
src/collision_monitor_node.cpp
src/polygon.cpp
src/velocity_polygon.cpp
src/circle.cpp
src/source.cpp
src/scan.cpp
Expand All @@ -59,6 +60,7 @@ add_library(${monitor_library_name} SHARED
add_library(${detector_library_name} SHARED
src/collision_detector_node.cpp
src/polygon.cpp
src/velocity_polygon.cpp
src/circle.cpp
src/source.cpp
src/scan.cpp
Expand Down
9 changes: 8 additions & 1 deletion nav2_collision_monitor/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,8 @@ The zones around the robot can take the following shapes:
* Arbitrary user-defined polygon relative to the robot base frame, which can be static in a configuration file or dynamically changing via a topic interface.
* Robot footprint polygon, which is used in the approach behavior model only. Will use the static user-defined polygon or the footprint topic to allow it to be dynamically adjusted over time.
* Circle: is made for the best performance and could be used in the cases where the zone or robot footprint could be approximated by round shape.
* VelocityPolygon: allow switching of polygons based on the command velocity. When the velocity is covered by multiple sub polygons, the first sub polygon in the `velocity_polygons` list will be used. This is useful for robots to set different safety zones based on their velocity (e.g. a robot that has a larger safety zone when moving at 1.0 m/s than when moving at 0.5 m/s).


The data may be obtained from different data sources:

Expand All @@ -48,8 +50,13 @@ The data may be obtained from different data sources:
The Collision Monitor is designed to operate below Nav2 as an independent safety node.
This acts as a filter on the `cmd_vel` topic coming out of the Controller Server. If no such zone is triggered, then the Controller's `cmd_vel` is used. Else, it is scaled or set to stop as appropriate.

The following diagram is showing the high-level design of Collision Monitor module. All shapes (Polygons and Circles) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model.
The following diagram is showing the high-level design of Collision Monitor module. All shapes (`Polygon`, `Circle` and `VelocityPolygon`) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model.
![HLD.png](doc/HLD.png)

`VelocityPolygon` can be configured with multiple sub polygons and can switch between them based on the velocity.
![dexory_velocity_polygon.gif](doc/dexory_velocity_polygon.gif)


### Configuration

Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://navigation.ros.org/tutorials/docs/using_collision_monitor.html) pages.
Expand Down
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
#include "nav2_collision_monitor/types.hpp"
#include "nav2_collision_monitor/polygon.hpp"
#include "nav2_collision_monitor/circle.hpp"
#include "nav2_collision_monitor/velocity_polygon.hpp"
#include "nav2_collision_monitor/source.hpp"
#include "nav2_collision_monitor/scan.hpp"
#include "nav2_collision_monitor/pointcloud.hpp"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
#include "nav2_collision_monitor/types.hpp"
#include "nav2_collision_monitor/polygon.hpp"
#include "nav2_collision_monitor/circle.hpp"
#include "nav2_collision_monitor/velocity_polygon.hpp"
#include "nav2_collision_monitor/source.hpp"
#include "nav2_collision_monitor/scan.hpp"
#include "nav2_collision_monitor/pointcloud.hpp"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -135,7 +135,7 @@ class Polygon
/**
* @brief Updates polygon from footprint subscriber (if any)
*/
void updatePolygon();
virtual void updatePolygon(const Velocity & /*cmd_vel_in*/);

/**
* @brief Gets number of points inside given polygon
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,115 @@
// Copyright (c) 2023 Dexory
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_
#define NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_

#include <memory>
#include <string>
#include <vector>

#include "geometry_msgs/msg/polygon_stamped.hpp"
#include "nav2_collision_monitor/polygon.hpp"
#include "nav2_collision_monitor/types.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"

namespace nav2_collision_monitor
{

/**
* @brief Velocity polygon class.
* This class contains all the points of the polygon and
* the expected condition of the velocity based polygon.
*/
class VelocityPolygon : public Polygon
{
public:
/**
* @brief VelocityPolygon constructor
* @param node Collision Monitor node pointer
* @param polygon_name Name of main polygon
*/
VelocityPolygon(
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);
/**
* @brief VelocityPolygon destructor
*/
virtual ~VelocityPolygon();

/**
* @brief Overriden getParameters function for VelocityPolygon parameters
* @param polygon_sub_topic Not used in VelocityPolygon
* @param polygon_pub_topic Output name of polygon publishing topic
* @param footprint_topic Not used in VelocityPolygon
* @return True if all parameters were obtained or false in failure case
*/
bool getParameters(
std::string & /*polygon_sub_topic*/, std::string & polygon_pub_topic,
std::string & /*footprint_topic*/) override;

/**
* @brief Overriden updatePolygon function for VelocityPolygon
* @param cmd_vel_in Robot twist command input
*/
void updatePolygon(const Velocity & cmd_vel_in) override;

protected:
/**
* @brief Custom struc to store the parameters of the sub-polygon
* @param poly_ The points of the sub-polygon
* @param velocity_polygon_name_ The name of the sub-polygon
* @param linear_min_ The minimum linear velocity
* @param linear_max_ The maximum linear velocity
* @param theta_min_ The minimum angular velocity
* @param theta_max_ The maximum angular velocity
* @param direction_end_angle_ The end angle of the direction(For holonomic robot only)
* @param direction_start_angle_ The start angle of the direction(For holonomic robot only)
*/
struct SubPolygonParameter
{
std::vector<Point> poly_;
std::string velocity_polygon_name_;
double linear_min_;
double linear_max_;
double theta_min_;
double theta_max_;
double direction_end_angle_;
double direction_start_angle_;
};

/**
* @brief Check if the velocities and direction is in expected range.
* @param cmd_vel_in Robot twist command input
* @param sub_polygon_param Sub polygon parameters
* @return True if speed and direction is within the condition
*/
bool isInRange(const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon_param);

// Clock
rclcpp::Clock::SharedPtr clock_;

// Variables
/// @brief Flag to indicate if the robot is holonomic
bool holonomic_;
/// @brief Vector to store the parameters of the sub-polygon
std::vector<SubPolygonParameter> sub_polygons_;
}; // class VelocityPolygon

} // namespace nav2_collision_monitor

#endif // NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_
41 changes: 39 additions & 2 deletions nav2_collision_monitor/params/collision_monitor_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,10 @@ collision_monitor:
stop_pub_timeout: 2.0
# Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
# and robot footprint for "approach" action type.
# Footprint could be "polygon" type with dynamically set footprint from footprint_topic
# or "circle" type with static footprint set by radius. "footprint_topic" parameter
# (1) Footprint could be "polygon" type with dynamically set footprint from footprint_topic
# (2) "circle" type with static footprint set by radius. "footprint_topic" parameter
# to be ignored in circular case.
# (3) "velocity_polygon" type with dynamically set polygon from velocity_polygons
polygons: ["PolygonStop"]
PolygonStop:
type: "polygon"
Expand Down Expand Up @@ -51,6 +52,42 @@ collision_monitor:
min_points: 6
visualize: False
enabled: True
VelocityPolygonStop:
type: "velocity_polygon"
action_type: "stop"
min_points: 6
visualize: True
enabled: True
polygon_pub_topic: "velocity_polygon_stop"
velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stopped"]
holonomic: false
rotation:
points: "[[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]"
linear_min: 0.0
linear_max: 0.05
theta_min: -1.0
theta_max: 1.0
translation_forward:
points: "[[0.35, 0.3], [0.35, -0.3], [-0.2, -0.3], [-0.2, 0.3]]"
linear_min: 0.0
linear_max: 1.0
theta_min: -1.0
theta_max: 1.0
translation_backward:
points: "[[0.2, 0.3], [0.2, -0.3], [-0.35, -0.3], [-0.35, 0.3]]"
linear_min: -1.0
linear_max: 0.0
theta_min: -1.0
theta_max: 1.0
# This is the last polygon to be checked, it should cover the entire range of robot's velocities
# It is used as the stopped polygon when the robot is not moving and as a fallback if the velocity
# is not covered by any of the other sub-polygons
stopped:
points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]"
linear_min: -1.0
linear_max: 1.0
theta_min: -1.0
theta_max: 1.0
observation_sources: ["scan"]
scan:
type: "scan"
Expand Down
4 changes: 4 additions & 0 deletions nav2_collision_monitor/src/collision_detector_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -207,6 +207,10 @@ bool CollisionDetector::configurePolygons(
polygons_.push_back(
std::make_shared<Circle>(
node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
} else if (polygon_type == "velocity_polygon") {
polygons_.push_back(
std::make_shared<VelocityPolygon>(
node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
} else { // Error if something else
RCLCPP_ERROR(
get_logger(),
Expand Down
6 changes: 5 additions & 1 deletion nav2_collision_monitor/src/collision_monitor_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -297,6 +297,10 @@ bool CollisionMonitor::configurePolygons(
polygons_.push_back(
std::make_shared<Circle>(
node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
} else if (polygon_type == "velocity_polygon") {
polygons_.push_back(
std::make_shared<VelocityPolygon>(
node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
} else { // Error if something else
RCLCPP_ERROR(
get_logger(),
Expand Down Expand Up @@ -440,7 +444,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
}

// Update polygon coordinates
polygon->updatePolygon();
polygon->updatePolygon(cmd_vel_in);

const ActionType at = polygon->getActionType();
if (at == STOP || at == SLOWDOWN || at == LIMIT) {
Expand Down
2 changes: 1 addition & 1 deletion nav2_collision_monitor/src/polygon.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -181,7 +181,7 @@ bool Polygon::isShapeSet()
return true;
}

void Polygon::updatePolygon()
void Polygon::updatePolygon(const Velocity & /*cmd_vel_in*/)
{
if (footprint_sub_ != nullptr) {
// Get latest robot footprint from footprint subscriber
Expand Down
Loading

0 comments on commit a3ed745

Please sign in to comment.