From 0263711ff827907f48f86b33b49b14815294745f Mon Sep 17 00:00:00 2001
From: Alex Johnson <alexjohnson@ufl.edu>
Date: Sat, 26 Oct 2024 00:44:31 +0000
Subject: [PATCH] Fixed PR comments

---
 .../drivers/mil_passive_sonar/CMakeLists.txt  |  2 +-
 .../scripts/ping_publisher.py                 | 35 ++++++++++---------
 .../drivers/mil_passive_sonar/src/pipeline.sh |  2 +-
 3 files changed, 21 insertions(+), 18 deletions(-)

diff --git a/mil_common/drivers/mil_passive_sonar/CMakeLists.txt b/mil_common/drivers/mil_passive_sonar/CMakeLists.txt
index 6c73b3328..eccce0884 100644
--- a/mil_common/drivers/mil_passive_sonar/CMakeLists.txt
+++ b/mil_common/drivers/mil_passive_sonar/CMakeLists.txt
@@ -37,4 +37,4 @@ add_executable(sylphase_sonar_ros_bridge src/sylphase_ros_bridge.cpp)
 target_link_libraries(sylphase_sonar_ros_bridge ${catkin_LIBRARIES})
 add_dependencies(sylphase_sonar_ros_bridge ${catkin_EXPORTED_TARGETS})
 
-install(PROGRAMS scripts/ping_publisher.py scripts/ping_printer scripts/ping_logger scripts/ping_plotter scripts/hydrophones DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(PROGRAMS scripts/ping_publisher scripts/ping_printer scripts/ping_logger scripts/ping_plotter scripts/hydrophones DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
diff --git a/mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py b/mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py
index aab9156d9..e4fbac285 100755
--- a/mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py
+++ b/mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py
@@ -13,32 +13,35 @@
 def main():
     # Define the server address and port
     HOST = "127.0.0.1"
-    PORT = 2007  # The port used by the server
+    PORT = 2007
 
     # Create a socket
     with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
+        pub = rospy.Publisher("hydrophones/solved", ProcessedPing, queue_size=10)
         s.connect((HOST, PORT))
-        print(f"Connected to {HOST}:{PORT}")
-        pub = rospy.Publisher("ping", ProcessedPing, queue_size=10)
-        rospy.init_node("pingpublisher", anonymous=True)
+        rospy.loginfo(
+            f"\nConnected to {HOST}:{PORT}\nPublishing to /hydrophones/solved",
+        )
+
+        # Need to ignore the first 2 JSON Parse Errors (nothing wrong)
+        parse_error_count = 0
 
         while not rospy.is_shutdown():
             # Receive data
-            data = s.recv(1024)  # Buffer size is 1024 bytes
+            data = s.recv(1024)
             if not data:
                 break
 
             # Parse the JSON data
             try:
                 json_data = json.loads(data.decode("utf-8"))
-                # Create a ProcessedPing message
                 ping_msg = ProcessedPing()
 
                 # Populate the header
                 ping_msg.header = Header()
-                ping_msg.header.seq += 1  # Increment sequence number
+                ping_msg.header.seq += 1
                 ping_msg.header.stamp = rospy.Time.now()
-                ping_msg.header.frame_id = "ping_frame"  # Set your frame_id
+                ping_msg.header.frame_id = "hydrophones"
 
                 # Populate the position
                 ping_msg.position = Point()
@@ -48,21 +51,21 @@ def main():
 
                 # Populate the frequency and amplitude
                 ping_msg.freq = json_data["frequency_Hz"]
-                ping_msg.amplitude = json_data[
-                    "origin_distance_m"
-                ]  # You can modify this as needed
-                ping_msg.valid = True  # Or set to False based on your logic
+                ping_msg.amplitude = json_data["origin_distance_m"]
+                ping_msg.valid = True
 
                 # Publish the message
                 pub.publish(ping_msg)
-                rospy.loginfo(f"Published: {ping_msg}")
-
             except json.JSONDecodeError as e:
-                rospy.loginfo(f"JSON Decode Error: {e}")
+                parse_error_count += 1
+                # ignore first two (normal behavior)
+                if parse_error_count > 2:
+                    rospy.logerr(f"JSONDecodeError: {e}")
             except KeyError as e:
-                rospy.loginfo(f"Key Error: {e}")
+                rospy.logerr(f"Key Error: {e}")
 
 
 if __name__ == "__main__":
     with contextlib.suppress(rospy.ROSInterruptException):
+        rospy.init_node("pingpublisher", anonymous=True)
         main()
diff --git a/mil_common/drivers/mil_passive_sonar/src/pipeline.sh b/mil_common/drivers/mil_passive_sonar/src/pipeline.sh
index 2a180afe2..5cff2b143 100755
--- a/mil_common/drivers/mil_passive_sonar/src/pipeline.sh
+++ b/mil_common/drivers/mil_passive_sonar/src/pipeline.sh
@@ -1,3 +1,3 @@
 #!/bin/bash
 
-sdgps connect-sonar-raw-tcp --host 192.168.37.51 --port 2006 ! filter-sonar-raw-samples --highpass 15e3 --lowpass 45e3 --notch 40e3 ! extract-robosub-pings ! robosub-ping-solver ! listen-robosub-ping-solution-tcp 2007
+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