Rviz state machine waypoint follower updates (#2227)

* working on canceling state machine for waypoint mode

* fixing cancelation logic

* fix linting isue

Author: SteveMacenski

nav2_rviz_plugins/src/nav2_panel.cpp

@@ -118,7 +118,7 @@ Nav2Panel::Nav2Panel(QWidget * parent)
   // State entered when navigate_to_pose action is not active
   accumulating_ = new QState();
   accumulating_->setObjectName("accumulating");
-  accumulating_->assignProperty(start_reset_button_, "text", "Reset");
+  accumulating_->assignProperty(start_reset_button_, "text", "Cancel Waypoint Mode");
   accumulating_->assignProperty(start_reset_button_, "toolTip", cancel_waypoint_msg);
   accumulating_->assignProperty(start_reset_button_, "enabled", true);
 
@@ -131,6 +131,18 @@ Nav2Panel::Nav2Panel(QWidget * parent)
   accumulating_->assignProperty(navigation_mode_button_, "toolTip", waypoint_goal_msg);
 
   accumulated_ = new QState();
+  accumulated_->setObjectName("accumulated");
+  accumulated_->assignProperty(start_reset_button_, "text", "Cancel");
+  accumulated_->assignProperty(start_reset_button_, "toolTip", cancel_msg);
+  accumulated_->assignProperty(start_reset_button_, "enabled", true);
+
+  accumulated_->assignProperty(pause_resume_button_, "text", "Pause");
+  accumulated_->assignProperty(pause_resume_button_, "enabled", false);
+  accumulated_->assignProperty(pause_resume_button_, "toolTip", pause_msg);
+
+  accumulated_->assignProperty(navigation_mode_button_, "text", "Start Navigation");
+  accumulated_->assignProperty(navigation_mode_button_, "enabled", false);
+  accumulated_->assignProperty(navigation_mode_button_, "toolTip", waypoint_goal_msg);
 
   // State entered to cancel the navigate_to_pose action
   canceled_ = new QState();
@@ -186,12 +198,12 @@ Nav2Panel::Nav2Panel(QWidget * parent)
   idle_->addTransition(navigation_mode_button_, SIGNAL(clicked()), accumulating_);
   accumulating_->addTransition(navigation_mode_button_, SIGNAL(clicked()), accumulated_);
   accumulating_->addTransition(start_reset_button_, SIGNAL(clicked()), idle_);
+  accumulated_->addTransition(start_reset_button_, SIGNAL(clicked()), canceled_);
 
   // Internal state transitions
   canceled_->addTransition(canceled_, SIGNAL(entered()), idle_);
   reset_->addTransition(reset_, SIGNAL(entered()), initial_);
   resumed_->addTransition(resumed_, SIGNAL(entered()), idle_);
-  accumulated_->addTransition(accumulated_, SIGNAL(entered()), idle_);
 
   // Pause/Resume button click transitions
   idle_->addTransition(pause_resume_button_, SIGNAL(clicked()), paused_);
@@ -206,6 +218,15 @@ Nav2Panel::Nav2Panel(QWidget * parent)
   runningTransition->setTargetState(idle_);
   running_->addTransition(runningTransition);
 
+  ROSActionQTransition * idleAccumulatedTransition =
+    new ROSActionQTransition(QActionState::INACTIVE);
+  idleAccumulatedTransition->setTargetState(accumulated_);
+  idle_->addTransition(idleAccumulatedTransition);
+
+  ROSActionQTransition * accumulatedTransition = new ROSActionQTransition(QActionState::ACTIVE);
+  accumulatedTransition->setTargetState(idle_);
+  accumulated_->addTransition(accumulatedTransition);
+
   initial_thread_ = new InitialThread(client_nav_, client_loc_);
   connect(initial_thread_, &InitialThread::finished, initial_thread_, &QObject::deleteLater);
 
@@ -408,7 +429,7 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame)
 void
 Nav2Panel::onCancelButtonPressed()
 {
-  if (state_machine_.configuration().contains(accumulating_)) {
+  if (waypoint_follower_goal_handle_) {
     auto future_cancel =
       waypoint_follower_action_client_->async_cancel_goal(waypoint_follower_goal_handle_);
 
@@ -418,7 +439,9 @@ Nav2Panel::onCancelButtonPressed()
       RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower");
       return;
     }
-  } else {
+  }
+
+  if (navigation_goal_handle_) {
     auto future_cancel = navigation_action_client_->async_cancel_goal(navigation_goal_handle_);
 
     if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) !=
@@ -449,7 +472,7 @@ Nav2Panel::onAccumulating()
 void
 Nav2Panel::timerEvent(QTimerEvent * event)
 {
-  if (state_machine_.configuration().contains(accumulating_)) {
+  if (state_machine_.configuration().contains(accumulated_)) {
     if (event->timerId() == timer_.timerId()) {
       if (!waypoint_follower_goal_handle_) {
         RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal");