MotionPlanningRvizPlugin: Enhanced External ROS Topic API #823

phy25 · 2018-04-05T06:31:48Z

Description

This PR derived from the idea in a ROS Answer question: Provide external topics setting the start state and the goal state in the MotionPlanningRvizPlugin with a RobotState object. This improves the current API in which the state can only be set with the current robot position.

To achieve this, two new topics are subscribed:

/rviz/moveit/update_custom_start_state
/rviz/moveit/update_custom_goal_state

What's more, for the completeness of the external API, stopping command is exposed too:

/rviz/moveit/stop

Finally, the state of allow_external_program UI checkbox can be kept across sessions now, in order to ease the usage. The slot function adds a init check, to prevent uninitialized error.

As I am new to MoveIt! and C++, any feedback is welcome. The feature has been tested locally, and I will complete the following checklist in a week, sorry about that.

Checklist

Required: Code is auto formatted using clang-format
Extended the tutorials / documentation, if necessary reference - Nothing in the docs for now except the joystick; I will not add more docs
Include a screenshot if changing a GUI - N/A
Optional: Created tests, which fail without this PR reference
Optional: Decide if this should be cherry-picked to other current ROS branches (Indigo, Jade, Kinetic)

- /rviz/moveit/update_start_state_RobotState - /rviz/moveit/update_goal_state_RobotState

v4hn

Nice submission, I attached some requests inline.

v4hn · 2018-04-05T10:35:28Z

...tion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h

  ros::Subscriber update_start_state_subscriber_;
  ros::Subscriber update_goal_state_subscriber_;
+  ros::Subscriber update_start_state_RobotState_subscriber_;
+  ros::Subscriber update_goal_state_RobotState_subscriber_;


Could you please change update_*_state_RobotState et al to update_custom_*_state throughout the request?
The datatype does not belong into the identifier.

v4hn · 2018-04-05T10:42:15Z

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp

+  if (first_time_)
+  {
+    return;
+  }


why is this needed here?

At first glance this seems unnecessary.

This is required because when resuming saved UI state (before first_time_ == true), this function will be executed because of the Qt signal, while something required for this doesn't complete loading (I guess), then it will segfault indefinitely. The quick fix applied here:

Don't execute unless first_time_ load completes;

Manually trigger the function after first_time_ load.

Not a neat solution, as you say.
It would be interesting for me to know what goes wrong here. The only thing I can imagine is that planning_display_->getRobotInteraction() is still a nullptr.

Please add a proper comment explaining this reasoning above the condition.

The marker_access_lock_ in the interactive marker is the issue. I cannot think of a nicer fix for now 😞

Thread 1 "rviz" received signal SIGSEGV, Segmentation fault.
__GI___pthread_mutex_lock (mutex=0x158) at ../nptl/pthread_mutex_lock.c:67
#0 __GI___pthread_mutex_lock (mutex=0x158) at ../nptl/pthread_mutex_lock.c:67
#1 0x00007ffff7a8b098 in boost::unique_lockboost::mutex::lock() () from /opt/ros/kinetic/lib/librviz.so
#2 0x00007fff90d87a55 in robot_interaction::RobotInteraction::toggleMoveInteractiveMarkerTopic(bool) ()
from /home/phy/moveit-clone/devel/lib/libmoveit_robot_interaction.so.0.9.11
#3 0x00007fff91ececc6 in moveit_rviz_plugin::MotionPlanningFrame::allowExternalProgramCommunication(bool) ()
from /home/phy/moveit-clone/devel/lib/libmoveit_motion_planning_rviz_plugin_core.so.0.9.11
#4 0x00007ffff1f80d2a in QMetaObject::activate(QObject*, int, int, void**) () from /usr/lib/x86_64-linux-gnu/libQt5Core.so.5
#5 0x00007ffff77fe312 in QAbstractButton::toggled(bool) () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
#6 0x00007ffff7561645 in ?? () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
#7 0x00007ffff7562acc in QAbstractButton::setChecked(bool) () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
#8 0x00007fff91f4113c in moveit_rviz_plugin::MotionPlanningDisplay::load(rviz::Config const&) ()

... which means getRobotInteraction() is not a valid object yet, I guess.
Fair enough. Please add a comment that explains this reasoning above the if here as well as in the lines below that trigger the lazy load.

v4hn · 2018-04-05T10:53:12Z

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp

+
+void MotionPlanningFrame::remoteUpdateStartStateRobotStateCallback(const moveit_msgs::RobotStateConstPtr& msg)
+{
+  moveit_msgs::RobotState msg_no_attached(*msg);


This is the unchanged message right now and thus redundant.

I think it is a good idea though to restrict these start state changes to forbid changes in the attached collision objects.
We don't have that possibility in the RViz GUI either
and it might cause weird state effects in the GUI.

I would propose to add

msg_no_attached.attached_collision_objects.clear(); msg_no_attached.is_diff = true;

after the new variable definition here and below.

phy25 · 2018-04-11T05:33:04Z

Renamed stuff, clang-formatted, and added the collision object remover, and local test looks good. A squash merge is preferred as there are too many commits 🔧.
I will check whether docs update is required later.

v4hn · 2018-04-11T05:54:11Z

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp

+                     &MotionPlanningFrame::remoteUpdateCustomStartStateCallback, this);
+    update_custom_goal_state_subscriber_ =
+        nh.subscribe("/rviz/moveit/update_goal_state_RobotState", 1,
+                     &MotionPlanningFrame::remoteUpdateCustomGoalStateCallback, this);


You forgot to change the topic names (*_custom_*_state), please do that too.

Forgot to push that single commit, sorry.

v4hn

Ok, one last request for change. Otherwise this looks good and I'm happy to include it.

Please also look at clang-format again, the CI failed

v4hn · 2018-04-11T13:03:11Z

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp

+  if (first_time_)
+  {
+    return;
+  }


... which means getRobotInteraction() is not a valid object yet, I guess.
Fair enough. Please add a comment that explains this reasoning above the if here as well as in the lines below that trigger the lazy load.

phy25 · 2018-04-11T15:40:15Z

Got it. Finished.

v4hn · 2018-04-12T13:01:05Z

Awesome, thank you for your endurance 👍

I approve to merge this into kinetic-devel. Now another maintainer can review and merge.

phy25 · 2018-05-06T09:00:48Z

Should I rebase this or someone else is working on this 😂 ?

rhaschke · 2018-05-06T09:01:38Z

@phy25 I'm working on merging this.

- /rviz/moveit/update_custom_start_state - /rviz/moveit/update_custom_goal_state stopping from external: - /rviz/moveit/stop

rhaschke · 2018-05-06T09:05:54Z

Merged after rebase: 83809de, github didn't notice.

- /rviz/moveit/update_custom_start_state - /rviz/moveit/update_custom_goal_state stopping from external: - /rviz/moveit/stop

phy25 added 3 commits April 4, 2018 21:24

Add external update state with RobotState

2a14b20

- /rviz/moveit/update_start_state_RobotState - /rviz/moveit/update_goal_state_RobotState

Save state of allow_external_program in UI

c08853f

Add stop external topic

4635c2b

v4hn requested changes Apr 5, 2018

View reviewed changes

phy25 added 2 commits April 10, 2018 21:22

Change variable name and fix coding style by clang-format

731a7c4

Prevent collision objects being modified by custom remote API + rename

e150a4e

phy25 changed the title ~~MotionPlanningRvizPlugin: Enhanced External ROS Topic API~~ WIP: MotionPlanningRvizPlugin: Enhanced External ROS Topic API Apr 11, 2018

Change topic name

f0349bc

phy25 changed the title ~~WIP: MotionPlanningRvizPlugin: Enhanced External ROS Topic API~~ MotionPlanningRvizPlugin: Enhanced External ROS Topic API Apr 11, 2018

v4hn reviewed Apr 11, 2018

View reviewed changes

v4hn requested changes Apr 11, 2018

View reviewed changes

Add comment about first_time_ and clang-formatted again

2e13b08

v4hn approved these changes Apr 12, 2018

View reviewed changes

fix typo

5cdf4bf

rhaschke force-pushed the ext-RobotState branch from 144c3be to 5cdf4bf Compare May 6, 2018 08:53

rhaschke pushed a commit that referenced this pull request May 6, 2018

rviz plugin: set start/goal RobotState from external (#823)

83809de

- /rviz/moveit/update_custom_start_state - /rviz/moveit/update_custom_goal_state stopping from external: - /rviz/moveit/stop

rhaschke closed this May 6, 2018

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

MotionPlanningRvizPlugin: Enhanced External ROS Topic API #823

MotionPlanningRvizPlugin: Enhanced External ROS Topic API #823

phy25 commented Apr 5, 2018 •

edited

Loading

v4hn left a comment

v4hn Apr 5, 2018

v4hn Apr 5, 2018

phy25 Apr 11, 2018

v4hn Apr 11, 2018

phy25 Apr 11, 2018

v4hn Apr 11, 2018

v4hn Apr 5, 2018

phy25 commented Apr 11, 2018

v4hn Apr 11, 2018

phy25 Apr 11, 2018

v4hn left a comment

v4hn Apr 11, 2018

phy25 commented Apr 11, 2018

v4hn commented Apr 12, 2018

phy25 commented May 6, 2018

rhaschke commented May 6, 2018

rhaschke commented May 6, 2018

MotionPlanningRvizPlugin: Enhanced External ROS Topic API #823

MotionPlanningRvizPlugin: Enhanced External ROS Topic API #823

Conversation

phy25 commented Apr 5, 2018 • edited Loading

Description

Checklist

v4hn left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

phy25 commented Apr 11, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

v4hn left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

phy25 commented Apr 11, 2018

v4hn commented Apr 12, 2018

phy25 commented May 6, 2018

rhaschke commented May 6, 2018

rhaschke commented May 6, 2018

phy25 commented Apr 5, 2018 •

edited

Loading