From b913408bf6b610fb67c2083c051ff196a0368134 Mon Sep 17 00:00:00 2001 From: estherag Date: Wed, 19 Nov 2025 12:24:03 +0100 Subject: [PATCH 1/2] Improve logging and goal sent behavior --- .../PatrollingNode.hpp | 3 +- .../PatrollingNode.cpp | 40 +++++++++---------- .../patrolling_node.py | 28 +++++++------ 3 files changed, 38 insertions(+), 33 deletions(-) diff --git a/exercises/easynav/easynav_playground/easynav_patrolling_behavior/include/easynav_patrolling_behavior/PatrollingNode.hpp b/exercises/easynav/easynav_playground/easynav_patrolling_behavior/include/easynav_patrolling_behavior/PatrollingNode.hpp index b367d98..bb659a0 100644 --- a/exercises/easynav/easynav_playground/easynav_patrolling_behavior/include/easynav_patrolling_behavior/PatrollingNode.hpp +++ b/exercises/easynav/easynav_playground/easynav_patrolling_behavior/include/easynav_patrolling_behavior/PatrollingNode.hpp @@ -56,8 +56,9 @@ class PatrollingNode : public rclcpp::Node bool initialized_ {false}; size_t send_retries_ {0}; - const size_t max_retries_ {3}; + const size_t max_retries_ {5}; uint last_control_type_ {0}; + GoalManagerClient::State last_nav_state_ {GoalManagerClient::State::IDLE}; std::string frame_id_; nav_msgs::msg::Goals goals_; diff --git a/exercises/easynav/easynav_playground/easynav_patrolling_behavior/src/easynav_patrolling_behavior/PatrollingNode.cpp b/exercises/easynav/easynav_playground/easynav_patrolling_behavior/src/easynav_patrolling_behavior/PatrollingNode.cpp index de1f3d4..2b7c2d2 100644 --- a/exercises/easynav/easynav_playground/easynav_patrolling_behavior/src/easynav_patrolling_behavior/PatrollingNode.cpp +++ b/exercises/easynav/easynav_playground/easynav_patrolling_behavior/src/easynav_patrolling_behavior/PatrollingNode.cpp @@ -87,10 +87,6 @@ PatrollingNode::cycle() nav_msgs::msg::Goals single_goal; single_goal.header = goals_.header; single_goal.goals.push_back(goals_.goals[current_goal_index_]); - // while (gm_client_->get_state() != GoalManagerClient::State::IDLE) - // { - // gm_client_->reset(); // Ensure the client is idle before sending new goals - // } gm_client_->send_goals(single_goal); RCLCPP_INFO(get_logger(), "Goals sent"); state_ = PatrolState::PATROLLING; @@ -99,28 +95,33 @@ PatrollingNode::cycle() case PatrolState::PATROLLING: { - auto nav_state = gm_client_->get_state(); + + // Show navigating message on state transition to ACCEPTED_AND_NAVIGATING + if (nav_state == GoalManagerClient::State::ACCEPTED_AND_NAVIGATING && + last_nav_state_ != GoalManagerClient::State::ACCEPTED_AND_NAVIGATING) + { + RCLCPP_INFO(get_logger(), "Navigating to waypoint %zu", current_goal_index_ + 1); + } + last_nav_state_ = nav_state; + switch (nav_state) { case GoalManagerClient::State::SENT_GOAL: - last_control_type_ = gm_client_->get_last_control().type; - - if (last_control_type_ == easynav_interfaces::msg::NavigationControl::REQUEST) { - if (send_retries_ < max_retries_) { - send_retries_++; - RCLCPP_INFO(get_logger(), "Waiting for ACCEPT... attempt %zu/%zu", send_retries_, - max_retries_); - } else { - RCLCPP_WARN(get_logger(), "No ACCEPT received after %zu attempts, resending goal", + send_retries_++; + if (send_retries_ >= max_retries_) { + RCLCPP_WARN(get_logger(), "No ACCEPT received after %zu attempts, resending goal...", max_retries_); - send_retries_ = 0; - state_ = PatrolState::IDLE; - } - } else if (last_control_type_ == easynav_interfaces::msg::NavigationControl::ACCEPT) { send_retries_ = 0; + // Recreate client to reset to IDLE state + gm_client_ = GoalManagerClient::make_shared(shared_from_this()); + last_nav_state_ = GoalManagerClient::State::IDLE; + state_ = PatrolState::IDLE; } break; + case GoalManagerClient::State::ACCEPTED_AND_NAVIGATING: + break; + case GoalManagerClient::State::NAVIGATION_REJECTED: case GoalManagerClient::State::NAVIGATION_FAILED: case GoalManagerClient::State::NAVIGATION_CANCELLED: @@ -139,8 +140,7 @@ PatrollingNode::cycle() state_ = PatrolState::DO_AT_WAYPOINT; break; - case GoalManagerClient::State::ACCEPTED_AND_NAVIGATING: - break; + default: break; } diff --git a/exercises/easynav/easynav_playground/easynav_patrolling_behavior_py/easynav_patrolling_behavior_py/patrolling_node.py b/exercises/easynav/easynav_playground/easynav_patrolling_behavior_py/easynav_patrolling_behavior_py/patrolling_node.py index 16567d3..37ddf78 100644 --- a/exercises/easynav/easynav_playground/easynav_patrolling_behavior_py/easynav_patrolling_behavior_py/patrolling_node.py +++ b/exercises/easynav/easynav_playground/easynav_patrolling_behavior_py/easynav_patrolling_behavior_py/patrolling_node.py @@ -93,7 +93,8 @@ def __init__(self): self.get_logger().info('PatrollingNode started') self._send_retries = 0 self._state = PatrolState.IDLE - self._max_retries = 3 + self._max_retries = 5 + self._last_nav_state = ClientState.IDLE def build_current_goal(self): goal = Goals() @@ -106,24 +107,27 @@ def _cycle(self): case PatrolState.IDLE: self._gm.send_goals(self.build_current_goal()) self._send_retries = 0 - self.get_logger().info(f'Sending goal to waypoint {self.current_goal_index + 1}') + self.get_logger().info('Goals sent') self._state = PatrolState.PATROLLING case PatrolState.PATROLLING: nav_state = self._gm.get_state() - current_type = self._gm.get_last_control().type + + # Show navigating message on state transition to ACCEPTED_AND_NAVIGATING + if (nav_state == ClientState.ACCEPTED_AND_NAVIGATING and + self._last_nav_state != ClientState.ACCEPTED_AND_NAVIGATING): + self.get_logger().info(f'Navigating to waypoint {self.current_goal_index + 1}') + self._last_nav_state = nav_state if nav_state == ClientState.SENT_GOAL: - if current_type == 0: # REQUEST - if self._send_retries < self._max_retries: - self._send_retries += 1 - self.get_logger().info(f"Waiting for ACCEPT... attempt {self._send_retries}/{self._max_retries}") - else: - self.get_logger().warn(f"No ACCEPT received after {self._max_retries} attempts, resending goal") - self._send_retries = 0 - self._state = PatrolState.IDLE - elif current_type == 1: # ACCEPT + self._send_retries += 1 + if self._send_retries >= self._max_retries: + self.get_logger().warn(f"No ACCEPT received after {self._max_retries} attempts, resending goal...") self._send_retries = 0 + # Recreate client to reset to IDLE state + self._gm = GoalManagerClient(node=self) + self._last_nav_state = ClientState.IDLE + self._state = PatrolState.IDLE elif ( nav_state == ClientState.NAVIGATION_REJECTED or nav_state == ClientState.NAVIGATION_FAILED or From b02296c5760e8a292376b142d9829dcab4415d18 Mon Sep 17 00:00:00 2001 From: estherag Date: Wed, 19 Nov 2025 13:21:29 +0100 Subject: [PATCH 2/2] Add resetting state to isolate resending goal action --- .../PatrollingNode.hpp | 2 +- .../PatrollingNode.cpp | 18 +++++++++++++----- .../patrolling_node.py | 16 +++++++++++----- 3 files changed, 25 insertions(+), 11 deletions(-) diff --git a/exercises/easynav/easynav_playground/easynav_patrolling_behavior/include/easynav_patrolling_behavior/PatrollingNode.hpp b/exercises/easynav/easynav_playground/easynav_patrolling_behavior/include/easynav_patrolling_behavior/PatrollingNode.hpp index bb659a0..37ca415 100644 --- a/exercises/easynav/easynav_playground/easynav_patrolling_behavior/include/easynav_patrolling_behavior/PatrollingNode.hpp +++ b/exercises/easynav/easynav_playground/easynav_patrolling_behavior/include/easynav_patrolling_behavior/PatrollingNode.hpp @@ -51,7 +51,7 @@ class PatrollingNode : public rclcpp::Node void initialize(); void cycle(); nav_msgs::msg::Goals build_current_goal(); - enum class PatrolState {IDLE, PATROLLING, FINISHED, ERROR, DO_AT_WAYPOINT}; + enum class PatrolState {IDLE, PATROLLING, FINISHED, ERROR, DO_AT_WAYPOINT, RESETTING}; PatrolState state_ {PatrolState::IDLE}; bool initialized_ {false}; diff --git a/exercises/easynav/easynav_playground/easynav_patrolling_behavior/src/easynav_patrolling_behavior/PatrollingNode.cpp b/exercises/easynav/easynav_playground/easynav_patrolling_behavior/src/easynav_patrolling_behavior/PatrollingNode.cpp index 2b7c2d2..7a72c06 100644 --- a/exercises/easynav/easynav_playground/easynav_patrolling_behavior/src/easynav_patrolling_behavior/PatrollingNode.cpp +++ b/exercises/easynav/easynav_playground/easynav_patrolling_behavior/src/easynav_patrolling_behavior/PatrollingNode.cpp @@ -109,13 +109,10 @@ PatrollingNode::cycle() case GoalManagerClient::State::SENT_GOAL: send_retries_++; if (send_retries_ >= max_retries_) { - RCLCPP_WARN(get_logger(), "No ACCEPT received after %zu attempts, resending goal...", + RCLCPP_WARN(get_logger(), "No ACCEPT received after %zu attempts, resetting...", max_retries_); send_retries_ = 0; - // Recreate client to reset to IDLE state - gm_client_ = GoalManagerClient::make_shared(shared_from_this()); - last_nav_state_ = GoalManagerClient::State::IDLE; - state_ = PatrolState::IDLE; + state_ = PatrolState::RESETTING; } break; @@ -166,6 +163,17 @@ PatrollingNode::cycle() // END DONE: Workshop task break; + case PatrolState::RESETTING: + { + // For stuck goals that never got accepted, we need to create a new client + // since reset() won't work on non-terminal states + RCLCPP_INFO(get_logger(), "Creating new goal manager client and retrying"); + gm_client_ = GoalManagerClient::make_shared(shared_from_this()); + last_nav_state_ = GoalManagerClient::State::IDLE; + state_ = PatrolState::IDLE; + } + break; + case PatrolState::FINISHED: RCLCPP_INFO(get_logger(), "Reset navigation"); current_goal_index_ = 0; diff --git a/exercises/easynav/easynav_playground/easynav_patrolling_behavior_py/easynav_patrolling_behavior_py/patrolling_node.py b/exercises/easynav/easynav_playground/easynav_patrolling_behavior_py/easynav_patrolling_behavior_py/patrolling_node.py index 37ddf78..b8453d0 100644 --- a/exercises/easynav/easynav_playground/easynav_patrolling_behavior_py/easynav_patrolling_behavior_py/patrolling_node.py +++ b/exercises/easynav/easynav_playground/easynav_patrolling_behavior_py/easynav_patrolling_behavior_py/patrolling_node.py @@ -36,6 +36,7 @@ class PatrolState(Enum): DO_AT_WAYPOINT = 2 FINISHED = 3 ERROR = 4 + RESETTING = 5 def quat_from_yaw(yaw: float) -> Tuple[float, float, float, float]: cy = math.cos(yaw * 0.5) @@ -122,12 +123,9 @@ def _cycle(self): if nav_state == ClientState.SENT_GOAL: self._send_retries += 1 if self._send_retries >= self._max_retries: - self.get_logger().warn(f"No ACCEPT received after {self._max_retries} attempts, resending goal...") + self.get_logger().warn(f"No ACCEPT received after {self._max_retries} attempts, resetting...") self._send_retries = 0 - # Recreate client to reset to IDLE state - self._gm = GoalManagerClient(node=self) - self._last_nav_state = ClientState.IDLE - self._state = PatrolState.IDLE + self._state = PatrolState.RESETTING elif ( nav_state == ClientState.NAVIGATION_REJECTED or nav_state == ClientState.NAVIGATION_FAILED or @@ -166,6 +164,14 @@ def _cycle(self): self.get_logger().info("All waypoints completed") self._state = PatrolState.FINISHED + case PatrolState.RESETTING: + # For stuck goals that never got accepted, we need to create a new client + # since reset() won't work on non-terminal states + self.get_logger().info('Creating new goal manager client and retrying') + self._gm = GoalManagerClient(node=self) + self._last_nav_state = ClientState.IDLE + self._state = PatrolState.IDLE + case PatrolState.FINISHED: self.get_logger().info('Reset navigation') self.current_goal_index = 0