Feat/interlock 241014 (#58)

* Update

* Forget to committing

* Add interlock

wall_tracking_executor/include/wall_tracking_executor/wall_tracking_executor.hpp

@@ -19,7 +19,10 @@
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <nav2_msgs/action/navigate_to_pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
-
+#include <std_msgs/msg/float32.hpp>
+#include <wall_tracking_msgs/msg/behavior_stamped_array.hpp>
+#include <wall_tracking_msgs/msg/behavior_stamped.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
 
 using WallTrackingAction = wall_tracking_msgs::action::WallTracking;
 using GoalHandleWallTracking = rclcpp_action::ServerGoalHandle<WallTrackingAction>;
@@ -75,6 +78,8 @@ class WallTracking : public rclcpp::Node {
 	void feedbackCallback([[maybe_unused]] typename std::shared_ptr<GoalHandleNavigateToPose>, 
 						[[maybe_unused]] const std::shared_ptr<const typename NavigateToPose::Feedback> feedback);
 	void resultCallback(const GoalHandleNavigateToPose::WrappedResult & result);
+	void addBehaviorStamedArray(std::string behavior_name);
+	void behaviorStampedPub(void);
 
 private:
 	rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
@@ -94,7 +99,10 @@ class WallTracking : public rclcpp::Node {
 
 	rclcpp_action::Client<NavigateToPose>::SendGoalOptions nav_send_goal_options_;
 	rclcpp_action::Client<NavigateToPose>::SharedPtr navigation_action_client_;
-
+
+	std::vector<wall_tracking_msgs::msg::BehaviorStamped> behavior_stamped_array_;
+	rclcpp::Publisher<wall_tracking_msgs::msg::BehaviorStamped>::SharedPtr behavior_stamped_array_pub_;
+
 	float distance_from_wall_;
 	float distance_to_stop_;
 	float max_linear_vel_;
@@ -118,7 +126,7 @@ class WallTracking : public rclcpp::Node {
 	bool open_place_linear_;
 	std::vector<double> select_angvel_;
 	std::vector<double> detection_div_deg_;
-    float pre_e_;
+	float pre_e_;
 	bool gnss_nan_;
 	bool recieved_nav_goal_;
 };

wall_tracking_executor/src/wall_tracking_executor.cpp

@@ -92,6 +92,7 @@ void WallTracking::init_pub()
     cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", rclcpp::QoS(10));
     open_place_arrived_pub_ = this->create_publisher<std_msgs::msg::Bool>("open_place_arrived", rclcpp::QoS(10));
     open_place_detection_pub_ = this->create_publisher<std_msgs::msg::String>("open_place_detection", rclcpp::QoS(10));
+    behavior_stamped_array_pub_ = this->create_publisher<wall_tracking_msgs::msg::BehaviorStamped>("behavior_stamped", rclcpp::QoS(10));
 }
 
 void WallTracking::init_action()
@@ -130,15 +131,20 @@ void WallTracking::feedbackCallback([[maybe_unused]] typename std::shared_ptr<Go
 void WallTracking::resultCallback(const GoalHandleNavigateToPose::WrappedResult & result){
     switch (result.code) {
       case rclcpp_action::ResultCode::SUCCEEDED:
-        RCLCPP_INFO(this->get_logger(), "Goal succeeded!");
         recieved_nav_goal_ = false;
-        RCLCPP_INFO(this->get_logger(), "Recieved nav goal: %d", recieved_nav_goal_);
+        RCLCPP_INFO(this->get_logger(), "Goal succeeded!");
+        addBehaviorStamedArray("Navigation Succeeded");
+        behaviorStampedPub();
+        // RCLCPP_INFO(this->get_logger(), "Recieved nav goal: %d", recieved_nav_goal_);
         break;
       case rclcpp_action::ResultCode::ABORTED:
         RCLCPP_WARN(this->get_logger(), "Goal was aborted");
+        addBehaviorStamedArray("Navigation Aborted");
+        behaviorStampedPub();
         break;
       case rclcpp_action::ResultCode::CANCELED:
         RCLCPP_WARN(this->get_logger(), "Goal was canceled");
+        addBehaviorStamedArray("Navigation canceled");
         // recieved_nav_goal_ = true;
         break;
       default:
@@ -147,6 +153,19 @@ void WallTracking::resultCallback(const GoalHandleNavigateToPose::WrappedResult
     }
 }
 
+void WallTracking::behaviorStampedPub(void)
+{
+    rclcpp::Rate pub_rate(1);
+    for(auto &b: behavior_stamped_array_){
+        wall_tracking_msgs::msg::BehaviorStamped tmp;
+        tmp.behavior_name = b.behavior_name;
+        tmp.stamp = b.stamp;
+        behavior_stamped_array_pub_->publish(tmp);
+        pub_rate.sleep();
+    }
+    behavior_stamped_array_.clear();
+}
+
 void WallTracking::init_variable()
 {
     ei_ = 0.0;
@@ -191,21 +210,23 @@ 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_);
-    if(wall_tracking_flg_) navigateOpenPlace();
+    if(wall_tracking_flg_ && recieved_nav_goal_) navigateOpenPlace();
     else pub_cmd_vel(0., 0.);
     // RCLCPP_INFO(this->get_logger(), "update scan data");
 }
 
 void WallTracking::goal_pose_callback(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
 {
     nav_goal_msgs_.pose = *msg;
+    behavior_stamped_array_.clear();
+    addBehaviorStamedArray("Navigation Start");
     recieved_nav_goal_ = true;
     RCLCPP_INFO(this->get_logger(), "Recieved nav goal: %d", recieved_nav_goal_);
-    navigation_action_client_->async_send_goal(nav_goal_msgs_, nav_send_goal_options_);
+	if(!wall_tracking_flg_) navigation_action_client_->async_send_goal(nav_goal_msgs_, nav_send_goal_options_);
 }
 
 void WallTracking::gnss_callback(sensor_msgs::msg::NavSatFix::ConstSharedPtr msg)
@@ -235,7 +256,7 @@ void WallTracking::turn()
     msg.linear.x = 0.0;
     msg.angular.z = DEG2RAD(-45);
     cmd_vel_pub_->publish(msg);
-    rclcpp::sleep_for(500ms);
+    rclcpp::sleep_for(100ms);
 }
 
 void WallTracking::wallTracking()
@@ -296,6 +317,15 @@ void WallTracking::navigateOpenPlace()
     }
 }
 
+void WallTracking::addBehaviorStamedArray(std::string behavior_name)
+{
+    wall_tracking_msgs::msg::BehaviorStamped tmp_behavior_stamped;
+    tmp_behavior_stamped.behavior_name = behavior_name;
+    tmp_behavior_stamped.stamp = now();
+    behavior_stamped_array_.push_back(tmp_behavior_stamped);
+    // RCLCPP_INFO(this->get_logger(), "Num of behavior stamped array: %ld", behavior_stamped_array_.size());
+}
+
 void WallTracking::pub_open_place_arrived(bool open_place_arrived)
 {
     open_place_arrived_msg_.data = open_place_arrived;
@@ -340,6 +370,7 @@ void WallTracking::execute(
         RCLCPP_INFO(this->get_logger(), "Send cancel navigation to server");
     }
     rclcpp::sleep_for(1000ms);
+    addBehaviorStamedArray("WallTracking Start");
     RCLCPP_INFO(this->get_logger(), "EXECUTE");
     const auto goal = goal_handle->get_goal();
     std::shared_ptr<WallTrackingAction::Feedback> feedback = std::make_shared<WallTrackingAction::Feedback>();
@@ -348,13 +379,16 @@ void WallTracking::execute(
     rclcpp::Rate loop_rate(20);
     while (rclcpp::ok()) {
         if (goal_handle->is_canceling()) {
+
             wall_tracking_flg_ = false;
             result->get = false;
             goal_handle->canceled(result);
             pub_cmd_vel(0.0, 0.0);
+            addBehaviorStamedArray("WallTracking Cancel");
             RCLCPP_INFO(this->get_logger(), "Goal Canceled");
             if(recieved_nav_goal_){
                 navigation_action_client_->async_send_goal(nav_goal_msgs_, nav_send_goal_options_);
+                addBehaviorStamedArray("Navigation Resume");
                 RCLCPP_INFO(this->get_logger(), "Resume navigation");
             }
             return;

wall_tracking_msgs/CMakeLists.txt

@@ -20,15 +20,22 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+
+set(msg_files
+  "msg/BehaviorStamped.msg"
+  "msg/BehaviorStampedArray.msg"
+)
+
 set(action_files
   "action/WallTracking.action"
 )
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   ${action_files}
+  ${msg_files}
   DEPENDENCIES 
     builtin_interfaces
   ADD_LINTER_TESTS
 )
 
-ament_auto_package()
+ament_auto_package()

wall_tracking_msgs/msg/BehaviorStamped.msg

@@ -0,0 +1,2 @@
+string behavior_name
+builtin_interfaces/Time stamp

wall_tracking_msgs/msg/BehaviorStampedArray.msg

@@ -0,0 +1 @@
+wall_tracking_msgs/BehaviorStamped[] behavior_stamped_array