diff --git a/CMakeLists.txt b/CMakeLists.txt index bafc94df4f1ae42559e9769c381573c587df35ba..1851015a20181da59c2d94b5c351bcbe88888b42 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs + nav_msgs tf tf_conversions tf2_ros @@ -154,6 +155,7 @@ add_library(subscriber_${PROJECT_NAME} add_library(publisher_${PROJECT_NAME} src/publisher_graph.cpp src/publisher_pose.cpp + src/publisher_trajectory.cpp src/publisher_state_block.cpp src/publisher_tf.cpp) diff --git a/include/publisher_trajectory.h b/include/publisher_trajectory.h new file mode 100644 index 0000000000000000000000000000000000000000..5b35a3dc7c6e2077a38dde9f03d690cb17e401f0 --- /dev/null +++ b/include/publisher_trajectory.h @@ -0,0 +1,88 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// 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/>. +// +//--------LICENSE_END-------- +#ifndef PUBLISHER_TRAJECTORY_H +#define PUBLISHER_TRAJECTORY_H + +/************************** + * WOLF includes * + **************************/ +#include "core/problem/problem.h" + +#include "publisher.h" + +/************************** + * 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_; + + geometry_msgs::PoseArray pose_array_msg_; + visualization_msgs::Marker marker_msg_; + std_msgs::ColorRGBA marker_color_; + SensorBasePtr sensor_; + std::string frame_id_, map_frame_id_; + + ros::Publisher pub_path_, pub_marker_, pub_odometry_; + + public: + PublisherTrajectory(const std::string& _unique_name, + const ParamsServer& _server, + const ProblemPtr _problem); + WOLF_PUBLISHER_CREATE(PublisherTrajectory); + + virtual ~PublisherTrajectory(){}; + + void initialize(ros::NodeHandle &nh, const std::string& topic) override; + + void publishDerived() override; + + void publishTrajectory(); + + protected: + + bool listenTf(); + Eigen::Quaterniond q_frame_; + Eigen::Vector3d t_frame_; + tf::TransformListener tfl_; +}; + +WOLF_REGISTER_PUBLISHER(PublisherTrajectory) +} + +#endif diff --git a/package.xml b/package.xml index 59bc1f4488d9b7c5cf6f5e219c95d92f390c832c..de43c538101cdb9822e207f24ef42eb7766058f3 100644 --- a/package.xml +++ b/package.xml @@ -51,12 +51,14 @@ <buildtool_depend>catkin</buildtool_depend> <build_depend>roscpp</build_depend> <build_depend>sensor_msgs</build_depend> + <build_depend>nav_msgs</build_depend> <build_depend>std_msgs</build_depend> <build_depend>tf</build_depend> <build_depend>tf2_ros</build_depend> <!-- <build_depend>roslib</build_depend> --> <build_export_depend>roscpp</build_export_depend> <build_export_depend>sensor_msgs</build_export_depend> + <build_export_depend>nav_msgs</build_export_depend> <build_export_depend>std_msgs</build_export_depend> <build_export_depend>tf</build_export_depend> <build_export_depend>tf2_ros</build_export_depend> diff --git a/src/publisher_trajectory.cpp b/src/publisher_trajectory.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9d52ad9c053b79b480dc4bb87fbcc1d3b9a3af0a --- /dev/null +++ b/src/publisher_trajectory.cpp @@ -0,0 +1,261 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// 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/>. +// +//--------LICENSE_END-------- +#include "publisher_trajectory.h" + +/************************** + * ROS includes * + **************************/ +#include <ros/ros.h> +#include "tf/transform_datatypes.h" +#include "tf_conversions/tf_eigen.h" + +namespace wolf +{ + +PublisherTrajectory::PublisherTrajectory(const std::string& _unique_name, + const ParamsServer& _server, + 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"); + + // 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); + +} + +void PublisherTrajectory::publishDerived() +{ + if (pub_path_.getNumSubscribers() == 0 and + pub_marker_.getNumSubscribers() == 0) + return; + + VectorComposite current_state = problem_->getState("PO"); + TimeStamp loc_ts = problem_->getTimeStamp(); + + // state not ready + if (current_state.count('P') == 0 or + current_state.count('O') == 0 or + not loc_ts.ok()) + { + return; + } + + // fill vector and quaternion + 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 + { + 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_ 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); + + if (success) + { + if (problem_->getDim() == 2) + throw std::runtime_error("not implemented"); + else + std::copy(cov.data(), cov.data() + cov.size(), pose_with_cov_msg_.pose.covariance.data()); + } + else + { + //WOLF_WARN("Last KF covariance could not be recovered, using the previous one"); + //pose_with_cov_msg_.pose.covariance[0] = -1; // not valid + } + + // Fill Trajectory msg + pose_with_cov_msg_.header.stamp = ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds()); + pose_with_cov_msg_.pose.pose.position.x = p(0); + pose_with_cov_msg_.pose.pose.position.y = p(1); + pose_with_cov_msg_.pose.pose.position.z = p(2); + + pose_with_cov_msg_.pose.pose.orientation.x = q.x(); + pose_with_cov_msg_.pose.pose.orientation.y = q.y(); + pose_with_cov_msg_.pose.pose.orientation.z = q.z(); + pose_with_cov_msg_.pose.pose.orientation.w = q.w(); + publishTrajectory(); +} + +void PublisherTrajectory::publishTrajectory() +{ + // fill msgs and publish + if (pub_path_.getNumSubscribers() != 0) + { + path_msg_.header.stamp = pose_with_cov_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(pose_with_cov_msg_.pose.pose); + + pub_path_.publish(path_msg_); + } + if (pub_marker_.getNumSubscribers() != 0) + { + marker_msg_.header.stamp = pose_with_cov_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); + } + + marker_msg_.points.push_back(pose_with_cov_msg_.pose.pose.position); + + pub_marker_.publish(marker_msg_); + } + if (pub_pose_with_cov_.getNumSubscribers() != 0) + { + pub_pose_with_cov_.publish(pose_with_cov_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); + + return true; + } + return false; +} + +}