From d33d17de96399bb98d91779891fee19bbd196df4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Mon, 26 Jun 2017 22:05:58 +0200 Subject: [PATCH 1/4] Support using urdf static transforms in replay node Controlled via a new command line argument, --urdf_filename. --- gmapping/CMakeLists.txt | 15 ++++++++++++-- gmapping/package.xml | 4 +++- gmapping/src/replay.cpp | 6 ++++-- gmapping/src/slam_gmapping.cpp | 12 ++++++++++- gmapping/src/slam_gmapping.h | 2 +- gmapping/src/urdf_reader.cpp | 38 ++++++++++++++++++++++++++++++++++ gmapping/src/urdf_reader.h | 6 ++++++ 7 files changed, 76 insertions(+), 7 deletions(-) create mode 100644 gmapping/src/urdf_reader.cpp create mode 100644 gmapping/src/urdf_reader.h diff --git a/gmapping/CMakeLists.txt b/gmapping/CMakeLists.txt index 209d4f3f..732f8e18 100644 --- a/gmapping/CMakeLists.txt +++ b/gmapping/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8) project(gmapping) -find_package(catkin REQUIRED nav_msgs openslam_gmapping roscpp rostest tf rosbag_storage) +find_package(catkin REQUIRED nav_msgs openslam_gmapping roscpp rostest tf rosbag_storage urdf) find_package(Boost REQUIRED signals) @@ -9,6 +9,17 @@ include_directories(${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) include_directories(src) +find_package(urdfdom_headers REQUIRED) +if(DEFINED urdfdom_headers_VERSION) + if(${urdfdom_headers_VERSION} GREATER 0.4.1) + add_definitions(-DURDFDOM_HEADERS_HAS_SHARED_PTR_DEFS) + endif() +endif() + +include_directories( + ${urdfdom_headers_INCLUDE_DIRS} +) + catkin_package() add_executable(slam_gmapping src/slam_gmapping.cpp src/main.cpp) @@ -20,7 +31,7 @@ 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) +add_executable(slam_gmapping_replay src/slam_gmapping.cpp src/replay.cpp src/urdf_reader.cpp) target_link_libraries(slam_gmapping_replay ${Boost_LIBRARIES} ${catkin_LIBRARIES}) if(catkin_EXPORTED_TARGETS) add_dependencies(slam_gmapping_replay ${catkin_EXPORTED_TARGETS}) diff --git a/gmapping/package.xml b/gmapping/package.xml index 59939dfb..7dd12dc8 100644 --- a/gmapping/package.xml +++ b/gmapping/package.xml @@ -20,13 +20,15 @@ rostest tf nodelet + urdf nav_msgs openslam_gmapping roscpp tf nodelet - + urdf + diff --git a/gmapping/src/replay.cpp b/gmapping/src/replay.cpp index f2c71b86..99ffd37e 100644 --- a/gmapping/src/replay.cpp +++ b/gmapping/src/replay.cpp @@ -32,7 +32,8 @@ main(int argc, char** argv) desc.add_options() ("help", "Print help messages") ("scan_topic", po::value()->default_value("/scan") ,"topic that contains the laserScan in the rosbag") - ("bag_filename", po::value()->required(), "ros bag filename") + ("bag_filename", po::value()->required(), "ros bag filename") + ("urdf_filename", po::value()->default_value(std::string()), "urdf filename") ("seed", po::value()->default_value(0), "seed") ("max_duration_buffer", po::value()->default_value(99999), "max tf buffer duration") ("on_done", po::value(), "command to execute when done") ; @@ -63,13 +64,14 @@ main(int argc, char** argv) } std::string bag_fname = vm["bag_filename"].as(); + std::string urdf_fname = vm["urdf_filename"].as(); std::string scan_topic = vm["scan_topic"].as(); unsigned long int seed = vm["seed"].as(); unsigned long int max_duration_buffer = vm["max_duration_buffer"].as(); ros::init(argc, argv, "slam_gmapping"); SlamGMapping gn(seed, max_duration_buffer) ; - gn.startReplay(bag_fname, scan_topic); + gn.startReplay(bag_fname, urdf_fname, scan_topic); ROS_INFO("replay stopped."); if ( vm.count("on_done") ) diff --git a/gmapping/src/slam_gmapping.cpp b/gmapping/src/slam_gmapping.cpp index bd9977c3..6d234787 100644 --- a/gmapping/src/slam_gmapping.cpp +++ b/gmapping/src/slam_gmapping.cpp @@ -106,6 +106,7 @@ Initial map dimensions and resolution: #include "slam_gmapping.h" +#include "urdf_reader.h" #include @@ -114,6 +115,7 @@ Initial map dimensions and resolution: #include "ros/ros.h" #include "ros/console.h" #include "nav_msgs/MapMetaData.h" +#include "tf2_ros/static_transform_broadcaster.h" #include "gmapping/sensor/sensor_range/rangesensor.h" #include "gmapping/sensor/sensor_odometry/odometrysensor.h" @@ -266,7 +268,7 @@ void SlamGMapping::startLiveSlam() transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_)); } -void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_topic) +void SlamGMapping::startReplay(const std::string & bag_fname, const std::string &urdf_fname, std::string scan_topic) { double transform_publish_period; ros::NodeHandle private_nh_("~"); @@ -274,6 +276,14 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t sst_ = node_.advertise("map", 1, true); sstm_ = node_.advertise("map_metadata", 1, true); ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this); + + tf2_ros::StaticTransformBroadcaster static_tf_broadcaster; + std::vector urdf_transforms; + if (!urdf_fname.empty()) { + urdf_transforms = + ReadStaticTransformsFromUrdf(urdf_fname); + } + static_tf_broadcaster.sendTransform(urdf_transforms); rosbag::Bag bag; bag.open(bag_fname, rosbag::bagmode::Read); diff --git a/gmapping/src/slam_gmapping.h b/gmapping/src/slam_gmapping.h index ae622b95..188bed54 100644 --- a/gmapping/src/slam_gmapping.h +++ b/gmapping/src/slam_gmapping.h @@ -40,7 +40,7 @@ class SlamGMapping void init(); void startLiveSlam(); - void startReplay(const std::string & bag_fname, std::string scan_topic); + void startReplay(const std::string & bag_fname, const std::string &urdf_fname, std::string scan_topic); void publishTransform(); void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan); diff --git a/gmapping/src/urdf_reader.cpp b/gmapping/src/urdf_reader.cpp new file mode 100644 index 00000000..ce9d11b9 --- /dev/null +++ b/gmapping/src/urdf_reader.cpp @@ -0,0 +1,38 @@ +#include "urdf_reader.h" + +#include +#include +#include "urdf/model.h" + +std::vector ReadStaticTransformsFromUrdf( + const std::string& urdf_filename) { + urdf::Model model; + model.initFile(urdf_filename); +#if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS + std::vector links; +#else + std::vector > links; +#endif + model.getLinks(links); + std::vector transforms; + for (const auto& link : links) { + if (!link->getParent() || link->parent_joint->type != urdf::Joint::FIXED) { + continue; + } + const urdf::Pose& pose = + link->parent_joint->parent_to_joint_origin_transform; + geometry_msgs::TransformStamped transform; + transform.transform.translation.x = pose.position.x; + transform.transform.translation.y = pose.position.y; + transform.transform.translation.z = pose.position.z; + transform.transform.rotation.x = pose.rotation.x; + transform.transform.rotation.y = pose.rotation.y; + transform.transform.rotation.z = pose.rotation.z; + transform.transform.rotation.w = pose.rotation.w; + transform.child_frame_id = link->name; + transform.header.frame_id = link->getParent()->name; + transform.header.stamp = ros::Time(0); + transforms.push_back(transform); + } + return transforms; +} diff --git a/gmapping/src/urdf_reader.h b/gmapping/src/urdf_reader.h new file mode 100644 index 00000000..93a87718 --- /dev/null +++ b/gmapping/src/urdf_reader.h @@ -0,0 +1,6 @@ +#include +#include +#include "tf/transform_listener.h" + +std::vector ReadStaticTransformsFromUrdf( + const std::string& urdf_filename); From fe58ba1b23cf0660d47753d55b7c15ca4cfc804e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Mon, 26 Jun 2017 22:06:55 +0200 Subject: [PATCH 2/4] Quit replay node on ctrl-C --- gmapping/src/slam_gmapping.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gmapping/src/slam_gmapping.cpp b/gmapping/src/slam_gmapping.cpp index 6d234787..db1baa4c 100644 --- a/gmapping/src/slam_gmapping.cpp +++ b/gmapping/src/slam_gmapping.cpp @@ -297,6 +297,9 @@ void SlamGMapping::startReplay(const std::string & bag_fname, const std::string std::queue > s_queue; foreach(rosbag::MessageInstance const m, viewall) { + if (!ros::ok()) { + break; + } tf::tfMessage::ConstPtr cur_tf = m.instantiate(); if (cur_tf != NULL) { for (size_t i = 0; i < cur_tf->transforms.size(); ++i) From bba81d5ee0faa105f3f73f081516369cdf4569da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Tue, 27 Jun 2017 17:03:22 +0200 Subject: [PATCH 3/4] Fix for building without C++11 --- gmapping/src/urdf_reader.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/gmapping/src/urdf_reader.cpp b/gmapping/src/urdf_reader.cpp index ce9d11b9..3c3f44bb 100644 --- a/gmapping/src/urdf_reader.cpp +++ b/gmapping/src/urdf_reader.cpp @@ -9,13 +9,16 @@ std::vector ReadStaticTransformsFromUrdf( urdf::Model model; model.initFile(urdf_filename); #if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS - std::vector links; + typedef urdf::LinkSharedPtr LinkSharedPtr; + std::vector links; #else - std::vector > links; + typedef boost::shared_ptr LinkSharedPtr; + std::vector links; #endif model.getLinks(links); std::vector transforms; - for (const auto& link : links) { + for (std::vector::iterator it = links.begin(); it != links.end(); it++) { + LinkSharedPtr &link = *it; if (!link->getParent() || link->parent_joint->type != urdf::Joint::FIXED) { continue; } From fe28d2666f896ae16c4ee4e89000c46381051ac7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Sat, 1 Jul 2017 13:35:15 +0200 Subject: [PATCH 4/4] Fix building other executables Add slam_gmapping.cpp and urdf_reader.cpp in a separate library to avoid repeating both files. --- gmapping/CMakeLists.txt | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/gmapping/CMakeLists.txt b/gmapping/CMakeLists.txt index 732f8e18..50daec58 100644 --- a/gmapping/CMakeLists.txt +++ b/gmapping/CMakeLists.txt @@ -22,17 +22,19 @@ include_directories( catkin_package() -add_executable(slam_gmapping src/slam_gmapping.cpp src/main.cpp) -target_link_libraries(slam_gmapping ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +add_library(lib_slam_gmapping src/slam_gmapping.cpp src/urdf_reader.cpp) + +add_executable(slam_gmapping src/main.cpp) +target_link_libraries(slam_gmapping lib_slam_gmapping ${Boost_LIBRARIES} ${catkin_LIBRARIES}) 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_library(slam_gmapping_nodelet src/nodelet.cpp) +target_link_libraries(slam_gmapping_nodelet lib_slam_gmapping ${catkin_LIBRARIES}) -add_executable(slam_gmapping_replay src/slam_gmapping.cpp src/replay.cpp src/urdf_reader.cpp) -target_link_libraries(slam_gmapping_replay ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +add_executable(slam_gmapping_replay src/replay.cpp) +target_link_libraries(slam_gmapping_replay lib_slam_gmapping ${Boost_LIBRARIES} ${catkin_LIBRARIES}) if(catkin_EXPORTED_TARGETS) add_dependencies(slam_gmapping_replay ${catkin_EXPORTED_TARGETS}) endif()