diff --git a/include/publisher_pose.h b/include/publisher_pose.h
index 29fdc157c095c4e81f57fd97c55b6b722e421251..dff4bac7531ea8fccf2c580fae43532f03aab046 100644
--- a/include/publisher_pose.h
+++ b/include/publisher_pose.h
@@ -14,6 +14,7 @@
#include <ros/ros.h>
#include <geometry_msgs/PoseArray.h>
#include <visualization_msgs/Marker.h>
+#include <tf/transform_listener.h>
namespace wolf
{
@@ -25,6 +26,9 @@ class PublisherPose: public Publisher
geometry_msgs::PoseArray pose_array_msg_;
visualization_msgs::Marker marker_msg_;
std_msgs::ColorRGBA marker_color_;
+ bool extrinsics_;
+ SensorBasePtr sensor_;
+ std::string frame_id_, map_frame_id_;
ros::Publisher pub_pose_array_, pub_marker_;
@@ -41,6 +45,13 @@ class PublisherPose: public Publisher
void publishDerived() override;
void publishPose(const geometry_msgs::Pose pose, const ros::Time& stamp);
+
+ protected:
+
+ bool listenTf();
+ Eigen::Quaterniond q_frame_;
+ Eigen::Vector3d t_frame_;
+ tf::TransformListener tfl_;
};
WOLF_REGISTER_PUBLISHER(PublisherPose)
diff --git a/src/publisher_pose.cpp b/src/publisher_pose.cpp
index 337cf8413010eecbbb415ccbe79cb95ad0ed4ab5..ae27c1ef00cf99b831c80a41182d828019baa307 100644
--- a/src/publisher_pose.cpp
+++ b/src/publisher_pose.cpp
@@ -5,6 +5,7 @@
**************************/
#include <ros/ros.h>
#include "tf/transform_datatypes.h"
+#include "tf_conversions/tf_eigen.h"
namespace wolf
{
@@ -24,23 +25,27 @@ PublisherPose::PublisherPose(const std::string& _unique_name,
marker_color_.b = col(2);
marker_color_.a = col(3);
}
+ extrinsics_ = _server.getParam<bool>(prefix_ + "/extrinsics");
+ if (extrinsics_)
+ sensor_ = _problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor"));
+ frame_id_ = _server.getParam<std::string>(prefix_ + "/frame_id");
}
void PublisherPose::initialize(ros::NodeHandle& nh, const std::string& topic)
{
std::string map_frame_id;
- nh.param<std::string>("map_frame_id", map_frame_id, "map");
+ nh.param<std::string>("map_frame_id", map_frame_id_, "map");
// initialize msg and publisher
if (pose_array_)
{
- pose_array_msg_.header.frame_id = map_frame_id;
+ pose_array_msg_.header.frame_id = frame_id_;
pub_pose_array_ = nh.advertise<geometry_msgs::PoseArray>(topic + "_pose_array", 1);
}
if (marker_)
{
- marker_msg_.header.frame_id = map_frame_id;
+ marker_msg_.header.frame_id = frame_id_;
marker_msg_.type = visualization_msgs::Marker::LINE_STRIP;
marker_msg_.action = visualization_msgs::Marker::ADD;
marker_msg_.ns = "trajectory";
@@ -65,31 +70,65 @@ void PublisherPose::publishDerived()
return;
}
- // Fill Pose msg
- geometry_msgs::Pose pose_msg;
+
+ // fill vector and quaternion
+ Eigen::Vector3d p = Eigen::Vector3d::Zero();
+ Eigen::Quaterniond q;
// 2D
if (problem_->getDim() == 2)
{
- pose_msg.position.x = current_state['P'](0);
- pose_msg.position.y = current_state['P'](1);
- pose_msg.position.z = 0;
-
- pose_msg.orientation = tf::createQuaternionMsgFromYaw(current_state['O'](0));
+ if (extrinsics_)
+ {
+ p.head(2) = current_state['P'] + Eigen::Rotation2Dd(current_state['O'](0)) * sensor_->getP()->getState().head(2);
+ if (sensor_->getO())
+ q = Eigen::Quaterniond(Eigen::AngleAxisd(current_state['O'](0) + sensor_->getO()->getState()(0),
+ Eigen::Vector3d::UnitZ()));
+ else
+ q = Eigen::Quaterniond(Eigen::AngleAxisd(current_state['O'](0),
+ Eigen::Vector3d::UnitZ()));
+ }
+ else
+ {
+ p.head(2) = current_state['P'];
+ q = Eigen::Quaterniond(Eigen::AngleAxisd(current_state['O'](0), Eigen::Vector3d::UnitZ()));
+ }
}
// 3D
else
{
- pose_msg.position.x = current_state['P'](0);
- pose_msg.position.y = current_state['P'](1);
- pose_msg.position.z = current_state['P'](2);
-
- pose_msg.orientation.x = current_state['O'](0);
- pose_msg.orientation.y = current_state['O'](1);
- pose_msg.orientation.z = current_state['O'](2);
- pose_msg.orientation.w = current_state['O'](3);
+ if (extrinsics_)
+ {
+ p = current_state['P'] + Eigen::Quaterniond(Eigen::Vector4d(current_state['O'])) * sensor_->getP()->getState();
+ if (sensor_->getO())
+ q = Eigen::Quaterniond(Eigen::Vector4d(current_state['O'])) * Eigen::Quaterniond(Eigen::Vector4d(sensor_->getO()->getState()));
+ else
+ q = Eigen::Quaterniond(Eigen::Vector4d(current_state['O']));
+ }
+ else
+ {
+ p = current_state['P'];
+ q = Eigen::Quaterniond(Eigen::Vector4d(current_state['O']));
+ }
}
+ // Change frame
+ if (frame_id_ != map_frame_id_)
+ {
+ p = t_frame_ + q_frame_ * p;
+ q = q_frame_ * q;
+ }
+
+ // Fill Pose msg
+ geometry_msgs::Pose pose_msg;
+ pose_msg.position.x = p(0);
+ pose_msg.position.y = p(1);
+ pose_msg.position.z = p(2);
+
+ pose_msg.orientation.x = q.x();
+ pose_msg.orientation.y = q.y();
+ pose_msg.orientation.z = q.z();
+ pose_msg.orientation.w = q.w();
publishPose(pose_msg, ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds()));
}
@@ -112,4 +151,21 @@ void PublisherPose::publishPose(const geometry_msgs::Pose pose, const ros::Time&
}
}
+bool PublisherPose::listenTf()
+{
+ tf::StampedTransform T;
+ if ( tfl_.waitForTransform(map_frame_id_, frame_id_, ros::Time(0), ros::Duration(0.01)) )
+ {
+ tfl_.lookupTransform(map_frame_id_, frame_id_, ros::Time(0), T);
+
+ Eigen::Matrix3d R;
+ tf::matrixTFToEigen(T.getBasis(), R);
+ tf::vectorTFToEigen(T.getOrigin(), t_frame_);
+ q_frame_ = Eigen::Quaterniond(R);
+
+ return true;
+ }
+ return false;
+}
+
}
diff --git a/src/visualizer.cpp b/src/visualizer.cpp
index 140c70d9ac39b9ed74322a3b6c65c5b44171c027..a3d5896abc558c98adf55e4dee605200dd1334ed 100644
--- a/src/visualizer.cpp
+++ b/src/visualizer.cpp
@@ -22,7 +22,7 @@ void Visualizer::initialize(ros::NodeHandle& nh)
nh.param<bool>( "viz_landmarks", viz_landmarks_, true);
nh.param<bool>( "viz_trajectory", viz_trajectory_, true);
// viz parameters
- nh.param<std::string>( "map_frame_name", map_frame_id_, "map");
+ nh.param<std::string>( "map_frame_id", map_frame_id_, "map");
nh.param<double>( "viz_scale", viz_scale_, 1);
nh.param<double>( "factors_width", factors_width_, 0.02);
nh.param<double>( "factors_absolute_height", factors_absolute_height_, 20);