Print battery status (#43)

* Print battery status * Update print format * Add param * Add percentage limit * Fix type
amslabtech · Nov 16, 2024 · debce9e · debce9e
1 parent e53eb95
commit debce9e
Show file tree

Hide file tree

Showing 3 changed files with 51 additions and 0 deletions.
diff --git a/include/motion_decision/motion_decision.h b/include/motion_decision/motion_decision.h
@@ -12,6 +12,7 @@
 #define RED "\033[31m"
 #define GREEN "\033[32m"
 #define CYAN "\033[36m"
+#define YELLOW "\033[33m"
 
 #include <geometry_msgs/Twist.h>
 #include <nav_msgs/Odometry.h>
@@ -22,6 +23,7 @@
 #include <sensor_msgs/LaserScan.h>
 #include <std_msgs/Bool.h>
 #include <std_srvs/SetBool.h>
+#include <std_msgs/Float32.h>
 #include <string>
 #include <utility>
 
@@ -86,6 +88,14 @@ struct LaserInfo
   float rear_min_range = -1.0;
 };
 
+struct BatteryInfo
+{
+  float full_charge_voltage = 0.0;
+  float cutoff_voltage = 0.0;
+  float current_percentage = 0.0;
+  bool used = false;
+};
+
 /**
  * @class MotionDecision
  * @brief Motion Decision Class
@@ -154,6 +164,12 @@ class MotionDecision
    */
   void local_map_callback(const nav_msgs::OccupancyGridConstPtr &msg);
 
+  /**
+   * @brief Battery voltage callback function
+   * @param [in] msg Msg of battery voltage
+   */
+  void battery_voltage_callback(const std_msgs::Float32ConstPtr &msg);
+
   /**
    * @brief Recovery mode flag callback function
    * @details This is not flag to run recovery mode. This is flag to use recovery mode.
@@ -263,6 +279,7 @@ class MotionDecision
   Flags flags_;
   Counters counters_;
   LaserInfo laser_info_;
+  BatteryInfo battery_info_;
 
   // motiom mode {first: <stop, move>, second: <manual, auto>}
   std::pair<std::string, std::string> mode_ = std::make_pair("stop", "manual");
@@ -278,6 +295,7 @@ class MotionDecision
   ros::Subscriber local_map_sub_;
   ros::Subscriber odom_sub_;
   ros::Subscriber rear_laser_sub_;
+  ros::Subscriber battery_voltage_sub_;
   ros::ServiceServer recovery_mode_flag_server_;
   ros::ServiceServer task_stop_flag_server_;
 

diff --git a/launch/motion_decision.launch b/launch/motion_decision.launch
@@ -29,6 +29,9 @@
     <arg name="recovery/yawrate_resolution" default="0.1"/>
     <arg name="recovery/spin_turn_speed" default="0.2"/>
     <arg name="recovery/time" default="3.0"/>
+    <!-- BatteryParams -->
+    <arg name="battery/full_charge_voltage" default="24.0"/>
+    <arg name="battery/cutoff_voltage" default="20.0"/>
     <!-- Published topic -->
     <arg name="cmd_vel" default="/cmd_vel"/>
     <!-- Subscribed topics -->
@@ -37,6 +40,7 @@
     <arg name="odom" default="/odom"/>
     <arg name="front_laser/scan" default="/front_laser/scan"/>
     <arg name="rear_laser/scan" default="/rear_laser/scan"/>
+    <arg name="battery_voltage" default="/battery_voltage"/>
 
     <!-- run motion_decision node -->
     <node pkg="motion_decision" type="motion_decision" name="motion_decision" ns="$(arg ns)" output="$(arg output)" respawn="true">
@@ -66,6 +70,9 @@
         <param name="recovery/yawrate_resolution" value="$(arg recovery/yawrate_resolution)"/>
         <param name="recovery/spin_turn_speed" value="$(arg recovery/spin_turn_speed)"/>
         <param name="recovery/time" value="$(arg recovery/time)"/>
+        <!-- BatteryParams -->
+        <param name="battery/full_charge_voltage" value="$(arg battery/full_charge_voltage)"/>
+        <param name="battery/cutoff_voltage" value="$(arg battery/cutoff_voltage)"/>
         <!-- Published topic -->
         <remap from="/cmd_vel" to="$(arg cmd_vel)"/>
         <!-- Subscribed topics -->
@@ -74,5 +81,6 @@
         <remap from="/odom" to="$(arg odom)"/>
         <remap from="/front_laser/scan" to="$(arg front_laser/scan)"/>
         <remap from="/rear_laser/scan" to="$(arg rear_laser/scan)"/>
+        <remap from="/battery_voltage" to="$(arg battery_voltage)"/>
     </node>
 </launch>
diff --git a/src/motion_decision.cpp b/src/motion_decision.cpp
@@ -24,6 +24,7 @@ MotionDecision::MotionDecision(void) : private_nh_("~")
   local_map_sub_ = nh_.subscribe("/local_map", 1, &MotionDecision::local_map_callback, this);
   odom_sub_ = nh_.subscribe("/odom", 1, &MotionDecision::odom_callback, this);
   rear_laser_sub_ = nh_.subscribe("/rear_laser/scan", 1, &MotionDecision::rear_laser_callback, this);
+  battery_voltage_sub_ = nh_.subscribe("/battery_voltage", 1, &MotionDecision::battery_voltage_callback, this);
 
   recovery_mode_flag_server_ =
       nh_.advertiseService("/recovery/available", &MotionDecision::recovery_mode_flag_callback, this);
@@ -64,6 +65,10 @@ void MotionDecision::load_params(void)
   private_nh_.param<float>("recovery/yawrate_resolution", params_of_recovery_.yawrate_resolution, 0.1);
   private_nh_.param<float>("recovery/spin_turn_speed", params_of_recovery_.spin_turn_speed, 0.2);
   private_nh_.param<float>("recovery/time", params_of_recovery_.time, 3.0);
+
+  // BatteryInfo
+  private_nh_.param<float>("battery/full_charge_voltage", battery_info_.full_charge_voltage, 24.0);
+  private_nh_.param<float>("battery/cutoff_voltage", battery_info_.cutoff_voltage, 20.0);
 }
 
 void MotionDecision::front_laser_callback(const sensor_msgs::LaserScanConstPtr &msg)
@@ -161,6 +166,14 @@ void MotionDecision::local_map_callback(const nav_msgs::OccupancyGridConstPtr &m
   counters_.not_received_rear_laser = 0;
 }
 
+void MotionDecision::battery_voltage_callback(const std_msgs::Float32ConstPtr &msg)
+{
+  battery_info_.current_percentage = (msg->data - battery_info_.cutoff_voltage) /
+                                     (battery_info_.full_charge_voltage - battery_info_.cutoff_voltage) * 100.0;
+  battery_info_.current_percentage = std::min(100.0f, battery_info_.current_percentage);
+  battery_info_.used = true;
+}
+
 bool MotionDecision::recovery_mode_flag_callback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
 {
   params_of_recovery_.available = req.data;
@@ -591,6 +604,18 @@ void MotionDecision::print_status(const geometry_msgs::Twist &cmd_vel)
     std::cout << "### intersection ###" << std::endl;
     std::cout << "####################" << std::endl;
   }
+  if (battery_info_.used)
+  {
+    if (battery_info_.current_percentage < 25.0)
+      std::cout << "battery         : " << RED << std::fixed << std::setprecision(1) << battery_info_.current_percentage
+                << " %" << RESET_COLOR << std::endl;
+    else if (battery_info_.current_percentage < 50.0)
+      std::cout << "battery         : " << YELLOW << std::fixed << std::setprecision(1)
+                << battery_info_.current_percentage << " %" << RESET_COLOR << std::endl;
+    else
+      std::cout << "battery         : " << std::fixed << std::setprecision(1) << battery_info_.current_percentage
+                << " %" << std::endl;
+  }
   if (laser_info_.front_min_range == -1.0)
     std::cout << "min front laser : " << RED << laser_info_.front_min_range << RESET_COLOR << std::endl;
   else