Publish navigation events

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 <INDEX> (REACHED|SKIPPED)`: Reached an intermediary waypoint in waypoint list navigation. The waypoint may have been physically reached or just skipped.
+  * `WP_NEW <INDEX>`: 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):

mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp

@@ -36,6 +36,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <std_msgs/msg/string.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
@@ -85,6 +86,8 @@ class ReactiveNav2DNode : public rclcpp::Node
 	rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
 		pubSelectedPtg_;
 
+	rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pubNavEvents_;
+
 	std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
 	std::shared_ptr<tf2_ros::TransformListener> 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};

mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp

@@ -70,9 +70,8 @@ ReactiveNav2DNode::ReactiveNav2DNode(const rclcpp::NodeOptions& options)
 	{
 		subRobotShape_ = this->create_subscription<geometry_msgs::msg::Polygon>(
 			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<visualization_msgs::msg::MarkerArray>(
 			pubTopicSelectedPtg_, 1);
 
+	pubNavEvents_ =
+		this->create_publisher<std_msgs::msg::String>(pubTopicEvents_, 1);
+
 	// Init ROS subs:
 	// -----------------------
 	subOdometry_ = this->create_subscription<nav_msgs::msg::Odometry>(
 		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<mrpt_msgs::msg::WaypointSequence>(
 		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<geometry_msgs::msg::PoseStamped>(
 		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<sensor_msgs::msg::PointCloud2>(
 		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<std::string>("pubTopicEvents", pubTopicEvents_);
+	get_parameter("pubTopicEvents", pubTopicEvents_);
+	RCLCPP_INFO(
+		this->get_logger(), "pubTopicEvents: %s", pubTopicEvents_.c_str());
+
 	declare_parameter<std::string>("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);