Skip to content

Improve logging and goal sent behavior #8

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
estherag wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Improve logging and goal sent behavior #8

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b913408
Improve logging and goal sent behavior
estherag Nov 19, 2025
b02296c
Add resetting state to isolate resending goal action
estherag Nov 19, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions ...ground/easynav_patrolling_behavior/include/easynav_patrolling_behavior/PatrollingNode.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,13 +51,14 @@ 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};
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_;
Expand Down
48 changes: 28 additions & 20 deletions ...playground/easynav_patrolling_behavior/src/easynav_patrolling_behavior/PatrollingNode.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -99,28 +95,30 @@ 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, resetting...",
max_retries_);
send_retries_ = 0;
state_ = PatrolState::IDLE;
}
} else if (last_control_type_ == easynav_interfaces::msg::NavigationControl::ACCEPT) {
send_retries_ = 0;
state_ = PatrolState::RESETTING;
}
break;

case GoalManagerClient::State::ACCEPTED_AND_NAVIGATING:
break;

case GoalManagerClient::State::NAVIGATION_REJECTED:
case GoalManagerClient::State::NAVIGATION_FAILED:
case GoalManagerClient::State::NAVIGATION_CANCELLED:
Expand All @@ -139,8 +137,7 @@ PatrollingNode::cycle()

state_ = PatrolState::DO_AT_WAYPOINT;
break;
case GoalManagerClient::State::ACCEPTED_AND_NAVIGATING:
break;

default:
break;
}
Expand All @@ -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;
Expand Down
34 changes: 22 additions & 12 deletions ...ayground/easynav_patrolling_behavior_py/easynav_patrolling_behavior_py/patrolling_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -93,7 +94,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()
Expand All @@ -106,24 +108,24 @@ 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, resetting...")
self._send_retries = 0
self._state = PatrolState.RESETTING
elif (
nav_state == ClientState.NAVIGATION_REJECTED or
nav_state == ClientState.NAVIGATION_FAILED or
Expand Down Expand Up @@ -162,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
Expand Down
Loading