From 8c30a04fdfd16c50359533ba200de8d8a83012ef Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 12 Jun 2020 14:39:24 +0200
Subject: [PATCH] new publisher
---
CMakeLists.txt | 2 ++
include/publisher_pose.h | 30 +++++++++++++++++
src/publisher_pose.cpp | 72 ++++++++++++++++++++++++++++++++++++++++
3 files changed, 104 insertions(+)
create mode 100644 include/publisher_pose.h
create mode 100644 src/publisher_pose.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 765e692..bd50787 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -151,6 +151,8 @@ add_executable(${PROJECT_NAME} src/node.cpp)
add_library(subscriber_${PROJECT_NAME}
src/subscriber_odom2d.cpp
src/subscriber_diffdrive.cpp)
+add_library(publisher_${PROJECT_NAME}
+ src/publisher_pose.cpp)
add_library(visualizer src/visualizer.cpp)
## Rename C++ executable without prefix
diff --git a/include/publisher_pose.h b/include/publisher_pose.h
new file mode 100644
index 0000000..4392b35
--- /dev/null
+++ b/include/publisher_pose.h
@@ -0,0 +1,30 @@
+#ifndef PUBLISHER_POSE_H
+#define PUBLISHER_POSE_H
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include "core/problem/problem.h"
+
+#include "factory_publisher.h"
+#include "publisher.h"
+
+namespace wolf
+{
+
+class PublisherPose: public Publisher
+{
+ std::string map_frame_id_;
+
+ public:
+ PublisherPose();
+ virtual ~PublisherPose(){};
+ void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+ void publish(const ProblemPtr problem) override;
+ static std::shared_ptr<Publisher> create();
+};
+
+WOLF_REGISTER_PUBLISHER(PublisherPose)
+}
+
+#endif
diff --git a/src/publisher_pose.cpp b/src/publisher_pose.cpp
new file mode 100644
index 0000000..9a81645
--- /dev/null
+++ b/src/publisher_pose.cpp
@@ -0,0 +1,72 @@
+#include "publisher_pose.h"
+
+/**************************
+ * ROS includes *
+ **************************/
+#include <ros/ros.h>
+#include "tf/transform_datatypes.h"
+#include <nav_msgs/Odometry.h>
+
+namespace wolf
+{
+
+PublisherPose::PublisherPose() :
+ Publisher()
+{
+}
+
+void PublisherPose::initialize(ros::NodeHandle& nh, const std::string& topic)
+{
+ publisher_ = nh.advertise<nav_msgs::Odometry>(topic, 1);
+ nh.param<std::string>("map_frame_id", map_frame_id_, "map");
+}
+
+void PublisherPose::publish(const ProblemPtr _problem)
+{
+ 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
+ loc_ts == TimeStamp(0))
+ {
+ return;
+ }
+
+ // Fill PoseStamped msg
+ nav_msgs::Odometry msg;
+ msg.header.frame_id = map_frame_id_;
+ msg.header.stamp = ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds());
+
+ // 2D
+ if (_problem->getDim() == 2)
+ {
+ msg.pose.pose.position.x = current_state["P"](0);
+ msg.pose.pose.position.y = current_state["P"](1);
+ msg.pose.pose.position.z = 0;
+
+ msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(current_state["O"](0));
+ }
+ // 3D
+ else
+ {
+ msg.pose.pose.position.x = current_state["P"](0);
+ msg.pose.pose.position.y = current_state["P"](1);
+ msg.pose.pose.position.z = current_state["P"](2);
+
+ msg.pose.pose.orientation.x = current_state["O"](0);
+ msg.pose.pose.orientation.y = current_state["O"](1);
+ msg.pose.pose.orientation.z = current_state["O"](2);
+ msg.pose.pose.orientation.w = current_state["O"](3);
+ }
+
+ publisher_.publish(msg);
+}
+
+std::shared_ptr<Publisher> PublisherPose::create()
+{
+ return std::make_shared<PublisherPose>();
+}
+
+}
--
GitLab