Merge PR #2198: Allow adding planning scene shapes from rviz panel

Add primitive shapes widgets

moveit_ros/visualization/CMakeLists.txt

@@ -48,21 +48,22 @@ find_package(Eigen3 REQUIRED)
 
 # Qt Stuff
 if(rviz_QT_VERSION VERSION_LESS "5")
-	find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui)
-	include(${QT_USE_FILE})
+  find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui)
+  include(${QT_USE_FILE})
   macro(qt_wrap_ui)
     qt4_wrap_ui(${ARGN})
   endmacro()
 else()
-	find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets)
-	set(QT_LIBRARIES Qt5::Widgets)
+  find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets)
+  set(QT_LIBRARIES Qt5::Widgets)
   macro(qt_wrap_ui)
     qt5_wrap_ui(${ARGN})
   endmacro()
 endif()
 
 set(CMAKE_INCLUDE_CURRENT_DIR ON)
 set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTORCC ON)
 add_definitions(-DQT_NO_KEYWORDS)
 
 catkin_package(

moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt

@@ -23,6 +23,7 @@ set(SOURCE_FILES
   src/motion_planning_display.cpp
   src/motion_planning_frame_manipulation.cpp
   src/motion_planning_param_widget.cpp
+  src/icons/icons.qrc
 )
 
 set(MOVEIT_LIB_NAME moveit_motion_planning_rviz_plugin)

moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h

@@ -134,6 +134,7 @@ class MotionPlanningDisplay : public PlanningSceneDisplay
   void setQueryStartState(const moveit::core::RobotState& start);
   void setQueryGoalState(const moveit::core::RobotState& goal);
 
+  void updateQueryStates(const moveit::core::RobotState& current_state);
   void updateQueryStartState();
   void updateQueryGoalState();
   void rememberPreviousStartState();

moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h

@@ -151,7 +151,6 @@ private Q_SLOTS:
 
   // Context tab
   void databaseConnectButtonClicked();
-  void publishSceneButtonClicked();
   void planningAlgorithmIndexChanged(int index);
   void resetDbButtonClicked();
   void approximateIKChanged(int state);
@@ -174,13 +173,17 @@ private Q_SLOTS:
   void onClearOctomapClicked();
 
   // Scene Objects tab
-  void importObjectFromFileButtonClicked();
-  void importObjectFromUrlButtonClicked();
-  void clearSceneButtonClicked();
+  void clearScene();
+  void publishScene();
+  void publishSceneIfNeeded();
+  void setLocalSceneEdited(bool dirty = true);
+  bool isLocalSceneDirty() const;
   void sceneScaleChanged(int value);
   void sceneScaleStartChange();
   void sceneScaleEndChange();
-  void removeObjectButtonClicked();
+  void shapesComboBoxChanged(const QString& text);
+  void addSceneObject();
+  void removeSceneObject();
   void selectedCollisionObjectChanged();
   void objectPoseValueChanged(double value);
   void collisionObjectChanged(QListWidgetItem* item);
@@ -244,8 +247,6 @@ private Q_SLOTS:
   void goalStateTextChangedExec(const std::string& goal_state);
 
   // Scene objects tab
-  void addObject(const collision_detection::WorldPtr& world, const std::string& id, const shapes::ShapeConstPtr& shape,
-                 const Eigen::Isometry3d& pose);
   void updateCollisionObjectPose(bool update_marker_position);
   void createSceneInteractiveMarker();
   void renameCollisionObject(QListWidgetItem* item);
@@ -306,7 +307,7 @@ private Q_SLOTS:
   ros::Subscriber update_custom_goal_state_subscriber_;
   // General
   void changePlanningGroupHelper();
-  void importResource(const std::string& path);
+  shapes::ShapePtr loadMeshResource(const std::string& url);
   void loadStoredStates(const std::string& pattern);
 
   void remotePlanCallback(const std_msgs::EmptyConstPtr& msg);

moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/icons.qrc

@@ -0,0 +1,7 @@
+<RCC>
+    <qresource prefix="/icons">
+        <file>list-add.png</file>
+        <file>list-remove.png</file>
+        <file>edit-clear.png</file>
+    </qresource>
+</RCC>

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp

@@ -1212,11 +1212,8 @@ void MotionPlanningDisplay::updateStateExceptModified(moveit::core::RobotState&
   dest = src_copy;
 }
 
-void MotionPlanningDisplay::onSceneMonitorReceivedUpdate(
-    planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
+void MotionPlanningDisplay::updateQueryStates(const moveit::core::RobotState& current_state)
 {
-  PlanningSceneDisplay::onSceneMonitorReceivedUpdate(update_type);
-  moveit::core::RobotState current_state = getPlanningSceneRO()->getCurrentState();
   std::string group = planning_group_property_->getStdString();
 
   if (query_start_state_ && query_start_state_property_->getBool() && !group.empty())
@@ -1232,7 +1229,13 @@ void MotionPlanningDisplay::onSceneMonitorReceivedUpdate(
     updateStateExceptModified(goal, current_state);
     setQueryGoalState(goal);
   }
+}
 
+void MotionPlanningDisplay::onSceneMonitorReceivedUpdate(
+    planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
+{
+  PlanningSceneDisplay::onSceneMonitorReceivedUpdate(update_type);
+  updateQueryStates(getPlanningSceneRO()->getCurrentState());
   if (frame_)
     frame_->sceneUpdate(update_type);
 }