diff --git a/include/publisher_trajectory.h b/include/publisher_trajectory.h
index 3be19185453590bbc70e7ecc83e5023ecc725e46..2d855cbb42558dd6265b50a9ebd70080a623d160 100644
--- a/include/publisher_trajectory.h
+++ b/include/publisher_trajectory.h
@@ -18,6 +18,12 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
+/*
+ * publisher_trajectory.h
+ *
+ * Created on: Feb 03, 2022
+ * Author: igeer
+ */
//--------LICENSE_END--------
#ifndef PUBLISHER_TRAJECTORY_H
#define PUBLISHER_TRAJECTORY_H
@@ -33,31 +39,18 @@
* ROS includes *
**************************/
#include <ros/ros.h>
-#include <geometry_msgs/PoseArray.h>
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Path.h>
-#include <nav_msgs/Odometry.h>
-#include <visualization_msgs/Marker.h>
-#include <tf/transform_listener.h>
namespace wolf
{
class PublisherTrajectory: public Publisher
{
- bool extrinsics_;
- int max_points_;
- double line_size_;
-
nav_msgs::Path path_msg_;
- nav_msgs::Odometry odometry_msg_;
-
- visualization_msgs::Marker marker_msg_;
- std_msgs::ColorRGBA marker_color_;
- SensorBasePtr sensor_;
- std::string frame_id_, map_frame_id_;
+
+ std::string frame_id_;
- ros::Publisher pub_path_, pub_marker_, pub_odometry_;
+ ros::Publisher pub_path_;
public:
PublisherTrajectory(const std::string& _unique_name,
@@ -73,12 +66,6 @@ class PublisherTrajectory: public Publisher
void publishTrajectory();
- protected:
-
- bool listenTf();
- Eigen::Quaterniond q_frame_;
- Eigen::Vector3d t_frame_;
- tf::TransformListener tfl_;
};
WOLF_REGISTER_PUBLISHER(PublisherTrajectory)
diff --git a/src/publisher_trajectory.cpp b/src/publisher_trajectory.cpp
index a453288192f0d891cfd3098dee09fe71fdaed473..2e460865deae086cc7bdef8db6b6ad449c21c01a 100644
--- a/src/publisher_trajectory.cpp
+++ b/src/publisher_trajectory.cpp
@@ -19,14 +19,19 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
+/*
+ * publisher_trajectory.cpp
+ *
+ * Created on: Feb 03, 2022
+ * Author: igeer
+ */
+
#include "publisher_trajectory.h"
/**************************
* ROS includes *
**************************/
#include <ros/ros.h>
-#include "tf/transform_datatypes.h"
-#include "tf_conversions/tf_eigen.h"
namespace wolf
{
@@ -36,233 +41,69 @@ PublisherTrajectory::PublisherTrajectory(const std::string& _unique_name,
const ProblemPtr _problem) :
Publisher(_unique_name, _server, _problem)
{
- Eigen::Vector4d marker_color_v;
- marker_color_v = getParamWithDefault<Eigen::Vector4d>(_server,
- prefix_ + "/marker_color",
- (Eigen::Vector4d() << 1, 0, 0, 1).finished()); // red
- marker_color_.r = marker_color_v(0);
- marker_color_.g = marker_color_v(1);
- marker_color_.b = marker_color_v(2);
- marker_color_.a = marker_color_v(3);
-
- max_points_ = getParamWithDefault<int>(_server,
- prefix_ + "/max_points",
- 1e4);
- line_size_ = getParamWithDefault<double>(_server,
- prefix_ + "/line_size",
- 0.1);
-
- 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 PublisherTrajectory::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>("frame_id", frame_id_, "map");
// initialize msg and publisher
// PATH
path_msg_.header.frame_id = frame_id_;
-
- pub_path_ = nh.advertise<nav_msgs::Path>(topic + "_trajectory", 1);
-
- // MARKER
- 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";
- marker_msg_.scale.x = line_size_;
- marker_msg_.color = marker_color_;
- marker_msg_.pose.orientation = tf::createQuaternionMsgFromYaw(0);
-
- pub_marker_ = nh.advertise<visualization_msgs::Marker>(topic + "_marker", 1);
-
- // Odometry
- odometry_msg_.header.frame_id = frame_id_;
-
- pub_odometry_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>(topic + "_pose_with_cov", 1);
-
-
+ pub_path_ = nh.advertise<nav_msgs::Path>(topic, 1);
}
void PublisherTrajectory::publishDerived()
{
- if (pub_path_.getNumSubscribers() == 0 and
- pub_marker_.getNumSubscribers() == 0 and
- pub_odometry_.getNumSubscribers() == 0 )
- return;
+ if (pub_path_.getNumSubscribers() != 0 )
+ publishTrajectory();
+}
- VectorComposite current_state = problem_->getState("PO");
- TimeStamp loc_ts = problem_->getTimeStamp();
+void PublisherTrajectory::publishTrajectory()
+{
+ path_msg_.header.stamp = ros::Time::now();
- // state not ready
- if (current_state.count('P') == 0 or
- current_state.count('O') == 0 or
- not loc_ts.ok())
- {
- return;
- }
+ auto trajectory = problem_->getTrajectory();
+ int frame_num = 0;
- // fill vector and quaternion
+ //Fill path message with PoseStamped from trajectory
+ geometry_msgs::PoseStamped framepose;
Eigen::Vector3d p = Eigen::Vector3d::Zero();
Eigen::Quaterniond q;
- // 2D
- if (problem_->getDim() == 2)
- {
- 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
+ for (auto frm: trajectory->getFrameMap())
{
- if (extrinsics_)
+ auto loc_ts = frm.first;
+ framepose.header.frame_id = frame_id_;
+ framepose.header.stamp = ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds());
+ if (problem_->getDim() == 2)
{
- 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']));
+ p.head(2) = frm.second->getP()->getState();
+ q = Eigen::Quaterniond(Eigen::AngleAxisd(frm.second->getO()->getState()(0), Eigen::Vector3d::UnitZ()));
}
else
{
- p = current_state['P'];
- q = Eigen::Quaterniond(Eigen::Vector4d(current_state['O']));
- }
- }
-
- // Change frame
- if (frame_id_ != map_frame_id_ and listenTf())
- {
- p = t_frame_ + q_frame_ * p;
- q = q_frame_ * q;
- }
-
- // Covariance
- Eigen::MatrixXd cov(6,6);
- auto KF = problem_->getLastFrame();
- bool success(true);
- success = success && problem_->getCovarianceBlock(KF->getP(), KF->getP(), cov, 0, 0);
- success = success && problem_->getCovarianceBlock(KF->getP(), KF->getO(), cov, 0, 3);
- success = success && problem_->getCovarianceBlock(KF->getO(), KF->getP(), cov, 3, 0);
- success = success && problem_->getCovarianceBlock(KF->getO(), KF->getO(), cov, 3, 3);
+ p = frm.second->getP()->getState();
+ q = Eigen::Quaterniond(Eigen::Vector4d(frm.second->getO()->getState()));
- if (success)
- {
- if (problem_->getDim() == 2)
- throw std::runtime_error("not implemented");
- else
- std::copy(cov.data(), cov.data() + cov.size(), odometry_msg_.pose.covariance.data());
- }
- else
- {
- //WOLF_WARN("Last KF covariance could not be recovered, using the previous one");
- //odometry_msg_.pose.covariance[0] = -1; // not valid
- }
-
- // Fill Trajectory msg
- odometry_msg_.header.stamp = ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds());
- odometry_msg_.pose.pose.position.x = p(0);
- odometry_msg_.pose.pose.position.y = p(1);
- odometry_msg_.pose.pose.position.z = p(2);
-
- odometry_msg_.pose.pose.orientation.x = q.x();
- odometry_msg_.pose.pose.orientation.y = q.y();
- odometry_msg_.pose.pose.orientation.z = q.z();
- odometry_msg_.pose.pose.orientation.w = q.w();
- publishTrajectory();
-}
-
-void PublisherTrajectory::publishTrajectory()
-{
- // fill msgs and publish
- if (pub_path_.getNumSubscribers() != 0)
- {
- path_msg_.header.stamp = odometry_msg_.header.stamp;
-
- if (max_points_ >= 0 and path_msg_.poses.size() >= max_points_)
- {
- int i = 1;
- while (i < path_msg_.poses.size())
- {
- path_msg_.poses.erase(path_msg_.poses.begin()+i);
- i++;
- }
- //path_msg_.poses.erase(path_msg_.poses.begin(),
- // path_msg_.poses.begin() + max_points_/2);
}
-
- path_msg_.poses.push_back(odometry_msg_.pose.pose);
-
- pub_path_.publish(path_msg_);
- }
- if (pub_marker_.getNumSubscribers() != 0)
- {
- marker_msg_.header.stamp = odometry_msg_.header.stamp;
-
- if (max_points_ >= 0 and marker_msg_.points.size() >= max_points_)
- {
- auto it = marker_msg_.points.begin();
- std::advance(it,1);
- while (it != marker_msg_.points.end())
- {
- it = marker_msg_.points.erase(it);
- if (it == marker_msg_.points.end())
- break;
- std::advance(it,1);
- }
- //int i = 1;
- //while (i < marker_msg_.points.size())
- //{
- // marker_msg_.points.erase(marker_msg_.points.begin()+i);
- // i++;
- //}
- //marker_msg_.points.erase(marker_msg_.points.begin(),
- // marker_msg_.points.begin() + max_points_/2);
+ framepose.pose.position.x = p(0);
+ framepose.pose.position.y = p(1);
+ framepose.pose.position.z = p(2);
+ framepose.pose.orientation.x = q.x();
+ framepose.pose.orientation.y = q.y();
+ framepose.pose.orientation.z = q.z();
+ framepose.pose.orientation.w = q.w();
+ path_msg_.poses.push_back(framepose);
}
- marker_msg_.points.push_back(odometry_msg_.pose.pose.position);
-
- pub_marker_.publish(marker_msg_);
- }
- if (pub_odometry_.getNumSubscribers() != 0)
- {
- pub_odometry_.publish(odometry_msg_);
- }
-}
-
-bool PublisherTrajectory::listenTf()
-{
- tf::StampedTransform T;
- if ( tfl_.waitForTransform(frame_id_, map_frame_id_, ros::Time(0), ros::Duration(0.01)) )
- {
- tfl_.lookupTransform(frame_id_, map_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);
+ //Publish path
+ pub_path_.publish(path_msg_);
- return true;
- }
- return false;
+ //clear msg
+ path_msg_.poses.clear();
}
}