Add rviz panel to control OctomapServer (trigger reset octomap service)

CHANGELOG.rst

@@ -1,3 +1,7 @@
+0.?.? (2017-??-??)
+------------------
+* Add OctomapServerControl Panel to trigger resetting the octomap from rviz.
+
 0.1.0 (2016-07-07)
 ------------------
 * Remove -ldefault_plugin from linker options, fixes `#19 <https://github.com/OctoMap/octomap_rviz_plugins/issues/19>`_

CMakeLists.txt

@@ -4,7 +4,7 @@ project(octomap_rviz_plugins)
 find_package(catkin REQUIRED COMPONENTS octomap_msgs
                                         roscpp
                                         rviz
-
+                                        std_srvs                                        
 )
 
 find_package(octomap REQUIRED)
@@ -38,17 +38,19 @@ link_directories(${catkin_LIBRARY_DIRS}
 QT4_WRAP_CPP(MOC_FILES
   include/octomap_rviz_plugins/occupancy_grid_display.h
   include/octomap_rviz_plugins/occupancy_map_display.h
+  include/octomap_rviz_plugins/octomap_server_control.h
   OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_LEXICAL_CAST_INCLUDED 
 )
 
 set(SOURCE_FILES
   src/occupancy_grid_display.cpp
   src/occupancy_map_display.cpp
+  src/octomap_server_control.cpp
   ${MOC_FILES} 
 )
 
 add_library(${PROJECT_NAME} ${SOURCE_FILES})
-target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${Boost_LIBRARIES} ${OCTOMAP_LIBRARIES} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${Boost_LIBRARIES} ${OCTOMAP_LIBRARIES} ${catkin_LIBRARIES} ${rviz_DEFAULT_PLUGIN_LIBRARIES})
 
 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
@@ -63,5 +65,3 @@ install(TARGETS ${PROJECT_NAME}
 install(FILES plugin_description.xml
         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
-
-

include/octomap_rviz_plugins/octomap_server_control.h

@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2017, Wolfgang Merkt
+ * 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 the Willow Garage, Inc. 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: Wolfgang Merkt ([email protected])
+ *
+ */
+
+#ifndef RVIZ_OCTOMAP_SERVER_CONTROL_H
+#define RVIZ_OCTOMAP_SERVER_CONTROL_H
+
+#ifndef Q_MOC_RUN
+
+#include <QtGui>
+
+#include <ros/ros.h>
+#include <rviz/panel.h>
+#include <std_srvs/Empty.h>
+
+#endif
+
+namespace octomap_rviz_plugin {
+
+class OctomapServerControl : public rviz::Panel {
+  Q_OBJECT
+
+ public:
+  OctomapServerControl(QWidget* parent = 0);
+
+ protected Q_SLOTS:
+  void resetOctomap();
+
+ protected:
+  QVBoxLayout* vbox_;
+  QPushButton* btnResetOctomap_;
+
+ private:
+  ros::NodeHandle nh_;
+  ros::ServiceClient srvResetOctomap_ =
+      nh_.serviceClient<std_srvs::Empty>("/octomap_server/reset");
+};
+}
+
+#endif

package.xml

@@ -20,11 +20,13 @@
   <build_depend>octomap</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>rviz</build_depend>
+  <build_depend>std_srvs</build_depend>
 
   <run_depend>octomap_msgs</run_depend>
   <run_depend>octomap</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>rviz</run_depend>
+  <run_depend>std_srvs</run_depend>
 
   <export>
     <rviz plugin="${prefix}/plugin_description.xml"/>

plugin_description.xml

@@ -1,4 +1,3 @@
-
 <library path="lib/liboctomap_rviz_plugins">
   <class name="octomap_rviz_plugin/OccupancyGrid"
          type="OcTreeGridDisplay"
@@ -36,5 +35,11 @@
       Displays projected 2D occupancy maps generated from compressed octomap messages.
     </description>
   </class>
+  <class name="octomap_rviz_plugin/OctomapServerControl"
+         type="OctomapServerControl"
+         base_class_type="rviz::Panel">
+    <description>
+      Buttons to trigger OctomapServer services.
+    </description>
+  </class>
 </library>
-

src/octomap_server_control.cpp

@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2017, Wolfgang Merkt
+ * 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 the Willow Garage, Inc. 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: Wolfgang Merkt ([email protected])
+ *
+ */
+
+#include "octomap_rviz_plugins/octomap_server_control.h"
+
+namespace octomap_rviz_plugin {
+OctomapServerControl::OctomapServerControl(QWidget* parent)
+    : rviz::Panel(parent) {
+  vbox_ = new QVBoxLayout();
+
+  btnResetOctomap_ = new QPushButton(tr("Reset Octomap"));
+  btnResetOctomap_->setSizePolicy(QSizePolicy::Expanding,
+                                  QSizePolicy::Expanding);
+
+  vbox_->addWidget(btnResetOctomap_);
+
+  connect(btnResetOctomap_, SIGNAL(clicked()), this, SLOT(resetOctomap()));
+
+  setLayout(vbox_);
+}
+
+void OctomapServerControl::resetOctomap() {
+  std_srvs::Empty req;
+  srvResetOctomap_.call(req);
+}
+}
+
+#include <pluginlib/class_list_macros.h>
+
+typedef octomap_rviz_plugin::OctomapServerControl OctomapServerControl;
+PLUGINLIB_EXPORT_CLASS(OctomapServerControl, rviz::Panel)