From 82c1b740623bfd878664d8d7523a2bb852f25293 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Fri, 20 Sep 2024 12:45:55 -0400 Subject: [PATCH] Block the wall-follow callbacks from running if the robot is presently docked --- turtlebot4_node/src/turtlebot4.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/turtlebot4_node/src/turtlebot4.cpp b/turtlebot4_node/src/turtlebot4.cpp index 61193a5..566d903 100644 --- a/turtlebot4_node/src/turtlebot4.cpp +++ b/turtlebot4_node/src/turtlebot4.cpp @@ -476,7 +476,7 @@ void Turtlebot4::undock_function_callback() */ void Turtlebot4::wall_follow_left_function_callback() { - if (wall_follow_client_ != nullptr) { + if (wall_follow_client_ != nullptr && !is_docked_) { RCLCPP_INFO(this->get_logger(), "Wall Follow Left"); auto goal_msg = std::make_shared(); auto runtime = builtin_interfaces::msg::Duration(); @@ -484,6 +484,8 @@ void Turtlebot4::wall_follow_left_function_callback() goal_msg->follow_side = WallFollow::Goal::FOLLOW_LEFT; goal_msg->max_runtime = runtime; wall_follow_client_->send_goal(goal_msg); + } else if (is_docked_) { + RCLCPP_ERROR(this->get_logger(), "Undock before following wall"); } else { RCLCPP_ERROR(this->get_logger(), "Follow client NULL"); } @@ -494,7 +496,7 @@ void Turtlebot4::wall_follow_left_function_callback() */ void Turtlebot4::wall_follow_right_function_callback() { - if (wall_follow_client_ != nullptr) { + if (wall_follow_client_ != nullptr && !is_docked_) { RCLCPP_INFO(this->get_logger(), "Wall Follow Right"); auto goal_msg = std::make_shared(); auto runtime = builtin_interfaces::msg::Duration(); @@ -502,6 +504,8 @@ void Turtlebot4::wall_follow_right_function_callback() goal_msg->follow_side = WallFollow::Goal::FOLLOW_RIGHT; goal_msg->max_runtime = runtime; wall_follow_client_->send_goal(goal_msg); + } else if (is_docked_) { + RCLCPP_ERROR(this->get_logger(), "Undock before following wall"); } else { RCLCPP_ERROR(this->get_logger(), "Follow client NULL"); }