Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Publish poses to /tf in ROSOutput3DWrapper #1

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Publish poses to /tf in ROSOutput3DWrapper #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
8b193a1
Add cmake minimum version (was getting a warning) and add find_packag…
Mar 24, 2017
31af6c1
Add tf as a dependency that gets linked with ROSOutput3DWrapper
Mar 24, 2017
7d116f7
Add a tf broadcaster in publishTrackedFrame()
Mar 24, 2017
d031b54
Add a private member for the tf_publisher
Mar 24, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions lsd_slam/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,2 +1,4 @@
cmake_minimum_required(VERSION 2.8.3)
project(lsd_slam)
find_package(catkin REQUIRED)
catkin_package()
2 changes: 1 addition & 1 deletion lsd_slam_core/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
21 changes: 20 additions & 1 deletion lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@
#include <ros/ros.h>
#include "util/settings.h"


#include "std_msgs/Float32MultiArray.h"
#include "lsd_slam_viewer/keyframeGraphMsg.h"
#include "lsd_slam_viewer/keyframeMsg.h"
Expand All @@ -34,6 +33,9 @@
#include "geometry_msgs/PoseStamped.h"
#include "GlobalMapping/g2oTypeSim3Sophus.h"

#include <geometry_msgs/TransformStamped.h>
#include <tf/transform_broadcaster.h>

namespace lsd_slam
{

Expand Down Expand Up @@ -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);
}


Expand Down
4 changes: 3 additions & 1 deletion lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@

#include <ros/ros.h>
#include "IOWrapper/Output3DWrapper.h"
#include <tf/transform_broadcaster.h>


namespace lsd_slam
Expand Down Expand Up @@ -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_;
};
}