diff --git a/riptide_acoustics/CMakeLists.txt b/riptide_acoustics/CMakeLists.txt index ba74c7d..22bf1b6 100644 --- a/riptide_acoustics/CMakeLists.txt +++ b/riptide_acoustics/CMakeLists.txt @@ -8,10 +8,8 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(rclcpp REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(geometry_msgs REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) @@ -19,16 +17,18 @@ find_package(geometry_msgs REQUIRED) add_executable(run_acoustics src/Acoustics.cpp) ament_target_dependencies(run_acoustics -std_msgs -geometry_msgs -rclcpp -tf2 -tf2_ros) +std_msgs +std_srvs +rclcpp) install(TARGETS run_acoustics DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + #yoink files file(GLOB src_py_files RELATIVE ${PROJECT_SOURCE_DIR} src/*.py) install(PROGRAMS ${src_py_files} diff --git a/riptide_acoustics/launch/acoustics.launch.py b/riptide_acoustics/launch/acoustics.launch.py new file mode 100644 index 0000000..0cc1d10 --- /dev/null +++ b/riptide_acoustics/launch/acoustics.launch.py @@ -0,0 +1,23 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument(name="mode", default_value="max", + description="Mode of comparing buffer 0 and buffer 1 to find nearest pinger"), + + Node( + package='riptide_acoustics', + executable='run_acoustics', + name='riptide_acoustics', + output='screen', + parameters=[ + { + "mode": LaunchConfiguration("mode") + } + ], + respawn=True + ) + ]) diff --git a/riptide_acoustics/package.xml b/riptide_acoustics/package.xml index d11f2a7..39b1686 100644 --- a/riptide_acoustics/package.xml +++ b/riptide_acoustics/package.xml @@ -12,9 +12,9 @@ ament_lint_auto ament_lint_common - riptide_msgs2 - rclpy - tf2 + rclcpp + std_msgs + std_srvs ament_cmake diff --git a/riptide_acoustics/src/Acoustics.cpp b/riptide_acoustics/src/Acoustics.cpp index 2a7c99a..cb452a2 100644 --- a/riptide_acoustics/src/Acoustics.cpp +++ b/riptide_acoustics/src/Acoustics.cpp @@ -1,90 +1,216 @@ #include -#include -#include - - +#include +#include +#include +#include #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/float32.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/transform.hpp" - -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/buffer.h" -#include "tf2/exceptions.h" +#include "std_msgs/msg/bool.hpp" +#include "std_srvs/srv/trigger.hpp" +#define MODE_MAX_FALLBACK_DIFF 1000.0f +#define P_VALUE 95.0f -using namespace std::chrono_literals; -using std::placeholders::_1; +using namespace std::placeholders; +// 1) start_sample -> collect amplitudes for buffer 0 +// 2) stop_sample -> close buffer 0 +// 3) start_sample -> collect ampltudes for buffer 1 +// 4) stop_sample -> close buffer 1, then compare and publish once to buffer_0_closer -class MinimalSubscriber : public rclcpp::Node +class Acoustics : public rclcpp::Node { public: - MinimalSubscriber() + Acoustics() : Node("riptide_acoustics") { - subscription_ = this->create_subscription( - "acoustics/delta_t", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); + this->declare_parameter("mode", "avg"); + + ampSubscription = this->create_subscription( + "/talos/ivc/pinger/selected_freq_amp_stream", 10, std::bind(&Acoustics::ampCallback, this, _1)); + + resultPublisher = this->create_publisher( + "/talos/acoustics/buffer_0_closer", 10); - //initialize tf buffer and tf listener - tf_buffer_ = std::make_unique(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); + resultTimer = this->create_wall_timer( + std::chrono::seconds(1), std::bind(&Acoustics::publishResult, this)); - portToStarboardTransformTimer = this->create_wall_timer(1s, std::bind(&MinimalSubscriber::getPortToStarboardTransform, this)); + startSampleService = this->create_service( + "/talos/acoustics/start_sample", std::bind(&Acoustics::startSample, this, _1, _2)); + + stopSampleService = this->create_service( + "/talos/acoustics/stop_sample", std::bind(&Acoustics::stopSample, this, _1, _2)); } private: - void topic_callback(const std_msgs::msg::Float32::SharedPtr msg) const + void ampCallback(const std_msgs::msg::Float32::SharedPtr msg) + { + if (sampling) { + buffers[activeBuffer].push_back(msg->data); + } + } + + void startSample(const std_srvs::srv::Trigger::Request::SharedPtr, + std_srvs::srv::Trigger::Response::SharedPtr response) { - geometry_msgs::msg::TransformStamped t; - - try { - t = tf_buffer_->lookupTransform( - "world", "talos/base_link", - tf2::TimePointZero); - } catch (const tf2::TransformException & ex) { - RCLCPP_INFO( - this->get_logger(), "Could not transform!"); + if (sampling) { + response->success = false; + response->message = "Already sampling"; + return; + } + + buffers[activeBuffer].clear(); + sampling = true; + haveResult = false; + + response->success = true; + response->message = "Sampling into buffer " + std::to_string(activeBuffer); + RCLCPP_INFO(this->get_logger(), "Started sampling into buffer %d", activeBuffer); + } + + void stopSample(const std_srvs::srv::Trigger::Request::SharedPtr, + std_srvs::srv::Trigger::Response::SharedPtr response) + { + if (!sampling) { + response->success = false; + response->message = "Not sampling"; + return; + } + + sampling = false; + + RCLCPP_INFO(this->get_logger(), "Stopped sampling buffer %d with %zu samples", + activeBuffer, buffers[activeBuffer].size()); + + if (activeBuffer == 0) { + activeBuffer = 1; + response->success = true; + response->message = "With mode [" + this->get_parameter("mode").as_string() + "] Buffer 0 closed with " + std::to_string(buffers[0].size()) + " samples"; return; } - RCLCPP_INFO(this->get_logger(), "X: %f", t.transform.translation.x); + // buffer 1 just closed, compare and reset + compareBuffers(response); + activeBuffer = 0; + } + std::string make_comparison_response(const std::string &mode) { + auto summary = [&](const std::string &mode_label) { + return "mode => [" + mode_label + "], result => buffer " + + std::to_string(buffer0Closer ? 0 : 1) + + " closer [buffer 0: " + std::to_string(result_0) + + " buffer 1: " + std::to_string(result_1) + "]"; + }; + + std::string resp = ""; + if (mode == "avg") { + resp = summary("avg"); + } else if (mode == "max") { + if (did_fallback_on_max) { + resp += "INFO: [FELLBACK TO AVG ON MAX MODE] "; + } + resp += summary("max"); + } else if (mode == "p95") { + resp = summary("p95"); + resp = "UNRECOGNIZED MODE, CHECK LAUNCH FILE OR SOMETHING "; + } + return resp; } - void getPortToStarboardTransform() + void compareBuffers(std_srvs::srv::Trigger::Response::SharedPtr response) { - //stamped msg - geometry_msgs::msg::TransformStamped stamped; + if (buffers[0].empty() || buffers[1].empty()) { + response->success = false; + response->message = "Cannot compare, buffer 0 has " + std::to_string(buffers[0].size()) + + " samples and buffer 1 has " + std::to_string(buffers[1].size()); + RCLCPP_WARN(this->get_logger(), "%s", response->message.c_str()); + return; + } - //get transform from tf buffer - try { + reduceBuffers(buffers[0], buffers[1], this->get_parameter("mode").as_string()); + haveResult = true; + publishResult(); - //TODO: change to port and starboard pod frames - stamped = tf_buffer_->lookupTransform("talos/pose_sensor_link", "talos/base_link", tf2::TimePointZero); - - portToStarboardTransform = stamped.transform; + response->success = true; + response->message = make_comparison_response(this->get_parameter("mode").as_string()); + // response->message = "mode[" + this->get_parameter("mode").as_string() + "]buffer " + std::to_string(buffer0Closer ? 0 : 1) + + // " closer (buffer 0: " + std::to_string(result_0) + + // ", buffer 1: " + std::to_string(result_1) + ")"; + RCLCPP_INFO(this->get_logger(), "%s", response->message.c_str()); + } - RCLCPP_INFO(this->get_logger(), "Successfuly collected port to starboard transform!"); - } catch (const tf2::TransformException & ex) { - RCLCPP_INFO( - this->get_logger(), "Could collect port to starboard transform, retrying!"); + void publishResult() + { + if (!haveResult) { return; } - portToStarboardTransformTimer->cancel(); + std_msgs::msg::Bool msg; + msg.data = buffer0Closer; + resultPublisher->publish(msg); + } + + void compare_avg_buffers(const std::vector &buffer0, const std::vector &buffer1) { + result_0 = std::accumulate(buffer0.begin(), buffer0.end(), 0.0) / buffer0.size(); + result_1 = std::accumulate(buffer1.begin(), buffer1.end(), 0.0) / buffer1.size(); + buffer0Closer = result_0 >= result_1; } - rclcpp::Subscription::SharedPtr subscription_; + void compare_p95_buffers(const std::vector &buffer0, const std::vector &buffer1) { + // it's c++ so sorting is O(n) right? (fire bar - balke) + std::vector sorted0 = buffer0; + std::vector sorted1 = buffer1; + size_t idx_0 = static_cast(std::clamp((double)P_VALUE, 0.0, 100.0) / 100.0 * (sorted0.size() - 1)); + size_t idx_1 = static_cast(std::clamp((double)P_VALUE, 0.0, 100.0) / 100.0 * (sorted1.size() - 1)); + std::nth_element(sorted0.begin(), sorted0.begin() + idx_0, sorted0.end()); + std::nth_element(sorted1.begin(), sorted1.begin() + idx_1, sorted1.end()); + result_0 = sorted0[idx_0]; + result_1 = sorted1[idx_1]; + buffer0Closer = result_0 >= result_1; + } + + void compare_max_buffers(const std::vector &buffer0, const std::vector &buffer1) { + result_0 = *std::max_element(buffer0.begin(), buffer0.end()); + result_1 = *std::max_element(buffer1.begin(), buffer1.end()); + if (std::abs(result_0 - result_1) < MODE_MAX_FALLBACK_DIFF) { + did_fallback_on_max = true; + compare_avg_buffers(buffer0, buffer1); + } + } + + void reduceBuffers(const std::vector &buffer0, const std::vector &buffer1, std::string mode) { + if (mode == "avg") { + compare_avg_buffers(buffer0, buffer1); + } + + if (mode == "max") { + compare_max_buffers(buffer0, buffer1); + } - //tf - std::shared_ptr tf_listener_{nullptr}; - std::unique_ptr tf_buffer_; + if (mode == "p95") { + compare_p95_buffers(buffer0, buffer1); + } + } - //port pod to starboard pod transform - geometry_msgs::msg::Transform portToStarboardTransform; - rclcpp::TimerBase::SharedPtr portToStarboardTransformTimer {nullptr}; + rclcpp::Subscription::SharedPtr ampSubscription; + rclcpp::Publisher::SharedPtr resultPublisher; + rclcpp::TimerBase::SharedPtr resultTimer; + rclcpp::Service::SharedPtr startSampleService; + rclcpp::Service::SharedPtr stopSampleService; + + // amplitude samples for the two comparison windows + std::vector buffers[2]; + int activeBuffer = 0; + bool sampling = false; + + // last comparison result, invalid until the first compare completes + bool buffer0Closer = false; + bool haveResult = false; + bool did_fallback_on_max = false; + // the results of the comparison run on the buffers + float result_0 = 0.0f; + float result_1 = 0.0f; }; @@ -92,7 +218,7 @@ class MinimalSubscriber : public rclcpp::Node int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/riptide_acoustics/src/Acoustics_Helper.py b/riptide_acoustics/src/Acoustics_Helper.py deleted file mode 100644 index 7ca8a96..0000000 --- a/riptide_acoustics/src/Acoustics_Helper.py +++ /dev/null @@ -1,113 +0,0 @@ -#! /usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from rclpy.qos import qos_profile_sensor_data -from rclpy.duration import Duration -from riptide_msgs2.msg import DshotRPMFeedback, DshotPartialTelemetry -from geometry_msgs.msg import Vector3Stamped -from tf2_ros import TransformException -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener -from time import sleep -import os -import datetime -from rclpy.time import Time - - -#the purpose of this node is to concatinate all thruster data into a single message that includes the latest rpm from each thruster - -#TODO figure out real topic names - -ACOUSTICS_PINGER_LOCATION = [3.0, 2.0, -1.0, 0.0, 0.0, 0.0] -BASE_FRAME = "map" -TARGET_FRAME_STARTBOARD = "talos/acoustics_starboard_link" -TARGET_FRAME_PORT = "talos/acoustics_port_link" -DATA_FOLDER_NAME = "acoustics_data" - - -class Acoustics_Helper(Node): - - def __init__(self): - #init super node - super().__init__("Acoustics_Helpers") - - self.acoustics_sub = self.create_subscription(Vector3Stamped, "/talos/acoustics/delta_t", self.acoustics_cb, qos_profile_sensor_data) - - - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) - - try: - #ensure there is a data file - os.mkdir(DATA_FOLDER_NAME) - except: - self.get_logger().info('Created Data Folder') - - #open the log file - now = datetime.datetime.now() - # Format the date and time as a string - formatted_date_time = now.strftime("%Y-%m-%d %H:%M:%S") - filename = DATA_FOLDER_NAME + "/data" + formatted_date_time + ".csv" - self.data_file = open(filename, "w") - - #write the file header - headers = "number,time,delta_t,pos x 1,pos y 1,pos z 1,or w 1,or x 1,or y 1, or z 1,pos x 2,pos y 2,pos z 2,or w 2,or x 2,or y 2, or z 2\n" - self.data_file.write(headers) - - self.points_written = 0 - - - - - async def acoustics_cb(self, msg): - #wait to ensure transforms are available - try: - - delta_t_time = msg.header.stamp - - starboard_transform = await self.tf_buffer.lookup_transform_async( - BASE_FRAME, - TARGET_FRAME_STARTBOARD, - delta_t_time) - - port_transform = await self.tf_buffer.lookup_transform_async( - BASE_FRAME, - TARGET_FRAME_PORT, - delta_t_time) - - - msg_time = msg.header.stamp.sec + msg.header.stamp.nanosec * 10e-9 - delta_t = msg.vector.x - - #formulate the output line - self.points_written = self.points_written + 1 - - output_line = str(self.points_written) + "," + str(msg_time) + "," + str(delta_t) + "," + \ - str(starboard_transform.transform.translation.x) + "," + str(starboard_transform.transform.translation.y) + "," + str(starboard_transform.transform.translation.z) + "," + \ - str(starboard_transform.transform.rotation.w) + "," + str(starboard_transform.transform.rotation.x) + "," + str(starboard_transform.transform.rotation.y) + "," + str(starboard_transform.transform.rotation.z) + "," + \ - str(port_transform.transform.translation.x) + "," + str(port_transform.transform.translation.y) + "," + str(port_transform.transform.translation.z) + "," + \ - str(port_transform.transform.rotation.w) + "," + str(port_transform.transform.rotation.x) + "," + str(port_transform.transform.rotation.y) + "," + str(port_transform.transform.rotation.z) + "\n" - - #write to csv - self.data_file.write(output_line) - - except TransformException as ex: - self.get_logger().info( - f'Could not transform to pinger frame: {ex}') - return - - - - - -def main(args=None): - - rclpy.init(args=args) - node = Acoustics_Helper() - rclpy.spin(node) - node.data_file.close() - - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/riptide_acoustics/src/Acoustics_Initial_Development.zip b/riptide_acoustics/src/Acoustics_Initial_Development.zip deleted file mode 100644 index f25317a..0000000 Binary files a/riptide_acoustics/src/Acoustics_Initial_Development.zip and /dev/null differ diff --git a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/Acoustics.SLDPRT b/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/Acoustics.SLDPRT deleted file mode 100644 index 360d9d6..0000000 Binary files a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/Acoustics.SLDPRT and /dev/null differ diff --git a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/acoustics.m b/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/acoustics.m deleted file mode 100644 index cbb1056..0000000 --- a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/acoustics.m +++ /dev/null @@ -1,118 +0,0 @@ -%Script to Calculate the resolution of 2 hydrophone acoustics system -clear; - -%All units in meters -hydrophone_base_width = 50;%0.05372; -water_speed = 1500; -adc_sampling_frequency = 5*10^5; - -%define transform between two pulses -x1 = -95.04; -y1 = 8.49; -heading_change = -21.8912/360 *2*pi(); %radians - -x2 = x1 - hydrophone_base_width * (1 - cos(heading_change)); -y2 = y1 + hydrophone_base_width * sin(heading_change); - - -%data gathered from acoustics -k1 = 2; -k2 = 19; - -% %equations to solve acoustics systems -% syms a1 a2 c1 c2 B1 B2 -% -% %equations of localization -% eq1 = sqrt(x1^2+y1^2) == sqrt((a1+c1)^2-B1^2) + sqrt((a2+c2^2)-B1^2); -% eq2 = sqrt(x2^2+y2^2) == sqrt((a1+c1+k1)^2-B2^2) + sqrt((a2+c2+k2)^2-B2^2); -% eq3 = B1^2 == (a2+c2)^2 -(((a1+c1)^2-(a2+c2)^2-(x1^2+y1^2))/(-2*sqrt(x1^2+y1^2)))^2; -% eq4 = B2^2 == (a2+c2+k2)^2 -(((a1+c1+k1)^2-(a2+c2+k2)^2-(x2^2+y2^2))/(-2*sqrt(x2^2+y2^2)))^2; -% eq5 = 2*(k1+1)*c1 + 2*k1*a1 + 4*a1*c1 == .5*hydrophone_base_width + k1^2; -% eq6 = 2*(k2+1)*c2 + 2*k2*a2 + 4*a2*c2 == .5*hydrophone_base_width + k2^2; -% -% solve(eq1,eq2,eq3,eq4,eq5,eq6,a1,a2,c1,c2,B1,B2) - -%equations to solve acoustics systems -%x0 = [64.7495,44,2.5505,12.30,46.2,41.04]'; - -answer_count = 20; -answer = zeros(6, answer_count); -counter = 0; - -while(counter < answer_count) - - norm1 = 5; - while(norm1 > .001) - - x0 = rand([6,1])*5000; - - %equations of localization -% F = @(x) [-sqrt(abs((x(1)+x(3))^2-x(5)^2)) + sqrt(abs((x(2)+x(4))^2-x(5)^2)) - sqrt(x1^2+y1^2); -% sqrt(abs((x(1)+x(3)+k1)^2-x(6)^2)) - sqrt(abs((x(2)+x(4)+k2)^2-x(6)^2))-sqrt(x2^2+y2^2); -% (x(2)+x(4))^2 - (((x(1)+x(3))^2-(x(2)+x(4))^2-(x1^2+y1^2))/(-2*sqrt(x1^2+y1^2)))^2 - x(5)^2; -% (x(2)+x(4)+k2)^2 - (((x(1)+x(3)+k1)^2-(x(2)+x(4)+k2)^2-(x2^2+y2^2))/(-2*sqrt(x2^2+y2^2)))^2 - x(6)^2; -% .5*hydrophone_base_width^2 - k1^2 - 2*(k1+x(3))*x(3) - 2*k1*x(1) - 4*x(1)*x(3); -% .5*hydrophone_base_width^2 - k2^2 - 2*(k2+x(4))*x(4) - 2*k2*x(2) - 4*x(2)*x(4)]; - - - - options = optimoptions('fsolve','Display','iter', "OptimalityTolerance", 10e-12); - - solve1 = fsolve(F, x0, options); - - norm1 = norm(F(solve1)); - end - - counter = counter + 1; - answer(:, counter) = solve1; - -end - - - -w = (2*answer(3,:)*k1 + answer(3,:).^2 + k1^2 + 2 .* answer(1,:) .* answer(3,:) + 2 * answer(1,:) * k1 - hydrophone_base_width^2 / 4) / -hydrophone_base_width; -h = sqrt(answer(1,:).^2 - w.^2); - -%rot = [cos(heading_change),-sin(heading_change);sin(heading_change),cos(heading_change)]; - -fx1 = [0;0]; -length = 1; -for counter = 1:answer_count - if(w(counter) < 0) - fx1(:,length) = [1;w(counter)]; - fy1(length)= h(counter); - - length = length + 1; - end -end - -b1 = inv(fx1*fx1') * fx1 * fy1'; - -w2t = (2*answer(4,:)*k2 + answer(4,:).^2 + k2^2 + 2 .* answer(2,:) .* answer(4,:) + 2 * answer(2,:) * k2 - hydrophone_base_width^2 / 4) / -hydrophone_base_width; -h2t = sqrt(answer(2,:).^2 - w2t.^2); - - -figure(1) - -fx2 = [ones(size(w2t(1,:)));w2t]; -fy2= h2t; - -b2 = inv(fx2*fx2') * fx2 * fy2'; -b2 = -tan(atan(b2) - heading_change); -b2_origin = [0; b2(1)]; -rot = [cos(heading_change), -sin(heading_change); sin(heading_change), cos(heading_change)]; -b2_origin = rot*b2_origin + [(x1+x2)/2; (y1+y2)/2]; - -plot(fx1(2,:),fy1, "or", w2t, h2t,"xg") - -figure(2) -w_vals = linspace(-200,200, 1000); -h1_vals = b1(1) + b1(2) * w_vals; -h2_vals = b2(2) * w_vals; - -plot(w_vals, h1_vals, w_vals + b2_origin(1), h2_vals + b2_origin(2), [-120.04], [2998], "go") - -b2_intercept = b2_origin(2) - b2_origin(1) * b2(2); - -intersection_x = -(b2_intercept - b1(1))/(b2(2) - b1(2)); -intersection_y = b1(2) * intersection_x + b1(1); diff --git a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/acoustics_two_pulse_accurary.m b/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/acoustics_two_pulse_accurary.m deleted file mode 100644 index 74a53c9..0000000 --- a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/acoustics_two_pulse_accurary.m +++ /dev/null @@ -1,114 +0,0 @@ -%do some accurary verification -%units = mm - -clear; - -%cases that need evaluated for - % - negative sqrts in eq1 and eq2 - % - sign of slope 2 - - -%using little boat base width - -hydrophone_base_width = 50; -water_speed = 1500000; -rp2040_adc_sampling_frequency = 5*10^5; -MHz1_adc_sampling_frequency = 5*10^6; - -adc_distance_accurarcy_rp2040 = 1 / (rp2040_adc_sampling_frequency) * water_speed; -adc_distance_accurarcy_1MHZ = 1 / (MHz1_adc_sampling_frequency) * water_speed; - -samples = 10; - -%should yeild (2998.5,95.04) -% distances = [2,19]; -% transform = [-95.04, 8.49, -.38207]; - -%should yeild (-44 ,98.17) -% distances = [20, -7.91]; -% transform = [-150, 10, -.6604]; - -%should yeild (52,116.92) -% distances = [-20, -18.3411]; -% transform = [-175, 10, -.6604]; - -%should yeild (52,116.92) -distances = [-20, 41.465]; -transform = [141.89, 22.89, -.26074]; - -%should yield [1,1] -%distances = [35.36;29.7]; - - -[slv_x,slv_y] = solve_two_pulse_system(distances(1),distances(2),transform(1),transform(2),transform(3),50, 3000) - -positions_5 = zeros(2,samples); -count = 1; -while count <= samples - - [x,y] = solve_two_pulse_system(distances(1) + (rand() - .5) *adc_distance_accurarcy_rp2040,distances(2) + (rand() - .5) *adc_distance_accurarcy_rp2040,transform(1),transform(2),transform(3),50, 1000); - positions_5(1, count) = x; - positions_5(2, count) = y; - - count = count + 1; -end - - -positions_6 = zeros(2,samples); -count = 1; -while count <= samples - - [x,y] = solve_two_pulse_system(distances(1) + (rand() - .5) *adc_distance_accurarcy_1MHZ,distances(2) + (rand() - .5) *adc_distance_accurarcy_1MHZ,transform(1),transform(2),transform(3),50, 1000); - positions_6(1, count) = x; - positions_6(2, count) = y; - - count = count + 1; -end - -figure(1) -plot([slv_x], [slv_y], "xg", positions_5(1,:), positions_5(2,:), "or", positions_6(1,:), positions_6(2,:), "ob") -title("Solve Results for Acoustics Target at -1m,1m for LB Baseline") -xlabel("Solved Distance (mm)") -ylabel("Solved Distance (mm)") -legend("Solve With No Error", "RP2040 ADC", "1MHz ADC") - -% -% %talos baseline -% -% %should yield[-1,1] -% distances = [141.2444;118.367]; -% -% %should yield[1,1] -% distances = [-141.2444; -167.7482]; -% -% [slv_x,slv_y] = solve_two_pulse_system(distances(1),distances(2),-400,100,.05,200, 10); -% -% positions_5 = zeros(2,samples); -% count = 1; -% while count <= samples -% -% [x,y] = solve_two_pulse_system(distances(1) + (rand() - .5) *adc_distance_accurarcy_rp2040,distances(2) + (rand() - .5) *adc_distance_accurarcy_rp2040,-400,100,.05,200, 10); -% positions_5(1, count) = x; -% positions_5(2, count) = y; -% -% count = count + 1 -% end -% -% -% positions_6 = zeros(2,samples); -% count = 1; -% while count <= samples -% -% [x,y] = solve_two_pulse_system(distances(1) + (rand() - .5) * adc_distance_accurarcy_1MHZ,distances(2) + (rand() - .5) *adc_distance_accurarcy_1MHZ,-400,100,.05,200, 10); -% positions_6(1, count) = x; -% positions_6(2, count) = y; -% -% count = count + 1 -% end -% -% figure(2) -% plot([slv_x], [slv_y], "xg", positions_5(1,:), positions_5(2,:), "or", positions_6(1,:), positions_6(2,:), "ob") -% title("Solve Results for Acoustics Target at -1m,1m for Talos Baseline") -% xlabel("Solved Distance (mm)") -% ylabel("Solved Distance (mm)") -% legend("Solve With No Error", "RP2040 ADC", "1MHz ADC") \ No newline at end of file diff --git a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/solve_two_pulse_system.m b/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/solve_two_pulse_system.m deleted file mode 100644 index c4286ba..0000000 --- a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/solve_two_pulse_system.m +++ /dev/null @@ -1,73 +0,0 @@ -function [intersection_x, intersection_y] = solve_two_pulse_system(k1, k2, x1, y1, heading_change, hydrophone_base_width, estimated_distance) -%k1: hydrophone 1 pulse time - hydrophone 2 pulse time for first pulse -%k2: hydrophone 1 pulse time - hydrophone 2 pulse time for second pulse -%x1: x distance traveled between pulses in the coordinate frame of first -%pulse -%y1: y distance traveled between pulses in the coordinate frame of second -%pulse -%heading change: the change in heading of vehicle between the two pulses: -%ccw is positive -%solves: the number of successful solves to use in estimatation process -%hydrophone_base_width: distance between the two hydrophones - -%transform location of other hydrophone -x2 = x1 + hydrophone_base_width * (cos(heading_change)); -y2 = y1 + hydrophone_base_width * sin(heading_change); - -%define core functions -Y = @(a,k) sqrt(a^2 + 2*a*k + k^2 - ((hydrophone_base_width^2 + 2*a*k +k^2)/(2*hydrophone_base_width))^2); -X = @(a,y,k) sqrt(a^2 - y^2) * -sign(k) - hydrophone_base_width / 2; - - -norm1 = 5; -answer = [0;0]; -while(norm1 > .001) - - x0 = [rand() * estimated_distance;rand() * estimated_distance];% [estimated_distance; estimated_distance; estimated_distance; estimated_distance; k1 / 2 ; k2 / 2] * (.2 - rand()*.2); - - %equations of localization -% F = @(x) [sqrt(abs((x(1)+x(3))^2-x(5)^2))*sign((x(1)+x(3))^2-x(5)^2) + sqrt(abs((x(2)+x(4))^2-x(5)^2))*sign((x(1)+x(3))^2-x(5)^2) - sqrt(x1^2+y1^2); -% sqrt(abs((x(1)+x(3)+k1)^2-x(6)^2))*sign((x(1)+x(3))^2-x(5)^2) + sqrt(abs((x(2)+x(4)+k2)^2-x(6)^2))*sign((x(1)+x(3))^2-x(5)^2) - sqrt(x2^2+y2^2); -% (x(2)+x(4))^2 - (((x(1)+x(3))^2-(x(2)+x(4))^2-(x1^2+y1^2))/(-2*sqrt(x1^2+y1^2)))^2 - x(5)^2; -% (x(2)+x(4)+k2)^2 - (((x(1)+x(3)+k1)^2-(x(2)+x(4)+k2)^2-(x2^2+y2^2))/(-2*sqrt(x2^2+y2^2)))^2 - x(6)^2; -% .5*hydrophone_base_width^2 - k1^2 - 2*(k1+x(3))*x(3) - 2*k1*x(1) - 4*x(1)*x(3); -% .5*hydrophone_base_width^2 - k2^2 - 2*(k2+x(4))*x(4) - 2*k2*x(2) - 4*x(2)*x(4)]; - - - - - %cramers rule & determinant of rot matrix is one - F = @(x) [cos(-heading_change)*(Y(x(1), k1) - (y1 + y2) / 2) + (sin(-heading_change) * (X(x(1), Y(x(1), k1), k1) - (x1 + x2 - hydrophone_base_width) / 2)) - Y(x(2),k2); - (X(x(1), Y(x(1), k1), k1) - (x1 + x2 - hydrophone_base_width) / 2) * cos(-heading_change) - (Y(x(1), k1) - (y1 + y2) / 2) * sin(-heading_change) - (X(x(2),Y(x(2),k2), k2))]; - - %options = optimoptions('fsolve','Display','iter', "OptimalityTolerance", 10e-12); - options = optimoptions('fsolve', 'Display', 'off', "OptimalityTolerance", 10e-6); - - solve1 = fsolve(F, x0, options); - - norm1 = norm(F(solve1)); - answer = solve1; -end - -%calculate position - -intersection_x = X(answer(1),Y(answer(1), k1),k1); -intersection_y = Y(answer(1), k1); - -%for quadratic fit -% intersection_x = (-(b2(2) - b1(2)) - sqrt(abs((b2(2) - b1(2))^2 - 4 * (b2(1) - b1(1))*(b2(3) - b1(3))))) / (2 * (b2(3) - b1(3))); -% intersection_y = b1(1) + b1(2)* intersection_x + b1(3) * intersection_x^2; - - -%plots for debug -% figure(1); -% plot(fx1(2,:),fy1, "or", rot_points(1,:), rot_points(2,:),"xg") - -% figure(2) -% w_vals = linspace(-1000,1000, 1000); -% h1_vals = b1(1) + b1(2) * w_vals + b1(3)*w_vals.^2; -% h2_vals = b2(2) * w_vals + b2(1) + b2(3) * w_vals.^2;%b2_intercept ; -% -% plot(w_vals, h1_vals,"r", w_vals, h2_vals, "g", [-1000], [1000], "bo") - - diff --git a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/solve_two_pulse_system_depth.asv b/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/solve_two_pulse_system_depth.asv deleted file mode 100644 index 11fe75a..0000000 --- a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/solve_two_pulse_system_depth.asv +++ /dev/null @@ -1,113 +0,0 @@ -function [intersection_x, intersection_y] = solve_two_pulse_system_depth(k1, k2, pose_1, pose_2, hydrophone_base_vector, estimated_distance, pinger_depth) -%k1: hydrophone 1 pulse time - hydrophone 2 pulse time for first pulse -%k2: hydrophone 1 pulse time - hydrophone 2 pulse time for second pulse -%pose_1: the pose of the front hydrophone at the time of the first pulse -%pose_2: the pose of the front hydrophone at the time of the second pulse -%hydrophone_base_vector: the position vector between hydrophone 1 and 2 -%estimated disance: the estimated distance from the front hydrophone and -%pinger -%pinger_depth: the estimated depth of the pinger - -%pose format = [x,y,z,rw,rx,ry,rz] - -%base frame is hydrophone 1 pulse 1 = [0,0,0,0,0,0] -x1_1 = 0; -y1_1 = 0; -z1_1 = 0; - -%calculate position of second hydrophone -%order [Z,Y,X] -eul_1 = quat2eul([pose_1(4), pose_1(5), pose_1(6), pose_1(7)]); -norm_matrix = eul2rotm([-eul_1(1), 0, 0]); -rot_1 = norm_matrix * quat2rotm([pose_1(4), pose_1(5), pose_1(6), pose_1(7)]); -pos2_1 = rot_1 * hydrophone_base_vector; - -x1_2 = pose_2(1) - pose_1(1); -y1_2 = pose_2(2) - pose_1(2); -z1_2 = pose_2(3) - pose; - -%calculate position of second hydrophone in second pulse -rot_2 = norm_matrix * quat2rotm([pose_2(4), pose_2(5), pose_2(6), pose_2(7)]); -pos2_2 = rot_2 * hydrophone_base_vector + [x1_2;y1_2;z1_2]; - -%base widths in x,y plane -h1 = pos2_1(1); -h2 = norm([pos2_2(1) - x1_2 , pos2_2(2) - y1_2]); - -%calculate delta positions -x1 = x1_2 - x1_1; -x2 = pos2_2(1) - x1_1; - -y1 = y1_2 - y1_1; -y2 = pos2_2(2) - pos2_1(2); - -%changes in depth -dz1_1 = pose_1(3) + pinger_depth; -dz2_1 = pos2_1(3) + pinger_depth; -dz1_2 = pose_2(3) + pinger_depth; -dz2_2 = pos2_2(3) + pinger_depth; - -%calculate heading change -eul_2 = rotm2eul(rot_2); -heading_change = eul_2(1); - -%define core functions -K = @(a,k_bar,z1,z2) (-(2*a) + sqrt(4*a^2 + 4 * (k_bar^2 + 2 * k_bar * sqrt(a^2 + z1^2) + z1^2 - z2^2))) / 2; -Y = @(a,k,h) sqrt(a^2 + 2*a*k + k^2 - ((h^2 + 2*a*k +k^2)/(2*h))^2); -X = @(a,y,k,h) sqrt(a^2 - y^2) * -sign(k) - h / 2; - -norm1 = 5; -answer = [0;0]; -while(norm1 > .001) - - x0 = [rand() * estimated_distance;rand() * estimated_distance];% [estimated_distance; estimated_distance; estimated_distance; estimated_distance; k1 / 2 ; k2 / 2] * (.2 - rand()*.2); - - %equations of localization -% F = @(x) [sqrt(abs((x(1)+x(3))^2-x(5)^2))*sign((x(1)+x(3))^2-x(5)^2) + sqrt(abs((x(2)+x(4))^2-x(5)^2))*sign((x(1)+x(3))^2-x(5)^2) - sqrt(x1^2+y1^2); -% sqrt(abs((x(1)+x(3)+k1)^2-x(6)^2))*sign((x(1)+x(3))^2-x(5)^2) + sqrt(abs((x(2)+x(4)+k2)^2-x(6)^2))*sign((x(1)+x(3))^2-x(5)^2) - sqrt(x2^2+y2^2); -% (x(2)+x(4))^2 - (((x(1)+x(3))^2-(x(2)+x(4))^2-(x1^2+y1^2))/(-2*sqrt(x1^2+y1^2)))^2 - x(5)^2; -% (x(2)+x(4)+k2)^2 - (((x(1)+x(3)+k1)^2-(x(2)+x(4)+k2)^2-(x2^2+y2^2))/(-2*sqrt(x2^2+y2^2)))^2 - x(6)^2; -% .5*hydrophone_base_width^2 - k1^2 - 2*(k1+x(3))*x(3) - 2*k1*x(1) - 4*x(1)*x(3); -% .5*hydrophone_base_width^2 - k2^2 - 2*(k2+x(4))*x(4) - 2*k2*x(2) - 4*x(2)*x(4)]; - - - %cramers rule & determinant of rot matrix is one -% F = @(x) [cos(-heading_change)*(Y(x(1), k1, h1) - (y1 + y2) / 2) + (sin(-heading_change) * (X(x(1), Y(x(1), k1, h1), k1, h1) - (x1 + x2 - hydrophone_base_vector(1)) / 2)) - Y(x(2),k2, h2); -% (X(x(1), Y(x(1), k1, h1), k1, h1) - (x1 + x2 - hydrophone_base_vector(1)) / 2) * cos(-heading_change) - (Y(x(1), k1, h1) - (y1 + y2) / 2) * sin(-heading_change) - (X(x(2),Y(x(2),k2,h2), k2, h2))]; - F = @(x) [cos(-heading_change)*(Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1) - (y1 + y2) / 2) + (sin(-heading_change) * (X(x(1), Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1), K(x(1), k1, dz1_1, dz2_1), h1) - (x1 + x2 - hydrophone_base_vector(1)) / 2)) - Y(x(2),K(x(2), k2, dz1_2, dz2_2), h2); - (X(x(1), Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1), K(x(1), k1, dz1_1, dz2_1), h1) - (x1 + x2 - hydrophone_base_vector(1)) / 2) * cos(-heading_change) - (Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1) - (y1 + y2) / 2) * sin(-heading_change) - (X(x(2),Y(x(2),K(x(2), k2, dz1_2, dz2_2),h2), K(x(2), k2, dz1_2, dz2_2), h2))]; - - %options = optimoptions('fsolve','Display','iter', "OptimalityTolerance", 10e-12); - options = optimoptions('fsolve', 'Display', 'off', "OptimalityTolerance", 10e-6); - - %solve non-linear system - solve1 = fsolve(F, x0, options); - - norm1 = norm(F(solve1)); - answer = solve1; - -end - - -%calculate position - -intersection_x = X(answer(1),Y(answer(1), K(answer(1), k1, dz1_1, dz2_1), h1), K(answer(1), k1, dz1_1, dz2_1), h1) + x1_1; -intersection_y = Y(answer(1), K(answer(1), k1, dz1_1, dz2_1), h1) + y1_1; - -%for quadratic fit -% intersection_x = (-(b2(2) - b1(2)) - sqrt(abs((b2(2) - b1(2))^2 - 4 * (b2(1) - b1(1))*(b2(3) - b1(3))))) / (2 * (b2(3) - b1(3))); -% intersection_y = b1(1) + b1(2)* intersection_x + b1(3) * intersection_x^2; - - -%plots for debug -% figure(1); -% plot(fx1(2,:),fy1, "or", rot_points(1,:), rot_points(2,:),"xg") - -% figure(2) -% w_vals = linspace(-1000,1000, 1000); -% h1_vals = b1(1) + b1(2) * w_vals + b1(3)*w_vals.^2; -% h2_vals = b2(2) * w_vals + b2(1) + b2(3) * w_vals.^2;%b2_intercept ; -% -% plot(w_vals, h1_vals,"r", w_vals, h2_vals, "g", [-1000], [1000], "bo") - - diff --git a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/solve_two_pulse_system_depth.m b/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/solve_two_pulse_system_depth.m deleted file mode 100644 index 81cd8f2..0000000 --- a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/solve_two_pulse_system_depth.m +++ /dev/null @@ -1,115 +0,0 @@ -function [intersection_x, intersection_y] = solve_two_pulse_system_depth(k1, k2, pose_1, pose_2, hydrophone_base_vector, estimated_distance, pinger_depth) -%k1: hydrophone 1 pulse time - hydrophone 2 pulse time for first pulse -%k2: hydrophone 1 pulse time - hydrophone 2 pulse time for second pulse -%pose_1: the pose of the front hydrophone at the time of the first pulse -%pose_2: the pose of the front hydrophone at the time of the second pulse -%hydrophone_base_vector: the position vector between hydrophone 1 and 2 -%estimated disance: the estimated distance from the front hydrophone and -%pinger -%pinger_depth: the estimated depth of the pinger - -%pose format = [x,y,z,rw,rx,ry,rz] - -%base frame is hydrophone 1 pulse 1 = [0,0,0,0,0,0] -x1_1 = 0; -y1_1 = 0; -z1_1 = 0; - -pinger_depth = pinger_depth - pose_1(3); - -%calculate position of second hydrophone -%order [Z,Y,X] -eul_1 = quat2eul([pose_1(4), pose_1(5), pose_1(6), pose_1(7)]); -norm_matrix = eul2rotm([-eul_1(1), 0, 0]); -rot_1 = norm_matrix * quat2rotm([pose_1(4), pose_1(5), pose_1(6), pose_1(7)]); -pos2_1 = rot_1 * hydrophone_base_vector; - -x1_2 = pose_2(1) - pose_1(1); -y1_2 = pose_2(2) - pose_1(2); -z1_2 = pose_2(3) - pose_1(3); - -%calculate position of second hydrophone in second pulse -rot_2 = norm_matrix * quat2rotm([pose_2(4), pose_2(5), pose_2(6), pose_2(7)]); -pos2_2 = rot_2 * hydrophone_base_vector + [x1_2;y1_2;z1_2]; - -%base widths in x,y plane -h1 = pos2_1(1); -h2 = norm([pos2_2(1) - x1_2 , pos2_2(2) - y1_2]); - -%calculate delta positions -x1 = x1_2 - x1_1; -x2 = pos2_2(1) - x1_1; - -y1 = y1_2 - y1_1; -y2 = pos2_2(2) - pos2_1(2); - -%changes in depth -dz1_1 = pinger_depth; -dz2_1 = pos2_1(3) + pinger_depth; -dz1_2 = pose_2(3) + pinger_depth; -dz2_2 = pos2_2(3) + pinger_depth; - -%calculate heading change -eul_2 = rotm2eul(rot_2); -heading_change = eul_2(1); - -%define core functions -K = @(a,k_bar,z1,z2) (-(2*a) + sqrt(4*a^2 + 4 * (k_bar^2 + 2 * k_bar * sqrt(a^2 + z1^2) + z1^2 - z2^2))) / 2; -Y = @(a,k,h) sqrt(a^2 + 2*a*k + k^2 - ((h^2 + 2*a*k +k^2)/(2*h))^2); -X = @(a,y,k,h) sqrt(a^2 - y^2) * -sign(k) - h / 2; - -norm1 = 5; -answer = [0;0]; -while(norm1 > .001) - - x0 = [rand() * estimated_distance;rand() * estimated_distance];% [estimated_distance; estimated_distance; estimated_distance; estimated_distance; k1 / 2 ; k2 / 2] * (.2 - rand()*.2); - - %equations of localization -% F = @(x) [sqrt(abs((x(1)+x(3))^2-x(5)^2))*sign((x(1)+x(3))^2-x(5)^2) + sqrt(abs((x(2)+x(4))^2-x(5)^2))*sign((x(1)+x(3))^2-x(5)^2) - sqrt(x1^2+y1^2); -% sqrt(abs((x(1)+x(3)+k1)^2-x(6)^2))*sign((x(1)+x(3))^2-x(5)^2) + sqrt(abs((x(2)+x(4)+k2)^2-x(6)^2))*sign((x(1)+x(3))^2-x(5)^2) - sqrt(x2^2+y2^2); -% (x(2)+x(4))^2 - (((x(1)+x(3))^2-(x(2)+x(4))^2-(x1^2+y1^2))/(-2*sqrt(x1^2+y1^2)))^2 - x(5)^2; -% (x(2)+x(4)+k2)^2 - (((x(1)+x(3)+k1)^2-(x(2)+x(4)+k2)^2-(x2^2+y2^2))/(-2*sqrt(x2^2+y2^2)))^2 - x(6)^2; -% .5*hydrophone_base_width^2 - k1^2 - 2*(k1+x(3))*x(3) - 2*k1*x(1) - 4*x(1)*x(3); -% .5*hydrophone_base_width^2 - k2^2 - 2*(k2+x(4))*x(4) - 2*k2*x(2) - 4*x(2)*x(4)]; - - - %cramers rule & determinant of rot matrix is one -% F = @(x) [cos(-heading_change)*(Y(x(1), k1, h1) - (y1 + y2) / 2) + (sin(-heading_change) * (X(x(1), Y(x(1), k1, h1), k1, h1) - (x1 + x2 - hydrophone_base_vector(1)) / 2)) - Y(x(2),k2, h2); -% (X(x(1), Y(x(1), k1, h1), k1, h1) - (x1 + x2 - hydrophone_base_vector(1)) / 2) * cos(-heading_change) - (Y(x(1), k1, h1) - (y1 + y2) / 2) * sin(-heading_change) - (X(x(2),Y(x(2),k2,h2), k2, h2))]; - F = @(x) [cos(-heading_change)*(Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1) - (y1 + y2) / 2) + (sin(-heading_change) * (X(x(1), Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1), K(x(1), k1, dz1_1, dz2_1), h1) - (x1 + x2 - hydrophone_base_vector(1)) / 2)) - Y(x(2),K(x(2), k2, dz1_2, dz2_2), h2); - (X(x(1), Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1), K(x(1), k1, dz1_1, dz2_1), h1) - (x1 + x2 - hydrophone_base_vector(1)) / 2) * cos(-heading_change) - (Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1) - (y1 + y2) / 2) * sin(-heading_change) - (X(x(2),Y(x(2),K(x(2), k2, dz1_2, dz2_2),h2), K(x(2), k2, dz1_2, dz2_2), h2))]; - - %options = optimoptions('fsolve','Display','iter', "OptimalityTolerance", 10e-12); - options = optimoptions('fsolve', 'Display', 'off', "OptimalityTolerance", 10e-6); - - %solve non-linear system - solve1 = fsolve(F, x0, options); - - norm1 = norm(F(solve1)); - answer = solve1; - -end - - -%calculate position - -intersection_x = X(answer(1),Y(answer(1), K(answer(1), k1, dz1_1, dz2_1), h1), K(answer(1), k1, dz1_1, dz2_1), h1) + pose_1(1); -intersection_y = Y(answer(1), K(answer(1), k1, dz1_1, dz2_1), h1) + pose_1(2); - -%for quadratic fit -% intersection_x = (-(b2(2) - b1(2)) - sqrt(abs((b2(2) - b1(2))^2 - 4 * (b2(1) - b1(1))*(b2(3) - b1(3))))) / (2 * (b2(3) - b1(3))); -% intersection_y = b1(1) + b1(2)* intersection_x + b1(3) * intersection_x^2; - - -%plots for debug -% figure(1); -% plot(fx1(2,:),fy1, "or", rot_points(1,:), rot_points(2,:),"xg") - -% figure(2) -% w_vals = linspace(-1000,1000, 1000); -% h1_vals = b1(1) + b1(2) * w_vals + b1(3)*w_vals.^2; -% h2_vals = b2(2) * w_vals + b2(1) + b2(3) * w_vals.^2;%b2_intercept ; -% -% plot(w_vals, h1_vals,"r", w_vals, h2_vals, "g", [-1000], [1000], "bo") - - diff --git a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/transform.m b/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/transform.m deleted file mode 100644 index 7df340c..0000000 --- a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/transform.m +++ /dev/null @@ -1,40 +0,0 @@ -pose_1 = [0,0,0,.9061,-.0747,.1802,.3753]; -pose_2 = [2,0,0,1,0,0,0]; -hydrophone_base_vector = [1;0;0]; - -disp("Point 1") -x1_1 = pose_1(1) -y1_1 = pose_1(2) -z1_1 = pose_1(3) - - -%calculate position of second hydrophone -%order [Z,Y,X] -eul_1 = quat2eul([pose_1(4), pose_1(5), pose_1(6), pose_1(7)]); -norm_matrix = eul2rotm([-eul_1(1), 0, 0]); -rot_1 = norm_matrix * quat2rotm([pose_1(4), pose_1(5), pose_1(6), pose_1(7)]); -pos2_1 = rot_1 * hydrophone_base_vector; - -disp("Disp") -x1_2 = pose_2(1) - x1_1; -y1_2 = pose_2(2) - y1_1; -z1_2 = pose_2(3) - z1_1; - -%calculate position of second hydrophone in second pulse -rot_2 = norm_matrix * quat2rotm([pose_2(4), pose_2(5), pose_2(6), pose_2(7)]); -pos2_2 = rot_2 * hydrophone_base_vector + [x1_2;y1_2;z1_2]; - -%base widths in x,y plane -h1 = pos2_1(1); -h2 = norm([pos2_2(1) - x1_2 , pos2_2(2) - y1_2]); - -%calculate delta positions -x1 = x1_2 - x1_1; -x2 = pos2_2(1) - pos2_1(1); - -y1 = y1_2 - y1_1; -y2 = pos2_2(2) - pos2_1(2); - -%calculate heading change -eul_2 = rotm2eul(rot_2); -heading_change = eul_2(1); \ No newline at end of file diff --git a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/two_pulse_depth_test.m b/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/two_pulse_depth_test.m deleted file mode 100644 index acb581c..0000000 --- a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/two_pulse_depth_test.m +++ /dev/null @@ -1,31 +0,0 @@ -%should yeild (52,116.92) -distances = [-20, 41.465]; -pose1 = [0,0,0,1,0,0,0]; -pose2 = [141.89, 22.89,0, .9915,0,0,-.1300]; -pinger_depth = 0; - - -[slv_x,slv_y] = solve_two_pulse_system_depth(distances(1),distances(2),pose1, pose2, [50;0;0], 3000, pinger_depth) - -%should yeild (52,116.92) -distances = [-10.893, 23.1377]; -pose1 = [0,0,0,1,0,0,0]; -pose2 = [141.89, 22.89,0, .9915,0,0,-.1300]; -pinger_depth = -200; - - -[slv_x,slv_y] = solve_two_pulse_system_depth(distances(1),distances(2),pose1, pose2, [50;0;0], 100000, pinger_depth) - - -%should yeild (152,216.92) -distances = [-10.893, 23.1377]; -pose1 = [100,100,0,1,0,0,0]; -pose2 = [241.89, 122.89,0, .9915,0,0,-.1300]; -pinger_depth = -200; - - -[slv_x,slv_y] = solve_two_pulse_system_depth(distances(1),distances(2),pose1, pose2, [50;0;0], 100000, pinger_depth) - -%should yeild (2998.5,95.04) -% distances = [2,19]; -% transform = [-95.04, 8.49, -.38207]; diff --git a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/untitled.asv b/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/untitled.asv deleted file mode 100644 index 05abb62..0000000 --- a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/untitled.asv +++ /dev/null @@ -1,33 +0,0 @@ -pose_1 = [0,0,0,.9061,-.0747,.1802,.3753]; -pose_2 = [2,0,0,1,0,0,0]; -hydrophone_base_vector = [1;0;0]; - -x1_1 = pose_1(1); -y1_1 = pose_1(2); -z1_1 = pose_1(3); - -%calculate position of second hydrophone -%order [Z,Y,X] -eul_1 = quat2eul([pose_1(4), pose_1(5), pose_1(6), pose_1(7)]); -norm_matrix = eul2rotm([-eul_1(1), 0, 0]); -rot_1 = norm_matrix * quat2rotm([pose_1(4), pose_1(5), pose_1(6), pose_1(7)]); -pos2_1 = rot_1 * hydrophone_base_vector; - -x1_2 = pose_2(1) - x1_1; -y1_2 = pose_2(2) - y1_1; -z1_2 = pose_2(3) - z1_1; - -%calculate position of second hydrophone in second pulse -rot_2 = norm_matrix * quat2rotm([pose_2(4), pose_2(5), pose_2(6), pose_2(7)]); -pos2_2 = rot_2 * hydrophone_base_vector + [x1_2;y1_2;z1_2]; - -%base widths in x,y plane -h1 = pos2_1(1); -h2 = norm([pos2_2(1) - x1_2 , pos2_2(2) - y1_2]); - -%calculate delta positions -x1 = x2_1 - x1_1; -x2 = pos2_2(1) - pos2_2; - -y1 = y1_2 - y1_1; -y2 = pos2_2(1) - pos2_1() diff --git a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/~$Acoustics.SLDPRT b/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/~$Acoustics.SLDPRT deleted file mode 100644 index 8d5a03a..0000000 Binary files a/riptide_acoustics/src/Acoustics_Initial_Development/Acoustics/~$Acoustics.SLDPRT and /dev/null differ diff --git a/riptide_acoustics/src/UndeterminantMethod/UndeterminantNode.asv b/riptide_acoustics/src/UndeterminantMethod/UndeterminantNode.asv deleted file mode 100644 index 747d94c..0000000 --- a/riptide_acoustics/src/UndeterminantMethod/UndeterminantNode.asv +++ /dev/null @@ -1,159 +0,0 @@ -clear; -clear global; - -%read from config -speed_of_sound = 1500; -pinger_depth = -1.0; - -%initialize node -global node; -node = ros2node("/acoustics_node"); - -%intialize tf2 -global tftree; -tftree = ros2tf(node); - -%initialize transform array -global measurements; - -%get acoustic pod transform -global origin_to_port_transform -global port_to_startboard_transform - -global initial_pinger_estimate -initial_pinger_estimate = [4.0,1.8,-1.1,0.0,0.0,0.0]; - -transforms_recieved = false; -while(transforms_recieved == false) - %loop until transforms become available - - try - origin_to_port_transform = getTransform(tftree, "talos/acoustics_port_link", "talos/origin"); - port_to_startboard_transform = getTransform(tftree, "talos/acoustics_starboard_link", "talos/acoustics_port_link"); - - transforms_recieved = true; - catch - pause(1); - disp("Waiting for acoustic transforms to become available!"); - end -end - - -%initialize publishers and subscribers -pub = ros2publisher(node, "/test_matlab", "std_msgs/UInt8"); - -%add subscriber to add to measurements -delta_t_sub = ros2subscriber(node, "/talos/acoustics/delta_t", @delta_t_callback); - -%add rate function to analyze measurements -r = ros2rate(node, 1); -reset(r) - -x = 0; -estimated_pinger_position = initial_pinger_estimate(1:3)'; -while(x < 1) - estimated_pinger_position = run_analysis(port_to_startboard_transform, origin_to_port_transform, measurements, pinger_depth, estimated_pinger_position, speed_of_sound) - waitfor(r) -end - -function [estimated_pinger_position] = run_analysis(port_to_startboard_transform, origin_to_port_transform, measurements, pinger_depth, pinger_pose, speed_of_sound) - %wait until at least five samples have been taken - points_to_use = 15; - - if(max(size((measurements))) > points_to_use) - disp("Running Analysis") - - %get base vector translation - hydrophone_base_vector = [port_to_startboard_transform.transform.translation.x; port_to_startboard_transform.transform.translation.y; port_to_startboard_transform.transform.translation.z]; - - %origin to port vector - origin_to_port_vector = [origin_to_port_transform.transform.translation.x; origin_to_port_transform.transform.translation.y; origin_to_port_transform.transform.translation.z]; - - %determine the points to use - num_points_saved = max(size(measurements)); - - points_index_array = linspace(num_points_saved - points_to_use + 1, num_points_saved, points_to_use); - counter = points_to_use - 1; - point_seperation = .1; - counter2 = num_points_saved - 1; - while(counter > 0 && counter2 > 0) - - auv_position_test = [measurements(counter2).auv_origin.transform.translation.x;measurements(counter2).auv_origin.transform.translation.y;measurements(counter2).auv_origin.transform.translation.z]; - auv_position_previous = [measurements(counter + 1).auv_origin.transform.translation.x;measurements(counter + 1).auv_origin.transform.translation.y;measurements(counter + 1).auv_origin.transform.translation.z]; - - %if the points are sperated by enough distance - if(norm(auv_position_test - auv_position_previous) > point_seperation) - points_index_array(counter) = counter2; - - counter = counter - 1; - end - - counter2 = counter2 - 1; - end - - %only run if the specified number of points was found - if(counter == 0) - - %assemble poses matrix and delta ds - poses = zeros(7, points_to_use); - delta_ds = zeros(points_to_use, 1); - for counter = 1:points_to_use - auv_position = [measurements(points_index_array(counter)).auv_origin.transform.translation.x;measurements(points_index_array(counter)).auv_origin.transform.translation.y;measurements(points_index_array(counter)).auv_origin.transform.translation.z]; - auv_rotation = [measurements(points_index_array(counter)).auv_origin.transform.rotation.w;measurements(points_index_array(counter)).auv_origin.transform.rotation.x;measurements(points_index_array(counter)).auv_origin.transform.rotation.y;measurements(points_index_array(counter)).auv_origin.transform.rotation.z]; - - %calculate the port hydrophones pose in world frame - poses(:, counter) = [auv_position + quat2rotm(auv_rotation') * origin_to_port_vector; auv_rotation]; - - %convert delta_t to delta ds - delta_ds(counter) = measurements(points_index_array(counter)).delta_t * speed_of_sound; - end - - disp(poses) - disp(delta_ds ) - - estimated_pinger_position = estimate_pinger_position(poses, delta_ds, pinger_pose, hydrophone_base_vector); - %estimated_pinger_position(3) = pinger_depth; - else - estimated_pinger_position = pinger_pose; - - disp("Points to close") - end - else - estimated_pinger_position = pinger_pose; - end - -end - - -function delta_t_callback(message) - %introduce global variables - global tftree; - global measurements; - - try - pause(.1) - - %get the transform at the time of the measurement - map_to_origin_transform = getTransform(tftree, "map", 'talos/origin', message.header.stamp); - - %fill out measurement data struct - measurement = struct(); - measurement.delta_t = message.vector.x; - measurement.frequency = message.vector.y; - measurement.stamp = message.header.stamp; - measurement.auv_origin = map_to_origin_transform; - - if(isempty(measurements)) - measurements = [measurement]; - - else - measurements(end+1) = measurement; - end - - catch - disp("Failed to get transform") - - %tftree.AvailableFrames - end - -end \ No newline at end of file diff --git a/riptide_acoustics/src/UndeterminantMethod/UndeterminantNode.m b/riptide_acoustics/src/UndeterminantMethod/UndeterminantNode.m deleted file mode 100644 index 0b108d0..0000000 --- a/riptide_acoustics/src/UndeterminantMethod/UndeterminantNode.m +++ /dev/null @@ -1,160 +0,0 @@ -clear; -clear global; - -%read from config -speed_of_sound = 1500; -pinger_depth = -1.0; - -%initialize node -global node; -node = ros2node("/acoustics_node"); - -%intialize tf2 -global tftree; -tftree = ros2tf(node); - -%initialize transform array -global measurements; - -%get acoustic pod transform -global origin_to_port_transform -global port_to_startboard_transform - -global initial_pinger_estimate -initial_pinger_estimate = [4.0,1.8,-1.1,0.0,0.0,0.0]; - -transforms_recieved = false; -while(transforms_recieved == false) - %loop until transforms become available - - try - origin_to_port_transform = getTransform(tftree, "talos/origin", "talos/acoustics_port_link"); - port_to_startboard_transform = getTransform(tftree, "talos/acoustics_port_link", "talos/acoustics_starboard_link"); - - transforms_recieved = true; - catch - pause(1); - disp("Waiting for acoustic transforms to become available!"); - end -end - - -%initialize publishers and subscribers -pub = ros2publisher(node, "/test_matlab", "std_msgs/UInt8"); - -%add subscriber to add to measurements -delta_t_sub = ros2subscriber(node, "/talos/acoustics/delta_t", @delta_t_callback); - -%add rate function to analyze measurements -r = ros2rate(node, 100); -reset(r) - -x = 0; -estimated_pinger_position = initial_pinger_estimate(1:3)'; -while(x < 1) - estimated_pinger_position = run_analysis(port_to_startboard_transform, origin_to_port_transform, measurements, pinger_depth, estimated_pinger_position, speed_of_sound) - waitfor(r) -end - -function [estimated_pinger_position] = run_analysis(port_to_startboard_transform, origin_to_port_transform, measurements, pinger_depth, pinger_pose, speed_of_sound) - %wait until at least five samples have been taken - points_to_use = 15; - - if(max(size((measurements))) > points_to_use) - disp("Running Analysis") - - %get base vector translation - hydrophone_base_vector = [port_to_startboard_transform.transform.translation.x; port_to_startboard_transform.transform.translation.y; port_to_startboard_transform.transform.translation.z]; - - %origin to port vector - origin_to_port_vector = [origin_to_port_transform.transform.translation.x; origin_to_port_transform.transform.translation.y; origin_to_port_transform.transform.translation.z]; - - %determine the points to use - num_points_saved = max(size(measurements)); - - points_index_array = linspace(num_points_saved - points_to_use + 1, num_points_saved, points_to_use); - counter = points_to_use - 1; - point_seperation = .1; - counter2 = num_points_saved - 1; - while(counter > 0 && counter2 > 0) - - auv_position_test = [measurements(counter2).auv_origin.transform.translation.x;measurements(counter2).auv_origin.transform.translation.y;measurements(counter2).auv_origin.transform.translation.z]; - auv_position_previous = [measurements(points_index_array(counter + 1)).auv_origin.transform.translation.x;measurements(points_index_array(counter + 1)).auv_origin.transform.translation.y;measurements(points_index_array(counter + 1)).auv_origin.transform.translation.z]; - - %if the points are sperated by enough distance - if(norm(auv_position_test - auv_position_previous) > point_seperation) - - points_index_array(counter) = counter2; - - counter = counter - 1; - end - - counter2 = counter2 - 1; - end - - %only run if the specified number of points was found - if(counter == 0) - - %assemble poses matrix and delta ds - poses = zeros(7, points_to_use); - delta_ds = zeros(points_to_use, 1); - for counter = 1:points_to_use - auv_position = [measurements(points_index_array(counter)).auv_origin.transform.translation.x;measurements(points_index_array(counter)).auv_origin.transform.translation.y;measurements(points_index_array(counter)).auv_origin.transform.translation.z]; - auv_rotation = [measurements(points_index_array(counter)).auv_origin.transform.rotation.w;measurements(points_index_array(counter)).auv_origin.transform.rotation.x;measurements(points_index_array(counter)).auv_origin.transform.rotation.y;measurements(points_index_array(counter)).auv_origin.transform.rotation.z]; - - %calculate the port hydrophones pose in world frame - poses(:, counter) = [auv_position + quat2rotm(auv_rotation') * origin_to_port_vector; auv_rotation]; - - %convert delta_t to delta ds - delta_ds(counter) = measurements(points_index_array(counter)).delta_t * speed_of_sound; - end - - disp(poses) - disp(delta_ds ) - - estimated_pinger_position = estimate_pinger_position(poses, delta_ds, pinger_pose, hydrophone_base_vector); - %estimated_pinger_position(3) = pinger_depth; - else - estimated_pinger_position = pinger_pose; - - disp("Points to close") - end - else - estimated_pinger_position = pinger_pose; - end - -end - - -function delta_t_callback(message) - %introduce global variables - global tftree; - global measurements; - - try - pause(.1) - - %get the transform at the time of the measurement - map_to_origin_transform = getTransform(tftree, "map", 'talos/origin', message.header.stamp); - - %fill out measurement data struct - measurement = struct(); - measurement.delta_t = message.vector.x; - measurement.frequency = message.vector.y; - measurement.stamp = message.header.stamp; - measurement.auv_origin = map_to_origin_transform; - - if(isempty(measurements)) - measurements = [measurement]; - - else - measurements(end+1) = measurement; - end - - catch - disp("Failed to get transform") - - %tftree.AvailableFrames - end - -end \ No newline at end of file diff --git a/riptide_acoustics/src/UndeterminantMethod/calculate_distance_from_solve.asv b/riptide_acoustics/src/UndeterminantMethod/calculate_distance_from_solve.asv deleted file mode 100644 index 7d73835..0000000 --- a/riptide_acoustics/src/UndeterminantMethod/calculate_distance_from_solve.asv +++ /dev/null @@ -1,113 +0,0 @@ -function [test_point, current_distance] = calculate_distance_from_solve(delta_d, characteristic_distance, transformed_point) - %delta_d - distance from hydrophone 2 to pinger - distance from - %hydrophone 1 to pinger - %characteristic distance - (+) distance between the two hydrophones - %hydrophone 1 is assumed to be at (0,0,0) - %transformed point - the point to be evalulated transformed into the - %frame in which h1 is (0,0,0) and h2 is (0, cd, 0) - - convergence_rate = 0.001; - - %calculate function based on delta d and cd - if(delta_d > 0) - y_xz = @(x, z, d, s) (d^3 + sqrt(d^4*s^2 - 2*d^2*s^4 + 4*d^2*s^2*x^2 + 4*d^2*s^2*z^2 + s^6 - 4*s^4*x^2 - 4*s^4*z^2) - d*s^2)/(2*(d^2 - s^2)); - else - y_xz = @(x, z, d, s) (d^3 - sqrt(d^4*s^2 - 2*d^2*s^4 + 4*d^2*s^2*x^2 + 4*d^2*s^2*z^2 + s^6 - 4*s^4*x^2 - 4*s^4*z^2) - d*s^2)/(2*(d^2 - s^2)); - end - - %dz_dx = @(x,y,d,s) (2*s*x) / sqrt(abs(d^4 - 2*d^2*s^2 + s^4 - 4*s^2*x^2 - 4*d^3*y + 4*d*s^2*y + 4*d^2*y^2 - 4*s^2*y^2)) * sign(d^4 - 2*d^2*s^2 + s^4 - 4*s^2*x^2 - 4*d^3*y + 4*d*s^2*y + 4*d^2*y^2 - 4*s^2*y^2); - %dz_dy = @(x,y,d,s) ((d^2 - s^2)*(d - 2*y))/(s*sqrt(abs(d^4 + s^4 - 4*d^3*y + 4*d*s^2*y - 2*d^2*(s^2 - 2*y^2) - 4*s^2*(x^2 + y^2)))) * sign(d^4 + s^4 - 4*d^3*y + 4*d*s^2*y - 2*d^2*(s^2 - 2*y^2) - 4*s^2*(x^2 + y^2)); - distance = @(x,y,z,point) norm(point - [x;y;z]); - %ddist_dx = @(x,y,d,s,point) -((point(1) - x) + (point(3) - z_xy(x,y,d,s)) * dz_dx(x,y,d,s)) / norm(point - [x,y,z_xy(x,y,d,s)]); - %ddist_dy = @(x,y,d,s,point) -((point(2) - y) + (point(3) - z_xy(x,y,d,s)) * dz_dy(x,y,d,s)) / norm(point - [x,y,z_xy(x,y,d,s)]); - - %guess where the cloest point is -- directly up - test_point = [transformed_point(1), y_xz(transformed_point(1), transformed_point(3), characteristic_distance, delta_d), transformed_point(3)]; - - %find the clostest point - counter = 5; - relative_error_threshold = 1e-4; - previous_distance = -1; - convergence_multiplier = .001; - while counter > 0 - %calculate the gradient - - temp_test_1 = test_point(1) - convergence_multiplier * (distance(test_point(1) + convergence_rate, y_xz(convergence_rate + test_point(1), test_point(3), characteristic_distance, delta_d), test_point(3), transformed_point) - distance(test_point(1) - convergence_rate, y_xz(test_point(1) - convergence_rate, test_point(3), characteristic_distance, delta_d), test_point(3), transformed_point)) / (2); - test_point(3) = test_point(3) - convergence_multiplier * (distance(test_point(1), y_xz(test_point(1), test_point(3) + convergence_rate, characteristic_distance, delta_d), test_point(3) + convergence_rate, transformed_point) - distance(test_point(1), y_xz(test_point(1), test_point(3) - convergence_rate, characteristic_distance, delta_d), test_point(3) - convergence_rate, transformed_point)) / (2); - test_point(1) = temp_test_1; - - test_point(2) = y_xz(test_point(1), test_point(3), characteristic_distance, delta_d); - - current_distance = distance(test_point(1), y_xz(test_point(1), test_point(3), characteristic_distance, delta_d), test_point(3), transformed_point); - - if(previous_distance - relative_error_threshold < current_distance) - counter = counter - 1; - else - counter = 5; - end - - if(previous_distance == -1 || previous_distance > current_distance) - previous_distance = current_distance; - else - convergence_multiplier = convergence_multiplier / 2; - end - - end - - %calculate the vector between the closest point and the guessed - %vector = test_point - transformed_point; - % - % gradient = [1 / ddist_dx(test_point(1), test_point(1), characteristic_distance, delta_d, transformed_point), 1 / ddist_dy(test_point(1), test_point(1), characteristic_distance, delta_d, transformed_point), 1] - % - % ddist_dx(-1, 1, characteristic_distance, delta_d, transformed_point) - % ddist_dy(-1, -1, characteristic_distance, delta_d, transformed_point) - % - % plotting vals for surface - - %UNCOMMENT FOR DEBUG - num_x = 200; - num_z = 100; - x_vals = linspace(-abs(test_point(1)) - .5, abs(test_point(1)) + .5, num_x); - z_vals = linspace(-abs(test_point(2) - .5), abs(test_point(2)) + .5, 100); - x_mat = zeros(1, num_x * 100); - z_mat = zeros(1, num_x * 100); - y_mat = zeros(1, num_x * 100); - dist_mat = zeros(1, num_x*100); - - for xi = 0:(num_x - 1) - for yi = 1:num_z - x_mat(1, xi * num_z + yi) = x_vals(xi + 1); - _mat(1, xi * num_y + yi) = y_vals(yi); - - y_mat(1, xi * num_z + yi) = y_xz(x_vals(xi + 1), z_vals(yi), characteristic_distance, delta_d); - dist_mat(1, xi * num_z + yi) = norm(transformed_point - [x_mat(1, xi * num_z + yi); z_mat(1, xi * num_z + yi); y_mat(1, xi * num_z + yi)]); - end - end - - figure; - hold on - plot3([0,0], [0,characteristic_distance], [0,0], "*r") - plot3(x_mat, z_mat, y_mat, ".b") - plot3(test_point(1), test_point(2), test_point(3), "*g") - plot3(transformed_point(1), transformed_point(2), transformed_point(3), "*g") - title("Calculate Distance Visualization") - subtitle_string = "Delta d: " + num2str(delta_d); - subtitle(subtitle_string); - xlabel("X") - ylabel("Y") - zlabel("Z") - hold off - - figure; - hold on - plot3(x_mat, z_mat, dist_mat, ".r") - plot3(test_point(1), test_point(2), distance(test_point(1), test_point(2), test_point(3), transformed_point), "*b") - title("Distance from pinger point") - subtitle_string = "Norm Distance: " + num2str(distance(test_point(1), test_point(2), test_point(3), transformed_point)); - subtitle(subtitle_string); - xlabel("X") - ylabel("Y") - zlabel("Dist") - hold off - -end diff --git a/riptide_acoustics/src/UndeterminantMethod/calculate_distance_from_solve.m b/riptide_acoustics/src/UndeterminantMethod/calculate_distance_from_solve.m deleted file mode 100644 index 241a5c6..0000000 --- a/riptide_acoustics/src/UndeterminantMethod/calculate_distance_from_solve.m +++ /dev/null @@ -1,113 +0,0 @@ -function [test_point, current_distance] = calculate_distance_from_solve(delta_d, characteristic_distance, transformed_point) - %delta_d - distance from hydrophone 2 to pinger - distance from - %hydrophone 1 to pinger - %characteristic distance - (+) distance between the two hydrophones - %hydrophone 1 is assumed to be at (0,0,0) - %transformed point - the point to be evalulated transformed into the - %frame in which h1 is (0,0,0) and h2 is (0, cd, 0) - - convergence_rate = 0.001; - - %calculate function based on delta d and cd - if(delta_d > 0) - y_xz = @(x, z, d, s) (d^3 + sqrt(d^4*s^2 - 2*d^2*s^4 + 4*d^2*s^2*x^2 + 4*d^2*s^2*z^2 + s^6 - 4*s^4*x^2 - 4*s^4*z^2) - d*s^2)/(2*(d^2 - s^2)); - else - y_xz = @(x, z, d, s) (d^3 - sqrt(d^4*s^2 - 2*d^2*s^4 + 4*d^2*s^2*x^2 + 4*d^2*s^2*z^2 + s^6 - 4*s^4*x^2 - 4*s^4*z^2) - d*s^2)/(2*(d^2 - s^2)); - end - - %dz_dx = @(x,y,d,s) (2*s*x) / sqrt(abs(d^4 - 2*d^2*s^2 + s^4 - 4*s^2*x^2 - 4*d^3*y + 4*d*s^2*y + 4*d^2*y^2 - 4*s^2*y^2)) * sign(d^4 - 2*d^2*s^2 + s^4 - 4*s^2*x^2 - 4*d^3*y + 4*d*s^2*y + 4*d^2*y^2 - 4*s^2*y^2); - %dz_dy = @(x,y,d,s) ((d^2 - s^2)*(d - 2*y))/(s*sqrt(abs(d^4 + s^4 - 4*d^3*y + 4*d*s^2*y - 2*d^2*(s^2 - 2*y^2) - 4*s^2*(x^2 + y^2)))) * sign(d^4 + s^4 - 4*d^3*y + 4*d*s^2*y - 2*d^2*(s^2 - 2*y^2) - 4*s^2*(x^2 + y^2)); - distance = @(x,y,z,point) norm(point - [x;y;z]); - %ddist_dx = @(x,y,d,s,point) -((point(1) - x) + (point(3) - z_xy(x,y,d,s)) * dz_dx(x,y,d,s)) / norm(point - [x,y,z_xy(x,y,d,s)]); - %ddist_dy = @(x,y,d,s,point) -((point(2) - y) + (point(3) - z_xy(x,y,d,s)) * dz_dy(x,y,d,s)) / norm(point - [x,y,z_xy(x,y,d,s)]); - - %guess where the cloest point is -- directly up - test_point = [transformed_point(1), y_xz(transformed_point(1), transformed_point(3), characteristic_distance, delta_d), transformed_point(3)]; - - %find the clostest point - counter = 5; - relative_error_threshold = 1e-8; - previous_distance = -1; - convergence_multiplier = .1; - while counter > 0 - %calculate the gradient - - temp_test_1 = test_point(1) - convergence_multiplier * (distance(test_point(1) + convergence_rate, y_xz(convergence_rate + test_point(1), test_point(3), characteristic_distance, delta_d), test_point(3), transformed_point) - distance(test_point(1) - convergence_rate, y_xz(test_point(1) - convergence_rate, test_point(3), characteristic_distance, delta_d), test_point(3), transformed_point)) / (2); - test_point(3) = test_point(3) - convergence_multiplier * (distance(test_point(1), y_xz(test_point(1), test_point(3) + convergence_rate, characteristic_distance, delta_d), test_point(3) + convergence_rate, transformed_point) - distance(test_point(1), y_xz(test_point(1), test_point(3) - convergence_rate, characteristic_distance, delta_d), test_point(3) - convergence_rate, transformed_point)) / (2); - test_point(1) = temp_test_1; - - test_point(2) = y_xz(test_point(1), test_point(3), characteristic_distance, delta_d); - - current_distance = distance(test_point(1), y_xz(test_point(1), test_point(3), characteristic_distance, delta_d), test_point(3), transformed_point); - - if(previous_distance - relative_error_threshold < current_distance) - counter = counter - 1; - else - counter = 5; - end - - if(previous_distance == -1 || previous_distance > current_distance) - previous_distance = current_distance; - else - convergence_multiplier = convergence_multiplier / 2; - end - - end - - %calculate the vector between the closest point and the guessed - %vector = test_point - transformed_point; - % - % gradient = [1 / ddist_dx(test_point(1), test_point(1), characteristic_distance, delta_d, transformed_point), 1 / ddist_dy(test_point(1), test_point(1), characteristic_distance, delta_d, transformed_point), 1] - % - % ddist_dx(-1, 1, characteristic_distance, delta_d, transformed_point) - % ddist_dy(-1, -1, characteristic_distance, delta_d, transformed_point) - % - % plotting vals for surface - - %UNCOMMENT FOR DEBUG - % num_x = 200; - % num_z = 100; - % x_vals = linspace(-abs(test_point(1)) - .5, abs(test_point(1)) + .5, num_x); - % z_vals = linspace(-abs(test_point(2) - .5), abs(test_point(2)) + .5, 100); - % x_mat = zeros(1, num_x * 100); - % z_mat = zeros(1, num_x * 100); - % y_mat = zeros(1, num_x * 100); - % dist_mat = zeros(1, num_x*100); - % - % for xi = 0:(num_x - 1) - % for zi = 1:num_z - % x_mat(1, xi * num_z + zi) = x_vals(xi + 1); - % z_mat(1, xi * num_z + zi) = z_vals(zi); - % - % y_mat(1, xi * num_z + zi) = y_xz(x_vals(xi + 1), z_vals(zi), characteristic_distance, delta_d); - % dist_mat(1, xi * num_z + zi) = norm(transformed_point - [x_mat(1, xi * num_z + zi); y_mat(1, xi * num_z + zi); z_mat(1, xi * num_z + zi);]); - % end - % end - % - % figure; - % hold on - % plot3([0,0], [0,characteristic_distance], [0,0], "*r") - % plot3(x_mat, y_mat, z_mat, ".b") - % plot3(test_point(1), test_point(2), test_point(3), "*g") - % plot3(transformed_point(1), transformed_point(2), transformed_point(3), "*g") - % title("Calculate Distance Visualization") - % subtitle_string = "Delta d: " + num2str(delta_d); - % subtitle(subtitle_string); - % xlabel("X") - % ylabel("Y") - % zlabel("Z") - % hold off - % - % figure; - % hold on - % plot3(x_mat, z_mat, dist_mat, ".r") - % plot3(test_point(1), test_point(3), distance(test_point(1), test_point(2), test_point(3), transformed_point), "*b") - % title("Distance from pinger point") - % subtitle_string = "Norm Distance: " + num2str(distance(test_point(1), test_point(2), test_point(3), transformed_point)); - % subtitle(subtitle_string); - % xlabel("X") - % ylabel("Z") - % zlabel("Dist") - % hold off - -end diff --git a/riptide_acoustics/src/UndeterminantMethod/estimate_pinger_position.asv b/riptide_acoustics/src/UndeterminantMethod/estimate_pinger_position.asv deleted file mode 100644 index 6cbf68f..0000000 --- a/riptide_acoustics/src/UndeterminantMethod/estimate_pinger_position.asv +++ /dev/null @@ -1,64 +0,0 @@ -function estimate = estimate_pinger_position(poses, delta_s, previous_estimate, hydrophone_base_vector) - - %transform so that the hydrophone base vector is parallel to the y axis - characteristic_distance = norm(hydrophone_base_vector); - - %from transform to positive y - unit_base_vector = hydrophone_base_vector / norm(hydrophone_base_vector); - cos_transform = sum(unit_base_vector .* [0,1,0]); - cross_transform = cross(unit_base_vector, [0,1,0]); - skew_sym_cross = [0, -cross_transform(3), cross_transform(2); - cross_transform(3), 0, -cross_transform(1); - -cross_transform(2), cross_transform(1),0]; - base_vector_rotm = eye(3) + skew_sym_cross + skew_sym_cross^2 / (1 + cos_transform); - - num_points = length(poses(1,:)); - - points = zeros(3, num_points); - - %weght function - weight_func = @(n) 1; - weights = zeros(1, num_points); - previous_weight = .01; - - - for counter = 1:num_points - - %from ([0,0,0,0,0,0,0]) to pose - rotm = quat2rotm(poses(4:7, counter)'); - transform = poses(1:3, counter); - - %transform so the origin is at ([0,0,0,0,0,0,0]) - previous_point_transformed = base_vector_rotm * rotm' * (previous_estimate - transform) - - [closest_point, dist] = calculate_distance_from_solve(delta_s(counter), characteristic_distance, previous_point_transformed); - closest_point = closest_point'; - points(:,counter) = (rotm * base_vector_rotm' * closest_point) + transform; - - weights(counter) = weight_func(dist); - end - - %find the center "mean" of all of the points - center_point = sum(points, 2) / num_points; - - %calculate the standard deviation (standard distance from the center) - sum_of_distance2 = 0; - distances_from_center = zeros(num_points, 1); - for counter = 1:num_points - distances_from_center(counter) = norm(center_point - points(:,counter)); - sum_of_distance2 = sum_of_distance2 + distances_from_center(counter)^2; - end - std_dev_from_center = sqrt(sum_of_distance2 / (num_points - 1)); - - %if greater than 1 std dev - do not include in estimate - weights = weights .* (std_dev_from_center > distances_from_center)'; - - % %plot points - % figure; - % plot3(points(1,:), points(2,:), points(3,:), "b*") - - - %find center of all points - estimate = (points * weights' + (previous_estimate) * previous_weight) / (sum(weights) + previous_weight); - -end \ No newline at end of file diff --git a/riptide_acoustics/src/UndeterminantMethod/estimate_pinger_position.m b/riptide_acoustics/src/UndeterminantMethod/estimate_pinger_position.m deleted file mode 100644 index b4573d3..0000000 --- a/riptide_acoustics/src/UndeterminantMethod/estimate_pinger_position.m +++ /dev/null @@ -1,64 +0,0 @@ -function estimate = estimate_pinger_position(poses, delta_s, previous_estimate, hydrophone_base_vector) - - %transform so that the hydrophone base vector is parallel to the y axis - characteristic_distance = norm(hydrophone_base_vector); - - %from transform to positive y - unit_base_vector = hydrophone_base_vector / norm(hydrophone_base_vector); - cos_transform = sum(unit_base_vector .* [0;1;0]); - cross_transform = cross(unit_base_vector, [0;1;0]); - skew_sym_cross = [0, -cross_transform(3), cross_transform(2); - cross_transform(3), 0, -cross_transform(1); - -cross_transform(2), cross_transform(1),0]; - base_vector_rotm = eye(3) + skew_sym_cross + skew_sym_cross^2 / (1 + cos_transform); - - num_points = length(poses(1,:)); - - points = zeros(3, num_points); - - %weght function - weight_func = @(n) 1; - weights = zeros(1, num_points); - previous_weight = .01; - - for counter = 1:num_points - - %from ([0,0,0,0,0,0,0]) to pose - rotm = quat2rotm(poses(4:7, counter)'); - transform = poses(1:3, counter); - - %transform so the origin is at ([0,0,0,0,0,0,0]) - previous_point_transformed = base_vector_rotm * rotm' * (previous_estimate - transform); - - [closest_point, dist] = calculate_distance_from_solve(delta_s(counter), characteristic_distance, previous_point_transformed); - closest_point = closest_point'; - points(:,counter) = (rotm * base_vector_rotm' * closest_point) + transform; - - weights(counter) = weight_func(dist); - end - - %find the center "mean" of all of the points - center_point = sum(points, 2) / num_points; - - %calculate the standard deviation (standard distance from the center) - sum_of_distance2 = 0; - distances_from_center = zeros(num_points, 1); - for counter = 1:num_points - distances_from_center(counter) = norm(center_point - points(:,counter)); - sum_of_distance2 = sum_of_distance2 + distances_from_center(counter)^2; - end - std_dev_from_center = sqrt(sum_of_distance2 / (num_points - 1)); - - %if greater than 1 std dev - do not include in estimate - % weights = weights .* (std_dev_from_center > distances_from_center)'; - - %find center of all points - estimate = (points * weights' + (previous_estimate) * previous_weight) / (sum(weights) + previous_weight); - - % %plot points - figure(1); - plot3(points(1,:), points(2,:), points(3,:), "b*", previous_estimate(1), previous_estimate(2), previous_estimate(3), "xr", estimate(1), estimate(2), estimate(3), "gx") - - - -end \ No newline at end of file diff --git a/riptide_acoustics/src/UndeterminantMethod/test1.csv b/riptide_acoustics/src/UndeterminantMethod/test1.csv deleted file mode 100644 index 2dc4bee..0000000 --- a/riptide_acoustics/src/UndeterminantMethod/test1.csv +++ /dev/null @@ -1,36 +0,0 @@ -number,time,delta_t,pos x 1,pos y 1,pos z 1,or w 1,or x 1,or y 1, or z 1,pos x 2,pos y 2,pos z 2,or w 2,or x 2,or y 2, or z 2 -1,1730602076.9605615,-0.00023910690471704902,0.17477051774202157,0.35526724639887375,0.9858247081319208,0.9999991883481831,-0.00030941976696492756,-0.00020121886321131685,0.0012194561705773605,0.17477051774202157,0.35526724639887375,0.9858247081319208,0.9999991883481831,-0.00030941976696492756,-0.00020121886321131685,0.0012194561705773605 -2,1730602078.9667318,-0.00023958979555123308,0.17345860967021187,0.3540682687286766,0.9809740862699532,0.9999987397548187,0.0004230851023879771,-0.000623163570459755,0.001397553195792181,0.17345860967021187,0.3540682687286766,0.9809740862699532,0.9999987397548187,0.0004230851023879771,-0.000623163570459755,0.001397553195792181 -3,1730602080.966533,-0.0002388495874544354,0.17483717066185328,0.355927146343053,0.9802572441585551,0.9999998968037208,-0.0004416842331025506,-0.00010073782498966525,3.4051086857910075e-05,0.17483717066185328,0.355927146343053,0.9802572441585551,0.9999998968037208,-0.0004416842331025506,-0.00010073782498966525,3.4051086857910075e-05 -4,1730602082.9712355,-0.00023941257498223103,0.17516954811031324,0.3541027266861226,0.9837919382535902,0.9999992478076677,0.00024717197072335225,3.100796699418819e-05,0.0012009698671241584,0.17516954811031324,0.3541027266861226,0.9837919382535902,0.9999992478076677,0.00024717197072335225,3.100796699418819e-05,0.0012009698671241584 -5,1730602084.9746716,-0.00023863820708125842,0.1750414084933162,0.35435604910995305,0.9840576722390639,0.9999999636524994,0.0001629940342028921,0.00010198172933705589,0.00018901764866265314,0.1750414084933162,0.35435604910995305,0.9840576722390639,0.9999999636524994,0.0001629940342028921,0.00010198172933705589,0.00018901764866265314 -6,1730602086.9681497,-0.00023915745126083633,0.17411772135577563,0.35464822014067027,0.9812986791837663,0.9999998353590444,-0.0001276187939803107,-0.0004907159973617589,0.0002686878069568441,0.17411772135577563,0.35464822014067027,0.9812986791837663,0.9999998353590444,-0.0001276187939803107,-0.0004907159973617589,0.0002686878069568441 -7,1730602088.9683607,-0.000238833529583436,0.17528023664455078,0.355235584416919,0.9831253755052829,0.9999999444069843,-0.00031540040670268826,-2.1628301252419562e-05,0.00010602277358149713,0.17528023664455078,0.355235584416919,0.9831253755052829,0.9999999444069843,-0.00031540040670268826,-2.1628301252419562e-05,0.00010602277358149713 -8,1730602090.9690344,-0.00023941608178562894,0.17526646836184295,0.35504694742199533,0.9849703278747103,0.9999992200511325,-0.0001414348480131425,-1.745818256528908e-05,0.0012408015644250602,0.17526646836184295,0.35504694742199533,0.9849703278747103,0.9999992200511325,-0.0001414348480131425,-1.745818256528908e-05,0.0012408015644250602 -9,1730602092.9703708,-0.0002394951921362415,0.17535911628723994,0.35459707221606707,0.9814910289161214,0.9999995501224405,0.0003087854734764475,0.0003472709585601525,0.0008269276445910311,0.17535911628723994,0.35459707221606707,0.9814910289161214,0.9999995501224405,0.0003087854734764475,0.0003472709585601525,0.0008269276445910311 -10,1730602094.9712648,-0.00023878828913218028,0.17490437564955694,0.3550755843765803,0.9805177758772854,0.9999998242940161,0.00012321582331858337,0.000125810530991369,0.0005660402000451104,0.17490437564955694,0.3550755843765803,0.9805177758772854,0.9999998242940161,0.00012321582331858337,0.000125810530991369,0.0005660402000451104 -11,1730602096.9651887,-0.0002389983471739289,0.17449848606204946,0.35578668260527746,0.9835136365982081,0.9999999452558151,-0.00026026695847683305,-0.00018312050417430666,-9.064412859775001e-05,0.17449848606204946,0.35578668260527746,0.9835136365982081,0.9999999452558151,-0.00026026695847683305,-0.00018312050417430666,-9.064412859775001e-05 -12,1730602098.9677374,-0.00023861968000385019,0.17523672558745987,0.3542890610484866,0.9872715425345799,0.9999997925733386,0.0003680067308857633,0.00019624886287545254,0.0004908265574917519,0.17523672558745987,0.3542890610484866,0.9872715425345799,0.9999997925733386,0.0003680067308857633,0.00019624886287545254,0.0004908265574917519 -13,1730602100.9710946,-0.00023890035560134018,0.17521591305199868,0.3546581917782677,0.9873188151868755,0.999999964264311,0.00016065918217922096,9.917171700903727e-06,-0.00021345175959599087,0.17521591305199868,0.3546581917782677,0.9873188151868755,0.999999964264311,0.00016065918217922096,9.917171700903727e-06,-0.00021345175959599087 -14,1730602102.9674559,-0.0002387520010607984,0.17507985614059032,0.3552776755797112,0.9838027042421368,0.9999999812267832,4.896899347367777e-05,0.00018236369435959004,4.34965953875846e-05,0.17507985614059032,0.3552776755797112,0.9838027042421368,0.9999999812267832,4.896899347367777e-05,0.00018236369435959004,4.34965953875846e-05 -15,1730602104.970081,-0.00023947379821559435,0.1737173275434046,0.3547664759948673,0.9850441483874908,0.9999988684000727,0.00020712660609594123,-0.0005733659981797373,0.0013753358045998938,0.1737173275434046,0.3547664759948673,0.9850441483874908,0.9999988684000727,0.00020712660609594123,-0.0005733659981797373,0.0013753358045998938 -16,1730602106.9679987,-0.00023852933988669989,0.17459786558139967,0.3549929292112845,0.9884943087971568,0.9999999672179588,2.8991724937339898e-05,-9.423100851987381e-05,-0.00023631351690418302,0.17459786558139967,0.3549929292112845,0.9884943087971568,0.9999999672179588,2.8991724937339898e-05,-9.423100851987381e-05,-0.00023631351690418302 -17,1730602108.9659603,-0.00023950819189385826,0.17431947329991643,0.35415530579555077,0.9845299433797811,0.9999993917169818,0.0001590372031228914,-0.00017590365806280728,0.0010771864915208517,0.17431947329991643,0.35415530579555077,0.9845299433797811,0.9999993917169818,0.0001590372031228914,-0.00017590365806280728,0.0010771864915208517 -18,1730602110.9644763,-0.00023966226061829322,0.17447077236287376,0.3559662721175278,0.9805980835420093,0.9999994280550956,-0.0005657306726253614,-0.00021435588225217816,0.0008819806367582533,0.17447077236287376,0.3559662721175278,0.9805980835420093,0.9999994280550956,-0.0005657306726253614,-0.00021435588225217816,0.0008819806367582533 -19,1730602112.971374,-0.0002446875439173555,0.0660843333711714,0.35505035955396336,0.9858507478343906,0.9999661777305295,7.720115837767317e-05,0.008213709164312412,0.000415230948086814,0.0660843333711714,0.35505035955396336,0.9858507478343906,0.9999661777305295,7.720115837767317e-05,0.008213709164312412,0.000415230948086814 -20,1730602114.9733787,-0.0002791140166842917,-0.4470946479847955,0.3540909917006062,1.0447844146760172,0.9989695117241044,0.00046144519347156526,0.04538345705544062,-0.00020866177102560747,-0.4470946479847955,0.3540909917006062,1.0447844146760172,0.9989695117241044,0.00046144519347156526,0.04538345705544062,-0.00020866177102560747 -21,1730602116.9665315,-0.0003162599332602687,-0.970256509581909,0.35373680136875885,1.0737710531423812,0.9992769090275209,-0.0002830823545975063,0.038009351338723546,0.0009317508217785118,-0.970256509581909,0.35373680136875885,1.0737710531423812,0.9992769090275209,-0.0002830823545975063,0.038009351338723546,0.0009317508217785118 -22,1730602118.9730878,-0.00033399703872171924,-1.207711916673252,0.44408844012201143,0.9828208566831272,0.9999936115663932,0.00317344884574182,-0.0016450072348369801,1.4959623928805418e-07,-1.207711916673252,0.44408844012201143,0.9828208566831272,0.9999936115663932,0.00317344884574182,-0.0016450072348369801,1.4959623928805418e-07 -23,1730602120.9634225,-0.0003691215677558217,-1.2853769922182812,1.0229603132549545,1.0009177405545453,0.999653578806899,0.025418360504437628,-0.006801685283811873,0.000605314071749189,-1.2853769922182812,1.0229603132549545,1.0009177405545453,0.999653578806899,0.025418360504437628,-0.006801685283811873,0.000605314071749189 -24,1730602122.9660347,-0.0003943946164293711,-1.3121946199196712,1.9234540408930478,1.1753650518091148,0.9982185728088924,0.059647213320038986,-0.001089718128976824,0.0008386637771986282,-1.3121946199196712,1.9234540408930478,1.1753650518091148,0.9982185728088924,0.059647213320038986,-0.001089718128976824,0.0008386637771986282 -25,1730602124.9647522,-0.0004037945431972722,-1.3458575233570151,2.406868512294075,1.212384835090746,0.9985968204017377,0.052918589400108684,0.0019773293704657934,0.0003214778425376011,-1.3458575233570151,2.406868512294075,1.212384835090746,0.9985968204017377,0.052918589400108684,0.0019773293704657934,0.0003214778425376011 -26,1730602126.967966,-0.00040708906153200884,-1.3584983879785515,2.573448252179173,1.1412939686450956,0.9994247287615419,0.03380745614661589,0.0020477552198020408,0.0017533246627500847,-1.3584983879785515,2.573448252179173,1.1412939686450956,0.9994247287615419,0.03380745614661589,0.0020477552198020408,0.0017533246627500847 -27,1730602128.971708,-0.00040806074219133727,-1.357064939791769,2.6647756786761647,1.08430485058301,0.9997822299691196,0.02077565107880742,0.001606737658886589,0.0011328522946685569,-1.357064939791769,2.6647756786761647,1.08430485058301,0.9997822299691196,0.02077565107880742,0.001606737658886589,0.0011328522946685569 -28,1730602130.9707863,-0.0004084979416342751,-1.3594705378747984,2.67836055534775,1.0353544663475671,0.9999404779925178,0.010781252924496729,0.000997760847145472,0.001345188003558663,-1.3594705378747984,2.67836055534775,1.0353544663475671,0.9999404779925178,0.010781252924496729,0.000997760847145472,0.001345188003558663 -29,1730602132.9623039,-0.0004083861324080657,-1.361951045196252,2.6906735546043663,1.0052461740355498,0.9999889658369797,0.004368917254852986,0.0004098944772908693,0.0016771263594692598,-1.361951045196252,2.6906735546043663,1.0052461740355498,0.9999889658369797,0.004368917254852986,0.0004098944772908693,0.0016771263594692598 -30,1730602134.971606,-0.0004082316128764602,-1.3577426907942822,2.700422672135693,0.9937708072419184,0.999998474271318,0.0016746051779782476,-0.00023254585456555367,0.00043940295801199545,-1.3577426907942822,2.700422672135693,0.9937708072419184,0.999998474271318,0.0016746051779782476,-0.00023254585456555367,0.00043940295801199545 -31,1730602136.9728036,-0.00040842323525591576,-1.3607768526676776,2.701118891804274,0.9867706461716464,0.9999992907387164,0.00029356118045768993,-6.184035214498375e-05,0.0011526142756239455,-1.3607768526676776,2.701118891804274,0.9867706461716464,0.9999992907387164,0.00029356118045768993,-6.184035214498375e-05,0.0011526142756239455 -32,1730602138.9738753,-0.0004080408535634715,-1.357350547391125,2.705735956868999,0.9784954488826209,0.9999996625972498,-0.0007154139937707934,-0.00010539908144492453,0.0003897168686109271,-1.357350547391125,2.705735956868999,0.9784954488826209,0.9999996625972498,-0.0007154139937707934,-0.00010539908144492453,0.0003897168686109271 -33,1730602140.9710107,-0.000408351601845452,-1.3581793011583123,2.7054241450450336,0.9759648420020786,0.9999993148573082,-0.0009650002900233382,-0.00015671634685985163,0.0006438162324049681,-1.3581793011583123,2.7054241450450336,0.9759648420020786,0.9999993148573082,-0.0009650002900233382,-0.00015671634685985163,0.0006438162324049681 -34,1730602142.9670005,-0.00040799146180058976,-1.3566049885819054,2.70583219114272,0.9772227938453851,0.999999611225209,-0.0007839807623736368,-0.0002887316346915098,0.00028205963608478595,-1.3566049885819054,2.70583219114272,0.9772227938453851,0.999999611225209,-0.0007839807623736368,-0.0002887316346915098,0.00028205963608478595 -35,1730602144.9683554,-0.000408348066079542,-1.3599165351918545,2.7027164519321696,0.9827807870870706,0.9999981139629581,-0.001220450448648613,0.0006018994470167279,0.0013857446679392607,-1.3599165351918545,2.7027164519321696,0.9827807870870706,0.9999981139629581,-0.001220450448648613,0.0006018994470167279,0.0013857446679392607 diff --git a/riptide_acoustics/src/UndeterminantMethod/test3.csv b/riptide_acoustics/src/UndeterminantMethod/test3.csv deleted file mode 100644 index df4dcc3..0000000 --- a/riptide_acoustics/src/UndeterminantMethod/test3.csv +++ /dev/null @@ -1,20 +0,0 @@ -number,time,delta_t,pos x 1,pos y 1,pos z 1,or w 1,or x 1,or y 1, or z 1,pos x 2,pos y 2,pos z 2,or w 2,or x 2,or y 2, or z 2 -1,1730606579.9802365,-0.0001253174677029977,-0.4163459671747822,0.5968035655233543,-0.13009170754716787,0.9996797023652668,0.006461941330008329,-0.023450988273185154,0.006984779320058735,-0.4156322472233951,1.2468224786111097,-0.12143704221311552,0.9996797023652668,0.006461941330008329,-0.023450988273185154,0.006984779320058735 -2,1730606581.9776223,-0.0001251644840117147,-0.4179850986623722,0.5967806670908133,-0.13012625665880345,0.9996800541537193,0.006533187693569448,-0.023409361541509927,0.007007751279572117,-0.4173030218089022,1.2467983900640658,-0.12138013798623497,0.9996800541537193,0.006533187693569448,-0.023409361541509927,0.007007751279572117 -3,1730606583.9795377,-0.00012521914510405066,-0.419616432675359,0.5967542522988211,-0.13032485875904767,0.9996780936847931,0.006813940737221492,-0.02340461501514544,0.007035852070671016,-0.41897936255841783,1.2467670215857867,-0.1212147596620875,0.9996780936847931,0.006813940737221492,-0.02340461501514544,0.007035852070671016 -4,1730606585.9722495,-0.00012510701653001928,-0.42124948425873543,0.5967287047536821,-0.13048799891217536,0.9996762942223656,0.007044266792109477,-0.023405168586172812,0.007062801048650141,-0.42065444110051303,1.2467372612949499,-0.12107936740820008,0.9996762942223656,0.007044266792109477,-0.023405168586172812,0.007062801048650141 -5,1730606587.972156,-0.00012510791402371317,-0.4229104169024931,0.5967186313880065,-0.13015997228803516,0.9996761234004469,0.006507179903331045,-0.02356441523833557,0.007072711448619975,-0.42231340924855504,1.2467368964166898,-0.12144798566263243,0.9996761234004469,0.006507179903331045,-0.02356441523833557,0.007072711448619975 -6,1730606589.9793305,-0.00012573835113941368,-0.3930678306723455,0.6038586364495109,-0.40652970499988694,0.9673025514229892,-0.0011557015035376628,0.25349261133366585,0.00811999777396901,-0.39494601270290997,1.253922416092197,-0.41021139332525924,0.9673025514229892,-0.0011557015035376628,0.25349261133366585,0.00811999777396901 -7,1730606591.9747446,-0.00014077767268860195,-0.27405960492570663,0.6105285826999282,-1.0007874319387178,0.9975713120363877,-0.0035568941513799248,0.06956153817234788,0.00013532587610615377,-0.2646535277559122,1.2605098634317726,-1.0067757881796024,0.9975713120363877,-0.0035568941513799248,0.06956153817234788,0.00013532587610615377 -8,1730606593.9788249,-0.00013911462769748914,-0.2032590714666731,0.6031998879739695,-0.9892140137723926,0.9998485061214814,-0.003932283808597593,0.016581788756566757,0.0035420661093946383,-0.19795357545544573,1.2532330026633849,-0.9945807184645078,0.9998485061214814,-0.003932283808597593,0.016581788756566757,0.0035420661093946383 -9,1730606595.9784057,-0.00015421170787955832,-0.16205597469502714,0.6031119269128142,-1.0316036576784025,0.999908936205534,-0.004634736641643484,0.0010544108170844532,-0.012630389162994879,-0.13564752976728106,1.2526239346262584,-1.037665495068887,0.999908936205534,-0.004634736641643484,0.0010544108170844532,-0.012630389162994879 -10,1730606597.9731364,-0.0001507145668751925,-0.012256016907346567,0.6049394091183984,-1.009169753056389,0.9999446132882163,-0.0038816473466427898,-0.009662977322558004,0.001526446533536237,-0.004193440804658083,1.2549480701308915,-1.0140416598016482,0.9999446132882163,-0.0038816473466427898,-0.009662977322558004,0.001526446533536237 -11,1730606599.97842,-0.00017441549370768663,0.5769791722630976,0.6077721500419295,-0.9892138874802936,0.9987557956860734,-0.0040956934980362,-0.04958145938979326,0.0034299800330018144,0.5827403359764699,1.257807624391154,-0.9937626323892503,0.9987557956860734,-0.0040956934980362,-0.04958145938979326,0.0034299800330018144 -12,1730606601.9789667,-0.0002855277072592073,1.4389062815266378,0.3432351939532685,-0.9827763774762791,0.9965118139182854,-0.02139823463545074,-0.08022211484833161,-0.008410265455735734,1.4619029577988203,0.9924147054945871,-1.0080175081048357,0.9965118139182854,-0.02139823463545074,-0.08022211484833161,-0.008410265455735734 -13,1730606603.9767227,-0.00039156238577056033,2.032189999942822,-0.3366907448514985,-0.9506967007037342,0.9978820143025261,-0.05010588412357974,-0.041475041352813435,-0.0008407452110572553,2.045947822807668,0.3100693403111179,-1.0148224720477494,0.9978820143025261,-0.05010588412357974,-0.041475041352813435,-0.0008407452110572553 -14,1730606605.9771225,-0.0004091697255268721,2.1353196690891325,-0.5673459636800655,-0.9485581375367372,0.9990499804463349,-0.03424341252172918,0.023798677076993607,-0.0126549689225031,2.1606815366830956,0.08065229103107868,-0.9938906596754523,0.9990499804463349,-0.03424341252172918,0.023798677076993607,-0.0126549689225031 -15,1730606607.9814677,-0.00041205963343828007,2.191563869763939,-0.6278439139769659,-0.9733444412757101,0.999797079141238,-0.018697379318775544,0.00702834225117261,-0.002609780155336798,2.2047839362870842,0.021637948950278707,-0.9978095096320301,0.999797079141238,-0.018697379318775544,0.00702834225117261,-0.002609780155336798 -16,1730606609.9771278,-0.00041278567022787243,2.2173207543314213,-0.8365530321600158,-0.9991215638073174,0.9998072260608244,-0.018224080069912742,-6.379096125897255e-05,0.007306815508710406,2.217824168721407,-0.18690805941193672,-1.022810294400148,0.9998072260608244,-0.018224080069912742,-6.379096125897255e-05,0.007306815508710406 -17,1730606611.9737067,-0.0004211143125894393,2.2013579542241986,-1.601488981622902,-0.9602122637260162,0.9988359607575722,-0.04802916479030291,-0.000957745330944172,-0.004359535631761697,2.2170781546380525,-0.954598698935752,-1.0225487495420074,0.9988359607575722,-0.04802916479030291,-0.000957745330944172,-0.004359535631761697 -18,1730606613.9787362,-0.00042406877741856756,2.196972526312652,-2.0617214901085608,-0.9580097535959066,0.9989405469254338,-0.04589544413627644,-0.0011434420389472718,-0.003175603273823597,2.211164431421146,-1.414535304433584,-1.0175801397023123,0.9989405469254338,-0.04589544413627644,-0.0011434420389472718,-0.003175603273823597 -19,1730606615.9805744,-0.00042650538273265415,2.210173591516032,-2.1371121601833827,-0.9681404530568304,0.9995345953228278,-0.0285877577228314,-0.006436451071234885,-0.008479679169586076,2.231428982167184,-1.4884339090686212,-1.0050827715948043,0.9995345953228278,-0.0285877577228314,-0.006436451071234885,-0.008479679169586076 diff --git a/riptide_acoustics/src/UndeterminantMethod/untitled2.asv b/riptide_acoustics/src/UndeterminantMethod/untitled2.asv deleted file mode 100644 index 40bcdc6..0000000 --- a/riptide_acoustics/src/UndeterminantMethod/untitled2.asv +++ /dev/null @@ -1,46 +0,0 @@ -close all; -clear; -clear global; - -data = readmatrix("test3.csv"); - -figure_count = 0; - -num_points = 17; -points = zeros(3, num_points); - -previous_estimate = [3; 2; -1]; - -%angle from correct -poses = data(1:num_points, 4:10)'; -delta_s = data(1:num_points, 3)' * 1500; - -poses = [ -0.1654 -0.0052 0.4661 0.7436 0.6187 0.0037 -0.7318 -1.0728 -1.1343 -1.1684 -1.1649 -1.0534 -0.5775 -0.3561 -0.2073 - 0.2949 0.2951 0.2949 0.2953 0.2943 0.2950 0.2949 0.2947 0.4725 0.6550 0.7949 0.7661 0.7213 0.6777 0.6443 - -0.9838 -0.9904 -0.9953 -0.9834 -0.9783 -0.9680 -0.9589 -0.9779 -0.9800 -0.9807 -0.9881 -0.9903 -0.9986 -0.9937 -0.9825 - 1.0000 0.9999 0.9995 1.0000 0.9999 0.9986 0.9969 0.9998 0.9999 0.9999 1.0000 1.0000 0.9994 0.9999 1.0000 - 0.0025 0.0024 0.0026 0.0024 0.0025 0.0025 0.0021 0.0024 0.0085 0.0113 0.0024 -0.0005 -0.0030 -0.0034 -0.0012 - -0.0002 -0.0102 -0.0325 0.0062 0.0157 0.0537 0.0787 0.0179 0.0098 0.0038 -0.0023 -0.0080 -0.0340 -0.0144 0.0077 - 0.0001 0.0002 -0.0001 -0.0007 0.0007 -0.0004 0.0003 0.0004 -0.0002 -0.0004 0.0004 -0.0003 0.0006 0.0009 0.0011] - -delta_s = [ -0.3577 - -0.3703 - -0.4121 - -0.4407 - -0.4273 - -0.3727 - -0.3183 - -0.2975 - -0.2748 - -0.2505 - -0.2323 - -0.2420 - -0.2728 - -0.2932 - -0.3080] - -for i = 1:100 - previous_estimate = estimate_pinger_position(poses, delta_s, previous_estimate, [-.01; .65; 0]) - %previous_estimate(3) = -1; - disp(i); -end diff --git a/riptide_acoustics/src/UndeterminantMethod/untitled2.m b/riptide_acoustics/src/UndeterminantMethod/untitled2.m deleted file mode 100644 index fa99326..0000000 --- a/riptide_acoustics/src/UndeterminantMethod/untitled2.m +++ /dev/null @@ -1,46 +0,0 @@ -close all; -clear; -clear global; - -data = readmatrix("test3.csv"); - -figure_count = 0; - -num_points = 5; -points = zeros(3, num_points); - -previous_estimate = [4.5; 2.1; -1.1]; - -%angle from correct -poses = data(1:num_points, 4:10)'; -delta_s = data(1:num_points, 3)' * 1500; - -poses = [ -0.1654 -0.0052 0.4661 0.7436 0.6187 0.0037 -0.7318 -1.0728 -1.1343 -1.1684 -1.1649 -1.0534 -0.5775 -0.3561 -0.2073 - 0.2949 0.2951 0.2949 0.2953 0.2943 0.2950 0.2949 0.2947 0.4725 0.6550 0.7949 0.7661 0.7213 0.6777 0.6443 - -0.9838 -0.9904 -0.9953 -0.9834 -0.9783 -0.9680 -0.9589 -0.9779 -0.9800 -0.9807 -0.9881 -0.9903 -0.9986 -0.9937 -0.9825 - 1.0000 0.9999 0.9995 1.0000 0.9999 0.9986 0.9969 0.9998 0.9999 0.9999 1.0000 1.0000 0.9994 0.9999 1.0000 - 0.0025 0.0024 0.0026 0.05024 0.0025 0.0025 0.0021 0.0024 0.0085 0.0113 0.0024 -0.0005 -0.0030 -0.0034 -0.0012 - -0.0002 -0.0102 -0.0325 0.0062 0.0157 0.0537 0.0787 0.0179 0.0098 0.0038 -0.0023 -0.0080 -0.0340 -0.0144 0.0077 - 0.0001 0.0002 -0.0001 -0.0007 0.0007 -0.0004 0.0003 0.0004 -0.0002 -0.0004 0.0004 -0.0003 0.0006 0.0009 0.0011] - -delta_s = [ -0.3577 - -0.3703 - -0.4121 - -0.4407 - -0.4273 - -0.3727 - -0.3183 - -0.2975 - -0.2748 - -0.2505 - -0.2323 - -0.2420 - -0.2728 - -0.2932 - -0.3080] - -for i = 1:1000 - previous_estimate = estimate_pinger_position(poses, delta_s, previous_estimate, [-.01; -.65; 0]) - %previous_estimate(3) = -1; - disp(i); -end diff --git a/riptide_acoustics/src/matlab/AcousticsNode.asv b/riptide_acoustics/src/matlab/AcousticsNode.asv deleted file mode 100644 index 59c1b32..0000000 --- a/riptide_acoustics/src/matlab/AcousticsNode.asv +++ /dev/null @@ -1,120 +0,0 @@ -clear; -clear global; - -%read from config -speed_of_sound = 1500; -pinger_depth = -1.0; - -%initialize node -global node; -node = ros2node("/acoustics_node"); - -%intialize tf2 -global tftree; -tftree = ros2tf(node); - -%initialize transform array -global measurements; - -%get acoustic pod transform -global port_to_origin_transform -global port_to_startboard_transform - -global pinger_pose -pinger_pose = [3.0,2.0,-1.0,0.0,0.0,0.0]; - -transforms_recieved = false; -while(transforms_recieved == false) - %loop until transforms become available - - try - port_to_origin_transform = getTransform(tftree, "talos/origin", "talos/acoustics_port_link"); - port_to_startboard_transform = getTransform(tftree, "talos/acoustics_starboard_link", "talos/acoustics_port_link"); - - transforms_recieved = true; - catch - pause(1); - disp("Waiting for acoustic transforms to become available!"); - end -end - - -%initialize publishers and subscribers -pub = ros2publisher(node, "/test_matlab", "std_msgs/UInt8"); - -delta_t_sub = ros2subscriber(node, "/talos/acoustics/delta_t", @delta_t_callback); - -x = 0; -while(x < 1) - if(length(measurements) > 2) - %select the two measurements to calculate with - m1 = measurements(end - 1); - m2 = measurements(end); - - %formatting... - p2o_translation = [port_to_origin_transform.transform.translation.x; port_to_origin_transform.transform.translation.y; port_to_origin_transform.transform.translation.z]; - p2o_quat = [port_to_origin_transform.transform.rotation.w; port_to_origin_transform.transform.rotation.x; port_to_origin_transform.transform.rotation.y; port_to_origin_transform.transform.rotation.z]; - p2o_rotation = quat2rotm(p2o_quat'); - - o2m1_translation = [m1.auv_origin.transform.translation.x; m1.auv_origin.transform.translation.y; m1.auv_origin.transform.translation.z]; - o2m1_quat = [m1.auv_origin.transform.rotation.w; m1.auv_origin.transform.rotation.x; m1.auv_origin.transform.rotation.y; m1.auv_origin.transform.rotation.z]; - o2m1_rotation = quat2rotm(o2m1_quat'); - - o2m2_translation = [m2.auv_origin.transform.translation.x; m2.auv_origin.transform.translation.y; m2.auv_origin.transform.translation.z]; - o2m2_quat = [m2.auv_origin.transform.rotation.w; m2.auv_origin.transform.rotation.x; m2.auv_origin.transform.rotation.y; m2.auv_origin.transform.rotation.z]; - o2m2_rotation = quat2rotm(o2m1_quat'); - - p2s_translation = [port_to_startboard_transform.transform.translation.x; port_to_startboard_transform.transform.translation.y; port_to_startboard_transform.transform.translation.z]; - - %transform each hydrophone into origin frame for each measurement - m1t = o2m1_rotation * p2o_translation + o2m1_translation - m1r = transpose(quatmultiply(p2o_quat', o2m1_quat')) - m2t = o2m2_rotation * p2o_translation + o2m2_translation - m2r = transpose(quatmultiply(p2o_quat', o2m2_quat')) - - [solve_x, solve_y] = solve_two_pulse_system_depth(m1.delta_t/speed_of_sound, m2.delta_t/speed_of_sound,[m1t;m1r],[m2t;m2r], p2s_translation, 1, [100,100], pinger_depth) - - pinger_location = o2m1_rotation * ([solve_x; solve_y; pinger_depth] + p2s_translation) + o2m1_translation - - timing_estimate_1 = GetTimingDIfference([m1t;m1r], p2s_translation, pinger_pose, speed_of_sound) - back_calc1 = GetTimingDIfference([m1t;m1r], p2s_translation, pinger_location, speed_of_sound) - m1.delta_t - - timing_estimate_2 = GetTimingDIfference([m2t;m2r], p2s_translation, pinger_pose, speed_of_sound) - back_calc2 = GetTimingDIfference([m2t;m2r], p2s_translation, pinger_location, speed_of_sound) - m2.delta_t - end - -end - - -function delta_t_callback(message) - %introduce global variables - global tftree; - global measurements; - - try - %get the transform at the time of the measurement - port_transform = getTransform(tftree, "map", 'talos/origin'); - - %fill out measurement data struct - measurement = struct(); - measurement.delta_t = message.vector.x; - measurement.frequency = message.vector.y; - measurement.stamp = message.header.stamp; - measurement.auv_origin = port_transform; - - if(length(measurements) == 0) - measurements = [measurement] - - else - measurements(end+1) = measurement; - end - - catch - disp("Failed to get transform") - - tftree.AvailableFrames - end - -end \ No newline at end of file diff --git a/riptide_acoustics/src/matlab/AcousticsNode.m b/riptide_acoustics/src/matlab/AcousticsNode.m deleted file mode 100644 index 9451126..0000000 --- a/riptide_acoustics/src/matlab/AcousticsNode.m +++ /dev/null @@ -1,130 +0,0 @@ -clear; -clear global; - -%read from config -speed_of_sound = 1500; -pinger_depth = -1.0; - -%initialize node -global node; -node = ros2node("/acoustics_node"); - -%intialize tf2 -global tftree; -tftree = ros2tf(node); - -%initialize transform array -global measurements; - -%get acoustic pod transform -global port_to_origin_transform -global port_to_startboard_transform - -global initial_pinger_estimate -initial_pinger_estimate = [3.0,2.0,-1.0,0.0,0.0,0.0]; - -transforms_recieved = false; -while(transforms_recieved == false) - %loop until transforms become available - - try - port_to_origin_transform = getTransform(tftree, "talos/origin", "talos/acoustics_port_link"); - port_to_startboard_transform = getTransform(tftree, "talos/acoustics_starboard_link", "talos/acoustics_port_link"); - - transforms_recieved = true; - catch - pause(1); - disp("Waiting for acoustic transforms to become available!"); - end -end - - -%initialize publishers and subscribers -pub = ros2publisher(node, "/test_matlab", "std_msgs/UInt8"); - -%add subscriber to add to measurements -delta_t_sub = ros2subscriber(node, "/talos/acoustics/delta_t", @delta_t_callback); - -%add rate function to analyze measurements -r = ros2rate(node, 1) -reset(r) - -x = 0 -while(x < 1) - run_analysis(port_to_startboard_transform, port_to_origin_transform, measurements, pinger_depth, initial_pinger_estimate, speed_of_sound); - waitfor(r) -end - -function [] = run_analysis(port_to_startboard_transform, port_to_origin_transform, measurements, pinger_depth, pinger_pose, speed_of_sound) - if(max(size((measurements))) > 5) - %select the two measurements to calculate with - m1 = measurements(5); - m2 = measurements(end); - - %formatting... - p2o_translation = [port_to_origin_transform.transform.translation.x; port_to_origin_transform.transform.translation.y; port_to_origin_transform.transform.translation.z]; - p2o_quat = [port_to_origin_transform.transform.rotation.w; port_to_origin_transform.transform.rotation.x; port_to_origin_transform.transform.rotation.y; port_to_origin_transform.transform.rotation.z]; - p2o_rotation = quat2rotm(p2o_quat'); - - o2m1_translation = [m1.auv_origin.transform.translation.x; m1.auv_origin.transform.translation.y; m1.auv_origin.transform.translation.z]; - o2m1_quat = [m1.auv_origin.transform.rotation.w; m1.auv_origin.transform.rotation.x; m1.auv_origin.transform.rotation.y; m1.auv_origin.transform.rotation.z]; - o2m1_rotation = quat2rotm(o2m1_quat'); - - o2m2_translation = [m2.auv_origin.transform.translation.x; m2.auv_origin.transform.translation.y; m2.auv_origin.transform.translation.z]; - o2m2_quat = [m2.auv_origin.transform.rotation.w; m2.auv_origin.transform.rotation.x; m2.auv_origin.transform.rotation.y; m2.auv_origin.transform.rotation.z]; - o2m2_rotation = quat2rotm(o2m1_quat'); - - p2s_translation = [port_to_startboard_transform.transform.translation.x; port_to_startboard_transform.transform.translation.y; port_to_startboard_transform.transform.translation.z]; - - %transform each hydrophone into origin frame for each measurement - m1t = o2m1_rotation * p2o_translation + o2m1_translation - m1r = transpose(quatmultiply(p2o_quat', o2m1_quat')) - m2t = o2m2_rotation * p2o_translation + o2m2_translation - m2r = transpose(quatmultiply(p2o_quat', o2m2_quat')) - - [solve_x, solve_y] = solve_two_pulse_system_depth(m1.delta_t/speed_of_sound, m2.delta_t/speed_of_sound,[m1t;m1r],[m2t;m2r], p2s_translation, 1, [100,100], pinger_depth) - - pinger_location = o2m1_rotation * ([solve_x; solve_y; pinger_depth] + p2s_translation) + o2m1_translation - - timing_estimate_1 = GetTimingDIfference([m1t;m1r], p2s_translation, pinger_pose, speed_of_sound) - back_calc1 = GetTimingDIfference([m1t;m1r], p2s_translation, pinger_location, speed_of_sound) - m1.delta_t - - timing_estimate_2 = GetTimingDIfference([m2t;m2r], p2s_translation, pinger_pose, speed_of_sound) - back_calc2 = GetTimingDIfference([m2t;m2r], p2s_translation, pinger_location, speed_of_sound) - m2.delta_t - end - -end - - -function delta_t_callback(message) - %introduce global variables - global tftree; - global measurements; - - try - %get the transform at the time of the measurement - port_transform = getTransform(tftree, "map", 'talos/origin'); - - %fill out measurement data struct - measurement = struct(); - measurement.delta_t = message.vector.x; - measurement.frequency = message.vector.y; - measurement.stamp = message.header.stamp; - measurement.auv_origin = port_transform; - - if(length(measurements) == 0) - measurements = [measurement]; - - else - measurements(end+1) = measurement; - end - - catch - disp("Failed to get transform") - - tftree.AvailableFrames - end - -end \ No newline at end of file diff --git a/riptide_acoustics/src/matlab/GetTimingDIfference.asv b/riptide_acoustics/src/matlab/GetTimingDIfference.asv deleted file mode 100644 index af3bdae..0000000 --- a/riptide_acoustics/src/matlab/GetTimingDIfference.asv +++ /dev/null @@ -1,9 +0,0 @@ -function [delta_t] = getTimingDifference(pose_1, hydrophone_base_vector, pinger_pos) - -%rotation matrix -rotm = quat2rotm([pose_1(4), pose_1(5), pose_1(6), pose_1(7)]); -hydrophone2_pos = getTimingDifference(1:3) + rotm * hydrophone_base_vector; - - - -end \ No newline at end of file diff --git a/riptide_acoustics/src/matlab/GetTimingDIfference.m b/riptide_acoustics/src/matlab/GetTimingDIfference.m deleted file mode 100644 index eb7edb0..0000000 --- a/riptide_acoustics/src/matlab/GetTimingDIfference.m +++ /dev/null @@ -1,12 +0,0 @@ -function [delta_t] = GetTimingDifference(pose_1, hydrophone_base_vector, pinger_pos, speed_of_sound) - -%rotation matrix -rotm = quat2rotm([pose_1(4), pose_1(5), pose_1(6), pose_1(7)]); -hydrophone2_pos = pose_1(1:3) + rotm * hydrophone_base_vector; - -time_1 = norm(pose_1(1:3) - pinger_pos) / speed_of_sound; -time_2 = norm(hydrophone2_pos - pinger_pos) / speed_of_sound; - -delta_t = time_2 - time_1; - -end \ No newline at end of file diff --git a/riptide_acoustics/src/matlab/data/test1.csv b/riptide_acoustics/src/matlab/data/test1.csv deleted file mode 100644 index 2dc4bee..0000000 --- a/riptide_acoustics/src/matlab/data/test1.csv +++ /dev/null @@ -1,36 +0,0 @@ -number,time,delta_t,pos x 1,pos y 1,pos z 1,or w 1,or x 1,or y 1, or z 1,pos x 2,pos y 2,pos z 2,or w 2,or x 2,or y 2, or z 2 -1,1730602076.9605615,-0.00023910690471704902,0.17477051774202157,0.35526724639887375,0.9858247081319208,0.9999991883481831,-0.00030941976696492756,-0.00020121886321131685,0.0012194561705773605,0.17477051774202157,0.35526724639887375,0.9858247081319208,0.9999991883481831,-0.00030941976696492756,-0.00020121886321131685,0.0012194561705773605 -2,1730602078.9667318,-0.00023958979555123308,0.17345860967021187,0.3540682687286766,0.9809740862699532,0.9999987397548187,0.0004230851023879771,-0.000623163570459755,0.001397553195792181,0.17345860967021187,0.3540682687286766,0.9809740862699532,0.9999987397548187,0.0004230851023879771,-0.000623163570459755,0.001397553195792181 -3,1730602080.966533,-0.0002388495874544354,0.17483717066185328,0.355927146343053,0.9802572441585551,0.9999998968037208,-0.0004416842331025506,-0.00010073782498966525,3.4051086857910075e-05,0.17483717066185328,0.355927146343053,0.9802572441585551,0.9999998968037208,-0.0004416842331025506,-0.00010073782498966525,3.4051086857910075e-05 -4,1730602082.9712355,-0.00023941257498223103,0.17516954811031324,0.3541027266861226,0.9837919382535902,0.9999992478076677,0.00024717197072335225,3.100796699418819e-05,0.0012009698671241584,0.17516954811031324,0.3541027266861226,0.9837919382535902,0.9999992478076677,0.00024717197072335225,3.100796699418819e-05,0.0012009698671241584 -5,1730602084.9746716,-0.00023863820708125842,0.1750414084933162,0.35435604910995305,0.9840576722390639,0.9999999636524994,0.0001629940342028921,0.00010198172933705589,0.00018901764866265314,0.1750414084933162,0.35435604910995305,0.9840576722390639,0.9999999636524994,0.0001629940342028921,0.00010198172933705589,0.00018901764866265314 -6,1730602086.9681497,-0.00023915745126083633,0.17411772135577563,0.35464822014067027,0.9812986791837663,0.9999998353590444,-0.0001276187939803107,-0.0004907159973617589,0.0002686878069568441,0.17411772135577563,0.35464822014067027,0.9812986791837663,0.9999998353590444,-0.0001276187939803107,-0.0004907159973617589,0.0002686878069568441 -7,1730602088.9683607,-0.000238833529583436,0.17528023664455078,0.355235584416919,0.9831253755052829,0.9999999444069843,-0.00031540040670268826,-2.1628301252419562e-05,0.00010602277358149713,0.17528023664455078,0.355235584416919,0.9831253755052829,0.9999999444069843,-0.00031540040670268826,-2.1628301252419562e-05,0.00010602277358149713 -8,1730602090.9690344,-0.00023941608178562894,0.17526646836184295,0.35504694742199533,0.9849703278747103,0.9999992200511325,-0.0001414348480131425,-1.745818256528908e-05,0.0012408015644250602,0.17526646836184295,0.35504694742199533,0.9849703278747103,0.9999992200511325,-0.0001414348480131425,-1.745818256528908e-05,0.0012408015644250602 -9,1730602092.9703708,-0.0002394951921362415,0.17535911628723994,0.35459707221606707,0.9814910289161214,0.9999995501224405,0.0003087854734764475,0.0003472709585601525,0.0008269276445910311,0.17535911628723994,0.35459707221606707,0.9814910289161214,0.9999995501224405,0.0003087854734764475,0.0003472709585601525,0.0008269276445910311 -10,1730602094.9712648,-0.00023878828913218028,0.17490437564955694,0.3550755843765803,0.9805177758772854,0.9999998242940161,0.00012321582331858337,0.000125810530991369,0.0005660402000451104,0.17490437564955694,0.3550755843765803,0.9805177758772854,0.9999998242940161,0.00012321582331858337,0.000125810530991369,0.0005660402000451104 -11,1730602096.9651887,-0.0002389983471739289,0.17449848606204946,0.35578668260527746,0.9835136365982081,0.9999999452558151,-0.00026026695847683305,-0.00018312050417430666,-9.064412859775001e-05,0.17449848606204946,0.35578668260527746,0.9835136365982081,0.9999999452558151,-0.00026026695847683305,-0.00018312050417430666,-9.064412859775001e-05 -12,1730602098.9677374,-0.00023861968000385019,0.17523672558745987,0.3542890610484866,0.9872715425345799,0.9999997925733386,0.0003680067308857633,0.00019624886287545254,0.0004908265574917519,0.17523672558745987,0.3542890610484866,0.9872715425345799,0.9999997925733386,0.0003680067308857633,0.00019624886287545254,0.0004908265574917519 -13,1730602100.9710946,-0.00023890035560134018,0.17521591305199868,0.3546581917782677,0.9873188151868755,0.999999964264311,0.00016065918217922096,9.917171700903727e-06,-0.00021345175959599087,0.17521591305199868,0.3546581917782677,0.9873188151868755,0.999999964264311,0.00016065918217922096,9.917171700903727e-06,-0.00021345175959599087 -14,1730602102.9674559,-0.0002387520010607984,0.17507985614059032,0.3552776755797112,0.9838027042421368,0.9999999812267832,4.896899347367777e-05,0.00018236369435959004,4.34965953875846e-05,0.17507985614059032,0.3552776755797112,0.9838027042421368,0.9999999812267832,4.896899347367777e-05,0.00018236369435959004,4.34965953875846e-05 -15,1730602104.970081,-0.00023947379821559435,0.1737173275434046,0.3547664759948673,0.9850441483874908,0.9999988684000727,0.00020712660609594123,-0.0005733659981797373,0.0013753358045998938,0.1737173275434046,0.3547664759948673,0.9850441483874908,0.9999988684000727,0.00020712660609594123,-0.0005733659981797373,0.0013753358045998938 -16,1730602106.9679987,-0.00023852933988669989,0.17459786558139967,0.3549929292112845,0.9884943087971568,0.9999999672179588,2.8991724937339898e-05,-9.423100851987381e-05,-0.00023631351690418302,0.17459786558139967,0.3549929292112845,0.9884943087971568,0.9999999672179588,2.8991724937339898e-05,-9.423100851987381e-05,-0.00023631351690418302 -17,1730602108.9659603,-0.00023950819189385826,0.17431947329991643,0.35415530579555077,0.9845299433797811,0.9999993917169818,0.0001590372031228914,-0.00017590365806280728,0.0010771864915208517,0.17431947329991643,0.35415530579555077,0.9845299433797811,0.9999993917169818,0.0001590372031228914,-0.00017590365806280728,0.0010771864915208517 -18,1730602110.9644763,-0.00023966226061829322,0.17447077236287376,0.3559662721175278,0.9805980835420093,0.9999994280550956,-0.0005657306726253614,-0.00021435588225217816,0.0008819806367582533,0.17447077236287376,0.3559662721175278,0.9805980835420093,0.9999994280550956,-0.0005657306726253614,-0.00021435588225217816,0.0008819806367582533 -19,1730602112.971374,-0.0002446875439173555,0.0660843333711714,0.35505035955396336,0.9858507478343906,0.9999661777305295,7.720115837767317e-05,0.008213709164312412,0.000415230948086814,0.0660843333711714,0.35505035955396336,0.9858507478343906,0.9999661777305295,7.720115837767317e-05,0.008213709164312412,0.000415230948086814 -20,1730602114.9733787,-0.0002791140166842917,-0.4470946479847955,0.3540909917006062,1.0447844146760172,0.9989695117241044,0.00046144519347156526,0.04538345705544062,-0.00020866177102560747,-0.4470946479847955,0.3540909917006062,1.0447844146760172,0.9989695117241044,0.00046144519347156526,0.04538345705544062,-0.00020866177102560747 -21,1730602116.9665315,-0.0003162599332602687,-0.970256509581909,0.35373680136875885,1.0737710531423812,0.9992769090275209,-0.0002830823545975063,0.038009351338723546,0.0009317508217785118,-0.970256509581909,0.35373680136875885,1.0737710531423812,0.9992769090275209,-0.0002830823545975063,0.038009351338723546,0.0009317508217785118 -22,1730602118.9730878,-0.00033399703872171924,-1.207711916673252,0.44408844012201143,0.9828208566831272,0.9999936115663932,0.00317344884574182,-0.0016450072348369801,1.4959623928805418e-07,-1.207711916673252,0.44408844012201143,0.9828208566831272,0.9999936115663932,0.00317344884574182,-0.0016450072348369801,1.4959623928805418e-07 -23,1730602120.9634225,-0.0003691215677558217,-1.2853769922182812,1.0229603132549545,1.0009177405545453,0.999653578806899,0.025418360504437628,-0.006801685283811873,0.000605314071749189,-1.2853769922182812,1.0229603132549545,1.0009177405545453,0.999653578806899,0.025418360504437628,-0.006801685283811873,0.000605314071749189 -24,1730602122.9660347,-0.0003943946164293711,-1.3121946199196712,1.9234540408930478,1.1753650518091148,0.9982185728088924,0.059647213320038986,-0.001089718128976824,0.0008386637771986282,-1.3121946199196712,1.9234540408930478,1.1753650518091148,0.9982185728088924,0.059647213320038986,-0.001089718128976824,0.0008386637771986282 -25,1730602124.9647522,-0.0004037945431972722,-1.3458575233570151,2.406868512294075,1.212384835090746,0.9985968204017377,0.052918589400108684,0.0019773293704657934,0.0003214778425376011,-1.3458575233570151,2.406868512294075,1.212384835090746,0.9985968204017377,0.052918589400108684,0.0019773293704657934,0.0003214778425376011 -26,1730602126.967966,-0.00040708906153200884,-1.3584983879785515,2.573448252179173,1.1412939686450956,0.9994247287615419,0.03380745614661589,0.0020477552198020408,0.0017533246627500847,-1.3584983879785515,2.573448252179173,1.1412939686450956,0.9994247287615419,0.03380745614661589,0.0020477552198020408,0.0017533246627500847 -27,1730602128.971708,-0.00040806074219133727,-1.357064939791769,2.6647756786761647,1.08430485058301,0.9997822299691196,0.02077565107880742,0.001606737658886589,0.0011328522946685569,-1.357064939791769,2.6647756786761647,1.08430485058301,0.9997822299691196,0.02077565107880742,0.001606737658886589,0.0011328522946685569 -28,1730602130.9707863,-0.0004084979416342751,-1.3594705378747984,2.67836055534775,1.0353544663475671,0.9999404779925178,0.010781252924496729,0.000997760847145472,0.001345188003558663,-1.3594705378747984,2.67836055534775,1.0353544663475671,0.9999404779925178,0.010781252924496729,0.000997760847145472,0.001345188003558663 -29,1730602132.9623039,-0.0004083861324080657,-1.361951045196252,2.6906735546043663,1.0052461740355498,0.9999889658369797,0.004368917254852986,0.0004098944772908693,0.0016771263594692598,-1.361951045196252,2.6906735546043663,1.0052461740355498,0.9999889658369797,0.004368917254852986,0.0004098944772908693,0.0016771263594692598 -30,1730602134.971606,-0.0004082316128764602,-1.3577426907942822,2.700422672135693,0.9937708072419184,0.999998474271318,0.0016746051779782476,-0.00023254585456555367,0.00043940295801199545,-1.3577426907942822,2.700422672135693,0.9937708072419184,0.999998474271318,0.0016746051779782476,-0.00023254585456555367,0.00043940295801199545 -31,1730602136.9728036,-0.00040842323525591576,-1.3607768526676776,2.701118891804274,0.9867706461716464,0.9999992907387164,0.00029356118045768993,-6.184035214498375e-05,0.0011526142756239455,-1.3607768526676776,2.701118891804274,0.9867706461716464,0.9999992907387164,0.00029356118045768993,-6.184035214498375e-05,0.0011526142756239455 -32,1730602138.9738753,-0.0004080408535634715,-1.357350547391125,2.705735956868999,0.9784954488826209,0.9999996625972498,-0.0007154139937707934,-0.00010539908144492453,0.0003897168686109271,-1.357350547391125,2.705735956868999,0.9784954488826209,0.9999996625972498,-0.0007154139937707934,-0.00010539908144492453,0.0003897168686109271 -33,1730602140.9710107,-0.000408351601845452,-1.3581793011583123,2.7054241450450336,0.9759648420020786,0.9999993148573082,-0.0009650002900233382,-0.00015671634685985163,0.0006438162324049681,-1.3581793011583123,2.7054241450450336,0.9759648420020786,0.9999993148573082,-0.0009650002900233382,-0.00015671634685985163,0.0006438162324049681 -34,1730602142.9670005,-0.00040799146180058976,-1.3566049885819054,2.70583219114272,0.9772227938453851,0.999999611225209,-0.0007839807623736368,-0.0002887316346915098,0.00028205963608478595,-1.3566049885819054,2.70583219114272,0.9772227938453851,0.999999611225209,-0.0007839807623736368,-0.0002887316346915098,0.00028205963608478595 -35,1730602144.9683554,-0.000408348066079542,-1.3599165351918545,2.7027164519321696,0.9827807870870706,0.9999981139629581,-0.001220450448648613,0.0006018994470167279,0.0013857446679392607,-1.3599165351918545,2.7027164519321696,0.9827807870870706,0.9999981139629581,-0.001220450448648613,0.0006018994470167279,0.0013857446679392607 diff --git a/riptide_acoustics/src/matlab/data/test2.csv b/riptide_acoustics/src/matlab/data/test2.csv deleted file mode 100644 index 3d88b8b..0000000 --- a/riptide_acoustics/src/matlab/data/test2.csv +++ /dev/null @@ -1,19 +0,0 @@ -number,time,delta_t,pos x 1,pos y 1,pos z 1,or w 1,or x 1,or y 1, or z 1,pos x 2,pos y 2,pos z 2,or w 2,or x 2,or y 2, or z 2 -1,1730606183.9822214,-0.0004049446010331419,-1.44818331372381,2.729122172380355,0.16447469907998422,0.9996439718404979,-0.005891315782799434,0.023512350475535227,0.011153086407656177,-1.44818331372381,2.729122172380355,0.16447469907998422,0.9996439718404979,-0.005891315782799434,0.023512350475535227,0.011153086407656177 -2,1730606185.978614,-0.0004048595658688002,-1.4464266503652385,2.72911422471794,0.16529892982310632,0.9996453243809861,-0.005700388419394981,0.023509993499049444,0.011136032541969262,-1.4464266503652385,2.72911422471794,0.16529892982310632,0.9996453243809861,-0.005700388419394981,0.023509993499049444,0.011136032541969262 -3,1730606187.9794643,-0.0004047982695540296,-1.4446275266997315,2.7293390766050543,0.16342223394915065,0.9996405590021175,-0.006146061219711183,0.023615575948105417,0.011103301403499738,-1.4446275266997315,2.7293390766050543,0.16342223394915065,0.9996405590021175,-0.006146061219711183,0.023615575948105417,0.011103301403499738 -4,1730606189.9816167,-0.0004047131798816803,-1.4427983524491972,2.7294270699991325,0.16362602898572473,0.9996364545547526,-0.006200093363465322,0.023785856070205978,0.011079287815376697,-1.4427983524491972,2.7294270699991325,0.16362602898572473,0.9996364545547526,-0.006200093363465322,0.023785856070205978,0.011079287815376697 -5,1730606191.9829905,-0.00040472188516482,-1.441067093081469,2.7294788007150315,0.16341365593629936,0.9996382825980462,-0.006185347903394746,0.023722443345314693,0.01105853143373917,-1.441067093081469,2.7294788007150315,0.16341365593629936,0.9996382825980462,-0.006185347903394746,0.023722443345314693,0.01105853143373917 -6,1730606193.9778209,-0.00040815079017685464,-1.4757537223195674,2.7222622651065036,0.09712637455239398,0.995473210093624,-0.0008116760643057892,-0.09462179852498112,0.008896314504340693,-1.4757537223195674,2.7222622651065036,0.09712637455239398,0.995473210093624,-0.0008116760643057892,-0.09462179852498112,0.008896314504340693 -7,1730606195.983692,-0.00041017851447404337,-1.4514179663061864,2.76113124628261,0.8571591137260269,0.9997483376423889,-9.890628782080295e-05,-0.02214686810668308,-0.003573210306068304,-1.4514179663061864,2.76113124628261,0.8571591137260269,0.9997483376423889,-9.890628782080295e-05,-0.02214686810668308,-0.003573210306068304 -8,1730606197.9891286,-0.0004112333212324556,-1.4789820499060147,2.750724095855923,0.9418069863898424,0.9999928231319808,0.001627135019988028,-0.003286417263314094,0.0009516184787096665,-1.4789820499060147,2.750724095855923,0.9418069863898424,0.9999928231319808,0.001627135019988028,-0.003286417263314094,0.0009516184787096665 -9,1730606199.9797802,-0.00041507892892091945,-1.5464585359197114,2.6811394135667284,0.9671956787992081,0.9999517767822987,8.59867674751213e-05,0.002463617118728545,0.009506172041995484,-1.5464585359197114,2.6811394135667284,0.9671956787992081,0.9999517767822987,8.59867674751213e-05,0.002463617118728545,0.009506172041995484 -10,1730606201.9832702,-0.0004068672756401804,-1.4930311454456546,2.2149640639809967,0.9306568594810922,0.9997275884952701,-0.023291403332014345,0.0011346247154914299,0.0009858798163705409,-1.4930311454456546,2.2149640639809967,0.9306568594810922,0.9997275884952701,-0.023291403332014345,0.0011346247154914299,0.0009858798163705409 -11,1730606203.9796584,-0.00038792189194332703,-1.5019731842600637,1.3072211982103648,0.8907994127436389,0.9981846247888271,-0.05997506346232245,0.0007835090179754385,0.005461932945170839,-1.5019731842600637,1.3072211982103648,0.8907994127436389,0.9981846247888271,-0.05997506346232245,0.0007835090179754385,0.005461932945170839 -12,1730606205.9803002,-0.0003232308396447451,-1.332108412861726,0.36207401326703664,0.961822281278685,0.9959573732678311,-0.08901467524896803,-0.0117906442680375,-0.0025057796206867395,-1.332108412861726,0.36207401326703664,0.961822281278685,0.9959573732678311,-0.08901467524896803,-0.0117906442680375,-0.0025057796206867395 -13,1730606207.9825418,-0.00020489405492753575,-0.7430562252439176,-0.29657880400565145,0.9969648611087392,0.9953320684872347,-0.07956339487390986,-0.054621099463337326,-0.0005245288145428069,-0.7430562252439176,-0.29657880400565145,0.9969648611087392,0.9953320684872347,-0.07956339487390986,-0.054621099463337326,-0.0005245288145428069 -14,1730606209.9806743,-0.0001671718758854756,-0.19801618269996957,-0.44045570501645587,1.0107209678952283,0.9973034118209274,-0.04494444739300005,-0.05801598201181403,-0.00021737083630478583,-0.19801618269996957,-0.44045570501645587,1.0107209678952283,0.9973034118209274,-0.04494444739300005,-0.05801598201181403,-0.00021737083630478583 -15,1730606211.9786365,-0.00014919102032094115,-0.01667539219850163,-0.5531548025930899,1.0037883317163254,0.999336416384447,-0.02282897914476973,-0.028380809412569156,-0.00030701170365328656,-0.01667539219850163,-0.5531548025930899,1.0037883317163254,0.999336416384447,-0.02282897914476973,-0.028380809412569156,-0.00030701170365328656 -16,1730606213.9846392,-0.00015347143110914244,0.0908327758412329,-0.5951806566066492,0.9892068628165283,0.9998369205121485,-0.010076477296627574,-0.012443051064079723,0.008352692158277778,0.0908327758412329,-0.5951806566066492,0.9892068628165283,0.9998369205121485,-0.010076477296627574,-0.012443051064079723,0.008352692158277778 -17,1730606215.980522,-0.0001446212059355562,0.11857213992520245,-0.588620405036433,0.9874957267047677,0.999972845200863,-0.006218312479291208,0.002642016737005114,-0.0029429913968840823,0.11857213992520245,-0.588620405036433,0.9874957267047677,0.999972845200863,-0.006218312479291208,0.002642016737005114,-0.0029429913968840823 -18,1730606217.9860191,-0.00014414287343270113,0.1374673538907063,-0.5897333885048757,0.8650125795394152,0.999897621880946,-0.0060957321213354065,0.012586032936664857,-0.003029782442844557,0.1374673538907063,-0.5897333885048757,0.8650125795394152,0.999897621880946,-0.0060957321213354065,0.012586032936664857,-0.003029782442844557 diff --git a/riptide_acoustics/src/matlab/data/test3.csv b/riptide_acoustics/src/matlab/data/test3.csv deleted file mode 100644 index df4dcc3..0000000 --- a/riptide_acoustics/src/matlab/data/test3.csv +++ /dev/null @@ -1,20 +0,0 @@ -number,time,delta_t,pos x 1,pos y 1,pos z 1,or w 1,or x 1,or y 1, or z 1,pos x 2,pos y 2,pos z 2,or w 2,or x 2,or y 2, or z 2 -1,1730606579.9802365,-0.0001253174677029977,-0.4163459671747822,0.5968035655233543,-0.13009170754716787,0.9996797023652668,0.006461941330008329,-0.023450988273185154,0.006984779320058735,-0.4156322472233951,1.2468224786111097,-0.12143704221311552,0.9996797023652668,0.006461941330008329,-0.023450988273185154,0.006984779320058735 -2,1730606581.9776223,-0.0001251644840117147,-0.4179850986623722,0.5967806670908133,-0.13012625665880345,0.9996800541537193,0.006533187693569448,-0.023409361541509927,0.007007751279572117,-0.4173030218089022,1.2467983900640658,-0.12138013798623497,0.9996800541537193,0.006533187693569448,-0.023409361541509927,0.007007751279572117 -3,1730606583.9795377,-0.00012521914510405066,-0.419616432675359,0.5967542522988211,-0.13032485875904767,0.9996780936847931,0.006813940737221492,-0.02340461501514544,0.007035852070671016,-0.41897936255841783,1.2467670215857867,-0.1212147596620875,0.9996780936847931,0.006813940737221492,-0.02340461501514544,0.007035852070671016 -4,1730606585.9722495,-0.00012510701653001928,-0.42124948425873543,0.5967287047536821,-0.13048799891217536,0.9996762942223656,0.007044266792109477,-0.023405168586172812,0.007062801048650141,-0.42065444110051303,1.2467372612949499,-0.12107936740820008,0.9996762942223656,0.007044266792109477,-0.023405168586172812,0.007062801048650141 -5,1730606587.972156,-0.00012510791402371317,-0.4229104169024931,0.5967186313880065,-0.13015997228803516,0.9996761234004469,0.006507179903331045,-0.02356441523833557,0.007072711448619975,-0.42231340924855504,1.2467368964166898,-0.12144798566263243,0.9996761234004469,0.006507179903331045,-0.02356441523833557,0.007072711448619975 -6,1730606589.9793305,-0.00012573835113941368,-0.3930678306723455,0.6038586364495109,-0.40652970499988694,0.9673025514229892,-0.0011557015035376628,0.25349261133366585,0.00811999777396901,-0.39494601270290997,1.253922416092197,-0.41021139332525924,0.9673025514229892,-0.0011557015035376628,0.25349261133366585,0.00811999777396901 -7,1730606591.9747446,-0.00014077767268860195,-0.27405960492570663,0.6105285826999282,-1.0007874319387178,0.9975713120363877,-0.0035568941513799248,0.06956153817234788,0.00013532587610615377,-0.2646535277559122,1.2605098634317726,-1.0067757881796024,0.9975713120363877,-0.0035568941513799248,0.06956153817234788,0.00013532587610615377 -8,1730606593.9788249,-0.00013911462769748914,-0.2032590714666731,0.6031998879739695,-0.9892140137723926,0.9998485061214814,-0.003932283808597593,0.016581788756566757,0.0035420661093946383,-0.19795357545544573,1.2532330026633849,-0.9945807184645078,0.9998485061214814,-0.003932283808597593,0.016581788756566757,0.0035420661093946383 -9,1730606595.9784057,-0.00015421170787955832,-0.16205597469502714,0.6031119269128142,-1.0316036576784025,0.999908936205534,-0.004634736641643484,0.0010544108170844532,-0.012630389162994879,-0.13564752976728106,1.2526239346262584,-1.037665495068887,0.999908936205534,-0.004634736641643484,0.0010544108170844532,-0.012630389162994879 -10,1730606597.9731364,-0.0001507145668751925,-0.012256016907346567,0.6049394091183984,-1.009169753056389,0.9999446132882163,-0.0038816473466427898,-0.009662977322558004,0.001526446533536237,-0.004193440804658083,1.2549480701308915,-1.0140416598016482,0.9999446132882163,-0.0038816473466427898,-0.009662977322558004,0.001526446533536237 -11,1730606599.97842,-0.00017441549370768663,0.5769791722630976,0.6077721500419295,-0.9892138874802936,0.9987557956860734,-0.0040956934980362,-0.04958145938979326,0.0034299800330018144,0.5827403359764699,1.257807624391154,-0.9937626323892503,0.9987557956860734,-0.0040956934980362,-0.04958145938979326,0.0034299800330018144 -12,1730606601.9789667,-0.0002855277072592073,1.4389062815266378,0.3432351939532685,-0.9827763774762791,0.9965118139182854,-0.02139823463545074,-0.08022211484833161,-0.008410265455735734,1.4619029577988203,0.9924147054945871,-1.0080175081048357,0.9965118139182854,-0.02139823463545074,-0.08022211484833161,-0.008410265455735734 -13,1730606603.9767227,-0.00039156238577056033,2.032189999942822,-0.3366907448514985,-0.9506967007037342,0.9978820143025261,-0.05010588412357974,-0.041475041352813435,-0.0008407452110572553,2.045947822807668,0.3100693403111179,-1.0148224720477494,0.9978820143025261,-0.05010588412357974,-0.041475041352813435,-0.0008407452110572553 -14,1730606605.9771225,-0.0004091697255268721,2.1353196690891325,-0.5673459636800655,-0.9485581375367372,0.9990499804463349,-0.03424341252172918,0.023798677076993607,-0.0126549689225031,2.1606815366830956,0.08065229103107868,-0.9938906596754523,0.9990499804463349,-0.03424341252172918,0.023798677076993607,-0.0126549689225031 -15,1730606607.9814677,-0.00041205963343828007,2.191563869763939,-0.6278439139769659,-0.9733444412757101,0.999797079141238,-0.018697379318775544,0.00702834225117261,-0.002609780155336798,2.2047839362870842,0.021637948950278707,-0.9978095096320301,0.999797079141238,-0.018697379318775544,0.00702834225117261,-0.002609780155336798 -16,1730606609.9771278,-0.00041278567022787243,2.2173207543314213,-0.8365530321600158,-0.9991215638073174,0.9998072260608244,-0.018224080069912742,-6.379096125897255e-05,0.007306815508710406,2.217824168721407,-0.18690805941193672,-1.022810294400148,0.9998072260608244,-0.018224080069912742,-6.379096125897255e-05,0.007306815508710406 -17,1730606611.9737067,-0.0004211143125894393,2.2013579542241986,-1.601488981622902,-0.9602122637260162,0.9988359607575722,-0.04802916479030291,-0.000957745330944172,-0.004359535631761697,2.2170781546380525,-0.954598698935752,-1.0225487495420074,0.9988359607575722,-0.04802916479030291,-0.000957745330944172,-0.004359535631761697 -18,1730606613.9787362,-0.00042406877741856756,2.196972526312652,-2.0617214901085608,-0.9580097535959066,0.9989405469254338,-0.04589544413627644,-0.0011434420389472718,-0.003175603273823597,2.211164431421146,-1.414535304433584,-1.0175801397023123,0.9989405469254338,-0.04589544413627644,-0.0011434420389472718,-0.003175603273823597 -19,1730606615.9805744,-0.00042650538273265415,2.210173591516032,-2.1371121601833827,-0.9681404530568304,0.9995345953228278,-0.0285877577228314,-0.006436451071234885,-0.008479679169586076,2.231428982167184,-1.4884339090686212,-1.0050827715948043,0.9995345953228278,-0.0285877577228314,-0.006436451071234885,-0.008479679169586076 diff --git a/riptide_acoustics/src/matlab/generatte_test_cases.asv b/riptide_acoustics/src/matlab/generatte_test_cases.asv deleted file mode 100644 index 1377e9a..0000000 --- a/riptide_acoustics/src/matlab/generatte_test_cases.asv +++ /dev/null @@ -1,57 +0,0 @@ - -close all -clear - -%speed of sound -speed_of_sound = 1500; - -%hydrophone base vector from 1 to 2 -base_vector = [0.01, 0.65, 0.0]; - -%initial pose of hydrophone 1 -%make sure to convert from ros quat to matlab quat -pose1_h1 = [-0.1644,0.295,-0.988,1,-0.0001,0.0002,0.001]; - -%second pose of hydrophone 2 -pose2_h1 = [-.1644,2.2512,-.9885,.9992,0.0402,0,0.0002]; - -%pinger pose -pose_pinger = [3, 2,-1,0,0,0,0]; - -%calculaete the second hydrophone poses -rotm1 = quat2rotm([pose1_h1(4), pose1_h1(5), pose1_h1(6), pose1_h1(7)]); -pose1_h2 = (rotm1 * base_vector' + pose1_h1(1:3)')'; - -rotm2 = quat2rotm([pose2_h1(4), pose2_h1(5), pose2_h1(6), pose2_h1(7)]); -pose2_h2 = (rotm2 * base_vector' + pose2_h1(1:3)')'; - -%calcate the distances for the four hydrophones from the pinger -dist1_h1 = norm(pose_pinger(1:3) - pose1_h1(1:3)); -dist1_h2 = norm(pose_pinger(1:3) - pose1_h2(1:3)); -dist2_h1 = norm(pose_pinger(1:3) - pose2_h1(1:3)); -dist2_h2 = norm(pose_pinger(1:3) - pose2_h2(1:3)); - -%calculate the time delta for each pose -delta_d1 = (dist1_h2 - dist1_h1); -delta_d2 = (dist2_h2 - dist2_h1); - -%calculate the distances othogonal from initial hydrophone base vector -pos1_mid = (pose1_h2 + pose1_h1(1:3)) / 2; -pos1_mid_to_pinger = [pose_pinger(1:2),0] - pos1_mid; - -%rotate to new frame -sol = rotm1 * pos1_mid_to_pinger'; - -%plot the senario -figure(1) -hold on; -plot3([pose1_h1(1);pose1_h2(1)], [pose1_h1(2);pose1_h2(2)],[pose1_h1(3);pose1_h2(3)], "b.-", [pose2_h1(1);pose2_h2(1)], [pose2_h1(2);pose2_h2(2)],[pose2_h1(3);pose2_h2(3)], "r.-") -plot3([pose_pinger(1)], [pose_pinger(2)], [pose_pinger(3)], "g*") -hold off - -%calcuate a1 and a2 values -disp("a1: " + num2str(norm(pose_pinger(1:2) - pose1_h1(1:2)))) -disp("a2: " + num2str(norm(pose_pinger(1:2) - pose2_h1(1:2)))) - -[solve_x_p, solve_y] = solve_two_pulse_system_depth(delta_d1, delta_d2, pose1_h1', pose2_h1', base_vector', 1, [100; 99], pose_pinger(3)); -[solve_x, solve_y] = solve_two_pulse_system_depth(delta_d1, delta_d2, pose1_h1', pose2_h1', base_vector', -1, [100; 99], pose_pinger(3)); diff --git a/riptide_acoustics/src/matlab/generatte_test_cases.m b/riptide_acoustics/src/matlab/generatte_test_cases.m deleted file mode 100644 index 768aeef..0000000 --- a/riptide_acoustics/src/matlab/generatte_test_cases.m +++ /dev/null @@ -1,57 +0,0 @@ - -close all -clear - -%speed of sound -speed_of_sound = 1500; - -%hydrophone base vector from 1 to 2 -base_vector = [0.2, 0.0, 0.0]; - -%initial pose of hydrophone 1 -%make sure to convert from ros quat to matlab quat -pose1_h1 = [0,0,0,1,-0.0001,0.0002,0.001]; - -%second pose of hydrophone 2 -pose2_h1 = [3.1644,0,0,.9992,0.0402,0,0]; - -%pinger pose -pose_pinger = [1, 7,-1,0,0,0,0]; - -%calculaete the second hydrophone poses -rotm1 = quat2rotm([pose1_h1(4), pose1_h1(5), pose1_h1(6), pose1_h1(7)]); -pose1_h2 = (rotm1 * base_vector' + pose1_h1(1:3)')'; - -rotm2 = quat2rotm([pose2_h1(4), pose2_h1(5), pose2_h1(6), pose2_h1(7)]); -pose2_h2 = (rotm2 * base_vector' + pose2_h1(1:3)')'; - -%calcate the distances for the four hydrophones from the pinger -dist1_h1 = norm(pose_pinger(1:3) - pose1_h1(1:3)); -dist1_h2 = norm(pose_pinger(1:3) - pose1_h2(1:3)); -dist2_h1 = norm(pose_pinger(1:3) - pose2_h1(1:3)); -dist2_h2 = norm(pose_pinger(1:3) - pose2_h2(1:3)); - -%calculate the time delta for each pose -delta_d1 = (dist1_h2 - dist1_h1); -delta_d2 = (dist2_h2 - dist2_h1); - -%calculate the distances othogonal from initial hydrophone base vector -pos1_mid = (pose1_h2 + pose1_h1(1:3)) / 2; -pos1_mid_to_pinger = [pose_pinger(1:2),0] - pos1_mid; - -%rotate to new frame -sol = rotm1 * pos1_mid_to_pinger'; - -%plot the senario -figure(1) -hold on; -plot3([pose1_h1(1);pose1_h2(1)], [pose1_h1(2);pose1_h2(2)],[pose1_h1(3);pose1_h2(3)], "b.-", [pose2_h1(1);pose2_h2(1)], [pose2_h1(2);pose2_h2(2)],[pose2_h1(3);pose2_h2(3)], "r.-") -plot3([pose_pinger(1)], [pose_pinger(2)], [pose_pinger(3)], "g*") -hold off - -%calcuate a1 and a2 values -disp("a1: " + num2str(norm(pose_pinger(1:2) - pose1_h1(1:2)))) -disp("a2: " + num2str(norm(pose_pinger(1:2) - pose2_h1(1:2)))) - -[solve_x_p, solve_y_p] = solve_two_pulse_system_depth(delta_d1, delta_d2, pose1_h1', pose2_h1', base_vector', 1, [7.41; 7.3], pose_pinger(3)); -[solve_x_n, solve_y_n] = solve_two_pulse_system_depth(delta_d1, delta_d2, pose1_h1', pose2_h1', base_vector', -1, [7.41; 7.3], pose_pinger(3)); diff --git a/riptide_acoustics/src/matlab/parse_measurements.asv b/riptide_acoustics/src/matlab/parse_measurements.asv deleted file mode 100644 index cae569e..0000000 --- a/riptide_acoustics/src/matlab/parse_measurements.asv +++ /dev/null @@ -1,38 +0,0 @@ -function pinger_location = parse_measurements(m1, m2, port_to_origin_transform, port_to_startboard_transform, speed_of_sound, pinger_depth) - - p2o_translation = [port_to_origin_transform.transform.translation.x; port_to_origin_transform.transform.translation.y; port_to_origin_transform.transform.translation.z]; - p2o_quat = [port_to_origin_transform.transform.rotation.w; port_to_origin_transform.transform.rotation.x; port_to_origin_transform.transform.rotation.y; port_to_origin_transform.transform.rotation.z]; - p2o_rotation = quat2rotm(p2o_quat'); - - %origin to world frame - o2m1_translation = [m1.auv_origin.transform.translation.x; m1.auv_origin.transform.translation.y; m1.auv_origin.transform.translation.z]; - o2m1_quat = [m1.auv_origin.transform.rotation.w; m1.auv_origin.transform.rotation.x; m1.auv_origin.transform.rotation.y; m1.auv_origin.transform.rotation.z]; - o2m1_rotation = quat2rotm(o2m1_quat'); - - - o2m2_translation = [m2.auv_origin.transform.translation.x; m2.auv_origin.transform.translation.y; m2.auv_origin.transform.translation.z]; - o2m2_quat = [m2.auv_origin.transform.rotation.w; m2.auv_origin.transform.rotation.x; m2.auv_origin.transform.rotation.y; m2.auv_origin.transform.rotation.z]; - o2m2_rotation = quat2rotm(o2m1_quat'); - - p2s_translation = [port_to_startboard_transform.transform.translation.x; port_to_startboard_transform.transform.translation.y; port_to_startboard_transform.transform.translation.z]; - - o2m1_rotation - o2m1_translation - - - %transform each hydrophone into world frame for each measurement - m1t = o2m1_rotation * p2o_translation + o2m1_translation; - m1r = transpose(quatmultiply(p2o_quat', o2m1_quat')); - m2t = o2m2_rotation * p2o_translation + o2m2_translation; - m2r = transpose(quatmultiply(p2o_quat', o2m2_quat')); - - o2m1_quat - p2o_quat - m1r - - %in world frame - [solve_x, solve_y, solve_depth] = solve_two_pulse_system_depth(m1.delta_t*speed_of_sound, m2.delta_t*speed_of_sound,[m1t;m1r],[m2t;m2r], p2s_translation, 3000, pinger_depth); - - pinger_location = ([solve_x; solve_y; pinger_depth]); - -end \ No newline at end of file diff --git a/riptide_acoustics/src/matlab/parse_measurements.m b/riptide_acoustics/src/matlab/parse_measurements.m deleted file mode 100644 index acd121b..0000000 --- a/riptide_acoustics/src/matlab/parse_measurements.m +++ /dev/null @@ -1,30 +0,0 @@ -function pinger_location = parse_measurements(m1, m2, port_to_origin_transform, port_to_startboard_transform, speed_of_sound, pinger_depth) - - p2o_translation = [port_to_origin_transform.transform.translation.x; port_to_origin_transform.transform.translation.y; port_to_origin_transform.transform.translation.z]; - p2o_quat = [port_to_origin_transform.transform.rotation.w; port_to_origin_transform.transform.rotation.x; port_to_origin_transform.transform.rotation.y; port_to_origin_transform.transform.rotation.z]; - p2o_rotation = quat2rotm(p2o_quat'); - - %origin to world frame - o2m1_translation = [m1.auv_origin.transform.translation.x; m1.auv_origin.transform.translation.y; m1.auv_origin.transform.translation.z]; - o2m1_quat = [m1.auv_origin.transform.rotation.w; m1.auv_origin.transform.rotation.x; m1.auv_origin.transform.rotation.y; m1.auv_origin.transform.rotation.z]; - o2m1_rotation = quat2rotm(o2m1_quat'); - - - o2m2_translation = [m2.auv_origin.transform.translation.x; m2.auv_origin.transform.translation.y; m2.auv_origin.transform.translation.z]; - o2m2_quat = [m2.auv_origin.transform.rotation.w; m2.auv_origin.transform.rotation.x; m2.auv_origin.transform.rotation.y; m2.auv_origin.transform.rotation.z]; - o2m2_rotation = quat2rotm(o2m1_quat'); - - p2s_translation = [port_to_startboard_transform.transform.translation.x; port_to_startboard_transform.transform.translation.y; port_to_startboard_transform.transform.translation.z]; - - %transform each hydrophone into world frame for each measurement - m1t = o2m1_rotation * p2o_translation + o2m1_translation; - m1r = transpose(quatmultiply(p2o_quat', o2m1_quat')); - m2t = o2m2_rotation * p2o_translation + o2m2_translation; - m2r = transpose(quatmultiply(p2o_quat', o2m2_quat')); - - %in world frame - [solve_x, solve_y, solve_depth] = solve_two_pulse_system_depth(m1.delta_t*speed_of_sound, m2.delta_t*speed_of_sound,[m1t;m1r],[m2t;m2r], p2s_translation, -1, [1;1], pinger_depth); - - pinger_location = ([solve_x; solve_y; pinger_depth]); - -end \ No newline at end of file diff --git a/riptide_acoustics/src/matlab/run_measurements_from_csv.asv b/riptide_acoustics/src/matlab/run_measurements_from_csv.asv deleted file mode 100644 index f1d1d1e..0000000 --- a/riptide_acoustics/src/matlab/run_measurements_from_csv.asv +++ /dev/null @@ -1,43 +0,0 @@ -clear; - -%constants -speed_of_sound = 1500; -pinger_depth = -1.0; - -%load in csv -filename = "data/test1.csv"; - -data = table2array(readtable(filename, "NumHeaderLines", 1)); - -%load into ros2 messages -transform = ros2message("geometry_msgs/TransformStamped"); - -line_1 = 2; -m1 = struct(); -m1.delta_t = data(line_1, 3); - -transform.transform.translation.x = data(line_1, 4); -transform.transform.translation.y = data(line_1, 5); -transform.transform.translation.z = data(line_1, 6); -transform.transform.rotation.w = data(line_1,7); -transform.transform.rotation.x = data(line_1,8); -transform.transform.rotation.y = data(line_1,9); -transform.transform.rotation.z = data(line_1,10); - -m1.auv_origin = transform; - -line_2 = 30; -m2 = struct(); -m2.delta_t = data(line_2, 3); - -transform.transform.translation.x = data(line_2, 4); -transform.transform.translation.y = data(line_2, 5); -transform.transform.translation.z = data(line_2, 6); -transform.transform.rotation.w = data(line_2,7); -transform.transform.rotation.x = data(line_2,8); -transform.transform.rotation.y = data(line_2,9); -transform.transform.rotation.z = data(line_2,10); - -m2.auv_origin = transform; - -parse_measurements(m1,m2,) diff --git a/riptide_acoustics/src/matlab/run_measurements_from_csv.m b/riptide_acoustics/src/matlab/run_measurements_from_csv.m deleted file mode 100644 index d1d284e..0000000 --- a/riptide_acoustics/src/matlab/run_measurements_from_csv.m +++ /dev/null @@ -1,42 +0,0 @@ -%run the node to use its initial parameters - -%load in csv -filename = "data/test3.csv"; - -data = table2array(readtable(filename, "NumHeaderLines", 1)); - -%load into ros2 messages -transform = ros2message("geometry_msgs/TransformStamped"); - -%pinger pose hardcoded -pinger_pose = [3.0,2.0,1.0,0.0,0.0,0.0]; - -line_1 = 7; -m1 = struct(); -m1.delta_t = data(line_1, 3); - -transform.transform.translation.x = data(line_1, 4); -transform.transform.translation.y = data(line_1, 5); -transform.transform.translation.z = data(line_1, 6); -transform.transform.rotation.w = data(line_1,7); -transform.transform.rotation.x = data(line_1,8); -transform.transform.rotation.y = data(line_1,9); -transform.transform.rotation.z = data(line_1,10); - -m1.auv_origin = transform; - -line_2 = 16; -m2 = struct(); -m2.delta_t = data(line_2, 3); - -transform.transform.translation.x = data(line_2, 4); -transform.transform.translation.y = data(line_2, 5); -transform.transform.translation.z = data(line_2, 6); -transform.transform.rotation.w = data(line_2,7); -transform.transform.rotation.x = data(line_2,8); -transform.transform.rotation.y = data(line_2,9); -transform.transform.rotation.z = data(line_2,10); - -m2.auv_origin = transform; - -parse_measurements(m1,m2,port_to_origin_transform, port_to_startboard_transform, speed_of_sound, pinger_depth) diff --git a/riptide_acoustics/src/matlab/solve_two_pulse_system_depth.asv b/riptide_acoustics/src/matlab/solve_two_pulse_system_depth.asv deleted file mode 100644 index 9d843dc..0000000 --- a/riptide_acoustics/src/matlab/solve_two_pulse_system_depth.asv +++ /dev/null @@ -1,126 +0,0 @@ -function [intersection_x, intersection_y, pinger_depth] = solve_two_pulse_system_depth(k1, k2, pose_1, pose_2, hydrophone_base_vector, estimated_distance, pinger_depth) -%k1: hydrophone 1 pulse time - hydrophone 2 pulse time for first pulse -%k2: hydrophone 1 pulse time - hydrophone 2 pulse time for second pulse -%pose_1: the pose of the front hydrophone at the time of the first pulse -%pose_2: the pose of the front hydrophone at the time of the second pulse -%hydrophone_base_vector: the position vector between hydrophone 1 and 2 -%estimated disance: the estimated distance from the front hydrophone and -%pinger -%pinger_depth: the estimated depth of the pinger - -%pose format = [x,y,z,rw,rx,ry,rz] - -pose_1(4:7) = quatmultiply(pose_1(4:7)', eul2quat([atan(hydrophone_base_vector(2)/hydrophone_base_vector(1)), 0, 0]))' -pose_2(4:7) = quatmultiply(pose_2(4:7)', eul2quat([atan(hydrophone_base_vector(2)/hydrophone_base_vector(1)), 0, 0]))'; -hydrophone_base_vector = [norm(hydrophone_base_vector(1:2)), 0, hydrophone_base_vector(3)]'; - -%base frame is hydrophone 1 pulse 1 = [0,0,0,0,0,0] -x1_1 = 0; -y1_1 = 0; -z1_1 = 0; - -pinger_depth = pinger_depth - pose_1(3); - -%calculate position of second hydrophone -%order [Z,Y,X] -eul_1 = quat2eul([pose_1(4), pose_1(5), pose_1(6), pose_1(7)]); -norm_matrix = eul2rotm([-eul_1(1), 0, 0]); -rot_1 = norm_matrix * quat2rotm([pose_1(4), pose_1(5), pose_1(6), pose_1(7)]); -pos2_1 = rot_1 * hydrophone_base_vector; - -pos1_2 = norm_matrix * [(pose_2(1) - pose_1(1)); pose_2(2) - pose_1(2); pose_2(3) - pose_1(3)]; - -%calculate position of second hydrophone in second pulse -rot_2 = norm_matrix * quat2rotm([pose_2(4), pose_2(5), pose_2(6), pose_2(7)]); -pos2_2 = rot_2 * hydrophone_base_vector + pos1_2; - -%base widths in x,y plane -h1 = norm([pos2_1(2),pos2_1(1)]); -h2 = norm([pos2_2(1) - pos1_2(1) , pos2_2(2) - pos1_2(2)]); - -%calculate delta positions -x1 = pos1_2(1) - x1_1; -x2 = pos2_2(1) - pos2_1(1); - -y1 = pos1_2(2) - y1_1; -y2 = pos2_2(2) - pos2_1(2); - -%changes in depth -dz1_1 = pinger_depth; -dz2_1 = pos2_1(3) + pinger_depth; -dz1_2 = pose_2(3) + pinger_depth; -dz2_2 = pos2_2(3) + pinger_depth; - -%calculate heading change -eul_2 = rotm2eul(rot_2); -heading_change = eul_2(1); - -%define core functions -K = @(a,k_bar,z1,z2) (-(2*a) + sqrt(4*a^2 + 4 * (k_bar^2 + 2 * k_bar * sqrt(a^2 + z1^2) + z1^2 - z2^2))) / 2; -Y = @(a,k,h) sqrt(a^2 + 2*a*k + k^2 - ((h^2 + 2*a*k +k^2)/(2*h))^2); -X = @(a,y,k,h) sqrt(a^2 - y^2) * -sign(k) - h / 2; - -norm1 = 5; -answer = [0;0]; -while(norm1 > .001) - - x0 = [rand() * estimated_distance;rand() * estimated_distance];% [estimated_distance; estimated_distance; estimated_distance; estimated_distance; k1 / 2 ; k2 / 2] * (.2 - rand()*.2); - - %equations of localization -% F = @(x) [sqrt(abs((x(1)+x(3))^2-x(5)^2))*sign((x(1)+x(3))^2-x(5)^2) + sqrt(abs((x(2)+x(4))^2-x(5)^2))*sign((x(1)+x(3))^2-x(5)^2) - sqrt(x1^2+y1^2); -% sqrt(abs((x(1)+x(3)+k1)^2-x(6)^2))*sign((x(1)+x(3))^2-x(5)^2) + sqrt(abs((x(2)+x(4)+k2)^2-x(6)^2))*sign((x(1)+x(3))^2-x(5)^2) - sqrt(x2^2+y2^2); -% (x(2)+x(4))^2 - (((x(1)+x(3))^2-(x(2)+x(4))^2-(x1^2+y1^2))/(-2*sqrt(x1^2+y1^2)))^2 - x(5)^2; -% (x(2)+x(4)+k2)^2 - (((x(1)+x(3)+k1)^2-(x(2)+x(4)+k2)^2-(x2^2+y2^2))/(-2*sqrt(x2^2+y2^2)))^2 - x(6)^2; -% .5*hydrophone_base_width^2 - k1^2 - 2*(k1+x(3))*x(3) - 2*k1*x(1) - 4*x(1)*x(3); -% .5*hydrophone_base_width^2 - k2^2 - 2*(k2+x(4))*x(4) - 2*k2*x(2) - 4*x(2)*x(4)]; - - - %cramers rule & determinant of rot matrix is one -% F = @(x) [cos(-heading_change)*(Y(x(1), k1, h1) - (y1 + y2) / 2) + (sin(-heading_change) * (X(x(1), Y(x(1), k1, h1), k1, h1) - (x1 + x2 - hydrophone_base_vector(1)) / 2)) - Y(x(2),k2, h2); -% (X(x(1), Y(x(1), k1, h1), k1, h1) - (x1 + x2 - hydrophone_base_vector(1)) / 2) * cos(-heading_change) - (Y(x(1), k1, h1) - (y1 + y2) / 2) * sin(-heading_change) - (X(x(2),Y(x(2),k2,h2), k2, h2))]; - F = @(x) [cos(-heading_change)*(Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1) - (y1 + y2) / 2) + (sin(-heading_change) * (X(x(1), Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1), K(x(1), k1, dz1_1, dz2_1), h1) - (x1 + x2) / 2)) - Y(x(2),K(x(2), k2, dz1_2, dz2_2), h2); - (X(x(1), Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1), K(x(1), k1, dz1_1, dz2_1), h1) - (x1 + x2) / 2) * cos(-heading_change) - (Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1) - (y1 + y2) / 2) * sin(-heading_change) - (X(x(2),Y(x(2),K(x(2), k2, dz1_2, dz2_2),h2), K(x(2), k2, dz1_2, dz2_2), h2))]; - - %options = optimoptions('fsolve','Display','iter', "OptimalityTolerance", 10e-12); - options = optimoptions("fsolve", 'Display', 'off', "OptimalityTolerance", 10e-6); - - %solve non-linear system - solve1 = fsolve(F, x0, options); - - norm1 = norm(F(solve1)); - answer = solve1; - -end - - -answer - -Y() - -%vector relative to pos 1 coordinate frame -%rotate to input frame -rel_vector = [X(answer(1),Y(answer(1), K(answer(1), k1, dz1_1, dz2_1), h1), K(answer(1), k1, dz1_1, dz2_1), h1); Y(answer(1), K(answer(1), k1, dz1_1, dz2_1), h1); 0] -global_vector = norm_matrix' * rel_vector + pose_1(1:3); - -%calculate position - -intersection_x = global_vector(1); -intersection_y = global_vector(2); - -%for quadratic fit -% intersection_x = (-(b2(2) - b1(2)) - sqrt(abs((b2(2) - b1(2))^2 - 4 * (b2(1) - b1(1))*(b2(3) - b1(3))))) / (2 * (b2(3) - b1(3))); -% intersection_y = b1(1) + b1(2)* intersection_x + b1(3) * intersection_x^2; - - -%plots for debug -% figure(1); -% plot(fx1(2,:),fy1, "or", rot_points(1,:), rot_points(2,:),"xg") - -% figure(2) -% w_vals = linspace(-1000,1000, 1000); -% h1_vals = b1(1) + b1(2) * w_vals + b1(3)*w_vals.^2; -% h2_vals = b2(2) * w_vals + b2(1) + b2(3) * w_vals.^2;%b2_intercept ; -% -% plot(w_vals, h1_vals,"r", w_vals, h2_vals, "g", [-1000], [1000], "bo") - - diff --git a/riptide_acoustics/src/matlab/solve_two_pulse_system_depth.m b/riptide_acoustics/src/matlab/solve_two_pulse_system_depth.m deleted file mode 100644 index 4d48998..0000000 --- a/riptide_acoustics/src/matlab/solve_two_pulse_system_depth.m +++ /dev/null @@ -1,130 +0,0 @@ -function [intersection_x, intersection_y, pinger_depth] = solve_two_pulse_system_depth(k1, k2, pose_1, pose_2, hydrophone_base_vector, y_sign, guess, pinger_depth) -%k1: hydrophone 1 pulse time - hydrophone 2 pulse time for first pulse -%k2: hydrophone 1 pulse time - hydrophone 2 pulse time for second pulse -%pose_1: the pose of the front hydrophone at the time of the first pulse -%pose_2: the pose of the front hydrophone at the time of the second pulse -%hydrophone_base_vector: the position vector between hydrophone 1 and 2 -%estimated disance: the estimated distance from the front hydrophone and -%pinger -%pinger_depth: the estimated depth of the pinger - -%pose format = [x,y,z,rw,rx,ry,rz] - -hydrophone_base_vector_rotm = eul2rotm([atan(hydrophone_base_vector(2)/hydrophone_base_vector(1)), 0, 0]) -pose_1(4:7) = quatmultiply(pose_1(4:7)', eul2quat([atan(hydrophone_base_vector(2)/hydrophone_base_vector(1)), 0, 0]))'; -pose_2(4:7) = quatmultiply(pose_2(4:7)', eul2quat([atan(hydrophone_base_vector(2)/hydrophone_base_vector(1)), 0, 0]))'; -hydrophone_base_vector = [norm(hydrophone_base_vector(1:2)), 0, hydrophone_base_vector(3)]'; - -%base frame is hydrophone 1 pulse 1 = [0,0,0,0,0,0] -x1_1 = 0; -y1_1 = 0; -z1_1 = 0; - -pinger_depth = pinger_depth - pose_1(3); - -%calculate position of second hydrophone -%order [Z,Y,X] -eul_1 = quat2eul([pose_1(4), pose_1(5), pose_1(6), pose_1(7)]); -norm_matrix = eul2rotm([-eul_1(1), 0, 0]); -rot_1 = norm_matrix * quat2rotm([pose_1(4), pose_1(5), pose_1(6), pose_1(7)]); -pos2_1 = rot_1 * hydrophone_base_vector; - -pos1_2 = norm_matrix * [(pose_2(1) - pose_1(1)); pose_2(2) - pose_1(2); pose_2(3) - pose_1(3)]; - -%calculate position of second hydrophone in second pulse -rot_2 = norm_matrix * quat2rotm([pose_2(4), pose_2(5), pose_2(6), pose_2(7)]); -pos2_2 = rot_2 * hydrophone_base_vector + pos1_2; - -%base widths in x,y plane -h1 = norm([pos2_1(2),pos2_1(1)]); -h2 = norm([pos2_2(1) - pos1_2(1) , pos2_2(2) - pos1_2(2)]); - -%calculate delta positions -x1 = pos1_2(1) - x1_1; -x2 = pos2_2(1) - pos2_1(1); - -y1 = pos1_2(2) - y1_1; -y2 = pos2_2(2) - pos2_1(2); - -%changes in depth -dz1_1 = pinger_depth; -dz2_1 = pos2_1(3) + pinger_depth; -dz1_2 = pose_2(3) + pinger_depth; -dz2_2 = pos2_2(3) + pinger_depth; - -%calculate heading change -eul_2 = rotm2eul(rot_2); -heading_change = eul_2(1); - -%define core functions -K = @(a,k_bar,z1,z2) (-(2*a) + sqrt(4*a^2 + 4 * (k_bar^2 + 2 * k_bar * sqrt(a^2 + z1^2) + z1^2 - z2^2))) / 2; -Y = @(a,k,h) sign(y_sign) * sqrt(a^2 + 2*a*k + k^2 - ((h^2 + 2*a*k +k^2)/(2*h))^2); -X = @(a,y,k,h) sqrt(a^2 - y^2) * -sign(k) - h / 2; - -norm1 = 5; -answer = [0;0]; -count = 0; -while(norm1 > .001 && count < 50) - - x0 = guess;% [estimated_distance; estimated_distance; estimated_distance; estimated_distance; k1 / 2 ; k2 / 2] * (.2 - rand()*.2); - - %equations of localization -% F = @(x) [sqrt(abs((x(1)+x(3))^2-x(5)^2))*sign((x(1)+x(3))^2-x(5)^2) + sqrt(abs((x(2)+x(4))^2-x(5)^2))*sign((x(1)+x(3))^2-x(5)^2) - sqrt(x1^2+y1^2); -% sqrt(abs((x(1)+x(3)+k1)^2-x(6)^2))*sign((x(1)+x(3))^2-x(5)^2) + sqrt(abs((x(2)+x(4)+k2)^2-x(6)^2))*sign((x(1)+x(3))^2-x(5)^2) - sqrt(x2^2+y2^2); -% (x(2)+x(4))^2 - (((x(1)+x(3))^2-(x(2)+x(4))^2-(x1^2+y1^2))/(-2*sqrt(x1^2+y1^2)))^2 - x(5)^2; -% (x(2)+x(4)+k2)^2 - (((x(1)+x(3)+k1)^2-(x(2)+x(4)+k2)^2-(x2^2+y2^2))/(-2*sqrt(x2^2+y2^2)))^2 - x(6)^2; -% .5*hydrophone_base_width^2 - k1^2 - 2*(k1+x(3))*x(3) - 2*k1*x(1) - 4*x(1)*x(3); -% .5*hydrophone_base_width^2 - k2^2 - 2*(k2+x(4))*x(4) - 2*k2*x(2) - 4*x(2)*x(4)]; - - - %cramers rule & determinant of rot matrix is one -% F = @(x) [cos(-heading_change)*(Y(x(1), k1, h1) - (y1 + y2) / 2) + (sin(-heading_change) * (X(x(1), Y(x(1), k1, h1), k1, h1) - (x1 + x2 - hydrophone_base_vector(1)) / 2)) - Y(x(2),k2, h2); -% (X(x(1), Y(x(1), k1, h1), k1, h1) - (x1 + x2 - hydrophone_base_vector(1)) / 2) * cos(-heading_change) - (Y(x(1), k1, h1) - (y1 + y2) / 2) * sin(-heading_change) - (X(x(2),Y(x(2),k2,h2), k2, h2))]; - F = @(x) [cos(-heading_change)*(Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1) - (y1 + y2) / 2) + (sin(-heading_change) * (X(x(1), Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1), K(x(1), k1, dz1_1, dz2_1), h1) - (x1 + x2) / 2)) - Y(x(2),K(x(2), k2, dz1_2, dz2_2), h2); - (X(x(1), Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1), K(x(1), k1, dz1_1, dz2_1), h1) - (x1 + x2) / 2) * cos(-heading_change) - (Y(x(1), K(x(1), k1, dz1_1, dz2_1), h1) - (y1 + y2) / 2) * sin(-heading_change) - (X(x(2),Y(x(2),K(x(2), k2, dz1_2, dz2_2),h2), K(x(2), k2, dz1_2, dz2_2), h2))]; - - %options = optimoptions('fsolve','Display','iter', "OptimalityTolerance", 10e-12); - options = optimoptions("fsolve", 'Display', 'off', "OptimalityTolerance", 10e-6); - - %solve non-linear system - solve1 = fsolve(F, x0, options); - - norm1 = norm(F(solve1)); - answer = solve1; - - count = count + 1; -end - - - -answer - -%vector relative to pos 1 coordinate frame -%rotate to input frame -rel_vector = [X(answer(1),Y(answer(1), K(answer(1), k1, dz1_1, dz2_1), h1), K(answer(1), k1, dz1_1, dz2_1), h1); Y(answer(1), K(answer(1), k1, dz1_1, dz2_1), h1); 0]; -global_vector = norm_matrix' * rel_vector + pose_1(1:3); - -%account for hydrophone base vector -hydrophone_base_account = norm_matrix' * hydrophone_base_vector - -%calculate position -intersection_x = global_vector(1); -intersection_y = global_vector(2); - -%for quadratic fit -% intersection_x = (-(b2(2) - b1(2)) - sqrt(abs((b2(2) - b1(2))^2 - 4 * (b2(1) - b1(1))*(b2(3) - b1(3))))) / (2 * (b2(3) - b1(3))); -% intersection_y = b1(1) + b1(2)* intersection_x + b1(3) * intersection_x^2; - - -%plots for debug -% figure(1); -% plot(fx1(2,:),fy1, "or", rot_points(1,:), rot_points(2,:),"xg") - -% figure(2) -% w_vals = linspace(-1000,1000, 1000); -% h1_vals = b1(1) + b1(2) * w_vals + b1(3)*w_vals.^2; -% h2_vals = b2(2) * w_vals + b2(1) + b2(3) * w_vals.^2;%b2_intercept ; -% -% plot(w_vals, h1_vals,"r", w_vals, h2_vals, "g", [-1000], [1000], "bo") - - diff --git a/riptide_acoustics/src/matlab/two_pulse_depth_test.m b/riptide_acoustics/src/matlab/two_pulse_depth_test.m deleted file mode 100644 index acb581c..0000000 --- a/riptide_acoustics/src/matlab/two_pulse_depth_test.m +++ /dev/null @@ -1,31 +0,0 @@ -%should yeild (52,116.92) -distances = [-20, 41.465]; -pose1 = [0,0,0,1,0,0,0]; -pose2 = [141.89, 22.89,0, .9915,0,0,-.1300]; -pinger_depth = 0; - - -[slv_x,slv_y] = solve_two_pulse_system_depth(distances(1),distances(2),pose1, pose2, [50;0;0], 3000, pinger_depth) - -%should yeild (52,116.92) -distances = [-10.893, 23.1377]; -pose1 = [0,0,0,1,0,0,0]; -pose2 = [141.89, 22.89,0, .9915,0,0,-.1300]; -pinger_depth = -200; - - -[slv_x,slv_y] = solve_two_pulse_system_depth(distances(1),distances(2),pose1, pose2, [50;0;0], 100000, pinger_depth) - - -%should yeild (152,216.92) -distances = [-10.893, 23.1377]; -pose1 = [100,100,0,1,0,0,0]; -pose2 = [241.89, 122.89,0, .9915,0,0,-.1300]; -pinger_depth = -200; - - -[slv_x,slv_y] = solve_two_pulse_system_depth(distances(1),distances(2),pose1, pose2, [50;0;0], 100000, pinger_depth) - -%should yeild (2998.5,95.04) -% distances = [2,19]; -% transform = [-95.04, 8.49, -.38207]; diff --git a/riptide_descriptions/config/simulator.yaml b/riptide_descriptions/config/simulator.yaml index 82e16bb..55578a5 100644 --- a/riptide_descriptions/config/simulator.yaml +++ b/riptide_descriptions/config/simulator.yaml @@ -7,51 +7,29 @@ #Some of these may be depricated parameters from the vehicle! Even if it looks like a vehicle parameter, its not one! vehicle_properties: - talos: - base_wrench: [0.0, 0.0, -4.9, -0.95, 2.1, 0.0] - cod: [-0.148, 0.040, -0.055] - inertia3x3: [1.6315, -0.0599, -0.0330, # Ixx Ixy Ixz - -0.0599, 0.7341, 0.0427, # Iyx Iyy Iyz - -0.0330, 0.0427, 1.6332] # Izx Izy Izz kg*m^ - - #center of mass discrepancy - dcom: [0.01, 0.0, 0.0] - - # drag = (k1 + k2 * abs(v) + k3 * exp(abs(v) / k4) )* sign(v) - damping: [-38.7588, -28.1146, 37.3211, 0.5034, #x - -7.4651, 17.7641, 7.0013, 0.3067, #y - -7.0964, 27.6349, 6.9723, 0.3827, #z - -4.6881, 5.2662, 6.2630, 1.2739, #r - -18.1269, -3.1893, 17.3711, 2.0801, #p - -20.4148, -8.2547, 20.2604, 1.6177] #yaw - - thruster_max_force: 28.0 - - #sim discrepency - allows to test control when feed forward is not perfect - sim_discrepancy: [0.0, 1.0, -0.5, 0.1, -0.05, 0.0] - - env_force: [0.0, 0.7, 0.0] - - liltank: #TODO: measure or estimate all liltank parameters - cob: [0.0, 0.0, 0.15] - cod: [0.0, 0.0, -0.055] - inertia3x3: [1.6315, -0.0599, -0.0330, # Ixx Ixy Ixz - -0.0599, 0.7341, 0.0427, # Iyx Iyy Iyz - -0.0330, 0.0427, 1.6332] # Izx Izy Izz kg*m^2 - - # drag = (k1 + k2 * abs(v) + k3 * exp(abs(v) / k4) )* sign(v) - damping: [-38.7588, -28.1146, 37.3211, 0.5034, #x - -7.4651, 17.7641, 7.0013, 0.3067, #y - -7.0964, 27.6349, 6.9723, 0.3827, #z - -4.6881, 5.2662, 6.2630, 1.2739, #r - -18.1269, -3.1893, 17.3711, 2.0801, #p - -20.4148, -8.2547, 20.2604, 1.6177] #yaw - - thruster_max_force: 28.0 - - #sim discrepency - allows to test control when feed forward is not perfect - sim_discrepancy: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - + base_wrench: [-0.35, -0.1, -3.00, 0.1, 2.4, 0.0] + cod: [-0.148, 0.040, -0.055] + inertia3x3: [1.6315, -0.0599, -0.0330, -0.0599, 0.7341, 0.0427, -0.0330, 0.0427, 1.6332] # Izx Izy Izz kg*m^ + + #center of mass discrepancy + dcom: [0.01, 0.0, 0.0] + + # drag = (k1 + k2 * abs(v) + k3 * exp(abs(v) / k4) )* sign(v) + damping: [-38.7588, -28.1146, 37.3211, 0.5034, #x + -7.4651, 17.7641, 7.0013, 0.3067, #y + -7.0964, 27.6349, 6.9723, 0.3827, #z + -4.6881, 5.2662, 6.2630, 1.2739, #r + -18.1269, -3.1893, 17.3711, 2.0801, #p + -20.4148, -8.2547, 20.2604, 1.6177] #yaw + + thruster_max_force: 28.0 + + + #sim discrepency - allows to test control when feed forward is not perfect +controller: + sim_discrepancy: [0.0, 1.0, -0.5, 0.1, -0.05, 0.0] + + env_force: [0.0, 0.0, 0.0] claw: fake_objects: #I've arbilitrarilly set these properties - measure at some point diff --git a/riptide_descriptions/config/talos.yaml b/riptide_descriptions/config/talos.yaml index 72b149b..a6c6626 100644 --- a/riptide_descriptions/config/talos.yaml +++ b/riptide_descriptions/config/talos.yaml @@ -6,7 +6,11 @@ base_link: [-0.14, 0.03, -0.09] # Vehicle mass properties mass: 31.998 -com: [-0.159, 0.042, -0.056] +# COM no IVC +# com: [-0.151, 0.041, -0.044] + +# COM w/ IVC +com: [-0.157, 0.040, -0.048] inertia: [0.72604692, 1.59322252, 1.65134701] # kg*m^2 hull_volume: .11 #make this real @@ -19,13 +23,13 @@ controller_overseer: controller: scaling_factor: 1000000 - active_force_mask: [2, 2, 2, 1, 1, 1] + active_force_mask: [2, 2, 2, 1, 1, 2] drag_enable: [1, 1, 0, 0, 0, 0] feed_forward: - #base_wrench: [0.03, 0.01, -5.28, 0.57, 1.77, -0.020] - base_wrench: [-1.0, -1.0, 0.0, -0.95, 2.1, 0.0] + base_wrench: [-0.35, -0.1, -3.00, 0.1, 2.4, 0.0] + # base_wrench: [-1.0, -1.0, 0.0, 0.0, 0.0, 0.0] auto_tune: #positional error threshold to begin autotuning @@ -122,14 +126,45 @@ controller: SMC_params: #[x,y,z,r,p,y] + # cameron tune + # #first order eta + # eta_order_0: [0.8, 1.5, 4.0, 0.2, 0.2, 1.3] + + # #second order eta + # eta_order_1: [0.1, 0.04, 0.07, 0.0, 0.0, 0.2] + + # #lambda + # lambda: [0.6, 0.2, 1.0, 10.0, 10.0, 0.35] + + # ethan tune 1 + # # eta_order_0: [0.8, 0.8, 10.0, 0.2, 0.2, 0.3] + # eta_order_0: [0.8, 1.0, 7.0, 0.2, 0.2, 0.3] + + # #second order eta + # # eta_order_1: [0.04, 0.04, 0.3, 0.0, 0.0, 0.0] + # eta_order_1: [0.04, 0.04, 0.05, 0.0, 0.0, 0.0] + + # #lambda + # lambda: [0.2, 0.2, 1.0, 1.0, 10.0, 0.35] + + # ethan tune 2 + # eta_order_0: [1.0, 1.0, 1.0, 0.0, 0.0, 1.0] + + # #second order eta + # # eta_order_1: [0.04, 0.04, 0.3, 0.0, 0.0, 0.0] + # eta_order_1: [0.3, 0.3, 0.0, 0.0, 0.0, 0.0] + + # #lambda + # lambda: [0.1, 0.1, 10.0, 10.0, 10.0, 0.35] + #first order eta - eta_order_0: [1.8, 0.5, 10.0, 0.2, 0.2, 0.3] + eta_order_0: [0.8, 1.0, 2.0, 0.2, 0.2, 0.3] #second order eta - eta_order_1: [0.08, 0.08, 0.3, 0.0, 0.0, 0.0] + eta_order_1: [0.04, 0.04, 0.3, 0.0, 0.0, 0.0] #lambda - lambda: [0.25, 0.25, 1.0, 10.0, 10.0, 0.35] + lambda: [0.2, 0.2, 0.2, 10.0, 10.0, 0.35] #format: 'a, m' for each degree of freedom #where: @@ -137,7 +172,7 @@ controller: # m = gravity well saturation error velocity_curve_params: [8.0, 0.8, #x 8.0, 0.8, #y - 10.0, 0.5, #z + 8.0, 0.5, #z 15.0, 1.0, #r 15.0, 1.0, #p 1.5, 0.3] #yaw @@ -151,11 +186,31 @@ controller: #all gains are in body frame #linear unit are newtons per meter, angular are newton meter per radian - #the default gains for normal operation - p_gains: [3.0, 18.0, 200.0, 3.5, 2.0, 2.5] - i_gains: [0.0, 0.0, 0.0, 0.01, 1.0, 0.0] - d_gains: [5.0, 5.0, 90.0, 1.0, 0.05, 0.9] + # #the default gains for normal operation + + # cameron tune + # p_gains: [3.0, 18.0, 200.0, 6.0, 2.0, 10.0] + # i_gains: [0.0, 0.0, 0.0, 0.01, 0.01, 1.0] + # d_gains: [5.0, 5.0, 90.0, 0.1, 0.1, 0.03] + + # ethan tune 1 + p_gains: [3.0, 18.0, 200.0, 3.0, 5.0, 6.0] + i_gains: [0.0, 0.0, 0.0, 0.01, 0.1, 0.01] + d_gains: [5.0, 5.0, 90.0, 5.0, 30.0, 0.9] + + # ethan tune 2 + # p_gains: [3.0, 18.0, 200.0, 5.0, 5.0, 1.0] + # i_gains: [0.0, 0.0, 0.0, 0.0, 0.1, 0.0] + # d_gains: [5.0, 5.0, 90.0, 0.5, -20.0, 0.0] + # p_gains: [3.0, 18.0, 200.0, 6.0, 1.0, 10.0] + # i_gains: [0.0, 0.0, 0.0, 0.01, 0.01, 1.0] + # d_gains: [5.0, 5.0, 90.0, 5.0, 0.01, 0.03] + + # #the default gains for normal operation + # p_gains: [3.0, 18.0, 200.0, 2.0, 3.5, 8.0] + # i_gains: [0.0, 0.0, 0.0, 0.0, 0.0, 2.0] + # d_gains: [5.0, 5.0, 90.0, 0.25, 0.25, 0.2] # the control value thresholds for each DOF -- body frame max_control_thresholds: [30, 30, 30, 15, 15, 15] reset_threshold: 0.5 @@ -289,27 +344,34 @@ poker: pose: [0.2286, 0.0127, -0.17145, 0, 0, 0] torpedoes: - pose: [0.048, 0.17200625, -0.0954, 0.0, 0.0, 0.0] #real z: -0.1524, real y: 0.17700625 + pose: [0.048, 0.18200625, -0.1324, 0.0, 0.0, 0.0] #real z: -0.1524, real y: 0.17700625 baseline: 0.0365125 droppers: - pose: [0.048, 0.17200625, -0.1501, 0.0, 0.0, 0.0] + pose: [0.0, 0.17200625, -0.1501, 0.0, 0.0, 0.0] hitch: pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +claw: + pose: [0.19, 0.0655, -0.3712, 0.0, 0.0, 0.0] + +magnet: + pose: [-0.2946, 0.0475, -0.391, 0.0, 0.0, 0.0] + cameras: [ { type: zed, name: ffc, - baseline: 0.12, # m. Distance between cameras (manually measured) - pose: [-0.0195, -0.0889, 0, 0, 0, 0] + baseline: 0.050, # m. Distance between cameras (manually measured) + pose: [-0.0195, -0.0889, 0.02, -0.01, 0.05, -0.0] }, { type: zed, name: dfc, baseline: 0.045, - pose: [-0.145, -0.080, -0.08, 1.5707, 1.5707, 0] + # pose: [-0.145, -0.080, -0.08, 1.5707, 1.5707, 0] + pose: [-0.24347, -0.065568, -0.15943, 1.5507, 1.5107, 0] } ] @@ -359,4 +421,4 @@ thrusters: [ type: 1, pose: [-0.134, 0.440, -0.244, 0.0, 0.0, 0.0] } -] +] \ No newline at end of file diff --git a/riptide_descriptions/urdf/robot_actuators.xacro b/riptide_descriptions/urdf/robot_actuators.xacro index 75d6b92..3dc2cf9 100644 --- a/riptide_descriptions/urdf/robot_actuators.xacro +++ b/riptide_descriptions/urdf/robot_actuators.xacro @@ -106,4 +106,20 @@ parent="${namespace}/origin" pose="${config['hitch']['pose']}" /> + + + + + + + + diff --git a/riptide_hardware/CMakeLists.txt b/riptide_hardware/CMakeLists.txt index ee07eda..93588a0 100644 --- a/riptide_hardware/CMakeLists.txt +++ b/riptide_hardware/CMakeLists.txt @@ -30,7 +30,9 @@ include_directories(include ${EIGEN3_INCLUDE_DIRS}) # Add executable for the DepthConverter add_executable(depth_converter src/depth_converter.cpp) -ament_target_dependencies(depth_converter +add_executable(pinger_broker src/pinger_broker.cpp) + +set(deps rclcpp geometry_msgs tf2_ros @@ -42,9 +44,13 @@ ament_target_dependencies(depth_converter nortek_dvl_msgs ) +ament_target_dependencies(depth_converter ${deps}) +ament_target_dependencies(pinger_broker ${deps}) + # Install the C++ executable install(TARGETS depth_converter + pinger_broker DESTINATION lib/${PROJECT_NAME} ) diff --git a/riptide_hardware/cfg/dfc_config.yaml b/riptide_hardware/cfg/dfc_config.yaml index a42807b..1502e50 100644 --- a/riptide_hardware/cfg/dfc_config.yaml +++ b/riptide_hardware/cfg/dfc_config.yaml @@ -1,13 +1,16 @@ /**: ros__parameters: general: - camera_name: "talos/dfc" + camera_model: zedxm serial_number: 51491740 - grab_resolution: 'HD1200' # For zedx/zedxm use HD1200, for zed2i use HD2K - optional_opencv_calibration_file: "/home/ros/zed_cals/zed_dfc_calibration2.yaml" + camera_id: 1 + camera_name: "talos/dfc" + grab_resolution: "HD1200" # For zedx/zedxm use HD1200, for zed2i use HD2K + optional_opencv_calibration_file: "/home/ros/zed_cals/dfc_charuco_test.yaml" + self_calib: false pub_resolution: "NATIVE" # The resolution used for image and depth map publishing. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission - pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM' - pub_frame_rate: 30.0 + #pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM' + grab_frame_rate: 15 pos_tracking: publish_tf: false debug: diff --git a/riptide_hardware/cfg/ffc_config.yaml b/riptide_hardware/cfg/ffc_config.yaml index cec331f..53b2ceb 100644 --- a/riptide_hardware/cfg/ffc_config.yaml +++ b/riptide_hardware/cfg/ffc_config.yaml @@ -1,13 +1,24 @@ /**: ros__parameters: general: + camera_model: zedxm + serial_number: 55348591 + camera_id: 0 camera_name: "talos/ffc" pub_resolution: "NATIVE" # The resolution used for image and depth map publishing. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM' - pub_frame_rate: 30.0 - optional_opencv_calibration_file: "/home/ros/zed_cals/taloszedxffc_cal5.yaml" + # pub_frame_rate: 30.0 + optional_opencv_calibration_file: "/home/ros/zed_cals/tirpak_ffc_charuco_3.yaml" grab_resolution: 'HD1200' + grab_frame_rate: 15 self_calib: false + sensors: + sensors_image_sync: true + # svo: + # svo_path: "/home/ros/svos/prequal/prequal1.svo2" + depth: + depth_mode: "NEURAL_PLUS" + max_depth: 4.0 pos_tracking: publish_tf: false debug: diff --git a/riptide_hardware/cfg/talos_ekf.yaml b/riptide_hardware/cfg/talos_ekf.yaml index 9e80c86..90c2bc4 100644 --- a/riptide_hardware/cfg/talos_ekf.yaml +++ b/riptide_hardware/cfg/talos_ekf.yaml @@ -9,7 +9,7 @@ publish_acceleration: true #make camera happy - # predict_to_current_time: true + predict_to_current_time: true # transform_time_offset: -0.1 diff --git a/riptide_hardware/launch/apriltag.launch.py b/riptide_hardware/launch/apriltag.launch.py index 41a034a..9b68174 100644 --- a/riptide_hardware/launch/apriltag.launch.py +++ b/riptide_hardware/launch/apriltag.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): namespace="apriltag", package='rclcpp_components', executable='component_container', - arguments=['--ros-args', '--log-level', 'error'], + arguments=['--ros-args', '--log-level', 'info'], composable_node_descriptions=[ ComposableNode( name='apriltag_36h11', diff --git a/riptide_hardware/launch/diagnostics.launch.py b/riptide_hardware/launch/diagnostics.launch.py index 3fc03d3..46a9b2a 100644 --- a/riptide_hardware/launch/diagnostics.launch.py +++ b/riptide_hardware/launch/diagnostics.launch.py @@ -77,7 +77,7 @@ def generate_launch_description(): electrical_monitor_node, firmware_monitor_node, voltage_monitor_node, - sensor_monitor_node, + # sensor_monitor_node, computer_monitor_node, aggregator ]) \ No newline at end of file diff --git a/riptide_hardware/launch/hardware.launch.py b/riptide_hardware/launch/hardware.launch.py index 1866813..76190ae 100644 --- a/riptide_hardware/launch/hardware.launch.py +++ b/riptide_hardware/launch/hardware.launch.py @@ -15,11 +15,6 @@ "launch", "copro_agent.launch.py", ) -diagnostics_launch_file = os.path.join( - get_package_share_directory('riptide_hardware2'), - "launch", "diagnostics.launch.py" -) - dvl_launch_file = os.path.join( get_package_share_directory('riptide_hardware2'), "launch", "dvl.launch.py" @@ -40,6 +35,11 @@ "launch", "apriltag.launch.py" ) +acoustics_launch_file = os.path.join( + get_package_share_directory('riptide_acoustics'), + "launch", "acoustics.launch.py" +) + def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('robot', default_value="tempest", @@ -49,14 +49,8 @@ def generate_launch_description(): PushRosNamespace( LC("robot") ), - # IncludeLaunchDescription( - # AnyLaunchDescriptionSource(copro_agent_launch_file), - # launch_arguments=[ - # ('robot', LC('robot')), - # ] - # ), IncludeLaunchDescription( - AnyLaunchDescriptionSource(diagnostics_launch_file), + AnyLaunchDescriptionSource(copro_agent_launch_file), launch_arguments=[ ('robot', LC('robot')), ] @@ -91,6 +85,12 @@ def generate_launch_description(): ('robot', LC('robot')), ] ), + IncludeLaunchDescription( + AnyLaunchDescriptionSource(acoustics_launch_file), + launch_arguments=[ + ('robot', LC('robot')), + ] + ), Node( package='riptide_hardware2', executable='simple_actuator_interface.py', @@ -113,10 +113,9 @@ def generate_launch_description(): ), Node( package='riptide_hardware2', - executable='pressure_monitor.py', - name='pressure_monitor', - output='screen', - parameters=[{"robot":LC('robot')}] + executable='pinger_broker', + name='pinger_broker', + output='screen' ) ], scoped=True) ]) diff --git a/riptide_hardware/launch/hardware_fake_dvl.launch.py b/riptide_hardware/launch/hardware_fake_dvl.launch.py index ac887a1..cb75afb 100644 --- a/riptide_hardware/launch/hardware_fake_dvl.launch.py +++ b/riptide_hardware/launch/hardware_fake_dvl.launch.py @@ -15,11 +15,6 @@ "launch", "copro_agent.launch.py", ) -diagnostics_launch_file = os.path.join( - get_package_share_directory('riptide_hardware2'), - "launch", "diagnostics.launch.py" -) - imu_launch_file = os.path.join( get_package_share_directory('riptide_imu'), "launch", "imu.launch.py" @@ -35,6 +30,11 @@ "launch", "apriltag.launch.py" ) +acoustics_launch_file = os.path.join( + get_package_share_directory('riptide_acoustics'), + "launch", "acoustics.launch.py" +) + # opbox_launch_file = os.path.join( # get_package_share_directory('opbox_ros_client'), # "launch", "opbox_ros_client.launch.py" @@ -56,14 +56,8 @@ def generate_launch_description(): # name='zenoh_router', # output='screen', # ), - # IncludeLaunchDescription( - # AnyLaunchDescriptionSource(copro_agent_launch_file), - # launch_arguments=[ - # ('robot', LC('robot')), - # ] - # ), IncludeLaunchDescription( - AnyLaunchDescriptionSource(diagnostics_launch_file), + AnyLaunchDescriptionSource(copro_agent_launch_file), launch_arguments=[ ('robot', LC('robot')), ] @@ -95,6 +89,13 @@ def generate_launch_description(): ('robot', LC('robot')), ] ), + + IncludeLaunchDescription( + AnyLaunchDescriptionSource(acoustics_launch_file), + launch_arguments=[ + ('robot', LC('robot')), + ] + ), # IncludeLaunchDescription( # AnyLaunchDescriptionSource(opbox_launch_file), # launch_arguments=[ @@ -115,10 +116,9 @@ def generate_launch_description(): ), Node( package='riptide_hardware2', - executable='pressure_monitor.py', - name='pressure_monitor', - output='screen', - parameters=[{"robot":LC('robot')}] - ) + executable='pinger_broker', + name='pinger_broker', + output='screen' + ) ], scoped=True) ]) diff --git a/riptide_hardware/launch/navigation.launch.py b/riptide_hardware/launch/navigation.launch.py index 30f3c1c..fad72ce 100644 --- a/riptide_hardware/launch/navigation.launch.py +++ b/riptide_hardware/launch/navigation.launch.py @@ -87,7 +87,7 @@ def evaluate_xacro(context, *args, **kwargs): # try: if robot == "talos": - nodes.append(get_zed_description("ffc", "zedx", robot, context, debug)) + nodes.append(get_zed_description("ffc", "zedxm", robot, context, debug)) nodes.append(get_zed_description("dfc", "zedxm", robot, context, debug)) if robot == "liltank": diff --git a/riptide_hardware/launch/zed.launch.py b/riptide_hardware/launch/zed.launch.py index 651e436..3cd2ad0 100644 --- a/riptide_hardware/launch/zed.launch.py +++ b/riptide_hardware/launch/zed.launch.py @@ -1,184 +1,65 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction, Shutdown +from launch.actions import DeclareLaunchArgument, GroupAction from launch_ros.actions import PushRosNamespace, ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode -from launch.conditions import IfCondition -from launch.substitutions import PythonExpression, LaunchConfiguration as LC +from launch.substitutions import LaunchConfiguration as LC from ament_index_python import get_package_share_directory import os - - def generate_launch_description(): + zed_wrapper_share = get_package_share_directory("zed_wrapper") + riptide_share = get_package_share_directory("riptide_hardware2") - # Define common configuration path - zed_config_path = os.path.join( - get_package_share_directory('zed_wrapper'), - "config", - "common_stereo.yaml" - ) + zed_common = os.path.join(zed_wrapper_share, "config", "common_stereo.yaml") + zedxm = os.path.join(zed_wrapper_share, "config", "zedxm.yaml") + ffc_cfg = os.path.join(riptide_share, "cfg", "ffc_config.yaml") + dfc_cfg = os.path.join(riptide_share, "cfg", "dfc_config.yaml") - # Paths for individual camera configurations - - # FFC Zed 2i - zed2i_camera_path = os.path.join( - get_package_share_directory('zed_wrapper'), - 'config', - 'zed2i.yaml' - ) - - # FFC Zed X - zedx_camera_path = os.path.join( - get_package_share_directory('zed_wrapper'), - 'config', - 'zedx.yaml' - ) - - # DFC Zed X Mini - zedxm_camera_path = os.path.join( - get_package_share_directory('zed_wrapper'), - 'config', - 'zedxm.yaml' - ) - - # FFC Overrides - ffc_config_path = os.path.join( - get_package_share_directory('riptide_hardware2'), - 'cfg', - 'ffc_config.yaml' + ffc_node = ComposableNode( + package="zed_components", + plugin="stereolabs::ZedCamera", + namespace="ffc", + name="zed_node", + parameters=[zed_common, zedxm, ffc_cfg], ) - # DFC Overrides - dfc_config_path = os.path.join( - get_package_share_directory('riptide_hardware2'), - 'cfg', - 'dfc_config.yaml' - ) - - tank_config_path = os.path.join( - get_package_share_directory('riptide_hardware2'), - 'cfg', - 'tank_ffc_config.yaml' + dfc_node = ComposableNode( + package="zed_components", + plugin="stereolabs::ZedCamera", + namespace="dfc", + name="zed_node", + parameters=[zed_common, zedxm, dfc_cfg], ) - - zed_compression_path = os.path.join( - get_package_share_directory('riptide_hardware2'), - 'cfg', - 'zed_compression.yaml' - ) - + return LaunchDescription([ - DeclareLaunchArgument( - name="robot", - default_value="tempest", - description="name of the robot" - ), - # Group actions under the robot namespace + DeclareLaunchArgument("robot", default_value="tempest"), + GroupAction([ PushRosNamespace(LC("robot")), ComposableNodeContainer( - name="ffc_container", - namespace="", - package="rclcpp_components", - executable="component_container", - output="screen", - respawn=True, - composable_node_descriptions=[ - # First ZED Node (FFC) - ComposableNode( - package="zed_components", - plugin="stereolabs::ZedCamera", - namespace="ffc", - name="zed_node", - parameters=[ - zed_config_path, - zedx_camera_path, - ffc_config_path, - ] - ), - ], - condition=IfCondition( - PythonExpression(["'", LC("robot"), "' == 'talos'"])) - ), - - ComposableNodeContainer( - name="dfc_container", + name="zed_container", namespace="", package="rclcpp_components", executable="component_container", output="screen", - respawn=True, - composable_node_descriptions=[ - # Second ZED Node (DFC) - ComposableNode( - package="zed_components", - plugin="stereolabs::ZedCamera", - namespace="dfc", - name="zed_node", - parameters=[ - zed_config_path, - zedxm_camera_path, - dfc_config_path, - ] - ), - ], - condition=IfCondition( - PythonExpression(["'", LC("robot"), "' == 'talos'"])) + respawn=False, + composable_node_descriptions=[ffc_node, dfc_node], ), - - # ComposableNodeContainer( - # name="ffc_container", - # namespace="/", - # package="rclcpp_components", - # executable="component_container", - # output="screen", - # respawn=True, - # composable_node_descriptions=[ - # # The tank camera - # ComposableNode( - # package="zed_components", - # plugin="stereolabs::ZedCamera", - # namespace="ffc", - # name="zed_node", - # parameters=[ - # # zedxm_camera_path, - # zed_config_path, - # tank_config_path, - # # zed_compression_path, - # {"general.camera_name": "liltank/ffc"}, - # {"mapping.clicked_point_topic": "/clicked_point"}, - # ] - # ), - # ], - # condition=IfCondition( - # PythonExpression(["'", LC("robot"), "' == 'liltank'"])) - # ), - - # Disabled for now as both are on by default - # # Launch the ZedManager node - # Node( - # package='riptide_hardware2', - # executable='zed_manager.py', - # name='ZedManager', - # output='screen' - # ) - - # Node( - # package='riptide_hardware2', - # executable='picture_taker.py', - # name='picture_taker', - # output='screen', - # parameters=[ - # {"robot_namespace": LC("robot")}, - # {"camera_name": "ffc"}, - # {"save_stereo": True}, - # {"save_split": True} - # ] - # ) - - ], scoped=True), - + Node( + package='riptide_hardware2', + executable='picture_taker.py', + name='picture_taker', + output='screen', + parameters=[ + {"robot_namespace": LC("robot")}, + {"camera_name": "ffc"}, + {"subscription_enabled": True}, + {"save_stereo": False}, + {"save_split": True} + ] + ) + ], scoped=True), ]) \ No newline at end of file diff --git a/riptide_hardware/riptide_hardware2/depth_converter.py b/riptide_hardware/riptide_hardware2/depth_converter.py index 1de7402..42147fc 100755 --- a/riptide_hardware/riptide_hardware2/depth_converter.py +++ b/riptide_hardware/riptide_hardware2/depth_converter.py @@ -51,7 +51,7 @@ def depthCb(self, msg): outMsg.header.frame_id = "odom" outMsg.pose.pose.position.z = msg.depth + addedDepth outMsg.pose.covariance[14] = msg.variance - outMsg.header.stamp = self.get_clock().now().to_msg() + outMsg.header.stamp = msg.header.stamp self.pub.publish(outMsg) diff --git a/riptide_hardware/riptide_hardware2/picture_taker.py b/riptide_hardware/riptide_hardware2/picture_taker.py index d41ed80..7514780 100644 --- a/riptide_hardware/riptide_hardware2/picture_taker.py +++ b/riptide_hardware/riptide_hardware2/picture_taker.py @@ -4,6 +4,7 @@ from rclpy.node import Node from sensor_msgs.msg import Image from std_srvs.srv import Trigger, SetBool +from rcl_interfaces.msg import SetParametersResult from cv_bridge import CvBridge import cv2 import os @@ -18,40 +19,37 @@ def __init__(self): self.declare_parameter('robot_namespace', 'talos') self.declare_parameter('camera_name', 'ffc') self.declare_parameter('save_directory', '/home/ros/cal_images') + self.declare_parameter('subscription_enabled', True) self.declare_parameter('save_stereo', False) self.declare_parameter('save_split', True) + self.add_on_set_parameters_callback(self.on_param_change) # Get parameter values - robot_namespace = self.get_parameter('robot_namespace').get_parameter_value().string_value - camera_name = self.get_parameter('camera_name').get_parameter_value().string_value + self.robot_namespace = self.get_parameter('robot_namespace').get_parameter_value().string_value + self.camera_name = self.get_parameter('camera_name').get_parameter_value().string_value self.save_directory = self.get_parameter('save_directory').get_parameter_value().string_value + self.subscription_enabled = self.get_parameter('subscription_enabled').get_parameter_value().bool_value self.save_stereo = self.get_parameter('save_stereo').get_parameter_value().bool_value self.save_split = self.get_parameter('save_split').get_parameter_value().bool_value # Build the full image topic path - self.image_topic = f"/{robot_namespace}/{camera_name}/zed_node/stereo_raw/image_raw_color" + self.image_topic = f"/{self.robot_namespace}/{self.camera_name}/zed_node/stereo_raw/image_raw_color" - # Create save directory if it doesn't exist - os.makedirs(self.save_directory, exist_ok=True) - - # Create subdirectories for split images if needed - if self.save_split: - os.makedirs(os.path.join(self.save_directory, 'left'), exist_ok=True) - os.makedirs(os.path.join(self.save_directory, 'right'), exist_ok=True) + self.create_save_dir() # Initialize CV bridge for image conversion self.bridge = CvBridge() # Store the latest image and subscription state self.latest_image = None - self.subscription_enabled = True self.image_subscriber = None # Log the topic we're subscribing to self.get_logger().info(f"Image topic: {self.image_topic}") # Create initial subscriber for image topic - self.create_image_subscriber() + if self.subscription_enabled: + self.create_image_subscriber() # Create service for capturing images self.capture_service = self.create_service( @@ -67,6 +65,60 @@ def __init__(self): self.enable_subscription_callback ) + def create_save_dir(self): + # Create save directory if it doesn't exist + os.makedirs(self.save_directory, exist_ok=True) + + # Create subdirectories for split images if needed + if self.save_split: + os.makedirs(os.path.join(self.save_directory, 'left'), exist_ok=True) + os.makedirs(os.path.join(self.save_directory, 'right'), exist_ok=True) + + def on_param_change(self, params): + changed = [] + for p in params: + if hasattr(self, p.name): + current = getattr(self, p.name) + if current != p.value: + setattr(self, p.name, p.value) + self.get_logger().info(f"Parameter '{p.name}' changed: {current} -> {p.value}") + changed.append(p) + + + self.handle_param_update(changed) + return SetParametersResult(successful=True) + + def handle_param_update(self, params): + topic_changed = update_save = update_sub = False + for p in params: + if p.name in ['camera_name', 'robot_namespace']: + topic_changed = True + elif p.name in ['save_directory', 'save_split']: + update_save = True + elif p.name == 'subscription_enabled': + update_sub = True + + if update_save: + self.create_save_dir() + + if topic_changed: + self.image_topic = f"/{self.robot_namespace}/{self.camera_name}/zed_node/stereo_raw/image_raw_color" + + if update_sub: + if self.subscription_enabled: + if self.image_subscriber is None: + self.create_image_subscriber() + else: + if topic_changed: + self.destroy_image_subscriber() + self.create_image_subscriber() + else: + self.destroy_image_subscriber() + else: + if topic_changed: + self.destroy_image_subscriber() + self.create_image_subscriber() + def create_image_subscriber(self): """Create the image subscriber""" if self.image_subscriber is None: @@ -99,12 +151,18 @@ def capture_image_callback(self, request, response): return response try: + + # if not self.save_stereo or self.save_split: + # return + # Generate filename with timestamp - timestamp = datetime.now().strftime("%Y%m%d_%H%M%S_%f")[:-3] + timestamp = datetime.now().strftime("%Y_%m_%d__%H_%M_%S_%f")[:-3] camera_name = self.get_parameter('camera_name').get_parameter_value().string_value saved_files = [] + self.create_save_dir() + # Save stereo image if enabled if self.save_stereo: stereo_filename = f"captured_image_{camera_name}_{timestamp}.png" @@ -112,7 +170,7 @@ def capture_image_callback(self, request, response): success = cv2.imwrite(stereo_filepath, self.latest_image, [cv2.IMWRITE_PNG_COMPRESSION, 0]) if success: - saved_files.append(f"stereo: {stereo_filename}") + saved_files.append(f"\nStereo: {stereo_filepath}") self.get_logger().info(f"Stereo image saved: {stereo_filepath}") else: self.get_logger().error("Failed to save stereo image") @@ -137,13 +195,13 @@ def capture_image_callback(self, request, response): right_success = cv2.imwrite(right_filepath, right_image, [cv2.IMWRITE_PNG_COMPRESSION, 0]) if left_success: - saved_files.append(f"left: {left_filename}") + saved_files.append(f"\nLeft: {left_filepath}") self.get_logger().info(f"Left image saved: {left_filepath}") else: self.get_logger().error("Failed to save left image") if right_success: - saved_files.append(f"right: {right_filename}") + saved_files.append(f"\nRight: {right_filepath}") self.get_logger().info(f"Right image saved: {right_filepath}") else: self.get_logger().error("Failed to save right image") diff --git a/riptide_hardware/src/pinger_broker.cpp b/riptide_hardware/src/pinger_broker.cpp new file mode 100644 index 0000000..6a8c736 --- /dev/null +++ b/riptide_hardware/src/pinger_broker.cpp @@ -0,0 +1,44 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int32.hpp" + +#include + + +using std::placeholders::_1; +using namespace std::chrono_literals; + +class PingerBroker : public rclcpp::Node { +public: + PingerBroker() : Node("pinger_broker") { + freqSelPub = this->create_publisher("ivc/pinger/set_freq_khz", 10); + freqSelSub = this->create_subscription("ivc/pinger/set_freq_broker_khz", 10, std::bind(&PingerBroker::freqSetCb, this, _1)); + + pubTimer = this->create_wall_timer(1000ms, std::bind(&PingerBroker::pubFreq, this)); + } + +private: + void freqSetCb(const std_msgs::msg::Int32& msg) { + freq_khz = msg.data; + } + + void pubFreq() { + std_msgs::msg::Int32 msg; + msg.data = freq_khz; + freqSelPub->publish(msg); + } + + rclcpp::Publisher::SharedPtr freqSelPub; + rclcpp::Subscription::SharedPtr freqSelSub; + + rclcpp::TimerBase::SharedPtr pubTimer; + + int freq_khz = 30; +}; + +int main(int argc, char * argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/riptide_msgs/srv/SetString.srv b/riptide_msgs/srv/SetString.srv new file mode 100644 index 0000000..7377b4c --- /dev/null +++ b/riptide_msgs/srv/SetString.srv @@ -0,0 +1,4 @@ +string data +--- +bool success +string message \ No newline at end of file diff --git a/riptide_msgs/srv/StartBinaryClassifier.srv b/riptide_msgs/srv/StartBinaryClassifier.srv new file mode 100644 index 0000000..0eb09cf --- /dev/null +++ b/riptide_msgs/srv/StartBinaryClassifier.srv @@ -0,0 +1,6 @@ +string class_name +string object1_name +string object2_name +--- +bool success +string message \ No newline at end of file