From 8b193a14ecc5edf56575e03839e8132f8ccdbd14 Mon Sep 17 00:00:00 2001 From: mknowles Date: Fri, 24 Mar 2017 17:02:37 -0400 Subject: [PATCH 1/4] Add cmake minimum version (was getting a warning) and add find_package(catkin REQUIRED) so that the catkin_package() macro stops causing an error --- lsd_slam/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lsd_slam/CMakeLists.txt b/lsd_slam/CMakeLists.txt index 3dc1ef3..611c5f8 100644 --- a/lsd_slam/CMakeLists.txt +++ b/lsd_slam/CMakeLists.txt @@ -1,2 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) project(lsd_slam) +find_package(catkin REQUIRED) catkin_package() From 31af6c18d4c1f96909dd194261fd0f2503ceb92d Mon Sep 17 00:00:00 2001 From: mknowles Date: Fri, 24 Mar 2017 18:19:52 -0400 Subject: [PATCH 2/4] Add tf as a dependency that gets linked with ROSOutput3DWrapper --- lsd_slam_core/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lsd_slam_core/CMakeLists.txt b/lsd_slam_core/CMakeLists.txt index 53b7938..6ede1f1 100644 --- a/lsd_slam_core/CMakeLists.txt +++ b/lsd_slam_core/CMakeLists.txt @@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 2.8.3) project(lsd_slam_core) # Load catkin and all dependencies required for this package # TODO: remove all from COMPONENTS that are not catkin packages. -find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure sensor_msgs roslib rosbag lsd_slam_viewer roscpp) +find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure sensor_msgs roslib rosbag lsd_slam_viewer roscpp tf) # include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) # CATKIN_MIGRATION: removed during catkin migration From 7d116f74c7cb9d286287486e0dbd9fff6c284941 Mon Sep 17 00:00:00 2001 From: mknowles Date: Fri, 24 Mar 2017 18:20:58 -0400 Subject: [PATCH 3/4] Add a tf broadcaster in publishTrackedFrame() --- .../src/IOWrapper/ROS/ROSOutput3DWrapper.cpp | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp b/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp index 78faa88..39e49c1 100644 --- a/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp +++ b/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp @@ -23,7 +23,6 @@ #include #include "util/settings.h" - #include "std_msgs/Float32MultiArray.h" #include "lsd_slam_viewer/keyframeGraphMsg.h" #include "lsd_slam_viewer/keyframeMsg.h" @@ -34,6 +33,9 @@ #include "geometry_msgs/PoseStamped.h" #include "GlobalMapping/g2oTypeSim3Sophus.h" +#include +#include + namespace lsd_slam { @@ -157,6 +159,23 @@ void ROSOutput3DWrapper::publishTrackedFrame(Frame* kf) pMsg.header.stamp = ros::Time(kf->timestamp()); pMsg.header.frame_id = "world"; pose_publisher.publish(pMsg); + + // also publish poses to /tf + geometry_msgs::TransformStamped tf_msg; + + tf_msg.header.stamp = ros::Time(kf->timestamp()); + tf_msg.header.frame_id = "world"; + tf_msg.child_frame_id = "/camera"; + + tf_msg.transform.translation.x = camToWorld.translation()[0]; + tf_msg.transform.translation.y = camToWorld.translation()[1]; + tf_msg.transform.translation.z = camToWorld.translation()[2]; + tf_msg.transform.rotation.x = camToWorld.so3().unit_quaternion().x(); + tf_msg.transform.rotation.y = camToWorld.so3().unit_quaternion().y(); + tf_msg.transform.rotation.z = camToWorld.so3().unit_quaternion().z(); + tf_msg.transform.rotation.w = camToWorld.so3().unit_quaternion().w(); + + tf_publisher_.sendTransform(tf_msg); } From d031b5475830f47f43f6af0007815acb261060c6 Mon Sep 17 00:00:00 2001 From: mknowles Date: Fri, 24 Mar 2017 18:21:27 -0400 Subject: [PATCH 4/4] Add a private member for the tf_publisher --- lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.h b/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.h index 9c5cab8..b7e40f0 100644 --- a/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.h +++ b/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.h @@ -22,6 +22,7 @@ #include #include "IOWrapper/Output3DWrapper.h" +#include namespace lsd_slam @@ -95,10 +96,11 @@ class ROSOutput3DWrapper : public Output3DWrapper std::string debugInfo_channel; ros::Publisher debugInfo_publisher; - std::string pose_channel; ros::Publisher pose_publisher; + tf::TransformBroadcaster tf_publisher_; + ros::NodeHandle nh_; }; }