Skip to content

Commit 2cf9779

Browse files
author
Long Vuong
committed
feat [ROS]: pub output as pose with cov msg type
1 parent 3f08908 commit 2cf9779

File tree

2 files changed

+8
-0
lines changed

2 files changed

+8
-0
lines changed

ros/src/OdometryServer.cpp

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -90,6 +90,7 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
9090
// Initialize publishers
9191
rclcpp::QoS qos((rclcpp::SystemDefaultsQoS().keep_last(1).durability_volatile()));
9292
odom_publisher_ = create_publisher<nav_msgs::msg::Odometry>("kiss/odometry", qos);
93+
pose_publisher_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("kiss/pose", qos);
9394
if (publish_debug_clouds_) {
9495
frame_publisher_ = create_publisher<sensor_msgs::msg::PointCloud2>("kiss/frame", qos);
9596
kpoints_publisher_ = create_publisher<sensor_msgs::msg::PointCloud2>("kiss/keypoints", qos);
@@ -221,6 +222,11 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &kiss_pose,
221222
odom_msg.pose.covariance[28] = orientation_covariance_;
222223
odom_msg.pose.covariance[35] = orientation_covariance_;
223224
odom_publisher_->publish(std::move(odom_msg));
225+
226+
geometry_msgs::msg::PoseWithCovarianceStamped pose_msg;
227+
pose_msg.header = odom_msg.header;
228+
pose_msg.pose = odom_msg.pose;
229+
pose_publisher_->publish(pose_msg);
224230
}
225231

226232
void OdometryServer::PublishClouds(const std::vector<Eigen::Vector3d> &frame,

ros/src/OdometryServer.hpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727

2828
// ROS 2
2929
#include <nav_msgs/msg/odometry.hpp>
30+
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
3031
#include <rclcpp/rclcpp.hpp>
3132
#include <sensor_msgs/msg/point_cloud2.hpp>
3233
#include <std_msgs/msg/header.hpp>
@@ -76,6 +77,7 @@ class OdometryServer : public rclcpp::Node {
7677

7778
/// Data publishers.
7879
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_;
80+
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_publisher_;
7981
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr frame_publisher_;
8082
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr kpoints_publisher_;
8183
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr map_publisher_;

0 commit comments

Comments
 (0)