Merge branch 'publisher_trajectory' into 'devel'

WP: Publisher Trajectory

See merge request !9

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)
 

include/publisher_trajectory.h

+//--------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

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>

src/publisher_trajectory.cpp

+//--------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();
+}
+
+}