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 b4145cb..1c160d2 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 @@ -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::SharedPtr open_place_detection_pub_; rclcpp::Subscription::SharedPtr gnss_pose_with_covariance_sub_; rclcpp::Subscription::SharedPtr goal_pose_sub_; - rclcpp::Subscription::SharedPtr alpha_sub_; rclcpp_action::Server::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 index 881a2c9..18a2fb9 100644 --- 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( "goal_pose", rclcpp::QoS(1), std::bind(&WallTracking::goal_pose_callback, this, std::placeholders::_1)); - alpha_sub_ = this->create_subscription( - "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)