Skip to content

Rviz state machine waypoint follower updates #2227

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

Merged
SteveMacenski merged 3 commits into from
Mar 9, 2021
Merged

Rviz state machine waypoint follower updates #2227

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1ba3770
working on canceling state machine for waypoint mode
SteveMacenski Mar 9, 2021
13d03ed
fixing cancelation logic
SteveMacenski Mar 9, 2021
fb894a5
fix linting isue
SteveMacenski Mar 9, 2021
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
33 changes: 28 additions & 5 deletions nav2_rviz_plugins/src/nav2_panel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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();
Expand Down Expand Up @@ -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_);
Expand All @@ -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);

Expand Down Expand Up @@ -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_);

Expand All @@ -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_) !=
Expand Down Expand Up @@ -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");
Expand Down