-
Notifications
You must be signed in to change notification settings - Fork 1.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
nav2_collision_monitor: collision detector (#3500)
- Loading branch information
Tony Najjar
authored
Aug 14, 2023
1 parent
9367e54
commit f1f968b
Showing
17 changed files
with
1,493 additions
and
33 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
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
159 changes: 159 additions & 0 deletions
159
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,159 @@ | ||
| // 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 "nav2_collision_monitor/types.hpp" | ||
| #include "nav2_collision_monitor/polygon.hpp" | ||
| #include "nav2_collision_monitor/circle.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 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
Oops, something went wrong.