forked from ros-navigation/navigation2
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
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]>
- Loading branch information
Showing
16 changed files
with
1,770 additions
and
15 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
164 changes: 164 additions & 0 deletions
164
nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,164 @@ | ||
| // Copyright (c) 2022 Samsung R&D Institute Russia | ||
| // Copyright (c) 2023 Pixel Robotics GmbH | ||
| // | ||
| // 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__COLLISION_DETECTOR_NODE_HPP_ | ||
| #define NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_ | ||
|
|
||
| #include <string> | ||
| #include <vector> | ||
| #include <memory> | ||
|
|
||
| #include "rclcpp/rclcpp.hpp" | ||
|
|
||
| #include "tf2/time.h" | ||
| #include "tf2_ros/buffer.h" | ||
| #include "tf2_ros/transform_listener.h" | ||
|
|
||
| #include "nav2_util/lifecycle_node.hpp" | ||
| #include "nav2_msgs/msg/collision_detector_state.hpp" | ||
| #include "visualization_msgs/msg/marker_array.hpp" | ||
|
|
||
| #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" | ||
| #include "nav2_collision_monitor/range.hpp" | ||
|
|
||
| namespace nav2_collision_monitor | ||
| { | ||
|
|
||
| /** | ||
| * @brief Collision Monitor ROS2 node | ||
| */ | ||
| class CollisionDetector : public nav2_util::LifecycleNode | ||
| { | ||
| public: | ||
| /** | ||
| * @brief Constructor for the nav2_collision_monitor::CollisionDetector | ||
| * @param options Additional options to control creation of the node. | ||
| */ | ||
| explicit CollisionDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); | ||
| /** | ||
| * @brief Destructor for the nav2_collision_monitor::CollisionDetector | ||
| */ | ||
| ~CollisionDetector(); | ||
|
|
||
| protected: | ||
| /** | ||
| * @brief: Initializes and obtains ROS-parameters, creates main subscribers and publishers, | ||
| * creates polygons and data sources objects | ||
| * @param state Lifecycle Node's state | ||
| * @return Success or Failure | ||
| */ | ||
| nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; | ||
| /** | ||
| * @brief: Activates LifecyclePublishers, polygons and main processor, creates bond connection | ||
| * @param state Lifecycle Node's state | ||
| * @return Success or Failure | ||
| */ | ||
| nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; | ||
| /** | ||
| * @brief: Deactivates LifecyclePublishers, polygons and main processor, destroys bond connection | ||
| * @param state Lifecycle Node's state | ||
| * @return Success or Failure | ||
| */ | ||
| nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; | ||
| /** | ||
| * @brief: Resets all subscribers/publishers, polygons/data sources arrays | ||
| * @param state Lifecycle Node's state | ||
| * @return Success or Failure | ||
| */ | ||
| nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; | ||
| /** | ||
| * @brief Called in shutdown state | ||
| * @param state Lifecycle Node's state | ||
| * @return Success or Failure | ||
| */ | ||
| nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; | ||
|
|
||
| protected: | ||
| /** | ||
| * @brief Supporting routine obtaining all ROS-parameters | ||
| * @return True if all parameters were obtained or false in failure case | ||
| */ | ||
| bool getParameters(); | ||
| /** | ||
| * @brief Supporting routine creating and configuring all polygons | ||
| * @param base_frame_id Robot base frame ID | ||
| * @param transform_tolerance Transform tolerance | ||
| * @return True if all polygons were configured successfully or false in failure case | ||
| */ | ||
| bool configurePolygons( | ||
| const std::string & base_frame_id, | ||
| const tf2::Duration & transform_tolerance); | ||
| /** | ||
| * @brief Supporting routine creating and configuring all data sources | ||
| * @param base_frame_id Robot base frame ID | ||
| * @param odom_frame_id Odometry frame ID. Used as global frame to get | ||
| * source->base time interpolated transform. | ||
| * @param transform_tolerance Transform tolerance | ||
| * @param source_timeout Maximum time interval in which data is considered valid | ||
| * @param base_shift_correction Whether to correct source data towards to base frame movement, | ||
| * considering the difference between current time and latest source time | ||
| * @return True if all sources were configured successfully or false in failure case | ||
| */ | ||
| bool configureSources( | ||
| const std::string & base_frame_id, | ||
| const std::string & odom_frame_id, | ||
| const tf2::Duration & transform_tolerance, | ||
| const rclcpp::Duration & source_timeout, | ||
| const bool base_shift_correction); | ||
|
|
||
| /** | ||
| * @brief Main processing routine | ||
| */ | ||
| void process(); | ||
|
|
||
| /** | ||
| * @brief Polygons publishing routine. Made for visualization. | ||
| */ | ||
| void publishPolygons() const; | ||
|
|
||
| // ----- Variables ----- | ||
|
|
||
| /// @brief TF buffer | ||
| std::shared_ptr<tf2_ros::Buffer> tf_buffer_; | ||
| /// @brief TF listener | ||
| std::shared_ptr<tf2_ros::TransformListener> tf_listener_; | ||
|
|
||
| /// @brief Polygons array | ||
| std::vector<std::shared_ptr<Polygon>> polygons_; | ||
| /// @brief Data sources array | ||
| std::vector<std::shared_ptr<Source>> sources_; | ||
|
|
||
| /// @brief collision monitor state publisher | ||
| rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionDetectorState>::SharedPtr | ||
| state_pub_; | ||
| /// @brief Collision points marker publisher | ||
| rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr | ||
| collision_points_marker_pub_; | ||
| /// @brief timer that runs actions | ||
| rclcpp::TimerBase::SharedPtr timer_; | ||
|
|
||
| /// @brief main loop frequency | ||
| double frequency_; | ||
| }; // class CollisionDetector | ||
|
|
||
| } // namespace nav2_collision_monitor | ||
|
|
||
| #endif // NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
115 changes: 115 additions & 0 deletions
115
nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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_ |
Oops, something went wrong.