Acoustics

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(<dependency> 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()

riptide_acoustics/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>riptide_acoustics</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="spyalexs@gmail.com">hollis</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

riptide_acoustics/src/Acoustics.cpp

@@ -0,0 +1,98 @@
+#include <memory>
+#include <chrono>
+#include <functional>
+
+
+
+#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<std_msgs::msg::Float32>(
+      "acoustics/delta_t", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
+
+      //initialize tf buffer and tf listener
+      tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+      tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*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<std_msgs::msg::Float32>::SharedPtr subscription_;
+
+    //tf
+    std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
+    std::unique_ptr<tf2_ros::Buffer> 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<MinimalSubscriber>());
+  rclcpp::shutdown();
+  return 0;
+}

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)