From a778f8e271bd24da3618b9cca7f5b7537f12b6a6 Mon Sep 17 00:00:00 2001
From: makotoyoshigoe <myoshigo0127@gmail.com>
Date: Mon, 2 Sep 2024 01:24:31 +0900
Subject: [PATCH] Change topic type and name

---
 .../wall_tracking_executor.hpp                   |  6 +++---
 .../src/wall_tracking_executor.cpp               | 16 ++++++++--------
 2 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/wall_tracking_executor/include/wall_tracking_executor/wall_tracking_executor.hpp b/wall_tracking_executor/include/wall_tracking_executor/wall_tracking_executor.hpp
index ade5c19..bfa3763 100644
--- a/wall_tracking_executor/include/wall_tracking_executor/wall_tracking_executor.hpp
+++ b/wall_tracking_executor/include/wall_tracking_executor/wall_tracking_executor.hpp
@@ -16,7 +16,7 @@
 #include <vector>
 #include "wall_tracking_msgs/action/wall_tracking.hpp"
 #include "wall_tracking_executor/ScanData.hpp"
-#include <nav_msgs/msg/odometry.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <nav2_msgs/action/navigate_to_pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 
@@ -50,7 +50,7 @@ class WallTracking : public rclcpp::Node {
 	void pub_open_place_arrived(bool open_place_arrived);
 	void pub_open_place_detection(std::string open_place_detection);
 	void wall_tracking_flg_callback(std_msgs::msg::Bool::ConstSharedPtr msg);
-	void odom_gnss_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg);
+	void gnss_pose_with_covariance_callback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg);
 	void goal_pose_callback(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg);
 	void cancel_nav();
 	void resume_nav();
@@ -96,7 +96,7 @@ void nav_execute(
 	rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr open_place_arrived_pub_;
 	rclcpp::Publisher<std_msgs::msg::String>::SharedPtr open_place_detection_pub_;
 	rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr wall_tracking_flg_sub_;
-	rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_gnss_sub_;
+	rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr gnss_pose_with_covariance_sub_;
 	rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_pose_sub_;
 
 	rclcpp_action::Server<WallTrackingAction>::SharedPtr wall_tracking_action_srv_;
diff --git a/wall_tracking_executor/src/wall_tracking_executor.cpp b/wall_tracking_executor/src/wall_tracking_executor.cpp
index 6863560..84eeb59 100644
--- a/wall_tracking_executor/src/wall_tracking_executor.cpp
+++ b/wall_tracking_executor/src/wall_tracking_executor.cpp
@@ -82,9 +82,9 @@ void WallTracking::init_sub()
     wall_tracking_flg_sub_ = this->create_subscription<std_msgs::msg::Bool>(
         "wall_tracking_flg", rclcpp::QoS(10),
         std::bind(&WallTracking::wall_tracking_flg_callback, this, std::placeholders::_1));
-    odom_gnss_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
-        "odom/gnss", rclcpp::QoS(10),
-        std::bind(&WallTracking::odom_gnss_callback, this, std::placeholders::_1));
+    gnss_pose_with_covariance_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+        "gnss_pose_with_covariance", rclcpp::QoS(10),
+        std::bind(&WallTracking::gnss_pose_with_covariance_callback, this, std::placeholders::_1));
     goal_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
         "goal_pose", rclcpp::QoS(1),
         std::bind(&WallTracking::goal_pose_callback, this, std::placeholders::_1));
@@ -158,7 +158,7 @@ void WallTracking::scan_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg
         // float p = scan_data_->openPlaceCheck(-90., 90., open_place_distance_, per, mean);
         scan_data_->openPlaceCheck(-90., 90., open_place_distance_, per, mean);
         open_place_ = !open_place_ ? (per >= 0.7) : per >= 0.4;
-        if(gnss_nan_) open_place_ = false;
+        // if(gnss_nan_) open_place_ = false;
         cmd_vel_ = !open_place_ ? max_linear_vel_ : vel_open_place_;
     }
     pub_open_place_arrived(open_place_);
@@ -178,7 +178,7 @@ void WallTracking::gnss_callback(sensor_msgs::msg::NavSatFix::ConstSharedPtr msg
     // RCLCPP_INFO(this->get_logger(), "outdoor: %d", outdoor_);
 }
 
-void  WallTracking::odom_gnss_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg)
+void  WallTracking::gnss_pose_with_covariance_callback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg)
 {
     if(std::isnan(msg->pose.pose.position.x) && std::isnan(msg->pose.pose.position.y)) gnss_nan_ = true;
     else gnss_nan_ = false;
@@ -247,7 +247,7 @@ void WallTracking::navigateOpenPlace()
                     scan_data_->openPlaceCheck(detection_div_deg_[i], detection_div_deg_[i+1], open_place_distance_, per, mean);
                     evals[j] = per < 0.7 ? -1. : per;
                     means[j] = mean;
-                    RCLCPP_INFO(this->get_logger(), "Range %d : eval=%lf, mean=%lf", j+1, evals[j], means[j]);
+                    // RCLCPP_INFO(this->get_logger(), "Range %d : eval=%lf, mean=%lf", j+1, evals[j], means[j]);
                     ++j;
                 }
                 auto max_iter = std::max_element(evals.begin(), evals.end());
@@ -303,9 +303,8 @@ void WallTracking::handle_accepted(
 void WallTracking::execute(
     const std::shared_ptr<GoalHandleWallTracking> goal_handle) 
 {
-    // cancel_nav();
+    cancel_nav();
     // rclcpp::sleep_for(5000ms);
-    // resume_nav();
     RCLCPP_INFO(this->get_logger(), "EXECUTE");
     const auto goal = goal_handle->get_goal();
     auto feedback = std::make_shared<WallTrackingAction::Feedback>();
@@ -320,6 +319,7 @@ void WallTracking::execute(
             goal_handle->canceled(result);
             pub_cmd_vel(0.0, 0.0);
             RCLCPP_INFO(this->get_logger(), "Goal Canceled");
+            resume_nav();
             return;
         }
         loop_rate.sleep();