Add nodelet implementation.

gmapping/CMakeLists.txt

@@ -17,6 +17,9 @@ if(catkin_EXPORTED_TARGETS)
   add_dependencies(slam_gmapping ${catkin_EXPORTED_TARGETS})
 endif()
 
+add_library(slam_gmapping_nodelet src/slam_gmapping.cpp src/nodelet.cpp)
+target_link_libraries(slam_gmapping_nodelet ${catkin_LIBRARIES})
+
 add_executable(slam_gmapping_replay src/slam_gmapping.cpp src/replay.cpp)
 target_link_libraries(slam_gmapping_replay ${Boost_LIBRARIES} ${catkin_LIBRARIES})
 if(catkin_EXPORTED_TARGETS)

gmapping/nodelet_plugins.xml

@@ -0,0 +1,7 @@
+<library path="lib/libslam_gmapping_nodelet">
+  <class name="SlamGMappingNodelet" type="SlamGMappingNodelet" base_class_type="nodelet::Nodelet">
+  <description>
+  Nodelet ROS wrapper for OpenSlam's Gmapping.
+  </description>
+  </class>
+</library>

gmapping/package.xml

@@ -19,9 +19,15 @@
   <build_depend>roscpp</build_depend>
   <build_depend>rostest</build_depend>
   <build_depend>tf</build_depend>
+  <build_depend>nodelet</build_depend>
 
   <run_depend>nav_msgs</run_depend>
   <run_depend>openslam_gmapping</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>tf</run_depend>
+  <run_depend>nodelet</run_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+  </export>
 </package>

gmapping/src/nodelet.cpp

@@ -0,0 +1,42 @@
+/*
+ * slam_gmapping
+ * Copyright (c) 2008, Willow Garage, Inc.
+ *
+ * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
+ * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
+ * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
+ * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
+ * 
+ * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
+ * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
+ * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
+ * CONDITIONS.
+ *
+ */
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+
+#include "slam_gmapping.h"
+
+class SlamGMappingNodelet : public nodelet::Nodelet
+{
+  public:
+    SlamGMappingNodelet()  {};
+
+    ~SlamGMappingNodelet() {}
+
+    virtual void onInit()
+    {
+      NODELET_INFO_STREAM("Initialising nodelet...");
+      sg_.reset(new SlamGMapping(getNodeHandle(), getPrivateNodeHandle()));
+      NODELET_INFO_STREAM("Starting live SLAM...");
+      sg_->startLiveSlam();
+    };
+
+  private:  
+    boost::shared_ptr<SlamGMapping> sg_;
+};
+
+PLUGINLIB_EXPORT_CLASS(SlamGMappingNodelet, nodelet::Nodelet)

gmapping/src/slam_gmapping.cpp

@@ -134,6 +134,14 @@ SlamGMapping::SlamGMapping():
   init();
 }
 
+SlamGMapping::SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh):
+  map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
+  laser_count_(0),node_(nh), private_nh_(pnh), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
+{
+  seed_ = time(NULL);
+  init();
+}
+
 SlamGMapping::SlamGMapping(long unsigned int seed, long unsigned int max_duration_buffer):
   map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
   laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL),

gmapping/src/slam_gmapping.h

@@ -34,6 +34,7 @@ class SlamGMapping
 {
   public:
     SlamGMapping();
+    SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh);
     SlamGMapping(unsigned long int seed, unsigned long int max_duration_buffer);
     ~SlamGMapping();