Skip to content

Commit

Permalink
Merge pull request #97 from aseligmann/tf2-branch
Browse files Browse the repository at this point in the history 
Add method for getting map transform and pose estimate in a user-specified frame
  • Loading branch information
@lennarthaller
lennarthaller authored Jan 8, 2021
2 parents 6ac1abd + f8b5bd2 commit 09b82ab
Show file tree
Hide file tree
Showing 2 changed files with 103 additions and 17 deletions.
14 changes: 12 additions & 2 deletions ros/include/Node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,10 +25,14 @@
#include <ros/ros.h>
#include <ros/time.h>
#include <image_transport/image_transport.h>
#include <tf/transform_broadcaster.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/core/core.hpp>

#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <dynamic_reconfigure/server.h>
#include <orb_slam2_ros/dynamic_reconfigureConfig.h>

Expand Down Expand Up @@ -69,7 +73,12 @@ class Node
bool SaveMapSrv (orb_slam2_ros::SaveMap::Request &req, orb_slam2_ros::SaveMap::Response &res);
void LoadOrbParameters (ORB_SLAM2::ORBParameters& parameters);

tf::Transform TransformFromMat (cv::Mat position_mat);
// initialization Transform listener
boost::shared_ptr<tf2_ros::Buffer> tfBuffer;
boost::shared_ptr<tf2_ros::TransformListener> tfListener;

tf2::Transform TransformFromMat (cv::Mat position_mat);
tf2::Transform TransformToTarget (tf2::Transform tf_in, std::string frame_in, std::string frame_target);
sensor_msgs::PointCloud2 MapPointsToPointCloud (std::vector<ORB_SLAM2::MapPoint*> map_points);

dynamic_reconfigure::Server<orb_slam2_ros::dynamic_reconfigureConfig> dynamic_param_server_;
Expand All @@ -88,6 +97,7 @@ class Node

std::string map_frame_id_param_;
std::string camera_frame_id_param_;
std::string target_frame_id_param_;
std::string map_file_name_param_;
std::string voc_file_name_param_;
bool load_map_param_;
Expand Down
106 changes: 91 additions & 15 deletions ros/src/Node.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@ void Node::Init () {
node_handle_.param(name_of_node_+ "/publish_tf", publish_tf_param_, true);
node_handle_.param<std::string>(name_of_node_+ "/pointcloud_frame_id", map_frame_id_param_, "map");
node_handle_.param<std::string>(name_of_node_+ "/camera_frame_id", camera_frame_id_param_, "camera_link");
node_handle_.param<std::string>(name_of_node_+ "/target_frame_id", target_frame_id_param_, "base_link");
node_handle_.param<std::string>(name_of_node_ + "/map_file", map_file_name_param_, "map.bin");
node_handle_.param<std::string>(name_of_node_ + "/voc_file", voc_file_name_param_, "file_not_set");
node_handle_.param(name_of_node_ + "/load_map", load_map_param_, false);
Expand All @@ -44,6 +45,10 @@ void Node::Init () {
dynamic_param_callback = boost::bind(&Node::ParamsChangedCallback, this, _1, _2);
dynamic_param_server_.setCallback(dynamic_param_callback);

// Initialization transformation listener
tfBuffer.reset(new tf2_ros::Buffer);
tfListener.reset(new tf2_ros::TransformListener(*tfBuffer));

rendered_image_publisher_ = image_transport_.advertise (name_of_node_+"/debug_image", 1);
if (publish_pointcloud_param_) {
map_points_publisher_ = node_handle_.advertise<sensor_msgs::PointCloud2> (name_of_node_+"/map_points", 1);
Expand All @@ -60,10 +65,12 @@ void Node::Update () {
cv::Mat position = orb_slam_->GetCurrentPosition();

if (!position.empty()) {
PublishPositionAsTransform (position);
if (publish_tf_param_){
PublishPositionAsTransform(position);
}

if (publish_pose_param_) {
PublishPositionAsPoseStamped (position);
PublishPositionAsPoseStamped(position);
}
}

Expand All @@ -81,20 +88,89 @@ void Node::PublishMapPoints (std::vector<ORB_SLAM2::MapPoint*> map_points) {
map_points_publisher_.publish (cloud);
}

tf2::Transform Node::TransformToTarget (tf2::Transform tf_in, std::string frame_in, std::string frame_target) {
// Transform tf_in from frame_in to frame_target
tf2::Transform tf_map2orig = tf_in;
tf2::Transform tf_orig2target;
tf2::Transform tf_map2target;

tf2::Stamped<tf2::Transform> transformStamped_temp;
try {
// Get the transform from camera to target
geometry_msgs::TransformStamped tf_msg = tfBuffer->lookupTransform(frame_in, frame_target, ros::Time(0));
// Convert to tf2
tf2::fromMsg(tf_msg, transformStamped_temp);
tf_orig2target.setBasis(transformStamped_temp.getBasis());
tf_orig2target.setOrigin(transformStamped_temp.getOrigin());

} catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
//ros::Duration(1.0).sleep();
tf_orig2target.setIdentity();
}

/*
// Print debug info
double roll, pitch, yaw;
// Print debug map2orig
tf2::Matrix3x3(tf_map2orig.getRotation()).getRPY(roll, pitch, yaw);
ROS_INFO("Static transform Map to Orig [%s -> %s]",
map_frame_id_param_.c_str(), frame_in.c_str());
ROS_INFO(" * Translation: {%.3f,%.3f,%.3f}",
tf_map2orig.getOrigin().x(), tf_map2orig.getOrigin().y(), tf_map2orig.getOrigin().z());
ROS_INFO(" * Rotation: {%.3f,%.3f,%.3f}",
RAD2DEG(roll), RAD2DEG(pitch), RAD2DEG(yaw));
// Print debug tf_orig2target
tf2::Matrix3x3(tf_orig2target.getRotation()).getRPY(roll, pitch, yaw);
ROS_INFO("Static transform Orig to Target [%s -> %s]",
frame_in.c_str(), frame_target.c_str());
ROS_INFO(" * Translation: {%.3f,%.3f,%.3f}",
tf_orig2target.getOrigin().x(), tf_orig2target.getOrigin().y(), tf_orig2target.getOrigin().z());
ROS_INFO(" * Rotation: {%.3f,%.3f,%.3f}",
RAD2DEG(roll), RAD2DEG(pitch), RAD2DEG(yaw));
// Print debug map2target
tf2::Matrix3x3(tf_map2target.getRotation()).getRPY(roll, pitch, yaw);
ROS_INFO("Static transform Map to Target [%s -> %s]",
map_frame_id_param_.c_str(), frame_target.c_str());
ROS_INFO(" * Translation: {%.3f,%.3f,%.3f}",
tf_map2target.getOrigin().x(), tf_map2target.getOrigin().y(), tf_map2target.getOrigin().z());
ROS_INFO(" * Rotation: {%.3f,%.3f,%.3f}",
RAD2DEG(roll), RAD2DEG(pitch), RAD2DEG(yaw));
*/

// Transform from map to target
tf_map2target = tf_map2orig * tf_orig2target;
return tf_map2target;
}

void Node::PublishPositionAsTransform (cv::Mat position) {
if(publish_tf_param_){
tf::Transform transform = TransformFromMat (position);
static tf::TransformBroadcaster tf_broadcaster;
tf_broadcaster.sendTransform(tf::StampedTransform(transform, current_frame_time_, map_frame_id_param_, camera_frame_id_param_));
}
// Get transform from map to camera frame
tf2::Transform tf_transform = TransformFromMat(position);

// Make transform from camera frame to target frame
tf2::Transform tf_map2target = TransformToTarget(tf_transform, camera_frame_id_param_, target_frame_id_param_);

// Make message
tf2::Stamped<tf2::Transform> tf_map2target_stamped;
tf_map2target_stamped = tf2::Stamped<tf2::Transform>(tf_map2target, current_frame_time_, map_frame_id_param_);
geometry_msgs::TransformStamped msg = tf2::toMsg(tf_map2target_stamped);
msg.child_frame_id = target_frame_id_param_;
// Broadcast tf
static tf2_ros::TransformBroadcaster tf_broadcaster;
tf_broadcaster.sendTransform(msg);
}

void Node::PublishPositionAsPoseStamped (cv::Mat position) {
tf::Transform grasp_tf = TransformFromMat (position);
tf::Stamped<tf::Pose> grasp_tf_pose(grasp_tf, current_frame_time_, map_frame_id_param_);
tf2::Transform tf_position = TransformFromMat(position);

// Make transform from camera frame to target frame
tf2::Transform tf_position_target = TransformToTarget(tf_position, camera_frame_id_param_, target_frame_id_param_);

// Make message
tf2::Stamped<tf2::Transform> tf_position_target_stamped;
tf_position_target_stamped = tf2::Stamped<tf2::Transform>(tf_position_target, current_frame_time_, map_frame_id_param_);
geometry_msgs::PoseStamped pose_msg;
tf::poseStampedTFToMsg (grasp_tf_pose, pose_msg);
tf2::toMsg(tf_position_target_stamped, pose_msg);
pose_publisher_.publish(pose_msg);
}

Expand All @@ -108,23 +184,23 @@ void Node::PublishRenderedImage (cv::Mat image) {
}


tf::Transform Node::TransformFromMat (cv::Mat position_mat) {
tf2::Transform Node::TransformFromMat (cv::Mat position_mat) {
cv::Mat rotation(3,3,CV_32F);
cv::Mat translation(3,1,CV_32F);

rotation = position_mat.rowRange(0,3).colRange(0,3);
translation = position_mat.rowRange(0,3).col(3);


tf::Matrix3x3 tf_camera_rotation (rotation.at<float> (0,0), rotation.at<float> (0,1), rotation.at<float> (0,2),
tf2::Matrix3x3 tf_camera_rotation (rotation.at<float> (0,0), rotation.at<float> (0,1), rotation.at<float> (0,2),
rotation.at<float> (1,0), rotation.at<float> (1,1), rotation.at<float> (1,2),
rotation.at<float> (2,0), rotation.at<float> (2,1), rotation.at<float> (2,2)
);

tf::Vector3 tf_camera_translation (translation.at<float> (0), translation.at<float> (1), translation.at<float> (2));
tf2::Vector3 tf_camera_translation (translation.at<float> (0), translation.at<float> (1), translation.at<float> (2));

//Coordinate transformation matrix from orb coordinate system to ros coordinate system
const tf::Matrix3x3 tf_orb_to_ros (0, 0, 1,
const tf2::Matrix3x3 tf_orb_to_ros (0, 0, 1,
-1, 0, 0,
0,-1, 0);

Expand All @@ -140,7 +216,7 @@ tf::Transform Node::TransformFromMat (cv::Mat position_mat) {
tf_camera_rotation = tf_orb_to_ros*tf_camera_rotation;
tf_camera_translation = tf_orb_to_ros*tf_camera_translation;

return tf::Transform (tf_camera_rotation, tf_camera_translation);
return tf2::Transform (tf_camera_rotation, tf_camera_translation);
}


Expand Down

0 comments on commit 09b82ab

Please sign in to comment.