diff --git a/gmapping/CMakeLists.txt b/gmapping/CMakeLists.txt index e2236f38..209d4f3f 100644 --- a/gmapping/CMakeLists.txt +++ b/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) diff --git a/gmapping/nodelet_plugins.xml b/gmapping/nodelet_plugins.xml new file mode 100644 index 00000000..5d7f34dd --- /dev/null +++ b/gmapping/nodelet_plugins.xml @@ -0,0 +1,7 @@ + + + + Nodelet ROS wrapper for OpenSlam's Gmapping. + + + diff --git a/gmapping/package.xml b/gmapping/package.xml index 8fb09bed..59939dfb 100644 --- a/gmapping/package.xml +++ b/gmapping/package.xml @@ -19,9 +19,15 @@ roscpp rostest tf + nodelet nav_msgs openslam_gmapping roscpp tf + nodelet + + + + diff --git a/gmapping/src/nodelet.cpp b/gmapping/src/nodelet.cpp new file mode 100644 index 00000000..8c043a96 --- /dev/null +++ b/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 +#include +#include + +#include "slam_gmapping.h" + +class SlamGMappingNodelet : public nodelet::Nodelet +{ + public: + SlamGMappingNodelet() {} + + ~SlamGMappingNodelet() {} + + virtual void onInit() + { + NODELET_INFO_STREAM("Initialising Slam GMapping nodelet..."); + sg_.reset(new SlamGMapping(getNodeHandle(), getPrivateNodeHandle())); + NODELET_INFO_STREAM("Starting live SLAM..."); + sg_->startLiveSlam(); + } + + private: + boost::shared_ptr sg_; +}; + +PLUGINLIB_EXPORT_CLASS(SlamGMappingNodelet, nodelet::Nodelet) diff --git a/gmapping/src/slam_gmapping.cpp b/gmapping/src/slam_gmapping.cpp index 8c1f0944..bd9977c3 100644 --- a/gmapping/src/slam_gmapping.cpp +++ b/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), diff --git a/gmapping/src/slam_gmapping.h b/gmapping/src/slam_gmapping.h index 2ecb0255..ae622b95 100644 --- a/gmapping/src/slam_gmapping.h +++ b/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();