From 8cf1e2e9cce7261c1e49b0ec30407b0ca580fa37 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 8 Apr 2024 14:36:50 +0200 Subject: [PATCH] Publish navigation events --- mrpt_reactivenav2d/README.md | 12 +++ .../mrpt_reactivenav2d_node.hpp | 42 ++++++++- .../src/mrpt_reactivenav2d_node.cpp | 87 +++++++++++++++---- 3 files changed, 122 insertions(+), 19 deletions(-) diff --git a/mrpt_reactivenav2d/README.md b/mrpt_reactivenav2d/README.md index 17c8dec..a4547ed 100644 --- a/mrpt_reactivenav2d/README.md +++ b/mrpt_reactivenav2d/README.md @@ -97,6 +97,18 @@ XXX ### Published topics * xxx +* `reactivenav_events` (`std_msgs/String`): One message with a string keyword will be published for each important navigation event. + The list possible message strings are: + * `START`: Start of navigation + * `END`: Successful end of navigation command (reach of single goal, or final waypoint of waypoint list). + * `WP_REACHED (REACHED|SKIPPED)`: Reached an intermediary waypoint in waypoint list navigation. The waypoint may have been physically reached or just skipped. + * `WP_NEW `: Heading towards a new intermediary/final waypoint in waypoint list navigation. + * `ERROR`: Error asking sensory data from robot or sending motor commands. + * `WAY_SEEMS_BLOCKED`: No progression made towards target for a predefined period of time. + * `APPARENT_COLLISION`: Apparent collision event (i.e. there is at least one obstacle point inside the robot shape). + * `CANNOT_GET_CLOSER`: Target seems to be blocked by an obstacle. + + ### Template ROS 2 launch files This package provides [launch/reactivenav.launch.py](launch/reactivenav.launch.py): diff --git a/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp b/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp index 8605637..dbaa0af 100644 --- a/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp +++ b/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -85,6 +86,8 @@ class ReactiveNav2DNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pubSelectedPtg_; + rclcpp::Publisher::SharedPtr pubNavEvents_; + std::shared_ptr tfBuffer_; std::shared_ptr tfListener_; /** @} */ @@ -102,6 +105,7 @@ class ReactiveNav2DNode : public rclcpp::Node std::string pubTopicCmdVel_ = "/cmd_vel"; std::string pubTopicSelectedPtg_ = "reactivenav_selected_ptg"; + std::string pubTopicEvents_ = "reactivenav_events"; std::string frameidReference_ = "map"; std::string frameidRobot_ = "base_link"; @@ -128,6 +132,8 @@ class ReactiveNav2DNode : public rclcpp::Node visualization_msgs::msg::MarkerArray log_to_margers( const mrpt::nav::CLogFileRecord& lr); + void publish_event_message(const std::string& text); + struct MyReactiveInterface : public mrpt::nav::CRobot2NavInterface { ReactiveNav2DNode& parent_; @@ -263,10 +269,38 @@ class ReactiveNav2DNode : public rclcpp::Node return ret; } - void sendNavigationStartEvent() override {} - void sendNavigationEndEvent() override {} - void sendNavigationEndDueToErrorEvent() override {} - void sendWaySeemsBlockedEvent() override {} + /** Callback: Start of navigation command */ + void sendNavigationStartEvent() override; + + /** Callback: End of navigation command (reach of single goal, or final + * waypoint of waypoint list) */ + void sendNavigationEndEvent() override; + + /** Callback: Reached an intermediary waypoint in waypoint list + * navigation. reached_nSkipped will be `true` if the waypoint was + * physically reached; `false` if it was actually "skipped". + */ + void sendWaypointReachedEvent( + int waypoint_index, bool reached_nSkipped) override; + + /** Callback: Heading towards a new intermediary/final waypoint in + * waypoint list navigation */ + void sendNewWaypointTargetEvent(int waypoint_index) override; + + /** Callback: Error asking sensory data from robot or sending motor + * commands. */ + void sendNavigationEndDueToErrorEvent() override; + + /** Callback: No progression made towards target for a predefined period + * of time. */ + void sendWaySeemsBlockedEvent() override; + + /** Callback: Apparent collision event (i.e. there is at least one + * obstacle point inside the robot shape) */ + void sendApparentCollisionEvent() override; + + /** Callback: Target seems to be blocked by an obstacle. */ + void sendCannotGetCloserToBlockedTargetEvent() override; }; MyReactiveInterface reactiveInterface_{*this}; diff --git a/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp b/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp index cf3ec91..4876227 100644 --- a/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp +++ b/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp @@ -70,9 +70,8 @@ ReactiveNav2DNode::ReactiveNav2DNode(const rclcpp::NodeOptions& options) { subRobotShape_ = this->create_subscription( subTopicRobotShape_, 1, - [this](const geometry_msgs::msg::Polygon::SharedPtr poly) { - this->on_set_robot_shape(poly); - }); + [this](const geometry_msgs::msg::Polygon::SharedPtr poly) + { this->on_set_robot_shape(poly); }); RCLCPP_INFO( this->get_logger(), @@ -140,31 +139,30 @@ ReactiveNav2DNode::ReactiveNav2DNode(const rclcpp::NodeOptions& options) this->create_publisher( pubTopicSelectedPtg_, 1); + pubNavEvents_ = + this->create_publisher(pubTopicEvents_, 1); + // Init ROS subs: // ----------------------- subOdometry_ = this->create_subscription( subTopicOdometry_, 1, - [this](const nav_msgs::msg::Odometry::SharedPtr odom) { - this->on_odometry_received(odom); - }); + [this](const nav_msgs::msg::Odometry::SharedPtr odom) + { this->on_odometry_received(odom); }); subWpSeq_ = this->create_subscription( subTopicWpSeq_, 1, - [this](const mrpt_msgs::msg::WaypointSequence::SharedPtr msg) { - this->on_waypoint_seq_received(msg); - }); + [this](const mrpt_msgs::msg::WaypointSequence::SharedPtr msg) + { this->on_waypoint_seq_received(msg); }); subNavGoal_ = this->create_subscription( subTopicNavGoal_, 1, - [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - this->on_goal_received(msg); - }); + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) + { this->on_goal_received(msg); }); subLocalObs_ = this->create_subscription( subTopicLocalObstacles_, 1, - [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - this->on_local_obstacles(msg); - }); + [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { this->on_local_obstacles(msg); }); // Init tf buffers // ---------------------------------------------------- @@ -227,6 +225,11 @@ void ReactiveNav2DNode::read_parameters() RCLCPP_INFO( this->get_logger(), "topic_cmd_vel: %s", pubTopicCmdVel_.c_str()); + declare_parameter("pubTopicEvents", pubTopicEvents_); + get_parameter("pubTopicEvents", pubTopicEvents_); + RCLCPP_INFO( + this->get_logger(), "pubTopicEvents: %s", pubTopicEvents_.c_str()); + declare_parameter("topic_obstacles", subTopicLocalObstacles_); get_parameter("topic_obstacles", subTopicLocalObstacles_); RCLCPP_INFO( @@ -542,6 +545,60 @@ visualization_msgs::msg::MarkerArray ReactiveNav2DNode::log_to_margers( return msg; } +void ReactiveNav2DNode::publish_event_message(const std::string& text) +{ + std_msgs::msg::String msg; + msg.data = text; + pubNavEvents_->publish(msg); +} + +void ReactiveNav2DNode::MyReactiveInterface::sendNavigationStartEvent() +{ + parent_.publish_event_message("START"); +} + +void ReactiveNav2DNode::MyReactiveInterface::sendNavigationEndEvent() +{ + parent_.publish_event_message("END"); +} + +void ReactiveNav2DNode::MyReactiveInterface::sendWaypointReachedEvent( + int waypoint_index, bool reached_nSkipped) +{ + using namespace std::string_literals; + parent_.publish_event_message( + "WP_REACHED "s + std::to_string(waypoint_index) + " "s + + (reached_nSkipped ? "REACHED" : "SKIPPED")); +} + +void ReactiveNav2DNode::MyReactiveInterface::sendNewWaypointTargetEvent( + int waypoint_index) +{ + using namespace std::string_literals; + parent_.publish_event_message("WP_NEW "s + std::to_string(waypoint_index)); +} + +void ReactiveNav2DNode::MyReactiveInterface::sendNavigationEndDueToErrorEvent() +{ + parent_.publish_event_message("ERROR"); +} + +void ReactiveNav2DNode::MyReactiveInterface::sendWaySeemsBlockedEvent() +{ + parent_.publish_event_message("WAY_SEEMS_BLOCKED"); +} + +void ReactiveNav2DNode::MyReactiveInterface::sendApparentCollisionEvent() +{ + parent_.publish_event_message("APPARENT_COLLISION"); +} + +void ReactiveNav2DNode::MyReactiveInterface:: + sendCannotGetCloserToBlockedTargetEvent() +{ + parent_.publish_event_message("CANNOT_GET_CLOSER"); +} + int main(int argc, char** argv) { rclcpp::init(argc, argv);