Hydrophones new type integration

catkin_ws/src/auv_msgs/msg/PingerBearing.msg

@@ -1,7 +1,5 @@
-geometry_msgs/Vector3 pinger1_bearing
-geometry_msgs/Vector3 pinger2_bearing
-geometry_msgs/Vector3 pinger3_bearing
-geometry_msgs/Vector3 pinger4_bearing
+int32 frequency_index
+geometry_msgs/Vector3 pinger_bearing
 
 float64 state_x
 float64 state_y

catkin_ws/src/auv_msgs/msg/PingerTimeDifference.msg

@@ -1,9 +1,2 @@
-bool is_pinger1_active
-bool is_pinger2_active
-bool is_pinger3_active
-bool is_pinger4_active
-
-float64[] dt_pinger1
-float64[] dt_pinger2
-float64[] dt_pinger3
-float64[] dt_pinger4
+int32 frequency
+uint32[] times

catkin_ws/src/bringup/launch/bringup.launch

@@ -9,6 +9,10 @@
 			<arg name="vision" value="$(arg vision)" />
 		</include>
 	</group>
+	<group if="$(arg sim)">
+		<include file="$(find sensors)/launch/sensors_status.launch"/>
+	</group>
+
 
 	<include file="$(find controls)/launch/controls.launch">
 		<arg name="sim" value="$(arg sim)" />

catkin_ws/src/sensors/launch/sensors_status.launch

@@ -1,5 +1,10 @@
 <launch>
      <param name="sensor_warning_interval" value="5"/> <!-- secs -->
      <param name="sensor_status_update_rate" value="10"/> <!-- Hz -->
+     <!-- Frequency of each pinger -->
+	<param name="pinger_frequency_1" value="1"/>
+	<param name="pinger_frequency_2" value="2"/>
+	<param name="pinger_frequency_3" value="3"/>
+	<param name="pinger_frequency_4" value="4"/>
      <node name="sensors_status" pkg="sensors" type="sensors_status.py" output="screen" respawn="true"/>
 </launch>

catkin_ws/src/sensors/src/sensors_status.py

@@ -231,9 +231,18 @@ class Hydrophones(Sensor):
     def __init__(self):
         super().__init__("Hydrophones")
 
-        self.are_pingers_active = [False, False, False, False]
-        self.current_reading = [None, None, None, None]
-        self.last_reading = [None, None, None, None]
+        # @TODO(Felipe): Subsbitute frequency_types values by 
+        #                real pinger values (waiting for Sam
+        #                to respond)  
+        self.frequency_types = [
+            rospy.get_param("pinger_frequency_1"), 
+            rospy.get_param("pinger_frequency_2"), 
+            rospy.get_param("pinger_frequency_3"), 
+            rospy.get_param("pinger_frequency_4")
+        ]
+        self.current_reading = [None] * len(self.frequency_types)
+        self.last_reading = [None] * len(self.frequency_types)
+        self.frequency_index = -1
 
         rospy.Subscriber(
             "/sensors/hydrophones/pinger_time_difference",
@@ -242,31 +251,20 @@ def __init__(self):
         )
 
     def hydrophones_cb(self, msg):
-        self.are_pingers_active = [
-            msg.is_pinger1_active,
-            msg.is_pinger2_active,
-            msg.is_pinger3_active,
-            msg.is_pinger4_active,
-        ]
-        self.current_reading = [
-            msg.dt_pinger1,
-            msg.dt_pinger2,
-            msg.dt_pinger3,
-            msg.dt_pinger4,
-        ]
-        self.update_last_reading()
+        self.frequency_index = self.frequency_types.index(msg.frequency)
+        if self.frequency_index != -1:
+            self.current_reading[self.frequency_index] = msg.times
+            self.update_last_reading()
 
     def update_last_reading(self):
         if self.current_reading != self.last_reading:
             self.last_unique_reading_time = rospy.get_time()
             self.last_reading = self.current_reading[:]
 
     def has_valid_data(self):
-        indexes_true = [i for i, e in enumerate(self.are_pingers_active) if e]
-        indexes_non_zero = [
-            i for i, e in enumerate(self.current_reading) if e != [0, 0, 0, 0]
-        ]
-        return len(indexes_true) != 0 and indexes_true == indexes_non_zero
+        return True
+        # return (self.frequency_index == -1 and 
+        #     all(element > 0 for element in self.current_reading[self.frequency_index]))
 
 
 # Actuator class inheriting from Sensor

catkin_ws/src/state_estimation/launch/hydrophones.launch

@@ -0,0 +1,20 @@
+<launch>
+     <!-- Distances to hydrophone 1 -->
+	<param name="hydrophones_dx" value="0.1"/>
+	<param name="hydrophones_dy" value="0.1"/>
+	<param name="hydrophones_dz" value="-0.1"/>
+	<!-- Frequency of each pinger -->
+	<param name="pinger_frequency_1" value="1"/>
+	<param name="pinger_frequency_2" value="2"/>
+	<param name="pinger_frequency_3" value="3"/>
+	<param name="pinger_frequency_4" value="4"/>
+
+	<param name="hydrophones_time_unit" value="0.0000001"/>
+
+	<node name="hydrophones_bearing"
+		pkg="state_estimation"
+		type="hydrophones_bearing.py"
+		output="screen"
+		respawn="true">
+	</node>
+</launch>

catkin_ws/src/state_estimation/launch/state_estimation.launch

@@ -17,28 +17,13 @@
 		<param name="auv_dvl_offset_x" value="0.0" />
 		<param name="auv_dvl_offset_y" value="0.0" />
 		<param name="auv_dvl_offset_z" value="-0.3" />
-
-		<param name="hydrophones_dx" value="0.1"/>
-		<param name="hydrophones_dy" value="0.1"/>
-		<param name="hydrophones_dz" value="-0.1"/>	<!-- For 4 Hydrophones -->
 
 		<param name="update_state_on_clock" value="false" />
 		<node name="state_aggregator" pkg="state_estimation" type="state_aggregator.py" respawn="true" output="screen"/>
 	</group>
 	<group if="$(arg sim)">
 		<param name="update_state_on_clock" value="true" />
 		<node name="state_aggregator" pkg="state_estimation" type="state_aggregator.py" respawn="true" output="screen"/>
-		<param name="hydrophones_dx" value="0.1"/>
-		<param name="hydrophones_dy" value="0.1"/>
-		<param name="hydrophones_dz" value="-0.1"/>	<!-- For 4 Hydrophones -->
-	</group>
-
-
-	<node name="hydrophones_bearing"
-		pkg="state_estimation"
-		type="hydrophones_bearing.py"
-		output="screen"
-		respawn="true">
-	</node>
-
+	</group>	
+	<include file="$(find state_estimation)/launch/hydrophones.launch"/>
 </launch>