Thrusters, Logging, Cleanup, Tests

command/sub8_launch/scripts/tf_republisher.py

@@ -9,7 +9,7 @@ def got_range(msg):
     '''TODO:
         - Make parallel to surface
     '''
-    translation = (0.0, 0.0, -msg.data)
+    translation = (0.0, 0.0, -msg.range)
     if rospy.Time.now() < rospy.Time(0.5):
         listener.clear()
     t = rospy.Time(0)

drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/src/nodelet.cpp

@@ -7,7 +7,7 @@
 #include <sensor_msgs/MagneticField.h>
 #include <eigen_conversions/eigen_msg.h>
 
-#include <mil_tools/param_helpers.h>
+#include <mil_tools/param_helpers.hpp>
 
 namespace magnetic_hardsoft_compensation {
 class Nodelet : public nodelet::Nodelet {

drivers/sub8_adis16400_imu/src/nodelet.cpp

@@ -5,7 +5,7 @@
 #include <pluginlib/class_list_macros.h>
 #include <ros/ros.h>
 
-#include <mil_tools/param_helpers.h>
+#include <mil_tools/param_helpers.hpp>
 
 #include "adis16400_imu/driver.h"
 

drivers/sub8_depth_driver/src/nodelet.cpp

@@ -5,7 +5,7 @@
 #include <pluginlib/class_list_macros.h>
 #include <ros/ros.h>
 
-#include <mil_tools/param_helpers.h>
+#include <mil_tools/param_helpers.hpp>
 #include <mil_msgs/DepthStamped.h>
 
 #include <depth_driver/driver.h>

drivers/sub8_rdi_dvl/include/rdi_explorer_dvl/driver.h → drivers/sub8_rdi_dvl/include/rdi_explorer_dvl/driver.hpp

@@ -1,5 +1,4 @@
-#ifndef DRIVER_H
-#define DRIVER_H
+#pragma once
 
 #include <cmath>
 #include <fstream>
@@ -9,113 +8,168 @@
 #include <boost/none.hpp>
 #include <boost/math/constants/constants.hpp>
 
-#include <mil_tools/msg_helpers.h>
+#include <mil_tools/msg_helpers.hpp>
 
 #include <mil_msgs/VelocityMeasurements.h>
 #include <mil_msgs/RangeStamped.h>
 
-namespace rdi_explorer_dvl {
-static uint16_t getu16le(uint8_t *i) { return *i | (*(i + 1) << 8); }
-static int32_t gets32le(uint8_t *i) {
+namespace rdi_explorer_dvl
+{
+
+static uint16_t getu16le(uint8_t *i)
+{
+  return *i | (*(i + 1) << 8);
+}
+
+static int32_t gets32le(uint8_t *i)
+{
   return *i | (*(i + 1) << 8) | (*(i + 2) << 16) | (*(i + 3) << 24);
 }
 
-class Device {
- private:
+class Device
+{
+
+private:
   typedef std::vector<boost::uint8_t> ByteVec;
 
   const std::string port;
   const int baudrate;
   boost::asio::io_service io;
   boost::asio::serial_port p;
 
-  bool read_byte(uint8_t &res) {
-    while (true) {
-      try {
+  /*
+    Reads byte into a uint8_t
+    res - uint8_t reference to write byte into
+  */
+  bool read_byte(uint8_t &res)
+  {
+    while(true)
+    {
+      try
+      {
         p.read_some(boost::asio::buffer(&res, sizeof(res)));
         return true;
-      } catch (const std::exception &exc) {
-        ROS_ERROR("error on read: %s; reopening", exc.what());
+      }
+      catch (const std::exception &exc)
+      {
+        ROS_ERROR_THROTTLE(0.1, "DVL: error on read: %s; reopening serial port", exc.what());
         open();
         return false;
       }
     }
   }
+
+  /*
+    Reads 2 bytes into a uint16_t
+    res - uint16_t reference to write byte into
+  */
   bool read_short(uint16_t &x) {
     uint8_t low;
-    if (!read_byte(low)) return false;
+    if(!read_byte(low)) return false;
     uint8_t high;
-    if (!read_byte(high)) return false;
+    if(!read_byte(high)) return false;
     x = 256 * high + low;
     return true;
   }
 
+  /*
+    Attempts to close and reopen the serial port
+    Sleeps for one second before returning if an exception is caught
+  */
   void open() {
-    try {
+    try
+    {
       p.close();
       p.open(port);
       p.set_option(boost::asio::serial_port::baud_rate(baudrate));
       return;
-    } catch (const std::exception &exc) {
-      ROS_ERROR("error on open(%s): %s; reopening after delay", port.c_str(), exc.what());
+    }
+    catch (const std::exception &exc)
+    {
+      ROS_ERROR("DVL: error on open(port=%s): %s; reopening after delay", port.c_str(), exc.what());
       boost::this_thread::sleep(boost::posix_time::seconds(1));
     }
   }
 
- public:
-  Device(const std::string port, int baudrate) : port(port), baudrate(baudrate), p(io) {
+public:
+  Device(const std::string port, int baudrate)
+  : port(port), baudrate(baudrate), p(io)
+  {
     // open is called on first read() in the _polling_ thread
   }
 
   void read(boost::optional<mil_msgs::VelocityMeasurements> &res,
-            boost::optional<mil_msgs::RangeStamped> &height_res) {
+            boost::optional<mil_msgs::RangeStamped> &height_res)
+  {
     res = boost::none;
     height_res = boost::none;
 
-    if (!p.is_open()) {
+    if(!p.is_open())  // Open serial port if closed
+    {
       open();
       return;
     }
 
     ByteVec ensemble;
     ensemble.resize(4);
 
-    if (!read_byte(ensemble[0])) return;  // Header ID
-    if (ensemble[0] != 0x7F) return;
+    // Header ID
+    if(!read_byte(ensemble[0]))
+      return;
+    if(ensemble[0] != 0x7F)
+      return;
 
     ros::Time stamp = ros::Time::now();
 
-    if (!read_byte(ensemble[1])) return;  // Data Source ID
-    if (ensemble[1] != 0x7F) return;
+    // Data Source ID
+    if(!read_byte(ensemble[1]))
+      return;
+    if(ensemble[1] != 0x7F)
+      return;
+
+    // Size low
+    if(!read_byte(ensemble[2]))
+      return;
+    // Size high
+    if(!read_byte(ensemble[3]))
+      return;
 
-    if (!read_byte(ensemble[2])) return;  // Size low
-    if (!read_byte(ensemble[3])) return;  // Size high
     uint16_t ensemble_size = getu16le(ensemble.data() + 2);
     ensemble.resize(ensemble_size);
-    for (int i = 4; i < ensemble_size; i++) {
-      if (!read_byte(ensemble[i])) return;
+    for(int i = 4; i < ensemble_size; i++)
+    {
+      if(!read_byte(ensemble[i])) return;
     }
 
     uint16_t checksum = 0;
-    BOOST_FOREACH (uint16_t b, ensemble)
+    BOOST_FOREACH(uint16_t b, ensemble)
       checksum += b;
     uint16_t received_checksum;
-    if (!read_short(received_checksum)) return;
-    if (received_checksum != checksum) {
-      ROS_ERROR("Invalid DVL ensemble checksum. received: %i calculated: %i size: %i",
+    if(!read_short(received_checksum))
+      return;
+    if(received_checksum != checksum)
+    {
+      ROS_ERROR("DVL: invalid ensemble checksum. received: %i calculated: %i size: %i",
                 received_checksum, checksum, ensemble_size);
       return;
     }
 
-    if (ensemble.size() < 6) return;
-    for (int dt = 0; dt < ensemble[5]; dt++) {
+    if(ensemble.size() < 6)
+      return;
+    for(int dt = 0; dt < ensemble[5]; dt++)
+    {
       int offset = getu16le(ensemble.data() + 6 + 2 * dt);
-      if (ensemble.size() - offset < 2) continue;
+      if(ensemble.size() - offset < 2)
+        continue;
+      // Three modes, encoded by the section_id: Bottom Track High Resolution Velocity
+      // Bottom Track, Bottom Track Range
       uint16_t section_id = getu16le(ensemble.data() + offset);
 
       std::vector<double> correlations(4, nan(""));
-      if (section_id == 0x5803) {  // Bottom Track High Resolution Velocity
-        if (ensemble.size() - offset < 2 + 4 * 4) continue;
+      if(section_id == 0x5803)  // Bottom Track High Resolution Velocity
+      {
+        if(ensemble.size() - offset < 2 + 4 * 4)
+          continue;
         res = boost::make_optional(mil_msgs::VelocityMeasurements());
         res->header.stamp = stamp;
 
@@ -129,40 +183,62 @@ class Device {
           dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(0, +x, -z));
           dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(0, -x, -z));
         }
-        for (int i = 0; i < 4; i++) {
+
+        // Keep track of which beams didn't return for logging
+        std::vector<size_t> invalid_beams(4);
+        for(int i = 0; i < 4; i++)
+        {
           mil_msgs::VelocityMeasurement m;
           m.direction = dirs[i];
           int32_t vel = gets32le(ensemble.data() + offset + 2 + 4 * i);
           m.velocity = -vel * .01e-3;
-          if (vel == -3276801) {  // -3276801 indicates no data
-            ROS_ERROR("DVL didn't return bottom velocity for beam %i", i + 1);
+          if(vel == -3276801)  // -3276801 indicates no data
+          {
+            invalid_beams.push_back(i + 1);
             m.velocity = nan("");
           }
           res->velocity_measurements.push_back(m);
         }
-      } else if (section_id == 0x0600) {  // Bottom Track
-        for (int i = 0; i < 4; i++) {
-          correlations[i] = *(ensemble.data() + offset + 32 + i);
+
+        // Report  a list of invalid beams
+        if(invalid_beams.size() > 0)
+        {
+          std::string to_log {"DVL: didn't return bottom velocity for beam(s): "};
+          for(auto beam : invalid_beams)
+            to_log += std::to_string(beam) + " ";
+          ROS_ERROR_THROTTLE(0.1, "%s", to_log.c_str());
         }
-      } else if (section_id == 0x5804) {  // Bottom Track Range
-        if (ensemble.size() - offset < 2 + 4 * 3) continue;
-        if (gets32le(ensemble.data() + offset + 10) <= 0) {
-          ROS_ERROR("DVL didn't return height over bottom");
+      }
+      else if(section_id == 0x0600)  // Bottom Track
+      {
+        for(int i = 0; i < 4; i++)
+          correlations[i] = *(ensemble.data() + offset + 32 + i);
+      }
+      else if(section_id == 0x5804)  // Bottom Track Range
+      {
+        if(ensemble.size() - offset < 2 + 4 * 3)
+          continue;
+        if(gets32le(ensemble.data() + offset + 10) <= 0)
+        {
+          ROS_ERROR_THROTTLE(0.1, "%s", "DVL: didn't return height over bottom");
           continue;
         }
         height_res = boost::make_optional(mil_msgs::RangeStamped());
         height_res->header.stamp = stamp;
         height_res->range = gets32le(ensemble.data() + offset + 10) * 0.1e-3;
       }
-      if (res) {
-        for (int i = 0; i < 4; i++) {
+      if(res)
+      {
+        for(int i = 0; i < 4; i++)
+        {
           res->velocity_measurements[i].correlation = correlations[i];
         }
       }
     }
   }
 
-  void send_heartbeat() {
+  void send_heartbeat()
+  {
     double maxdepth = 15;
 
     std::stringstream buf;
@@ -182,18 +258,25 @@ class Device {
 
     std::string str = buf.str();
 
-    try {
+    try  // Write heartbeat to serial port
+    {
       size_t written = 0;
-      while (written < str.size()) {
+      while (written < str.size())
+      {
         written += p.write_some(boost::asio::buffer(str.data() + written, str.size() - written));
       }
-    } catch (const std::exception &exc) {
-      ROS_ERROR("error on write: %s; dropping", exc.what());
+    }
+    catch (const std::exception &exc)
+    {
+      ROS_ERROR_THROTTLE(0.1, "DVL: error on write: %s; dropping heartbeat", exc.what());
     }
   }
 
-  void abort() { p.close(); }
+  void abort()
+  {
+    p.close();
+  }
 };
-}
 
-#endif
+} // namespace rdi_explorer_dvl
+

drivers/sub8_rdi_dvl/src/nodelet.cpp

@@ -6,9 +6,9 @@
 #include <pluginlib/class_list_macros.h>
 #include <ros/ros.h>
 
-#include <mil_tools/param_helpers.h>
+#include <mil_tools/param_helpers.hpp>
 
-#include "rdi_explorer_dvl/driver.h"
+#include "rdi_explorer_dvl/driver.hpp"
 
 namespace rdi_explorer_dvl {
 

gnc/sub8_thruster_mapper/launch/thruster_mapper.launch

@@ -1,7 +1,7 @@
 <launch>
   <rosparam>
     busses:
-      - port: /dev/serial/by-id/usb-MIL_Data_Merge_Board_Ports_1_to_4_DMBP14-if01-port0
+      - port: /dev/serial/by-id/usb-MIL_Data_Merge_Board_Ports_1_to_4_DMBP14-if00-port0
         thrusters:
           FLH: {
             node_id: 0,
@@ -15,7 +15,7 @@
             position: [0.1583, 0.169, 0.0142],
             direction: [0, 0, -1]
           }
-      - port: /dev/serial/by-id/usb-MIL_Data_Merge_Board_Ports_1_to_4_DMBP14-if00-port0
+      - port: /dev/serial/by-id/usb-MIL_Data_Merge_Board_Ports_1_to_4_DMBP14-if01-port0
         thrusters:
           FRH: {
             node_id: 2,

gnc/sub8_thruster_mapper/test/test_sub8_mapper.test

@@ -2,7 +2,7 @@
   <param name="simulate" value="true" />
 
   <!-- Start up alarm server -->
-  <node name="alarm_sever" pkg="ros_alarms" type="alarm_server.py" output="screen">
+  <node name="alarm_sever" pkg="ros_alarms" type="alarm_server.py">
       <!-- A folder that is in your system path containing all alarm handlers-->
       <param name="handler_module" type="string" value="alarm_handlers"/>
   </node>