diff --git a/riptide_acoustics/CMakeLists.txt b/riptide_acoustics/CMakeLists.txt new file mode 100644 index 0000000..9dc62ea --- /dev/null +++ b/riptide_acoustics/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(riptide_acoustics) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs 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) + +add_executable(run_acoustics src/Acoustics.cpp) + +ament_target_dependencies(run_acoustics +std_msgs +geometry_msgs +rclcpp +tf2 +tf2_ros) + +install(TARGETS run_acoustics + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/riptide_acoustics/package.xml b/riptide_acoustics/package.xml new file mode 100644 index 0000000..4c60ea9 --- /dev/null +++ b/riptide_acoustics/package.xml @@ -0,0 +1,18 @@ + + + + riptide_acoustics + 0.0.0 + TODO: Package description + hollis + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/riptide_acoustics/src/Acoustics.cpp b/riptide_acoustics/src/Acoustics.cpp new file mode 100644 index 0000000..2a7c99a --- /dev/null +++ b/riptide_acoustics/src/Acoustics.cpp @@ -0,0 +1,98 @@ +#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" + + +using namespace std::chrono_literals; +using std::placeholders::_1; + + +class MinimalSubscriber : public rclcpp::Node +{ + public: + MinimalSubscriber() + : Node("riptide_acoustics") + { + subscription_ = this->create_subscription( + "acoustics/delta_t", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); + + //initialize tf buffer and tf listener + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + portToStarboardTransformTimer = this->create_wall_timer(1s, std::bind(&MinimalSubscriber::getPortToStarboardTransform, this)); + } + + private: + void topic_callback(const std_msgs::msg::Float32::SharedPtr msg) const + { + 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!"); + return; + } + + RCLCPP_INFO(this->get_logger(), "X: %f", t.transform.translation.x); + + } + + void getPortToStarboardTransform() + { + //stamped msg + geometry_msgs::msg::TransformStamped stamped; + + //get transform from tf buffer + try { + + //TODO: change to port and starboard pod frames + stamped = tf_buffer_->lookupTransform("talos/pose_sensor_link", "talos/base_link", tf2::TimePointZero); + + portToStarboardTransform = stamped.transform; + + 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!"); + return; + } + + portToStarboardTransformTimer->cancel(); + } + + rclcpp::Subscription::SharedPtr subscription_; + + //tf + std::shared_ptr tf_listener_{nullptr}; + std::unique_ptr tf_buffer_; + + //port pod to starboard pod transform + geometry_msgs::msg::Transform portToStarboardTransform; + rclcpp::TimerBase::SharedPtr portToStarboardTransformTimer {nullptr}; + +}; + + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/riptide_descriptions/config/talos.yaml b/riptide_descriptions/config/talos.yaml index 0541bd2..fd76790 100644 --- a/riptide_descriptions/config/talos.yaml +++ b/riptide_descriptions/config/talos.yaml @@ -19,7 +19,7 @@ controller: feed_forward: #base_wrench: [0.3, 0.0, -6.0, 0.275, 0.050, -0.085] - base_wrench: [0.00, 0.0, -0.38, 0.058, 0.010, -0.020] + base_wrench: [0.00, 0.0, -6.0, 0.058, 0.010, -0.020] SMC: # drag = (k1 + k2 * abs(v) + k3 * exp(abs(v) / k4) )* sign(v)