From 6e1b824288740d9f30b635c45763d8993a438d9a Mon Sep 17 00:00:00 2001
From: Davide Graziato <dgraziato10@gmail.com>
Date: Fri, 19 Jan 2024 11:56:34 +0100
Subject: [PATCH] Topic names refactor and readme changes.

---
 README.md                     |  3 ++-
 include/imu_zupt/imu_zupt.hpp |  4 ++--
 params/params.yaml            |  4 ++--
 src/imu_zupt.cpp              | 12 ++++++------
 4 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/README.md b/README.md
index 71b75ae..cfd8bb2 100644
--- a/README.md
+++ b/README.md
@@ -9,7 +9,8 @@ The cumulative error while stationary due to the movement of the Earth on the Ya
 ## Parametrization:
 - topics:
     - imu: Input topic for the IMU data. (sensor_msgs::msg::Imu)
-    - rover_status: Input topic for the system's status. (std_msgs::msg::Bool)
+    - input_status: Input topic for the system's status. (std_msgs::msg::Bool)
+    - filter_status: Output topic that publish the filter's state. (std_msgs::msg::Bool)
     - output: output topic for the filtered measurements. (sensor_msgs::msg::Imu)
 - zero_velocity_detection: 
     - seconds: time frame (in sec) which the algorithm will wait to establish if the stationary conditions are met. (double) 
diff --git a/include/imu_zupt/imu_zupt.hpp b/include/imu_zupt/imu_zupt.hpp
index 6c3c724..8d0cb0e 100644
--- a/include/imu_zupt/imu_zupt.hpp
+++ b/include/imu_zupt/imu_zupt.hpp
@@ -51,8 +51,8 @@ class ImuZupt : public rclcpp::Node
   std::string source_topic_imu;
   std::string dest_topic;
   std::string err_topic;
-  std::string status_topic;
-  std::string rover_status_topic;
+  std::string filter_status_topic;
+  std::string input_status_topic;
   bool publish_status;
   bool publish_err;
   bool use_degree;
diff --git a/params/params.yaml b/params/params.yaml
index 65ee475..74327a2 100644
--- a/params/params.yaml
+++ b/params/params.yaml
@@ -1,11 +1,11 @@
 imu_zupt:
   ros__parameters:
     topics:
-      locomotion: /locomotion/odom
       imu: /imu/data
       output: /imu/data_zupt
       error: /imu/yaw_err
-      status: /imu_zupt/active
+      filter_status: /imu_zupt/active
+      input_status: /input_status
     covariance:
       override: true
       value: 0.001
diff --git a/src/imu_zupt.cpp b/src/imu_zupt.cpp
index d65f514..0ed7ae3 100644
--- a/src/imu_zupt.cpp
+++ b/src/imu_zupt.cpp
@@ -15,18 +15,18 @@ ImuZupt::ImuZupt(const rclcpp::NodeOptions & options)
   source_topic_imu = declare_parameter<std::string>("topics.imu", "/imu/data");
   dest_topic = declare_parameter<std::string>("topics.output", "/imu/data_zupt");
   err_topic = declare_parameter<std::string>("topics.error", "/imu/yaw_err");
-  status_topic =
-    declare_parameter<std::string>("topics.zero_velocity_detected", "/imu_zupt/active");
+  filter_status_topic =
+    declare_parameter<std::string>("topics.filter_status", "/imu_zupt/active");
   publish_err = declare_parameter<bool>("error.publish", true);
   use_degree = declare_parameter<bool>("error.in_degrees", true);
   override_covariance = declare_parameter<bool>("covariance.override", true);
   covariance = declare_parameter<double>("covariance.value", 0.001);
   publish_status = declare_parameter<bool>("zero_velocity_detection.publish", true);
-  rover_status_topic = declare_parameter<std::string>("topics.rover_status", "/rover_status");
+  input_status_topic = declare_parameter<std::string>("topics.input_status", "/input_status");
 
   rover_status_subs_ =
     create_subscription<std_msgs::msg::Bool>(
-    rover_status_topic, 1,
+    input_status_topic, 1,
     std::bind(&ImuZupt::status_callback, this, _1));
   imu_subs_ =
     create_subscription<sensor_msgs::msg::Imu>(
@@ -34,14 +34,14 @@ ImuZupt::ImuZupt(const rclcpp::NodeOptions & options)
     std::bind(&ImuZupt::imu_callback, this, _1));
   zupt_publ_ = create_publisher<sensor_msgs::msg::Imu>(dest_topic, 1);
   err_publ_ = create_publisher<std_msgs::msg::Float64>(err_topic, 1);
-  status_publ_ = create_publisher<std_msgs::msg::Bool>(status_topic, 1);
+  status_publ_ = create_publisher<std_msgs::msg::Bool>(filter_status_topic, 1);
 }
 
 void ImuZupt::imu_callback(const sensor_msgs::msg::Imu::SharedPtr imu_msg)
 {
   if (!active) {return;}
   std::unique_ptr<sensor_msgs::msg::Imu> pkt_imu_zupt(new sensor_msgs::msg::Imu);
-  pkt_imu_zupt->header = imu_msg->header;
+  *pkt_imu_zupt = *imu_msg;
 
   tf2::fromMsg(imu_msg->orientation, q1);
   tf2::Matrix3x3 m(q1);