Merge pull request #167 from kas-lab/reaction_time

Reaction time

suave_managing/suave_bt/include/suave_bt/action_recharge_battery.hpp

@@ -33,7 +33,7 @@ class RechargeBattery :  public BT::StatefulActionNode{
 public:
   RechargeBattery(const std::string& name, const BT::NodeConfig & conf);
 
-  BT::NodeStatus onStart() override {return BT::NodeStatus::RUNNING;};
+  BT::NodeStatus onStart() override;
 
   BT::NodeStatus onRunning() override;
 

suave_managing/suave_bt/src/suave_bt/action_recharge_battery.cpp

@@ -31,6 +31,11 @@ namespace suave_bt
     recharged_ = msg.data;
   }
 
+  BT::NodeStatus RechargeBattery::onStart() {
+    recharged_ = false;
+    return BT::NodeStatus::RUNNING;
+  }
+
   BT::NodeStatus RechargeBattery::onRunning() {
     std::this_thread::sleep_for(std::chrono::milliseconds(50));
 

suave_managing/suave_bt/src/suave_bt/suave_mission.cpp

@@ -76,6 +76,7 @@ namespace suave_bt
   }
 
   void SuaveMission::set_search_started(){
+    if(search_started_ == true) return;
     start_time_ = this->get_clock()->now();
     search_started_ = true;
   }

suave_metrics/suave_metrics/mission_metrics.py

@@ -56,9 +56,12 @@ def __init__(self, node_name: str = 'suave_metrics'):
         self.declare_parameter('mission_name', 'inspection')
         # self.declare_parameter('metrics_header', [''])
         self.declare_parameter(
-            'water_visibiity_threshold', rclpy.Parameter.Type.DOUBLE_ARRAY)
+            'water_visibiity_threshold', [3.25, 2.25, 1.25])
         self.declare_parameter(
-            'expected_altitude', rclpy.Parameter.Type.DOUBLE_ARRAY)
+            'expected_altitude', [3.0, 2.0, 1.0])
+
+        self.declare_parameter(
+            'battery_limit', 0.25)
 
         #: path where results must be saved
         self.result_path = self.get_parameter('result_path').value
@@ -83,10 +86,15 @@ def __init__(self, node_name: str = 'suave_metrics'):
 
         self.wrong_altitude = False
         self.thrusters_failed = False
+        self.battery_low = False
         self.wrong_altitude_time = None
         self.thrusters_failed_time = None
+        self.battery_low_time = None
         self.component_recovery_time = []
         self.wv_reaction_time = []
+        self.battery_reaction_time = []
+
+        self.measured_wv = None
 
         self.state_sub = self.create_subscription(
             State,
@@ -148,6 +156,14 @@ def __init__(self, node_name: str = 'suave_metrics'):
             callback_group=ReentrantCallbackGroup()
         )
 
+        self.generate_recharge_path_node_state_sub = self.create_subscription(
+            TransitionEvent,
+            '/generate_recharge_path_node/transition_event',
+            self.generate_recharge_path_transition_cb,
+            self.qos,
+            callback_group=ReentrantCallbackGroup()
+        )
+
         self.param_change_sub = self.create_subscription(
             ParameterEvent,
             "/parameter_events",
@@ -208,16 +224,19 @@ def diagnostics_cb(self, msg):
         for diagnostic_status in msg.status:
             if diagnostic_status.message.lower() in measurement_messages:
                 self.check_altitude(diagnostic_status, time)
+                self.check_battery(diagnostic_status, time)
             if diagnostic_status.message.lower() in component_messages:
                 thrusters_ok = self.check_thrusther(diagnostic_status, time)
 
     def check_altitude(self, diagnostic_status, time):
         for value in diagnostic_status.values:
             if value.key == 'water_visibility':
+                self.measured_wv = float(value.value)
                 altitude = self.get_spiral_altitude()
                 expected = self.get_expected_spiral_altitude(value.value)
                 correct_altitude =  altitude == expected
-                if self.wrong_altitude is False and correct_altitude is False:
+                if self.battery_low is False and self.wrong_altitude is False \
+                 and correct_altitude is False:
                     self.wrong_altitude = True
                     self.wrong_altitude_time = time
                 return
@@ -229,8 +248,8 @@ def get_spiral_altitude(self):
 
     def get_expected_spiral_altitude(self, measured_wv):
         wv_threshold = self.get_parameter('water_visibiity_threshold').value
-        wv_expected = self.get_parameter('expected_altitude').value
-        for threshold, expected in zip(wv_threshold, wv_expected):
+        expected_altitude = self.get_parameter('expected_altitude').value
+        for threshold, expected in zip(wv_threshold, expected_altitude):
             if float(measured_wv) >= threshold:
                 return expected
         return None
@@ -243,6 +262,18 @@ def check_thrusther(self, diagnostic_status, time):
                     self.thrusters_failed_time = time
                 return
 
+    def check_battery(self, diagnostic_status, time):
+        for value in diagnostic_status.values:
+            if value.key == 'battery_level':
+                battery_limit = self.get_parameter('battery_limit').value
+                if self.battery_low is False and float(value.value) < battery_limit:
+                    self.battery_low = True
+                    self.battery_low_time = time
+                    self.wrong_altitude = False
+                if self.battery_low is True and float(value.value) > battery_limit:
+                    self.battery_low = False
+                return
+
     def maintain_motion_transition_cb(self, msg):
         if msg.goal_state.label == "active" and self.thrusters_failed is True:
             reaction_time = self.get_clock().now() - self.thrusters_failed_time
@@ -252,12 +283,24 @@ def maintain_motion_transition_cb(self, msg):
             self.get_logger().info(
                 'Component recovery time: {} seconds'.format(reaction_time))
 
+    def generate_recharge_path_transition_cb(self, msg):
+        if msg.goal_state.label == "active" and self.battery_low is True:
+            reaction_time = self.get_clock().now() - self.battery_low_time
+            reaction_time = reaction_time.nanoseconds * 1e-9
+            self.battery_reaction_time.append(reaction_time)
+            # self.battery_low = False
+            self.get_logger().info(
+                'Battery reaction time: {} seconds'.format(reaction_time))
+
     def param_change_cb(self, msg):
+        time = self.get_clock().now()
         if msg.node == "/f_generate_search_path_node"  \
          and self.wrong_altitude is True:
+            expected_altitude = self.get_expected_spiral_altitude(
+                self.measured_wv)
             for param in msg.changed_parameters:
-                if param.name == "spiral_altitude":
-                    reaction_time = self.get_clock().now() - self.wrong_altitude_time
+                if param.name == "spiral_altitude" and param.value.double_value == expected_altitude:
+                    reaction_time =  time - self.wrong_altitude_time
                     reaction_time = reaction_time.nanoseconds * 1e-9
                     self.wv_reaction_time.append(
                         reaction_time)
@@ -321,7 +364,9 @@ def save_mission_results(self) -> None:
             detection_time_delta,
             self.distance_inspected,
             statistics.fmean(
-                self.component_recovery_time + self.wv_reaction_time)
+                self.component_recovery_time +
+                self.wv_reaction_time +
+                self.battery_reaction_time)
         ]
 
         self.save_metrics(
@@ -348,6 +393,15 @@ def save_mission_results(self) -> None:
                 [self.mission_name, date, t]
             )
 
+        battery_file = self.result_filename +'_battery_reaction_time'
+        for t in self.battery_reaction_time:
+            self.save_metrics(
+                self.result_path,
+                battery_file,
+                ['mission_name', 'datetime', 'reaction time (s)'],
+                [self.mission_name, date, t]
+            )
+
         # TODO: make this a parameter
         os.system("touch ~/suave_ws/mission.done")
 

suave_missions/config/mission_config.yaml

@@ -3,6 +3,7 @@
     result_path: "~/suave/results" #Path to save results
     water_visibiity_threshold: [3.25, 2.25, 1.25]
     expected_altitude: [3.0, 2.0, 1.0]
+    battery_limit: 0.25
 
 /mission_node:
   ros__parameters:

suave_monitor/suave_monitor/battery_monitor.py

@@ -43,7 +43,7 @@ def __init__(self):
 
         self.battery_level = 1.0
 
-        self.get_interpolated_path_srv = self.create_service(
+        self.recharge_battery_srv = self.create_service(
             Trigger,
             'battery_monitor/recharge',
             self.recharge_battery_cb)
@@ -59,6 +59,9 @@ def status_cb(self, msg):
             self.destroy_subscription(self.mavros_state_sub)
 
     def qa_publisher_cb(self):
+        self.publish_battery_level()
+
+    def publish_battery_level(self):
         discharge_time = self.get_parameter('discharge_time').value
 
         current_time = self.get_clock().now().to_msg().sec
@@ -90,6 +93,7 @@ def qa_publisher_cb(self):
 
     def recharge_battery_cb(self, req, res):
         self.battery_level = 1.0
+        self.publish_battery_level()
         res.success = True
         self.battery_recharged_pub.publish(Bool(data=True))
         return res