Merge branch 'nav_tube_driver' of https://github.com/uf-mil/mil2 into…

… nav_tube_driver

.github/workflows/autopush.yaml

@@ -23,11 +23,32 @@ env:
   PRE_COMMIT_CACHE: ~/.cache/pre-commit
 
 jobs:
+  ensure_autopusher:
+    name: Ensure user has autopush access
+    runs-on: ubuntu-latest
+    steps:
+      - name: Check autopushers team membership
+        uses: actions/github-script@v7
+        with:
+          github-token: ${{ secrets.INVESTIGATOR_BOT_TOKEN }}
+          script: |
+            const team = 'autopushers';
+            const teamResponse = await github.rest.teams.listMembersInOrg({
+              org: context.repo.owner,
+              team_slug: team,
+            });
+            const members = teamResponse.data.map((member) => member.login);
+            // Check if the user is a member of the team
+            if (!members.includes(context.actor)) {
+              core.setFailed(`User ${context.actor} is not a member of the ` +
+              `${team} team. Found: ${members.join(', ')}`);
+            }
+
   pre-commit:
     name: Run pre-commit
     runs-on:
       group: mala-lab-pre-commit
-    if: github.event.sender.login == 'cbrxyz'
+    needs: [ensure_autopusher]
     steps:
       - name: Check out code from GitHub
         uses: actions/checkout@v3.0.2

scripts/setup.bash

@@ -36,7 +36,7 @@ alias fd="fdfind"
 
 # potentially borrowed from forrest
 autopush() {
-	git push origin +"${1:-HEAD}":refs/heads/autopush-cameron-"$(uuidgen --random | cut -c1-8)"-citmp
+	git push origin +"${1:-HEAD}":refs/heads/autopush-"$USER"-"$(uuidgen --random | cut -c1-8)"-citmp
 }
 
 # ssd utils

src/mil_common/mil_passive_sonar/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+project(mil_passive_sonar)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(service_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/ProcessedPing.msg"
+  DEPENDENCIES std_msgs geometry_msgs service_msgs
+  ADD_LINTER_TESTS
+)
+
+ament_export_dependencies(
+	rosidl_default_runtime
+	std_msgs
+	geometry_msgs
+	service_msgs
+	rclcpp
+)
+
+ament_package()
+
+install(PROGRAMS scripts/ping_publisher.py scripts/fakeping_pipeline.sh scripts/pipeline.sh DESTINATION lib/${PROJECT_NAME})

src/mil_common/mil_passive_sonar/README.md

@@ -0,0 +1,9 @@
+# mil_passive_sonar
+This package includes the main scripts for receiving the location of a pinger. Location is published to a ros2 topic /hydrophones/solved.
+
+# Running:
+Run 'ros2 run mil_passive_sonar pipeline.sh' or 'ros2 run mil_passive_sonar fakeping_pipeline.sh' for testing with a simulated pinger.
+
+Then run 'ros2 run mil_passive_sonar ping_publisher.py'
+
+Now, you can listen to published messages via 'ros2 topic echo /hydrophones/solved'

src/mil_common/mil_passive_sonar/msg/ProcessedPing.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+geometry_msgs/Point position
+float64 freq
+float64 amplitude
+bool valid

src/mil_common/mil_passive_sonar/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>mil_passive_sonar</name>
+  <version>2.0.0</version>
+  <description>MIL Passive Sonar</description>
+  <maintainer email="alexjohnson@ufl.edu">Alex Johnson</maintainer>
+  <license>BSD</license>
+  <author>Alex Johnson</author>
+  <!-- Dependencies which this package needs to build itself. -->
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <!-- Dependencies needed to compile this package. -->
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>message_runtime</build_depend>
+  <build_depend>rclpy</build_depend>
+  <build_depend>service_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <!-- Dependencies needed after this package is compiled. -->
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>message_generation</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>service_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/mil_common/mil_passive_sonar/scripts/fakeping_pipeline.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+sdgps generate-fake-sonar-solution --rate 1 --position '[-30, 10, -2]' --circle '{"radius":3,"axis":[0,0,1],"period":30}' ! sonar-simulate --role rover --transmit-pattern robosub-2019 --base-hydrophone-arrangement robosub-2019 --rover-hydrophone-arrangement uf-mil-sub7 --cn0 80 --multipath extreme ! filter-sonar-raw-samples --highpass 15e3 --lowpass 45e3 --notch 40e3 ! extract-robosub-pings ! robosub-ping-solver ! listen-robosub-ping-solution-tcp 2007

src/mil_common/mil_passive_sonar/scripts/ping_publisher.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+
+import json
+import socket
+
+import rclpy
+from geometry_msgs.msg import Point
+from mil_passive_sonar.msg import ProcessedPing
+from std_msgs.msg import Header
+
+
+def main():
+    # Define the server address and port
+    HOST = "127.0.0.1"
+    PORT = 2007
+
+    # Create a socket
+    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
+        pub = node.create_publisher(ProcessedPing, "hydrophones/solved", 10)
+        s.connect((HOST, PORT))
+        node.get_logger().info(
+            f"\nping_publisher connected to {HOST}:{PORT}, forwarding TCP messages to {pub.topic_name}...",
+        )
+
+        # Need to ignore the first 2 JSON Parse Errors (nothing wrong)
+        parse_error_count = 0
+
+        while rclpy.ok():
+            # Receive data
+            data = s.recv(1024)
+            if not data:
+                break
+
+            # Parse the JSON data
+            try:
+                json_data = json.loads(data.decode("utf-8"))
+                ping_msg = ProcessedPing()
+
+                # Populate the header
+                ping_msg.header = Header()
+                ping_msg.header.stamp = node.get_clock().now().to_msg()
+                ping_msg.header.frame_id = "hydrophones"
+
+                # Populate the position
+                ping_msg.position = Point()
+                ping_msg.position.x = json_data["origin_direction_body"][0]
+                ping_msg.position.y = json_data["origin_direction_body"][1]
+                ping_msg.position.z = json_data["origin_direction_body"][2]
+
+                # Populate the frequency and amplitude
+                ping_msg.freq = float(json_data["frequency_Hz"])
+                ping_msg.amplitude = float(json_data["origin_distance_m"])
+                ping_msg.valid = True
+
+                # Publish the message
+                pub.publish(ping_msg)
+            except json.JSONDecodeError as e:
+                parse_error_count += 1
+                # ignore first two (normal behavior)
+                if parse_error_count > 2:
+                    node.get_logger().error(f"JSONDecodeError: {e}")
+            except KeyError as e:
+                node.get_logger().error(f"Key Error: {e}")
+
+
+if __name__ == "__main__":
+    # initialize rclpy
+    rclpy.init()
+
+    # create publisher node
+    node = rclpy.create_node("pingpublisher")
+    rclpy.get_global_executor().add_node(node)
+
+    main()

src/mil_common/mil_passive_sonar/scripts/pipeline.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+sdgps connect-sonar-raw-tcp --host 192.168.37.61 --port 2006 ! filter-sonar-raw-samples --highpass 15e3 --lowpass 45e3 --notch 40e3 ! extract-robosub-pings ! robosub-ping-solver ! listen-robosub-ping-solution-tcp 2007

src/mil_common/mil_tools/test/os.cpp

@@ -9,12 +9,12 @@
 #include "mil_tools/fs/path.hpp"
 #include "mil_tools/os/TemporaryFile.hpp"
 
-// TEST(mil_tools_os, open)
-// {
-//   EXPECT_FALSE(fd_null.valid());
-//   auto fd_readme = mil_tools::os::open(mil_tools::fs::path::expanduser("~/mil2/README.md"), O_RDONLY);
-//   EXPECT_EQ(fd_readme.read_as_string(5), "![Col");
-// }
+TEST(mil_tools_os, open)
+{
+  // TODO (cameron): fix this in CI :(
+  // auto fd_readme = mil_tools::os::open(mil_tools::fs::path::expanduser("~/mil2/README.md"), O_RDONLY);
+  // EXPECT_EQ(fd_readme.read_as_string(5), "![Col");
+}
 
 TEST(mil_tools_os, write)
 {