Commit aebb4059 authored by simalpha's avatar simalpha
Browse files

publishing pose map_in_odom instead of TF

parent 88a5e414
......@@ -5,6 +5,7 @@
#include "aicp_utils/visualizer.hpp"
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Path.h>
#include <eigen_conversions/eigen_msg.h>
......@@ -51,7 +52,10 @@ public:
int param, std::string name, int64_t utime);
// Publish tf from fixed_frame to odom
void publishFixedFrameToOdomTF(Eigen::Isometry3d& fixed_frame_to_base_eigen, ros::Time msg_time);
void publishFixedFrameToOdomTF(const Eigen::Isometry3d& fixed_frame_to_base_eigen,
ros::Time msg_time);
void publishFixedFrameToOdomPose(const Eigen::Isometry3d& fixed_frame_to_base_eigen,
ros::Time msg_time);
// Gets
const PathPoses& getPath(){
......@@ -64,6 +68,7 @@ private:
ros::Publisher prior_map_pub_;
ros::Publisher aligned_map_pub_;
ros::Publisher pose_pub_;
ros::Publisher fixed_to_odom_pub_;
// Duplicates the list in collections renderer. assumed to be 3xN colors
std::vector<double> colors_;
// Path (vector of poses)
......@@ -72,9 +77,16 @@ private:
std::string fixed_frame_; // map or map_test
std::string odom_frame_;
std::string base_frame_;
std::string fixed_to_odom_prefix_ = "/localization_manager/";
// TF listener and broadcaster
tf::TransformListener tf_listener_;
tf::TransformBroadcaster tf_broadcaster_;
geometry_msgs::PoseWithCovarianceStamped fixed_to_odom_msg_;
tf::Pose temp_tf_pose_;
void computeFixedFrameToOdom(const Eigen::Isometry3d &fixed_frame_to_base_eigen,
Eigen::Isometry3d& fixed_frame_to_odom_eigen);
};
}
......@@ -90,7 +90,8 @@ void AppROS::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedCon
// Publish fixed_frame to odom tf
ros::Time msg_time(pose_msg_in->header.stamp.sec, pose_msg_in->header.stamp.nsec);
vis_ros_->publishFixedFrameToOdomTF(corrected_pose_, msg_time);
// vis_ros_->publishFixedFrameToOdomTF(corrected_pose_, msg_time);
vis_ros_->publishFixedFrameToOdomPose(corrected_pose_, msg_time);
// Publish /aicp/pose_corrected
tf::poseEigenToTF(corrected_pose_, temp_tf_pose_);
......
......@@ -18,6 +18,7 @@ ROSVisualizer::ROSVisualizer(ros::NodeHandle& nh, string fixed_frame) : nh_(nh),
prior_map_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/aicp/prior_map", 10);
aligned_map_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/aicp/aligned_map", 10);
pose_pub_ = nh_.advertise<nav_msgs::Path>("/aicp/poses",100);
fixed_to_odom_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(fixed_to_odom_prefix_ + fixed_frame_ + "_to_odom", 10);
colors_ = {
51/255.0, 160/255.0, 44/255.0, //0
166/255.0, 206/255.0, 227/255.0,
......@@ -152,7 +153,21 @@ void ROSVisualizer::publishPoses(PathPoses poses, int param, string name, int64_
pose_pub_.publish(path_msg);
}
void ROSVisualizer::publishFixedFrameToOdomTF(Eigen::Isometry3d& fixed_frame_to_base_eigen, ros::Time msg_time)
void ROSVisualizer::publishFixedFrameToOdomPose(const Eigen::Isometry3d &fixed_frame_to_base_eigen,
ros::Time msg_time)
{
Eigen::Isometry3d fixed_frame_to_odom_eigen;
this->computeFixedFrameToOdom(fixed_frame_to_base_eigen, fixed_frame_to_odom_eigen);
tf::poseEigenToTF(fixed_frame_to_odom_eigen, temp_tf_pose_);
tf::poseTFToMsg(temp_tf_pose_, fixed_to_odom_msg_.pose.pose);
fixed_to_odom_msg_.header.stamp = msg_time;
fixed_to_odom_msg_.header.frame_id = odom_frame_;
fixed_to_odom_pub_.publish(fixed_to_odom_msg_);
}
void ROSVisualizer::computeFixedFrameToOdom(const Eigen::Isometry3d &fixed_frame_to_base_eigen,
Eigen::Isometry3d& fixed_frame_to_odom_eigen)
{
// TF listener
tf::StampedTransform base_to_odom_tf;
......@@ -171,8 +186,14 @@ void ROSVisualizer::publishFixedFrameToOdomTF(Eigen::Isometry3d& fixed_frame_to_
tf::transformTFToEigen(base_to_odom_tf, base_to_odom_eigen);
// Multiply
Eigen::Isometry3d fixed_frame_to_odom_eigen;
fixed_frame_to_odom_eigen = fixed_frame_to_base_eigen * base_to_odom_eigen.inverse();
}
void ROSVisualizer::publishFixedFrameToOdomTF(const Eigen::Isometry3d& fixed_frame_to_base_eigen,
ros::Time msg_time)
{
Eigen::Isometry3d fixed_frame_to_odom_eigen;
this->computeFixedFrameToOdom(fixed_frame_to_base_eigen, fixed_frame_to_odom_eigen);
// Convert to TF
tf::StampedTransform fixed_frame_to_odom_tf;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment