diff --git a/include/subscriber_gnss_fix_publisher_ecef.h b/include/subscriber_gnss_fix_publisher_ecef.h index 7881408704b5cead7f7cc4b794308353d69b9437..73588e56d9c8194347106047051c43b30ecb2c01 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 3ae30373a5a160168f07eac0a752adb620bb0e14..b146b6d1c726be5ed7f0b7c65ed81362554b07e8 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 2240096c675d8e5bb6f562f42a625d4c54319a70..d038bc0edefe171459b8dcd155a8785b97767ff4 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 6f46c056eee1b9637d66bf311d674213a0cf2f54..7050dd059f2652aae976ffd2ebf2416d6c182d63 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;