From bf44d193ca61d9ed48e602e9ef114147de9cee63 Mon Sep 17 00:00:00 2001 From: Alex Johnson Date: Sun, 20 Oct 2024 01:12:25 +0000 Subject: [PATCH 1/3] python publisher for sdgps solution to hydrophone pings --- .../drivers/mil_passive_sonar/CMakeLists.txt | 2 +- .../scripts/ping_publisher.py | 68 +++++++++++++++++++ .../src/fakeping_pipeline.sh | 3 + .../drivers/mil_passive_sonar/src/pipeline.sh | 3 + 4 files changed, 75 insertions(+), 1 deletion(-) create mode 100755 mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py create mode 100755 mil_common/drivers/mil_passive_sonar/src/fakeping_pipeline.sh create mode 100755 mil_common/drivers/mil_passive_sonar/src/pipeline.sh diff --git a/mil_common/drivers/mil_passive_sonar/CMakeLists.txt b/mil_common/drivers/mil_passive_sonar/CMakeLists.txt index 8ec65d3e7..6c73b3328 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_printer scripts/ping_logger scripts/ping_plotter scripts/hydrophones DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install(PROGRAMS scripts/ping_publisher.py 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 new file mode 100755 index 000000000..aab9156d9 --- /dev/null +++ b/mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 + +import contextlib +import json +import socket + +import rospy +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 # The port used by the server + + # Create a socket + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: + s.connect((HOST, PORT)) + print(f"Connected to {HOST}:{PORT}") + pub = rospy.Publisher("ping", ProcessedPing, queue_size=10) + rospy.init_node("pingpublisher", anonymous=True) + + while not rospy.is_shutdown(): + # Receive data + data = s.recv(1024) # Buffer size is 1024 bytes + 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.stamp = rospy.Time.now() + ping_msg.header.frame_id = "ping_frame" # Set your frame_id + + # 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 = 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 + + # 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}") + except KeyError as e: + rospy.loginfo(f"Key Error: {e}") + + +if __name__ == "__main__": + with contextlib.suppress(rospy.ROSInterruptException): + main() diff --git a/mil_common/drivers/mil_passive_sonar/src/fakeping_pipeline.sh b/mil_common/drivers/mil_passive_sonar/src/fakeping_pipeline.sh new file mode 100755 index 000000000..cff851f10 --- /dev/null +++ b/mil_common/drivers/mil_passive_sonar/src/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 diff --git a/mil_common/drivers/mil_passive_sonar/src/pipeline.sh b/mil_common/drivers/mil_passive_sonar/src/pipeline.sh new file mode 100755 index 000000000..2a180afe2 --- /dev/null +++ b/mil_common/drivers/mil_passive_sonar/src/pipeline.sh @@ -0,0 +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 From 0263711ff827907f48f86b33b49b14815294745f Mon Sep 17 00:00:00 2001 From: Alex Johnson Date: Sat, 26 Oct 2024 00:44:31 +0000 Subject: [PATCH 2/3] 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 From 39f9006a8f7f2c94fff58354522e3bfeaa64d02c Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Sun, 27 Oct 2024 22:48:45 -0400 Subject: [PATCH 3/3] scripts/ping_publisher.py: Make obvious that first log is from hydrophones library --- mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e4fbac285..bfe8c43a0 100755 --- a/mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py +++ b/mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py @@ -20,7 +20,7 @@ def main(): pub = rospy.Publisher("hydrophones/solved", ProcessedPing, queue_size=10) s.connect((HOST, PORT)) rospy.loginfo( - f"\nConnected to {HOST}:{PORT}\nPublishing to /hydrophones/solved", + f"\nping_publisher connected to {HOST}:{PORT}, forwarding TCP messages to {pub.resolved_name}...", ) # Need to ignore the first 2 JSON Parse Errors (nothing wrong)