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");
  }