From 265dc233719c8e0606c74d3a7026bfb3e5c43c3e Mon Sep 17 00:00:00 2001
From: Wojciech Kwolek <me@irth.pl>
Date: Thu, 20 May 2021 12:51:28 +0000
Subject: [PATCH] publish charge/battery state (not tested, our pionieers don't
 do this)

---
 src/ros2aria/CMakeLists.txt   |  3 ++-
 src/ros2aria/src/publish.cpp  |  2 ++
 src/ros2aria/src/ros2aria.cpp |  3 +++
 src/ros2aria/src/ros2aria.hpp |  6 ++++++
 src/ros2aria/src/state.cpp    | 23 +++++++++++++++++++++++
 5 files changed, 36 insertions(+), 1 deletion(-)
 create mode 100644 src/ros2aria/src/state.cpp

diff --git a/src/ros2aria/CMakeLists.txt b/src/ros2aria/CMakeLists.txt
index 36928ce..57ec68f 100644
--- a/src/ros2aria/CMakeLists.txt
+++ b/src/ros2aria/CMakeLists.txt
@@ -38,7 +38,8 @@ add_executable(ros2aria
   src/pose.cpp
   src/gripper.cpp
   src/wheels.cpp
-  src/clutch.cpp)
+  src/clutch.cpp
+  src/state.cpp)
   
 ament_target_dependencies(ros2aria rclcpp)
 ament_target_dependencies(ros2aria std_srvs)
diff --git a/src/ros2aria/src/publish.cpp b/src/ros2aria/src/publish.cpp
index 3b8353d..80f5d19 100644
--- a/src/ros2aria/src/publish.cpp
+++ b/src/ros2aria/src/publish.cpp
@@ -14,4 +14,6 @@ void Ros2Aria::publish()
 
     sensor_msgs::msg::JointState wheels = handleWheels(t);
     publishWheels(wheels);
+
+    publishState(t);
 }
\ No newline at end of file
diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp
index bfccbc9..26ed9dd 100644
--- a/src/ros2aria/src/ros2aria.cpp
+++ b/src/ros2aria/src/ros2aria.cpp
@@ -22,6 +22,9 @@ Ros2Aria::Ros2Aria()
 
     wheels_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("wheels", 1000);
 
+    battery_recharge_state_pub_ = this->create_publisher<std_msgs::msg::Int8>("battery_recharge_state", 10); // TODO: original rosaria latches this - what does that mean and how does it relate to ros2?
+    battery_state_of_charge_pub_ = this->create_publisher<std_msgs::msg::Float32>("battery_state_of_charge", 10);
+
     // services
     stop_service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&Ros2Aria::stop, this, _1, _2));
     gripper_open_service_ = this->create_service<std_srvs::srv::Empty>("gripper_open", std::bind(&Ros2Aria::gripper_open_callback, this, _1, _2));
diff --git a/src/ros2aria/src/ros2aria.hpp b/src/ros2aria/src/ros2aria.hpp
index 9e9aa1f..80efaa1 100644
--- a/src/ros2aria/src/ros2aria.hpp
+++ b/src/ros2aria/src/ros2aria.hpp
@@ -8,6 +8,8 @@
 #include "nav_msgs/msg/odometry.hpp"
 #include "std_srvs/srv/empty.hpp"
 #include "std_msgs/msg/bool.hpp"
+#include "std_msgs/msg/int8.hpp"
+#include "std_msgs/msg/float32.hpp"
 
 #include "./raiibot.cpp"
 
@@ -44,6 +46,10 @@ private:
     rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr wheels_pub_;
     void publishWheels(sensor_msgs::msg::JointState wheels);
 
+    rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr battery_recharge_state_pub_;
+    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr battery_state_of_charge_pub_;
+    void publishState(rclcpp::Time stamp);
+
     // subscribers
     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
     void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
diff --git a/src/ros2aria/src/state.cpp b/src/ros2aria/src/state.cpp
new file mode 100644
index 0000000..9001fa5
--- /dev/null
+++ b/src/ros2aria/src/state.cpp
@@ -0,0 +1,23 @@
+#include "ros2aria.hpp"
+
+void Ros2Aria::publishState(rclcpp::Time stamp)
+{
+    auto r = this->robot->getRobot();
+
+    // TODO: original rosaria only publishes it when it changes.
+    if (this->battery_recharge_state_pub_->get_subscription_count() > 0)
+    {
+        char s = r->getChargeState();
+        std_msgs::msg::Int8 recharge_state;
+        recharge_state.data = s;
+        this->battery_recharge_state_pub_->publish(recharge_state);
+    }
+
+    // TODO: our robots don't have this, cannot test
+    if (r->haveStateOfCharge() && this->battery_state_of_charge_pub_->get_subscription_count() > 0)
+    {
+        std_msgs::msg::Float32 soc;
+        soc.data = r->getStateOfCharge() / 100.0;
+        this->battery_state_of_charge_pub_->publish(soc);
+    }
+}
\ No newline at end of file