Refactor NavigateToPose action client into rviz Nav2 panel and expose Cancel/Shutdown by bpwilcox · Pull Request #944 · ros-navigation/navigation2

bpwilcox · 2019-07-17T22:10:13Z

Description of contribution

creates GoalPoseUpdater QObject to emit signal for goal poses. Creates a global object to be accessed in both the nav2 panel and nav2 goal tool
modifiesnav2_rviz_plugin::GoalTool to send goal poses via Qt signal using `GoalPoseUpdater' QObject
adds action client and supporting goal-related functionality to Nav2Panel
the cancel button state has been added to the panel
the shutdown button now transitions to cancel when the NavigateToPose action is active, when the action is over or is canceled by the user, the shutdown button state is again present
- created custom ROSActionEvent and ROSActionTransition to handle Qt button state transitions

mjeronimo · 2019-07-18T21:53:15Z

nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp

 {

+/// Custom Event to track state of ROS Action
+struct ROSActionEvent : public QEvent


Minor nit: I would've split this into its own header/implementation files - ros_action_event.[cpp,h]

mjeronimo · 2019-07-18T21:55:44Z

nav2_rviz_plugins/src/nav2_panel.cpp

+  completedTransition->setTargetState(running_);
+  completed_->addTransition(completedTransition);

  machine_.addState(initial_);


Probably should have called this "state_machine_" instead of "machine_". I haven't looked at this code in a while and was a bit confused for a sec.

mjeronimo · 2019-07-18T23:33:47Z

nav2_rviz_plugins/src/nav2_panel.cpp

+  // State entered while the NavigateToPose action is active
+  running_ = new QState();
+  running_->setObjectName("running");
+  running_->assignProperty(start_stop_button_, "text", "Cancel");


The start_stop_button is not longer just start and stop, but now also cancel. Perhaps rename the variable?

Any name suggestions? You can argue cancel is semantically a stop as well, if you don't mind the term overloading.

Yeah, that's true. Never mind on the original comment. I think it's clear enough as is.

nav2_rviz_plugins/include/nav2_rviz_plugins/goal_tool.hpp

SteveMacenski · 2019-07-19T01:42:01Z

nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp

+  rclcpp::Node::SharedPtr goal_node_;
+
+  // Executor needed to spin node in thread
+  std::unique_ptr<rclcpp::executors::SingleThreadedExecutor> executor_;


Similar question: why does this need to be a ptr?

bpwilcox · 2019-07-19T22:06:37Z

Addressed feedback. I decided to remove the nav2_rviz_plugins::GoalTool because I realized it was going to be no different that the existing rviz_default_plugins::GoalTool since the panel subscribes to the goal topic.

ghost · 2019-07-22T21:41:39Z

@bpwilcox Please change the topic name or something to prevent both the panel and bt_navigator from responding to the topic.

bpwilcox · 2019-07-25T01:02:28Z

I've updated this PR to address feedback from our meeting to not use a subscriber in the nav2 panel to listen for the goal pose. Per @crdelsey's suggestion, I defined a custom global object to allow access between the panel and tool plugins in rviz. This custom QObject allows us to send the goal pose from the goal tool to the panel by emitting a Qsignal with the updated pose.

bpwilcox · 2019-08-07T00:46:14Z

@mjeronimo @crdelsey Can you re-review since the changes I made?

ghost

This could really use some design documentation to go along with it.

ghost · 2019-08-08T16:54:30Z

nav2_rviz_plugins/CMakeLists.txt

 find_package(tf2_geometry_msgs REQUIRED)

 set(nav2_rviz_plugins_headers_to_moc
+  include/nav2_rviz_plugins/goal_pose_updater;


It doesn't seem that these lines should be terminated by a semicolon

nav2_rviz_plugins/src/nav2_panel.cpp

ghost · 2019-08-08T17:01:55Z

nav2_rviz_plugins/src/nav2_panel.cpp

+
+  auto options = rclcpp::NodeOptions().arguments(
+    {"__node:=navigation_dialog_action_client"});
+  client_node_ = std::make_shared<rclcpp::Node>("_", options);


What are you doing here? Why create the node by the name of "_" but then use a argument to rename it?

This wasn't my code but moved from the navigation_dialog.cpp before my refactor. Seemingly, this is done several places in the code (see bt_navigator.cpp). If I recall, this was done to support remapping?

ghost · 2019-08-08T17:14:30Z

nav2_rviz_plugins/src/nav2_panel.cpp

+  if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
+    status == action_msgs::msg::GoalStatus::STATUS_EXECUTING)
+  {
+    state_machine_.postEvent(new ROSActionQEvent(true));


If I understand this correctly, you are posting a bool as an event. Is that correct? That conveys little information. The events should be an enum at a minimum. That way you could post something like "NAV_ACTIVATED" or "NAV_DEACTIVATED"

Same functionality but I suppose an enum indicates it is more specific to actions being active/inactive.

ghost · 2019-08-08T17:15:44Z

nav2_rviz_plugins/src/nav2_panel.cpp

+  // Send the goal pose
+  goal_.pose = pose;
+
+  // Enable result awareness by providing an empty lambda function


What's "result awareness"? Can you elaborate on this comment

I'm not entirely sure as this wasn't my code/comment, just part of refactor, perhaps @mjeronimo could explain better.

"Result awareness" was introduced into the ROS2 action client. There were some relatively recent changes (a couple months ago or so), where one now provides callbacks in the SendGoalOptions for the goal response, feedback, and result. The SendGoalOptions are then passed to the client's async_send_goal method. In the navigation stack we already had code written to the old style of explicitly sending the goal request, etc. This SendGoalOptions change broke our code. One way I found to work around it was to provide an empty callback for the result_callback. Providing this callback causes the ROS2 to set the result aware flag (whatever that means) and allows us to call the existing functions. Alternatively, we could update our client code to conform to the new callback model. See the ROS2 code in rclcpp_action for more about "result awareness." I haven't found any docs.

bpwilcox · 2019-08-08T19:04:59Z

Here's a state diagram for the transitions of the rviz panel button.

bpwilcox · 2019-08-08T20:17:26Z

Here's a class diagram for the nav2 rviz plugins:

So now, the sequence from sending a goal position becomes:

codecov · 2019-08-08T20:49:45Z

Codecov Report

Merging #944 into master will decrease coverage by 5.28%.
The diff coverage is n/a.

@@            Coverage Diff             @@
##           master     #944      +/-   ##
==========================================
- Coverage   33.71%   28.43%   -5.29%     
==========================================
  Files         197      196       -1     
  Lines       10241    10006     -235     
  Branches     4038     3226     -812     
==========================================
- Hits         3453     2845     -608     
- Misses       4466     5397     +931     
+ Partials     2322     1764     -558

Impacted Files	Coverage Δ
...ns/include/dwb_plugins/standard_traj_generator.hpp	`0% <0%> (-100%)`	⬇️
...ller/dwb_critics/include/dwb_critics/goal_dist.hpp	`0% <0%> (-100%)`	⬇️
...ller/dwb_critics/include/dwb_critics/path_dist.hpp	`0% <0%> (-100%)`	⬇️
...ugins/include/dwb_plugins/kinematic_parameters.hpp	`0% <0%> (-100%)`	⬇️
...dwb_critics/include/dwb_critics/rotate_to_goal.hpp	`0% <0%> (-100%)`	⬇️
.../dwb_critics/include/dwb_critics/base_obstacle.hpp	`0% <0%> (-100%)`	⬇️
..._2d/include/nav2_costmap_2d/observation_buffer.hpp	`0% <0%> (-100%)`	⬇️
...oller/dwb_critics/include/dwb_critics/map_grid.hpp	`0% <0%> (-85.72%)`	⬇️
...nav2_dwb_controller/dwb_critics/src/path_align.cpp	`0% <0%> (-85%)`	⬇️
.../nav2_dwb_controller/nav_2d_utils/src/path_ops.cpp	`0% <0%> (-77.78%)`	⬇️
... and 72 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 7fa7400...10caf3a. Read the comment docs.

mjeronimo

I'm fine with the code pending the few minor changes I mentioned in the latest review.

mjeronimo · 2019-08-09T22:27:21Z

nav2_rviz_plugins/src/nav2_panel.cpp

 #include <QPushButton>
 #include <QtConcurrent/QtConcurrent>
 #include <QVBoxLayout>
+#include <QEventTransition>


Some of these headers are no longer needed and can be removed (dirent, QHBoxLayout, QLabel, QEventTransition).

mjeronimo · 2019-08-09T22:28:58Z

nav2_rviz_plugins/src/nav2_panel.cpp

+    this, SLOT(onNewGoal(double,double,double,QString)));  // NOLINT
+
+  // Launch a thread to run the node
+  thread_ = std::make_unique<std::thread>(


You could use a QBasicTimer (see the old code) instead of creating a node just to create a timer.

mjeronimo · 2019-08-09T22:30:22Z

nav2_rviz_plugins/src/nav2_panel.cpp

+      executor_.remove_node(node->get_node_base_interface());
+    }, timer_node_);
+
+  timer_ = timer_node_->create_wall_timer(1s, std::bind(&Nav2Panel::timerActionEvent, this));


You could check more frequently than once per second. This would make it more responsive to detecting the completed navigation at little expense. For example, running the callback at 5Hz should be negligible.

mjeronimo · 2019-08-09T22:45:32Z

nav2_rviz_plugins/src/nav2_panel.cpp

+  // Send the goal pose
+  goal_.pose = pose;
+
+  // Enable result awareness by providing an empty lambda function


"Result awareness" was introduced into the ROS2 action client. There were some relatively recent changes (a couple months ago or so), where one now provides callbacks in the SendGoalOptions for the goal response, feedback, and result. The SendGoalOptions are then passed to the client's async_send_goal method. In the navigation stack we already had code written to the old style of explicitly sending the goal request, etc. This SendGoalOptions change broke our code. One way I found to work around it was to provide an empty callback for the result_callback. Providing this callback causes the ROS2 to set the result aware flag (whatever that means) and allows us to call the existing functions. Alternatively, we could update our client code to conform to the new callback model. See the ROS2 code in rclcpp_action for more about "result awareness." I haven't found any docs.

clean up remove extra log info

minor clean up

fix compile issues remove extra semicolons fix uncrustify

cleanup timer

bpwilcox requested review from a user, mhpanah, mjeronimo and orduno July 17, 2019 22:10

bpwilcox changed the title ~~Refactor NavigateToPose action client into Nav2 panel and expose Cancel/Shutdown~~ Refactor NavigateToPose action client into rviz Nav2 panel and expose Cancel/Shutdown Jul 17, 2019

mjeronimo approved these changes Jul 18, 2019

View reviewed changes

SteveMacenski reviewed Jul 19, 2019

View reviewed changes

bpwilcox force-pushed the rviz_panel_actions_update branch from 1152b58 to b9e2b23 Compare July 19, 2019 22:00

SteveMacenski approved these changes Jul 19, 2019

View reviewed changes

ghost removed request for mhpanah and orduno July 22, 2019 22:31

ghost added the do not merge label Jul 22, 2019

bpwilcox force-pushed the rviz_panel_actions_update branch 2 times, most recently from de8cfce to a247cbf Compare July 25, 2019 00:50

bpwilcox requested review from mjeronimo and mkhansenbot July 25, 2019 00:55

bpwilcox removed the do not merge label Jul 25, 2019

bpwilcox force-pushed the rviz_panel_actions_update branch from a247cbf to cbb04be Compare August 7, 2019 00:44

bpwilcox mentioned this pull request Aug 7, 2019

spin up thread for goal tool to not block frontend #1004

Closed

ghost reviewed Aug 8, 2019

View reviewed changes

ghost approved these changes Aug 8, 2019

View reviewed changes

bpwilcox force-pushed the rviz_panel_actions_update branch from cbb04be to 1540ec5 Compare August 8, 2019 20:49

bpwilcox force-pushed the rviz_panel_actions_update branch from 1540ec5 to 42d18cc Compare August 8, 2019 20:52

bpwilcox force-pushed the rviz_panel_actions_update branch from 42d18cc to e4e73e4 Compare August 8, 2019 21:10

mjeronimo approved these changes Aug 9, 2019

View reviewed changes

bpwilcox added 7 commits August 13, 2019 10:50

refactor goal handle into panel

c16844e

clean up remove extra log info

remove nav2 goal tool and address review feedback

07e7a5e

fix lint issue

4d2fbb4

send pose via qt signal through goal tool

3bf47f8

remove navigation dialog

9e31a4e

minor clean up

use enum for action states

0a46d42

fix compile issues remove extra semicolons fix uncrustify

add QBasicTimer

c36e90b

cleanup timer

bpwilcox force-pushed the rviz_panel_actions_update branch from e4e73e4 to c36e90b Compare August 13, 2019 18:06

remove more unnecessary headers

10caf3a

bpwilcox merged commit a587327 into ros-navigation:master Aug 13, 2019

mlherd mentioned this pull request Aug 24, 2019

[Rviz Plugins] Navigation2 Goal Tool hangs rviz for a few seconds #1002

Closed

bpwilcox mentioned this pull request Aug 28, 2019

changing pose topics to goal_pose #1071

Merged

Conversation

bpwilcox commented Jul 17, 2019 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description of contribution

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

bpwilcox commented Jul 19, 2019

Uh oh!

ghost commented Jul 22, 2019

Uh oh!

bpwilcox commented Jul 25, 2019

Uh oh!

bpwilcox commented Aug 7, 2019

Uh oh!

ghost left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

bpwilcox commented Aug 8, 2019

Uh oh!

bpwilcox commented Aug 8, 2019 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

codecov bot commented Aug 8, 2019 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

mjeronimo left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

bpwilcox commented Jul 17, 2019 •

edited

Loading

bpwilcox commented Aug 8, 2019 •

edited

Loading

codecov bot commented Aug 8, 2019 •

edited

Loading