diff --git a/ros/rviz/kiss_icp.rviz b/ros/rviz/kiss_icp.rviz index 918ff216..7320bdd0 100644 --- 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 index cfc68d55..dfdc975e 100644 --- a/ros/src/OdometryServer.cpp +++ b/ros/src/OdometryServer.cpp @@ -40,7 +40,6 @@ #include #include #include -#include #include #include #include @@ -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("/kiss/odometry", qos); - traj_publisher_ = create_publisher("/kiss/trajectory", qos); - path_msg_.header.frame_id = odom_frame_; if (publish_debug_clouds_) { frame_publisher_ = create_publisher("/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 index 3be0c189..7ca7ff8c 100644 --- a/ros/src/OdometryServer.hpp +++ b/ros/src/OdometryServer.hpp @@ -31,7 +31,6 @@ #include #include -#include #include #include #include @@ -80,10 +79,6 @@ class OdometryServer : public rclcpp::Node { rclcpp::Publisher::SharedPtr kpoints_publisher_; rclcpp::Publisher::SharedPtr map_publisher_; - /// Path publisher - nav_msgs::msg::Path path_msg_; - rclcpp::Publisher::SharedPtr traj_publisher_; - /// KISS-ICP std::unique_ptr kiss_icp_;