Skip to content

Commit a3ed745

Browse files
kaichieSteveMacenski
kaichie
and
SteveMacenski
committed
Add velocity based polygon (ros-navigation#3708)
* 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 <[email protected]> * update review comments and description Signed-off-by: nelson <[email protected]> * add VelocityPolygon to detector node Signed-off-by: nelson <[email protected]> * review update Signed-off-by: nelson <[email protected]> * fix cpplint Signed-off-by: nelson <[email protected]> * Update nav2_collision_monitor/src/velocity_polygon.cpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: nelson <[email protected]> * add velocity polygon tests Signed-off-by: nelson <[email protected]> * fix cpplint Signed-off-by: nelson <[email protected]> * add in-line comment Signed-off-by: nelson <[email protected]> * fix push back Signed-off-by: nelson <[email protected]> * minor change and update README Signed-off-by: nelson <[email protected]> * update README Signed-off-by: nelson <[email protected]> --------- Signed-off-by: nelson <[email protected]> Co-authored-by: Steve Macenski <[email protected]>
1 parent 4c3174e commit a3ed745

16 files changed

+1092
-10
lines changed

‎nav2_collision_monitor/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,7 @@ set(detector_library_name ${detector_executable_name}_core)
4949
add_library(${monitor_library_name} SHARED
5050
src/collision_monitor_node.cpp
5151
src/polygon.cpp
52+
src/velocity_polygon.cpp
5253
src/circle.cpp
5354
src/source.cpp
5455
src/scan.cpp
@@ -59,6 +60,7 @@ add_library(${monitor_library_name} SHARED
5960
add_library(${detector_library_name} SHARED
6061
src/collision_detector_node.cpp
6162
src/polygon.cpp
63+
src/velocity_polygon.cpp
6264
src/circle.cpp
6365
src/source.cpp
6466
src/scan.cpp

‎nav2_collision_monitor/README.md‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,8 @@ The zones around the robot can take the following shapes:
3636
* 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.
3737
* 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.
3838
* 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.
39+
* 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).
40+
3941

4042
The data may be obtained from different data sources:
4143

@@ -48,8 +50,13 @@ The data may be obtained from different data sources:
4850
The Collision Monitor is designed to operate below Nav2 as an independent safety node.
4951
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.
5052

51-
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.
53+
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.
5254
![HLD.png](doc/HLD.png)
55+
56+
`VelocityPolygon` can be configured with multiple sub polygons and can switch between them based on the velocity.
57+
![dexory_velocity_polygon.gif](doc/dexory_velocity_polygon.gif)
58+
59+
5360
### Configuration
5461

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

‎nav2_collision_monitor/doc/dexory_velocity_polygon.gif‎

7.85 MB
Loading

‎nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@
3333
#include "nav2_collision_monitor/types.hpp"
3434
#include "nav2_collision_monitor/polygon.hpp"
3535
#include "nav2_collision_monitor/circle.hpp"
36+
#include "nav2_collision_monitor/velocity_polygon.hpp"
3637
#include "nav2_collision_monitor/source.hpp"
3738
#include "nav2_collision_monitor/scan.hpp"
3839
#include "nav2_collision_monitor/pointcloud.hpp"

‎nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@
3333
#include "nav2_collision_monitor/types.hpp"
3434
#include "nav2_collision_monitor/polygon.hpp"
3535
#include "nav2_collision_monitor/circle.hpp"
36+
#include "nav2_collision_monitor/velocity_polygon.hpp"
3637
#include "nav2_collision_monitor/source.hpp"
3738
#include "nav2_collision_monitor/scan.hpp"
3839
#include "nav2_collision_monitor/pointcloud.hpp"

‎nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -135,7 +135,7 @@ class Polygon
135135
/**
136136
* @brief Updates polygon from footprint subscriber (if any)
137137
*/
138-
void updatePolygon();
138+
virtual void updatePolygon(const Velocity & /*cmd_vel_in*/);
139139

140140
/**
141141
* @brief Gets number of points inside given polygon

‎nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp‎

Lines changed: 115 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
// Copyright (c) 2023 Dexory
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_
16+
#define NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_
17+
18+
#include <memory>
19+
#include <string>
20+
#include <vector>
21+
22+
#include "geometry_msgs/msg/polygon_stamped.hpp"
23+
#include "nav2_collision_monitor/polygon.hpp"
24+
#include "nav2_collision_monitor/types.hpp"
25+
#include "nav2_util/lifecycle_node.hpp"
26+
#include "rclcpp/rclcpp.hpp"
27+
#include "tf2_ros/buffer.h"
28+
29+
namespace nav2_collision_monitor
30+
{
31+
32+
/**
33+
* @brief Velocity polygon class.
34+
* This class contains all the points of the polygon and
35+
* the expected condition of the velocity based polygon.
36+
*/
37+
class VelocityPolygon : public Polygon
38+
{
39+
public:
40+
/**
41+
* @brief VelocityPolygon constructor
42+
* @param node Collision Monitor node pointer
43+
* @param polygon_name Name of main polygon
44+
*/
45+
VelocityPolygon(
46+
const nav2_util::LifecycleNode::WeakPtr & node, const std::string & polygon_name,
47+
const std::shared_ptr<tf2_ros::Buffer> tf_buffer, const std::string & base_frame_id,
48+
const tf2::Duration & transform_tolerance);
49+
/**
50+
* @brief VelocityPolygon destructor
51+
*/
52+
virtual ~VelocityPolygon();
53+
54+
/**
55+
* @brief Overriden getParameters function for VelocityPolygon parameters
56+
* @param polygon_sub_topic Not used in VelocityPolygon
57+
* @param polygon_pub_topic Output name of polygon publishing topic
58+
* @param footprint_topic Not used in VelocityPolygon
59+
* @return True if all parameters were obtained or false in failure case
60+
*/
61+
bool getParameters(
62+
std::string & /*polygon_sub_topic*/, std::string & polygon_pub_topic,
63+
std::string & /*footprint_topic*/) override;
64+
65+
/**
66+
* @brief Overriden updatePolygon function for VelocityPolygon
67+
* @param cmd_vel_in Robot twist command input
68+
*/
69+
void updatePolygon(const Velocity & cmd_vel_in) override;
70+
71+
protected:
72+
/**
73+
* @brief Custom struc to store the parameters of the sub-polygon
74+
* @param poly_ The points of the sub-polygon
75+
* @param velocity_polygon_name_ The name of the sub-polygon
76+
* @param linear_min_ The minimum linear velocity
77+
* @param linear_max_ The maximum linear velocity
78+
* @param theta_min_ The minimum angular velocity
79+
* @param theta_max_ The maximum angular velocity
80+
* @param direction_end_angle_ The end angle of the direction(For holonomic robot only)
81+
* @param direction_start_angle_ The start angle of the direction(For holonomic robot only)
82+
*/
83+
struct SubPolygonParameter
84+
{
85+
std::vector<Point> poly_;
86+
std::string velocity_polygon_name_;
87+
double linear_min_;
88+
double linear_max_;
89+
double theta_min_;
90+
double theta_max_;
91+
double direction_end_angle_;
92+
double direction_start_angle_;
93+
};
94+
95+
/**
96+
* @brief Check if the velocities and direction is in expected range.
97+
* @param cmd_vel_in Robot twist command input
98+
* @param sub_polygon_param Sub polygon parameters
99+
* @return True if speed and direction is within the condition
100+
*/
101+
bool isInRange(const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon_param);
102+
103+
// Clock
104+
rclcpp::Clock::SharedPtr clock_;
105+
106+
// Variables
107+
/// @brief Flag to indicate if the robot is holonomic
108+
bool holonomic_;
109+
/// @brief Vector to store the parameters of the sub-polygon
110+
std::vector<SubPolygonParameter> sub_polygons_;
111+
}; // class VelocityPolygon
112+
113+
} // namespace nav2_collision_monitor
114+
115+
#endif // NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_

‎nav2_collision_monitor/params/collision_monitor_params.yaml‎

Lines changed: 39 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -11,9 +11,10 @@ collision_monitor:
1111
stop_pub_timeout: 2.0
1212
# Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
1313
# and robot footprint for "approach" action type.
14-
# Footprint could be "polygon" type with dynamically set footprint from footprint_topic
15-
# or "circle" type with static footprint set by radius. "footprint_topic" parameter
14+
# (1) Footprint could be "polygon" type with dynamically set footprint from footprint_topic
15+
# (2) "circle" type with static footprint set by radius. "footprint_topic" parameter
1616
# to be ignored in circular case.
17+
# (3) "velocity_polygon" type with dynamically set polygon from velocity_polygons
1718
polygons: ["PolygonStop"]
1819
PolygonStop:
1920
type: "polygon"
@@ -51,6 +52,42 @@ collision_monitor:
5152
min_points: 6
5253
visualize: False
5354
enabled: True
55+
VelocityPolygonStop:
56+
type: "velocity_polygon"
57+
action_type: "stop"
58+
min_points: 6
59+
visualize: True
60+
enabled: True
61+
polygon_pub_topic: "velocity_polygon_stop"
62+
velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stopped"]
63+
holonomic: false
64+
rotation:
65+
points: "[[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]"
66+
linear_min: 0.0
67+
linear_max: 0.05
68+
theta_min: -1.0
69+
theta_max: 1.0
70+
translation_forward:
71+
points: "[[0.35, 0.3], [0.35, -0.3], [-0.2, -0.3], [-0.2, 0.3]]"
72+
linear_min: 0.0
73+
linear_max: 1.0
74+
theta_min: -1.0
75+
theta_max: 1.0
76+
translation_backward:
77+
points: "[[0.2, 0.3], [0.2, -0.3], [-0.35, -0.3], [-0.35, 0.3]]"
78+
linear_min: -1.0
79+
linear_max: 0.0
80+
theta_min: -1.0
81+
theta_max: 1.0
82+
# This is the last polygon to be checked, it should cover the entire range of robot's velocities
83+
# It is used as the stopped polygon when the robot is not moving and as a fallback if the velocity
84+
# is not covered by any of the other sub-polygons
85+
stopped:
86+
points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]"
87+
linear_min: -1.0
88+
linear_max: 1.0
89+
theta_min: -1.0
90+
theta_max: 1.0
5491
observation_sources: ["scan"]
5592
scan:
5693
type: "scan"

‎nav2_collision_monitor/src/collision_detector_node.cpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -207,6 +207,10 @@ bool CollisionDetector::configurePolygons(
207207
polygons_.push_back(
208208
std::make_shared<Circle>(
209209
node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
210+
} else if (polygon_type == "velocity_polygon") {
211+
polygons_.push_back(
212+
std::make_shared<VelocityPolygon>(
213+
node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
210214
} else { // Error if something else
211215
RCLCPP_ERROR(
212216
get_logger(),

‎nav2_collision_monitor/src/collision_monitor_node.cpp‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -297,6 +297,10 @@ bool CollisionMonitor::configurePolygons(
297297
polygons_.push_back(
298298
std::make_shared<Circle>(
299299
node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
300+
} else if (polygon_type == "velocity_polygon") {
301+
polygons_.push_back(
302+
std::make_shared<VelocityPolygon>(
303+
node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
300304
} else { // Error if something else
301305
RCLCPP_ERROR(
302306
get_logger(),
@@ -440,7 +444,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
440444
}
441445

442446
// Update polygon coordinates
443-
polygon->updatePolygon();
447+
polygon->updatePolygon(cmd_vel_in);
444448

445449
const ActionType at = polygon->getActionType();
446450
if (at == STOP || at == SLOWDOWN || at == LIMIT) {

0 commit comments

Comments
 (0)