diff --git a/mil_common/drivers/mil_passive_sonar/CMakeLists.txt b/mil_common/drivers/mil_passive_sonar/CMakeLists.txt index 8ec65d3e7..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_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 new file mode 100755 index 000000000..bfe8c43a0 --- /dev/null +++ b/mil_common/drivers/mil_passive_sonar/scripts/ping_publisher.py @@ -0,0 +1,71 @@ +#!/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 + + # 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)) + rospy.loginfo( + 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) + parse_error_count = 0 + + while not rospy.is_shutdown(): + # 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.seq += 1 + ping_msg.header.stamp = rospy.Time.now() + 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 = json_data["frequency_Hz"] + ping_msg.amplitude = 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: + rospy.logerr(f"JSONDecodeError: {e}") + except KeyError as 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/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..5cff2b143 --- /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.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