From 2022cb10f885c3f461157166fdd1a1df73da14bc Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 2 Jul 2020 16:09:42 +0200
Subject: [PATCH] subscribers changes
---
include/subscriber_gnss_fix_publisher_ecef.h | 6 ++++--
include/subscriber_gnss_receiver.h | 5 +++--
src/subscriber_gnss_fix_publisher_ecef.cpp | 14 ++++++++++++++
src/subscriber_gnss_receiver.cpp | 18 ++++++++++++++++++
4 files changed, 39 insertions(+), 4 deletions(-)
diff --git a/include/subscriber_gnss_fix_publisher_ecef.h b/include/subscriber_gnss_fix_publisher_ecef.h
index 7881408..73588e5 100644
--- a/include/subscriber_gnss_fix_publisher_ecef.h
+++ b/include/subscriber_gnss_fix_publisher_ecef.h
@@ -7,6 +7,7 @@
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/PoseArray.h>
+#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.h>
@@ -37,11 +38,12 @@ class SubscriberGnssFixPublisherEcef : public Subscriber
// publisher
ros::Time last_publish_stamp_;
double period_;
- bool pose_array_, marker_;
+ bool pose_array_, pose_, marker_;
geometry_msgs::PoseArray pose_array_msg_;
+ geometry_msgs::PoseStamped pose_msg_;
visualization_msgs::Marker marker_msg_;
std_msgs::ColorRGBA marker_color_;
- ros::Publisher pub_pose_array_, pub_marker_;
+ ros::Publisher pub_pose_array_, pub_marker_, pub_pose_;
public:
// Constructor
diff --git a/include/subscriber_gnss_receiver.h b/include/subscriber_gnss_receiver.h
index 3ae3037..b146b6d 100644
--- a/include/subscriber_gnss_receiver.h
+++ b/include/subscriber_gnss_receiver.h
@@ -60,16 +60,17 @@ class SubscriberGnssReceiver : public Subscriber
// Publish fix
void publishFixComp();
void publishPose(const geometry_msgs::Pose pose, const ros::Time& stamp, const std::string& comp);
- bool publish_fix_, pose_array_, marker_;
+ bool publish_fix_, pose_, pose_array_, marker_;
std::string topic_fix_;
GnssUtils::Options compute_pos_opt_;
// comparative
+ std::map<std::string, geometry_msgs::PoseStamped> pose_msgs_;
std::map<std::string, geometry_msgs::PoseArray> pose_array_msgs_;
std::map<std::string, visualization_msgs::Marker> marker_msgs_;
std::map<std::string, bool> last_fixes_ok_;
std::map<std::string, geometry_msgs::Pose> last_fixes_pose_;
- std::map<std::string, ros::Publisher> publishers_pose_array_, publishers_marker_;
+ std::map<std::string, ros::Publisher> publishers_pose_, publishers_pose_array_, publishers_marker_;
std::vector<std::string> comp_options_;
//TF
diff --git a/src/subscriber_gnss_fix_publisher_ecef.cpp b/src/subscriber_gnss_fix_publisher_ecef.cpp
index 2240096..d038bc0 100644
--- a/src/subscriber_gnss_fix_publisher_ecef.cpp
+++ b/src/subscriber_gnss_fix_publisher_ecef.cpp
@@ -14,6 +14,7 @@ SubscriberGnssFixPublisherEcef::SubscriberGnssFixPublisherEcef(const std::string
, ENU_defined_(false)
{
period_ = _server.getParam<double>(prefix_ + "/period");
+ pose_ = _server.getParam<bool>(prefix_ + "/pose_msg");
pose_array_ = _server.getParam<bool>(prefix_ + "/pose_array_msg");
marker_ = _server.getParam<bool>(prefix_ + "/marker_msg");
if (marker_)
@@ -32,6 +33,12 @@ void SubscriberGnssFixPublisherEcef::initialize(ros::NodeHandle& nh, const std::
sub_ = nh.subscribe(topic, 1, &SubscriberGnssFixPublisherEcef::callback, this);
// initialize msg and publisher
+ if (pose_)
+ {
+ pose_msg_.header.frame_id = "ENU";
+
+ pub_pose_ = nh.advertise<geometry_msgs::PoseStamped>(topic + "_ENU_pose", 1);
+ }
if (pose_array_)
{
pose_array_msg_.header.frame_id = "ENU";
@@ -103,6 +110,13 @@ bool SubscriberGnssFixPublisherEcef::listenTf()
void SubscriberGnssFixPublisherEcef::publishPose(const geometry_msgs::Pose pose, const ros::Time& stamp)
{
// fill msgs and publish
+ if (pose_)
+ {
+ pose_msg_.header.stamp = stamp;
+ pose_msg_.pose = pose;
+
+ pub_pose_.publish(pose_msg_);
+ }
if (pose_array_)
{
pose_array_msg_.header.stamp = stamp;
diff --git a/src/subscriber_gnss_receiver.cpp b/src/subscriber_gnss_receiver.cpp
index 6f46c05..7050dd0 100644
--- a/src/subscriber_gnss_receiver.cpp
+++ b/src/subscriber_gnss_receiver.cpp
@@ -30,6 +30,7 @@ SubscriberGnssReceiver::SubscriberGnssReceiver(const std::string& _unique_name,
comp_options_.push_back("200");
// params
+ pose_ = _server.getParam<bool>(prefix_ + "/publish_fix/pose_msg");
pose_array_ = _server.getParam<bool>(prefix_ + "/publish_fix/pose_array_msg");
marker_ = _server.getParam<bool>(prefix_ + "/publish_fix/marker_msg");
topic_fix_ = _server.getParam<std::string>(prefix_ + "/publish_fix/topic");
@@ -117,6 +118,16 @@ void SubscriberGnssReceiver::initialize(ros::NodeHandle& nh, const std::string&
if (publish_fix_)
{
// initialize msg and publisher
+ if (pose_)
+ {
+ for (auto comp : comp_options_)
+ {
+ pose_msgs_[comp] = geometry_msgs::PoseStamped();
+ pose_msgs_.at(comp).header.frame_id = "ENU";
+
+ publishers_pose_[comp] = nh.advertise<geometry_msgs::PoseStamped>(topic_fix_ + comp + "_pose", 1);
+ }
+ }
if (pose_array_)
{
for (auto comp : comp_options_)
@@ -233,6 +244,13 @@ void SubscriberGnssReceiver::publishFixComp()
void SubscriberGnssReceiver::publishPose(const geometry_msgs::Pose pose, const ros::Time& stamp, const std::string& comp)
{
// fill msgs and publish
+ if (pose_)
+ {
+ pose_msgs_.at(comp).header.stamp = stamp;
+ pose_msgs_.at(comp).pose = pose;
+
+ publishers_pose_.at(comp).publish(pose_msgs_.at(comp));
+ }
if (pose_array_)
{
pose_array_msgs_.at(comp).header.stamp = stamp;
--
GitLab