Adapted to python node's implementation and workaround for issue #16

UoY-RoboStar · Aug 30, 2023 · 062e74d · 062e74d
1 parent e135735
commit 062e74d
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 21 deletions.
diff --git a/examples/bmon/gen.xml b/examples/bmon/gen.xml
@@ -18,14 +18,15 @@
         </catkin>
 
         <include src="std_msgs/Float32.h" />
-        <include src="sensor_msgs/BatteryState.h" />
+        <!--<include src="sensor_msgs/BatteryState.h" />-->
+        <include src="battery_monitor/BatteryState.h" />
         <include src="battery_monitor/BatteryStatus.h" />
         <include src="battery_monitor/InputAccepted.h" />
 
         <!-- Tell the generator that this channel maps non-trivially to a ROS type: -->
-        <channel name="batteryInfo.in" type="sensor_msgs::BatteryState" topic="/fcs/battery_state" />
-        <channel name="batteryStatus.out" type="battery_monitor::BatteryStatus" topic="/battery_monitor/battery_status" />
-        <channel name="inputAccepted" type="battery_monitor::InputAccepted" topic="/battery_monitor/input_accepted" />
+        <channel name="batteryInfo.in" type="battery_monitor::BatteryState" topic="/battery_state" />
+        <channel name="batteryStatus.out" type="battery_monitor::BatteryStatus" topic="/battery_status" />
+        <channel name="inputAccepted" type="battery_monitor::InputAccepted" topic="/input_accepted" />
 
     </cpp>
 </rtcg-gen>
diff --git a/examples/bmon/ros/src/convert/convert.cpp b/examples/bmon/ros/src/convert/convert.cpp
@@ -4,18 +4,18 @@
 // BatteryState messages.
 
 #include "convert.h"
-#include "sensor_msgs/BatteryState.h"
+#include "battery_monitor/BatteryState.h"
 #include "battery_monitor/BatteryStatus.h"
 
-unsigned int fromBatteryInfo(const sensor_msgs::BatteryState::ConstPtr& msg)
+unsigned int fromBatteryInfo(const BatteryInfoMsg msg)
 {
   // Model is nat 0-100, ROS is float 0.0-1.0
   return static_cast<unsigned int>(msg->percentage * 100);
 }
 
-sensor_msgs::BatteryState toBatteryInfo(unsigned int value)
+BatteryInfoVal toBatteryInfo(unsigned int value)
 {
-  sensor_msgs::BatteryState msg;
+  BatteryInfoVal msg;
 
   msg.percentage = static_cast<float>(value) / 100.0;
 
@@ -24,16 +24,25 @@ sensor_msgs::BatteryState toBatteryInfo(unsigned int value)
 
 std::string fromBatteryStatus(const BatteryStatusMsg msg)
 {
-  switch (msg->status) {
-    case battery_monitor::BatteryStatus::OK: 
-      return "Ok";
-    case battery_monitor::BatteryStatus::MISSION_CRITICAL: 
-      return "MissionCritical";
-    case battery_monitor::BatteryStatus::SAFETY_CRITICAL:
-      return "SafetyCritical";
-    default:
-      return "Invalid";
+  if (!msg->status.compare("OK")) {
+    return "Ok";
+  } else if (!msg->status.compare("MC")) {
+    return "MissionCritical";
+  } else if (!msg->status.compare("SC")) {
+    return "SafetyCritical";
+  } else {
+    return "Invalid";
   }
+  // switch (msg->status) {
+  //   case battery_monitor::BatteryStatus::OK: 
+  //     return "Ok";
+  //   case battery_monitor::BatteryStatus::MISSION_CRITICAL: 
+  //     return "MissionCritical";
+  //   case battery_monitor::BatteryStatus::SAFETY_CRITICAL:
+  //     return "SafetyCritical";
+  //   default:
+  //     return "Invalid";
+  // }
   //return std::string(msg->data.c_str());
 }
 
@@ -42,14 +51,18 @@ BatteryStatusVal toBatteryStatus(const std::string value)
   BatteryStatusVal msg;
   //std_msgs::String msg;
   //msg.data = value.c_str();
-  msg.status = BatteryStatusVal::UNSET;
+  //msg.status = BatteryStatusVal::UNSET;
+  msg.status = "UNSET";
 
   if (value.compare("Ok")) {
-    msg.status = BatteryStatusVal::OK;
+    //msg.status = BatteryStatusVal::OK;
+    msg.status = "OK";
   } else if (value.compare("MissionCritical")) {
-    msg.status = BatteryStatusVal::MISSION_CRITICAL;
+    //msg.status = BatteryStatusVal::MISSION_CRITICAL;
+    msg.status = "MC";
   } else if (value.compare("SafetyCritical")) {
-    msg.status = BatteryStatusVal::SAFETY_CRITICAL;
+    //msg.status = BatteryStatusVal::SAFETY_CRITICAL;
+    msg.status = "SC";
   }
 
   return msg;

diff --git a/internal/gen/catkin/embed/templates/CMakeLists.txt.tmpl b/internal/gen/catkin/embed/templates/CMakeLists.txt.tmpl
@@ -35,6 +35,9 @@ if (NOT TARGET convert)
     add_library(convert
         ../convert/convert.cpp
     )
+    add_dependencies(convert
+        battery_monitor_generate_messages_cpp
+    )
 endif()
 
 if (NOT TARGET rtcg)