Skip to content

Commit 779c163

Browse files
kaichieSteveMacenski
authored and
cheriehu
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 03e081c commit 779c163

16 files changed

+1770
-15
lines changed

‎nav2_collision_monitor/CMakeLists.txt‎

Lines changed: 17 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,18 @@ set(library_name ${executable_name}_core)
4545
add_library(${library_name} SHARED
4646
src/collision_monitor_node.cpp
4747
src/polygon.cpp
48+
src/velocity_polygon.cpp
49+
src/circle.cpp
50+
src/source.cpp
51+
src/scan.cpp
52+
src/pointcloud.cpp
53+
src/range.cpp
54+
src/kinematics.cpp
55+
)
56+
add_library(${detector_library_name} SHARED
57+
src/collision_detector_node.cpp
58+
src/polygon.cpp
59+
src/velocity_polygon.cpp
4860
src/circle.cpp
4961
src/source.cpp
5062
src/scan.cpp
@@ -53,8 +65,11 @@ add_library(${library_name} SHARED
5365
src/kinematics.cpp
5466
)
5567

56-
add_executable(${executable_name}
57-
src/main.cpp
68+
add_executable(${monitor_executable_name}
69+
src/collision_monitor_main.cpp
70+
)
71+
add_executable(${detector_executable_name}
72+
src/collision_detector_main.cpp
5873
)
5974

6075
ament_target_dependencies(${library_name}

‎nav2_collision_monitor/README.md‎

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -27,9 +27,11 @@ The following models of safety behaviors are employed by Collision Monitor:
2727

2828
The zones around the robot can take the following shapes:
2929

30-
* Arbitrary user-defined polygon relative to the robot base frame.
31-
* Circle: is made for the best performance and could be used in the cases where the zone or robot could be approximated by round shape.
32-
* Robot footprint polygon, which is used in the approach behavior model only. Will use the footprint topic to allow it to be dynamically adjusted over time.
30+
* 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.
31+
* 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.
32+
* 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.
33+
* 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).
34+
3335

3436
The data may be obtained from different data sources:
3537

@@ -42,10 +44,14 @@ The data may be obtained from different data sources:
4244
The Collision Monitor is designed to operate below Nav2 as an independent safety node.
4345
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.
4446

45-
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.
46-
![HDL.png](doc/HLD.png)
47+
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.
48+
![HLD.png](doc/HLD.png)
49+
50+
`VelocityPolygon` can be configured with multiple sub polygons and can switch between them based on the velocity.
51+
![dexory_velocity_polygon.gif](doc/dexory_velocity_polygon.gif)
52+
4753

48-
## Configuration
54+
### Configuration
4955

5056
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.
5157

‎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: 164 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,164 @@
1+
// Copyright (c) 2022 Samsung R&D Institute Russia
2+
// Copyright (c) 2023 Pixel Robotics GmbH
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#ifndef NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_
17+
#define NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_
18+
19+
#include <string>
20+
#include <vector>
21+
#include <memory>
22+
23+
#include "rclcpp/rclcpp.hpp"
24+
25+
#include "tf2/time.h"
26+
#include "tf2_ros/buffer.h"
27+
#include "tf2_ros/transform_listener.h"
28+
29+
#include "nav2_util/lifecycle_node.hpp"
30+
#include "nav2_msgs/msg/collision_detector_state.hpp"
31+
#include "visualization_msgs/msg/marker_array.hpp"
32+
33+
#include "nav2_collision_monitor/types.hpp"
34+
#include "nav2_collision_monitor/polygon.hpp"
35+
#include "nav2_collision_monitor/circle.hpp"
36+
#include "nav2_collision_monitor/velocity_polygon.hpp"
37+
#include "nav2_collision_monitor/source.hpp"
38+
#include "nav2_collision_monitor/scan.hpp"
39+
#include "nav2_collision_monitor/pointcloud.hpp"
40+
#include "nav2_collision_monitor/range.hpp"
41+
42+
namespace nav2_collision_monitor
43+
{
44+
45+
/**
46+
* @brief Collision Monitor ROS2 node
47+
*/
48+
class CollisionDetector : public nav2_util::LifecycleNode
49+
{
50+
public:
51+
/**
52+
* @brief Constructor for the nav2_collision_monitor::CollisionDetector
53+
* @param options Additional options to control creation of the node.
54+
*/
55+
explicit CollisionDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
56+
/**
57+
* @brief Destructor for the nav2_collision_monitor::CollisionDetector
58+
*/
59+
~CollisionDetector();
60+
61+
protected:
62+
/**
63+
* @brief: Initializes and obtains ROS-parameters, creates main subscribers and publishers,
64+
* creates polygons and data sources objects
65+
* @param state Lifecycle Node's state
66+
* @return Success or Failure
67+
*/
68+
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
69+
/**
70+
* @brief: Activates LifecyclePublishers, polygons and main processor, creates bond connection
71+
* @param state Lifecycle Node's state
72+
* @return Success or Failure
73+
*/
74+
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
75+
/**
76+
* @brief: Deactivates LifecyclePublishers, polygons and main processor, destroys bond connection
77+
* @param state Lifecycle Node's state
78+
* @return Success or Failure
79+
*/
80+
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
81+
/**
82+
* @brief: Resets all subscribers/publishers, polygons/data sources arrays
83+
* @param state Lifecycle Node's state
84+
* @return Success or Failure
85+
*/
86+
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
87+
/**
88+
* @brief Called in shutdown state
89+
* @param state Lifecycle Node's state
90+
* @return Success or Failure
91+
*/
92+
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
93+
94+
protected:
95+
/**
96+
* @brief Supporting routine obtaining all ROS-parameters
97+
* @return True if all parameters were obtained or false in failure case
98+
*/
99+
bool getParameters();
100+
/**
101+
* @brief Supporting routine creating and configuring all polygons
102+
* @param base_frame_id Robot base frame ID
103+
* @param transform_tolerance Transform tolerance
104+
* @return True if all polygons were configured successfully or false in failure case
105+
*/
106+
bool configurePolygons(
107+
const std::string & base_frame_id,
108+
const tf2::Duration & transform_tolerance);
109+
/**
110+
* @brief Supporting routine creating and configuring all data sources
111+
* @param base_frame_id Robot base frame ID
112+
* @param odom_frame_id Odometry frame ID. Used as global frame to get
113+
* source->base time interpolated transform.
114+
* @param transform_tolerance Transform tolerance
115+
* @param source_timeout Maximum time interval in which data is considered valid
116+
* @param base_shift_correction Whether to correct source data towards to base frame movement,
117+
* considering the difference between current time and latest source time
118+
* @return True if all sources were configured successfully or false in failure case
119+
*/
120+
bool configureSources(
121+
const std::string & base_frame_id,
122+
const std::string & odom_frame_id,
123+
const tf2::Duration & transform_tolerance,
124+
const rclcpp::Duration & source_timeout,
125+
const bool base_shift_correction);
126+
127+
/**
128+
* @brief Main processing routine
129+
*/
130+
void process();
131+
132+
/**
133+
* @brief Polygons publishing routine. Made for visualization.
134+
*/
135+
void publishPolygons() const;
136+
137+
// ----- Variables -----
138+
139+
/// @brief TF buffer
140+
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
141+
/// @brief TF listener
142+
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
143+
144+
/// @brief Polygons array
145+
std::vector<std::shared_ptr<Polygon>> polygons_;
146+
/// @brief Data sources array
147+
std::vector<std::shared_ptr<Source>> sources_;
148+
149+
/// @brief collision monitor state publisher
150+
rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionDetectorState>::SharedPtr
151+
state_pub_;
152+
/// @brief Collision points marker publisher
153+
rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
154+
collision_points_marker_pub_;
155+
/// @brief timer that runs actions
156+
rclcpp::TimerBase::SharedPtr timer_;
157+
158+
/// @brief main loop frequency
159+
double frequency_;
160+
}; // class CollisionDetector
161+
162+
} // namespace nav2_collision_monitor
163+
164+
#endif // NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_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
@@ -32,6 +32,7 @@
3232
#include "nav2_collision_monitor/types.hpp"
3333
#include "nav2_collision_monitor/polygon.hpp"
3434
#include "nav2_collision_monitor/circle.hpp"
35+
#include "nav2_collision_monitor/velocity_polygon.hpp"
3536
#include "nav2_collision_monitor/source.hpp"
3637
#include "nav2_collision_monitor/scan.hpp"
3738
#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
@@ -118,7 +118,7 @@ class Polygon
118118
/**
119119
* @brief Updates polygon from footprint subscriber (if any)
120120
*/
121-
void updatePolygon();
121+
virtual void updatePolygon(const Velocity & /*cmd_vel_in*/);
122122

123123
/**
124124
* @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_

0 commit comments

Comments
 (0)