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 17b696f..91086f8 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 @@ -49,48 +49,32 @@ class WallTracking : public rclcpp::Node { void navigateOpenPlace(); 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 gnss_pose_with_covariance_callback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); void goal_pose_callback(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg); -rclcpp_action::GoalResponse handle_goal( - [[maybe_unused]] const rclcpp_action::GoalUUID & uuid, - [[maybe_unused]] std::shared_ptr goal -); + // For wall_tracking action server + rclcpp_action::GoalResponse handle_goal( + [[maybe_unused]] const rclcpp_action::GoalUUID & uuid, + [[maybe_unused]] std::shared_ptr goal + ); -rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr goal_handle -); + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr goal_handle + ); -void handle_accepted( - const std::shared_ptr goal_handle -); + void handle_accepted( + const std::shared_ptr goal_handle + ); -void execute( - const std::shared_ptr goal_handle -); + void execute( + const std::shared_ptr goal_handle + ); -// rclcpp_action::GoalResponse nav_handle_goal( -// [[maybe_unused]] const rclcpp_action::GoalUUID & uuid, -// [[maybe_unused]] std::shared_ptr goal -// ); - -rclcpp_action::CancelResponse nav_handle_cancel( - const std::shared_ptr goal_handle -); - -void nav_handle_accepted( - const std::shared_ptr goal_handle -); - -void nav_execute( - const std::shared_ptr goal_handle -); - -void goalResponceCallback(const std::shared_ptr &goal_handle); -void feedbackCallback([[maybe_unused]] typename std::shared_ptr, - [[maybe_unused]] const std::shared_ptr feedback); -void resultCallback(const GoalHandleNavigateToPose::WrappedResult & result); + // For navigate_to_pose action client + void goalResponceCallback(const std::shared_ptr &goal_handle); + void feedbackCallback([[maybe_unused]] typename std::shared_ptr, + [[maybe_unused]] const std::shared_ptr feedback); + void resultCallback(const GoalHandleNavigateToPose::WrappedResult & result); private: rclcpp::Subscription::SharedPtr scan_sub_; @@ -98,7 +82,6 @@ void resultCallback(const GoalHandleNavigateToPose::WrappedResult & result); rclcpp::Subscription::SharedPtr gnss_sub_; rclcpp::Publisher::SharedPtr open_place_arrived_pub_; rclcpp::Publisher::SharedPtr open_place_detection_pub_; - rclcpp::Subscription::SharedPtr wall_tracking_flg_sub_; rclcpp::Subscription::SharedPtr gnss_pose_with_covariance_sub_; rclcpp::Subscription::SharedPtr goal_pose_sub_; @@ -111,7 +94,6 @@ void resultCallback(const GoalHandleNavigateToPose::WrappedResult & result); rclcpp_action::Client::SendGoalOptions nav_send_goal_options_; rclcpp_action::Client::SharedPtr navigation_action_client_; - // rclcpp_action::Server::SharedPtr navigation_action_srv_; float distance_from_wall_; float distance_to_stop_; diff --git a/wall_tracking_executor/src/wall_tracking_executor.cpp b/wall_tracking_executor/src/wall_tracking_executor.cpp index 0294e06..8e2f63d 100644 --- a/wall_tracking_executor/src/wall_tracking_executor.cpp +++ b/wall_tracking_executor/src/wall_tracking_executor.cpp @@ -79,9 +79,6 @@ void WallTracking::init_sub() gnss_sub_ = this->create_subscription( "gnss/fix", rclcpp::QoS(10), std::bind(&WallTracking::gnss_callback, this, std::placeholders::_1)); - wall_tracking_flg_sub_ = this->create_subscription( - "wall_tracking_flg", rclcpp::QoS(10), - std::bind(&WallTracking::wall_tracking_flg_callback, this, std::placeholders::_1)); gnss_pose_with_covariance_sub_ = this->create_subscription( "gnss_pose_with_covariance", rclcpp::QoS(10), std::bind(&WallTracking::gnss_pose_with_covariance_callback, this, std::placeholders::_1)); @@ -223,11 +220,6 @@ void WallTracking::gnss_pose_with_covariance_callback(geometry_msgs::msg::PoseW else gnss_nan_ = false; } -void WallTracking::wall_tracking_flg_callback(std_msgs::msg::Bool::ConstSharedPtr msg) -{ - wall_tracking_flg_ = msg->data; -} - float WallTracking::lateral_pid_control(float input) { float e = input - distance_from_wall_;