Block the wall-follow callbacks from running if the robot is presentl…

…y docked
turtlebot · Sep 20, 2024 · 82c1b74 · 82c1b74
1 parent 33048fc
commit 82c1b74
Showing 1 changed file with 6 additions and 2 deletions.
diff --git a/turtlebot4_node/src/turtlebot4.cpp b/turtlebot4_node/src/turtlebot4.cpp
@@ -476,14 +476,16 @@ 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<WallFollow::Goal>();
     auto runtime = builtin_interfaces::msg::Duration();
     runtime.sec = 10;
     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,14 +496,16 @@ 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<WallFollow::Goal>();
     auto runtime = builtin_interfaces::msg::Duration();
     runtime.sec = 10;
     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");
   }