Added move_group capability for clearing octomap.

moveit_ros/.gitignore

@@ -1,3 +1,4 @@
+*~
 ~$
 .swp$
 build/

moveit_ros/move_group/CMakeLists.txt

@@ -53,6 +53,7 @@ add_library(moveit_move_group_default_capabilities
   src/default_capabilities/state_validation_service_capability.cpp
   src/default_capabilities/cartesian_path_service_capability.cpp
   src/default_capabilities/get_planning_scene_service_capability.cpp
+  src/default_capabilities/clear_octomap_service_capability.cpp
   )
 
 

moveit_ros/move_group/default_capabilities_plugin_description.xml

@@ -48,4 +48,10 @@
     </description>
   </class>
 
+  <class name="move_group/ClearOctomapService" type="move_group::ClearOctomapService" base_class_type="move_group::MoveGroupCapability">
+    <description>
+      Provide a ROS service that allows for clearing the octomap
+    </description>
+  </class>
+
 </library>

moveit_ros/move_group/include/moveit/move_group/capability_names.h

@@ -51,6 +51,7 @@ static const std::string FK_SERVICE_NAME = "compute_fk"; // name of fk service
 static const std::string STATE_VALIDITY_SERVICE_NAME = "check_state_validity"; // name of the service that validates states
 static const std::string CARTESIAN_PATH_SERVICE_NAME = "compute_cartesian_path"; // name of the service that computes cartesian paths
 static const std::string GET_PLANNING_SCENE_SERVICE_NAME = "get_planning_scene"; // name of the service that can be used to query the planning scene
+static const std::string CLEAR_OCTOMAP_SERVICE_NAME = "clear_octomap"; // name of the service that can be used to clear the octomap
 
 }
 

moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp

@@ -0,0 +1,66 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, SRI, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: David Hershberger */
+
+#include "clear_octomap_service_capability.h"
+#include <moveit/move_group/capability_names.h>
+
+move_group::ClearOctomapService::ClearOctomapService():
+  MoveGroupCapability("ExecutePathService")
+{
+}
+
+void move_group::ClearOctomapService::initialize()
+{
+  service_ = root_node_handle_.advertiseService(CLEAR_OCTOMAP_SERVICE_NAME, &ClearOctomapService::clearOctomap, this);
+}
+
+bool move_group::ClearOctomapService::clearOctomap(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
+{
+  if (!context_->planning_scene_monitor_)
+  {
+    ROS_ERROR("Cannot clear octomap since planning_scene_monitor_ does not exist.");
+    return true;
+  }
+
+  ROS_INFO("Clearing octomap...");
+  context_->planning_scene_monitor_->clearOctomap();
+  ROS_INFO("Octomap cleared.");
+  return true;
+}
+
+
+#include <class_loader/class_loader.h>
+CLASS_LOADER_REGISTER_CLASS(move_group::ClearOctomapService, move_group::MoveGroupCapability)

moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h

@@ -0,0 +1,63 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, SRI, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: David Hershberger */
+
+#ifndef MOVEIT_MOVE_GROUP_CLEAR_OCTOMAP_SERVICE_CAPABILITY_
+#define MOVEIT_MOVE_GROUP_CLEAR_OCTOMAP_SERVICE_CAPABILITY_
+
+#include <moveit/move_group/move_group_capability.h>
+#include <std_srvs/Empty.h>
+
+namespace move_group
+{
+
+class ClearOctomapService : public MoveGroupCapability
+{
+public:
+
+  ClearOctomapService();
+
+  virtual void initialize();
+
+private:
+
+  bool clearOctomap(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
+
+  ros::ServiceServer service_;
+};
+
+}
+
+#endif // MOVEIT_MOVE_GROUP_CLEAR_OCTOMAP_SERVICE_CAPABILITY_

moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h

@@ -347,6 +347,8 @@ class PlanningSceneMonitor : private boost::noncopyable
   /** \brief Lock the scene from writing (only one thread can lock for writing and no other thread can lock for reading) */
   void unlockSceneWrite();
 
+  void clearOctomap();
+
 protected:
 
   /** @brief Initialize the planning scene monitor

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -447,6 +447,13 @@ void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneCallback(cons
   newPlanningSceneMessage(*scene);
 }
 
+void planning_scene_monitor::PlanningSceneMonitor::clearOctomap()
+{
+  octomap_monitor_->getOcTreePtr()->lockWrite();
+  octomap_monitor_->getOcTreePtr()->clear();
+  octomap_monitor_->getOcTreePtr()->unlockWrite();
+}
+
 void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene)
 {
   if (scene_)

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

@@ -142,6 +142,7 @@ private Q_SLOTS:
   void pathConstraintsIndexChanged(int index);
   void useStartStateButtonClicked();
   void useGoalStateButtonClicked();
+  void onClearOctomapClicked();
 
   //Scene Objects tab
   void importFileButtonClicked();
@@ -278,7 +279,7 @@ private Q_SLOTS:
   std::vector< std::pair<std::string, bool> > known_collision_objects_;
   long unsigned int known_collision_objects_version_;
   bool first_time_;
-
+  ros::ServiceClient clear_octomap_service_client_;
 };
 
 // \todo THIS IS REALLY BAD. NEED TO MOVE THIS AND RELATED FUNCTIONALITY OUT OF HERE

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp

@@ -36,12 +36,15 @@
 
 #include <moveit/motion_planning_rviz_plugin/motion_planning_frame.h>
 #include <moveit/motion_planning_rviz_plugin/motion_planning_display.h>
+#include <moveit/move_group/capability_names.h>
 
 #include <geometric_shapes/shape_operations.h>
 
 #include <rviz/display_context.h>
 #include <rviz/frame_manager.h>
 
+#include <std_srvs/Empty.h>
+
 #include <QMessageBox>
 #include <QInputDialog>
 #include <QShortcut>
@@ -56,7 +59,8 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay *pdisplay, rviz::
   planning_display_(pdisplay),
   context_(context),
   ui_(new Ui::MotionPlanningUI()),
-  first_time_(true)
+  first_time_(true),
+  clear_octomap_service_client_(nh_.serviceClient<std_srvs::Empty>(move_group::CLEAR_OCTOMAP_SERVICE_NAME))
 {
   // set up the GUI
   ui_->setupUi(this);
@@ -96,6 +100,7 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay *pdisplay, rviz::
   connect( ui_->collision_objects_list, SIGNAL( itemSelectionChanged() ), this, SLOT( selectedCollisionObjectChanged() ));
   connect( ui_->collision_objects_list, SIGNAL( itemChanged( QListWidgetItem * ) ), this, SLOT( collisionObjectChanged( QListWidgetItem * ) ));
   connect( ui_->path_constraints_combo_box, SIGNAL( currentIndexChanged ( int ) ), this, SLOT( pathConstraintsIndexChanged( int ) ));
+  connect( ui_->clear_octomap_button, SIGNAL( clicked() ), this, SLOT( onClearOctomapClicked() ));
   connect( ui_->planning_scene_tree, SIGNAL( itemChanged( QTreeWidgetItem *, int ) ), this, SLOT( warehouseItemNameChanged( QTreeWidgetItem *, int ) ));
   connect( ui_->reset_db_button, SIGNAL( clicked() ), this, SLOT( resetDbButtonClicked() ));
   connect( ui_->export_scene_text_button, SIGNAL( clicked() ), this, SLOT( exportAsTextButtonClicked() ));

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp

@@ -89,6 +89,7 @@ void MotionPlanningFrame::clearSceneButtonClicked()
     planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
     planning_display_->queueRenderSceneGeometry();
   }
+//  publishSceneButtonClicked();
 }
 
 void MotionPlanningFrame::sceneScaleChanged(int value)
@@ -155,6 +156,7 @@ void MotionPlanningFrame::removeObjectButtonClicked()
     planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
     planning_display_->queueRenderSceneGeometry();
   }
+//  publishSceneButtonClicked();
 }
 
 static QString decideStatusText(const collision_detection::CollisionWorld::ObjectConstPtr &obj)
@@ -335,6 +337,7 @@ void MotionPlanningFrame::updateCollisionObjectPose(bool update_marker_position)
       }
     }
   }
+//  publishSceneButtonClicked();
 }
 
 void MotionPlanningFrame::collisionObjectChanged(QListWidgetItem *item)
@@ -935,6 +938,7 @@ void MotionPlanningFrame::computeImportFromText(const std::string &path)
     else
       ROS_WARN("Unable to load scene geometry from '%s'", path.c_str());
   }
+//  publishSceneButtonClicked();
 }
 
 void MotionPlanningFrame::importFromTextButtonClicked()

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp

@@ -40,6 +40,8 @@
 #include <moveit/kinematic_constraints/utils.h>
 #include <moveit/robot_state/conversions.h>
 
+#include <std_srvs/Empty.h>
+
 #include "ui_motion_planning_rviz_plugin_frame.h"
 
 namespace moveit_rviz_plugin
@@ -90,6 +92,12 @@ void MotionPlanningFrame::pathConstraintsIndexChanged(int index)
   }
 }
 
+void MotionPlanningFrame::onClearOctomapClicked()
+{
+  std_srvs::Empty srv;
+  clear_octomap_service_client_.call(srv);
+}
+
 void MotionPlanningFrame::computePlanButtonClicked()
 {
   if (!move_group_)

moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui

@@ -662,6 +662,13 @@
             </item>
            </layout>
           </item>
+          <item>
+           <widget class="QPushButton" name="clear_octomap_button">
+            <property name="text">
+             <string>Clear octomap</string>
+            </property>
+           </widget>
+          </item>
           <item>
            <spacer name="verticalSpacer_2">
             <property name="orientation">