Multiple gliders

.gitignore

@@ -1 +1,2 @@
+*.*~
 .vscode

README.md

@@ -3,8 +3,12 @@ Kinematics control plugin for WHOI hybrid gliders
 
 ## Requirements
 ```diff
+- It's using specific dave repo branch : tidal_current_fix_before_reorg (May,11, 2021)
+- https://github.com/Field-Robotics-Lab/DAVE/tree/tidal_current_fix_before_reorg
 - The IMU/GPS sensor included in this repo requires hector libraries. You may install with following command
 sudo apt-get install ros-melodic-hector-gazebo-plugins
+- The kinematics/dynamics plugin uses UwGliderStatus/UwGliderCommand msg to interact with the vehicle
+git clone https://github.com/Field-Robotics-Lab/frl_msgs
 ```
 
 ## How-to
@@ -23,6 +27,7 @@ sudo apt-get install ros-melodic-hector-gazebo-plugins
          3. Run the container with `run.bash` script
              ```
              ./run.bash
+             source ~/glider_hybrid_whoi/install/setup.bash
              ```
          * Opening additional terminals
              ```
@@ -35,16 +40,20 @@ sudo apt-get install ros-melodic-hector-gazebo-plugins
 ### Quickstart
 * Running the simulator (Run each commands in separate terminal window)
     1. Spawn underwater world with gazebo
-        ```
-        roslaunch glider_hybrid_whoi_gazebo start_demo_kinematics.launch
+        ```bash
+        roslaunch glider_hybrid_whoi_gazebo start_demo_kinematics_stratified_current.launch
+        # or
+        roslaunch glider_hybrid_whoi_gazebo BuzzBay_stratified_current.launch
+        # if in docker environment
+        roslaunch glider_hybrid_whoi_gazebo BuzzBay_stratified_current_docker.launch
         ```
     2. Control glider with ROS
         ```
-        rosrun direct_kinematics_ros_plugins test_directKinematics.py
+        rosrun kinematics_ros_plugins test_directKinematics.py
         ```
     4. Glider status
         ```
-        rostopic echo /glider_hybrid_whoi/direct_kinematics/UwGliderStatus
+        rostopic echo /glider_hybrid_whoi/kinematics/UwGliderStatus
         ```
 ## Interface
 ![alt text](https://github.com/Field-Robotics-Lab/glider_hybrid_whoi/blob/master/uw_glider_interface.png?raw=true)
@@ -58,13 +67,19 @@ provided by https://gitlab.com/mit-mers/ros/slocum_glider
 - Buzzbay bathymetry is included
   - Roughly 1500x1500 m tiles with 50 m overlap regions are included (almost 780 MB)
   - Click the `play` button on the Gazebo window and wait for the first bathymetry to be spawned. Next bathymetry tile will be spawned and the previous tile will be removed automatically according to the glider position.
-  ```
-  roslaunch glider_hybrid_whoi_gazebo BuzzBay.launch
+  ```bash
+  roslaunch glider_hybrid_whoi_gazebo BuzzBay_stratified_current.launch
+  # if in docker environment
+  roslaunch glider_hybrid_whoi_gazebo BuzzBay_stratified_current_docker.launch
   ```
 
 ### Surface detection
 - If the glider reach the surface, the pitch value is set to zero and the position is kept on surface unless it's heading back down.
 
+### Multiple gliders support
+- https://github.com/Field-Robotics-Lab/glider_hybrid_whoi/pull/31
+- Demo case : `roslaunch glider_hybrid_whoi_gazebo start_demo_kinematics_stratified_current_two_gliders.launch`
+
 ### Glider dynamic parameters
 - Parametes for pitch control, buoyancy induced velocity with the flight model, and thruster power is defined at [glider_hybrid_whoi_base_kinematics.xacro](https://github.com/Field-Robotics-Lab/glider_hybrid_whoi/blob/10524388cce32865ae051e285dbe631ea89159e4/glider_hybrid_whoi_description/urdf/glider_hybrid_whoi_base_kinematics.xacro#L139)
 #### Pitch control

docker/Dockerfile

@@ -5,13 +5,13 @@
 
 #------------------------------------------------------------------#
 # Dependency versions
-ARG version_dave=c595d7dce247edd7e66f4c4944deb71d00159203
+ARG version_dave=e6f6aa517422ba2ce2e0a8db430462bd2d57cdf3
 ARG version_uuv_simulator=0.6.13
 ARG version_uuv_manipulators=0.6.1
-ARG version_uw_sensors_gazebo=v3.1.0
+ARG version_uw_sensors_gazebo=b9083c054c48fc9553eddeadae5dec447058607a
 ARG ds_sim_commit=36835b85c30f5e063384378992f6ea60b5410d66
 ARG ds_msgs_commit=4a7d4a5002d17631e67eb01c45b974293fe5f3c2
-ARG frl_msgs_commit=f2eb86e1a82445006c9c49b3508bd44a0de54fd7
+ARG frl_msgs_commit=a41b1d3d192dacd32a7e0a717a9c1b0a30eaca64
 #------------------------------------------------------------------#
 
 #------------------------------------------------------------------#

docker/Dockerfile.dev

@@ -2,14 +2,14 @@
 
 #------------------------------------------------------------------#
 # Dependency versions
-ARG version_dave=c595d7dce247edd7e66f4c4944deb71d00159203
+ARG version_dave=5929bbbd19457ee3267fdb4b51972a922a65c002
 ARG version_uuv_simulator=0.6.13
 ARG version_uuv_manipulators=0.6.1
-ARG version_uw_sensors_gazebo=2fc1111d326ccd8d89ad3c5c8e765f9187fe1fc0
+ARG version_uw_sensors_gazebo=b9083c054c48fc9553eddeadae5dec447058607a
 ARG acoustic_msgs_commit=e4c93b6f8169d45b103289faf4500c40ab705ac9
 ARG ds_sim_commit=36835b85c30f5e063384378992f6ea60b5410d66
 ARG ds_msgs_commit=4a7d4a5002d17631e67eb01c45b974293fe5f3c2
-ARG frl_msgs_commit=f2eb86e1a82445006c9c49b3508bd44a0de54fd7
+ARG frl_msgs_commit=a41b1d3d192dacd32a7e0a717a9c1b0a30eaca64
 #------------------------------------------------------------------#
 
 #------------------------------------------------------------------#

glider_deadreckoning/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(glider_deadreckoning)
+
+find_package(catkin REQUIRED)
+catkin_package(
+)
+include_directories(
+)
+
+install(DIRECTORY launch
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+        PATTERN "*~" EXCLUDE)
+
+catkin_install_python(PROGRAMS nodes/deadreckoning_estimator.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

glider_deadreckoning/launch/deadreckon.launch

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<launch>
+  <node pkg="glider_deadreckoning"
+	type="deadreckoning_estimator.py"
+	name="deadreckoning_estimator"
+	output="screen">
+    <param name="init_lat" value="41.5"/>
+    <param name="init_lon" value="-70.7"/>
+    <param name="angleofattack" value="-3.0"/>
+    <param name="update_rate" value="5.0"/>
+    <!-- Remap the inputs to application specific topics -->
+    <remap from="imu" to="/glider_hybrid_whoi/hector_imu" />
+    <remap from="fix" to="/glider_hybrid_whoi/hector_gps" />
+    <!-- TODO - Is there a FluidPressure message being published? -->
+    <remap from="pressure" to="/glider_hybrid_whoi/pressure" />
+
+  </node>
+</launch>

glider_deadreckoning/nodes/deadreckoning_estimator.py

@@ -0,0 +1,200 @@
+#!/usr/bin/env python
+'''
+Node to implement dead reckoning estimation.
+
+'''
+
+# System imports
+from math import *
+
+# ROS/Gazebo imports
+import rospy
+import tf
+from sensor_msgs.msg import Imu
+from sensor_msgs.msg import NavSatFix
+from sensor_msgs.msg import FluidPressure
+
+def decibars2depth_salt(decibars,latitude):
+    '''
+    From Seabird application note 69 (AN69).
+    Return depth in meters
+    Latitude is given in units of decimal degrees
+    '''
+    p = decibars
+    x = ( sin(latitude/57.29578) )**2
+    g = 9.780318*(1.0+( 5.2788e-3 + 2.36e-5 * x) * x ) + 1.092e-6 * p
+    d = ((((-1.82e-15 * p+2.279e-10)*p-2.2512e-5)*p+9.72659)*p) / g
+    return d
+
+def pascals2depth(pascals):
+    ''' 
+    Wrapper function with latitude hardcoded for now
+    '''
+    decibars = pascals/1.0e5
+    return decibars2depth_salt(decibars, 41.0)
+
+def  mdeglat(lat):
+    '''
+    Provides meters-per-degree latitude at a given latitude
+    From AlvinXY 
+
+    Args:
+      lat (float): latitude
+    Returns:
+      float: meters-per-degree value
+    '''
+    latrad = lat*2.0*pi/360.0 ;
+
+    dy = 111132.09 - 566.05 * cos(2.0*latrad) \
+         + 1.20 * cos(4.0*latrad) \
+         - 0.002 * cos(6.0*latrad)
+    return dy
+
+def mdeglon(lat):
+    '''
+    Provides meters-per-degree longitude at a given latitude
+    From AlvinXY
+
+    Args:
+      lat (float): latitude in decimal degrees
+    Returns:
+      float: meters per degree longitude
+    '''
+    latrad = lat*2.0*pi/360.0 
+    dx = 111415.13 * cos(latrad) \
+         - 94.55 * cos(3.0*latrad) \
+	+ 0.12 * cos(5.0*latrad)
+    return dx
+
+class Node():
+    def __init__(self, init_lat, init_lon, angleofattack_deg):
+
+        # Current estimate
+        self.dr_msg = NavSatFix()
+        self.dr_msg.latitude = init_lat
+        self.dr_msg.longitude = init_lon
+
+        # Fixec AOA
+        self.angleofattack_rad = angleofattack_deg * pi/180.0
+
+        # Keep track of time and depth
+        self.t0 = rospy.get_time()
+        while self.t0 < 0.01:
+            rospy.logwarn("Waiting for ROS tme to be received")
+            rospy.sleep(0.5)
+            self.t0 = rospy.get_time()
+        self.d0 = None
+        # Flag for when we receive GPS updates
+        self.new_gps = False
+
+        # Init messages
+        self.imu_msg = Imu()
+        self.pressure_msg = FluidPressure()
+        self.gps_msg = NavSatFix()
+
+        # Pubs and subs
+        self.sub_imu = rospy.Subscriber("imu", Imu, self.callback_imu)
+        self.sub_pressure = rospy.Subscriber("pressure",
+                                             FluidPressure,
+                                             self.callback_pressure)
+        self.sub_gps = rospy.Subscriber("fix", NavSatFix, self.callback_gps)
+        self.pub_fix = rospy.Publisher("deadreckon", NavSatFix, queue_size=1)
+
+
+    def callback_imu(self, data):
+        self.imu_msg = data
+
+    def callback_pressure(self, data):
+        self.pressure_msg = data
+
+    def callback_gps(self, data):
+        self.gps_msg = data
+        self.new_gps = True
+
+    def update(self):
+        '''
+        Estimate
+        '''
+        # Timing
+        now = rospy.get_time()
+        dt = now - self.t0
+        if dt < 1.0e-3:
+            rospy.logwarn("Timestep is too small (%f) - skipping this update"
+                          %dt)
+            return
+        self.t0 = now
+
+        # Convert fluid pressure to depth
+        depth = pascals2depth(self.pressure_msg.fluid_pressure)
+
+        # Need to intialize depth on first pass
+        if self.d0 is None:
+            self.d0 = depth
+            return
+
+        # Convert IMU attitude to Euler
+        q = (self.imu_msg.orientation.x,
+             self.imu_msg.orientation.y,
+             self.imu_msg.orientation.z,
+             self.imu_msg.orientation.w)
+        rpy = tf.transformations.euler_from_quaternion(q)
+
+        # If we have GPS - just use that
+        if self.new_gps:
+            self.dr_msg.latitude = self.gps_msg.latitude
+            self.dr_msg.longitude = self.gps_msg.longitude
+            self.dr_msg.header.stamp = rospy.Time.now()
+            self.pub_fix.publish(self.dr_msg)
+            return
+
+        # Algo. from:
+        # https://github.com/Field-Robotics-Lab/glider_hybrid_whoi/issues/3
+        dz = depth - self.d0
+        self.d0 = depth
+        # Assume constant angle of attack
+        # Note that coordinates are in ENU
+        aoa = 4.0 * pi/180.0
+        glide_angle = rpy[1] + aoa
+        if abs(glide_angle) < pi/100.0:
+            v_x = 0.0
+            v_y = 0.0
+        else: 
+            v_x = dz/(dt*tan(glide_angle))*sin(rpy[2])
+            v_y = dz/(dt*tan(glide_angle))*cos(rpy[2])
+        dx = v_x * dt
+        dy = v_y * dt
+
+        # Increment lat/lon 
+        self.dr_msg.latitude += dy/mdeglat(self.dr_msg.latitude)
+        self.dr_msg.longitude += dx/mdeglat(self.dr_msg.latitude)
+
+        # Publish
+        self.dr_msg.header.stamp = rospy.Time.now()
+        self.pub_fix.publish(self.dr_msg)
+        return
+
+if __name__ == '__main__':
+
+    # Start node
+    rospy.init_node('deadreckoning_estimator')
+
+    # Initial lat/lon.  Default is in Buzzard's Bay
+    init_lat = rospy.get_param('~init_lat', 41.5501)
+    init_lon = rospy.get_param('~init_lon', -70.71428)
+    # Specified in degrees, positive is bow down
+    angleofattack_deg = rospy.get_param('~angleofattack',  -4.0 )
+
+    # Update rate
+    update_rate = rospy.get_param('~update_rate', 5.0)
+
+    # Initiate node object
+    node=Node(init_lat, init_lon, angleofattack_deg)
+
+    # Spin
+    r = rospy.Rate(update_rate)
+    try:
+        while not rospy.is_shutdown():
+            node.update()
+            r.sleep()
+    except rospy.ROSInterruptException:
+        pass

glider_deadreckoning/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>glider_deadreckoning</name>
+  <version>0.0.0</version>
+  <description>The glider_deadreckoning package</description>
+  <maintainer email="briansbingham@gmail.com">bsb</maintainer>
+  <license>TODO</license>
+
+  <depend>rospy</depend>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+  </export>
+</package>