Remove unnecesary path publishing pipeline (#284)

PRBonn · Mar 18, 2024 · 8450d58 · 8450d58
1 parent a71c0e7
commit 8450d58
Show file tree

Hide file tree

Showing 3 changed files with 0 additions and 44 deletions.
diff --git a/ros/rviz/kiss_icp.rviz b/ros/rviz/kiss_icp.rviz
@@ -174,34 +174,6 @@ Visualization Manager:
             Reliability Policy: Best Effort
             Value: /kiss/odometry
           Value: true
-        - Alpha: 1
-          Buffer Length: 1
-          Class: rviz_default_plugins/Path
-          Color: 53; 132; 228
-          Enabled: true
-          Head Diameter: 0.30000001192092896
-          Head Length: 0.20000000298023224
-          Length: 0.30000001192092896
-          Line Style: Billboards
-          Line Width: 0.20000000298023224
-          Name: trajectory
-          Offset:
-            X: 0
-            Y: 0
-            Z: 0
-          Pose Color: 255; 85; 255
-          Pose Style: None
-          Radius: 0.029999999329447746
-          Shaft Diameter: 0.10000000149011612
-          Shaft Length: 0.10000000149011612
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Best Effort
-            Value: /kiss/trajectory
-          Value: true
       Enabled: true
       Name: odometry
     - Class: rviz_default_plugins/Axes

diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
@@ -40,7 +40,6 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
-#include <nav_msgs/msg/path.hpp>
 #include <rclcpp/qos.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -86,8 +85,6 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
     // Initialize publishers
     rclcpp::QoS qos((rclcpp::SystemDefaultsQoS().keep_last(1).durability_volatile()));
     odom_publisher_ = create_publisher<nav_msgs::msg::Odometry>("/kiss/odometry", qos);
-    traj_publisher_ = create_publisher<nav_msgs::msg::Path>("/kiss/trajectory", qos);
-    path_msg_.header.frame_id = odom_frame_;
     if (publish_debug_clouds_) {
         frame_publisher_ = create_publisher<sensor_msgs::msg::PointCloud2>("/kiss/frame", qos);
         kpoints_publisher_ =
@@ -161,14 +158,6 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &pose,
         tf_broadcaster_->sendTransform(transform_msg);
     }
 
-    // publish trajectory msg
-    geometry_msgs::msg::PoseStamped pose_msg;
-    pose_msg.header.stamp = stamp;
-    pose_msg.header.frame_id = odom_frame_;
-    pose_msg.pose = tf2::sophusToPose(pose);
-    path_msg_.poses.push_back(pose_msg);
-    traj_publisher_->publish(path_msg_);
-
     // publish odometry msg
     nav_msgs::msg::Odometry odom_msg;
     odom_msg.header.stamp = stamp;

diff --git a/ros/src/OdometryServer.hpp b/ros/src/OdometryServer.hpp
@@ -31,7 +31,6 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <nav_msgs/msg/odometry.hpp>
-#include <nav_msgs/msg/path.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <string>
@@ -80,10 +79,6 @@ class OdometryServer : public rclcpp::Node {
     rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr kpoints_publisher_;
     rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr map_publisher_;
 
-    /// Path publisher
-    nav_msgs::msg::Path path_msg_;
-    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr traj_publisher_;
-
     /// KISS-ICP
     std::unique_ptr<kiss_icp::pipeline::KissICP> kiss_icp_;