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..2d855cbb42558dd6265b50a9ebd70080a623d160 --- /dev/null +++ b/include/publisher_trajectory.h @@ -0,0 +1,74 @@ +//--------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/>. +// +/* + * publisher_trajectory.h + * + * Created on: Feb 03, 2022 + * Author: igeer + */ +//--------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 <nav_msgs/Path.h> + +namespace wolf +{ + +class PublisherTrajectory: public Publisher +{ + nav_msgs::Path path_msg_; + + std::string frame_id_; + + ros::Publisher pub_path_; + + 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(); + +}; + +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..2e460865deae086cc7bdef8db6b6ad449c21c01a --- /dev/null +++ b/src/publisher_trajectory.cpp @@ -0,0 +1,109 @@ +//--------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-------- +/* + * publisher_trajectory.cpp + * + * Created on: Feb 03, 2022 + * Author: igeer + */ + +#include "publisher_trajectory.h" + +/************************** + * ROS includes * + **************************/ +#include <ros/ros.h> + +namespace wolf +{ + +PublisherTrajectory::PublisherTrajectory(const std::string& _unique_name, + const ParamsServer& _server, + const ProblemPtr _problem) : + Publisher(_unique_name, _server, _problem) +{ + frame_id_ = _server.getParam<std::string>(prefix_ + "/frame_id"); +} + +void PublisherTrajectory::initialize(ros::NodeHandle& nh, const std::string& topic) +{ + 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, 1); +} + +void PublisherTrajectory::publishDerived() +{ + if (pub_path_.getNumSubscribers() != 0 ) + publishTrajectory(); +} + +void PublisherTrajectory::publishTrajectory() +{ + path_msg_.header.stamp = ros::Time::now(); + + auto trajectory = problem_->getTrajectory(); + int frame_num = 0; + + //Fill path message with PoseStamped from trajectory + geometry_msgs::PoseStamped framepose; + Eigen::Vector3d p = Eigen::Vector3d::Zero(); + Eigen::Quaterniond q; + + for (auto frm: trajectory->getFrameMap()) + { + 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.head(2) = frm.second->getP()->getState(); + q = Eigen::Quaterniond(Eigen::AngleAxisd(frm.second->getO()->getState()(0), Eigen::Vector3d::UnitZ())); + } + else + { + p = frm.second->getP()->getState(); + q = Eigen::Quaterniond(Eigen::Vector4d(frm.second->getO()->getState())); + + } + 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); + } + + //Publish path + pub_path_.publish(path_msg_); + + //clear msg + path_msg_.poses.clear(); +} + +}