Support reading .urdf transforms in replay node

gmapping/CMakeLists.txt

@@ -1,27 +1,40 @@
 cmake_minimum_required(VERSION 2.8)
 project(gmapping)
 
-find_package(catkin REQUIRED nav_msgs nodelet openslam_gmapping roscpp tf rosbag_storage)
+find_package(catkin REQUIRED nav_msgs nodelet openslam_gmapping roscpp tf rosbag_storage urdf)
 
 find_package(Boost REQUIRED signals)
 
 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)
-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)
-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()

gmapping/package.xml

@@ -20,13 +20,15 @@
   <build_depend>rostest</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>nodelet</build_depend>
+  <build_depend>urdf</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>
-
+  <run_depend>urdf</run_depend>
+
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml" />
   </export>

gmapping/src/replay.cpp

@@ -32,7 +32,8 @@ main(int argc, char** argv)
     desc.add_options() 
     ("help", "Print help messages") 
     ("scan_topic",  po::value<std::string>()->default_value("/scan") ,"topic that contains the laserScan in the rosbag")
-    ("bag_filename", po::value<std::string>()->required(), "ros bag filename") 
+    ("bag_filename", po::value<std::string>()->required(), "ros bag filename")
+    ("urdf_filename", po::value<std::string>()->default_value(std::string()), "urdf filename")
     ("seed", po::value<unsigned long int>()->default_value(0), "seed")
     ("max_duration_buffer", po::value<unsigned long int>()->default_value(99999), "max tf buffer duration")
     ("on_done", po::value<std::string>(), "command to execute when done") ;
@@ -63,13 +64,14 @@ main(int argc, char** argv)
     } 
 
     std::string bag_fname = vm["bag_filename"].as<std::string>();
+    std::string urdf_fname = vm["urdf_filename"].as<std::string>();
     std::string scan_topic = vm["scan_topic"].as<std::string>();
     unsigned long int seed = vm["seed"].as<unsigned long int>();
     unsigned long int max_duration_buffer = vm["max_duration_buffer"].as<unsigned long int>();
 
     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") )

gmapping/src/slam_gmapping.cpp

@@ -106,6 +106,7 @@ Initial map dimensions and resolution:
 
 
 #include "slam_gmapping.h"
+#include "urdf_reader.h"
 
 #include <iostream>
 
@@ -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,14 +268,22 @@ 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_("~");
   entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
   sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
   sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
   ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
+
+  tf2_ros::StaticTransformBroadcaster static_tf_broadcaster;
+  std::vector<geometry_msgs::TransformStamped> 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);
@@ -287,6 +297,9 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t
   std::queue<std::pair<sensor_msgs::LaserScan::ConstPtr, std::string> > s_queue;
   foreach(rosbag::MessageInstance const m, viewall)
   {
+    if (!ros::ok()) {
+      break;
+    }
     tf::tfMessage::ConstPtr cur_tf = m.instantiate<tf::tfMessage>();
     if (cur_tf != NULL) {
       for (size_t i = 0; i < cur_tf->transforms.size(); ++i)

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);

gmapping/src/urdf_reader.cpp

@@ -0,0 +1,41 @@
+#include "urdf_reader.h"
+
+#include <string>
+#include <vector>
+#include "urdf/model.h"
+
+std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
+    const std::string& urdf_filename) {
+  urdf::Model model;
+  model.initFile(urdf_filename);
+#if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS
+  typedef urdf::LinkSharedPtr LinkSharedPtr;
+  std::vector<LinkSharedPtr> links;
+#else
+  typedef boost::shared_ptr<urdf::Link> LinkSharedPtr;
+  std::vector<LinkSharedPtr> links;
+#endif
+  model.getLinks(links);
+  std::vector<geometry_msgs::TransformStamped> transforms;
+  for (std::vector<LinkSharedPtr>::iterator it = links.begin(); it != links.end(); it++) {
+    LinkSharedPtr &link = *it;
+    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;
+}

gmapping/src/urdf_reader.h

@@ -0,0 +1,6 @@
+#include <vector>
+#include <string>
+#include "tf/transform_listener.h"
+
+std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
+    const std::string& urdf_filename);