Add interlock

makotoyoshigoe · Oct 14, 2024 · 6f21a5f · 6f21a5f
1 parent f026a84
commit 6f21a5f
Show file tree

Hide file tree

Showing 2 changed files with 1 addition and 12 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
@@ -54,7 +54,6 @@ class WallTracking : public rclcpp::Node {
 	void pub_open_place_detection(std::string open_place_detection);
 	void gnss_pose_with_covariance_callback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg);
 	void goal_pose_callback(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg);
-	void alpha_callback(std_msgs::msg::Float32::ConstSharedPtr msg);
 
 	// For wall_tracking action server
 	rclcpp_action::GoalResponse handle_goal(
@@ -90,7 +89,6 @@ class WallTracking : public rclcpp::Node {
 	rclcpp::Publisher<std_msgs::msg::String>::SharedPtr open_place_detection_pub_;
 	rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr gnss_pose_with_covariance_sub_;
 	rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_pose_sub_;
-	rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr alpha_sub_;
 
 	rclcpp_action::Server<WallTrackingAction>::SharedPtr wall_tracking_action_srv_;
 
@@ -131,8 +129,6 @@ class WallTracking : public rclcpp::Node {
 	float pre_e_;
 	bool gnss_nan_;
 	bool recieved_nav_goal_;
-	bool only_navigate_open_place_flg_ = false;
-	float alpha_;
 };
 
 } // namespace WallTracking

diff --git a/wall_tracking_executor/src/wall_tracking_executor.cpp b/wall_tracking_executor/src/wall_tracking_executor.cpp
@@ -85,13 +85,6 @@ void WallTracking::init_sub()
     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));
-    alpha_sub_ = this->create_subscription<std_msgs::msg::Float32>(
-        "alpha", rclcpp::QoS(2), std::bind(&WallTracking::alpha_callback, this, std::placeholders::_1));
-}
-
-void WallTracking::alpha_callback(std_msgs::msg::Float32::ConstSharedPtr msg)
-{
-    alpha_ = msg->data;
 }
 
 void WallTracking::init_pub()
@@ -233,7 +226,7 @@ void WallTracking::goal_pose_callback(geometry_msgs::msg::PoseStamped::ConstShar
     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)