Nav tube update

SubjuGator/command/subjugator_launch/launch/subsystems/nav_box.launch

@@ -17,10 +17,11 @@
 
   <!-- Depth Sensor -->
   <group if="$(eval depth and environment == 'real')">
-    <node pkg="subjugator_launch" type="depth_conn" name="depth_conn" respawn="true" respawn_delay = "10"/>
-    <node pkg="nodelet" type="nodelet" name="depth_driver" args="standalone depth_driver/nodelet">
-      <param name="port" type="string" value="/tmp/depth"/>
-      <param name="frame_id" type="string" value="/depth"/>
+    <node pkg="nav_tube_driver" type="nav_tube_driver_node" name="nav_tube_driver_node" respawn="true">
+      <param name="ip" type="string" value="192.168.37.61" />
+      <param name="port" type="int" value="33056" />
+      <param name="frame_id" type="string" value="/depth" />
+      <param name="hz" type="int" value="20" />
     </node>
   </group>
 

SubjuGator/command/subjugator_launch/package.xml

@@ -31,6 +31,7 @@
   <run_depend>mil_passive_sonar</run_depend>
   <run_depend>mil_pneumatic_actuator</run_depend>
   <run_depend>mil_blueview_driver</run_depend>
+  <run_depend>nav_tube_driver</run_depend>
   <run_depend>subjugator_perception</run_depend>
   <run_depend>yolov7_ros</run_depend>
   <buildtool_depend>catkin</buildtool_depend>

SubjuGator/drivers/nav_tube_driver/CMakeLists.txt

@@ -0,0 +1,47 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(nav_tube_driver)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  mil_msgs
+  nodelet
+  roscpp
+)
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES nav_tube_driver
+#  CATKIN_DEPENDS mil_msgs nodelet roscpp
+#  DEPENDS system_lib
+)
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node src/nav_tube_driver.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_node
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+install(TARGETS ${PROJECT_NAME}_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

SubjuGator/drivers/nav_tube_driver/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>nav_tube_driver</name>
+  <version>0.5.0</version>
+  <description>nav_tube_driver</description>
+  <maintainer email="[email protected]">andrew</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>mil_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_export_depend>mil_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <exec_depend>mil_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+</package>

SubjuGator/drivers/nav_tube_driver/src/nav_tube_driver.cpp

@@ -0,0 +1,220 @@
+#include <bits/stdint-uintn.h>
+#include <endian.h>
+#include <mil_msgs/DepthStamped.h>
+#include <ros/ros.h>
+
+#include <boost/asio.hpp>
+#include <boost/smart_ptr/make_shared_array.hpp>
+#include <boost/smart_ptr/shared_ptr.hpp>
+#include <chrono>
+#include <limits>
+#include <mutex>
+#include <thread>
+
+// modeled after mil_passive_sonar/sylphase_ros_bridge
+
+using tcp = boost::asio::ip::tcp;
+
+class NavTubeDriver
+{
+private:
+  ros::NodeHandle nh_;
+  ros::NodeHandle private_nh_;
+
+  ros::Publisher pub_;
+
+  std::string ip_;
+  int port_;
+  std::string frame_id_;
+  uint16_t hz_;
+
+  uint64_t acceptable_frequency;
+
+  ros::Time prev;
+
+  std::thread timer_thread;
+
+  std::mutex m;
+
+  bool running = true;
+
+  bool initialized = false;
+
+  static const uint8_t sync1 = 0x37;
+  static const uint8_t sync2 = 0x01;
+
+  uint8_t heartbeat_packet[2 + sizeof(hz_)];
+
+  boost::shared_ptr<tcp::socket> connect();
+
+  void send_heartbeat(boost::shared_ptr<tcp::socket> socket);
+
+  void read_messages(boost::shared_ptr<tcp::socket> socket);
+
+  double calculate_pressure(uint16_t analog_input);
+
+public:
+  NavTubeDriver(ros::NodeHandle nh, ros::NodeHandle private_nh);
+
+  ~NavTubeDriver();
+
+  void run();
+};
+
+NavTubeDriver::NavTubeDriver(ros::NodeHandle nh, ros::NodeHandle private_nh) : nh_(nh), private_nh_(private_nh)
+{
+  pub_ = nh.advertise<mil_msgs::DepthStamped>("depth", 10);
+  ip_ = private_nh.param<std::string>("ip", std::string("192.168.37.61"));
+  port_ = private_nh.param<int>("port", 33056);
+  frame_id_ = private_nh.param<std::string>("frame_id", "/depth");
+
+  int hz__ = private_nh.param<int>("hz", 20);
+
+  if (hz__ > std::numeric_limits<uint16_t>::max())
+  {
+    ROS_WARN_STREAM("Depth polling frequency is greater than 16 bits!");
+  }
+
+  hz_ = hz__;
+
+  acceptable_frequency = (1'000'000'000 * 1.25) / (uint64_t)hz_;
+
+  heartbeat_packet[0] = sync1;
+  heartbeat_packet[1] = sync2;
+  uint16_t nw_ordering = htons(hz_);
+  reinterpret_cast<uint16_t*>(&heartbeat_packet[2])[0] = nw_ordering;
+}
+
+NavTubeDriver::~NavTubeDriver()
+{
+  {
+    std::lock_guard<std::mutex> lock(m);
+    running = false;
+  }
+  timer_thread.join();
+}
+
+boost::shared_ptr<tcp::socket> NavTubeDriver::connect()
+{
+  using ip_address = boost::asio::ip::address;
+  tcp::endpoint endpoint(ip_address::from_string(ip_), port_);
+
+  ROS_INFO_STREAM("Connecting to Depth Server");
+  boost::asio::io_service io_service;
+  boost::shared_ptr<tcp::socket> socket = boost::make_shared<tcp::socket>(io_service);
+  socket->connect(endpoint);
+  ROS_INFO_STREAM("Connection to Depth Server established");
+
+  return socket;
+}
+
+void NavTubeDriver::run()
+{
+  while (ros::ok())
+  {
+    try
+    {
+      prev = ros::Time::now();
+      boost::shared_ptr<tcp::socket> socket;
+
+      socket = connect();
+      timer_thread = std::thread(&NavTubeDriver::send_heartbeat, this, socket);
+      initialized = true;
+      read_messages(socket);
+    }
+    catch (boost::system::system_error const& e)
+    {
+      ros::Duration wait_time(5);
+      ROS_WARN_STREAM("Error with NavTube Depth driver TCP socket " << e.what() << ". Trying again in "
+                                                                    << wait_time.toSec() << " seconds");
+
+      if (initialized)
+        timer_thread.join();
+      initialized = false;
+      wait_time.sleep();
+    }
+  }
+}
+
+void NavTubeDriver::send_heartbeat(boost::shared_ptr<tcp::socket> socket)
+{
+  try
+  {
+    while (true)
+    {
+      boost::asio::write(*socket, boost::asio::buffer(heartbeat_packet));
+
+      {
+        std::lock_guard<std::mutex> lock(m);
+        if (!running)
+          return;
+      }
+
+      std::this_thread::sleep_for(std::chrono::milliseconds(250));
+    }
+  }
+  catch (boost::system::system_error const& e)
+  {
+  }
+}
+
+void NavTubeDriver::read_messages(boost::shared_ptr<tcp::socket> socket)
+{
+  mil_msgs::DepthStamped msg;
+  msg.header.frame_id = frame_id_;
+  msg.header.seq = 0;
+
+  uint8_t backing[10];
+
+  auto buffer = boost::asio::buffer(backing, sizeof(backing));
+
+  while (ros::ok())
+  {
+    if (ros::Time::now().toNSec() - prev.toNSec() > acceptable_frequency)
+    {
+      ROS_WARN_STREAM("Depth sampling rate is falling behind.");
+    }
+
+    if (!boost::asio::buffer_size(buffer))
+    {
+      // Bytes are out of sync so try and resync
+      if (backing[0] != sync1 || backing[1] != sync2)
+      {
+        for (int i = 0; i < (sizeof(backing) / sizeof(backing[0])) - 1; i++)
+        {
+          backing[i] = backing[i + 1];
+        }
+        buffer = boost::asio::buffer(backing + (sizeof(backing) / sizeof(backing[0])) - sizeof(backing[0]),
+                                     sizeof(backing[0]));
+      }
+      else
+      {
+        ++msg.header.seq;
+        msg.header.stamp = ros::Time::now();
+
+        uint64_t bits = be64toh(*reinterpret_cast<uint64_t*>(&backing[2]));
+        double pressure = *reinterpret_cast<double*>(&bits);
+        msg.depth = pressure;
+
+        pub_.publish(msg);
+        buffer = boost::asio::buffer(backing, sizeof(backing));
+      }
+    }
+
+    size_t bytes_read = socket->read_some(buffer);
+
+    buffer = boost::asio::buffer(buffer + bytes_read);
+    prev = ros::Time::now();
+  }
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "nav_tube_driver");
+
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh("~");
+
+  NavTubeDriver node(nh, private_nh);
+  node.run();
+}