Made nav_tube_driver an executable, fixed typing and package dependen…

…cy issues, and updated the package.xml with formatted information

src/subjugator/drivers/nav_tube_driver/CMakeLists.txt

@@ -10,17 +10,32 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(mil_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+
+add_executable(nav_tube_driver src/nav_tube_driver.cpp)
+target_include_directories(nav_tube_driver PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+target_compile_features(nav_tube_driver PUBLIC c_std_99 cxx_std_17)
+ament_target_dependencies(
+  nav_tube_driver
+  "rclcpp"
+  "std_msgs"
+  "nav_msgs"
+  "mil_msgs"
+)
+
+install(TARGETS nav_tube_driver
+  DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
   # comment the line when a copyright and license is added to all source files
   set(ament_cmake_copyright_FOUND TRUE)
   # the following line skips cpplint (only works in a git repo)
   # comment the line when this package is in a git repo and when
   # a copyright and license is added to all source files
   set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
 endif()
 
 ament_package()

src/subjugator/drivers/nav_tube_driver/package.xml

@@ -3,20 +3,20 @@
 <package format="3">
   <name>nav_tube_driver</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="daniel27parra@gmail.com">daniel</maintainer>
+  <description>Reads depth sensor data and combines it with odometry to compensate for motion induced pressure.</description>
+  <author>Andrew Knee</author>
+  <author>Cameron Brown</author>
+  <maintainer email="daniel27parra@gmail.com">Daniel Parra</maintainer>
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
 
   <exec_depend>mil_msgs</exec_depend>
 
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/subjugator/drivers/nav_tube_driver/src/nav_tube_driver.cpp

@@ -62,7 +62,7 @@ class NavTubeDriver : public rclcpp::Node
   void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
 };
 
-NavTubeDriver::NavTubeDriver()
+NavTubeDriver::NavTubeDriver():rclcpp::Node("nav_tube_driver")
 {
   pub_ = this->create_publisher<mil_msgs::msg::DepthStamped>("depth", 10);
   odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
@@ -139,32 +139,31 @@ void NavTubeDriver::read_messages(boost::shared_ptr<tcp::socket> socket)
 {
   mil_msgs::msg::DepthStamped msg;
   msg.header.frame_id = frame_id_;
-  msg.header.seq = 0;
+  msg.header.stamp = rclcpp::Clock().now();
 
   uint8_t backing[10];
 
   auto buffer = boost::asio::buffer(backing, sizeof(backing));
 
   while (rclcpp::ok()) {
-    if (rclcpp::Clock().now().nanoseconds() - prev.nanoseconds() > acceptable_frequency) {
+    if (rclcpp::Clock().now().nanoseconds() - prev.nanoseconds() > static_cast<long>(acceptable_frequency)) {
       RCLCPP_WARN(this->get_logger(), "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++) {
+        for (int i = 0; i < int(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 = rclcpp::Clock().now();
 
         uint64_t bits = be64toh(*reinterpret_cast<uint64_t *>(&backing[2]));
         double pressure = *reinterpret_cast<double *>(&bits);
-        if (recent_odom_msg_.header.seq) {
+        if (recent_odom_msg_.header.stamp.sec > msg.header.stamp.sec) {
           // Accounts for the dynamic pressure applied to the pressure sensor
           // when the sub is moving forwards or backwards
           double velocity = recent_odom_msg_.twist.twist.linear.x;
@@ -202,7 +201,7 @@ void NavTubeDriver::run()
     } catch (boost::system::system_error const & e) {
       std::chrono::seconds wait_time(5);
       RCLCPP_WARN(this->get_logger(),
-        "Error with NavTube Depth driver TCP socket %s. Trying again in %f seconds",
+        "Error with NavTube Depth driver TCP socket %s. Trying again in %ld seconds",
         e.what(), wait_time.count());
 
       if (initialized) {