Merge pull request #19 from EthanDelage/terminate-node

Terminate node
tdameros · Dec 1, 2023 · eb2b724 · eb2b724
2 parents d342fda + e2b4156
commit eb2b724
Show file tree

Hide file tree

Showing 7 changed files with 51 additions and 11 deletions.
diff --git a/src/navigation/include/Navigation.hpp b/src/navigation/include/Navigation.hpp
@@ -43,13 +43,15 @@ class Navigation : public rclcpp::Node {
 
 	rclcpp::Subscription<ros_gz_interfaces::msg::ParamVec>::SharedPtr	_pinger;
 	rclcpp::Subscription<ros_gz_interfaces::msg::ParamVec>::SharedPtr	_buoyPinger;
+	rclcpp::Subscription<ros_gz_interfaces::msg::ParamVec>::SharedPtr	_taskInfo;
 	rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr		_alliesPos;
 	rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr				_publisherThrust;
 	rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr				_publisherPos;
 
 
 	void	pingerCallback(ros_gz_interfaces::msg::ParamVec::SharedPtr msg);
 	void	buoyPingerCallback(ros_gz_interfaces::msg::ParamVec::SharedPtr msg);
+	void	taskInfoCallback(ros_gz_interfaces::msg::ParamVec::SharedPtr msg);
 	void	setHeading();
 	double	calculateThrust(double regulation);
 	double	regulator(double bearing);

diff --git a/src/navigation/src/Navigation.cpp b/src/navigation/src/Navigation.cpp
@@ -12,6 +12,8 @@ Navigation::Navigation() : Node("navigation") {
 	_exitBuoy = false;
 	_pinger = create_subscription<ros_gz_interfaces::msg::ParamVec>("/navigation/pinger", 10,
 				std::bind(&Navigation::pingerCallback, this, _1));
+	_taskInfo = create_subscription<ros_gz_interfaces::msg::ParamVec>("/vrx/task/info", 10,
+				std::bind(&Navigation::taskInfoCallback, this, _1));
 	_buoyPinger = create_subscription<ros_gz_interfaces::msg::ParamVec>("/wamv/sensors/acoustics/receiver/range_bearing", 10,
 				std::bind(&Navigation::buoyPingerCallback, this, _1));
 	_publisherThrust = create_publisher<std_msgs::msg::Float64>("/wamv/thrusters/main/thrust", 5);
@@ -76,6 +78,15 @@ void Navigation::buoyPingerCallback(ros_gz_interfaces::msg::ParamVec::SharedPtr
 	}
 }
 
+void Navigation::taskInfoCallback(ros_gz_interfaces::msg::ParamVec::SharedPtr msg) {
+	for (const auto & param: msg->params) {
+		if (param.name == "state" && param.value.string_value == "finished") {
+			rclcpp::shutdown();
+			exit(0);
+		}
+	}
+}
+
 void Navigation::setHeading() {
 	auto 	posMsg = std_msgs::msg::Float64();
 	auto 	thrustMsg = std_msgs::msg::Float64();

diff --git a/src/pathfinding/include/Pathfinding.hpp b/src/pathfinding/include/Pathfinding.hpp
@@ -71,6 +71,7 @@ class Pathfinding : public rclcpp::Node {
 	// Publisher/Subscriber
 	rclcpp::Subscription<ros_gz_interfaces::msg::ParamVec>::SharedPtr	_pinger;
 	rclcpp::Subscription<ros_gz_interfaces::msg::ParamVec>::SharedPtr	_perception;
+	rclcpp::Subscription<ros_gz_interfaces::msg::ParamVec>::SharedPtr	_taskInfo;
 	rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr				_phase;
 	rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr		_gps;
 	rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr				_imu;
@@ -80,6 +81,7 @@ class Pathfinding : public rclcpp::Node {
 	// Callback functions
 	void	pingerCallback(ros_gz_interfaces::msg::ParamVec::SharedPtr msg);
 	void	perceptionCallback(ros_gz_interfaces::msg::ParamVec::SharedPtr msg);
+	void	taskInfoCallback(ros_gz_interfaces::msg::ParamVec::SharedPtr msg);
 	void	phaseCallback(std_msgs::msg::UInt32::SharedPtr msg);
 	void 	gpsCallback(sensor_msgs::msg::NavSatFix::SharedPtr msg);
 	void 	imuCallback(sensor_msgs::msg::Imu::SharedPtr msg);

diff --git a/src/pathfinding/src/Pathfinding.cpp b/src/pathfinding/src/Pathfinding.cpp
@@ -24,6 +24,8 @@ Pathfinding::Pathfinding() :
 				std::bind(&Pathfinding::pingerCallback, this, _1));
 	_perception = create_subscription<ros_gz_interfaces::msg::ParamVec>("/perception/pinger", 10,
 				std::bind(&Pathfinding::perceptionCallback, this, _1));
+	_taskInfo = create_subscription<ros_gz_interfaces::msg::ParamVec>("/vrx/task/info", 10,
+				std::bind(&Pathfinding::taskInfoCallback, this, _1));
 	_phase = create_subscription<std_msgs::msg::UInt32>("/vrx/patrolandfollow/current_phase", 10,
 				std::bind(&Pathfinding::phaseCallback, this, _1));
 	_gps = create_subscription<sensor_msgs::msg::NavSatFix>("/wamv/sensors/gps/gps/fix", 10,

diff --git a/src/pathfinding/src/callback.cpp b/src/pathfinding/src/callback.cpp
@@ -50,6 +50,15 @@ void Pathfinding::perceptionCallback(ros_gz_interfaces::msg::ParamVec::SharedPtr
 	_path = calculatePath(_boatPos);
 }
 
+void Pathfinding::taskInfoCallback(ros_gz_interfaces::msg::ParamVec::SharedPtr msg) {
+	for (const auto & param: msg->params) {
+		if (param.name == "state" && param.value.string_value == "finished") {
+			rclcpp::shutdown();
+			exit(0);
+		}
+	}
+}
+
 void Pathfinding::publishScan(double value) {
 	auto	paramVecMsg = ros_gz_interfaces::msg::ParamVec();
 	rcl_interfaces::msg::Parameter	scanMsg;

diff --git a/src/perception/include/Perception.hpp b/src/perception/include/Perception.hpp
@@ -49,21 +49,23 @@ class Perception : public rclcpp::Node {
 	std::vector<double>		_rangeHistory;
 	uint32_t 				_state;
 
-	rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr		_imageSubscriber;
-	rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr	_cameraSubscriber;
-	rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr	_pointCloudSubscriber;
-	rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr			_imuSubscriber;
-	rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr	_gpsSubscriber;
-	rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr			_stateSubscriber;
-	rclcpp::Publisher<ros_gz_interfaces::msg::ParamVec>::SharedPtr	_perceptionPublisher;
-	rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr	_alertPublisher;
+	rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr			_imageSubscriber;
+	rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr		_cameraSubscriber;
+	rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr		_pointCloudSubscriber;
+	rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr				_imuSubscriber;
+	rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr		_gpsSubscriber;
+	rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr				_stateSubscriber;
+	rclcpp::Subscription<ros_gz_interfaces::msg::ParamVec>::SharedPtr	_taskInfo;
+	rclcpp::Publisher<ros_gz_interfaces::msg::ParamVec>::SharedPtr		_perceptionPublisher;
+	rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr		_alertPublisher;
 
 	void imageCallback(sensor_msgs::msg::Image::SharedPtr msg);
 	void cameraCallback(sensor_msgs::msg::CameraInfo::SharedPtr msg);
 	void pointCloudCallback(sensor_msgs::msg::PointCloud2::SharedPtr msg);
 	void imuCallback(sensor_msgs::msg::Imu::SharedPtr msg);
 	void gpsCallback(sensor_msgs::msg::NavSatFix::SharedPtr msg);
 	void stateCallback(std_msgs::msg::UInt32::SharedPtr msg);
+	void taskInfoCallback(ros_gz_interfaces::msg::ParamVec::SharedPtr msg);
 
 	//	computePosition
 	void calculateMapPos(double latitude, double longitude);

diff --git a/src/perception/src/Perception.cpp b/src/perception/src/Perception.cpp
@@ -25,9 +25,13 @@ Perception::Perception(): Node("perception") {
 			10,
 			std::bind(&Perception::imuCallback, this, _1));
 	_gpsSubscriber = create_subscription<sensor_msgs::msg::NavSatFix>(
-		"/wamv/sensors/gps/gps/fix",
-		10,
-		std::bind(&Perception::gpsCallback, this, _1));
+			"/wamv/sensors/gps/gps/fix",
+			10,
+			std::bind(&Perception::gpsCallback, this, _1));
+	_taskInfo = create_subscription<ros_gz_interfaces::msg::ParamVec>(
+			"/vrx/task/info",
+			10,
+			std::bind(&Perception::taskInfoCallback, this, _1));
 	_perceptionPublisher = create_publisher<ros_gz_interfaces::msg::ParamVec>(
 			"/perception/pinger",
 			10);
@@ -110,6 +114,14 @@ void Perception::gpsCallback(sensor_msgs::msg::NavSatFix::SharedPtr msg) {
 	_gpsPing = true;
 }
 
+void Perception::taskInfoCallback(ros_gz_interfaces::msg::ParamVec::SharedPtr msg) {
+	for (const auto & param: msg->params) {
+		if (param.name == "state" && param.value.string_value == "finished") {
+			rclcpp::shutdown();
+			exit(0);
+		}
+	}
+}
 
 void Perception::publishPathfinding() {
 	auto	paramVecMsg = ros_gz_interfaces::msg::ParamVec();