From 0c605794f8003ac6dd9b9deb8046345011eb80b3 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Mon, 8 Sep 2025 17:20:57 +0200 Subject: [PATCH 01/24] refactor: renamed from encoder_read to read_encoders --- .../include/gripper_interface/gripper_interface_driver.hpp | 2 +- gripper_interface/src/gripper_interface_driver.cpp | 4 ++-- gripper_interface/src/gripper_interface_node.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp b/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp index 60416c2..8604cb8 100644 --- a/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp +++ b/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp @@ -69,7 +69,7 @@ class GripperInterfaceDriver { * order */ - std::vector encoder_read(); + std::vector read_encoders(); private: int bus_fd_; // File descriptor for I2C bus diff --git a/gripper_interface/src/gripper_interface_driver.cpp b/gripper_interface/src/gripper_interface_driver.cpp index 28d61be..420b9a1 100644 --- a/gripper_interface/src/gripper_interface_driver.cpp +++ b/gripper_interface/src/gripper_interface_driver.cpp @@ -44,7 +44,7 @@ void GripperInterfaceDriver::send_pwm( 1 + 3 * 2; // 3 thrusters * (1xMSB + 1xLSB) std::array i2c_data_array; - i2c_data_array.at(0) = 0x00; // "Start" byte + i2c_data_array[0] = 0x00; // "Start" byte for (std::size_t i = 1; i < 4; i++) { i2c_data_array[2 * i - 1] = @@ -100,7 +100,7 @@ void GripperInterfaceDriver::start_gripper() { } } -std::vector GripperInterfaceDriver::encoder_read() { +std::vector GripperInterfaceDriver::read_encoders() { constexpr std::size_t i2c_data_size = 6; // 6 bytes -> 3 angles. constexpr std::size_t num_angles = i2c_data_size / 2; std::array i2c_data_array; diff --git a/gripper_interface/src/gripper_interface_node.cpp b/gripper_interface/src/gripper_interface_node.cpp index bedeb3d..6bacefe 100644 --- a/gripper_interface/src/gripper_interface_node.cpp +++ b/gripper_interface/src/gripper_interface_node.cpp @@ -68,7 +68,7 @@ void GripperInterface::joy_callback( } void GripperInterface::encoder_angles_callback() { - std::vector angles = gripper_driver_->encoder_read(); + std::vector angles = gripper_driver_->read_encoders(); if (angles.empty()) { return; } From 92ae9de176cd35eae22d73c9bf6db86817b875d5 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Sat, 13 Sep 2025 19:45:40 +0200 Subject: [PATCH 02/24] refactor: added num_servos constant and simplified uint16 to uint8 conversion --- .../src/gripper_interface_driver.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/gripper_interface/src/gripper_interface_driver.cpp b/gripper_interface/src/gripper_interface_driver.cpp index 420b9a1..561f6c7 100644 --- a/gripper_interface/src/gripper_interface_driver.cpp +++ b/gripper_interface/src/gripper_interface_driver.cpp @@ -40,20 +40,19 @@ std::uint16_t GripperInterfaceDriver::joy_to_pwm(const double joy_value) { void GripperInterfaceDriver::send_pwm( const std::vector& pwm_values) { try { + constexpr std::size_t num_servos = 3; constexpr std::size_t i2c_data_size = - 1 + 3 * 2; // 3 thrusters * (1xMSB + 1xLSB) - std::array i2c_data_array; + 1 + num_servos * 2; // 3 thrusters * (1xMSB + 1xLSB) + std::array buf; - i2c_data_array[0] = 0x00; // "Start" byte + buf[0] = 0x00; // "Start" byte - for (std::size_t i = 1; i < 4; i++) { - i2c_data_array[2 * i - 1] = - static_cast((pwm_values[i] >> 8) & 0xFF); - i2c_data_array[2 * i] = static_cast(pwm_values[i] & 0xFF); + for (std::size_t i = 0; i < 3; i++) { + buf[1 + 2 * i] = static_cast((pwm_values[i] >> 8) & 0xFF); + buf[1 + 2 * i + 1] = static_cast(pwm_values[i] & 0xFF); } - if (write(bus_fd_, i2c_data_array.data(), i2c_data_size) != - i2c_data_size) { + if (write(bus_fd_, buf.data(), i2c_data_size) != i2c_data_size) { throw std::runtime_error(std::format( "Error: Failed to write to I2C device : {}", strerror(errno))); } From b906d7475a354a60ea1cec3e0a2457b089696f47 Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Sat, 31 Jan 2026 22:04:16 +0100 Subject: [PATCH 03/24] initial: initial commit --- .../gripper_interface/gripper_controller.hpp | 0 .../gripper_controller_ros.hpp | 8 +++ .../gripper_controller_utils.hpp | 0 .../gripper_interface_node.hpp | 59 ------------------- gripper_interface/src/gripper_controller.cpp | 0 .../src/gripper_controller_node.cpp | 10 ++++ .../src/gripper_controller_ros.cpp | 0 .../src/gripper_controller_utils.cpp | 0 8 files changed, 18 insertions(+), 59 deletions(-) create mode 100644 gripper_interface/include/gripper_interface/gripper_controller.hpp create mode 100644 gripper_interface/include/gripper_interface/gripper_controller_ros.hpp create mode 100644 gripper_interface/include/gripper_interface/gripper_controller_utils.hpp delete mode 100644 gripper_interface/include/gripper_interface/gripper_interface_node.hpp create mode 100644 gripper_interface/src/gripper_controller.cpp create mode 100644 gripper_interface/src/gripper_controller_node.cpp create mode 100644 gripper_interface/src/gripper_controller_ros.cpp create mode 100644 gripper_interface/src/gripper_controller_utils.cpp diff --git a/gripper_interface/include/gripper_interface/gripper_controller.hpp b/gripper_interface/include/gripper_interface/gripper_controller.hpp new file mode 100644 index 0000000..e69de29 diff --git a/gripper_interface/include/gripper_interface/gripper_controller_ros.hpp b/gripper_interface/include/gripper_interface/gripper_controller_ros.hpp new file mode 100644 index 0000000..0c93f5c --- /dev/null +++ b/gripper_interface/include/gripper_interface/gripper_controller_ros.hpp @@ -0,0 +1,8 @@ +#ifndef GRIPPER_CONTROLLER_DP__GRIPPER_CONTROLLER_ROS_HPP_ +#define GRIPPER_CONTROLLER_DP__GRIPPER_CONTROLLER_ROS_HPP_ + + +class GripperControllerNode() : public rclcpp::Node { + public: + GripperControllerNode(); +}; diff --git a/gripper_interface/include/gripper_interface/gripper_controller_utils.hpp b/gripper_interface/include/gripper_interface/gripper_controller_utils.hpp new file mode 100644 index 0000000..e69de29 diff --git a/gripper_interface/include/gripper_interface/gripper_interface_node.hpp b/gripper_interface/include/gripper_interface/gripper_interface_node.hpp deleted file mode 100644 index 627e26e..0000000 --- a/gripper_interface/include/gripper_interface/gripper_interface_node.hpp +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef GRIPPER_INTERFACE_HPP -#define GRIPPER_INTERFACE_HPP - -#include -#include -#include -#include -#include -#include -#include -#include -#include "gripper_interface/gripper_interface_driver.hpp" - -class GripperInterface : public rclcpp::Node { - public: - GripperInterface(); - - private: - /** - * @brief Extract parameters from the config file. - */ - void extract_parameters(); - - void set_publisher_and_subsribers(); - - /** - * @brief Callback function for the joystick message. - * @param msg The joystick message. - */ - void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg); - - void encoder_angles_callback(); - - /** - * @brief Convert a vector of PWM values to a ROS message. - * @param vec The vector of PWM values. - * @return The ROS message. - */ - std_msgs::msg::Int16MultiArray vec_to_msg(std::vector vec); - - std::string joy_topic_; - std::string pwm_topic_; - std::string joint_state_topic_; - int i2c_bus_; - int i2c_address_; - int pwm_gain_; - int pwm_idle_; - - std::unique_ptr gripper_driver_; - - rclcpp::Publisher::SharedPtr joint_state_pub_; - - rclcpp::Subscription::SharedPtr joy_sub_; - rclcpp::Publisher::SharedPtr pwm_pub_; - rclcpp::TimerBase::SharedPtr watchdog_timer_; - rclcpp::Time last_msg_time_; -}; - -#endif // GRIPPER_INTERFACE_HPP diff --git a/gripper_interface/src/gripper_controller.cpp b/gripper_interface/src/gripper_controller.cpp new file mode 100644 index 0000000..e69de29 diff --git a/gripper_interface/src/gripper_controller_node.cpp b/gripper_interface/src/gripper_controller_node.cpp new file mode 100644 index 0000000..a9bd72c --- /dev/null +++ b/gripper_interface/src/gripper_controller_node.cpp @@ -0,0 +1,10 @@ +#include "vortex-gripper/gripper_controller_ros.hpp" + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started Gripper Controller Node"); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/gripper_interface/src/gripper_controller_ros.cpp b/gripper_interface/src/gripper_controller_ros.cpp new file mode 100644 index 0000000..e69de29 diff --git a/gripper_interface/src/gripper_controller_utils.cpp b/gripper_interface/src/gripper_controller_utils.cpp new file mode 100644 index 0000000..e69de29 From 0fa7bd348ff590151c0d7ee727fd8a7898fecb86 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Sun, 1 Feb 2026 13:28:17 +0100 Subject: [PATCH 04/24] refactor: remove exceptions, send data in little endian --- .../gripper_interface_driver.hpp | 13 +- .../src/gripper_interface_driver.cpp | 123 +++++++----------- 2 files changed, 54 insertions(+), 82 deletions(-) diff --git a/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp b/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp index 8604cb8..067802c 100644 --- a/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp +++ b/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp @@ -37,6 +37,13 @@ class GripperInterfaceDriver { int pwm_gain, int pwm_idle); + + /** + * @brief init i2c + * @return 0 on success + */ + int init_i2c(); + /** * @brief Convert joystick value to PWM value. * @param joy_value The joystick value. @@ -48,19 +55,19 @@ class GripperInterfaceDriver { * @brief Send PWM values to the gripper. * @param pwm_values The PWM values. */ - void send_pwm(const std::vector& pwm_values); + int send_pwm(const std::vector& pwm_values); /** * @brief Start gripper by sending 0x02 first byte * @param None */ - void start_gripper(); + int start_gripper(); /** * @brief Stop gripper by sending 0x01 first byte * @param None */ - void stop_gripper(); + int stop_gripper(); /** * @brief Reads the raw angle of each encoder diff --git a/gripper_interface/src/gripper_interface_driver.cpp b/gripper_interface/src/gripper_interface_driver.cpp index 561f6c7..de3c01e 100644 --- a/gripper_interface/src/gripper_interface_driver.cpp +++ b/gripper_interface/src/gripper_interface_driver.cpp @@ -8,22 +8,21 @@ GripperInterfaceDriver::GripperInterfaceDriver(short i2c_bus, : i2c_bus_(i2c_bus), i2c_address_(i2c_address), pwm_gain_(pwm_gain), - pwm_idle_(pwm_idle) { + pwm_idle_(pwm_idle) {} + +int GripperInterfaceDriver::init_i2c() { std::string i2c_filename = std::format("/dev/i2c-{}", i2c_bus_); bus_fd_ = open(i2c_filename.c_str(), O_RDWR); // Open the I2C bus for reading and writing (O_RDWR) if (bus_fd_ < 0) { - throw std::runtime_error( - std::format("ERROR: Failed to open I2C bus {} : {}", i2c_bus_, - strerror(errno))); + return bus_fd_; } if (ioctl(bus_fd_, I2C_SLAVE, i2c_address_) < 0) { - throw std::runtime_error(std::format("Failed to open I2C bus {} : {}", - i2c_bus_, strerror(errno))); - return; + return -1; } + return 0; } GripperInterfaceDriver::~GripperInterfaceDriver() { @@ -37,66 +36,41 @@ std::uint16_t GripperInterfaceDriver::joy_to_pwm(const double joy_value) { return static_cast(pwm_idle_ + pwm_gain_ * joy_value); } -void GripperInterfaceDriver::send_pwm( +int GripperInterfaceDriver::send_pwm( const std::vector& pwm_values) { - try { - constexpr std::size_t num_servos = 3; - constexpr std::size_t i2c_data_size = - 1 + num_servos * 2; // 3 thrusters * (1xMSB + 1xLSB) - std::array buf; - - buf[0] = 0x00; // "Start" byte - - for (std::size_t i = 0; i < 3; i++) { - buf[1 + 2 * i] = static_cast((pwm_values[i] >> 8) & 0xFF); - buf[1 + 2 * i + 1] = static_cast(pwm_values[i] & 0xFF); - } - - if (write(bus_fd_, buf.data(), i2c_data_size) != i2c_data_size) { - throw std::runtime_error(std::format( - "Error: Failed to write to I2C device : {}", strerror(errno))); - } - } catch (const std::exception& e) { - spdlog::error("ERROR: Failed to send PWM values - {}", e.what()); - } catch (...) { - spdlog::error("ERROR: Failed to send PWM values - unknown error"); + constexpr std::size_t num_servos = 3; + constexpr std::size_t i2c_data_size = + 1 + num_servos * 2; // 3 thrusters * (1xMSB + 1xLSB) + std::array buf; + + buf[0] = 0x00; // "Start" byte + + std::memcpy(buf.data(), pwm_values.data(), i2c_data_size); + + if (write(bus_fd_, buf.data(), i2c_data_size) != i2c_data_size) { + return -1; } + return 0; } -void GripperInterfaceDriver::stop_gripper() { - try { - constexpr std::size_t i2c_data_size = 1; - std::uint8_t i2c_message = 0x01; - - if (write(bus_fd_, &i2c_message, i2c_data_size) != i2c_data_size) { - throw std::runtime_error(std::format( - "Error: Failed to write to I2C device : {}", strerror(errno))); - } - } catch (const std::exception& e) { - spdlog::error("ERROR: Failed to send stop gripper command - {}", - e.what()); - } catch (...) { - spdlog::error( - "ERROR: Failed to send stop gripper command - unknown error"); +int GripperInterfaceDriver::stop_gripper() { + constexpr std::size_t i2c_data_size = 1; + std::uint8_t i2c_message = 0x01; + + if (write(bus_fd_, &i2c_message, i2c_data_size) != i2c_data_size) { + return -1; } + return 0; } -void GripperInterfaceDriver::start_gripper() { - try { - constexpr std::size_t i2c_data_size = 1; - std::uint8_t i2c_message = 0x02; - - if (write(bus_fd_, &i2c_message, i2c_data_size) != i2c_data_size) { - throw std::runtime_error(std::format( - "Error: Failed to write to I2C device : {}", strerror(errno))); - } - } catch (const std::exception& e) { - spdlog::error("ERROR: Failed to send start gripper command - {}", - e.what()); - } catch (...) { - spdlog::error( - "ERROR: Failed to send start gripper command - unknown error"); +int GripperInterfaceDriver::start_gripper() { + constexpr std::size_t i2c_data_size = 1; + std::uint8_t i2c_message = 0x02; + + if (write(bus_fd_, &i2c_message, i2c_data_size) != i2c_data_size) { + return -1; } + return 0; } std::vector GripperInterfaceDriver::read_encoders() { @@ -106,25 +80,16 @@ std::vector GripperInterfaceDriver::read_encoders() { std::vector encoder_angles; encoder_angles.reserve(num_angles); - try { - if (read(bus_fd_, i2c_data_array.data(), i2c_data_size) != - static_cast(i2c_data_size)) { - throw std::runtime_error(std::format( - "Error: Failed to read from I2C device: {}", strerror(errno))); - } - - for (std::size_t i = 0; i < num_angles; ++i) { - std::array pair = {i2c_data_array[2 * i], - i2c_data_array[2 * i + 1]}; - std::uint16_t raw_angle = i2c_to_encoder_angles(pair); - encoder_angles.push_back(raw_angle_to_radians(raw_angle)); - } - - return encoder_angles; - } catch (const std::exception& e) { - spdlog::error("ERROR: Failed to read encoder values - {}", e.what()); - } catch (...) { - spdlog::error("ERROR: Failed to read encoder values - unknown error"); + if (read(bus_fd_, i2c_data_array.data(), i2c_data_size) != i2c_data_size) { + return {}; + } + + for (std::size_t i = 0; i < num_angles; ++i) { + std::array pair = {i2c_data_array[2 * i], + i2c_data_array[2 * i + 1]}; + std::uint16_t raw_angle = i2c_to_encoder_angles(pair); + encoder_angles.push_back(raw_angle_to_radians(raw_angle)); } - return {}; + + return encoder_angles; } From 7921014d2e97b5c583bcfc0f64cf21d9be1dfd76 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Sun, 1 Feb 2026 14:46:06 +0100 Subject: [PATCH 05/24] fix: wrong offset in memcpy --- gripper_interface/src/gripper_interface_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gripper_interface/src/gripper_interface_driver.cpp b/gripper_interface/src/gripper_interface_driver.cpp index de3c01e..a7cbb81 100644 --- a/gripper_interface/src/gripper_interface_driver.cpp +++ b/gripper_interface/src/gripper_interface_driver.cpp @@ -45,7 +45,7 @@ int GripperInterfaceDriver::send_pwm( buf[0] = 0x00; // "Start" byte - std::memcpy(buf.data(), pwm_values.data(), i2c_data_size); + std::memcpy(buf.data() + 1, pwm_values.data(), i2c_data_size - 1); if (write(bus_fd_, buf.data(), i2c_data_size) != i2c_data_size) { return -1; From af50e2c288623867e7c358fce4a212eb2a7a322a Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Mon, 9 Feb 2026 20:42:34 +0100 Subject: [PATCH 06/24] moved controller module related files originally made in interface folder as well as removed the previously added dependencies as consequence of making the controller module within the interface --- gripper_interface/CMakeLists.txt | 2 ++ .../include/gripper_interface/gripper_controller.hpp | 0 .../gripper_interface/gripper_controller_ros.hpp | 8 -------- .../gripper_interface/gripper_controller_utils.hpp | 0 gripper_interface/package.xml | 3 ++- gripper_interface/src/gripper_controller.cpp | 0 gripper_interface/src/gripper_controller_node.cpp | 10 ---------- gripper_interface/src/gripper_controller_ros.cpp | 0 gripper_interface/src/gripper_controller_utils.cpp | 0 9 files changed, 4 insertions(+), 19 deletions(-) delete mode 100644 gripper_interface/include/gripper_interface/gripper_controller.hpp delete mode 100644 gripper_interface/include/gripper_interface/gripper_controller_ros.hpp delete mode 100644 gripper_interface/include/gripper_interface/gripper_controller_utils.hpp delete mode 100644 gripper_interface/src/gripper_controller.cpp delete mode 100644 gripper_interface/src/gripper_controller_node.cpp delete mode 100644 gripper_interface/src/gripper_controller_ros.cpp delete mode 100644 gripper_interface/src/gripper_controller_utils.cpp diff --git a/gripper_interface/CMakeLists.txt b/gripper_interface/CMakeLists.txt index 246f39c..d17cb2a 100644 --- a/gripper_interface/CMakeLists.txt +++ b/gripper_interface/CMakeLists.txt @@ -16,6 +16,8 @@ find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) +find_package(geometry_msgs REQUIRED) + add_executable(${PROJECT_NAME}_node src/gripper_interface_node.cpp diff --git a/gripper_interface/include/gripper_interface/gripper_controller.hpp b/gripper_interface/include/gripper_interface/gripper_controller.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/gripper_interface/include/gripper_interface/gripper_controller_ros.hpp b/gripper_interface/include/gripper_interface/gripper_controller_ros.hpp deleted file mode 100644 index 0c93f5c..0000000 --- a/gripper_interface/include/gripper_interface/gripper_controller_ros.hpp +++ /dev/null @@ -1,8 +0,0 @@ -#ifndef GRIPPER_CONTROLLER_DP__GRIPPER_CONTROLLER_ROS_HPP_ -#define GRIPPER_CONTROLLER_DP__GRIPPER_CONTROLLER_ROS_HPP_ - - -class GripperControllerNode() : public rclcpp::Node { - public: - GripperControllerNode(); -}; diff --git a/gripper_interface/include/gripper_interface/gripper_controller_utils.hpp b/gripper_interface/include/gripper_interface/gripper_controller_utils.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/gripper_interface/package.xml b/gripper_interface/package.xml index 303a612..ef4357d 100644 --- a/gripper_interface/package.xml +++ b/gripper_interface/package.xml @@ -11,7 +11,8 @@ rclcpp sensor_msgs - + geometry_msgs + ament_cmake diff --git a/gripper_interface/src/gripper_controller.cpp b/gripper_interface/src/gripper_controller.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/gripper_interface/src/gripper_controller_node.cpp b/gripper_interface/src/gripper_controller_node.cpp deleted file mode 100644 index a9bd72c..0000000 --- a/gripper_interface/src/gripper_controller_node.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "vortex-gripper/gripper_controller_ros.hpp" - -int main(int argc, char **argv) -{ - rclcpp::init(argc, argv); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started Gripper Controller Node"); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/gripper_interface/src/gripper_controller_ros.cpp b/gripper_interface/src/gripper_controller_ros.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/gripper_interface/src/gripper_controller_utils.cpp b/gripper_interface/src/gripper_controller_utils.cpp deleted file mode 100644 index e69de29..0000000 From 92e2ac623a9b2e32f31587dd1c94ad01ef474a02 Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Mon, 9 Feb 2026 20:45:11 +0100 Subject: [PATCH 07/24] Moved controller module to new, dedicated folder, added contents were originally in the gripper_interface/ --- gripper_controller/CMakeLists.txt | 40 ++++++++++++++ .../gripper_controller/gripper_controller.hpp | 0 .../gripper_controller_ros.hpp | 53 +++++++++++++++++++ .../gripper_controller_utils.hpp | 0 gripper_controller/package.xml | 0 gripper_controller/src/gripper_controller.cpp | 0 .../src/gripper_controller_node.cpp | 10 ++++ .../src/gripper_controller_ros.cpp | 0 .../src/gripper_controller_utils.cpp | 0 9 files changed, 103 insertions(+) create mode 100644 gripper_controller/CMakeLists.txt create mode 100644 gripper_controller/include/gripper_controller/gripper_controller.hpp create mode 100644 gripper_controller/include/gripper_controller/gripper_controller_ros.hpp create mode 100644 gripper_controller/include/gripper_controller/gripper_controller_utils.hpp create mode 100644 gripper_controller/package.xml create mode 100644 gripper_controller/src/gripper_controller.cpp create mode 100644 gripper_controller/src/gripper_controller_node.cpp create mode 100644 gripper_controller/src/gripper_controller_ros.cpp create mode 100644 gripper_controller/src/gripper_controller_utils.cpp diff --git a/gripper_controller/CMakeLists.txt b/gripper_controller/CMakeLists.txt new file mode 100644 index 0000000..9b0796c --- /dev/null +++ b/gripper_controller/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.8) +project(gripper_controller) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include_directories(include) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) + + +add_executable(${PROJECT_NAME}_node + src/gripper_controller_node.cpp + src/gripper_controller.cpp +) + + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + geometry_msgs +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/gripper_controller/include/gripper_controller/gripper_controller.hpp b/gripper_controller/include/gripper_controller/gripper_controller.hpp new file mode 100644 index 0000000..e69de29 diff --git a/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp b/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp new file mode 100644 index 0000000..92a6fae --- /dev/null +++ b/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp @@ -0,0 +1,53 @@ +#ifndef GRIPPER_CONTROLLER_DP__GRIPPER_CONTROLLER_ROS_HPP_ +#define GRIPPER_CONTROLLER_DP__GRIPPER_CONTROLLER_ROS_HPP_ + +#include +#include +#include + + +class GripperControllerNode() : public rclcpp::Node { + public: + GripperControllerNode(); + explicit GripperControllerNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions() + ); + private: + // @brief Set the subscribers and publishers + void set_subscribers_and_publisher(); + + //TODO: Define the callback for the GripperReference node after it's been made + // + // + // + // + // + // + + //TODO: You won't be needing this one (PoseStamped) nephew, consult Cyprian if remove + + // @brief Callback for the reference topic + // @param msg The reference message + void reference_callback( + const geometry_msgs::msg::PoseStamped::SharedPtr msg); + + //TODO: Nor will you probably need this (PoseWithCovStamped) one too, consult Cyprian if remove + + // @brief Callback for the pose topic + // @pram msg The pose message + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + + rclcpp::Publisher::SharedPtr + reference_pub_; + + rclcpp::Subscription::SharedPtr + reference_sub_; + + rclcpp::Subscription Date: Mon, 9 Feb 2026 20:49:41 +0100 Subject: [PATCH 08/24] made a guidance module which closely relates to reference_filter_dp in vortex_auv repo: vortex-auv/guidance/reference_filter_dp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit -Rather than following the recipe to construct the model after p. 337 EQ 12.11 and 12.12 in Thor I. Fossens book, dimensions and names have been changed to match the p.336 EQ 12.5 and 12.6. -Notice several TODO's for future work and consultation with Cyp & Jøg --- guidance/CMakeLists.txt | 77 ++++ guidance/README.md | 42 ++ .../gripper_eigen_typedefs.hpp | 21 + .../gripper_reference_filter.hpp | 44 ++ .../gripper_reference_filter_ros.hpp | 108 +++++ guidance/package.xml | 24 + guidance/src/gripper_reference_filter.cpp | 38 ++ guidance/src/gripper_reference_filter_ros.cpp | 418 ++++++++++++++++++ guidance/test/CMakeLists.txt | 21 + .../test/test_gripper_reference_filter.cpp | 58 +++ 10 files changed, 851 insertions(+) create mode 100644 guidance/CMakeLists.txt create mode 100644 guidance/README.md create mode 100644 guidance/include/gripper_reference_filter/gripper_eigen_typedefs.hpp create mode 100644 guidance/include/gripper_reference_filter/gripper_reference_filter.hpp create mode 100644 guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp create mode 100644 guidance/package.xml create mode 100644 guidance/src/gripper_reference_filter.cpp create mode 100644 guidance/src/gripper_reference_filter_ros.cpp create mode 100644 guidance/test/CMakeLists.txt create mode 100644 guidance/test/test_gripper_reference_filter.cpp diff --git a/guidance/CMakeLists.txt b/guidance/CMakeLists.txt new file mode 100644 index 0000000..d775446 --- /dev/null +++ b/guidance/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.8) +project(reference_filter_dp) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +include_directories(include) + +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED + src/reference_filter.cpp + src/reference_filter_ros.cpp) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + rclcpp_action + geometry_msgs + nav_msgs + Eigen3 + vortex_msgs + vortex_utils + vortex_utils_ros + spdlog + fmt +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "ReferenceFilterNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_export_targets(export_${LIB_NAME}) + +install(TARGETS ${LIB_NAME} + EXPORT export_${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_package() diff --git a/guidance/README.md b/guidance/README.md new file mode 100644 index 0000000..0ccfbf2 --- /dev/null +++ b/guidance/README.md @@ -0,0 +1,42 @@ +## Reference filter + +The reference filter (or reference model) is of third order and provides reference for pose, velocity and acceleration. So that $x_d := [\eta_d^\top, \dot{\eta}_d^\top, \ddot{\eta}_d^\top] \in \mathbb{R}^{3n}$. + +The state-space representation is, according to (Fossen, 2021), +```math +\dot{x}_d = A_d x_d + B_d r +``` +where +```math +A_d = \begin{bmatrix} +0_{n\times n} & I_n & 0_{n\times n} \\ +0_{n\times n} & 0_{n \times n} & I_n \\ +-\Omega^3 & -(2 \Delta + I_n) \Omega^2 & -(2 \Delta + I_n) \Omega +\end{bmatrix} +``` +and +```math +B_d = \begin{bmatrix} +0_{n \times n} \\ +0_{n \times n} \\ +\Omega^3 +\end{bmatrix}. +``` + +The steady-state pose for a constant reference signal $r$ is +```math +\lim_{t \to \infty} \eta_d = r +``` + +The state at the next time step is calculated using forward Euler integration +```math +x_{i+1} = x_i + \dot{x}_i * dt +``` + +## Action Server +The action server is responsible for handling goal requests and publishing guidance commands. The server will always prioritize new goal request, and will abort ongoing request when getting a new request. The action definition can be found [here](https://github.com/vortexntnu/vortex-msgs/blob/main/action/ReferenceFilterWaypoint.action). + +- Action name: /reference_filter +- Goal type: PoseStamped +- Result type: bool +- Guidance topic: /dp/reference diff --git a/guidance/include/gripper_reference_filter/gripper_eigen_typedefs.hpp b/guidance/include/gripper_reference_filter/gripper_eigen_typedefs.hpp new file mode 100644 index 0000000..44e1dc0 --- /dev/null +++ b/guidance/include/gripper_reference_filter/gripper_eigen_typedefs.hpp @@ -0,0 +1,21 @@ +/** + * @file gripper_eigen_typedefs.hpp + * @brief Contains Eigen typedefs used in this package. + */ + +#ifndef GRIPPER_REFERENCE_FILTER__GRIPPER_EIGEN_TYPEDEFS_HPP_ +#define GRIPPER_REFERENCE_FILTER__GRIPPER_EIGEN_TYPEDEFS_HPP_ + +#include + +namespace Eigen { + +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Matrix6x2d; +typedef Eigen::Matrix Matrix2d; +typedef Eigen::Matrix Vector2d; +typedef Eigen::Matrix Vector6d; + +} // namespace Eigen + +#endif // GRIPPER_REFERENCE_FILTER__GRIPPER_EIGEN_TYPEDEFS_HPP_ diff --git a/guidance/include/gripper_reference_filter/gripper_reference_filter.hpp b/guidance/include/gripper_reference_filter/gripper_reference_filter.hpp new file mode 100644 index 0000000..38de432 --- /dev/null +++ b/guidance/include/gripper_reference_filter/gripper_reference_filter.hpp @@ -0,0 +1,44 @@ +#ifndef GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_HPP_ +#define GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_HPP_ + +#include "gripper_reference_filter/gripper_eigen_typedefs.hpp" + +namespace vortex::guidance { + +struct GripperReferenceFilterParams { + Eigen::Vector2d omega = Eigen::Vector2d::Zero(); + Eigen::Vector2d zeta = Eigen::Vector2d::Zero(); +}; + +class GripperReferenceFilter { + public: + explicit GripperReferenceFilter(const ReferenceFilterParams& params); + + // @brief Calculate the state derivative + // @param x The state vector 6x1 + // @param r The reference vector 2x1 + // @return The state derivative 6x1 + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 336 eq: 12.5 + Eigen::Vector2d calculate_x_dot(const Eigen::Vector6d& x, + const Eigen::Vector2d& r); + + // @brief Calculate the state transition matrix + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 336 eq: 12.6 + void calculate_Ad(const Eigen::Vector2d& omega, + const Eigen::Vector2d& zeta); + + // @brief Calculate the input matrix + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 336 eq: 12.6 + void calculate_Bd(const Eigen::Vector2d& omega); + + private: + Eigen::Matrix6d Ad_; + Eigen::Matrix6x2d Bd_; +}; + +} // namespace vortex::guidance + +#endif // GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_HPP_ diff --git a/guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp b/guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp new file mode 100644 index 0000000..3e59745 --- /dev/null +++ b/guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp @@ -0,0 +1,108 @@ +#ifndef GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_HPP_ +#define GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_HPP_ + +#include +#include +#include +#include //TODO: Need to change this to gripper_reference_filter_waypoint when made in vortex_msgs +#include +#include "gripper_reference_filter/gripper_eigen_typedefs.hpp" +#include "gripper_reference_filter/gripper_reference_filter.hpp" + +namespace vortex::guidance { + +class GripperReferenceFilterNode : public rclcpp::Node { + public: + explicit GripperReferenceFilterNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + private: + // @brief Set the subscribers and publishers + void set_subscribers_and_publisher(); + + // @brief Set the action server + void set_action_server(); + + // @brief Initializes the reference filter with ROS parameters. + void set_refererence_filter(); + + + // @brief Handle the goal request + // @param uuid The goal UUID + // @param goal The goal message + // @return The goal response + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr< + const vortex_msgs::action::ReferenceFilterWaypoint::Goal> goal); // TODO: make a GripperReferenceFilterWaypoint in vortex_msgs + + + // @brief Handle the cancel request + // @param goal_handle The goal handle + // @return The cancel response + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr> goal_handle); + + // @brief Handle the accepted request + // @param goal_handle The goal handle + void handle_accepted( + const std::shared_ptr> goal_handle); + + // @brief Execute the goal + // @param goal_handle The goal handle + void execute( + const std::shared_ptr> goal_handle); + + Eigen::Vector6d fill_reference_state(); + + Eigen::Vector2d fill_reference_goal(const geometry_msgs::msg::Pose& goal); + + Eigen::Vector2d apply_mode_logic(const Eigen::Vector2d& r_in, uint8_t mode); + + void publish_hold_reference(); + + vortex_msgs::msg::ReferenceFilter fill_reference_msg(); + + rclcpp_action::Server< + vortex_msgs::action::ReferenceFilterWaypoint>::SharedPtr action_server_; + + std::unique_ptr reference_filter_{}; + + rclcpp::Publisher::SharedPtr + reference_pub_; + + rclcpp::Subscription::SharedPtr //TODO: PoseStamped should become GripperState topic eventually + reference_sub_; + + + + rclcpp::TimerBase::SharedPtr reference_pub_timer_; + + std::chrono::milliseconds time_step_{}; + + + + // x is [nu, nu_dot] (ref. page 336 in Fossen, 2021 + // nu is 2 degree of freedom (roll of the gripper AND pinch) + Eigen::Vector6d x_; + + // The reference signal vector with 2 degrees of freedom [nu] + Eigen::Vector2d r_; + + std::mutex mutex_; + + rclcpp_action::GoalUUID preempted_goal_id_; + + std::shared_ptr> //TODO: this needs to be changed to GripperReferenceFilterWaypoint + goal_handle_; + + rclcpp::CallbackGroup::SharedPtr cb_group_; +}; + +} // namespace vortex::guidance + +#endif // GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_HPP_ diff --git a/guidance/package.xml b/guidance/package.xml new file mode 100644 index 0000000..40793be --- /dev/null +++ b/guidance/package.xml @@ -0,0 +1,24 @@ + + + + reference_filter_dp + 0.0.0 + Reference model for the DP controller + andeshog + MIT + + ament_cmake + + rclcpp + rclcpp_action + geometry_msgs + vortex_msgs + vortex_utils + vortex_utils_ros + nav_msgs + eigen + + + ament_cmake + + diff --git a/guidance/src/gripper_reference_filter.cpp b/guidance/src/gripper_reference_filter.cpp new file mode 100644 index 0000000..a015d7b --- /dev/null +++ b/guidance/src/gripper_reference_filter.cpp @@ -0,0 +1,38 @@ +#include "gripper_reference_filter/gripper_reference_filter.hpp" + +namespace vortex::guidance { + +GripperReferenceFilter::GripperReferenceFilter(const GripperReferenceFilterParams& params) { + calculate_Ad(params.omega, params.zeta); + calculate_Bd(params.omega); +} + +Eigen::Vector6d GripperReferenceFilter::calculate_x_dot(const Eigen::Vector6d& x, + const Eigen::Vector6d& r) { + Eigen::Vector6d x_dot = Ad_ * x + Bd_ * r; + + return x_dot; +} + +void GripperReferenceFilter::calculate_Ad(const Eigen::Vector6d& omega, + const Eigen::Vector6d& zeta) { + //TODO: Clean up and clarify dimensions to define as per Fossen 336 (12.6) + Eigen::Matrix6d omega_diag = omega.asDiagonal(); + Eigen::Matrix6d delta = zeta.asDiagonal(); + Eigen::Matrix6d omega_diag_squared = omega_diag * omega_diag; + Eigen::Matrix6d omega_diag_cubed = omega_diag * omega_diag * omega_diag; + Ad_.block<6, 6>(0, 6) = Eigen::Matrix6d::Identity(); + Ad_.block<6, 6>(12, 0) = -omega_diag_cubed; + Ad_.block<6, 6>(12, 6) = + -(2 * delta + Eigen::Matrix6d::Identity()) * omega_diag_squared; + Ad_.block<6, 6>(12, 12) = + -(2 * delta + Eigen::Matrix6d::Identity()) * omega_diag; + Ad_.block<6, 6>(6, 12) = Eigen::Matrix6d::Identity(); +} + //TODO: Same thing applies here as per last TODO +void GripperReferenceFilter::calculate_Bd(const Eigen::Vector6d& omega) { + Eigen::Matrix6d omega_diag = omega.asDiagonal(); + Bd_.block<6, 6>(12, 0) = omega_diag * omega_diag * omega_diag; +} + +} // namespace vortex::guidance diff --git a/guidance/src/gripper_reference_filter_ros.cpp b/guidance/src/gripper_reference_filter_ros.cpp new file mode 100644 index 0000000..001dcf3 --- /dev/null +++ b/guidance/src/gripper_reference_filter_ros.cpp @@ -0,0 +1,418 @@ +#include "gripper_reference_filter/gripper_reference_filter_ros.hpp" +#include +#include +#include +#include +#include +#include +#include + +const auto start_message = R"( + + ____ _ ____ __ _____ _ _ _ + / ___|_ __ (_)_ __ _ __ ___ _ __ | _ \ ___ / _| ___ _ __ ___ _ __ ___ ___ | ___(_) | |_ ___ _ __ +| | _| '_ \| | '_ \| '_ \ / _ \ '__| | |_) / _ \ |_ / _ \ '__/ _ \ '_ \ / __/ _ \ | |_ | | | __/ _ \ '__| +| |_| | |_) | | |_) | |_) | __/ | | _ < __/ _| __/ | | __/ | | | (_| __/ | _| | | | || __/ | + \____| .__/|_| .__/| .__/ \___|_| |_| \_\___|_| \___|_| \___|_| |_|\___\___| |_| |_|_|\__\\___|_| + |_| |_| |_| + + )"; + +namespace vortex::guidance { + + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +GripperReferenceFilterNode::ReferenceFilterNode(const rclcpp::NodeOptions& options) + : Node("reference_filter_node", options) { + time_step_ = std::chrono::milliseconds(10); + + set_subscribers_and_publisher(); + + set_action_server(); + + set_refererence_filter(); + + spdlog::info(start_message); +} + + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +void GripperReferenceFilterNode::set_subscribers_and_publisher() { + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.twist"); + this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.reference_pose"); + + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + std::string twist_topic = this->get_parameter("topics.twist").as_string(); + std::string guidance_topic = + this->get_parameter("topics.guidance.dp").as_string(); + std::string reference_pose_topic = + this->get_parameter("topics.reference_pose").as_string(); + + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + reference_pub_ = this->create_publisher( + guidance_topic, qos_sensor_data); + reference_sub_ = this->create_subscription( + reference_pose_topic, qos_sensor_data, + std::bind(&GripperReferenceFilterNode::reference_callback, this, + std::placeholders::_1)); + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, qos_sensor_data, + std::bind(&GripperReferenceFilterNode::pose_callback, this, + std::placeholders::_1)); + twist_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + twist_topic, qos_sensor_data, + std::bind(&GripperReferenceFilterNode::twist_callback, this, + std::placeholders::_1)); +} + + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +void GripperReferenceFilterNode::set_action_server() { + this->declare_parameter("action_servers.reference_filter"); + std::string action_server_name = + this->get_parameter("action_servers.reference_filter").as_string(); + cb_group_ = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + action_server_ = rclcpp_action::create_server< + vortex_msgs::action::ReferenceFilterWaypoint>( + this, action_server_name, + std::bind(&GripperReferenceFilterNode::handle_goal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&GripperReferenceFilterNode::handle_cancel, this, + std::placeholders::_1), + std::bind(&GripperReferenceFilterNode::handle_accepted, this, + std::placeholders::_1), + rcl_action_server_get_default_options(), cb_group_); +} + + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +void GripperReferenceFilterNode::set_refererence_filter() { + this->declare_parameter>("zeta"); + this->declare_parameter>("omega"); + + std::vector zeta = this->get_parameter("zeta").as_double_array(); + std::vector omega = this->get_parameter("omega").as_double_array(); + + Eigen::Vector6d zeta_eigen = Eigen::Map(zeta.data()); + Eigen::Vector6d omega_eigen = Eigen::Map(omega.data()); + + ReferenceFilterParams filter_params{omega_eigen, zeta_eigen}; + reference_filter_ = std::make_unique(filter_params); +} + + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +void GripperReferenceFilterNode::reference_callback( + const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + double x = msg->pose.position.x; + double y = msg->pose.position.y; + double z = msg->pose.position.z; + const auto& o = msg->pose.orientation; + Eigen::Quaterniond q(o.w, o.x, o.y, o.z); + Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); + double roll{euler_angles(0)}; + double pitch{euler_angles(1)}; + double yaw{euler_angles(2)}; + r_ << x, y, z, roll, pitch, yaw; +} + + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +void GripperReferenceFilterNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + std::lock_guard lock(mutex_); + current_pose_ = *msg; +} + + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +void GripperReferenceFilterNode::twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + std::lock_guard lock(mutex_); + current_twist_ = *msg; +} + + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +rclcpp_action::GoalResponse GripperReferenceFilterNode::handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr + goal) { + (void)uuid; + (void)goal; + { + std::lock_guard lock(mutex_); + if (goal_handle_) { + if (goal_handle_->is_active()) { + spdlog::info("Aborting current goal and accepting new goal"); + preempted_goal_id_ = goal_handle_->get_goal_id(); + } + } + } + spdlog::info("Accepted goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +rclcpp_action::CancelResponse GripperReferenceFilterNode::handle_cancel( + const std::shared_ptr> goal_handle) { + spdlog::info("Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; +} + + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +void GripperReferenceFilterNode::handle_accepted( + const std::shared_ptr> goal_handle) { + execute(goal_handle); +} + + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +Eigen::Vector18d GripperReferenceFilterNode::fill_reference_state() { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + x(0) = current_pose_.pose.pose.position.x; + x(1) = current_pose_.pose.pose.position.y; + x(2) = current_pose_.pose.pose.position.z; + + const auto& o = current_pose_.pose.pose.orientation; + Eigen::Quaterniond q(o.w, o.x, o.y, o.z); + Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); + double roll{euler_angles(0)}; + double pitch{euler_angles(1)}; + double yaw{euler_angles(2)}; + + x(3) = vortex::utils::math::ssa(roll); + x(4) = vortex::utils::math::ssa(pitch); + x(5) = vortex::utils::math::ssa(yaw); + + vortex::utils::types::PoseEuler pose{current_pose_.pose.pose.position.x, + current_pose_.pose.pose.position.y, + current_pose_.pose.pose.position.z, + roll, + pitch, + yaw}; + Eigen::Matrix6d J = pose.as_j_matrix(); + vortex::utils::types::Twist twist{current_twist_.twist.twist.linear.x, + current_twist_.twist.twist.linear.y, + current_twist_.twist.twist.linear.z, + current_twist_.twist.twist.angular.x, + current_twist_.twist.twist.angular.y, + current_twist_.twist.twist.angular.z}; + Eigen::Vector6d pose_dot = J * twist.to_vector(); + + x(6) = pose_dot(0); + x(7) = pose_dot(1); + x(8) = pose_dot(2); + x(9) = pose_dot(3); + x(10) = pose_dot(4); + x(11) = pose_dot(5); + + return x; +} + + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +Eigen::Vector6d GripperReferenceFilterNode::fill_reference_goal( + const geometry_msgs::msg::Pose& goal) { + double x{goal.position.x}; + double y{goal.position.y}; + double z{goal.position.z}; + + const auto& o = goal.orientation; + Eigen::Quaterniond q(o.w, o.x, o.y, o.z); + Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); + double roll{euler_angles(0)}; + double pitch{euler_angles(1)}; + double yaw{euler_angles(2)}; + + Eigen::Vector6d r; + r << x, y, z, roll, pitch, yaw; + + return r; +} + + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +vortex_msgs::msg::ReferenceFilter GripperReferenceFilterNode::fill_reference_msg() { + vortex_msgs::msg::ReferenceFilter feedback_msg; + feedback_msg.x = x_(0); + feedback_msg.y = x_(1); + feedback_msg.z = x_(2); + feedback_msg.roll = vortex::utils::math::ssa(x_(3)); + feedback_msg.pitch = vortex::utils::math::ssa(x_(4)); + feedback_msg.yaw = vortex::utils::math::ssa(x_(5)); + feedback_msg.x_dot = x_(6); + feedback_msg.y_dot = x_(7); + feedback_msg.z_dot = x_(8); + feedback_msg.roll_dot = x_(9); + feedback_msg.pitch_dot = x_(10); + feedback_msg.yaw_dot = x_(11); + + return feedback_msg; +} + + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +Eigen::Vector6d GripperReferenceFilterNode::apply_mode_logic( + const Eigen::Vector6d& r_in, + uint8_t mode) { + Eigen::Vector6d r_out = r_in; + + switch (mode) { + case vortex_msgs::msg::Waypoint::FULL_POSE: + break; + + case vortex_msgs::msg::Waypoint::ONLY_POSITION: + r_out(3) = x_(3); + r_out(4) = x_(4); + r_out(5) = x_(5); + break; + + case vortex_msgs::msg::Waypoint::FORWARD_HEADING: { + double dx = r_in(0) - x_(0); + double dy = r_in(1) - x_(1); + + double forward_heading = std::atan2(dy, dx); + + r_out(3) = 0.0; + r_out(4) = 0.0; + r_out(5) = vortex::utils::math::ssa(forward_heading); + break; + } + + case vortex_msgs::msg::Waypoint::ONLY_ORIENTATION: + r_out(0) = x_(0); + r_out(1) = x_(1); + r_out(2) = x_(2); + break; + } + + return r_out; +} + //TODO: Discuss necessary goal with this with Cyprian and Jorgen +void GripperReferenceFilterNode::publish_hold_reference() { + if (!reference_pub_) { + return; + } + + const auto& p = current_pose_.pose.pose.position; + const auto& o = current_pose_.pose.pose.orientation; + + Eigen::Quaterniond q(o.w, o.x, o.y, o.z); + if (q.norm() < 1e-9) { + q = Eigen::Quaterniond::Identity(); + } else { + q.normalize(); + } + + Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); + + vortex_msgs::msg::ReferenceFilter hold_msg; + hold_msg.x = p.x; + hold_msg.y = p.y; + hold_msg.z = p.z; + hold_msg.roll = vortex::utils::math::ssa(euler_angles(0)); + hold_msg.pitch = vortex::utils::math::ssa(euler_angles(1)); + hold_msg.yaw = vortex::utils::math::ssa(euler_angles(2)); + + hold_msg.x_dot = 0.0; + hold_msg.y_dot = 0.0; + hold_msg.z_dot = 0.0; + hold_msg.roll_dot = 0.0; + hold_msg.pitch_dot = 0.0; + hold_msg.yaw_dot = 0.0; + + reference_pub_->publish(hold_msg); +} + +void GripperReferenceFilterNode::execute( + const std::shared_ptr> goal_handle) { //TODO: Outdated vortex_msgs, change to the GripperReferenceFilterWaypoint when its made + { + std::lock_guard lock(mutex_); + this->goal_handle_ = goal_handle; + } + + spdlog::info("Executing goal"); + + x_ = fill_reference_state(); + //TODO: change all this given the new waypoint server for the GripperReferenceFilterNode + const geometry_msgs::msg::Pose goal = + goal_handle->get_goal()->waypoint.pose; + uint8_t mode = goal_handle->get_goal()->waypoint.mode; + double convergence_threshold = + goal_handle->get_goal()->convergence_threshold; + + if (convergence_threshold <= 0.0) { + convergence_threshold = 0.1; + spdlog::warn( + "GripperReferenceFilter: Invalid convergence_threshold received (<= 0). " + "Using default 0.1"); + } + + Eigen::Vector6d r_temp = fill_reference_goal(goal); + r_ = apply_mode_logic(r_temp, mode); + + auto feedback = std::make_shared< + vortex_msgs::action::ReferenceFilterWaypoint::Feedback>(); + auto result = std::make_shared< + vortex_msgs::action::ReferenceFilterWaypoint::Result>(); + + rclcpp::Rate loop_rate(1000.0 / time_step_.count()); + + while (rclcpp::ok()) { + { + std::lock_guard lock(mutex_); + if (goal_handle->get_goal_id() == preempted_goal_id_) { + publish_hold_reference(); + result->success = false; + goal_handle->abort(result); + return; + } + } + { + std::lock_guard lock(mutex_); + if (goal_handle->is_canceling()) { + publish_hold_reference(); + result->success = false; + goal_handle->canceled(result); + spdlog::info("Goal canceled"); + return; + } + } + Eigen::Vector18d x_dot = reference_filter_->calculate_x_dot(x_, r_); + x_ += x_dot * time_step_.count() / 1000.0; + + vortex_msgs::msg::ReferenceFilter feedback_msg = fill_reference_msg(); + + feedback->reference = feedback_msg; + + goal_handle->publish_feedback(feedback); + reference_pub_->publish(feedback_msg); + + if ((x_.head(6) - r_.head(6)).norm() < convergence_threshold) { + result->success = true; + goal_handle->succeed(result); + x_.head(6) = r_.head(6); + vortex_msgs::msg::ReferenceFilter feedback_msg = + fill_reference_msg(); + reference_pub_->publish(feedback_msg); + spdlog::info("Goal reached"); + return; + } + + loop_rate.sleep(); + } + if (!rclcpp::ok() && goal_handle->is_active()) { + auto result = std::make_shared< + vortex_msgs::action::ReferenceFilterWaypoint::Result>(); + result->success = false; + + try { + goal_handle->abort(result); + } catch (...) { + // Ignore exceptions during shutdown + } + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(GripperReferenceFilterNode) + +} // namespace vortex::guidance diff --git a/guidance/test/CMakeLists.txt b/guidance/test/CMakeLists.txt new file mode 100644 index 0000000..6c00288 --- /dev/null +++ b/guidance/test/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) +add_executable( + ${TEST_BINARY_NAME} + test_reference_filter.cpp +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${LIB_NAME} + GTest::GTest +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) + +gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/guidance/test/test_gripper_reference_filter.cpp b/guidance/test/test_gripper_reference_filter.cpp new file mode 100644 index 0000000..5b42962 --- /dev/null +++ b/guidance/test/test_gripper_reference_filter.cpp @@ -0,0 +1,58 @@ +#include + +#include "reference_filter_dp/eigen_typedefs.hpp" +#include "reference_filter_dp/reference_filter.hpp" + +namespace vortex::guidance { + +class ReferenceFilterTests : public ::testing::Test { + protected: + ReferenceFilterTests() : reference_filter_{get_filter_params()} {} + + ReferenceFilterParams get_filter_params() { + ReferenceFilterParams params; + params.omega = Eigen::Vector6d::Ones(); + params.zeta = Eigen::Vector6d::Ones(); + return params; + } + + ReferenceFilter reference_filter_; +}; + +TEST_F(ReferenceFilterTests, T01_positive_command_positive_xdot) { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + Eigen::Vector6d r = Eigen::Vector6d::Ones(); + Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); + + for (int i = 0; i < xdot.size(); ++i) { + EXPECT_GE(xdot(i), 0.0); + } +} + +TEST_F(ReferenceFilterTests, T02_negative_command_negative_xdot) { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + Eigen::Vector6d r = -Eigen::Vector6d::Ones(); + Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); + + for (int i = 0; i < xdot.size(); ++i) { + EXPECT_LE(xdot(i), 0.0); + } +} + +TEST_F(ReferenceFilterTests, T03_zero_command_zero_xdot) { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + Eigen::Vector6d r = Eigen::Vector6d::Zero(); + Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); + + for (int i = 0; i < xdot.size(); ++i) { + EXPECT_DOUBLE_EQ(xdot(i), 0.0); + } +} + +} // namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} From c304c509ecc7e780cd09bee6578212eabf7c5ee5 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Wed, 25 Mar 2026 16:17:40 +0100 Subject: [PATCH 09/24] feat: add can interface --- gripper_interface/include/can_interface.hpp | 54 ++++++ gripper_interface/src/can_interface.cpp | 199 ++++++++++++++++++++ 2 files changed, 253 insertions(+) create mode 100644 gripper_interface/include/can_interface.hpp create mode 100644 gripper_interface/src/can_interface.cpp diff --git a/gripper_interface/include/can_interface.hpp b/gripper_interface/include/can_interface.hpp new file mode 100644 index 0000000..ff7f4a9 --- /dev/null +++ b/gripper_interface/include/can_interface.hpp @@ -0,0 +1,54 @@ +#ifndef CAN_INTERFACE_H +#define CAN_INTERFACE_H + +#include +#include +#include +#include +#include +#include + +enum class can_status { + OK = 0, + ERR_ALREADY_INITIALIZED = -1, + ERR_SOCKET = -2, + ERR_CANFD_SUPPORT = -3, + ERR_LOOPBACK = -4, + ERR_INTERFACE_INDEX = -5, + ERR_BIND = -6, + ERR_NOT_INITIALIZED = -7, + ERR_DATA_LENGTH = -8, + ERR_SEND = -9, + ERR_RECEIVE = -10, + ERR_SETTING_TIMEOUT = -11, + ERR_SETTING_FILTER = -12, + ERR_CLEARING_FILTERS = -13 +}; + +class can_interface { +private: + int socket_fd_; + bool is_initialized_; + std::string interface_name_; + + std::thread receive_thread_; + std::atomic receiving_ = false; + +public: + can_interface(); + ~can_interface(); + + can_status init(const std::string& ifname = "can0"); + can_status send(uint32_t can_id, const uint8_t* data, uint8_t len, bool use_brs = true); + can_status receive(struct canfd_frame& frame); + can_status receive(struct canfd_frame& frame, int timeout_ms); + can_status start_async_receive(std::function callback); + void stop_async_receive(); + can_status set_filter(uint32_t can_id, uint32_t can_mask = 0x7FF); + can_status set_filters(const struct can_filter* filters, size_t num_filters); + can_status clear_filters(); + bool is_initialized() const; + std::string get_interface_name() const; +}; + +#endif // CAN_INTERFACE_H diff --git a/gripper_interface/src/can_interface.cpp b/gripper_interface/src/can_interface.cpp new file mode 100644 index 0000000..893b5a7 --- /dev/null +++ b/gripper_interface/src/can_interface.cpp @@ -0,0 +1,199 @@ +#include "can_interface.hpp" +#include +#include +#include +#include +#include +#include +#include + +can_interface::can_interface() : socket_fd_(-1), is_initialized_(false) {} + +can_interface::~can_interface() { + if (is_initialized_) { + close(socket_fd_); + } +} + +can_status can_interface::init(const std::string& ifname) { + interface_name_ = ifname; + + socket_fd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (socket_fd_ < 0) { + return can_status::ERR_SOCKET; + } + + int enable_canfd = 1; + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, + &enable_canfd, sizeof(enable_canfd)) < 0) { + close(socket_fd_); + return can_status::ERR_CANFD_SUPPORT; + } + + // Enable loopback (receive own frames). Only use for vcan testing + //int recv_own_msgs = 1; + //if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, + // &recv_own_msgs, sizeof(recv_own_msgs)) < 0) { + // close(socket_fd_); + // return can_status::ERR_LOOPBACK; + //} + + struct ifreq ifr; + std::strcpy(ifr.ifr_name, interface_name_.c_str()); + if (ioctl(socket_fd_, SIOCGIFINDEX, &ifr) < 0) { + close(socket_fd_); + return can_status::ERR_INTERFACE_INDEX; + } + + struct sockaddr_can addr; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(socket_fd_, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + close(socket_fd_); + return can_status::ERR_BIND; + } + + is_initialized_ = true; + + return can_status::OK; +} + +can_status can_interface::send(uint32_t can_id, const uint8_t* data, uint8_t len, bool use_brs) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + if (len > 64) { + return can_status::ERR_DATA_LENGTH; + } + + struct canfd_frame frame; + memset(&frame, 0, sizeof(frame)); + + frame.can_id = can_id; + frame.len = len; + frame.flags = use_brs ? CANFD_BRS : 0; + + if (data != nullptr && len > 0) { + memcpy(frame.data, data, len); + } + + if (write(socket_fd_, &frame, sizeof(frame)) != sizeof(frame)) { + return can_status::ERR_SEND; + } + + return can_status::OK; +} + +can_status can_interface::receive(struct canfd_frame& frame) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + ssize_t nbytes = read(socket_fd_, &frame, sizeof(frame)); + + if (nbytes != CANFD_MTU) { + return can_status::ERR_RECEIVE; + } + + return can_status::OK; +} + +can_status can_interface::receive(struct canfd_frame& frame, int timeout_ms) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + struct timeval tv; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + if (setsockopt(socket_fd_, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) { + return can_status::ERR_SETTING_TIMEOUT; + } + + ssize_t nbytes = read(socket_fd_, &frame, sizeof(frame)); + + if (nbytes != CANFD_MTU) { + return can_status::ERR_RECEIVE; + } + + return can_status::OK; +} + +can_status can_interface::start_async_receive(std::function callback) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + receiving_ = true; + + receive_thread_ = std::thread([this, callback]() { + while (receiving_) { + struct canfd_frame frame; + can_status status = receive(frame, 1000); + callback(frame, status); + } + }); + return can_status::OK; +} + +void can_interface::stop_async_receive() { + receiving_ = false; + if (receive_thread_.joinable()) { + receive_thread_.join(); + } +} + +can_status can_interface::set_filter(uint32_t can_id, uint32_t can_mask) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + struct can_filter filter; + filter.can_id = can_id; + filter.can_mask = can_mask; + + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) { + return can_status::ERR_SETTING_FILTER; + } + + return can_status::OK; +} + +can_status can_interface::set_filters(const struct can_filter* filters, size_t num_filters) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FILTER, filters, num_filters * sizeof(struct can_filter)) < 0) { + return can_status::ERR_SETTING_FILTER; + } + + return can_status::OK; +} + +can_status can_interface::clear_filters() { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + struct can_filter filter; + filter.can_id = 0x0; + filter.can_mask = 0x0; + + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) { + return can_status::ERR_CLEARING_FILTERS; + } + + return can_status::OK; +} + +bool can_interface::is_initialized() const { + return is_initialized_; +} + +std::string can_interface::get_interface_name() const { + return interface_name_; +} From 2c3f684714c3237733765dda86b70499b4bd99a2 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Wed, 25 Mar 2026 16:36:24 +0100 Subject: [PATCH 10/24] feat: convert from i2c to can --- .../gripper_interface_driver.hpp | 3 + .../src/gripper_interface_driver.cpp | 71 ++++++++----------- 2 files changed, 31 insertions(+), 43 deletions(-) diff --git a/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp b/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp index 067802c..e81c525 100644 --- a/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp +++ b/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp @@ -38,6 +38,7 @@ class GripperInterfaceDriver { int pwm_idle); + /** * @brief init i2c * @return 0 on success @@ -84,6 +85,7 @@ class GripperInterfaceDriver { int i2c_address_; // I2C address of the microcontroller int pwm_gain_; int pwm_idle_; + can_interface can_; /** * @brief Convert PWM value to I2C data. @@ -113,6 +115,7 @@ class GripperInterfaceDriver { static constexpr double raw_angle_to_radians(std::uint16_t raw_angle) { return (static_cast(raw_angle) / 0x3FFF) * (2.0 * M_PI); } + }; // class GripperInterfaceDriver #endif // GRIPPER_INTERFACE_DRIVER_HPP diff --git a/gripper_interface/src/gripper_interface_driver.cpp b/gripper_interface/src/gripper_interface_driver.cpp index a7cbb81..ae4ffba 100644 --- a/gripper_interface/src/gripper_interface_driver.cpp +++ b/gripper_interface/src/gripper_interface_driver.cpp @@ -1,28 +1,19 @@ #include "gripper_interface/gripper_interface_driver.hpp" #include +#include "can_interface.hpp" GripperInterfaceDriver::GripperInterfaceDriver(short i2c_bus, int i2c_address, int pwm_gain, int pwm_idle) - : i2c_bus_(i2c_bus), - i2c_address_(i2c_address), - pwm_gain_(pwm_gain), - pwm_idle_(pwm_idle) {} - -int GripperInterfaceDriver::init_i2c() { - std::string i2c_filename = std::format("/dev/i2c-{}", i2c_bus_); - bus_fd_ = - open(i2c_filename.c_str(), - O_RDWR); // Open the I2C bus for reading and writing (O_RDWR) - if (bus_fd_ < 0) { - return bus_fd_; - } + : pwm_gain_(pwm_gain), pwm_idle_(pwm_idle) {} - if (ioctl(bus_fd_, I2C_SLAVE, i2c_address_) < 0) { - return -1; +can_status GripperInterfaceDriver::init_can() { + if (can_.init("can0")) { + return can_status::ERR_NOT_INITIALIZED; } - return 0; + + return (can_.set_filter(0x46D)); } GripperInterfaceDriver::~GripperInterfaceDriver() { @@ -36,51 +27,45 @@ std::uint16_t GripperInterfaceDriver::joy_to_pwm(const double joy_value) { return static_cast(pwm_idle_ + pwm_gain_ * joy_value); } -int GripperInterfaceDriver::send_pwm( +can_status GripperInterfaceDriver::send_pwm( const std::vector& pwm_values) { + static constexpr uint32_t GRIPPER_PWM_CAN_ID = 0x46C; constexpr std::size_t num_servos = 3; - constexpr std::size_t i2c_data_size = - 1 + num_servos * 2; // 3 thrusters * (1xMSB + 1xLSB) - std::array buf; - - buf[0] = 0x00; // "Start" byte + constexpr std::size_t data_size = + num_servos * 2; // 3 thrusters * (1xMSB + 1xLSB) + std::array buf; - std::memcpy(buf.data() + 1, pwm_values.data(), i2c_data_size - 1); + std::memcpy(buf.data(), pwm_values.data(), data_size); - if (write(bus_fd_, buf.data(), i2c_data_size) != i2c_data_size) { - return -1; - } - return 0; + return (can_.send(GRIPPER_PWM_CAN_ID, buf, data_size, true) != + can_status::OK); } int GripperInterfaceDriver::stop_gripper() { - constexpr std::size_t i2c_data_size = 1; - std::uint8_t i2c_message = 0x01; + static constexpr uint32_t GRIPPER_STOP_CAN_ID = 0x469; + constexpr std::size_t data_size = 1; + std::uint8_t data = 0x00; - if (write(bus_fd_, &i2c_message, i2c_data_size) != i2c_data_size) { - return -1; - } - return 0; + return (can_.send(GRIPPER_STOP_CAN_ID, &data, data_size, true) != + can_status::OK); } int GripperInterfaceDriver::start_gripper() { - constexpr std::size_t i2c_data_size = 1; - std::uint8_t i2c_message = 0x02; + static constexpr uint32_t GRIPPER_START_CAN_ID = 0x46A; + constexpr std::size_t data_size = 1; + std::uint8_t data = 0x02; - if (write(bus_fd_, &i2c_message, i2c_data_size) != i2c_data_size) { - return -1; - } - return 0; + return (can_.send(GRIPPER_START_CAN_ID, &data, data_size, true) != + can_status::OK); } std::vector GripperInterfaceDriver::read_encoders() { - constexpr std::size_t i2c_data_size = 6; // 6 bytes -> 3 angles. - constexpr std::size_t num_angles = i2c_data_size / 2; - std::array i2c_data_array; + constexpr std::size_t num_angles = 2; std::vector encoder_angles; encoder_angles.reserve(num_angles); - if (read(bus_fd_, i2c_data_array.data(), i2c_data_size) != i2c_data_size) { + canfd_frame encoder_data{}; + if (can_.receive(encoder_data, 1000){ return {}; } From 429ee307de256a851288157522374a8eb33387f9 Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Tue, 31 Mar 2026 16:07:50 +0200 Subject: [PATCH 11/24] feat: gripper controller skeleton implemented, builds and can launch with entire system, needs actual servo states to simulate proper topic relationships --- gripper_controller/CMakeLists.txt | 85 +++- .../config/gripper_controller_params.yaml | 19 + .../gripper_controller/gripper_controller.hpp | 32 ++ .../gripper_controller_ros.hpp | 103 +++-- .../gripper_controller_translator.hpp | 49 ++ .../gripper_controller_typedefs.hpp | 22 + .../gripper_controller_utils.hpp | 0 .../launch/gripper_controller.launch.py | 21 + gripper_controller/package.xml | 28 ++ gripper_controller/src/gripper_controller.cpp | 34 ++ .../src/gripper_controller_node.cpp | 20 +- .../src/gripper_controller_ros.cpp | 142 ++++++ .../src/gripper_controller_utils.cpp | 0 gripper_controller/test/CMakeLists.txt | 22 + .../test/test_gripper_controller.cpp | 210 +++++++++ {guidance => gripper_guidance}/CMakeLists.txt | 8 +- {guidance => gripper_guidance}/README.md | 0 .../gripper_reference_filter_params.yaml | 14 + .../gripper_eigen_typedefs.hpp | 0 .../gripper_reference_filter.hpp | 4 +- .../gripper_reference_filter_ros.hpp | 127 ++++++ .../launch/gripper_reference_filter.launch.py | 32 ++ {guidance => gripper_guidance}/package.xml | 6 +- .../src/gripper_reference_filter.cpp | 42 ++ .../src/gripper_reference_filter_ros.cpp | 318 +++++++++++++ .../test/CMakeLists.txt | 2 +- .../test/test_gripper_reference_filter.cpp | 83 ++++ .../interface_to_gripper_state.hpp | 24 + .../interface_to_gripper_state.cpp | 32 ++ .../gripper_reference_filter_ros.hpp | 108 ----- guidance/src/gripper_reference_filter.cpp | 38 -- guidance/src/gripper_reference_filter_ros.cpp | 418 ------------------ .../test/test_gripper_reference_filter.cpp | 91 ++-- 33 files changed, 1459 insertions(+), 675 deletions(-) create mode 100644 gripper_controller/config/gripper_controller_params.yaml create mode 100644 gripper_controller/include/gripper_controller/gripper_controller_translator.hpp create mode 100644 gripper_controller/include/gripper_controller/gripper_controller_typedefs.hpp delete mode 100644 gripper_controller/include/gripper_controller/gripper_controller_utils.hpp create mode 100644 gripper_controller/launch/gripper_controller.launch.py delete mode 100644 gripper_controller/src/gripper_controller_utils.cpp create mode 100644 gripper_controller/test/CMakeLists.txt create mode 100644 gripper_controller/test/test_gripper_controller.cpp rename {guidance => gripper_guidance}/CMakeLists.txt (89%) rename {guidance => gripper_guidance}/README.md (100%) create mode 100644 gripper_guidance/config/gripper_reference_filter_params.yaml rename {guidance => gripper_guidance}/include/gripper_reference_filter/gripper_eigen_typedefs.hpp (100%) rename {guidance => gripper_guidance}/include/gripper_reference_filter/gripper_reference_filter.hpp (90%) create mode 100644 gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp create mode 100644 gripper_guidance/launch/gripper_reference_filter.launch.py rename {guidance => gripper_guidance}/package.xml (78%) create mode 100644 gripper_guidance/src/gripper_reference_filter.cpp create mode 100644 gripper_guidance/src/gripper_reference_filter_ros.cpp rename {guidance => gripper_guidance}/test/CMakeLists.txt (90%) create mode 100644 gripper_guidance/test/test_gripper_reference_filter.cpp create mode 100644 gripper_interface/include/gripper_interface/interface_to_gripper_state.hpp create mode 100644 gripper_interface/interface_to_gripper_state/interface_to_gripper_state.cpp delete mode 100644 guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp delete mode 100644 guidance/src/gripper_reference_filter.cpp delete mode 100644 guidance/src/gripper_reference_filter_ros.cpp diff --git a/gripper_controller/CMakeLists.txt b/gripper_controller/CMakeLists.txt index 9b0796c..e37362d 100644 --- a/gripper_controller/CMakeLists.txt +++ b/gripper_controller/CMakeLists.txt @@ -9,32 +9,89 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -include_directories(include) - find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(geometry_msgs REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) +include_directories(include) -add_executable(${PROJECT_NAME}_node - src/gripper_controller_node.cpp +# --------------------------------------------------------------------------- +# 1. Component Library (Shared) +# --------------------------------------------------------------------------- +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED src/gripper_controller.cpp + src/gripper_controller_ros.cpp ) +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + rclcpp_action + Eigen3 + vortex_msgs + vortex_utils + vortex_utils_ros + spdlog + fmt +) + +# --------------------------------------------------------------------------- +# 2. Core Library (Static) - Used for Unit Testing +# --------------------------------------------------------------------------- +set(CORE_LIB_NAME "${PROJECT_NAME}_core") -ament_target_dependencies(${PROJECT_NAME}_node +add_library(${CORE_LIB_NAME} STATIC + src/gripper_controller.cpp +) + +ament_target_dependencies(${CORE_LIB_NAME} PUBLIC + Eigen3 + vortex_utils +) + +# --------------------------------------------------------------------------- +# 3. Component Registration +# --------------------------------------------------------------------------- +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "vortex::controller::GripperControllerNode" + EXECUTABLE ${PROJECT_NAME}_node +) +# --------------------------------------------------------------------------- +# 4. Standalone Executable +# --------------------------------------------------------------------------- +add_executable(${PROJECT_NAME}_standalone + src/gripper_controller_node.cpp +) +target_link_libraries(${PROJECT_NAME}_standalone PUBLIC + ${LIB_NAME} +) +ament_target_dependencies(${PROJECT_NAME}_standalone PUBLIC rclcpp - geometry_msgs ) -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} +ament_export_targets(export_${LIB_NAME}) +install(TARGETS ${LIB_NAME} ${PROJECT_NAME}_standalone + EXPORT export_${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install(TARGETS - ${PROJECT_NAME}_node - DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY include/ DESTINATION include) +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}/) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() ament_package() diff --git a/gripper_controller/config/gripper_controller_params.yaml b/gripper_controller/config/gripper_controller_params.yaml new file mode 100644 index 0000000..11ff76d --- /dev/null +++ b/gripper_controller/config/gripper_controller_params.yaml @@ -0,0 +1,19 @@ + /**: + ros__parameters: + # Control loop timing + time_step_ms: 10 # milliseconds — must match reference filter time step + + # Proportional gain — tune after gear ratio calibration + # Diagonal P-controller: vel = diag(kp_roll, kp_pinch) * position_error + kp: + roll: 1.0 # TODO: tune + pinch: 1.0 # TODO: tune + + # Topics + topics: + # Input: smoothed reference from the gripper reference filter + reference: "vortex/gripper/guidance" + # Input: raw measured gripper state from the interface node + state: "vortex/gripper/state" + # Output: velocity command (JointState.velocity) to the interface node + control: "vortex/gripper/control" diff --git a/gripper_controller/include/gripper_controller/gripper_controller.hpp b/gripper_controller/include/gripper_controller/gripper_controller.hpp index e69de29..4fde033 100644 --- a/gripper_controller/include/gripper_controller/gripper_controller.hpp +++ b/gripper_controller/include/gripper_controller/gripper_controller.hpp @@ -0,0 +1,32 @@ +#ifndef GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_HPP_ +#define GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_HPP_ + +#include "gripper_controller/gripper_controller_typedefs.hpp" + + +class GripperController { + public: + GripperController(); + + // @brief Calculate velocity command from position error + // @param state: struct containing measured gripper state [roll, pinch] + // @param reference: struct containing desired gripper state [roll, pinch] + // @return 2D vector containing velocity commands [roll_vel, pinch_vel] + types::Vector2d calculate_velocity(const types::GripperState& state, + const types::GripperState& reference); + + // @brief Set the proportional gain matrix + // @param Kp: 2x2 matrix containing the proportional gain matrix + void set_kp(const types::Matrix2d& Kp); + + // @brief Set the time step + // @param dt: time step in seconds + void set_time_step(double timestep); + + private: + types::Matrix2d Kp_; + double timestep_; +}; + + +#endif // GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_HPP_ diff --git a/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp b/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp index 92a6fae..6069336 100644 --- a/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp +++ b/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp @@ -1,53 +1,64 @@ -#ifndef GRIPPER_CONTROLLER_DP__GRIPPER_CONTROLLER_ROS_HPP_ -#define GRIPPER_CONTROLLER_DP__GRIPPER_CONTROLLER_ROS_HPP_ +#ifndef GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_ROS_HPP_ +#define GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_ROS_HPP_ -#include -#include +#include +#include +#include #include +#include +#include +#include +#include +#include +#include "gripper_controller/gripper_controller.hpp" +#include "gripper_controller/gripper_controller_translator.hpp" +#include "gripper_controller/gripper_controller_typedefs.hpp" +// --------------------------------------------------------------------------- +// Responsibility: ROS wiring only — subscriptions, publications, parameter +// loading, and timer setup. All control mathematics live in GripperController. +// All message translation lives in gripper_controller::translator. +// --------------------------------------------------------------------------- -class GripperControllerNode() : public rclcpp::Node { - public: - GripperControllerNode(); - explicit GripperControllerNode( - const rclcpp::NodeOptions& options = rclcpp::NodeOptions() - ); - private: - // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(); - - //TODO: Define the callback for the GripperReference node after it's been made - // - // - // - // - // - // - - //TODO: You won't be needing this one (PoseStamped) nephew, consult Cyprian if remove - - // @brief Callback for the reference topic - // @param msg The reference message - void reference_callback( - const geometry_msgs::msg::PoseStamped::SharedPtr msg); - - //TODO: Nor will you probably need this (PoseWithCovStamped) one too, consult Cyprian if remove - - // @brief Callback for the pose topic - // @pram msg The pose message - void pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - - - rclcpp::Publisher::SharedPtr - reference_pub_; - - rclcpp::Subscription::SharedPtr - reference_sub_; - - rclcpp::Subscription::SharedPtr + reference_sub_; + rclcpp::Subscription::SharedPtr + state_sub_; + rclcpp::Publisher::SharedPtr + control_pub_; + + rclcpp::TimerBase::SharedPtr control_timer_; + std::chrono::milliseconds time_step_; + + std::mutex state_mutex_; + + double roll_ref_ = 0.0; + double pinch_ref_ = 0.0; + double roll_measured_ = 0.0; + double pinch_measured_ = 0.0; }; -#endif // GRIPPER_CONTROLLER_DP__GRIPPER_CONTROLLER_ROS_HPP_ +} // namespace vortex::controller + +#endif // GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_ROS_HPP_ diff --git a/gripper_controller/include/gripper_controller/gripper_controller_translator.hpp b/gripper_controller/include/gripper_controller/gripper_controller_translator.hpp new file mode 100644 index 0000000..99f3126 --- /dev/null +++ b/gripper_controller/include/gripper_controller/gripper_controller_translator.hpp @@ -0,0 +1,49 @@ +#ifndef GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_TRANSLATOR_HPP_ +#define GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_TRANSLATOR_HPP_ + +#include +#include +#include +#include "gripper_controller/gripper_controller_typedefs.hpp" + +// --------------------------------------------------------------------------- +// Responsibility: translate between domain structs and ROS message types. +// This is intentionally a namespace of stateless free functions rather than +// a class — there is no invariant to maintain, no state to encapsulate. +// Keeping translation logic here (instead of inside the node) satisfies SRP: +// the node only orchestrates, the translator only converts. +// --------------------------------------------------------------------------- + +namespace gripper_controller::translator { + +/// @brief Extract position-only GripperState from a smoothed reference message. +inline types::GripperState reference_filter_msg_to_gripper_state( + const vortex_msgs::msg::GripperReferenceFilter& reference_filter_msg) { + types::GripperState gripper_state; + gripper_state.roll = reference_filter_msg.roll; + gripper_state.pinch = reference_filter_msg.pinch; + return gripper_state; +} + +/// @brief Convert a raw GripperState ROS message to the domain GripperState struct. +inline types::GripperState gripper_state_msg_to_gripper_state( + const vortex_msgs::msg::GripperState& gripper_state_msg) { + types::GripperState gripper_state; + gripper_state.roll = gripper_state_msg.roll; + gripper_state.pinch = gripper_state_msg.pinch; + return gripper_state; +} + +/// @brief Pack a 2D velocity command vector into a GripperStateVelocityCommand message. +inline vortex_msgs::msg::GripperStateVelocityCommand +velocity_command_to_gripper_velocity_command_msg( + const types::Vector2d& velocity_command) { + vortex_msgs::msg::GripperStateVelocityCommand velocity_command_msg; + velocity_command_msg.roll_velocity = velocity_command(0); + velocity_command_msg.pinch_velocity = velocity_command(1); + return velocity_command_msg; +} + +} // namespace gripper_controller::translator + +#endif // GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_TRANSLATOR_HPP_ diff --git a/gripper_controller/include/gripper_controller/gripper_controller_typedefs.hpp b/gripper_controller/include/gripper_controller/gripper_controller_typedefs.hpp new file mode 100644 index 0000000..002ba5a --- /dev/null +++ b/gripper_controller/include/gripper_controller/gripper_controller_typedefs.hpp @@ -0,0 +1,22 @@ +#ifndef GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_TYPEDEFS_HPP_ +#define GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_TYPEDEFS_HPP_ + +#include + +namespace types { + +using Vector2d = Eigen::Vector2d; +using Matrix2d = Eigen::Matrix2d; + +struct GripperState { + double roll = 0.0; + double pinch = 0.0; +}; + +// Velocity saturation limits — tune after gear ratio calibration +constexpr double MAX_ROLL_VEL = 1.0; // TODO: tune +constexpr double MAX_PINCH_VEL = 1.0; // TODO: tune + +} // namespace types + +#endif // GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_TYPEDEFS_HPP_ diff --git a/gripper_controller/include/gripper_controller/gripper_controller_utils.hpp b/gripper_controller/include/gripper_controller/gripper_controller_utils.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/gripper_controller/launch/gripper_controller.launch.py b/gripper_controller/launch/gripper_controller.launch.py new file mode 100644 index 0000000..f5d76e9 --- /dev/null +++ b/gripper_controller/launch/gripper_controller.launch.py @@ -0,0 +1,21 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + config_file_path = os.path.join( + get_package_share_directory("gripper_controller"), + "config", + "gripper_controller_params.yaml", + ) + + gripper_controller_node = Node( + package="gripper_controller", + executable="gripper_controller_standalone", + name="gripper_controller_node", + parameters=[config_file_path], + output="screen", + ) + + return LaunchDescription([gripper_controller_node]) diff --git a/gripper_controller/package.xml b/gripper_controller/package.xml index e69de29..076f4cf 100644 --- a/gripper_controller/package.xml +++ b/gripper_controller/package.xml @@ -0,0 +1,28 @@ + + + + gripper_controller + 0.0.0 + Package for the control algorithm of the gripper + patricsh + MIT + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_components geometry_msgs + vortex_msgs + vortex_utils + vortex_utils_ros + + eigen + spdlog + fmt + + ament_cmake_gtest + + + ament_cmake + + diff --git a/gripper_controller/src/gripper_controller.cpp b/gripper_controller/src/gripper_controller.cpp index e69de29..617ff97 100644 --- a/gripper_controller/src/gripper_controller.cpp +++ b/gripper_controller/src/gripper_controller.cpp @@ -0,0 +1,34 @@ +#include "gripper_controller/gripper_controller.hpp" + +#include + +GripperController::GripperController() + : Kp_(types::Matrix2d::Identity()), timestep_(0.01) {} + +types::Vector2d GripperController::calculate_velocity( + const types::GripperState& measured_state, + const types::GripperState& reference_state) { + + types::Vector2d position_error; + position_error(0) = reference_state.roll - measured_state.roll; + position_error(1) = reference_state.pinch - measured_state.pinch; + + types::Vector2d velocity_command = Kp_ * position_error; + + velocity_command(0) = std::clamp(velocity_command(0), + -types::MAX_ROLL_VEL, + types::MAX_ROLL_VEL); + velocity_command(1) = std::clamp(velocity_command(1), + -types::MAX_PINCH_VEL, + types::MAX_PINCH_VEL); + + return velocity_command; +} + +void GripperController::set_kp(const types::Matrix2d& proportional_gain_matrix) { + Kp_ = proportional_gain_matrix; +} + +void GripperController::set_time_step(double timestep) { + timestep_ = timestep; +} diff --git a/gripper_controller/src/gripper_controller_node.cpp b/gripper_controller/src/gripper_controller_node.cpp index a9bd72c..6d356d5 100644 --- a/gripper_controller/src/gripper_controller_node.cpp +++ b/gripper_controller/src/gripper_controller_node.cpp @@ -1,10 +1,14 @@ -#include "vortex-gripper/gripper_controller_ros.hpp" +#include +#include "gripper_controller/gripper_controller_ros.hpp" -int main(int argc, char **argv) -{ - rclcpp::init(argc, argv); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started Gripper Controller Node"); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + + auto gripper_controller_node = + std::make_shared( + rclcpp::NodeOptions()); + + rclcpp::spin(gripper_controller_node); + rclcpp::shutdown(); + return 0; } diff --git a/gripper_controller/src/gripper_controller_ros.cpp b/gripper_controller/src/gripper_controller_ros.cpp index e69de29..1b20f53 100644 --- a/gripper_controller/src/gripper_controller_ros.cpp +++ b/gripper_controller/src/gripper_controller_ros.cpp @@ -0,0 +1,142 @@ +#include "gripper_controller/gripper_controller_ros.hpp" + +#include +#include +#include +#include +#include "gripper_controller/gripper_controller_translator.hpp" + +const auto start_message = R"( + + ____ _ ____ _ _ _ +/ ___|_ __ (_)_ __ _ __ ___ _ __ / ___|___ _ __ | |_ _ __ ___ | | | ___ _ __ +| | _| '_ \| | '_ \| '_ \ / _ \ '__|| | / _ \| '_ \| __| '__/ _ \| | |/ _ \ '__| +| |_| | |_) | | |_) | |_) | __/ | | |__| (_) | | | | |_| | | (_) | | | __/ | + \____| .__/|_| .__/| .__/ \___|_| \____\___/|_| |_|\__|_| \___/|_|_|\___|_| + |_| |_| |_| + +)"; + +namespace vortex::controller { + +// --------------------------------------------------------------------------- +// CONSTRUCTOR +// --------------------------------------------------------------------------- + +GripperControllerNode::GripperControllerNode(const rclcpp::NodeOptions & options) +: Node("gripper_controller_node", options) { + time_step_ = std::chrono::milliseconds(10); + + set_controller_params(); + + set_subscribers_and_publisher(); + + spdlog::info(start_message); +} + +// --------------------------------------------------------------------------- +// SETUP +// --------------------------------------------------------------------------- + +void GripperControllerNode::set_controller_params() { + const int time_step_ms = + this->declare_parameter("time_step_ms", 10); + time_step_ = std::chrono::milliseconds(time_step_ms); + + const double kp_roll = + this->declare_parameter("kp.roll", 1.0); + const double kp_pinch = + this->declare_parameter("kp.pinch", 1.0); + + types::Matrix2d proportional_gain_matrix = types::Matrix2d::Zero(); + proportional_gain_matrix(0, 0) = kp_roll; + proportional_gain_matrix(1, 1) = kp_pinch; + + controller_.set_kp(proportional_gain_matrix); + controller_.set_time_step(static_cast(time_step_ms) / 1000.0); + + spdlog::info("GripperController: kp_roll={:.3f} kp_pinch={:.3f} dt={}ms", + kp_roll, kp_pinch, time_step_ms); +} + +void GripperControllerNode::set_subscribers_and_publisher() { + const std::string reference_topic = + this->declare_parameter("topics.reference"); + const std::string state_topic = + this->declare_parameter("topics.state"); + const std::string control_topic = + this->declare_parameter("topics.control"); + + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + + reference_sub_ = + this->create_subscription( + reference_topic, qos_sensor_data, + std::bind(&GripperControllerNode::reference_callback, this, + std::placeholders::_1)); + + state_sub_ = + this->create_subscription( + state_topic, qos_sensor_data, + std::bind(&GripperControllerNode::state_callback, this, + std::placeholders::_1)); + + control_pub_ = + this->create_publisher( + control_topic, qos_sensor_data); + + // Timer is created last so that controller_ and the publisher are fully + // initialised before the first publish_control() fires. + control_timer_ = this->create_wall_timer( + time_step_, + std::bind(&GripperControllerNode::publish_control, this)); +} + +// --------------------------------------------------------------------------- +// CALLBACKS +// --------------------------------------------------------------------------- + +void GripperControllerNode::reference_callback( + const vortex_msgs::msg::GripperReferenceFilter::SharedPtr reference_msg) { + std::lock_guard lock(state_mutex_); + roll_ref_ = reference_msg->roll; + pinch_ref_ = reference_msg->pinch; +} + +void GripperControllerNode::state_callback( + const vortex_msgs::msg::GripperState::SharedPtr state_msg) { + std::lock_guard lock(state_mutex_); + roll_measured_ = state_msg->roll; + pinch_measured_ = state_msg->pinch; +} + +// --------------------------------------------------------------------------- +// CONTROL LOOP +// --------------------------------------------------------------------------- + +void GripperControllerNode::publish_control() { + types::GripperState measured_state; + types::GripperState reference_state; + + { + std::lock_guard lock(state_mutex_); + measured_state.roll = roll_measured_; + measured_state.pinch = pinch_measured_; + reference_state.roll = roll_ref_; + reference_state.pinch = pinch_ref_; + } + + const types::Vector2d velocity_command = + controller_.calculate_velocity(measured_state, reference_state); + + vortex_msgs::msg::GripperStateVelocityCommand velocity_command_msg = + gripper_controller::translator::velocity_command_to_gripper_velocity_command_msg( + velocity_command); + velocity_command_msg.header.stamp = this->now(); + + control_pub_->publish(velocity_command_msg); +} + + RCLCPP_COMPONENTS_REGISTER_NODE(GripperControllerNode) + +} // namespace vortex::controller diff --git a/gripper_controller/src/gripper_controller_utils.cpp b/gripper_controller/src/gripper_controller_utils.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/gripper_controller/test/CMakeLists.txt b/gripper_controller/test/CMakeLists.txt new file mode 100644 index 0000000..5441a5c --- /dev/null +++ b/gripper_controller/test/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) + +add_executable( + ${TEST_BINARY_NAME} + test_gripper_controller.cpp +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${CORE_LIB_NAME} + GTest::GTest +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) + +gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/gripper_controller/test/test_gripper_controller.cpp b/gripper_controller/test/test_gripper_controller.cpp new file mode 100644 index 0000000..00dd406 --- /dev/null +++ b/gripper_controller/test/test_gripper_controller.cpp @@ -0,0 +1,210 @@ +#include +#include "gripper_controller/gripper_controller.hpp" +#include "gripper_controller/gripper_controller_typedefs.hpp" + +// --------------------------------------------------------------------------- +// Test fixture +// Sets up a GripperController with identity Kp (gain = 1.0 on both axes) +// so that expected output equals the position error exactly, up to saturation. +// --------------------------------------------------------------------------- +class GripperControllerTest : public ::testing::Test { + protected: + void SetUp() override { + types::Matrix2d identity_gain = types::Matrix2d::Identity(); + controller_.set_kp(identity_gain); + controller_.set_time_step(0.01); + } + + // @brief Helper: run calculate_velocity with explicit scalar inputs. + types::Vector2d compute_velocity(double measured_roll, + double measured_pinch, + double reference_roll, + double reference_pinch) { + types::GripperState measured_state; + measured_state.roll = measured_roll; + measured_state.pinch = measured_pinch; + + types::GripperState reference_state; + reference_state.roll = reference_roll; + reference_state.pinch = reference_pinch; + + return controller_.calculate_velocity(measured_state, reference_state); + } + + GripperController controller_; +}; + +// --------------------------------------------------------------------------- +// Zero error +// --------------------------------------------------------------------------- + +TEST_F(GripperControllerTest, ZeroError_ProducesZeroVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 0.0, 0.0); + + EXPECT_DOUBLE_EQ(velocity_command(0), 0.0); + EXPECT_DOUBLE_EQ(velocity_command(1), 0.0); +} + +TEST_F(GripperControllerTest, StateEqualsReference_ProducesZeroVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.5, 0.3, 0.5, 0.3); + + EXPECT_DOUBLE_EQ(velocity_command(0), 0.0); + EXPECT_DOUBLE_EQ(velocity_command(1), 0.0); +} + +// --------------------------------------------------------------------------- +// Correct sign and axis isolation +// --------------------------------------------------------------------------- + +TEST_F(GripperControllerTest, PositiveRollError_ProducesPositiveRollVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 0.5, 0.0); + + EXPECT_GT(velocity_command(0), 0.0); + EXPECT_DOUBLE_EQ(velocity_command(1), 0.0); +} + +TEST_F(GripperControllerTest, NegativeRollError_ProducesNegativeRollVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.5, 0.0, 0.0, 0.0); + + EXPECT_LT(velocity_command(0), 0.0); + EXPECT_DOUBLE_EQ(velocity_command(1), 0.0); +} + +TEST_F(GripperControllerTest, PositivePinchError_ProducesPositivePinchVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 0.0, 0.5); + + EXPECT_DOUBLE_EQ(velocity_command(0), 0.0); + EXPECT_GT(velocity_command(1), 0.0); +} + +TEST_F(GripperControllerTest, NegativePinchError_ProducesNegativePinchVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.0, 0.5, 0.0, 0.0); + + EXPECT_DOUBLE_EQ(velocity_command(0), 0.0); + EXPECT_LT(velocity_command(1), 0.0); +} + +// --------------------------------------------------------------------------- +// Proportionality — with identity Kp, output equals error exactly +// (as long as error stays within saturation limits) +// --------------------------------------------------------------------------- + +TEST_F(GripperControllerTest, OutputProportionalToError_WithinSaturationLimits) { + const double roll_error = 0.3; + const double pinch_error = 0.6; + + const types::Vector2d velocity_command = + compute_velocity(0.0, 0.0, roll_error, pinch_error); + + EXPECT_NEAR(velocity_command(0), roll_error, 1e-9); + EXPECT_NEAR(velocity_command(1), pinch_error, 1e-9); +} + +// --------------------------------------------------------------------------- +// Velocity saturation +// --------------------------------------------------------------------------- + +TEST_F(GripperControllerTest, LargePositiveRollError_SaturatesAtMaxRollVelocity) { + // Error of 100.0 with gain 1.0 would produce 100.0 — well above MAX_ROLL_VEL + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 100.0, 0.0); + + EXPECT_DOUBLE_EQ(velocity_command(0), types::MAX_ROLL_VEL); +} + +TEST_F(GripperControllerTest, LargeNegativeRollError_SaturatesAtNegativeMaxRollVelocity) { + const types::Vector2d velocity_command = compute_velocity(100.0, 0.0, 0.0, 0.0); + + EXPECT_DOUBLE_EQ(velocity_command(0), -types::MAX_ROLL_VEL); +} + +TEST_F(GripperControllerTest, LargePositivePinchError_SaturatesAtMaxPinchVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 0.0, 100.0); + + EXPECT_DOUBLE_EQ(velocity_command(1), types::MAX_PINCH_VEL); +} + +TEST_F(GripperControllerTest, LargeNegativePinchError_SaturatesAtNegativeMaxPinchVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.0, 100.0, 0.0, 0.0); + + EXPECT_DOUBLE_EQ(velocity_command(1), -types::MAX_PINCH_VEL); +} + +TEST_F(GripperControllerTest, SaturationIsSymmetric_RollAxis) { + const types::Vector2d positive_saturation = compute_velocity(0.0, 0.0, 100.0, 0.0); + const types::Vector2d negative_saturation = compute_velocity(100.0, 0.0, 0.0, 0.0); + + EXPECT_DOUBLE_EQ(positive_saturation(0), -negative_saturation(0)); +} + +TEST_F(GripperControllerTest, SaturationIsSymmetric_PinchAxis) { + const types::Vector2d positive_saturation = compute_velocity(0.0, 0.0, 0.0, 100.0); + const types::Vector2d negative_saturation = compute_velocity(0.0, 100.0, 0.0, 0.0); + + EXPECT_DOUBLE_EQ(positive_saturation(1), -negative_saturation(1)); +} + +// --------------------------------------------------------------------------- +// Gain matrix effects +// --------------------------------------------------------------------------- + +TEST_F(GripperControllerTest, ZeroGainMatrix_AlwaysProducesZeroVelocity) { + controller_.set_kp(types::Matrix2d::Zero()); + + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 1.0, 1.0); + + EXPECT_DOUBLE_EQ(velocity_command(0), 0.0); + EXPECT_DOUBLE_EQ(velocity_command(1), 0.0); +} + +TEST_F(GripperControllerTest, ScaledDiagonalGain_ScalesOutputCorrectly) { + types::Matrix2d scaled_gain = types::Matrix2d::Zero(); + scaled_gain(0, 0) = 0.5; // roll gain + scaled_gain(1, 1) = 2.0; // pinch gain + controller_.set_kp(scaled_gain); + + // Error of 0.4 on both axes — both stay within saturation after scaling + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 0.4, 0.4); + + EXPECT_NEAR(velocity_command(0), 0.5 * 0.4, 1e-9); + EXPECT_NEAR(velocity_command(1), 2.0 * 0.4, 1e-9); +} + +TEST_F(GripperControllerTest, OffDiagonalGain_CrossCouplingPropagatesCorrectly) { + // Set cross-coupling: roll error should also drive pinch output + types::Matrix2d cross_coupled_gain = types::Matrix2d::Identity(); + cross_coupled_gain(1, 0) = 0.5; // pinch output gains from roll error + controller_.set_kp(cross_coupled_gain); + + // Pure roll error of 0.4 + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 0.4, 0.0); + + EXPECT_NEAR(velocity_command(0), 0.4, 1e-9); // direct roll term + EXPECT_NEAR(velocity_command(1), 0.5 * 0.4, 1e-9); // cross-coupled pinch term +} + +// --------------------------------------------------------------------------- +// set_kp takes effect on the next call +// --------------------------------------------------------------------------- + +TEST_F(GripperControllerTest, UpdatedGain_TakesEffectImmediately) { + // First call with identity gain + const types::Vector2d first_velocity = compute_velocity(0.0, 0.0, 0.4, 0.0); + EXPECT_NEAR(first_velocity(0), 0.4, 1e-9); + + // Double the roll gain + types::Matrix2d doubled_gain = types::Matrix2d::Identity(); + doubled_gain(0, 0) = 2.0; + controller_.set_kp(doubled_gain); + + const types::Vector2d second_velocity = compute_velocity(0.0, 0.0, 0.4, 0.0); + EXPECT_NEAR(second_velocity(0), 0.8, 1e-9); +} + +// --------------------------------------------------------------------------- +// Entry point +// --------------------------------------------------------------------------- + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/guidance/CMakeLists.txt b/gripper_guidance/CMakeLists.txt similarity index 89% rename from guidance/CMakeLists.txt rename to gripper_guidance/CMakeLists.txt index d775446..e873456 100644 --- a/guidance/CMakeLists.txt +++ b/gripper_guidance/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(reference_filter_dp) +project(gripper_reference_filter) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 20) @@ -27,8 +27,8 @@ include_directories(include) set(LIB_NAME "${PROJECT_NAME}_component") add_library(${LIB_NAME} SHARED - src/reference_filter.cpp - src/reference_filter_ros.cpp) + src/gripper_reference_filter.cpp + src/gripper_reference_filter_ros.cpp) ament_target_dependencies(${LIB_NAME} PUBLIC rclcpp @@ -46,7 +46,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC rclcpp_components_register_node( ${LIB_NAME} - PLUGIN "ReferenceFilterNode" + PLUGIN "vortex::guidance::GripperReferenceFilterNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/guidance/README.md b/gripper_guidance/README.md similarity index 100% rename from guidance/README.md rename to gripper_guidance/README.md diff --git a/gripper_guidance/config/gripper_reference_filter_params.yaml b/gripper_guidance/config/gripper_reference_filter_params.yaml new file mode 100644 index 0000000..d50adda --- /dev/null +++ b/gripper_guidance/config/gripper_reference_filter_params.yaml @@ -0,0 +1,14 @@ + /**: + ros__parameters: + # Filter dynamics parameters + zeta: [1.0, 1.0] + omega: [0.2, 0.2] + + # Topics + topics: + guidance_gripper: "vortex/gripper/guidance" + gripper_state: "vortex/gripper/state" + + # Action servers + action_servers: + gripper_reference_filter: "vortex/gripper/reference_filter" diff --git a/guidance/include/gripper_reference_filter/gripper_eigen_typedefs.hpp b/gripper_guidance/include/gripper_reference_filter/gripper_eigen_typedefs.hpp similarity index 100% rename from guidance/include/gripper_reference_filter/gripper_eigen_typedefs.hpp rename to gripper_guidance/include/gripper_reference_filter/gripper_eigen_typedefs.hpp diff --git a/guidance/include/gripper_reference_filter/gripper_reference_filter.hpp b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter.hpp similarity index 90% rename from guidance/include/gripper_reference_filter/gripper_reference_filter.hpp rename to gripper_guidance/include/gripper_reference_filter/gripper_reference_filter.hpp index 38de432..3f42390 100644 --- a/guidance/include/gripper_reference_filter/gripper_reference_filter.hpp +++ b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter.hpp @@ -12,7 +12,7 @@ struct GripperReferenceFilterParams { class GripperReferenceFilter { public: - explicit GripperReferenceFilter(const ReferenceFilterParams& params); + explicit GripperReferenceFilter(const GripperReferenceFilterParams& params); // @brief Calculate the state derivative // @param x The state vector 6x1 @@ -20,7 +20,7 @@ class GripperReferenceFilter { // @return The state derivative 6x1 // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen // 2021 p. 336 eq: 12.5 - Eigen::Vector2d calculate_x_dot(const Eigen::Vector6d& x, + Eigen::Vector6d calculate_x_dot(const Eigen::Vector6d& x, const Eigen::Vector2d& r); // @brief Calculate the state transition matrix diff --git a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp new file mode 100644 index 0000000..75e4889 --- /dev/null +++ b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp @@ -0,0 +1,127 @@ +#ifndef GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_HPP_ +#define GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "gripper_reference_filter/gripper_eigen_typedefs.hpp" +#include "gripper_reference_filter/gripper_reference_filter.hpp" + +namespace vortex::guidance { + +class GripperReferenceFilterNode : public rclcpp::Node { + public: + explicit GripperReferenceFilterNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + private: + // @brief Set the subscribers and publishers + void set_subscribers_and_publisher(); + + // @brief Set the action server + void set_action_server(); + + // @brief Initializes the reference filter with ROS parameters + void set_refererence_filter(); + + // @brief Callback for incoming gripper state messages + // @param msg The gripper state message + void reference_callback( + const vortex_msgs::msg::GripperState::SharedPtr msg); + + // @brief Handle the goal request + // @param uuid The goal UUID + // @param goal The goal message + // @return The goal response + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr< + const vortex_msgs::action::GripperReferenceFilterWaypoint::Goal> goal); + + // @brief Handle the cancel request + // @param goal_handle The goal handle + // @return The cancel response + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr> goal_handle); + + // @brief Handle the accepted request + // @param goal_handle The goal handle + void handle_accepted( + const std::shared_ptr> goal_handle); + + // @brief Execute the goal + // @param goal_handle The goal handle + void execute( + const std::shared_ptr> goal_handle); + + // @brief Fill the initial state vector from current gripper state + // @return 6D state vector [roll, pinch, roll_dot, pinch_dot, roll_dotdot, pinch_dotdot] + Eigen::Vector6d fill_reference_state(); + + // @brief Fill the reference goal vector from a GripperWaypoint + // @param goal The gripper waypoint message + // @return 2D reference vector [roll, pinch] + Eigen::Vector2d fill_reference_goal( + const vortex_msgs::msg::GripperWaypoint& goal); + + // @brief Apply mode logic to the reference vector + // @param reference_in The input reference vector + // @param mode The mode (ROLL_AND_PINCH, ONLY_ROLL, ONLY_PINCH) + // @return The modified reference vector + Eigen::Vector2d apply_mode_logic( + const Eigen::Vector2d& reference_in, uint8_t mode); + + // @brief Publish a hold reference message at the current gripper state + void publish_hold_reference(); + + // @brief Fill the reference filter output message from the current state + // @return The gripper reference filter message + vortex_msgs::msg::GripperReferenceFilter fill_reference_msg(); + + // Action server + rclcpp_action::Server< + vortex_msgs::action::GripperReferenceFilterWaypoint>::SharedPtr + action_server_; + + // Reference filter instance + std::unique_ptr gripper_reference_filter_{}; + + // Publisher: smoothed reference to controller + rclcpp::Publisher::SharedPtr + reference_pub_; + + // Subscriber: raw gripper state from hardware + rclcpp::Subscription::SharedPtr + reference_sub_; + + std::chrono::milliseconds time_step_{}; + + // x_ is [roll, pinch, roll_dot, pinch_dot, roll_dotdot, pinch_dotdot] + // 6D state vector (2 DOF x position/velocity/acceleration) + Eigen::Vector6d x_; + + // reference_ holds the live gripper state [roll, pinch] from the subscriber + Eigen::Vector2d reference_; + + std::mutex mutex_; + + rclcpp_action::GoalUUID preempted_goal_id_; + + std::shared_ptr> + goal_handle_; + + rclcpp::CallbackGroup::SharedPtr cb_group_; +}; + +} // namespace vortex::guidance + +#endif // GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_HPP_ diff --git a/gripper_guidance/launch/gripper_reference_filter.launch.py b/gripper_guidance/launch/gripper_reference_filter.launch.py new file mode 100644 index 0000000..d1f7fab --- /dev/null +++ b/gripper_guidance/launch/gripper_reference_filter.launch.py @@ -0,0 +1,32 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + # Path to the specific gripper reference filter parameters + config_file_path = os.path.join( + get_package_share_directory("gripper_reference_filter"), + "config", + "gripper_reference_filter_params.yaml", + ) + + container = ComposableNodeContainer( + name='gripper_reference_filter_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='gripper_reference_filter', + # THIS PLUGIN NAME MUST MATCH YOUR NAMESPACE AND CLASS NAME EXACTLY + plugin='vortex::guidance::GripperReferenceFilterNode', + name='gripper_reference_filter_node', + parameters=[config_file_path], + ) + ], + output='screen', + ) + + return LaunchDescription([container]) diff --git a/guidance/package.xml b/gripper_guidance/package.xml similarity index 78% rename from guidance/package.xml rename to gripper_guidance/package.xml index 40793be..baef78d 100644 --- a/guidance/package.xml +++ b/gripper_guidance/package.xml @@ -1,10 +1,10 @@ - reference_filter_dp + gripper_reference_filter 0.0.0 - Reference model for the DP controller - andeshog + Reference filter for the gripper + patricsh MIT ament_cmake diff --git a/gripper_guidance/src/gripper_reference_filter.cpp b/gripper_guidance/src/gripper_reference_filter.cpp new file mode 100644 index 0000000..afd6dce --- /dev/null +++ b/gripper_guidance/src/gripper_reference_filter.cpp @@ -0,0 +1,42 @@ +#include "gripper_reference_filter/gripper_reference_filter.hpp" + +namespace vortex::guidance { + +GripperReferenceFilter::GripperReferenceFilter(const GripperReferenceFilterParams& params) { + calculate_Ad(params.omega, params.zeta); + calculate_Bd(params.omega); +} + +Eigen::Vector6d GripperReferenceFilter::calculate_x_dot(const Eigen::Vector6d& x, + const Eigen::Vector2d& r) { + Eigen::Vector6d x_dot = Ad_ * x + Bd_ * r; + + return x_dot; +} + + +void GripperReferenceFilter::calculate_Ad(const Eigen::Vector2d& omega, + const Eigen::Vector2d& zeta) { + Eigen::Matrix2d omega_diag = omega.asDiagonal(); + Eigen::Matrix2d zeta_diag = zeta.asDiagonal(); + Eigen::Matrix2d omega_diag_squared = omega_diag * omega_diag; + Eigen::Matrix2d omega_diag_cubed = omega_diag_squared * omega_diag; + + Ad_.block<2,2>(0, 2) = Eigen::Matrix2d::Identity(); + + Ad_.block<2,2>(2, 4) = Eigen::Matrix2d::Identity(); + + Ad_.block<2,2>(4, 0) = -omega_diag_cubed; + Ad_.block<2,2>(4, 2) = -2 * (2*zeta_diag + Eigen::Matrix2d::Identity()) * omega_diag_squared; + Ad_.block<2,2>(4, 4) = -2 * (2*zeta_diag + Eigen::Matrix2d::Identity()) * omega_diag; +} + +void GripperReferenceFilter::calculate_Bd(const Eigen::Vector2d& omega) { + Eigen::Matrix2d omega_diag = omega.asDiagonal(); + Eigen::Matrix2d omega_diag_squared = omega_diag * omega_diag; + Eigen::Matrix2d omega_diag_cubed = omega_diag_squared * omega_diag; + + Bd_.block<2,2>(4, 0) = omega_diag_cubed; +} + +} // namespace vortex::guidance diff --git a/gripper_guidance/src/gripper_reference_filter_ros.cpp b/gripper_guidance/src/gripper_reference_filter_ros.cpp new file mode 100644 index 0000000..2dd8033 --- /dev/null +++ b/gripper_guidance/src/gripper_reference_filter_ros.cpp @@ -0,0 +1,318 @@ +#include "gripper_reference_filter/gripper_reference_filter_ros.hpp" +#include +#include +#include +#include +#include +#include + +const auto start_message = R"( + + ____ _ ____ __ _____ _ _ _ + / ___|_ __ (_)_ __ _ __ ___ _ __ | _ \ ___ / _| ___ _ __ ___ _ __ ___ ___ | ___(_) | |_ ___ _ __ +| | _| '_ \| | '_ \| '_ \ / _ \ '__| | |_) / _ \ |_ / _ \ '__/ _ \ '_ \ / __/ _ \ | |_ | | | __/ _ \ '__| +| |_| | |_) | | |_) | |_) | __/ | | _ < __/ _| __/ | | __/ | | | (_| __/ | _| | | | || __/ | + \____| .__/|_| .__/| .__/ \___|_| |_| \_\___|_| \___|_| \___|_| |_|\___\___| |_| |_|_|\__\\___|_| + |_| |_| |_| + + )"; + +namespace vortex::guidance { + +GripperReferenceFilterNode::GripperReferenceFilterNode(const rclcpp::NodeOptions& options) + : Node("gripper_reference_filter_node", options) { + time_step_ = std::chrono::milliseconds(10); + + set_subscribers_and_publisher(); + + set_action_server(); + + set_refererence_filter(); + + spdlog::info(start_message); +} + +void GripperReferenceFilterNode::set_subscribers_and_publisher() { + std::string guidance_topic = + this->declare_parameter("topics.guidance_gripper"); + std::string gripper_state_topic = + this->declare_parameter("topics.gripper_state"); + + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + + reference_pub_ = this->create_publisher( + guidance_topic, qos_sensor_data); + + reference_sub_ = this->create_subscription( + gripper_state_topic, qos_sensor_data, + std::bind(&GripperReferenceFilterNode::reference_callback, this, + std::placeholders::_1)); +} + +void GripperReferenceFilterNode::set_action_server() { + this->declare_parameter("action_servers.gripper_reference_filter"); + std::string action_server_name = + this->get_parameter("action_servers.gripper_reference_filter").as_string(); + cb_group_ = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + action_server_ = rclcpp_action::create_server< + vortex_msgs::action::GripperReferenceFilterWaypoint>( + + this, + + action_server_name, + + std::bind(&GripperReferenceFilterNode::handle_goal, this, + std::placeholders::_1, std::placeholders::_2), + + std::bind(&GripperReferenceFilterNode::handle_cancel, this, + std::placeholders::_1), + + std::bind(&GripperReferenceFilterNode::handle_accepted, this, + std::placeholders::_1), + + rcl_action_server_get_default_options(), cb_group_); +} + +void GripperReferenceFilterNode::set_refererence_filter() { + this->declare_parameter>("zeta"); + this->declare_parameter>("omega"); + + std::vector zeta = this->get_parameter("zeta").as_double_array(); + std::vector omega = this->get_parameter("omega").as_double_array(); + + Eigen::Vector2d zeta_eigen = Eigen::Map(zeta.data()); + Eigen::Vector2d omega_eigen = Eigen::Map(omega.data()); + + GripperReferenceFilterParams filter_params{omega_eigen, zeta_eigen}; + gripper_reference_filter_ = std::make_unique(filter_params); +} + +void GripperReferenceFilterNode::reference_callback( + const vortex_msgs::msg::GripperState::SharedPtr msg) { + std::lock_guard lock(mutex_); + reference_ << msg->roll, msg->pinch; + } + +rclcpp_action::GoalResponse GripperReferenceFilterNode::handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr + goal) { + (void)uuid; + (void)goal; + { + std::lock_guard lock(mutex_); + if (goal_handle_) { + if (goal_handle_->is_active()) { + spdlog::info("Aborting current goal and accepting new goal"); + preempted_goal_id_ = goal_handle_->get_goal_id(); + } + } + } + spdlog::info("Accepted goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse GripperReferenceFilterNode::handle_cancel( + const std::shared_ptr> goal_handle) { + spdlog::info("Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; +} + + +void GripperReferenceFilterNode::handle_accepted( + const std::shared_ptr> goal_handle) { + std::thread{std::bind(&GripperReferenceFilterNode::execute, this, + std::placeholders::_1), goal_handle}.detach(); + } + + +//void GripperReferenceFilterNode::handle_accepted( +// const std::shared_ptr> goal_handle) { +// execute(goal_handle); +//} + +Eigen::Vector6d GripperReferenceFilterNode::fill_reference_state() { + + Eigen::Vector6d x = Eigen::Vector6d::Zero(); + + x(0) = vortex::utils::math::ssa(reference_(0)); //roll + x(1) = reference_(1); //pinch + + x(2) = 0.0; //roll_dot + x(3) = 0.0; //pinch_dot + + x(4) = 0.0; //roll_dotdot + x(5) = 0.0; //pinch_dotdot + + return x; +} + +Eigen::Vector2d GripperReferenceFilterNode::fill_reference_goal( + const vortex_msgs::msg::GripperWaypoint& goal) { + + double roll{goal.roll.roll}; + double pinch{goal.pinch.pinch}; + + Eigen::Vector2d reference; + reference << roll, pinch; + + return reference; +} + +vortex_msgs::msg::GripperReferenceFilter GripperReferenceFilterNode::fill_reference_msg() { + vortex_msgs::msg::GripperReferenceFilter feedback_msg; + + feedback_msg.roll = vortex::utils::math::ssa(x_(0)); + feedback_msg.pinch = x_(1); + + feedback_msg.roll_dot = x_(2); + feedback_msg.pinch_dot = x_(3); + + feedback_msg.roll_dotdot = x_(4); + feedback_msg.pinch_dotdot = x_(5); + + return feedback_msg; +} + +Eigen::Vector2d GripperReferenceFilterNode::apply_mode_logic( + const Eigen::Vector2d& reference_in, uint8_t mode) { + + Eigen::Vector2d reference_out = reference_in; + + switch (mode) { + case vortex_msgs::msg::GripperWaypoint::ROLL_AND_PINCH: + break; + + case vortex_msgs::msg::GripperWaypoint::ONLY_ROLL: + reference_out(1) = reference_(1); + break; + + case vortex_msgs::msg::GripperWaypoint::ONLY_PINCH: + reference_out(0) = vortex::utils::math::ssa(reference_(0)); + break; + } + + return reference_out; +} + +void GripperReferenceFilterNode::publish_hold_reference() { + if (!reference_pub_) { + return; + } + const double roll = reference_(0); + const double pinch = reference_(1); + + vortex_msgs::msg::GripperReferenceFilter hold_msg; + hold_msg.roll = vortex::utils::math::ssa(roll); + hold_msg.pinch = pinch; + + hold_msg.roll_dot = 0.0; + hold_msg.pinch_dot = 0.0; + + hold_msg.roll_dotdot = 0.0; + hold_msg.pinch_dotdot = 0.0; + + reference_pub_->publish(hold_msg); +} + +void GripperReferenceFilterNode::execute( + const std::shared_ptr> goal_handle) { + { + std::lock_guard lock(mutex_); + this->goal_handle_ = goal_handle; + } + + spdlog::info("Executing goal"); + + x_ = fill_reference_state(); + + const vortex_msgs::msg::GripperWaypoint goal = + goal_handle->get_goal()->waypoint; + + uint8_t mode = goal.mode; + + double convergence_threshold = + goal_handle->get_goal()->convergence_threshold; + + if (convergence_threshold <= 0.0) { + convergence_threshold = 0.1; + spdlog::warn( + "GripperReferenceFilter: Invalid convergence_threshold received (<= 0). " + "Using default 0.1"); + } + + Eigen::Vector2d reference_temp = fill_reference_goal(goal); + + reference_ = apply_mode_logic(reference_temp, mode); + + auto feedback = std::make_shared< + vortex_msgs::action::GripperReferenceFilterWaypoint::Feedback>(); + auto result = std::make_shared< + vortex_msgs::action::GripperReferenceFilterWaypoint::Result>(); + + rclcpp::Rate loop_rate(1000.0 / time_step_.count()); + + while (rclcpp::ok()) { + { + std::lock_guard lock(mutex_); + if (goal_handle->get_goal_id() == preempted_goal_id_) { + publish_hold_reference(); + result->success = false; + goal_handle->abort(result); + return; + } + } + { + std::lock_guard lock(mutex_); + if (goal_handle->is_canceling()) { + publish_hold_reference(); + result->success = false; + goal_handle->canceled(result); + spdlog::info("Goal canceled"); + return; + } + } + + Eigen::Vector6d x_dot = gripper_reference_filter_->calculate_x_dot(x_, reference_); + x_ += x_dot * time_step_.count() / 1000.0; + + vortex_msgs::msg::GripperReferenceFilter feedback_msg = fill_reference_msg(); + feedback->reference = feedback_msg; + reference_pub_->publish(feedback_msg); + goal_handle->publish_feedback(feedback); + + if ((x_.head(2) - reference_.head(2)).norm() < convergence_threshold) { + result->success = true; + goal_handle->succeed(result); + x_.head(2) = reference_.head(2); + vortex_msgs::msg::GripperReferenceFilter final_msg = fill_reference_msg(); + reference_pub_->publish(final_msg); + spdlog::info("Goal reached"); + return; + } + + loop_rate.sleep(); + } + + if (!rclcpp::ok() && goal_handle->is_active()) { + auto result = std::make_shared< + vortex_msgs::action::GripperReferenceFilterWaypoint::Result>(); + result->success = false; + try { + goal_handle->abort(result); + } catch (...) { + // Ignore exceptions during shutdown + } + } +} + +} // namespace vortex::guidance + +RCLCPP_COMPONENTS_REGISTER_NODE(vortex::guidance::GripperReferenceFilterNode) diff --git a/guidance/test/CMakeLists.txt b/gripper_guidance/test/CMakeLists.txt similarity index 90% rename from guidance/test/CMakeLists.txt rename to gripper_guidance/test/CMakeLists.txt index 6c00288..99ed167 100644 --- a/guidance/test/CMakeLists.txt +++ b/gripper_guidance/test/CMakeLists.txt @@ -6,7 +6,7 @@ include(GoogleTest) set(TEST_BINARY_NAME ${PROJECT_NAME}_test) add_executable( ${TEST_BINARY_NAME} - test_reference_filter.cpp + test_gripper_reference_filter.cpp ) target_link_libraries( diff --git a/gripper_guidance/test/test_gripper_reference_filter.cpp b/gripper_guidance/test/test_gripper_reference_filter.cpp new file mode 100644 index 0000000..1390454 --- /dev/null +++ b/gripper_guidance/test/test_gripper_reference_filter.cpp @@ -0,0 +1,83 @@ +#include +#include "gripper_reference_filter/gripper_reference_filter.hpp" + +using namespace vortex::guidance; + +class ReferenceFilterTest : public ::testing::Test { +protected: + void SetUp() override { + GripperReferenceFilterParams params; + params.omega << 1.0, 1.0; + params.zeta << 1.0, 1.0; + filter = std::make_unique(params); + } + + Eigen::Vector6d integrate_steps(Eigen::Vector6d x, const Eigen::Vector2d& r, int steps, double dt = 0.01) { + for (int i = 0; i < steps; ++i) { + x += dt * filter->calculate_x_dot(x, r); + } + return x; + } + + bool position_converged_to_reference(const Eigen::Vector6d& x, const Eigen::Vector2d& r, double tol = 1e-2) { + return std::abs(x(0) - r(0)) < tol && std::abs(x(1) - r(1)) < tol; + } + + bool velocity_within_bounds(const Eigen::Vector6d& x, double vel_bound = 10.0, double acc_bound = 100.0) { + return std::abs(x(2)) < vel_bound && std::abs(x(4)) < acc_bound; + } + + bool no_overshoot(const Eigen::Vector6d& x, const Eigen::Vector2d& r, double tol = 1e-6) { + return x(0) <= r(0) + tol && x(1) <= r(1) + tol; + } + + std::unique_ptr filter; +}; + +// Zero input, zero state -> zero derivative +TEST_F(ReferenceFilterTest, ZeroInputZeroState) { + Eigen::Vector6d x_dot = filter->calculate_x_dot(Eigen::Vector6d::Zero(), Eigen::Vector2d::Zero()); + EXPECT_TRUE(x_dot.isZero(1e-9)); +} + +// Step response: x_d should converge to r over time (Fossen Eq. 12.11-12.12) +TEST_F(ReferenceFilterTest, StepResponseConverges) { + Eigen::Vector2d r; + r << 1.0, 0.5; + + Eigen::Vector6d x = integrate_steps(Eigen::Vector6d::Zero(), r, 2000); + + EXPECT_TRUE(position_converged_to_reference(x, r)); +} + +// Velocity and acceleration states should stay bounded throughout transient +TEST_F(ReferenceFilterTest, VelocityAndAccelerationBounded) { + Eigen::Vector2d r; + r << 1.0, 1.0; + Eigen::Vector6d x = Eigen::Vector6d::Zero(); + + for (int i = 0; i < 1000; ++i) { + x += 0.01 * filter->calculate_x_dot(x, r); + EXPECT_TRUE(velocity_within_bounds(x)); + } +} + +// Critically damped (zeta=1) should produce no overshoot +TEST_F(ReferenceFilterTest, NoCriticallyDampedOvershoot) { + Eigen::Vector2d r; + r << 1.0, 1.0; + Eigen::Vector6d x = Eigen::Vector6d::Zero(); + + for (int i = 0; i < 2000; ++i) { + x += 0.01 * filter->calculate_x_dot(x, r); + EXPECT_TRUE(no_overshoot(x, r)); + } +} + +// namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/gripper_interface/include/gripper_interface/interface_to_gripper_state.hpp b/gripper_interface/include/gripper_interface/interface_to_gripper_state.hpp new file mode 100644 index 0000000..f8750be --- /dev/null +++ b/gripper_interface/include/gripper_interface/interface_to_gripper_state.hpp @@ -0,0 +1,24 @@ +// gripper_controller_translator.hpp +// Mirrors gripper_state_translator.hpp on the input side. +// Translates controller velocity commands back through gear ratio +// to a JointState velocity msg for the interface node. + +#include "sensor_msgs/msg/joint_state.hpp" + +// TODO: Must match the gear ratios in gripper_state_translator.hpp exactly. +// If those are updated, update these too — consider sharing via a common header. +constexpr double CONTROLLER_GEAR_RATIO_ROLL = 1.0; // TODO: measure +constexpr double CONTROLLER_GEAR_RATIO_PINCH = 1.0; // TODO: measure + +constexpr std::size_t CONTROLLER_MIN_VELOCITY_SIZE = 2; + +inline sensor_msgs::msg::JointState translate_to_joint_velocity( + double roll_vel, double pinch_vel) +{ + sensor_msgs::msg::JointState msg; + msg.name = {"roll", "grip"}; + msg.velocity = {roll_vel / CONTROLLER_GEAR_RATIO_ROLL, + pinch_vel / CONTROLLER_GEAR_RATIO_PINCH}; + // position and effort left empty — velocity command only + return msg; +} diff --git a/gripper_interface/interface_to_gripper_state/interface_to_gripper_state.cpp b/gripper_interface/interface_to_gripper_state/interface_to_gripper_state.cpp new file mode 100644 index 0000000..4c6e352 --- /dev/null +++ b/gripper_interface/interface_to_gripper_state/interface_to_gripper_state.cpp @@ -0,0 +1,32 @@ +#include +#include +#include "sensor_msgs/msg/joint_state.hpp" +#include "vortex_msgs/msg/gripper_state.hpp" + +namespace vortex::guidance { + +// TODO: Calibrate gear ratios experimentally before deployment. +// Roll: suspect direct drive but may have a worm gear reduction. +// Pinch: leadscrew/rack — gear_ratio_pinch converts encoder radians +// to jaw displacement (metres or radians depending on convention). +constexpr double GEAR_RATIO_ROLL = 1.0; // TODO: measure — direct drive assumed for now +constexpr double GEAR_RATIO_PINCH = 1.0; // TODO: measure — leadscrew pitch unknown + +constexpr std::size_t ENCODER_INDEX_ROLL = 0; // wrist → roll +constexpr std::size_t ENCODER_INDEX_PINCH = 1; // grip → pinch +constexpr std::size_t ENCODER_MIN_SIZE = 2; + +inline vortex_msgs::msg::GripperState translate_to_gripper_state( + const sensor_msgs::msg::JointState::SharedPtr& msg) + { + if (msg->position.size() < ENCODER_MIN_SIZE) { + throw std::runtime_error( + "JointState has fewer than 2 positions — expected [roll, grip]"); + } + + vortex_msgs::msg::GripperState state; + state.roll = msg->position[ENCODER_INDEX_ROLL] * GEAR_RATIO_ROLL; + state.pinch = msg->position[ENCODER_INDEX_PINCH] * GEAR_RATIO_PINCH; + return state; + } +} diff --git a/guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp b/guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp deleted file mode 100644 index 3e59745..0000000 --- a/guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp +++ /dev/null @@ -1,108 +0,0 @@ -#ifndef GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_HPP_ -#define GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_HPP_ - -#include -#include -#include -#include //TODO: Need to change this to gripper_reference_filter_waypoint when made in vortex_msgs -#include -#include "gripper_reference_filter/gripper_eigen_typedefs.hpp" -#include "gripper_reference_filter/gripper_reference_filter.hpp" - -namespace vortex::guidance { - -class GripperReferenceFilterNode : public rclcpp::Node { - public: - explicit GripperReferenceFilterNode( - const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - - private: - // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(); - - // @brief Set the action server - void set_action_server(); - - // @brief Initializes the reference filter with ROS parameters. - void set_refererence_filter(); - - - // @brief Handle the goal request - // @param uuid The goal UUID - // @param goal The goal message - // @return The goal response - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID& uuid, - std::shared_ptr< - const vortex_msgs::action::ReferenceFilterWaypoint::Goal> goal); // TODO: make a GripperReferenceFilterWaypoint in vortex_msgs - - - // @brief Handle the cancel request - // @param goal_handle The goal handle - // @return The cancel response - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr> goal_handle); - - // @brief Handle the accepted request - // @param goal_handle The goal handle - void handle_accepted( - const std::shared_ptr> goal_handle); - - // @brief Execute the goal - // @param goal_handle The goal handle - void execute( - const std::shared_ptr> goal_handle); - - Eigen::Vector6d fill_reference_state(); - - Eigen::Vector2d fill_reference_goal(const geometry_msgs::msg::Pose& goal); - - Eigen::Vector2d apply_mode_logic(const Eigen::Vector2d& r_in, uint8_t mode); - - void publish_hold_reference(); - - vortex_msgs::msg::ReferenceFilter fill_reference_msg(); - - rclcpp_action::Server< - vortex_msgs::action::ReferenceFilterWaypoint>::SharedPtr action_server_; - - std::unique_ptr reference_filter_{}; - - rclcpp::Publisher::SharedPtr - reference_pub_; - - rclcpp::Subscription::SharedPtr //TODO: PoseStamped should become GripperState topic eventually - reference_sub_; - - - - rclcpp::TimerBase::SharedPtr reference_pub_timer_; - - std::chrono::milliseconds time_step_{}; - - - - // x is [nu, nu_dot] (ref. page 336 in Fossen, 2021 - // nu is 2 degree of freedom (roll of the gripper AND pinch) - Eigen::Vector6d x_; - - // The reference signal vector with 2 degrees of freedom [nu] - Eigen::Vector2d r_; - - std::mutex mutex_; - - rclcpp_action::GoalUUID preempted_goal_id_; - - std::shared_ptr> //TODO: this needs to be changed to GripperReferenceFilterWaypoint - goal_handle_; - - rclcpp::CallbackGroup::SharedPtr cb_group_; -}; - -} // namespace vortex::guidance - -#endif // GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_HPP_ diff --git a/guidance/src/gripper_reference_filter.cpp b/guidance/src/gripper_reference_filter.cpp deleted file mode 100644 index a015d7b..0000000 --- a/guidance/src/gripper_reference_filter.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include "gripper_reference_filter/gripper_reference_filter.hpp" - -namespace vortex::guidance { - -GripperReferenceFilter::GripperReferenceFilter(const GripperReferenceFilterParams& params) { - calculate_Ad(params.omega, params.zeta); - calculate_Bd(params.omega); -} - -Eigen::Vector6d GripperReferenceFilter::calculate_x_dot(const Eigen::Vector6d& x, - const Eigen::Vector6d& r) { - Eigen::Vector6d x_dot = Ad_ * x + Bd_ * r; - - return x_dot; -} - -void GripperReferenceFilter::calculate_Ad(const Eigen::Vector6d& omega, - const Eigen::Vector6d& zeta) { - //TODO: Clean up and clarify dimensions to define as per Fossen 336 (12.6) - Eigen::Matrix6d omega_diag = omega.asDiagonal(); - Eigen::Matrix6d delta = zeta.asDiagonal(); - Eigen::Matrix6d omega_diag_squared = omega_diag * omega_diag; - Eigen::Matrix6d omega_diag_cubed = omega_diag * omega_diag * omega_diag; - Ad_.block<6, 6>(0, 6) = Eigen::Matrix6d::Identity(); - Ad_.block<6, 6>(12, 0) = -omega_diag_cubed; - Ad_.block<6, 6>(12, 6) = - -(2 * delta + Eigen::Matrix6d::Identity()) * omega_diag_squared; - Ad_.block<6, 6>(12, 12) = - -(2 * delta + Eigen::Matrix6d::Identity()) * omega_diag; - Ad_.block<6, 6>(6, 12) = Eigen::Matrix6d::Identity(); -} - //TODO: Same thing applies here as per last TODO -void GripperReferenceFilter::calculate_Bd(const Eigen::Vector6d& omega) { - Eigen::Matrix6d omega_diag = omega.asDiagonal(); - Bd_.block<6, 6>(12, 0) = omega_diag * omega_diag * omega_diag; -} - -} // namespace vortex::guidance diff --git a/guidance/src/gripper_reference_filter_ros.cpp b/guidance/src/gripper_reference_filter_ros.cpp deleted file mode 100644 index 001dcf3..0000000 --- a/guidance/src/gripper_reference_filter_ros.cpp +++ /dev/null @@ -1,418 +0,0 @@ -#include "gripper_reference_filter/gripper_reference_filter_ros.hpp" -#include -#include -#include -#include -#include -#include -#include - -const auto start_message = R"( - - ____ _ ____ __ _____ _ _ _ - / ___|_ __ (_)_ __ _ __ ___ _ __ | _ \ ___ / _| ___ _ __ ___ _ __ ___ ___ | ___(_) | |_ ___ _ __ -| | _| '_ \| | '_ \| '_ \ / _ \ '__| | |_) / _ \ |_ / _ \ '__/ _ \ '_ \ / __/ _ \ | |_ | | | __/ _ \ '__| -| |_| | |_) | | |_) | |_) | __/ | | _ < __/ _| __/ | | __/ | | | (_| __/ | _| | | | || __/ | - \____| .__/|_| .__/| .__/ \___|_| |_| \_\___|_| \___|_| \___|_| |_|\___\___| |_| |_|_|\__\\___|_| - |_| |_| |_| - - )"; - -namespace vortex::guidance { - - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -GripperReferenceFilterNode::ReferenceFilterNode(const rclcpp::NodeOptions& options) - : Node("reference_filter_node", options) { - time_step_ = std::chrono::milliseconds(10); - - set_subscribers_and_publisher(); - - set_action_server(); - - set_refererence_filter(); - - spdlog::info(start_message); -} - - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -void GripperReferenceFilterNode::set_subscribers_and_publisher() { - this->declare_parameter("topics.pose"); - this->declare_parameter("topics.twist"); - this->declare_parameter("topics.guidance.dp"); - this->declare_parameter("topics.reference_pose"); - - std::string pose_topic = this->get_parameter("topics.pose").as_string(); - std::string twist_topic = this->get_parameter("topics.twist").as_string(); - std::string guidance_topic = - this->get_parameter("topics.guidance.dp").as_string(); - std::string reference_pose_topic = - this->get_parameter("topics.reference_pose").as_string(); - - auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); - reference_pub_ = this->create_publisher( - guidance_topic, qos_sensor_data); - reference_sub_ = this->create_subscription( - reference_pose_topic, qos_sensor_data, - std::bind(&GripperReferenceFilterNode::reference_callback, this, - std::placeholders::_1)); - pose_sub_ = this->create_subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>( - pose_topic, qos_sensor_data, - std::bind(&GripperReferenceFilterNode::pose_callback, this, - std::placeholders::_1)); - twist_sub_ = this->create_subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>( - twist_topic, qos_sensor_data, - std::bind(&GripperReferenceFilterNode::twist_callback, this, - std::placeholders::_1)); -} - - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -void GripperReferenceFilterNode::set_action_server() { - this->declare_parameter("action_servers.reference_filter"); - std::string action_server_name = - this->get_parameter("action_servers.reference_filter").as_string(); - cb_group_ = - this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - - action_server_ = rclcpp_action::create_server< - vortex_msgs::action::ReferenceFilterWaypoint>( - this, action_server_name, - std::bind(&GripperReferenceFilterNode::handle_goal, this, - std::placeholders::_1, std::placeholders::_2), - std::bind(&GripperReferenceFilterNode::handle_cancel, this, - std::placeholders::_1), - std::bind(&GripperReferenceFilterNode::handle_accepted, this, - std::placeholders::_1), - rcl_action_server_get_default_options(), cb_group_); -} - - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -void GripperReferenceFilterNode::set_refererence_filter() { - this->declare_parameter>("zeta"); - this->declare_parameter>("omega"); - - std::vector zeta = this->get_parameter("zeta").as_double_array(); - std::vector omega = this->get_parameter("omega").as_double_array(); - - Eigen::Vector6d zeta_eigen = Eigen::Map(zeta.data()); - Eigen::Vector6d omega_eigen = Eigen::Map(omega.data()); - - ReferenceFilterParams filter_params{omega_eigen, zeta_eigen}; - reference_filter_ = std::make_unique(filter_params); -} - - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -void GripperReferenceFilterNode::reference_callback( - const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - double x = msg->pose.position.x; - double y = msg->pose.position.y; - double z = msg->pose.position.z; - const auto& o = msg->pose.orientation; - Eigen::Quaterniond q(o.w, o.x, o.y, o.z); - Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); - double roll{euler_angles(0)}; - double pitch{euler_angles(1)}; - double yaw{euler_angles(2)}; - r_ << x, y, z, roll, pitch, yaw; -} - - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -void GripperReferenceFilterNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - std::lock_guard lock(mutex_); - current_pose_ = *msg; -} - - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -void GripperReferenceFilterNode::twist_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - std::lock_guard lock(mutex_); - current_twist_ = *msg; -} - - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -rclcpp_action::GoalResponse GripperReferenceFilterNode::handle_goal( - const rclcpp_action::GoalUUID& uuid, - std::shared_ptr - goal) { - (void)uuid; - (void)goal; - { - std::lock_guard lock(mutex_); - if (goal_handle_) { - if (goal_handle_->is_active()) { - spdlog::info("Aborting current goal and accepting new goal"); - preempted_goal_id_ = goal_handle_->get_goal_id(); - } - } - } - spdlog::info("Accepted goal request"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -rclcpp_action::CancelResponse GripperReferenceFilterNode::handle_cancel( - const std::shared_ptr> goal_handle) { - spdlog::info("Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; -} - - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -void GripperReferenceFilterNode::handle_accepted( - const std::shared_ptr> goal_handle) { - execute(goal_handle); -} - - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -Eigen::Vector18d GripperReferenceFilterNode::fill_reference_state() { - Eigen::Vector18d x = Eigen::Vector18d::Zero(); - x(0) = current_pose_.pose.pose.position.x; - x(1) = current_pose_.pose.pose.position.y; - x(2) = current_pose_.pose.pose.position.z; - - const auto& o = current_pose_.pose.pose.orientation; - Eigen::Quaterniond q(o.w, o.x, o.y, o.z); - Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); - double roll{euler_angles(0)}; - double pitch{euler_angles(1)}; - double yaw{euler_angles(2)}; - - x(3) = vortex::utils::math::ssa(roll); - x(4) = vortex::utils::math::ssa(pitch); - x(5) = vortex::utils::math::ssa(yaw); - - vortex::utils::types::PoseEuler pose{current_pose_.pose.pose.position.x, - current_pose_.pose.pose.position.y, - current_pose_.pose.pose.position.z, - roll, - pitch, - yaw}; - Eigen::Matrix6d J = pose.as_j_matrix(); - vortex::utils::types::Twist twist{current_twist_.twist.twist.linear.x, - current_twist_.twist.twist.linear.y, - current_twist_.twist.twist.linear.z, - current_twist_.twist.twist.angular.x, - current_twist_.twist.twist.angular.y, - current_twist_.twist.twist.angular.z}; - Eigen::Vector6d pose_dot = J * twist.to_vector(); - - x(6) = pose_dot(0); - x(7) = pose_dot(1); - x(8) = pose_dot(2); - x(9) = pose_dot(3); - x(10) = pose_dot(4); - x(11) = pose_dot(5); - - return x; -} - - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -Eigen::Vector6d GripperReferenceFilterNode::fill_reference_goal( - const geometry_msgs::msg::Pose& goal) { - double x{goal.position.x}; - double y{goal.position.y}; - double z{goal.position.z}; - - const auto& o = goal.orientation; - Eigen::Quaterniond q(o.w, o.x, o.y, o.z); - Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); - double roll{euler_angles(0)}; - double pitch{euler_angles(1)}; - double yaw{euler_angles(2)}; - - Eigen::Vector6d r; - r << x, y, z, roll, pitch, yaw; - - return r; -} - - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -vortex_msgs::msg::ReferenceFilter GripperReferenceFilterNode::fill_reference_msg() { - vortex_msgs::msg::ReferenceFilter feedback_msg; - feedback_msg.x = x_(0); - feedback_msg.y = x_(1); - feedback_msg.z = x_(2); - feedback_msg.roll = vortex::utils::math::ssa(x_(3)); - feedback_msg.pitch = vortex::utils::math::ssa(x_(4)); - feedback_msg.yaw = vortex::utils::math::ssa(x_(5)); - feedback_msg.x_dot = x_(6); - feedback_msg.y_dot = x_(7); - feedback_msg.z_dot = x_(8); - feedback_msg.roll_dot = x_(9); - feedback_msg.pitch_dot = x_(10); - feedback_msg.yaw_dot = x_(11); - - return feedback_msg; -} - - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -Eigen::Vector6d GripperReferenceFilterNode::apply_mode_logic( - const Eigen::Vector6d& r_in, - uint8_t mode) { - Eigen::Vector6d r_out = r_in; - - switch (mode) { - case vortex_msgs::msg::Waypoint::FULL_POSE: - break; - - case vortex_msgs::msg::Waypoint::ONLY_POSITION: - r_out(3) = x_(3); - r_out(4) = x_(4); - r_out(5) = x_(5); - break; - - case vortex_msgs::msg::Waypoint::FORWARD_HEADING: { - double dx = r_in(0) - x_(0); - double dy = r_in(1) - x_(1); - - double forward_heading = std::atan2(dy, dx); - - r_out(3) = 0.0; - r_out(4) = 0.0; - r_out(5) = vortex::utils::math::ssa(forward_heading); - break; - } - - case vortex_msgs::msg::Waypoint::ONLY_ORIENTATION: - r_out(0) = x_(0); - r_out(1) = x_(1); - r_out(2) = x_(2); - break; - } - - return r_out; -} - //TODO: Discuss necessary goal with this with Cyprian and Jorgen -void GripperReferenceFilterNode::publish_hold_reference() { - if (!reference_pub_) { - return; - } - - const auto& p = current_pose_.pose.pose.position; - const auto& o = current_pose_.pose.pose.orientation; - - Eigen::Quaterniond q(o.w, o.x, o.y, o.z); - if (q.norm() < 1e-9) { - q = Eigen::Quaterniond::Identity(); - } else { - q.normalize(); - } - - Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); - - vortex_msgs::msg::ReferenceFilter hold_msg; - hold_msg.x = p.x; - hold_msg.y = p.y; - hold_msg.z = p.z; - hold_msg.roll = vortex::utils::math::ssa(euler_angles(0)); - hold_msg.pitch = vortex::utils::math::ssa(euler_angles(1)); - hold_msg.yaw = vortex::utils::math::ssa(euler_angles(2)); - - hold_msg.x_dot = 0.0; - hold_msg.y_dot = 0.0; - hold_msg.z_dot = 0.0; - hold_msg.roll_dot = 0.0; - hold_msg.pitch_dot = 0.0; - hold_msg.yaw_dot = 0.0; - - reference_pub_->publish(hold_msg); -} - -void GripperReferenceFilterNode::execute( - const std::shared_ptr> goal_handle) { //TODO: Outdated vortex_msgs, change to the GripperReferenceFilterWaypoint when its made - { - std::lock_guard lock(mutex_); - this->goal_handle_ = goal_handle; - } - - spdlog::info("Executing goal"); - - x_ = fill_reference_state(); - //TODO: change all this given the new waypoint server for the GripperReferenceFilterNode - const geometry_msgs::msg::Pose goal = - goal_handle->get_goal()->waypoint.pose; - uint8_t mode = goal_handle->get_goal()->waypoint.mode; - double convergence_threshold = - goal_handle->get_goal()->convergence_threshold; - - if (convergence_threshold <= 0.0) { - convergence_threshold = 0.1; - spdlog::warn( - "GripperReferenceFilter: Invalid convergence_threshold received (<= 0). " - "Using default 0.1"); - } - - Eigen::Vector6d r_temp = fill_reference_goal(goal); - r_ = apply_mode_logic(r_temp, mode); - - auto feedback = std::make_shared< - vortex_msgs::action::ReferenceFilterWaypoint::Feedback>(); - auto result = std::make_shared< - vortex_msgs::action::ReferenceFilterWaypoint::Result>(); - - rclcpp::Rate loop_rate(1000.0 / time_step_.count()); - - while (rclcpp::ok()) { - { - std::lock_guard lock(mutex_); - if (goal_handle->get_goal_id() == preempted_goal_id_) { - publish_hold_reference(); - result->success = false; - goal_handle->abort(result); - return; - } - } - { - std::lock_guard lock(mutex_); - if (goal_handle->is_canceling()) { - publish_hold_reference(); - result->success = false; - goal_handle->canceled(result); - spdlog::info("Goal canceled"); - return; - } - } - Eigen::Vector18d x_dot = reference_filter_->calculate_x_dot(x_, r_); - x_ += x_dot * time_step_.count() / 1000.0; - - vortex_msgs::msg::ReferenceFilter feedback_msg = fill_reference_msg(); - - feedback->reference = feedback_msg; - - goal_handle->publish_feedback(feedback); - reference_pub_->publish(feedback_msg); - - if ((x_.head(6) - r_.head(6)).norm() < convergence_threshold) { - result->success = true; - goal_handle->succeed(result); - x_.head(6) = r_.head(6); - vortex_msgs::msg::ReferenceFilter feedback_msg = - fill_reference_msg(); - reference_pub_->publish(feedback_msg); - spdlog::info("Goal reached"); - return; - } - - loop_rate.sleep(); - } - if (!rclcpp::ok() && goal_handle->is_active()) { - auto result = std::make_shared< - vortex_msgs::action::ReferenceFilterWaypoint::Result>(); - result->success = false; - - try { - goal_handle->abort(result); - } catch (...) { - // Ignore exceptions during shutdown - } - } -} - -RCLCPP_COMPONENTS_REGISTER_NODE(GripperReferenceFilterNode) - -} // namespace vortex::guidance diff --git a/guidance/test/test_gripper_reference_filter.cpp b/guidance/test/test_gripper_reference_filter.cpp index 5b42962..a4fc23e 100644 --- a/guidance/test/test_gripper_reference_filter.cpp +++ b/guidance/test/test_gripper_reference_filter.cpp @@ -1,55 +1,80 @@ -#include + #include +#include "gripper_reference_filter/gripper_reference_filter.hpp" -#include "reference_filter_dp/eigen_typedefs.hpp" -#include "reference_filter_dp/reference_filter.hpp" +using namespace vortex::guidance; -namespace vortex::guidance { +class ReferenceFilterTest : public ::testing::Test { +protected: + void SetUp() override { + GripperReferenceFilterParams params; + params.omega << 1.0, 1.0; + params.zeta << 1.0, 1.0; + filter = std::make_unique(params); + } + + Eigen::Vector6d integrate_steps(Eigen::Vector6d x, const Eigen::Vector2d& r, int steps, double dt = 0.01) { + for (int i = 0; i < steps; ++i) { + x += dt * filter->calculate_x_dot(x, r); + } + return x; + } + + bool position_converged_to_reference(const Eigen::Vector6d& x, const Eigen::Vector2d& r, double tol = 1e-2) { + return std::abs(x(0) - r(0)) < tol && std::abs(x(1) - r(1)) < tol; + } -class ReferenceFilterTests : public ::testing::Test { - protected: - ReferenceFilterTests() : reference_filter_{get_filter_params()} {} + bool velocity_within_bounds(const Eigen::Vector6d& x, double vel_bound = 10.0, double acc_bound = 100.0) { + return std::abs(x(2)) < vel_bound && std::abs(x(4)) < acc_bound; + } - ReferenceFilterParams get_filter_params() { - ReferenceFilterParams params; - params.omega = Eigen::Vector6d::Ones(); - params.zeta = Eigen::Vector6d::Ones(); - return params; + bool no_overshoot(const Eigen::Vector6d& x, const Eigen::Vector2d& r, double tol = 1e-6) { + return x(0) <= r(0) + tol && x(1) <= r(1) + tol; } - ReferenceFilter reference_filter_; + std::unique_ptr filter; }; -TEST_F(ReferenceFilterTests, T01_positive_command_positive_xdot) { - Eigen::Vector18d x = Eigen::Vector18d::Zero(); - Eigen::Vector6d r = Eigen::Vector6d::Ones(); - Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); +// Zero input, zero state -> zero derivative +TEST_F(ReferenceFilterTest, ZeroInputZeroState) { + Eigen::Vector6d x_dot = filter->calculate_x_dot(Eigen::Vector6d::Zero(), Eigen::Vector2d::Zero()); + EXPECT_TRUE(x_dot.isZero(1e-9)); +} - for (int i = 0; i < xdot.size(); ++i) { - EXPECT_GE(xdot(i), 0.0); - } +// Step response: x_d should converge to r over time (Fossen Eq. 12.11-12.12) +TEST_F(ReferenceFilterTest, StepResponseConverges) { + Eigen::Vector2d r; + r << 1.0, 0.5; + + Eigen::Vector6d x = integrate_steps(Eigen::Vector6d::Zero(), r, 2000); + + EXPECT_TRUE(position_converged_to_reference(x, r)); } -TEST_F(ReferenceFilterTests, T02_negative_command_negative_xdot) { - Eigen::Vector18d x = Eigen::Vector18d::Zero(); - Eigen::Vector6d r = -Eigen::Vector6d::Ones(); - Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); +// Velocity and acceleration states should stay bounded throughout transient +TEST_F(ReferenceFilterTest, VelocityAndAccelerationBounded) { + Eigen::Vector2d r; + r << 1.0, 1.0; + Eigen::Vector6d x = Eigen::Vector6d::Zero(); - for (int i = 0; i < xdot.size(); ++i) { - EXPECT_LE(xdot(i), 0.0); + for (int i = 0; i < 1000; ++i) { + x += 0.01 * filter->calculate_x_dot(x, r); + EXPECT_TRUE(velocity_within_bounds(x)); } } -TEST_F(ReferenceFilterTests, T03_zero_command_zero_xdot) { - Eigen::Vector18d x = Eigen::Vector18d::Zero(); - Eigen::Vector6d r = Eigen::Vector6d::Zero(); - Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); +// Critically damped (zeta=1) should produce no overshoot +TEST_F(ReferenceFilterTest, NoCriticallyDampedOvershoot) { + Eigen::Vector2d r; + r << 1.0, 1.0; + Eigen::Vector6d x = Eigen::Vector6d::Zero(); - for (int i = 0; i < xdot.size(); ++i) { - EXPECT_DOUBLE_EQ(xdot(i), 0.0); + for (int i = 0; i < 2000; ++i) { + x += 0.01 * filter->calculate_x_dot(x, r); + EXPECT_TRUE(no_overshoot(x, r)); } } -} // namespace vortex::guidance +// namespace vortex::guidance int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From 8adf510ccafada259a36386796558d695d3f755f Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Thu, 2 Apr 2026 19:24:59 +0200 Subject: [PATCH 12/24] can -adjusted gripper interface n driver --- .../config/gripper_controller_params.yaml | 2 +- gripper_interface/CMakeListsALT.txt | 88 +++++++++++++ .../gripper_interface/gripper_can_ids.hpp | 25 ++++ .../gripper_interface_can_ros.hpp | 78 ++++++++++++ .../gripper_interface_can_typedefs.hpp | 65 ++++++++++ .../interface_to_gripper_state.hpp | 24 ---- .../gripper_interface_can_translator.hpp | 98 ++++++++++++++ gripper_interface/packageALT.xml | 30 +++++ .../src/gripper_interface_can_node.cpp | 10 ++ .../src/gripper_interface_can_ros.cpp | 120 ++++++++++++++++++ 10 files changed, 515 insertions(+), 25 deletions(-) create mode 100644 gripper_interface/CMakeListsALT.txt create mode 100644 gripper_interface/include/gripper_interface/gripper_can_ids.hpp create mode 100644 gripper_interface/include/gripper_interface/gripper_interface_can_ros.hpp create mode 100644 gripper_interface/include/gripper_interface/gripper_interface_can_typedefs.hpp delete mode 100644 gripper_interface/include/gripper_interface/interface_to_gripper_state.hpp create mode 100644 gripper_interface/interface_to_gripper_state/gripper_interface_can_translator.hpp create mode 100644 gripper_interface/packageALT.xml create mode 100644 gripper_interface/src/gripper_interface_can_node.cpp create mode 100644 gripper_interface/src/gripper_interface_can_ros.cpp diff --git a/gripper_controller/config/gripper_controller_params.yaml b/gripper_controller/config/gripper_controller_params.yaml index 11ff76d..70b0e1e 100644 --- a/gripper_controller/config/gripper_controller_params.yaml +++ b/gripper_controller/config/gripper_controller_params.yaml @@ -15,5 +15,5 @@ reference: "vortex/gripper/guidance" # Input: raw measured gripper state from the interface node state: "vortex/gripper/state" - # Output: velocity command (JointState.velocity) to the interface node + # Output: velocity command to the interface node control: "vortex/gripper/control" diff --git a/gripper_interface/CMakeListsALT.txt b/gripper_interface/CMakeListsALT.txt new file mode 100644 index 0000000..79ec76d --- /dev/null +++ b/gripper_interface/CMakeListsALT.txt @@ -0,0 +1,88 @@ +#change to CMakeLists.txt if working + +cmake_minimum_required(VERSION 3.8) +project(gripper_interface) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include_directories(include) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils_ros REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +# --------------------------------------------------------------------------- +# 1. CAN Driver (static — no ROS dependency, unit-testable in isolation) +# --------------------------------------------------------------------------- +add_library(${PROJECT_NAME}_can_driver STATIC + src/gripper_can_driver.cpp +) +target_include_directories(${PROJECT_NAME}_can_driver PUBLIC include) +target_link_libraries(${PROJECT_NAME}_can_driver PUBLIC spdlog::spdlog fmt::fmt) +# SocketCAN is part of the Linux kernel headers — no extra find_package needed + +# --------------------------------------------------------------------------- +# 2. Component library (shared — ROS node as composable component) +# --------------------------------------------------------------------------- +set(COMPONENT_LIB ${PROJECT_NAME}_component) + +add_library(${COMPONENT_LIB} SHARED + src/gripper_interface_ros.cpp +) +target_link_libraries(${COMPONENT_LIB} PUBLIC + ${PROJECT_NAME}_can_driver +) +ament_target_dependencies(${COMPONENT_LIB} PUBLIC + rclcpp + rclcpp_components + vortex_msgs + vortex_utils_ros + spdlog + fmt +) + +rclcpp_components_register_node( + ${COMPONENT_LIB} + PLUGIN "vortex::interface::GripperInterfaceNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +# --------------------------------------------------------------------------- +# 3. Standalone executable +# --------------------------------------------------------------------------- +add_executable(${PROJECT_NAME}_standalone + src/gripper_interface_node.cpp +) +target_link_libraries(${PROJECT_NAME}_standalone PUBLIC ${COMPONENT_LIB}) +ament_target_dependencies(${PROJECT_NAME}_standalone PUBLIC rclcpp) + +# --------------------------------------------------------------------------- +# Install +# --------------------------------------------------------------------------- +install(TARGETS + ${COMPONENT_LIB} + ${PROJECT_NAME}_can_driver + ${PROJECT_NAME}_standalone + EXPORT export_${COMPONENT_LIB} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY include/ DESTINATION include) +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}/) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_package() diff --git a/gripper_interface/include/gripper_interface/gripper_can_ids.hpp b/gripper_interface/include/gripper_interface/gripper_can_ids.hpp new file mode 100644 index 0000000..cd3a556 --- /dev/null +++ b/gripper_interface/include/gripper_interface/gripper_can_ids.hpp @@ -0,0 +1,25 @@ +#ifndef GRIPPER_INTERFACE__GRIPPER_CAN_IDS_HPP_ +#define GRIPPER_INTERFACE__GRIPPER_CAN_IDS_HPP_ + +#include + +// --------------------------------------------------------------------------- +// CAN IDs — must match can_common.h on the MCU firmware side. +// Update these if the firmware IDs change. +// --------------------------------------------------------------------------- +namespace gripper_can_ids { + +// Host → MCU +constexpr uint32_t SET_PWM = 0x469; // 4 bytes: uint16_t[2] duty_us [roll, pinch] +constexpr uint32_t START_GRIPPER = 0x45a; // 0 bytes: enable servos +constexpr uint32_t STOP_GRIPPER = 0x45b; // 0 bytes: disable servos +constexpr uint32_t RESET_MCU = 0x45c; // 0 bytes: NVIC_SystemReset + +// MCU → Host +constexpr uint32_t SEND_ANGLES = 0x46a; // 6 bytes: uint16_t[3] raw encoder angles + // [wrist, grip, unused] (only first 4 bytes valid + // when NUM_ENCODERS=2) + +} // namespace gripper_can_ids + +#endif // GRIPPER_INTERFACE__GRIPPER_CAN_IDS_HPP_ diff --git a/gripper_interface/include/gripper_interface/gripper_interface_can_ros.hpp b/gripper_interface/include/gripper_interface/gripper_interface_can_ros.hpp new file mode 100644 index 0000000..dcbd0ac --- /dev/null +++ b/gripper_interface/include/gripper_interface/gripper_interface_can_ros.hpp @@ -0,0 +1,78 @@ +#ifndef GRIPPER_INTERFACE__GRIPPER_INTERFACE_ROS_HPP_ +#define GRIPPER_INTERFACE__GRIPPER_INTERFACE_ROS_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +#include "gripper_interface/gripper_can_driver.hpp" +#include "gripper_interface/gripper_interface_translator.hpp" +#include "gripper_interface/gripper_interface_typedefs.hpp" + +// --------------------------------------------------------------------------- +// Responsibility: ROS wiring only. +// - Polls CAN for encoder frames on a timer → publishes GripperState +// - Subscribes to GripperStateVelocityCommand → converts → send_pwm over CAN +// +// The node is the glue between: +// GripperReferenceFilterNode ──(GripperReferenceFilter)──► +// GripperControllerNode ──(GripperStateVelocityCommand)──► +// GripperInterfaceNode ──(CAN SET_PWM)──► MCU +// +// MCU ──(CAN SEND_ANGLES)──► GripperInterfaceNode ──(GripperState)──► +// GripperReferenceFilterNode + GripperControllerNode +// --------------------------------------------------------------------------- + +namespace vortex::interface { + +class GripperInterfaceNode : public rclcpp::Node { +public: + explicit GripperInterfaceNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + ~GripperInterfaceNode() override; + +private: + // Setup + void declare_and_load_parameters(); + void setup_pub_sub(); + void init_can_driver(); + + // Timer: polls CAN socket for encoder frames, publishes GripperState. + // Rate is independent from the controller rate — MCU sends at its own TC0 + // timer rate (~1 Hz based on TC0 period of 46874 @ DIV1024 / 48 MHz). + // The ROS poll timer runs faster to catch frames promptly. + void encoder_poll_callback(); + + // Velocity command from GripperControllerNode → CAN SET_PWM. + void velocity_command_callback( + const vortex_msgs::msg::GripperStateVelocityCommand::SharedPtr msg); + + // --------------------------------------------------------------------------- + // ROS interfaces + // --------------------------------------------------------------------------- + rclcpp::Publisher::SharedPtr state_pub_; + + rclcpp::Subscription::SharedPtr + velocity_cmd_sub_; + + rclcpp::TimerBase::SharedPtr encoder_poll_timer_; + + // --------------------------------------------------------------------------- + // Hardware + // --------------------------------------------------------------------------- + std::unique_ptr can_driver_; + + // --------------------------------------------------------------------------- + // Parameters (loaded once at construction) + // --------------------------------------------------------------------------- + std::string can_interface_; + std::chrono::milliseconds poll_period_; +}; + +} // namespace vortex::interface + +#endif // GRIPPER_INTERFACE__GRIPPER_INTERFACE_ROS_HPP_ diff --git a/gripper_interface/include/gripper_interface/gripper_interface_can_typedefs.hpp b/gripper_interface/include/gripper_interface/gripper_interface_can_typedefs.hpp new file mode 100644 index 0000000..83f0683 --- /dev/null +++ b/gripper_interface/include/gripper_interface/gripper_interface_can_typedefs.hpp @@ -0,0 +1,65 @@ +#ifndef GRIPPER_INTERFACE__GRIPPER_INTERFACE_TYPEDEFS_HPP_ +#define GRIPPER_INTERFACE__GRIPPER_INTERFACE_TYPEDEFS_HPP_ + +#include +#include + +namespace gripper_interface::types { + +// --------------------------------------------------------------------------- +// Raw CAN payloads — exactly what crosses the CAN bus. +// All multi-byte fields are big-endian (MCU packs MSB first). +// --------------------------------------------------------------------------- + +// MCU → Host: 6 bytes, indices [0..1]=wrist, [2..3]=grip, [4..5]=unused +// Raw 14-bit encoder value (AS5048 / compatible) packed in uint16_t. +// NOTE: firmware bug suspected in read_encoders() — buf = out + enc_num +// should likely be out + enc_num*2. Verify on hardware before trusting +// bytes 2-3 when running NUM_ENCODERS=2. +struct RawEncoderFrame { + std::array bytes{}; +}; + +// Host → MCU: 4 bytes = uint16_t[2], duty cycle in microseconds [roll, pinch] +// Servo range: 700 µs (full reverse) – 1500 µs (idle) – 2300 µs (full forward) +struct RawPwmFrame { + std::array bytes{}; +}; + +// Decoded encoder angles in radians +struct EncoderAngles { + double roll = 0.0; // wrist encoder → roll + double pinch = 0.0; // grip encoder → pinch +}; + +// Velocity command from controller (rad/s) +struct VelocityCommand { + double roll_vel = 0.0; + double pinch_vel = 0.0; +}; + +// --------------------------------------------------------------------------- +// Servo / encoder constants +// --------------------------------------------------------------------------- +constexpr double ENCODER_COUNTS = 16383.0; // 0x3FFF — 14-bit AS5048 +constexpr double TWO_PI = 6.283185307179586; + +// Servo PWM limits (microseconds) — from tcc.c comments +constexpr uint16_t PWM_IDLE_US = 1500; +constexpr uint16_t PWM_MAX_US = 2300; +constexpr uint16_t PWM_MIN_US = 700; +constexpr uint16_t PWM_RANGE_US = PWM_MAX_US - PWM_IDLE_US; // 800 + +// Gear ratios — both assumed 1.0 until experimentally calibrated +// TODO: measure worm/leadscrew reduction after servo installation +constexpr double GEAR_RATIO_ROLL = 1.0; +constexpr double GEAR_RATIO_PINCH = 1.0; + +// Velocity saturation — must match types::MAX_ROLL_VEL / MAX_PINCH_VEL +// in gripper_controller_typedefs.hpp +constexpr double MAX_ROLL_VEL = 1.0; // rad/s +constexpr double MAX_PINCH_VEL = 1.0; // rad/s + +} // namespace gripper_interface::types + +#endif // GRIPPER_INTERFACE__GRIPPER_INTERFACE_TYPEDEFS_HPP_ diff --git a/gripper_interface/include/gripper_interface/interface_to_gripper_state.hpp b/gripper_interface/include/gripper_interface/interface_to_gripper_state.hpp deleted file mode 100644 index f8750be..0000000 --- a/gripper_interface/include/gripper_interface/interface_to_gripper_state.hpp +++ /dev/null @@ -1,24 +0,0 @@ -// gripper_controller_translator.hpp -// Mirrors gripper_state_translator.hpp on the input side. -// Translates controller velocity commands back through gear ratio -// to a JointState velocity msg for the interface node. - -#include "sensor_msgs/msg/joint_state.hpp" - -// TODO: Must match the gear ratios in gripper_state_translator.hpp exactly. -// If those are updated, update these too — consider sharing via a common header. -constexpr double CONTROLLER_GEAR_RATIO_ROLL = 1.0; // TODO: measure -constexpr double CONTROLLER_GEAR_RATIO_PINCH = 1.0; // TODO: measure - -constexpr std::size_t CONTROLLER_MIN_VELOCITY_SIZE = 2; - -inline sensor_msgs::msg::JointState translate_to_joint_velocity( - double roll_vel, double pinch_vel) -{ - sensor_msgs::msg::JointState msg; - msg.name = {"roll", "grip"}; - msg.velocity = {roll_vel / CONTROLLER_GEAR_RATIO_ROLL, - pinch_vel / CONTROLLER_GEAR_RATIO_PINCH}; - // position and effort left empty — velocity command only - return msg; -} diff --git a/gripper_interface/interface_to_gripper_state/gripper_interface_can_translator.hpp b/gripper_interface/interface_to_gripper_state/gripper_interface_can_translator.hpp new file mode 100644 index 0000000..796adc6 --- /dev/null +++ b/gripper_interface/interface_to_gripper_state/gripper_interface_can_translator.hpp @@ -0,0 +1,98 @@ +#ifndef GRIPPER_INTERFACE__GRIPPER_INTERFACE_TRANSLATOR_HPP_ +#define GRIPPER_INTERFACE__GRIPPER_INTERFACE_TRANSLATOR_HPP_ + +#include +#include +#include + +#include +#include + +#include "gripper_interface/gripper_interface_typedefs.hpp" + +// --------------------------------------------------------------------------- +// Responsibility: pure, stateless conversions between raw CAN byte payloads +// and ROS/domain types. No I2C, no file descriptors, no ROS dependencies +// beyond message types. +// +// Encoding conventions (matching MCU firmware): +// - Encoder values: big-endian uint16_t, 14-bit (mask with 0x3FFF) +// - PWM values: big-endian uint16_t, microseconds +// --------------------------------------------------------------------------- + +namespace gripper_interface::translator { + +// --------------------------------------------------------------------------- +// Encoder helpers (mirroring MCU's raw_angle_to_radians / i2c_to_encoder_angles) +// --------------------------------------------------------------------------- + +inline uint16_t bytes_to_uint16(uint8_t msb, uint8_t lsb) { + return static_cast((static_cast(msb) << 8) | lsb); +} + +inline double raw_angle_to_radians(uint16_t raw) { + return (static_cast(raw & 0x3FFF) / types::ENCODER_COUNTS) * types::TWO_PI; +} + +// --------------------------------------------------------------------------- +// RawEncoderFrame → GripperState (position, radians) +// +// Byte layout from MCU (NUM_ENCODERS=2 path): +// bytes[0..1] = wrist encoder → roll +// bytes[2..3] = grip encoder → pinch +// bytes[4..5] = unused +// +// WARNING: firmware read_encoders() uses `buf = out + enc_num` instead of +// `out + enc_num * 2`. Verify byte layout on hardware — if enc_num indexing +// is wrong the bytes[2..3] pinch data may be corrupted. +// --------------------------------------------------------------------------- +inline vortex_msgs::msg::GripperState +encoder_frame_to_gripper_state(const types::RawEncoderFrame& frame) { + const uint16_t raw_roll = bytes_to_uint16(frame.bytes[0], frame.bytes[1]); + const uint16_t raw_pinch = bytes_to_uint16(frame.bytes[2], frame.bytes[3]); + + vortex_msgs::msg::GripperState msg; + msg.roll = raw_angle_to_radians(raw_roll) * types::GEAR_RATIO_ROLL; + msg.pinch = raw_angle_to_radians(raw_pinch) * types::GEAR_RATIO_PINCH; + return msg; +} + +// --------------------------------------------------------------------------- +// VelocityCommand → RawPwmFrame +// +// Velocity is normalised by MAX_VEL, then mapped to [PWM_MIN_US, PWM_MAX_US]. +// Idle (zero velocity) = PWM_IDLE_US = 1500 µs. +// +// Formula: duty_us = IDLE + clamp(vel / MAX_VEL, -1, 1) * RANGE +// +// The 4-byte payload sent on CAN SET_PWM: +// bytes[0..1] = roll duty (µs) big-endian uint16_t +// bytes[2..3] = pinch duty (µs) big-endian uint16_t +// --------------------------------------------------------------------------- +inline uint16_t velocity_to_duty_us(double velocity, double max_velocity) { + const double normalised = std::clamp(velocity / max_velocity, -1.0, 1.0); + const double duty = types::PWM_IDLE_US + normalised * types::PWM_RANGE_US; + return static_cast( + std::clamp(duty, + static_cast(types::PWM_MIN_US), + static_cast(types::PWM_MAX_US))); +} + +inline types::RawPwmFrame velocity_command_to_pwm_frame( + const vortex_msgs::msg::GripperStateVelocityCommand& cmd) { + + const uint16_t roll_us = velocity_to_duty_us(cmd.roll_velocity, types::MAX_ROLL_VEL); + const uint16_t pinch_us = velocity_to_duty_us(cmd.pinch_velocity, types::MAX_PINCH_VEL); + + types::RawPwmFrame frame; + // Big-endian packing — MSB first, matching MCU memcpy expectation + frame.bytes[0] = static_cast((roll_us >> 8) & 0xFF); + frame.bytes[1] = static_cast( roll_us & 0xFF); + frame.bytes[2] = static_cast((pinch_us >> 8) & 0xFF); + frame.bytes[3] = static_cast( pinch_us & 0xFF); + return frame; +} + +} // namespace gripper_interface::translator + +#endif // GRIPPER_INTERFACE__GRIPPER_INTERFACE_TRANSLATOR_HPP_ diff --git a/gripper_interface/packageALT.xml b/gripper_interface/packageALT.xml new file mode 100644 index 0000000..52a049f --- /dev/null +++ b/gripper_interface/packageALT.xml @@ -0,0 +1,30 @@ + + + + gripper_interface + 0.0.0 + + Gripper interface node. Bridges the ROS controller pipeline to the gripper + MCU via SocketCAN. Publishes GripperState from encoder CAN frames and + forwards GripperStateVelocityCommand as PWM duty cycles over CAN. + + patricsh + MIT + + ament_cmake + + rclcpp + rclcpp_components + vortex_msgs + vortex_utils_ros + spdlog + fmt + + + + ament_cmake_gtest + + + ament_cmake + + diff --git a/gripper_interface/src/gripper_interface_can_node.cpp b/gripper_interface/src/gripper_interface_can_node.cpp new file mode 100644 index 0000000..65eddfd --- /dev/null +++ b/gripper_interface/src/gripper_interface_can_node.cpp @@ -0,0 +1,10 @@ +#include +#include "gripper_interface/gripper_interface_ros.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared( + rclcpp::NodeOptions())); + rclcpp::shutdown(); + return 0; +} diff --git a/gripper_interface/src/gripper_interface_can_ros.cpp b/gripper_interface/src/gripper_interface_can_ros.cpp new file mode 100644 index 0000000..e89898d --- /dev/null +++ b/gripper_interface/src/gripper_interface_can_ros.cpp @@ -0,0 +1,120 @@ +#include "gripper_interface/gripper_interface_ros.hpp" + +#include + +const auto start_message = R"( + + ____ _ ___ _ __ + / ___|_ __ (_)_ __ _ __ ___|_ _|_ __ | |_ ___ _ __ / _| __ _ ___ ___ +| | _| '__| | '_ \| '_ \ / _ \| || '_ \| __/ _ \ '__| |_ / _` |/ __/ _ \ +| |_| | | | | |_) | |_) | __/| || | | | || __/ | | _| (_| | (_| __/ + \____|_| |_| .__/| .__/ \___|___|_| |_|\__\___|_| |_| \__,_|\___\___| + |_| |_| + +)"; + +namespace vortex::interface { + +// --------------------------------------------------------------------------- +// Construction / Destruction +// --------------------------------------------------------------------------- + +GripperInterfaceNode::GripperInterfaceNode(const rclcpp::NodeOptions& options) +: Node("gripper_interface_node", options) { + declare_and_load_parameters(); + init_can_driver(); // throws on CAN open failure — node will not start + setup_pub_sub(); + spdlog::info(start_message); +} + +GripperInterfaceNode::~GripperInterfaceNode() { + // can_driver_ destructor sends STOP_GRIPPER and closes the socket. + // Nothing extra needed here, but log it clearly. + spdlog::info("GripperInterfaceNode: shutting down, sending STOP_GRIPPER"); +} + +// --------------------------------------------------------------------------- +// Setup +// --------------------------------------------------------------------------- + +void GripperInterfaceNode::declare_and_load_parameters() { + can_interface_ = this->declare_parameter("can_interface", "can0"); + + const int poll_ms = this->declare_parameter("encoder_poll_ms", 20); + poll_period_ = std::chrono::milliseconds(poll_ms); + + spdlog::info("GripperInterfaceNode: can_interface={} encoder_poll_ms={}", + can_interface_, poll_ms); +} + +void GripperInterfaceNode::init_can_driver() { + can_driver_ = std::make_unique(can_interface_); + can_driver_->start_gripper(); +} + +void GripperInterfaceNode::setup_pub_sub() { + const std::string state_topic = + this->declare_parameter("topics.gripper_state"); + const std::string cmd_topic = + this->declare_parameter("topics.velocity_command"); + + auto qos = vortex::utils::qos_profiles::sensor_data_profile(1); + + state_pub_ = this->create_publisher( + state_topic, qos); + + velocity_cmd_sub_ = + this->create_subscription( + cmd_topic, qos, + std::bind(&GripperInterfaceNode::velocity_command_callback, + this, std::placeholders::_1)); + + // Poll timer: runs at encoder_poll_ms to check the non-blocking CAN socket. + // The MCU transmits encoder frames at its own TC0 rate; we just need to + // drain them promptly. Any frame received triggers an immediate publish. + encoder_poll_timer_ = this->create_wall_timer( + poll_period_, + std::bind(&GripperInterfaceNode::encoder_poll_callback, this)); + + spdlog::info("GripperInterfaceNode: state_pub={} cmd_sub={}", + state_topic, cmd_topic); +} + +// --------------------------------------------------------------------------- +// Timer: drain CAN socket, publish on each valid encoder frame +// --------------------------------------------------------------------------- + +void GripperInterfaceNode::encoder_poll_callback() { + // Drain all pending frames in one timer tick — the socket may have buffered + // multiple frames if the poll period is longer than the MCU TX period. + while (true) { + auto maybe_frame = can_driver_->read_encoder_frame(); + if (!maybe_frame.has_value()) { + break; + } + + vortex_msgs::msg::GripperState state_msg = + gripper_interface::translator::encoder_frame_to_gripper_state( + maybe_frame.value()); + + state_msg.header.stamp = this->now(); + state_pub_->publish(state_msg); + } +} + +// --------------------------------------------------------------------------- +// Subscription: velocity command → CAN PWM +// --------------------------------------------------------------------------- + +void GripperInterfaceNode::velocity_command_callback( + const vortex_msgs::msg::GripperStateVelocityCommand::SharedPtr msg) { + + const gripper_interface::types::RawPwmFrame pwm_frame = + gripper_interface::translator::velocity_command_to_pwm_frame(*msg); + + can_driver_->send_pwm(pwm_frame); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(GripperInterfaceNode) + +} // namespace vortex::interface From 71848df34ec97e03d3d55c9ae592babcc0bbd9c1 Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Sun, 5 Apr 2026 16:22:58 +0200 Subject: [PATCH 13/24] feat: implemented skeleton for can hardware interface a.w.a gripper sim interface with functioning goal sends to ref filter playing out in sim environment --- .../src/gripper_reference_filter.cpp | 6 +- .../src/gripper_reference_filter_ros.cpp | 82 ++-- gripper_interface/CMakeLists.txt | 72 ++- gripper_interface/CMakeListsALT.txt | 88 ---- .../config/gripper_interface_params.yaml | 5 + gripper_interface/config/params.yaml | 14 - .../{ => gripper_interface}/can_interface.hpp | 0 .../gripper_interface/gripper_can_ids.hpp | 25 -- .../gripper_interface_can_ros.hpp | 78 ---- .../gripper_interface_driver.hpp | 121 ------ .../gripper_interface_ros.hpp | 38 ++ .../gripper_interface_translator.hpp} | 0 ...efs.hpp => gripper_interface_typedefs.hpp} | 0 .../launch/gripper_interface.launch.py | 18 +- gripper_interface/package.xml | 18 +- gripper_interface/packageALT.xml | 30 -- gripper_interface/src/can_interface.cpp | 2 +- .../src/gripper_interface_can_node.cpp | 10 - .../src/gripper_interface_can_ros.cpp | 120 ----- .../src/gripper_interface_driver.cpp | 80 ---- .../src/gripper_interface_node.cpp | 105 +---- .../src/gripper_interface_ros.cpp | 147 +++++++ gripper_interface/test/CMakeLists.txt | 26 ++ .../test_gripper_interface_translator.cpp | 410 ++++++++++++++++++ .../gripper_sim_interface/__init__.py | 0 .../gripper_sim_bridge.py | 122 ++++++ .../launch/gripper_sim_interface.launch.py | 13 + gripper_sim_interface/package.xml | 22 + .../resource/gripper_sim_interface | 0 gripper_sim_interface/setup.cfg | 4 + gripper_sim_interface/setup.py | 23 + 31 files changed, 937 insertions(+), 742 deletions(-) delete mode 100644 gripper_interface/CMakeListsALT.txt create mode 100644 gripper_interface/config/gripper_interface_params.yaml delete mode 100644 gripper_interface/config/params.yaml rename gripper_interface/include/{ => gripper_interface}/can_interface.hpp (100%) delete mode 100644 gripper_interface/include/gripper_interface/gripper_can_ids.hpp delete mode 100644 gripper_interface/include/gripper_interface/gripper_interface_can_ros.hpp delete mode 100644 gripper_interface/include/gripper_interface/gripper_interface_driver.hpp create mode 100644 gripper_interface/include/gripper_interface/gripper_interface_ros.hpp rename gripper_interface/{interface_to_gripper_state/gripper_interface_can_translator.hpp => include/gripper_interface/gripper_interface_translator.hpp} (100%) rename gripper_interface/include/gripper_interface/{gripper_interface_can_typedefs.hpp => gripper_interface_typedefs.hpp} (100%) delete mode 100644 gripper_interface/packageALT.xml delete mode 100644 gripper_interface/src/gripper_interface_can_node.cpp delete mode 100644 gripper_interface/src/gripper_interface_can_ros.cpp delete mode 100644 gripper_interface/src/gripper_interface_driver.cpp create mode 100644 gripper_interface/src/gripper_interface_ros.cpp create mode 100644 gripper_interface/test/CMakeLists.txt create mode 100644 gripper_interface/test/test_gripper_interface_translator.cpp create mode 100644 gripper_sim_interface/gripper_sim_interface/__init__.py create mode 100644 gripper_sim_interface/gripper_sim_interface/gripper_sim_bridge.py create mode 100644 gripper_sim_interface/launch/gripper_sim_interface.launch.py create mode 100644 gripper_sim_interface/package.xml create mode 100644 gripper_sim_interface/resource/gripper_sim_interface create mode 100644 gripper_sim_interface/setup.cfg create mode 100644 gripper_sim_interface/setup.py diff --git a/gripper_guidance/src/gripper_reference_filter.cpp b/gripper_guidance/src/gripper_reference_filter.cpp index afd6dce..648ddad 100644 --- a/gripper_guidance/src/gripper_reference_filter.cpp +++ b/gripper_guidance/src/gripper_reference_filter.cpp @@ -3,8 +3,10 @@ namespace vortex::guidance { GripperReferenceFilter::GripperReferenceFilter(const GripperReferenceFilterParams& params) { - calculate_Ad(params.omega, params.zeta); - calculate_Bd(params.omega); + Ad_.setZero(); + Bd_.setZero(); + calculate_Ad(params.omega, params.zeta); + calculate_Bd(params.omega); } Eigen::Vector6d GripperReferenceFilter::calculate_x_dot(const Eigen::Vector6d& x, diff --git a/gripper_guidance/src/gripper_reference_filter_ros.cpp b/gripper_guidance/src/gripper_reference_filter_ros.cpp index 2dd8033..8d00e34 100644 --- a/gripper_guidance/src/gripper_reference_filter_ros.cpp +++ b/gripper_guidance/src/gripper_reference_filter_ros.cpp @@ -12,7 +12,7 @@ const auto start_message = R"( / ___|_ __ (_)_ __ _ __ ___ _ __ | _ \ ___ / _| ___ _ __ ___ _ __ ___ ___ | ___(_) | |_ ___ _ __ | | _| '_ \| | '_ \| '_ \ / _ \ '__| | |_) / _ \ |_ / _ \ '__/ _ \ '_ \ / __/ _ \ | |_ | | | __/ _ \ '__| | |_| | |_) | | |_) | |_) | __/ | | _ < __/ _| __/ | | __/ | | | (_| __/ | _| | | | || __/ | - \____| .__/|_| .__/| .__/ \___|_| |_| \_\___|_| \___|_| \___|_| |_|\___\___| |_| |_|_|\__\\___|_| + \____| .__/|_| .__/| .__/ \___|_| |_| \_\___|_| \___|_| \___|_| |_|\___\___| |_| |_|_|\__\\\___|_| |_| |_| |_| )"; @@ -91,9 +91,9 @@ void GripperReferenceFilterNode::set_refererence_filter() { void GripperReferenceFilterNode::reference_callback( const vortex_msgs::msg::GripperState::SharedPtr msg) { - std::lock_guard lock(mutex_); - reference_ << msg->roll, msg->pinch; - } + std::lock_guard lock(mutex_); + reference_ << msg->roll, msg->pinch; +} rclcpp_action::GoalResponse GripperReferenceFilterNode::handle_goal( const rclcpp_action::GoalUUID& uuid, @@ -122,33 +122,22 @@ rclcpp_action::CancelResponse GripperReferenceFilterNode::handle_cancel( return rclcpp_action::CancelResponse::ACCEPT; } - void GripperReferenceFilterNode::handle_accepted( const std::shared_ptr> goal_handle) { - std::thread{std::bind(&GripperReferenceFilterNode::execute, this, - std::placeholders::_1), goal_handle}.detach(); - } - - -//void GripperReferenceFilterNode::handle_accepted( -// const std::shared_ptr> goal_handle) { -// execute(goal_handle); -//} + std::thread{std::bind(&GripperReferenceFilterNode::execute, this, + std::placeholders::_1), goal_handle}.detach(); +} Eigen::Vector6d GripperReferenceFilterNode::fill_reference_state() { - Eigen::Vector6d x = Eigen::Vector6d::Zero(); - x(0) = vortex::utils::math::ssa(reference_(0)); //roll - x(1) = reference_(1); //pinch - - x(2) = 0.0; //roll_dot - x(3) = 0.0; //pinch_dot - - x(4) = 0.0; //roll_dotdot - x(5) = 0.0; //pinch_dotdot + x(0) = vortex::utils::math::ssa(reference_(0)); // roll + x(1) = reference_(1); // pinch + x(2) = 0.0; // roll_dot + x(3) = 0.0; // pinch_dot + x(4) = 0.0; // roll_dotdot + x(5) = 0.0; // pinch_dotdot return x; } @@ -168,13 +157,11 @@ Eigen::Vector2d GripperReferenceFilterNode::fill_reference_goal( vortex_msgs::msg::GripperReferenceFilter GripperReferenceFilterNode::fill_reference_msg() { vortex_msgs::msg::GripperReferenceFilter feedback_msg; - feedback_msg.roll = vortex::utils::math::ssa(x_(0)); - feedback_msg.pinch = x_(1); - - feedback_msg.roll_dot = x_(2); - feedback_msg.pinch_dot = x_(3); - - feedback_msg.roll_dotdot = x_(4); + feedback_msg.roll = vortex::utils::math::ssa(x_(0)); + feedback_msg.pinch = x_(1); + feedback_msg.roll_dot = x_(2); + feedback_msg.pinch_dot = x_(3); + feedback_msg.roll_dotdot = x_(4); feedback_msg.pinch_dotdot = x_(5); return feedback_msg; @@ -205,17 +192,15 @@ void GripperReferenceFilterNode::publish_hold_reference() { if (!reference_pub_) { return; } - const double roll = reference_(0); + const double roll = reference_(0); const double pinch = reference_(1); vortex_msgs::msg::GripperReferenceFilter hold_msg; - hold_msg.roll = vortex::utils::math::ssa(roll); - hold_msg.pinch = pinch; - - hold_msg.roll_dot = 0.0; - hold_msg.pinch_dot = 0.0; - - hold_msg.roll_dotdot = 0.0; + hold_msg.roll = vortex::utils::math::ssa(roll); + hold_msg.pinch = pinch; + hold_msg.roll_dot = 0.0; + hold_msg.pinch_dot = 0.0; + hold_msg.roll_dotdot = 0.0; hold_msg.pinch_dotdot = 0.0; reference_pub_->publish(hold_msg); @@ -232,15 +217,13 @@ void GripperReferenceFilterNode::execute( spdlog::info("Executing goal"); x_ = fill_reference_state(); - - const vortex_msgs::msg::GripperWaypoint goal = + + const vortex_msgs::msg::GripperWaypoint goal = goal_handle->get_goal()->waypoint; uint8_t mode = goal.mode; - - double convergence_threshold = - goal_handle->get_goal()->convergence_threshold; + double convergence_threshold = goal_handle->get_goal()->convergence_threshold; if (convergence_threshold <= 0.0) { convergence_threshold = 0.1; spdlog::warn( @@ -249,9 +232,8 @@ void GripperReferenceFilterNode::execute( } Eigen::Vector2d reference_temp = fill_reference_goal(goal); - - reference_ = apply_mode_logic(reference_temp, mode); - + Eigen::Vector2d goal_reference = apply_mode_logic(reference_temp, mode); // ← fixed: local, not reference_ + auto feedback = std::make_shared< vortex_msgs::action::GripperReferenceFilterWaypoint::Feedback>(); auto result = std::make_shared< @@ -280,7 +262,7 @@ void GripperReferenceFilterNode::execute( } } - Eigen::Vector6d x_dot = gripper_reference_filter_->calculate_x_dot(x_, reference_); + Eigen::Vector6d x_dot = gripper_reference_filter_->calculate_x_dot(x_, goal_reference); // ← fixed x_ += x_dot * time_step_.count() / 1000.0; vortex_msgs::msg::GripperReferenceFilter feedback_msg = fill_reference_msg(); @@ -288,10 +270,10 @@ void GripperReferenceFilterNode::execute( reference_pub_->publish(feedback_msg); goal_handle->publish_feedback(feedback); - if ((x_.head(2) - reference_.head(2)).norm() < convergence_threshold) { + if ((x_.head(2) - goal_reference).norm() < convergence_threshold) { // ← fixed result->success = true; goal_handle->succeed(result); - x_.head(2) = reference_.head(2); + x_.head(2) = goal_reference; // ← fixed vortex_msgs::msg::GripperReferenceFilter final_msg = fill_reference_msg(); reference_pub_->publish(final_msg); spdlog::info("Goal reached"); diff --git a/gripper_interface/CMakeLists.txt b/gripper_interface/CMakeLists.txt index d17cb2a..853212c 100644 --- a/gripper_interface/CMakeLists.txt +++ b/gripper_interface/CMakeLists.txt @@ -9,38 +9,74 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -include_directories(include) - find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(spdlog REQUIRED) -find_package(fmt REQUIRED) -find_package(geometry_msgs REQUIRED) +# Global include — inherited by all targets in this directory AND add_subdirectory(test) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) -add_executable(${PROJECT_NAME}_node - src/gripper_interface_node.cpp - src/gripper_interface_driver.cpp +# --------------------------------------------------------------------------- +# Component library (composable node) +# --------------------------------------------------------------------------- +set(COMPONENT_LIB ${PROJECT_NAME}_component) + +add_library(${COMPONENT_LIB} SHARED + src/can_interface.cpp + src/gripper_interface_ros.cpp ) -target_link_libraries(${PROJECT_NAME}_node fmt::fmt) +target_include_directories(${COMPONENT_LIB} PUBLIC + $ + $ +) -ament_target_dependencies(${PROJECT_NAME}_node +ament_target_dependencies(${COMPONENT_LIB} PUBLIC rclcpp - sensor_msgs + rclcpp_components + vortex_msgs + vortex_utils_ros spdlog - fmt ) -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} +rclcpp_components_register_node( + ${COMPONENT_LIB} + PLUGIN "vortex::interface::GripperInterfaceNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +# --------------------------------------------------------------------------- +# Standalone executable +# --------------------------------------------------------------------------- +add_executable(${PROJECT_NAME}_standalone + src/gripper_interface_node.cpp +) + +target_link_libraries(${PROJECT_NAME}_standalone PUBLIC + ${COMPONENT_LIB} + fmt::fmt ) +ament_target_dependencies(${PROJECT_NAME}_standalone PUBLIC rclcpp) +# --------------------------------------------------------------------------- +# Install +# --------------------------------------------------------------------------- install(TARGETS - ${PROJECT_NAME}_node - DESTINATION lib/${PROJECT_NAME}) + ${COMPONENT_LIB} + ${PROJECT_NAME}_standalone + EXPORT export_${COMPONENT_LIB} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY include/ DESTINATION include) +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}/) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() ament_package() diff --git a/gripper_interface/CMakeListsALT.txt b/gripper_interface/CMakeListsALT.txt deleted file mode 100644 index 79ec76d..0000000 --- a/gripper_interface/CMakeListsALT.txt +++ /dev/null @@ -1,88 +0,0 @@ -#change to CMakeLists.txt if working - -cmake_minimum_required(VERSION 3.8) -project(gripper_interface) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 20) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -include_directories(include) - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(vortex_utils_ros REQUIRED) -find_package(spdlog REQUIRED) -find_package(fmt REQUIRED) - -# --------------------------------------------------------------------------- -# 1. CAN Driver (static — no ROS dependency, unit-testable in isolation) -# --------------------------------------------------------------------------- -add_library(${PROJECT_NAME}_can_driver STATIC - src/gripper_can_driver.cpp -) -target_include_directories(${PROJECT_NAME}_can_driver PUBLIC include) -target_link_libraries(${PROJECT_NAME}_can_driver PUBLIC spdlog::spdlog fmt::fmt) -# SocketCAN is part of the Linux kernel headers — no extra find_package needed - -# --------------------------------------------------------------------------- -# 2. Component library (shared — ROS node as composable component) -# --------------------------------------------------------------------------- -set(COMPONENT_LIB ${PROJECT_NAME}_component) - -add_library(${COMPONENT_LIB} SHARED - src/gripper_interface_ros.cpp -) -target_link_libraries(${COMPONENT_LIB} PUBLIC - ${PROJECT_NAME}_can_driver -) -ament_target_dependencies(${COMPONENT_LIB} PUBLIC - rclcpp - rclcpp_components - vortex_msgs - vortex_utils_ros - spdlog - fmt -) - -rclcpp_components_register_node( - ${COMPONENT_LIB} - PLUGIN "vortex::interface::GripperInterfaceNode" - EXECUTABLE ${PROJECT_NAME}_node -) - -# --------------------------------------------------------------------------- -# 3. Standalone executable -# --------------------------------------------------------------------------- -add_executable(${PROJECT_NAME}_standalone - src/gripper_interface_node.cpp -) -target_link_libraries(${PROJECT_NAME}_standalone PUBLIC ${COMPONENT_LIB}) -ament_target_dependencies(${PROJECT_NAME}_standalone PUBLIC rclcpp) - -# --------------------------------------------------------------------------- -# Install -# --------------------------------------------------------------------------- -install(TARGETS - ${COMPONENT_LIB} - ${PROJECT_NAME}_can_driver - ${PROJECT_NAME}_standalone - EXPORT export_${COMPONENT_LIB} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) -install(DIRECTORY include/ DESTINATION include) -install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}/) - -if(BUILD_TESTING) - add_subdirectory(test) -endif() - -ament_package() diff --git a/gripper_interface/config/gripper_interface_params.yaml b/gripper_interface/config/gripper_interface_params.yaml new file mode 100644 index 0000000..3449da6 --- /dev/null +++ b/gripper_interface/config/gripper_interface_params.yaml @@ -0,0 +1,5 @@ +gripper_interface_node: + ros__parameters: + can_interface: "vcan0" + topics.gripper_state: "vortex/gripper/state" + topics.velocity_command: "vortex/gripper/control" diff --git a/gripper_interface/config/params.yaml b/gripper_interface/config/params.yaml deleted file mode 100644 index a95a1f7..0000000 --- a/gripper_interface/config/params.yaml +++ /dev/null @@ -1,14 +0,0 @@ -/gripper_interface_node: - ros__parameters: - topics: - joy: "/joy" - pwm: "/gripper_pwm" - joint_state: "/joint_state_gripper" - - pwm: - gain: 200 # value range: pwm_idle +/- pwm_gain - idle: 1500 - - i2c: - bus: 7 - address: 0x31 diff --git a/gripper_interface/include/can_interface.hpp b/gripper_interface/include/gripper_interface/can_interface.hpp similarity index 100% rename from gripper_interface/include/can_interface.hpp rename to gripper_interface/include/gripper_interface/can_interface.hpp diff --git a/gripper_interface/include/gripper_interface/gripper_can_ids.hpp b/gripper_interface/include/gripper_interface/gripper_can_ids.hpp deleted file mode 100644 index cd3a556..0000000 --- a/gripper_interface/include/gripper_interface/gripper_can_ids.hpp +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef GRIPPER_INTERFACE__GRIPPER_CAN_IDS_HPP_ -#define GRIPPER_INTERFACE__GRIPPER_CAN_IDS_HPP_ - -#include - -// --------------------------------------------------------------------------- -// CAN IDs — must match can_common.h on the MCU firmware side. -// Update these if the firmware IDs change. -// --------------------------------------------------------------------------- -namespace gripper_can_ids { - -// Host → MCU -constexpr uint32_t SET_PWM = 0x469; // 4 bytes: uint16_t[2] duty_us [roll, pinch] -constexpr uint32_t START_GRIPPER = 0x45a; // 0 bytes: enable servos -constexpr uint32_t STOP_GRIPPER = 0x45b; // 0 bytes: disable servos -constexpr uint32_t RESET_MCU = 0x45c; // 0 bytes: NVIC_SystemReset - -// MCU → Host -constexpr uint32_t SEND_ANGLES = 0x46a; // 6 bytes: uint16_t[3] raw encoder angles - // [wrist, grip, unused] (only first 4 bytes valid - // when NUM_ENCODERS=2) - -} // namespace gripper_can_ids - -#endif // GRIPPER_INTERFACE__GRIPPER_CAN_IDS_HPP_ diff --git a/gripper_interface/include/gripper_interface/gripper_interface_can_ros.hpp b/gripper_interface/include/gripper_interface/gripper_interface_can_ros.hpp deleted file mode 100644 index dcbd0ac..0000000 --- a/gripper_interface/include/gripper_interface/gripper_interface_can_ros.hpp +++ /dev/null @@ -1,78 +0,0 @@ -#ifndef GRIPPER_INTERFACE__GRIPPER_INTERFACE_ROS_HPP_ -#define GRIPPER_INTERFACE__GRIPPER_INTERFACE_ROS_HPP_ - -#include -#include - -#include -#include -#include -#include -#include - -#include "gripper_interface/gripper_can_driver.hpp" -#include "gripper_interface/gripper_interface_translator.hpp" -#include "gripper_interface/gripper_interface_typedefs.hpp" - -// --------------------------------------------------------------------------- -// Responsibility: ROS wiring only. -// - Polls CAN for encoder frames on a timer → publishes GripperState -// - Subscribes to GripperStateVelocityCommand → converts → send_pwm over CAN -// -// The node is the glue between: -// GripperReferenceFilterNode ──(GripperReferenceFilter)──► -// GripperControllerNode ──(GripperStateVelocityCommand)──► -// GripperInterfaceNode ──(CAN SET_PWM)──► MCU -// -// MCU ──(CAN SEND_ANGLES)──► GripperInterfaceNode ──(GripperState)──► -// GripperReferenceFilterNode + GripperControllerNode -// --------------------------------------------------------------------------- - -namespace vortex::interface { - -class GripperInterfaceNode : public rclcpp::Node { -public: - explicit GripperInterfaceNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - ~GripperInterfaceNode() override; - -private: - // Setup - void declare_and_load_parameters(); - void setup_pub_sub(); - void init_can_driver(); - - // Timer: polls CAN socket for encoder frames, publishes GripperState. - // Rate is independent from the controller rate — MCU sends at its own TC0 - // timer rate (~1 Hz based on TC0 period of 46874 @ DIV1024 / 48 MHz). - // The ROS poll timer runs faster to catch frames promptly. - void encoder_poll_callback(); - - // Velocity command from GripperControllerNode → CAN SET_PWM. - void velocity_command_callback( - const vortex_msgs::msg::GripperStateVelocityCommand::SharedPtr msg); - - // --------------------------------------------------------------------------- - // ROS interfaces - // --------------------------------------------------------------------------- - rclcpp::Publisher::SharedPtr state_pub_; - - rclcpp::Subscription::SharedPtr - velocity_cmd_sub_; - - rclcpp::TimerBase::SharedPtr encoder_poll_timer_; - - // --------------------------------------------------------------------------- - // Hardware - // --------------------------------------------------------------------------- - std::unique_ptr can_driver_; - - // --------------------------------------------------------------------------- - // Parameters (loaded once at construction) - // --------------------------------------------------------------------------- - std::string can_interface_; - std::chrono::milliseconds poll_period_; -}; - -} // namespace vortex::interface - -#endif // GRIPPER_INTERFACE__GRIPPER_INTERFACE_ROS_HPP_ diff --git a/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp b/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp deleted file mode 100644 index e81c525..0000000 --- a/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp +++ /dev/null @@ -1,121 +0,0 @@ -#ifndef GRIPPER_INTERFACE_DRIVER_HPP -#define GRIPPER_INTERFACE_DRIVER_HPP - -#include -#include -#include -#include -#include -#include // for std::transform -#include -#include -#include -#include -#include -#include -#include // for std::iota -#include -#include -#include - -/** - * @brief Class for interfacing with the gripper. - */ -class GripperInterfaceDriver { - public: - ~GripperInterfaceDriver(); - - /** - * @brief Constructor for the GripperInterfaceDriver class. - * @param i2c_bus The I2C bus number. - * @param i2c_address The I2C address of the microcontroller. - * @param pwm_gain The gain for converting joystick values to PWM values. - * @param pwm_idle The idle PWM value. - */ - GripperInterfaceDriver(short i2c_bus, - int i2c_address, - int pwm_gain, - int pwm_idle); - - - - /** - * @brief init i2c - * @return 0 on success - */ - int init_i2c(); - - /** - * @brief Convert joystick value to PWM value. - * @param joy_value The joystick value. - * @return The PWM value. - */ - std::uint16_t joy_to_pwm(const double joy_value); - - /** - * @brief Send PWM values to the gripper. - * @param pwm_values The PWM values. - */ - int send_pwm(const std::vector& pwm_values); - - /** - * @brief Start gripper by sending 0x02 first byte - * @param None - */ - int start_gripper(); - - /** - * @brief Stop gripper by sending 0x01 first byte - * @param None - */ - int stop_gripper(); - - /** - * @brief Reads the raw angle of each encoder - * @param None - * @Return vector containing the angles of the shoulder, wrist and grip in - * order - */ - - std::vector read_encoders(); - - private: - int bus_fd_; // File descriptor for I2C bus - int i2c_bus_; // I2C bus number - int i2c_address_; // I2C address of the microcontroller - int pwm_gain_; - int pwm_idle_; - can_interface can_; - - /** - * @brief Convert PWM value to I2C data. - * @param pwm The PWM value. - * @return The I2C data. - */ - static constexpr std::array pwm_to_i2c_data( - std::uint16_t pwm) { - return {static_cast((pwm >> 8) & 0xFF), - static_cast(pwm & 0xFF)}; - } - - /** - * @brief Converts two uint8_t to uint16_t - *@param Array containing to uint8_t - *@return Encoder angles - */ - static constexpr std::uint16_t i2c_to_encoder_angles( - std::array data) { - return (static_cast(data[0]) << 8) | data[1]; - } - /** - *@brief Converts raw encoder angle to radians - *@param Raw encoder angle (uint16_t) - *@return angle in radians (double) - */ - static constexpr double raw_angle_to_radians(std::uint16_t raw_angle) { - return (static_cast(raw_angle) / 0x3FFF) * (2.0 * M_PI); - } - -}; // class GripperInterfaceDriver - -#endif // GRIPPER_INTERFACE_DRIVER_HPP diff --git a/gripper_interface/include/gripper_interface/gripper_interface_ros.hpp b/gripper_interface/include/gripper_interface/gripper_interface_ros.hpp new file mode 100644 index 0000000..439d6f5 --- /dev/null +++ b/gripper_interface/include/gripper_interface/gripper_interface_ros.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include +#include +#include +#include "gripper_interface/can_interface.hpp" +#include "gripper_interface/gripper_interface_translator.hpp" + +#include +#include + +namespace vortex::interface { + +class GripperInterfaceNode : public rclcpp::Node { +public: + explicit GripperInterfaceNode(const rclcpp::NodeOptions& options); + ~GripperInterfaceNode() override; + +private: + void declare_and_load_parameters(); + void init_can(); + void setup_pub_sub(); + + void on_can_frame(const struct canfd_frame& frame, can_status status); + void velocity_command_callback( + const vortex_msgs::msg::GripperStateVelocityCommand::SharedPtr msg); + + can_interface can_; + std::string can_interface_name_; + std::mutex pub_mutex_; + + rclcpp::Publisher::SharedPtr state_pub_; + rclcpp::Subscription::SharedPtr velocity_cmd_sub_; +}; + +} // namespace vortex::interface diff --git a/gripper_interface/interface_to_gripper_state/gripper_interface_can_translator.hpp b/gripper_interface/include/gripper_interface/gripper_interface_translator.hpp similarity index 100% rename from gripper_interface/interface_to_gripper_state/gripper_interface_can_translator.hpp rename to gripper_interface/include/gripper_interface/gripper_interface_translator.hpp diff --git a/gripper_interface/include/gripper_interface/gripper_interface_can_typedefs.hpp b/gripper_interface/include/gripper_interface/gripper_interface_typedefs.hpp similarity index 100% rename from gripper_interface/include/gripper_interface/gripper_interface_can_typedefs.hpp rename to gripper_interface/include/gripper_interface/gripper_interface_typedefs.hpp diff --git a/gripper_interface/launch/gripper_interface.launch.py b/gripper_interface/launch/gripper_interface.launch.py index b005602..efb5273 100644 --- a/gripper_interface/launch/gripper_interface.launch.py +++ b/gripper_interface/launch/gripper_interface.launch.py @@ -1,20 +1,22 @@ -from os import path - +import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node -params = path.join( - get_package_share_directory('gripper_interface'), 'config', 'params.yaml' -) - def generate_launch_description(): + config_file_path = os.path.join( + get_package_share_directory('gripper_interface'), + 'config', + 'gripper_interface_params.yaml', + ) + gripper_interface_node = Node( package='gripper_interface', - executable='gripper_interface_node', + executable='gripper_interface_standalone', name='gripper_interface_node', - parameters=[params], + parameters=[config_file_path], output='screen', ) + return LaunchDescription([gripper_interface_node]) diff --git a/gripper_interface/package.xml b/gripper_interface/package.xml index ef4357d..7d16668 100644 --- a/gripper_interface/package.xml +++ b/gripper_interface/package.xml @@ -2,17 +2,23 @@ gripper_interface - 0.0.0 - Package to take joy inputs and turn them into pwm signals for the gripper servos - andeshog + 0.0.1 + CAN-bus interface node between ROS 2 and the gripper MCU firmware. + Vortex NTNU MIT ament_cmake rclcpp - sensor_msgs - geometry_msgs - + rclcpp_components + vortex_msgs + vortex_utils_ros + spdlog + fmt + + ament_lint_auto + ament_lint_common + ament_cmake diff --git a/gripper_interface/packageALT.xml b/gripper_interface/packageALT.xml deleted file mode 100644 index 52a049f..0000000 --- a/gripper_interface/packageALT.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - gripper_interface - 0.0.0 - - Gripper interface node. Bridges the ROS controller pipeline to the gripper - MCU via SocketCAN. Publishes GripperState from encoder CAN frames and - forwards GripperStateVelocityCommand as PWM duty cycles over CAN. - - patricsh - MIT - - ament_cmake - - rclcpp - rclcpp_components - vortex_msgs - vortex_utils_ros - spdlog - fmt - - - - ament_cmake_gtest - - - ament_cmake - - diff --git a/gripper_interface/src/can_interface.cpp b/gripper_interface/src/can_interface.cpp index 893b5a7..ec6da01 100644 --- a/gripper_interface/src/can_interface.cpp +++ b/gripper_interface/src/can_interface.cpp @@ -1,4 +1,4 @@ -#include "can_interface.hpp" +#include "gripper_interface/can_interface.hpp" #include #include #include diff --git a/gripper_interface/src/gripper_interface_can_node.cpp b/gripper_interface/src/gripper_interface_can_node.cpp deleted file mode 100644 index 65eddfd..0000000 --- a/gripper_interface/src/gripper_interface_can_node.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include -#include "gripper_interface/gripper_interface_ros.hpp" - -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared( - rclcpp::NodeOptions())); - rclcpp::shutdown(); - return 0; -} diff --git a/gripper_interface/src/gripper_interface_can_ros.cpp b/gripper_interface/src/gripper_interface_can_ros.cpp deleted file mode 100644 index e89898d..0000000 --- a/gripper_interface/src/gripper_interface_can_ros.cpp +++ /dev/null @@ -1,120 +0,0 @@ -#include "gripper_interface/gripper_interface_ros.hpp" - -#include - -const auto start_message = R"( - - ____ _ ___ _ __ - / ___|_ __ (_)_ __ _ __ ___|_ _|_ __ | |_ ___ _ __ / _| __ _ ___ ___ -| | _| '__| | '_ \| '_ \ / _ \| || '_ \| __/ _ \ '__| |_ / _` |/ __/ _ \ -| |_| | | | | |_) | |_) | __/| || | | | || __/ | | _| (_| | (_| __/ - \____|_| |_| .__/| .__/ \___|___|_| |_|\__\___|_| |_| \__,_|\___\___| - |_| |_| - -)"; - -namespace vortex::interface { - -// --------------------------------------------------------------------------- -// Construction / Destruction -// --------------------------------------------------------------------------- - -GripperInterfaceNode::GripperInterfaceNode(const rclcpp::NodeOptions& options) -: Node("gripper_interface_node", options) { - declare_and_load_parameters(); - init_can_driver(); // throws on CAN open failure — node will not start - setup_pub_sub(); - spdlog::info(start_message); -} - -GripperInterfaceNode::~GripperInterfaceNode() { - // can_driver_ destructor sends STOP_GRIPPER and closes the socket. - // Nothing extra needed here, but log it clearly. - spdlog::info("GripperInterfaceNode: shutting down, sending STOP_GRIPPER"); -} - -// --------------------------------------------------------------------------- -// Setup -// --------------------------------------------------------------------------- - -void GripperInterfaceNode::declare_and_load_parameters() { - can_interface_ = this->declare_parameter("can_interface", "can0"); - - const int poll_ms = this->declare_parameter("encoder_poll_ms", 20); - poll_period_ = std::chrono::milliseconds(poll_ms); - - spdlog::info("GripperInterfaceNode: can_interface={} encoder_poll_ms={}", - can_interface_, poll_ms); -} - -void GripperInterfaceNode::init_can_driver() { - can_driver_ = std::make_unique(can_interface_); - can_driver_->start_gripper(); -} - -void GripperInterfaceNode::setup_pub_sub() { - const std::string state_topic = - this->declare_parameter("topics.gripper_state"); - const std::string cmd_topic = - this->declare_parameter("topics.velocity_command"); - - auto qos = vortex::utils::qos_profiles::sensor_data_profile(1); - - state_pub_ = this->create_publisher( - state_topic, qos); - - velocity_cmd_sub_ = - this->create_subscription( - cmd_topic, qos, - std::bind(&GripperInterfaceNode::velocity_command_callback, - this, std::placeholders::_1)); - - // Poll timer: runs at encoder_poll_ms to check the non-blocking CAN socket. - // The MCU transmits encoder frames at its own TC0 rate; we just need to - // drain them promptly. Any frame received triggers an immediate publish. - encoder_poll_timer_ = this->create_wall_timer( - poll_period_, - std::bind(&GripperInterfaceNode::encoder_poll_callback, this)); - - spdlog::info("GripperInterfaceNode: state_pub={} cmd_sub={}", - state_topic, cmd_topic); -} - -// --------------------------------------------------------------------------- -// Timer: drain CAN socket, publish on each valid encoder frame -// --------------------------------------------------------------------------- - -void GripperInterfaceNode::encoder_poll_callback() { - // Drain all pending frames in one timer tick — the socket may have buffered - // multiple frames if the poll period is longer than the MCU TX period. - while (true) { - auto maybe_frame = can_driver_->read_encoder_frame(); - if (!maybe_frame.has_value()) { - break; - } - - vortex_msgs::msg::GripperState state_msg = - gripper_interface::translator::encoder_frame_to_gripper_state( - maybe_frame.value()); - - state_msg.header.stamp = this->now(); - state_pub_->publish(state_msg); - } -} - -// --------------------------------------------------------------------------- -// Subscription: velocity command → CAN PWM -// --------------------------------------------------------------------------- - -void GripperInterfaceNode::velocity_command_callback( - const vortex_msgs::msg::GripperStateVelocityCommand::SharedPtr msg) { - - const gripper_interface::types::RawPwmFrame pwm_frame = - gripper_interface::translator::velocity_command_to_pwm_frame(*msg); - - can_driver_->send_pwm(pwm_frame); -} - -RCLCPP_COMPONENTS_REGISTER_NODE(GripperInterfaceNode) - -} // namespace vortex::interface diff --git a/gripper_interface/src/gripper_interface_driver.cpp b/gripper_interface/src/gripper_interface_driver.cpp deleted file mode 100644 index ae4ffba..0000000 --- a/gripper_interface/src/gripper_interface_driver.cpp +++ /dev/null @@ -1,80 +0,0 @@ -#include "gripper_interface/gripper_interface_driver.hpp" -#include -#include "can_interface.hpp" - -GripperInterfaceDriver::GripperInterfaceDriver(short i2c_bus, - int i2c_address, - int pwm_gain, - int pwm_idle) - : pwm_gain_(pwm_gain), pwm_idle_(pwm_idle) {} - -can_status GripperInterfaceDriver::init_can() { - if (can_.init("can0")) { - return can_status::ERR_NOT_INITIALIZED; - } - - return (can_.set_filter(0x46D)); -} - -GripperInterfaceDriver::~GripperInterfaceDriver() { - if (bus_fd_ >= 0) { - send_pwm(std::vector(3, pwm_idle_)); - close(bus_fd_); - } -} - -std::uint16_t GripperInterfaceDriver::joy_to_pwm(const double joy_value) { - return static_cast(pwm_idle_ + pwm_gain_ * joy_value); -} - -can_status GripperInterfaceDriver::send_pwm( - const std::vector& pwm_values) { - static constexpr uint32_t GRIPPER_PWM_CAN_ID = 0x46C; - constexpr std::size_t num_servos = 3; - constexpr std::size_t data_size = - num_servos * 2; // 3 thrusters * (1xMSB + 1xLSB) - std::array buf; - - std::memcpy(buf.data(), pwm_values.data(), data_size); - - return (can_.send(GRIPPER_PWM_CAN_ID, buf, data_size, true) != - can_status::OK); -} - -int GripperInterfaceDriver::stop_gripper() { - static constexpr uint32_t GRIPPER_STOP_CAN_ID = 0x469; - constexpr std::size_t data_size = 1; - std::uint8_t data = 0x00; - - return (can_.send(GRIPPER_STOP_CAN_ID, &data, data_size, true) != - can_status::OK); -} - -int GripperInterfaceDriver::start_gripper() { - static constexpr uint32_t GRIPPER_START_CAN_ID = 0x46A; - constexpr std::size_t data_size = 1; - std::uint8_t data = 0x02; - - return (can_.send(GRIPPER_START_CAN_ID, &data, data_size, true) != - can_status::OK); -} - -std::vector GripperInterfaceDriver::read_encoders() { - constexpr std::size_t num_angles = 2; - std::vector encoder_angles; - encoder_angles.reserve(num_angles); - - canfd_frame encoder_data{}; - if (can_.receive(encoder_data, 1000){ - return {}; - } - - for (std::size_t i = 0; i < num_angles; ++i) { - std::array pair = {i2c_data_array[2 * i], - i2c_data_array[2 * i + 1]}; - std::uint16_t raw_angle = i2c_to_encoder_angles(pair); - encoder_angles.push_back(raw_angle_to_radians(raw_angle)); - } - - return encoder_angles; -} diff --git a/gripper_interface/src/gripper_interface_node.cpp b/gripper_interface/src/gripper_interface_node.cpp index 6bacefe..b8ff36e 100644 --- a/gripper_interface/src/gripper_interface_node.cpp +++ b/gripper_interface/src/gripper_interface_node.cpp @@ -1,99 +1,22 @@ -#include "gripper_interface/gripper_interface_node.hpp" +#include +#include +#include +#include "gripper_interface/gripper_interface_ros.hpp" -GripperInterface::GripperInterface() : Node("gripper_interface_node") { - extract_parameters(); - joy_sub_ = this->create_subscription( - joy_topic_, 10, - std::bind(&GripperInterface::joy_callback, this, - std::placeholders::_1)); - pwm_pub_ = - this->create_publisher(pwm_topic_, 10); - joint_state_pub_ = this->create_publisher( - joint_state_topic_, 10); - gripper_driver_ = std::make_unique( - i2c_bus_, i2c_address_, pwm_gain_, pwm_idle_); - - watchdog_timer_ = this->create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&GripperInterface::encoder_angles_callback, this)); - - last_msg_time_ = this->now(); - spdlog::info("Gripper interface node started"); -} - -void GripperInterface::extract_parameters() { - this->declare_parameter("topics.joy"); - this->declare_parameter("topics.pwm"); - this->declare_parameter("topics.joint_state"); - this->declare_parameter("pwm.gain"); - this->declare_parameter("pwm.idle"); - this->declare_parameter("i2c.bus"); - this->declare_parameter("i2c.address"); - - this->joy_topic_ = this->get_parameter("topics.joy").as_string(); - this->pwm_topic_ = this->get_parameter("topics.pwm").as_string(); - this->joint_state_topic_ = - this->get_parameter("topics.joint_state").as_string(); - this->pwm_gain_ = this->get_parameter("pwm.gain").as_int(); - this->pwm_idle_ = this->get_parameter("pwm.idle").as_int(); - this->i2c_bus_ = this->get_parameter("i2c.bus").as_int(); - this->i2c_address_ = this->get_parameter("i2c.address").as_int(); -} - -void GripperInterface::joy_callback( - const sensor_msgs::msg::Joy::SharedPtr msg) { - std::vector pwm_values; - double shoulder_value = msg->axes[1]; - double wrist_value = msg->axes[0]; - double grip_value = msg->axes[3]; - - std::uint16_t shoulder_pwm = gripper_driver_->joy_to_pwm(shoulder_value); - std::uint16_t wrist_pwm = gripper_driver_->joy_to_pwm(wrist_value); - std::uint16_t grip_pwm = gripper_driver_->joy_to_pwm(grip_value); - - pwm_values.push_back(shoulder_pwm); - pwm_values.push_back(wrist_pwm); - pwm_values.push_back(grip_pwm); - - std_msgs::msg::Int16MultiArray pwm_msg = vec_to_msg(pwm_values); - pwm_pub_->publish(pwm_msg); - - gripper_driver_->send_pwm(pwm_values); - - if (msg->buttons[0]) { - gripper_driver_->start_gripper(); - } else if (msg->buttons[1]) { - gripper_driver_->stop_gripper(); - } -} - -void GripperInterface::encoder_angles_callback() { - std::vector angles = gripper_driver_->read_encoders(); - if (angles.empty()) { - return; - } - - auto joint_state_msg = sensor_msgs::msg::JointState(); +int main(int argc, char* argv[]) { + // Force spdlog to stdout at debug level before anything else runs + spdlog::set_default_logger(spdlog::stdout_color_mt("gripper")); + spdlog::set_level(spdlog::level::debug); + spdlog::info("main: starting gripper_interface_node"); - joint_state_msg.header.stamp = this->now(); - joint_state_msg.name = {"shoulder", "wrist", "grip"}; - joint_state_msg.position = angles; + rclcpp::init(argc, argv); - joint_state_pub_->publish(joint_state_msg); -} + rclcpp::NodeOptions options; + auto node = std::make_shared(options); -std_msgs::msg::Int16MultiArray GripperInterface::vec_to_msg( - std::vector vec) { - std_msgs::msg::Int16MultiArray msg; - for (std::uint16_t value : vec) { - msg.data.push_back(value); - } - return msg; -} + spdlog::info("main: node constructed, spinning"); + rclcpp::spin(node); -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/gripper_interface/src/gripper_interface_ros.cpp b/gripper_interface/src/gripper_interface_ros.cpp new file mode 100644 index 0000000..7428c1b --- /dev/null +++ b/gripper_interface/src/gripper_interface_ros.cpp @@ -0,0 +1,147 @@ +#include "gripper_interface/gripper_interface_ros.hpp" + +#include + +const auto start_message = R"( + + ____ _ ___ _ __ + / ___|_ __ (_)_ __ _ __ ___|_ _|_ __ | |_ ___ _ __ / _| __ _ ___ ___ +| | _| '__| | '_ \| '_ \ / _ \| || '_ \| __/ _ \ '__| |_ / _` |/ __/ _ \ +| |_| | | | | |_) | |_) | __/| || | | | || __/ | | _| (_| | (_| __/ + \____|_| |_| .__/| .__/ \___|___|_| |_|\__\___|_| |_| \__,_|\___\___| + |_| |_| + +)"; + +namespace vortex::interface { + +// --------------------------------------------------------------------------- +// Construction / Destruction +// --------------------------------------------------------------------------- + +GripperInterfaceNode::GripperInterfaceNode(const rclcpp::NodeOptions& options) +: Node("gripper_interface_node", options) { + declare_and_load_parameters(); + init_can(); + setup_pub_sub(); + spdlog::info(start_message); +} + +GripperInterfaceNode::~GripperInterfaceNode() { + spdlog::info("GripperInterfaceNode: shutting down"); + can_.stop_async_receive(); + can_.send(0x45b, nullptr, 0); // STOP_GRIPPER +} + +// --------------------------------------------------------------------------- +// Setup +// --------------------------------------------------------------------------- + +void GripperInterfaceNode::declare_and_load_parameters() { + can_interface_name_ = this->declare_parameter("can_interface", "can0"); + spdlog::info("GripperInterfaceNode: can_interface={}", can_interface_name_); +} + +void GripperInterfaceNode::init_can() { + const can_status status = can_.init(can_interface_name_); + if (status != can_status::OK) { + throw std::runtime_error( + "GripperInterfaceNode: failed to initialise CAN on '" + + can_interface_name_ + "' (status " + + std::to_string(static_cast(status)) + ")"); + } + + + // Filter to SEND_ANGLES (0x46a) only — discard all other bus traffic. + can_.set_filter(0x46a, CAN_SFF_MASK); + + // Start receive thread. on_can_frame is called for every frame that passes + // the filter, plus ERR_RECEIVE on 1-second timeouts (handled gracefully). + can_.start_async_receive( + [this](const struct canfd_frame& frame, can_status s) { + on_can_frame(frame, s); + }); + + can_.send(0x45a, nullptr, 0); // START_GRIPPER + spdlog::info("GripperInterfaceNode: CAN initialised on {}", can_interface_name_); +} + +void GripperInterfaceNode::setup_pub_sub() { + const std::string state_topic = + this->declare_parameter("topics.gripper_state", "/gripper/state"); + const std::string cmd_topic = + this->declare_parameter("topics.velocity_command", "/gripper/velocity_command"); + + auto qos = vortex::utils::qos_profiles::sensor_data_profile(1); + + state_pub_ = this->create_publisher(state_topic, qos); + + velocity_cmd_sub_ = + this->create_subscription( + cmd_topic, qos, + std::bind(&GripperInterfaceNode::velocity_command_callback, + this, std::placeholders::_1)); + + spdlog::info("GripperInterfaceNode: publishing GripperState on '{}'", state_topic); + spdlog::info("GripperInterfaceNode: subscribing velocity commands on '{}'", cmd_topic); +} + +// --------------------------------------------------------------------------- +// CAN receive (called from can_interface's internal thread) +// --------------------------------------------------------------------------- + +void GripperInterfaceNode::on_can_frame(const struct canfd_frame& frame, + can_status status) { + if (status == can_status::ERR_RECEIVE) { + return; // 1-second timeout, not an error + } + + if (status != can_status::OK) { + spdlog::warn("GripperInterfaceNode: CAN receive error (status {})", + static_cast(status)); + return; + } + + if ((frame.can_id & CAN_SFF_MASK) != 0x46a) { // not SEND_ANGLES + return; + } + + if (frame.len < 4) { + spdlog::warn("GripperInterfaceNode: SEND_ANGLES frame too short (len={})", + static_cast(frame.len)); + return; + } + + gripper_interface::types::RawEncoderFrame encoder_frame{}; + std::memcpy(encoder_frame.bytes.data(), frame.data, + std::min(frame.len, encoder_frame.bytes.size())); + + vortex_msgs::msg::GripperState state_msg = + gripper_interface::translator::encoder_frame_to_gripper_state(encoder_frame); + state_msg.header.stamp = this->now(); + + { + std::lock_guard lock(pub_mutex_); + state_pub_->publish(state_msg); + } +} + +// --------------------------------------------------------------------------- +// Velocity command → CAN (called from ROS executor thread) +// --------------------------------------------------------------------------- + +void GripperInterfaceNode::velocity_command_callback( + const vortex_msgs::msg::GripperStateVelocityCommand::SharedPtr msg) { + + const gripper_interface::types::RawPwmFrame pwm_frame = + gripper_interface::translator::velocity_command_to_pwm_frame(*msg); + + can_.send(0x469, // SET_PWM + pwm_frame.bytes.data(), + static_cast(pwm_frame.bytes.size()), + /*use_brs=*/false); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(GripperInterfaceNode) + +} // namespace vortex::interface diff --git a/gripper_interface/test/CMakeLists.txt b/gripper_interface/test/CMakeLists.txt new file mode 100644 index 0000000..71ea0e8 --- /dev/null +++ b/gripper_interface/test/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +find_package(vortex_msgs REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) + +add_executable( + ${TEST_BINARY_NAME} + test_gripper_interface_translator.cpp +) + +# Inherit include dirs from the component lib (PUBLIC target_include_directories +# on COMPONENT_LIB propagates here) + the directory-level include_directories +# set in the parent CMakeLists.txt +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${PROJECT_NAME}_component + GTest::GTest +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC vortex_msgs) + +gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/gripper_interface/test/test_gripper_interface_translator.cpp b/gripper_interface/test/test_gripper_interface_translator.cpp new file mode 100644 index 0000000..09279b8 --- /dev/null +++ b/gripper_interface/test/test_gripper_interface_translator.cpp @@ -0,0 +1,410 @@ +#include +#include +#include + +#include "gripper_interface/gripper_interface_translator.hpp" +#include "gripper_interface/gripper_interface_typedefs.hpp" + +using namespace gripper_interface; + +// --------------------------------------------------------------------------- +// Helper: decode a big-endian uint16_t from two consecutive bytes +// --------------------------------------------------------------------------- +static uint16_t decode_be(uint8_t msb, uint8_t lsb) +{ + return static_cast((static_cast(msb) << 8) | lsb); +} + +// =========================================================================== +// bytes_to_uint16 +// =========================================================================== + +TEST(BytesToUint16, Zero_ProducesZero) +{ + EXPECT_EQ(translator::bytes_to_uint16(0x00, 0x00), 0u); +} + +TEST(BytesToUint16, MsbOnly_ShiftedCorrectly) +{ + EXPECT_EQ(translator::bytes_to_uint16(0x01, 0x00), 256u); +} + +TEST(BytesToUint16, LsbOnly_NotShifted) +{ + EXPECT_EQ(translator::bytes_to_uint16(0x00, 0xFF), 255u); +} + +TEST(BytesToUint16, BothBytes_BigEndianAssembly) +{ + // 0x12 << 8 | 0x34 = 0x1234 = 4660 + EXPECT_EQ(translator::bytes_to_uint16(0x12, 0x34), 0x1234u); +} + +TEST(BytesToUint16, MaxValue_ProducesFFFF) +{ + EXPECT_EQ(translator::bytes_to_uint16(0xFF, 0xFF), 0xFFFFu); +} + +// =========================================================================== +// raw_angle_to_radians +// =========================================================================== + +TEST(RawAngleToRadians, Zero_ProducesZero) +{ + EXPECT_DOUBLE_EQ(translator::raw_angle_to_radians(0), 0.0); +} + +TEST(RawAngleToRadians, HalfScale_ProducesPi) +{ + // 8192 / 16384 * 2π = π + const double expected = (8192.0 / 16384.0) * 2.0 * M_PI; + EXPECT_NEAR(translator::raw_angle_to_radians(8192), expected, 1e-10); +} + +TEST(RawAngleToRadians, FullScale14Bit_JustUnderTwoPi) +{ + // 0x3FFF = 16383 — largest valid 14-bit value + const double expected = (16383.0 / 16384.0) * 2.0 * M_PI; + EXPECT_NEAR(translator::raw_angle_to_radians(0x3FFF), expected, 1e-10); +} + +TEST(RawAngleToRadians, Bit14Set_MaskedToZero) +{ + // 0x4000 — bit 14 set, after 0x3FFF mask → 0 → 0 radians + EXPECT_DOUBLE_EQ(translator::raw_angle_to_radians(0x4000), 0.0); +} + +TEST(RawAngleToRadians, UpperBitsMasked_EquivalentToMaskedValue) +{ + // 0xFFFF masked with 0x3FFF == 0x3FFF + EXPECT_DOUBLE_EQ(translator::raw_angle_to_radians(0xFFFF), + translator::raw_angle_to_radians(0x3FFF)); +} + +TEST(RawAngleToRadians, OutputAlwaysNonNegative) +{ + // Masking ensures raw is never negative after cast + EXPECT_GE(translator::raw_angle_to_radians(0x8000), 0.0); + EXPECT_GE(translator::raw_angle_to_radians(0xFFFF), 0.0); +} + +// =========================================================================== +// encoder_frame_to_gripper_state +// =========================================================================== + +TEST(EncoderFrameToGripperState, ZeroFrame_ProducesZeroState) +{ + types::RawEncoderFrame frame{}; + auto state = translator::encoder_frame_to_gripper_state(frame); + + EXPECT_DOUBLE_EQ(state.roll, 0.0); + EXPECT_DOUBLE_EQ(state.pinch, 0.0); +} + +TEST(EncoderFrameToGripperState, RollBytesOnly_PinchRemainsZero) +{ + types::RawEncoderFrame frame{}; + frame.bytes[0] = 0x20; + frame.bytes[1] = 0x00; + + auto state = translator::encoder_frame_to_gripper_state(frame); + + EXPECT_GT(state.roll, 0.0); + EXPECT_DOUBLE_EQ(state.pinch, 0.0); +} + +TEST(EncoderFrameToGripperState, PinchBytesOnly_RollRemainsZero) +{ + types::RawEncoderFrame frame{}; + frame.bytes[2] = 0x20; + frame.bytes[3] = 0x00; + + auto state = translator::encoder_frame_to_gripper_state(frame); + + EXPECT_DOUBLE_EQ(state.roll, 0.0); + EXPECT_GT(state.pinch, 0.0); +} + +TEST(EncoderFrameToGripperState, IdenticalRawValues_ProducesIdenticalAngles) +{ + // With GEAR_RATIO_ROLL == GEAR_RATIO_PINCH == 1.0, same raw → same output + types::RawEncoderFrame frame{}; + frame.bytes[0] = 0x10; + frame.bytes[1] = 0x00; + frame.bytes[2] = 0x10; + frame.bytes[3] = 0x00; + + auto state = translator::encoder_frame_to_gripper_state(frame); + + EXPECT_DOUBLE_EQ(state.roll, state.pinch); +} + +TEST(EncoderFrameToGripperState, RollMatchesManualCalculation) +{ + types::RawEncoderFrame frame{}; + frame.bytes[0] = 0x10; + frame.bytes[1] = 0x80; + + const uint16_t raw = translator::bytes_to_uint16(0x10, 0x80); + const double expected = translator::raw_angle_to_radians(raw) * types::GEAR_RATIO_ROLL; + + auto state = translator::encoder_frame_to_gripper_state(frame); + + EXPECT_DOUBLE_EQ(state.roll, expected); +} + +TEST(EncoderFrameToGripperState, PinchMatchesManualCalculation) +{ + types::RawEncoderFrame frame{}; + frame.bytes[2] = 0x08; + frame.bytes[3] = 0x40; + + const uint16_t raw = translator::bytes_to_uint16(0x08, 0x40); + const double expected = translator::raw_angle_to_radians(raw) * types::GEAR_RATIO_PINCH; + + auto state = translator::encoder_frame_to_gripper_state(frame); + + EXPECT_DOUBLE_EQ(state.pinch, expected); +} + +TEST(EncoderFrameToGripperState, UnusedBytes_DoNotAffectOutput) +{ + types::RawEncoderFrame frame_a{}; + frame_a.bytes[0] = 0x10; + frame_a.bytes[1] = 0x00; + frame_a.bytes[2] = 0x08; + frame_a.bytes[3] = 0x00; + frame_a.bytes[4] = 0x00; + frame_a.bytes[5] = 0x00; + + types::RawEncoderFrame frame_b = frame_a; + frame_b.bytes[4] = 0xFF; + frame_b.bytes[5] = 0xFF; + + auto state_a = translator::encoder_frame_to_gripper_state(frame_a); + auto state_b = translator::encoder_frame_to_gripper_state(frame_b); + + EXPECT_DOUBLE_EQ(state_a.roll, state_b.roll); + EXPECT_DOUBLE_EQ(state_a.pinch, state_b.pinch); +} + +// =========================================================================== +// velocity_to_duty_us +// =========================================================================== + +TEST(VelocityToDutyUs, ZeroVelocity_ProducesIdlePwm) +{ + EXPECT_EQ(translator::velocity_to_duty_us(0.0, 1.0), + static_cast(types::PWM_IDLE_US)); +} + +TEST(VelocityToDutyUs, MaxPositiveVelocity_ProducesMaxPwm) +{ + EXPECT_EQ(translator::velocity_to_duty_us(1.0, 1.0), + static_cast(types::PWM_MAX_US)); +} + +TEST(VelocityToDutyUs, MaxNegativeVelocity_ProducesMinPwm) +{ + EXPECT_EQ(translator::velocity_to_duty_us(-1.0, 1.0), + static_cast(types::PWM_MIN_US)); +} + +TEST(VelocityToDutyUs, HalfPositiveVelocity_MidpointBetweenIdleAndMax) +{ + const uint16_t expected = + static_cast(types::PWM_IDLE_US + 0.5 * types::PWM_RANGE_US); + + EXPECT_EQ(translator::velocity_to_duty_us(0.5, 1.0), expected); +} + +TEST(VelocityToDutyUs, HalfNegativeVelocity_MidpointBetweenMinAndIdle) +{ + const uint16_t expected = + static_cast(types::PWM_IDLE_US - 0.5 * types::PWM_RANGE_US); + + EXPECT_EQ(translator::velocity_to_duty_us(-0.5, 1.0), expected); +} + +TEST(VelocityToDutyUs, OverspeedPositive_ClampsToMaxPwm) +{ + EXPECT_EQ(translator::velocity_to_duty_us(999.0, 1.0), + static_cast(types::PWM_MAX_US)); +} + +TEST(VelocityToDutyUs, OverspeedNegative_ClampsToMinPwm) +{ + EXPECT_EQ(translator::velocity_to_duty_us(-999.0, 1.0), + static_cast(types::PWM_MIN_US)); +} + +TEST(VelocityToDutyUs, ClampingIsSymmetricAboutIdle) +{ + const uint16_t pos = translator::velocity_to_duty_us( 999.0, 1.0); + const uint16_t neg = translator::velocity_to_duty_us(-999.0, 1.0); + + EXPECT_EQ(pos + neg, static_cast(2 * types::PWM_IDLE_US)); +} + +TEST(VelocityToDutyUs, OutputAlwaysWithinHardwareLimits) +{ + for (double v : {-10.0, -1.0, -0.5, 0.0, 0.5, 1.0, 10.0}) { + const uint16_t duty = translator::velocity_to_duty_us(v, 1.0); + EXPECT_GE(duty, static_cast(types::PWM_MIN_US)); + EXPECT_LE(duty, static_cast(types::PWM_MAX_US)); + } +} + +// =========================================================================== +// velocity_command_to_pwm_frame +// =========================================================================== + +TEST(VelocityCommandToPwmFrame, ZeroCommand_AllBytesEncodeIdlePwm) +{ + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = 0.0; + cmd.pinch_velocity = 0.0; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + + EXPECT_EQ(decode_be(frame.bytes[0], frame.bytes[1]), + static_cast(types::PWM_IDLE_US)); + EXPECT_EQ(decode_be(frame.bytes[2], frame.bytes[3]), + static_cast(types::PWM_IDLE_US)); +} + +TEST(VelocityCommandToPwmFrame, FullPositiveRoll_RollBytesEncodeMaxPwm_PinchIdle) +{ + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = types::MAX_ROLL_VEL; + cmd.pinch_velocity = 0.0; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + + EXPECT_EQ(decode_be(frame.bytes[0], frame.bytes[1]), + static_cast(types::PWM_MAX_US)); + EXPECT_EQ(decode_be(frame.bytes[2], frame.bytes[3]), + static_cast(types::PWM_IDLE_US)); +} + +TEST(VelocityCommandToPwmFrame, FullNegativeRoll_RollBytesEncodeMinPwm_PinchIdle) +{ + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = -types::MAX_ROLL_VEL; + cmd.pinch_velocity = 0.0; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + + EXPECT_EQ(decode_be(frame.bytes[0], frame.bytes[1]), + static_cast(types::PWM_MIN_US)); + EXPECT_EQ(decode_be(frame.bytes[2], frame.bytes[3]), + static_cast(types::PWM_IDLE_US)); +} + +TEST(VelocityCommandToPwmFrame, FullPositivePinch_PinchBytesEncodeMaxPwm_RollIdle) +{ + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = 0.0; + cmd.pinch_velocity = types::MAX_PINCH_VEL; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + + EXPECT_EQ(decode_be(frame.bytes[0], frame.bytes[1]), + static_cast(types::PWM_IDLE_US)); + EXPECT_EQ(decode_be(frame.bytes[2], frame.bytes[3]), + static_cast(types::PWM_MAX_US)); +} + +TEST(VelocityCommandToPwmFrame, FullNegativePinch_PinchBytesEncodeMinPwm_RollIdle) +{ + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = 0.0; + cmd.pinch_velocity = -types::MAX_PINCH_VEL; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + + EXPECT_EQ(decode_be(frame.bytes[0], frame.bytes[1]), + static_cast(types::PWM_IDLE_US)); + EXPECT_EQ(decode_be(frame.bytes[2], frame.bytes[3]), + static_cast(types::PWM_MIN_US)); +} + +TEST(VelocityCommandToPwmFrame, AxesAreIndependent_OppositeExtremes) +{ + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = types::MAX_ROLL_VEL; + cmd.pinch_velocity = -types::MAX_PINCH_VEL; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + + EXPECT_EQ(decode_be(frame.bytes[0], frame.bytes[1]), + static_cast(types::PWM_MAX_US)); + EXPECT_EQ(decode_be(frame.bytes[2], frame.bytes[3]), + static_cast(types::PWM_MIN_US)); +} + +TEST(VelocityCommandToPwmFrame, BigEndianPacking_MsbInLowerByteIndex) +{ + // 0.5 * MAX_ROLL_VEL → duty = 1750 µs = 0x06D6 + // bytes[0] must be 0x06, bytes[1] must be 0xD6 + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = 0.5 * types::MAX_ROLL_VEL; + cmd.pinch_velocity = 0.0; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + const uint16_t expected = + static_cast(types::PWM_IDLE_US + 0.5 * types::PWM_RANGE_US); + + EXPECT_EQ(frame.bytes[0], static_cast((expected >> 8) & 0xFF)); + EXPECT_EQ(frame.bytes[1], static_cast( expected & 0xFF)); +} + +TEST(VelocityCommandToPwmFrame, PwmSymmetry_PositiveAndNegativeSymmetricAboutIdle) +{ + vortex_msgs::msg::GripperStateVelocityCommand pos_cmd; + pos_cmd.roll_velocity = 0.7 * types::MAX_ROLL_VEL; + pos_cmd.pinch_velocity = 0.0; + + vortex_msgs::msg::GripperStateVelocityCommand neg_cmd; + neg_cmd.roll_velocity = -0.7 * types::MAX_ROLL_VEL; + neg_cmd.pinch_velocity = 0.0; + + auto pos_frame = translator::velocity_command_to_pwm_frame(pos_cmd); + auto neg_frame = translator::velocity_command_to_pwm_frame(neg_cmd); + + const uint16_t pos_us = decode_be(pos_frame.bytes[0], pos_frame.bytes[1]); + const uint16_t neg_us = decode_be(neg_frame.bytes[0], neg_frame.bytes[1]); + + EXPECT_EQ(pos_us + neg_us, static_cast(2 * types::PWM_IDLE_US)); +} + +TEST(VelocityCommandToPwmFrame, AllOutputBytesWithinHardwarePwmRange) +{ + for (double rv : {-2.0, -1.0, 0.0, 1.0, 2.0}) { + for (double pv : {-2.0, -1.0, 0.0, 1.0, 2.0}) { + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = rv; + cmd.pinch_velocity = pv; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + + const uint16_t roll_us = decode_be(frame.bytes[0], frame.bytes[1]); + const uint16_t pinch_us = decode_be(frame.bytes[2], frame.bytes[3]); + + EXPECT_GE(roll_us, static_cast(types::PWM_MIN_US)); + EXPECT_LE(roll_us, static_cast(types::PWM_MAX_US)); + EXPECT_GE(pinch_us, static_cast(types::PWM_MIN_US)); + EXPECT_LE(pinch_us, static_cast(types::PWM_MAX_US)); + } + } +} + +// --------------------------------------------------------------------------- +// Entry point +// --------------------------------------------------------------------------- + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gripper_sim_interface/gripper_sim_interface/__init__.py b/gripper_sim_interface/gripper_sim_interface/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gripper_sim_interface/gripper_sim_interface/gripper_sim_bridge.py b/gripper_sim_interface/gripper_sim_interface/gripper_sim_bridge.py new file mode 100644 index 0000000..fc13265 --- /dev/null +++ b/gripper_sim_interface/gripper_sim_interface/gripper_sim_bridge.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python3 +""" +Gripper Sim Bridge +Rewires the vortex gripper pipeline to use Stonefish/Nautilus simulator topics. + +Joystick reference (joystick_interface_auv, nautilus branch): + /nautilus/gripper_servos <- JointState + name = [arm_joint, finger_joint1, finger_joint2] + velocity = [roll, grip, grip ] + frame_id = "base_link" + +This bridge mirrors that format exactly, driven by the gripper pipeline: + /vortex/gripper/control (GripperStateVelocityCommand) + roll_dot -> arm_joint velocity (≡ rotate) + pinch_dot -> finger_joint1/2 velocity (≡ grip) + +State feedback — raw radians, no normalisation: + /nautilus/servo_state (JointState) -> /vortex/gripper/state (GripperState) + Finger avg: -0.33 rad = fully open, ~0.0 rad = fully closed. + +NOTE: Keep the AUV in AUTONOMOUS mode during gripper pipeline tests to avoid +a topic conflict — the joystick also publishes to /nautilus/gripper_servos in +MANUAL and REFERENCE modes. +""" + +import rclpy +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from sensor_msgs.msg import JointState +from vortex_msgs.msg import GripperState, GripperStateVelocityCommand + +BEST_EFFORT_QOS = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=1, + durability=DurabilityPolicy.VOLATILE, +) + +RELIABLE_QOS = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=10, + durability=DurabilityPolicy.VOLATILE, +) + + +class GripperSimBridge(Node): + def __init__(self): + super().__init__('gripper_sim_bridge') + + self.create_subscription( + JointState, + '/nautilus/servo_state', + self.on_servo_state, + BEST_EFFORT_QOS, + ) + self.create_subscription( + GripperStateVelocityCommand, + '/vortex/gripper/control', + self.on_velocity_command, + BEST_EFFORT_QOS, + ) + + self.state_pub = self.create_publisher( + GripperState, '/vortex/gripper/state', RELIABLE_QOS + ) + self.cmd_pub = self.create_publisher( + JointState, '/nautilus/gripper_servos', BEST_EFFORT_QOS + ) + + self.get_logger().info('GripperSimBridge: online') + self.get_logger().info(' IN /nautilus/servo_state -> /vortex/gripper/state') + self.get_logger().info(' OUT /vortex/gripper/control -> /nautilus/gripper_servos') + self.get_logger().warn( + 'Keep AUV in AUTONOMOUS mode to avoid joystick conflict on /nautilus/gripper_servos' + ) + + def on_servo_state(self, msg: JointState): + try: + idx_arm = msg.name.index('nautilus/arm_joint') + idx_f1 = msg.name.index('nautilus/finger_joint1') + idx_f2 = msg.name.index('nautilus/finger_joint2') + except ValueError as e: + self.get_logger().warn(f'Joint name missing: {e}', throttle_duration_sec=5.0) + return + + state = GripperState() + state.header.stamp = self.get_clock().now().to_msg() + state.roll = msg.position[idx_arm] + state.pinch = (msg.position[idx_f1] + msg.position[idx_f2]) / 2.0 + + self.state_pub.publish(state) + + def on_velocity_command(self, msg: GripperStateVelocityCommand): + cmd = JointState() + cmd.header.stamp = self.get_clock().now().to_msg() + cmd.header.frame_id = 'base_link' + cmd.name = [ + 'nautilus/arm_joint', + 'nautilus/finger_joint1', + 'nautilus/finger_joint2', + ] + + cmd.position = [] + cmd.velocity = [msg.roll_velocity, msg.pinch_velocity, msg.pinch_velocity] + cmd.effort = [] + self.cmd_pub.publish(cmd) + +def main(): + rclpy.init() + node = GripperSimBridge() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/gripper_sim_interface/launch/gripper_sim_interface.launch.py b/gripper_sim_interface/launch/gripper_sim_interface.launch.py new file mode 100644 index 0000000..4ecf5f7 --- /dev/null +++ b/gripper_sim_interface/launch/gripper_sim_interface.launch.py @@ -0,0 +1,13 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='gripper_sim_interface', + executable='gripper_sim_bridge', + name='gripper_sim_bridge', + output='screen', + ) + ]) diff --git a/gripper_sim_interface/package.xml b/gripper_sim_interface/package.xml new file mode 100644 index 0000000..56b6f4a --- /dev/null +++ b/gripper_sim_interface/package.xml @@ -0,0 +1,22 @@ + + + + gripper_sim_interface + 0.1.0 + Sim bridge between the vortex gripper pipeline and Nautilus/Stonefish topics + patricsh + MIT + + rclpy + sensor_msgs + vortex_msgs + + ament_python + ament_copyright + ament_flake8 + ament_pep257 + + + ament_python + + diff --git a/gripper_sim_interface/resource/gripper_sim_interface b/gripper_sim_interface/resource/gripper_sim_interface new file mode 100644 index 0000000..e69de29 diff --git a/gripper_sim_interface/setup.cfg b/gripper_sim_interface/setup.cfg new file mode 100644 index 0000000..7d6b7df --- /dev/null +++ b/gripper_sim_interface/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/gripper_sim_interface +[install] +install_scripts=$base/lib/gripper_sim_interface diff --git a/gripper_sim_interface/setup.py b/gripper_sim_interface/setup.py new file mode 100644 index 0000000..bf3a0f3 --- /dev/null +++ b/gripper_sim_interface/setup.py @@ -0,0 +1,23 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'gripper_sim_interface' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + entry_points={ + 'console_scripts': [ + 'gripper_sim_bridge = gripper_sim_interface.gripper_sim_bridge:main', + ], + }, +) From 11842fed02c915080039e275e2a42ad76a1136f4 Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Thu, 9 Apr 2026 15:58:58 +0200 Subject: [PATCH 14/24] idk --- .../config/gripper_reference_filter_params.yaml | 2 +- gripper_guidance/src/gripper_reference_filter_ros.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gripper_guidance/config/gripper_reference_filter_params.yaml b/gripper_guidance/config/gripper_reference_filter_params.yaml index d50adda..840a49f 100644 --- a/gripper_guidance/config/gripper_reference_filter_params.yaml +++ b/gripper_guidance/config/gripper_reference_filter_params.yaml @@ -2,7 +2,7 @@ ros__parameters: # Filter dynamics parameters zeta: [1.0, 1.0] - omega: [0.2, 0.2] + omega: [0.4, 0.4] # Topics topics: diff --git a/gripper_guidance/src/gripper_reference_filter_ros.cpp b/gripper_guidance/src/gripper_reference_filter_ros.cpp index 8d00e34..f884558 100644 --- a/gripper_guidance/src/gripper_reference_filter_ros.cpp +++ b/gripper_guidance/src/gripper_reference_filter_ros.cpp @@ -132,7 +132,7 @@ void GripperReferenceFilterNode::handle_accepted( Eigen::Vector6d GripperReferenceFilterNode::fill_reference_state() { Eigen::Vector6d x = Eigen::Vector6d::Zero(); - x(0) = vortex::utils::math::ssa(reference_(0)); // roll + x(0) = reference_(0); // roll x(1) = reference_(1); // pinch x(2) = 0.0; // roll_dot x(3) = 0.0; // pinch_dot @@ -157,7 +157,7 @@ Eigen::Vector2d GripperReferenceFilterNode::fill_reference_goal( vortex_msgs::msg::GripperReferenceFilter GripperReferenceFilterNode::fill_reference_msg() { vortex_msgs::msg::GripperReferenceFilter feedback_msg; - feedback_msg.roll = vortex::utils::math::ssa(x_(0)); + feedback_msg.roll = x_(0); feedback_msg.pinch = x_(1); feedback_msg.roll_dot = x_(2); feedback_msg.pinch_dot = x_(3); @@ -181,7 +181,7 @@ Eigen::Vector2d GripperReferenceFilterNode::apply_mode_logic( break; case vortex_msgs::msg::GripperWaypoint::ONLY_PINCH: - reference_out(0) = vortex::utils::math::ssa(reference_(0)); + reference_out(0) = reference_(0); break; } @@ -196,7 +196,7 @@ void GripperReferenceFilterNode::publish_hold_reference() { const double pinch = reference_(1); vortex_msgs::msg::GripperReferenceFilter hold_msg; - hold_msg.roll = vortex::utils::math::ssa(roll); + hold_msg.roll = roll; hold_msg.pinch = pinch; hold_msg.roll_dot = 0.0; hold_msg.pinch_dot = 0.0; From 3b5ae25fdefc537b758d817a04c78cb182451909 Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Mon, 18 May 2026 15:05:32 +0200 Subject: [PATCH 15/24] Modified gripper controller to actually interface with gripper state awa removing steady state error --- .../config/gripper_controller_params.yaml | 6 +- .../gripper_reference_filter_params.yaml | 6 +- .../gripper_reference_filter_ros.hpp | 7 ++ .../src/gripper_reference_filter_ros.cpp | 68 ++++++++++++++++--- .../config/gripper_interface_params.yaml | 4 +- 5 files changed, 73 insertions(+), 18 deletions(-) diff --git a/gripper_controller/config/gripper_controller_params.yaml b/gripper_controller/config/gripper_controller_params.yaml index 70b0e1e..2efa89a 100644 --- a/gripper_controller/config/gripper_controller_params.yaml +++ b/gripper_controller/config/gripper_controller_params.yaml @@ -12,8 +12,8 @@ # Topics topics: # Input: smoothed reference from the gripper reference filter - reference: "vortex/gripper/guidance" + reference: "nautilus/gripper/guidance" # Input: raw measured gripper state from the interface node - state: "vortex/gripper/state" + state: "nautilus/gripper/state" # Output: velocity command to the interface node - control: "vortex/gripper/control" + control: "nautilus/gripper/control" diff --git a/gripper_guidance/config/gripper_reference_filter_params.yaml b/gripper_guidance/config/gripper_reference_filter_params.yaml index 840a49f..03f5643 100644 --- a/gripper_guidance/config/gripper_reference_filter_params.yaml +++ b/gripper_guidance/config/gripper_reference_filter_params.yaml @@ -6,9 +6,9 @@ # Topics topics: - guidance_gripper: "vortex/gripper/guidance" - gripper_state: "vortex/gripper/state" + guidance_gripper: "nautilus/gripper/guidance" + gripper_state: "nautilus/gripper/state" # Action servers action_servers: - gripper_reference_filter: "vortex/gripper/reference_filter" + gripper_reference_filter: "nautilus/gripper/reference_filter" diff --git a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp index 75e4889..e1aeced 100644 --- a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp +++ b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp @@ -82,6 +82,9 @@ class GripperReferenceFilterNode : public rclcpp::Node { // @brief Publish a hold reference message at the current gripper state void publish_hold_reference(); + // @brief Publish hold reference on a timer when no goal is active + void publish_hold_timer(); + // @brief Fill the reference filter output message from the current state // @return The gripper reference filter message vortex_msgs::msg::GripperReferenceFilter fill_reference_msg(); @@ -120,6 +123,10 @@ class GripperReferenceFilterNode : public rclcpp::Node { goal_handle_; rclcpp::CallbackGroup::SharedPtr cb_group_; + + rclcpp::TimerBase::SharedPtr hold_timer_; + vortex_msgs::msg::GripperReferenceFilter hold_reference_msg_; + bool hold_active_{false}; }; } // namespace vortex::guidance diff --git a/gripper_guidance/src/gripper_reference_filter_ros.cpp b/gripper_guidance/src/gripper_reference_filter_ros.cpp index f884558..8b50032 100644 --- a/gripper_guidance/src/gripper_reference_filter_ros.cpp +++ b/gripper_guidance/src/gripper_reference_filter_ros.cpp @@ -29,6 +29,10 @@ GripperReferenceFilterNode::GripperReferenceFilterNode(const rclcpp::NodeOptions set_refererence_filter(); + hold_timer_ = this->create_wall_timer( + time_step_, + std::bind(&GripperReferenceFilterNode::publish_hold_timer, this)); + spdlog::info(start_message); } @@ -103,6 +107,7 @@ rclcpp_action::GoalResponse GripperReferenceFilterNode::handle_goal( (void)goal; { std::lock_guard lock(mutex_); + hold_active_ = false; if (goal_handle_) { if (goal_handle_->is_active()) { spdlog::info("Aborting current goal and accepting new goal"); @@ -192,16 +197,31 @@ void GripperReferenceFilterNode::publish_hold_reference() { if (!reference_pub_) { return; } - const double roll = reference_(0); - const double pinch = reference_(1); + vortex_msgs::msg::GripperReferenceFilter hold_msg; + { + std::lock_guard lock(mutex_); + hold_msg.roll = reference_(0); + hold_msg.pinch = reference_(1); + hold_msg.roll_dot = 0.0; + hold_msg.pinch_dot = 0.0; + hold_msg.roll_dotdot = 0.0; + hold_msg.pinch_dotdot = 0.0; + hold_reference_msg_ = hold_msg; + hold_active_ = true; + } + + reference_pub_->publish(hold_msg); +} +void GripperReferenceFilterNode::publish_hold_timer() { vortex_msgs::msg::GripperReferenceFilter hold_msg; - hold_msg.roll = roll; - hold_msg.pinch = pinch; - hold_msg.roll_dot = 0.0; - hold_msg.pinch_dot = 0.0; - hold_msg.roll_dotdot = 0.0; - hold_msg.pinch_dotdot = 0.0; + { + std::lock_guard lock(mutex_); + if (!hold_active_ || !reference_pub_) { + return; + } + hold_msg = hold_reference_msg_; + } reference_pub_->publish(hold_msg); } @@ -266,16 +286,44 @@ void GripperReferenceFilterNode::execute( x_ += x_dot * time_step_.count() / 1000.0; vortex_msgs::msg::GripperReferenceFilter feedback_msg = fill_reference_msg(); + { + std::lock_guard lock(mutex_); + hold_reference_msg_ = feedback_msg; + } feedback->reference = feedback_msg; reference_pub_->publish(feedback_msg); goal_handle->publish_feedback(feedback); - if ((x_.head(2) - goal_reference).norm() < convergence_threshold) { // ← fixed + Eigen::Vector2d current_state; + { + std::lock_guard lock(mutex_); + current_state = reference_; + } + + Eigen::Vector2d error = current_state - goal_reference; + switch (mode) { + case vortex_msgs::msg::GripperWaypoint::ONLY_ROLL: + error(1) = 0.0; + break; + case vortex_msgs::msg::GripperWaypoint::ONLY_PINCH: + error(0) = 0.0; + break; + case vortex_msgs::msg::GripperWaypoint::ROLL_AND_PINCH: + default: + break; + } + + if (error.norm() < convergence_threshold) { result->success = true; goal_handle->succeed(result); - x_.head(2) = goal_reference; // ← fixed + x_.head(2) = goal_reference; vortex_msgs::msg::GripperReferenceFilter final_msg = fill_reference_msg(); reference_pub_->publish(final_msg); + { + std::lock_guard lock(mutex_); + hold_reference_msg_ = final_msg; + hold_active_ = true; + } spdlog::info("Goal reached"); return; } diff --git a/gripper_interface/config/gripper_interface_params.yaml b/gripper_interface/config/gripper_interface_params.yaml index 3449da6..e16cfa9 100644 --- a/gripper_interface/config/gripper_interface_params.yaml +++ b/gripper_interface/config/gripper_interface_params.yaml @@ -1,5 +1,5 @@ gripper_interface_node: ros__parameters: can_interface: "vcan0" - topics.gripper_state: "vortex/gripper/state" - topics.velocity_command: "vortex/gripper/control" + topics.gripper_state: "nautilus/gripper/state" + topics.velocity_command: "nautilus/gripper/control" From cd8e56cad4f5bd4c283daa4b13db730b9833a0c7 Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Mon, 18 May 2026 16:35:21 +0200 Subject: [PATCH 16/24] guidance(gripper): refactor reference filter ROS node per PR review Addresses the following PR review comments on gripper_guidance/src/gripper_reference_filter_ros.cpp: L53 (jorgenfj): "Consider using lambdas instead of std::bind to avoid using std::placeholders" All std::bind+std::placeholders sites converted to lambdas: subscription callback, action server callbacks (handle_goal, handle_cancel, handle_accepted), and the detached execute thread spawn site (now spawns via std::thread{[this, goal_handle]{...}}). L79 (jorgenfj): "What is this line spacing? I need a vertical monitor to read this..." rclcpp_action::create_server(...) call now matches the pattern from vortex-auv/guidance/reference_filter_dp_quat: terse lambdas [this](const auto& uuid, auto goal) {...} instead of std::bind with placeholders separated by blank lines. The whole call now fits in a single readable block. L107 (jorgenfj): "For unused function args a more explicit approach is to comment out the args themselves instead of using (void)." handle_goal: (void)uuid; (void)goal; removed; signature now uses /*uuid*/ and /*goal*/. handle_cancel: (void)goal_handle; removed; signature uses /*goal_handle*/. L135 (jorgenfj): "This node currently uses both a Reentrant callback group for the action server and detaches a thread for the execute function, which IMO is overkill. We also had some race condition issues with this setup in the previous implementation of the reference filter, which is now fixed so you can take inspiration from that." Adopted the pattern from vortex-auv/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp: * Removed rclcpp::CallbackGroup cb_group_ and the CallbackGroupType::Reentrant group. * Removed UUID-based preemption (preempted_goal_id_, goal_handle_ member, UUID comparisons inside the loop). * Added std::atomic preempted_, std::mutex execute_mutex_, and std::thread execute_thread_ as members. * Destructor sets preempted_ and joins the thread. * handle_accepted now: locks execute_mutex_, signals preempt, joins the prior thread, clears preempt, then spawns the new execute thread. This eliminates the race the previous implementation had. * The execute loop now polls preempted_.load() instead of comparing goal UUIDs. Hold-timer behaviour (hold_active_, hold_reference_msg_, hold_timer_) is preserved unchanged. L173 (jorgenfj): "Maybe move these conversion functions to a separate util file to avoid clutter in the ros node" fill_reference_state, fill_reference_goal, fill_reference_msg, and apply_mode_logic moved from member functions on the node to free functions in a new header gripper_reference_filter/gripper_reference_filter_ros_utils.hpp. Mirrors the layout in vortex-auv/.../reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp. Corresponding member declarations removed from the node header. L294 (jorgenfj): "Should use unique pointer for this kind of pipeline style publishing. Ref: ...Composable_nodes" Every reference_pub_->publish(...) site converted to std::make_unique + publish(std::move(...)): * publish_hold_reference * publish_hold_timer * the periodic feedback publish inside execute * the final-convergence publish inside execute Enables the zero-copy intra-process path documented in the Composable Nodes wiki entry. L258 (jorgenfj) companion to the action-definition trim in vortex-msgs: goal_handle->publish_feedback(feedback) removed from execute; the local feedback shared_ptr removed. The topic publish remains the single source of reference data. L24 (jorgenfj): "should rename the variable to time_step_ms_ to make it more explicit that it is milliseconds and not seconds. Also would be nice to expose this as a ros param" NOT addressed in this commit. time_step_ is typed std::chrono::milliseconds (unit is in the type, not the name) and is set inside the constructor. Exposing it as a ROS parameter would be a separate behavioural change; flagged here for follow-up rather than silently included with the refactor. Additional changes required by the .msg/.action trim landed in vortex-msgs (GripperReferenceFilter, GripperWaypoint, action feedback): * fill_reference_msg now writes only roll and pinch; the dot/dotdot assignments are gone. * publish_hold_reference no longer zeroes derivative fields. * fill_reference_goal reads waypoint_goal.roll / .pinch directly (no longer .roll.roll / .pinch.pinch). * The 6D filter_state_ still drives the dynamics internally; only the wire format shrank. Member renames for readability (Code Complete ch. 11): * x_ to filter_state_ (the rclcpp publish-side rename in this file intentionally does not touch GripperReferenceFilter::calculate_x_dot, which keeps its mathematical name). --- .../gripper_reference_filter_ros.hpp | 74 +---- .../gripper_reference_filter_ros_utils.hpp | 53 ++++ .../src/gripper_reference_filter_ros.cpp | 260 +++++++----------- 3 files changed, 162 insertions(+), 225 deletions(-) create mode 100644 gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp diff --git a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp index e1aeced..88ba7a1 100644 --- a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp +++ b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp @@ -1,8 +1,10 @@ #ifndef GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_HPP_ #define GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_HPP_ +#include #include #include +#include #include #include #include @@ -19,114 +21,68 @@ class GripperReferenceFilterNode : public rclcpp::Node { explicit GripperReferenceFilterNode( const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + ~GripperReferenceFilterNode(); + private: - // @brief Set the subscribers and publishers void set_subscribers_and_publisher(); - // @brief Set the action server void set_action_server(); - // @brief Initializes the reference filter with ROS parameters void set_refererence_filter(); - // @brief Callback for incoming gripper state messages - // @param msg The gripper state message void reference_callback( - const vortex_msgs::msg::GripperState::SharedPtr msg); + const vortex_msgs::msg::GripperState::SharedPtr state_msg); - // @brief Handle the goal request - // @param uuid The goal UUID - // @param goal The goal message - // @return The goal response rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID& uuid, + const rclcpp_action::GoalUUID& /*uuid*/, std::shared_ptr< - const vortex_msgs::action::GripperReferenceFilterWaypoint::Goal> goal); + const vortex_msgs::action::GripperReferenceFilterWaypoint::Goal> + /*goal*/); - // @brief Handle the cancel request - // @param goal_handle The goal handle - // @return The cancel response rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr> goal_handle); + vortex_msgs::action::GripperReferenceFilterWaypoint>> + /*goal_handle*/); - // @brief Handle the accepted request - // @param goal_handle The goal handle void handle_accepted( const std::shared_ptr> goal_handle); - // @brief Execute the goal - // @param goal_handle The goal handle void execute( const std::shared_ptr> goal_handle); - // @brief Fill the initial state vector from current gripper state - // @return 6D state vector [roll, pinch, roll_dot, pinch_dot, roll_dotdot, pinch_dotdot] - Eigen::Vector6d fill_reference_state(); - - // @brief Fill the reference goal vector from a GripperWaypoint - // @param goal The gripper waypoint message - // @return 2D reference vector [roll, pinch] - Eigen::Vector2d fill_reference_goal( - const vortex_msgs::msg::GripperWaypoint& goal); - - // @brief Apply mode logic to the reference vector - // @param reference_in The input reference vector - // @param mode The mode (ROLL_AND_PINCH, ONLY_ROLL, ONLY_PINCH) - // @return The modified reference vector - Eigen::Vector2d apply_mode_logic( - const Eigen::Vector2d& reference_in, uint8_t mode); - - // @brief Publish a hold reference message at the current gripper state void publish_hold_reference(); - // @brief Publish hold reference on a timer when no goal is active void publish_hold_timer(); - // @brief Fill the reference filter output message from the current state - // @return The gripper reference filter message - vortex_msgs::msg::GripperReferenceFilter fill_reference_msg(); - - // Action server rclcpp_action::Server< vortex_msgs::action::GripperReferenceFilterWaypoint>::SharedPtr action_server_; - // Reference filter instance std::unique_ptr gripper_reference_filter_{}; - // Publisher: smoothed reference to controller rclcpp::Publisher::SharedPtr reference_pub_; - // Subscriber: raw gripper state from hardware rclcpp::Subscription::SharedPtr reference_sub_; std::chrono::milliseconds time_step_{}; - // x_ is [roll, pinch, roll_dot, pinch_dot, roll_dotdot, pinch_dotdot] - // 6D state vector (2 DOF x position/velocity/acceleration) - Eigen::Vector6d x_; + Eigen::Vector6d filter_state_; - // reference_ holds the live gripper state [roll, pinch] from the subscriber Eigen::Vector2d reference_; std::mutex mutex_; - rclcpp_action::GoalUUID preempted_goal_id_; - - std::shared_ptr> - goal_handle_; - - rclcpp::CallbackGroup::SharedPtr cb_group_; - rclcpp::TimerBase::SharedPtr hold_timer_; vortex_msgs::msg::GripperReferenceFilter hold_reference_msg_; bool hold_active_{false}; + + std::atomic preempted_{false}; + std::mutex execute_mutex_; + std::thread execute_thread_; }; } // namespace vortex::guidance diff --git a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp new file mode 100644 index 0000000..96466a1 --- /dev/null +++ b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp @@ -0,0 +1,53 @@ +#ifndef GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_UTILS_HPP_ +#define GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_UTILS_HPP_ + +#include +#include +#include "gripper_reference_filter/gripper_eigen_typedefs.hpp" + +namespace vortex::guidance { + +inline Eigen::Vector6d fill_reference_state( + const Eigen::Vector2d& current_reference) { + Eigen::Vector6d state_vector = Eigen::Vector6d::Zero(); + state_vector(0) = current_reference(0); + state_vector(1) = current_reference(1); + return state_vector; +} + +inline Eigen::Vector2d fill_reference_goal( + const vortex_msgs::msg::GripperWaypoint& waypoint_goal) { + Eigen::Vector2d reference; + reference << waypoint_goal.roll, waypoint_goal.pinch; + return reference; +} + +inline vortex_msgs::msg::GripperReferenceFilter fill_reference_msg( + const Eigen::Vector6d& state_vector) { + vortex_msgs::msg::GripperReferenceFilter reference_msg; + reference_msg.roll = state_vector(0); + reference_msg.pinch = state_vector(1); + return reference_msg; +} + +inline Eigen::Vector2d apply_mode_logic( + const Eigen::Vector2d& requested_reference, + const Eigen::Vector2d& current_reference, + uint8_t mode) { + Eigen::Vector2d resolved_reference = requested_reference; + switch (mode) { + case vortex_msgs::msg::GripperWaypoint::ROLL_AND_PINCH: + break; + case vortex_msgs::msg::GripperWaypoint::ONLY_ROLL: + resolved_reference(1) = current_reference(1); + break; + case vortex_msgs::msg::GripperWaypoint::ONLY_PINCH: + resolved_reference(0) = current_reference(0); + break; + } + return resolved_reference; +} + +} // namespace vortex::guidance + +#endif // GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_UTILS_HPP_ diff --git a/gripper_guidance/src/gripper_reference_filter_ros.cpp b/gripper_guidance/src/gripper_reference_filter_ros.cpp index 8b50032..ac23c19 100644 --- a/gripper_guidance/src/gripper_reference_filter_ros.cpp +++ b/gripper_guidance/src/gripper_reference_filter_ros.cpp @@ -2,18 +2,17 @@ #include #include #include -#include #include -#include +#include "gripper_reference_filter/gripper_reference_filter_ros_utils.hpp" const auto start_message = R"( - - ____ _ ____ __ _____ _ _ _ - / ___|_ __ (_)_ __ _ __ ___ _ __ | _ \ ___ / _| ___ _ __ ___ _ __ ___ ___ | ___(_) | |_ ___ _ __ + + ____ _ ____ __ _____ _ _ _ + / ___|_ __ (_)_ __ _ __ ___ _ __ | _ \ ___ / _| ___ _ __ ___ _ __ ___ ___ | ___(_) | |_ ___ _ __ | | _| '_ \| | '_ \| '_ \ / _ \ '__| | |_) / _ \ |_ / _ \ '__/ _ \ '_ \ / __/ _ \ | |_ | | | __/ _ \ '__| -| |_| | |_) | | |_) | |_) | __/ | | _ < __/ _| __/ | | __/ | | | (_| __/ | _| | | | || __/ | - \____| .__/|_| .__/| .__/ \___|_| |_| \_\___|_| \___|_| \___|_| |_|\___\___| |_| |_|_|\__\\\___|_| - |_| |_| |_| +| |_| | |_) | | |_) | |_) | __/ | | _ < __/ _| __/ | | __/ | | | (_| __/ | _| | | | || __/ | + \____| .__/|_| .__/| .__/ \___|_| |_| \_\___|_| \___|_| \___|_| |_|\___\___| |_| |_|_|\__\\\___|_| + |_| |_| |_| )"; @@ -31,89 +30,78 @@ GripperReferenceFilterNode::GripperReferenceFilterNode(const rclcpp::NodeOptions hold_timer_ = this->create_wall_timer( time_step_, - std::bind(&GripperReferenceFilterNode::publish_hold_timer, this)); + [this]() { publish_hold_timer(); }); spdlog::info(start_message); } +GripperReferenceFilterNode::~GripperReferenceFilterNode() { + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } +} + void GripperReferenceFilterNode::set_subscribers_and_publisher() { - std::string guidance_topic = + const std::string guidance_topic = this->declare_parameter("topics.guidance_gripper"); - std::string gripper_state_topic = + const std::string gripper_state_topic = this->declare_parameter("topics.gripper_state"); - auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); - + const auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + reference_pub_ = this->create_publisher( guidance_topic, qos_sensor_data); reference_sub_ = this->create_subscription( gripper_state_topic, qos_sensor_data, - std::bind(&GripperReferenceFilterNode::reference_callback, this, - std::placeholders::_1)); + [this](const vortex_msgs::msg::GripperState::SharedPtr state_msg) { + reference_callback(state_msg); + }); } void GripperReferenceFilterNode::set_action_server() { this->declare_parameter("action_servers.gripper_reference_filter"); - std::string action_server_name = + const std::string action_server_name = this->get_parameter("action_servers.gripper_reference_filter").as_string(); - cb_group_ = - this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); action_server_ = rclcpp_action::create_server< vortex_msgs::action::GripperReferenceFilterWaypoint>( - - this, - - action_server_name, - - std::bind(&GripperReferenceFilterNode::handle_goal, this, - std::placeholders::_1, std::placeholders::_2), - - std::bind(&GripperReferenceFilterNode::handle_cancel, this, - std::placeholders::_1), - - std::bind(&GripperReferenceFilterNode::handle_accepted, this, - std::placeholders::_1), - - rcl_action_server_get_default_options(), cb_group_); + this, action_server_name, + [this](const auto& uuid, auto goal) { + return handle_goal(uuid, std::move(goal)); + }, + [this](auto goal_handle) { return handle_cancel(goal_handle); }, + [this](auto goal_handle) { handle_accepted(goal_handle); }); } void GripperReferenceFilterNode::set_refererence_filter() { this->declare_parameter>("zeta"); this->declare_parameter>("omega"); - std::vector zeta = this->get_parameter("zeta").as_double_array(); - std::vector omega = this->get_parameter("omega").as_double_array(); + const std::vector zeta = this->get_parameter("zeta").as_double_array(); + const std::vector omega = this->get_parameter("omega").as_double_array(); - Eigen::Vector2d zeta_eigen = Eigen::Map(zeta.data()); - Eigen::Vector2d omega_eigen = Eigen::Map(omega.data()); + const Eigen::Vector2d zeta_eigen = Eigen::Map(zeta.data()); + const Eigen::Vector2d omega_eigen = Eigen::Map(omega.data()); - GripperReferenceFilterParams filter_params{omega_eigen, zeta_eigen}; + const GripperReferenceFilterParams filter_params{omega_eigen, zeta_eigen}; gripper_reference_filter_ = std::make_unique(filter_params); } void GripperReferenceFilterNode::reference_callback( - const vortex_msgs::msg::GripperState::SharedPtr msg) { + const vortex_msgs::msg::GripperState::SharedPtr state_msg) { std::lock_guard lock(mutex_); - reference_ << msg->roll, msg->pinch; + reference_ << state_msg->roll, state_msg->pinch; } rclcpp_action::GoalResponse GripperReferenceFilterNode::handle_goal( - const rclcpp_action::GoalUUID& uuid, + const rclcpp_action::GoalUUID& /*uuid*/, std::shared_ptr - goal) { - (void)uuid; - (void)goal; + /*goal*/) { { std::lock_guard lock(mutex_); hold_active_ = false; - if (goal_handle_) { - if (goal_handle_->is_active()) { - spdlog::info("Aborting current goal and accepting new goal"); - preempted_goal_id_ = goal_handle_->get_goal_id(); - } - } } spdlog::info("Accepted goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; @@ -121,127 +109,68 @@ rclcpp_action::GoalResponse GripperReferenceFilterNode::handle_goal( rclcpp_action::CancelResponse GripperReferenceFilterNode::handle_cancel( const std::shared_ptr> goal_handle) { + vortex_msgs::action::GripperReferenceFilterWaypoint>> /*goal_handle*/) { spdlog::info("Received request to cancel goal"); - (void)goal_handle; return rclcpp_action::CancelResponse::ACCEPT; } void GripperReferenceFilterNode::handle_accepted( const std::shared_ptr> goal_handle) { - std::thread{std::bind(&GripperReferenceFilterNode::execute, this, - std::placeholders::_1), goal_handle}.detach(); -} - -Eigen::Vector6d GripperReferenceFilterNode::fill_reference_state() { - Eigen::Vector6d x = Eigen::Vector6d::Zero(); - - x(0) = reference_(0); // roll - x(1) = reference_(1); // pinch - x(2) = 0.0; // roll_dot - x(3) = 0.0; // pinch_dot - x(4) = 0.0; // roll_dotdot - x(5) = 0.0; // pinch_dotdot - - return x; -} - -Eigen::Vector2d GripperReferenceFilterNode::fill_reference_goal( - const vortex_msgs::msg::GripperWaypoint& goal) { - - double roll{goal.roll.roll}; - double pinch{goal.pinch.pinch}; - - Eigen::Vector2d reference; - reference << roll, pinch; - - return reference; -} - -vortex_msgs::msg::GripperReferenceFilter GripperReferenceFilterNode::fill_reference_msg() { - vortex_msgs::msg::GripperReferenceFilter feedback_msg; - - feedback_msg.roll = x_(0); - feedback_msg.pinch = x_(1); - feedback_msg.roll_dot = x_(2); - feedback_msg.pinch_dot = x_(3); - feedback_msg.roll_dotdot = x_(4); - feedback_msg.pinch_dotdot = x_(5); - - return feedback_msg; -} - -Eigen::Vector2d GripperReferenceFilterNode::apply_mode_logic( - const Eigen::Vector2d& reference_in, uint8_t mode) { - - Eigen::Vector2d reference_out = reference_in; - - switch (mode) { - case vortex_msgs::msg::GripperWaypoint::ROLL_AND_PINCH: - break; - - case vortex_msgs::msg::GripperWaypoint::ONLY_ROLL: - reference_out(1) = reference_(1); - break; - - case vortex_msgs::msg::GripperWaypoint::ONLY_PINCH: - reference_out(0) = reference_(0); - break; + std::lock_guard lock(execute_mutex_); + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); } + preempted_ = false; - return reference_out; + execute_thread_ = + std::thread([this, goal_handle]() { execute(goal_handle); }); } void GripperReferenceFilterNode::publish_hold_reference() { if (!reference_pub_) { return; } - vortex_msgs::msg::GripperReferenceFilter hold_msg; + auto hold_message = std::make_unique(); { std::lock_guard lock(mutex_); - hold_msg.roll = reference_(0); - hold_msg.pinch = reference_(1); - hold_msg.roll_dot = 0.0; - hold_msg.pinch_dot = 0.0; - hold_msg.roll_dotdot = 0.0; - hold_msg.pinch_dotdot = 0.0; - hold_reference_msg_ = hold_msg; + hold_message->roll = reference_(0); + hold_message->pinch = reference_(1); + hold_reference_msg_ = *hold_message; hold_active_ = true; } - reference_pub_->publish(hold_msg); + reference_pub_->publish(std::move(hold_message)); } void GripperReferenceFilterNode::publish_hold_timer() { - vortex_msgs::msg::GripperReferenceFilter hold_msg; + auto hold_message = std::make_unique(); { std::lock_guard lock(mutex_); if (!hold_active_ || !reference_pub_) { return; } - hold_msg = hold_reference_msg_; + *hold_message = hold_reference_msg_; } - reference_pub_->publish(hold_msg); + reference_pub_->publish(std::move(hold_message)); } void GripperReferenceFilterNode::execute( const std::shared_ptr> goal_handle) { + spdlog::info("Executing goal"); + { std::lock_guard lock(mutex_); - this->goal_handle_ = goal_handle; + filter_state_ = fill_reference_state(reference_); } - spdlog::info("Executing goal"); - - x_ = fill_reference_state(); - - const vortex_msgs::msg::GripperWaypoint goal = + const vortex_msgs::msg::GripperWaypoint waypoint_goal = goal_handle->get_goal()->waypoint; - uint8_t mode = goal.mode; + const uint8_t mode = waypoint_goal.mode; double convergence_threshold = goal_handle->get_goal()->convergence_threshold; if (convergence_threshold <= 0.0) { @@ -251,48 +180,47 @@ void GripperReferenceFilterNode::execute( "Using default 0.1"); } - Eigen::Vector2d reference_temp = fill_reference_goal(goal); - Eigen::Vector2d goal_reference = apply_mode_logic(reference_temp, mode); // ← fixed: local, not reference_ + Eigen::Vector2d goal_reference; + { + std::lock_guard lock(mutex_); + goal_reference = + apply_mode_logic(fill_reference_goal(waypoint_goal), reference_, mode); + } - auto feedback = std::make_shared< - vortex_msgs::action::GripperReferenceFilterWaypoint::Feedback>(); auto result = std::make_shared< vortex_msgs::action::GripperReferenceFilterWaypoint::Result>(); rclcpp::Rate loop_rate(1000.0 / time_step_.count()); while (rclcpp::ok()) { - { - std::lock_guard lock(mutex_); - if (goal_handle->get_goal_id() == preempted_goal_id_) { - publish_hold_reference(); - result->success = false; - goal_handle->abort(result); - return; - } + if (preempted_.load()) { + publish_hold_reference(); + result->success = false; + goal_handle->abort(result); + spdlog::info("Goal preempted by newer goal"); + return; } - { - std::lock_guard lock(mutex_); - if (goal_handle->is_canceling()) { - publish_hold_reference(); - result->success = false; - goal_handle->canceled(result); - spdlog::info("Goal canceled"); - return; - } + + if (goal_handle->is_canceling()) { + publish_hold_reference(); + result->success = false; + goal_handle->canceled(result); + spdlog::info("Goal canceled"); + return; } - Eigen::Vector6d x_dot = gripper_reference_filter_->calculate_x_dot(x_, goal_reference); // ← fixed - x_ += x_dot * time_step_.count() / 1000.0; + const Eigen::Vector6d state_derivative = + gripper_reference_filter_->calculate_x_dot(filter_state_, goal_reference); + filter_state_ += state_derivative * time_step_.count() / 1000.0; - vortex_msgs::msg::GripperReferenceFilter feedback_msg = fill_reference_msg(); + auto reference_message = + std::make_unique( + fill_reference_msg(filter_state_)); { std::lock_guard lock(mutex_); - hold_reference_msg_ = feedback_msg; + hold_reference_msg_ = *reference_message; } - feedback->reference = feedback_msg; - reference_pub_->publish(feedback_msg); - goal_handle->publish_feedback(feedback); + reference_pub_->publish(std::move(reference_message)); Eigen::Vector2d current_state; { @@ -314,16 +242,18 @@ void GripperReferenceFilterNode::execute( } if (error.norm() < convergence_threshold) { - result->success = true; - goal_handle->succeed(result); - x_.head(2) = goal_reference; - vortex_msgs::msg::GripperReferenceFilter final_msg = fill_reference_msg(); - reference_pub_->publish(final_msg); + filter_state_.head(2) = goal_reference; + auto final_message = + std::make_unique( + fill_reference_msg(filter_state_)); { std::lock_guard lock(mutex_); - hold_reference_msg_ = final_msg; + hold_reference_msg_ = *final_message; hold_active_ = true; } + reference_pub_->publish(std::move(final_message)); + result->success = true; + goal_handle->succeed(result); spdlog::info("Goal reached"); return; } @@ -332,8 +262,6 @@ void GripperReferenceFilterNode::execute( } if (!rclcpp::ok() && goal_handle->is_active()) { - auto result = std::make_shared< - vortex_msgs::action::GripperReferenceFilterWaypoint::Result>(); result->success = false; try { goal_handle->abort(result); From 080574a9fe65eade48d84261318cf0fdb5146077 Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Mon, 18 May 2026 16:35:37 +0200 Subject: [PATCH 17/24] guidance(gripper): const-correct local variables in filter math Addresses the PR review comment on gripper_controller/src/gripper_controller_ros.cpp L127 (jorgenfj): "These local state variables should preferably be a const. Should look into this for complex const initialization using lambdas". Although the comment was filed against the controller, the same const- correctness principle was applied across both packages. In this file: * calculate_x_dot: x_dot is now const Eigen::Vector6d. It is computed once from Ad_*x + Bd_*r and never mutated; const prevents future accidental writes that would invalidate the return value. * calculate_Ad: omega_diag, zeta_diag, omega_diag_squared, omega_diag_cubed are now const Eigen::Matrix2d. All four are derived from omega/zeta and only ever read when assembling Ad_ blocks; const documents that they are mathematical intermediates. * calculate_Bd: omega_diag, omega_diag_squared, omega_diag_cubed likewise const; same reasoning. No member variables were made const. Ad_ and Bd_ are assembled incrementally inside calculate_Ad/calculate_Bd and so genuinely require mutation. --- .../src/gripper_reference_filter.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/gripper_guidance/src/gripper_reference_filter.cpp b/gripper_guidance/src/gripper_reference_filter.cpp index 648ddad..d87eddb 100644 --- a/gripper_guidance/src/gripper_reference_filter.cpp +++ b/gripper_guidance/src/gripper_reference_filter.cpp @@ -11,7 +11,7 @@ GripperReferenceFilter::GripperReferenceFilter(const GripperReferenceFilterParam Eigen::Vector6d GripperReferenceFilter::calculate_x_dot(const Eigen::Vector6d& x, const Eigen::Vector2d& r) { - Eigen::Vector6d x_dot = Ad_ * x + Bd_ * r; + const Eigen::Vector6d x_dot = Ad_ * x + Bd_ * r; return x_dot; } @@ -19,11 +19,11 @@ Eigen::Vector6d GripperReferenceFilter::calculate_x_dot(const Eigen::Vector6d& x void GripperReferenceFilter::calculate_Ad(const Eigen::Vector2d& omega, const Eigen::Vector2d& zeta) { - Eigen::Matrix2d omega_diag = omega.asDiagonal(); - Eigen::Matrix2d zeta_diag = zeta.asDiagonal(); - Eigen::Matrix2d omega_diag_squared = omega_diag * omega_diag; - Eigen::Matrix2d omega_diag_cubed = omega_diag_squared * omega_diag; - + const Eigen::Matrix2d omega_diag = omega.asDiagonal(); + const Eigen::Matrix2d zeta_diag = zeta.asDiagonal(); + const Eigen::Matrix2d omega_diag_squared = omega_diag * omega_diag; + const Eigen::Matrix2d omega_diag_cubed = omega_diag_squared * omega_diag; + Ad_.block<2,2>(0, 2) = Eigen::Matrix2d::Identity(); Ad_.block<2,2>(2, 4) = Eigen::Matrix2d::Identity(); @@ -34,10 +34,10 @@ void GripperReferenceFilter::calculate_Ad(const Eigen::Vector2d& omega, } void GripperReferenceFilter::calculate_Bd(const Eigen::Vector2d& omega) { - Eigen::Matrix2d omega_diag = omega.asDiagonal(); - Eigen::Matrix2d omega_diag_squared = omega_diag * omega_diag; - Eigen::Matrix2d omega_diag_cubed = omega_diag_squared * omega_diag; - + const Eigen::Matrix2d omega_diag = omega.asDiagonal(); + const Eigen::Matrix2d omega_diag_squared = omega_diag * omega_diag; + const Eigen::Matrix2d omega_diag_cubed = omega_diag_squared * omega_diag; + Bd_.block<2,2>(4, 0) = omega_diag_cubed; } From a9e3c7a41c472d80fb2b1a072a47ed73a10d709d Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Mon, 18 May 2026 16:35:56 +0200 Subject: [PATCH 18/24] controller(gripper): apply const, lambdas and unique_ptr publish Addresses three PR review comments. Two were filed against gripper_guidance/src/gripper_reference_filter_ros.cpp but apply equally here; one was filed directly against this file. L127 (jorgenfj): "These local state variables should preferably be a const. Should look into this for complex const initialization using lambdas" set_controller_params: proportional_gain_matrix is now const, initialised via an IIFE lambda that builds the 2x2 zero matrix, sets the diagonal entries, and returns it. Prevents any later accidental write to the gain matrix between construction and the controller_.set_kp(...) call. set_subscribers_and_publisher: qos_sensor_data is now const. publish_control: measured_state and reference_state are now const, obtained via an IIFE lambda that takes state_mutex_, snapshots the four members into local structs, and returns them as a std::pair. The lambda's scope owns the lock; the const locals are visible outside the critical section, which both minimises lock duration and prevents accidental mutation afterward. velocity_command is also const. L53 (jorgenfj): "Consider using lambdas instead of std::bind to avoid using std::placeholders" Both create_subscription callbacks (reference_sub_, state_sub_) and the create_wall_timer callback now use lambdas: [this](const T::SharedPtr message) { method(message); } [this]() { publish_control(); } Lambdas were chosen because: (1) readability, argument intent is visible at the call site, (2) no std::placeholders::_N ordering bug surface, (3) explicit capture documents which state the callback closes over. L294 (jorgenfj): "Should use unique pointer for this kind of pipeline style publishing. Ref: ...Composable_nodes" control_pub_->publish(velocity_command_msg) (lvalue copy path) converted to std::make_unique(...) + publish(std::move(velocity_command_msg)). header.stamp is set through the unique_ptr before the move. Enables the zero-copy intra-process path. --- .../src/gripper_controller_ros.cpp | 70 ++++++++----------- 1 file changed, 30 insertions(+), 40 deletions(-) diff --git a/gripper_controller/src/gripper_controller_ros.cpp b/gripper_controller/src/gripper_controller_ros.cpp index 1b20f53..5d4ae77 100644 --- a/gripper_controller/src/gripper_controller_ros.cpp +++ b/gripper_controller/src/gripper_controller_ros.cpp @@ -1,7 +1,7 @@ #include "gripper_controller/gripper_controller_ros.hpp" #include -#include +#include #include #include #include "gripper_controller/gripper_controller_translator.hpp" @@ -19,10 +19,6 @@ const auto start_message = R"( namespace vortex::controller { -// --------------------------------------------------------------------------- -// CONSTRUCTOR -// --------------------------------------------------------------------------- - GripperControllerNode::GripperControllerNode(const rclcpp::NodeOptions & options) : Node("gripper_controller_node", options) { time_step_ = std::chrono::milliseconds(10); @@ -34,10 +30,6 @@ GripperControllerNode::GripperControllerNode(const rclcpp::NodeOptions & options spdlog::info(start_message); } -// --------------------------------------------------------------------------- -// SETUP -// --------------------------------------------------------------------------- - void GripperControllerNode::set_controller_params() { const int time_step_ms = this->declare_parameter("time_step_ms", 10); @@ -48,9 +40,12 @@ void GripperControllerNode::set_controller_params() { const double kp_pinch = this->declare_parameter("kp.pinch", 1.0); - types::Matrix2d proportional_gain_matrix = types::Matrix2d::Zero(); - proportional_gain_matrix(0, 0) = kp_roll; - proportional_gain_matrix(1, 1) = kp_pinch; + const types::Matrix2d proportional_gain_matrix = [&] { + types::Matrix2d matrix = types::Matrix2d::Zero(); + matrix(0, 0) = kp_roll; + matrix(1, 1) = kp_pinch; + return matrix; + }(); controller_.set_kp(proportional_gain_matrix); controller_.set_time_step(static_cast(time_step_ms) / 1000.0); @@ -67,19 +62,21 @@ void GripperControllerNode::set_subscribers_and_publisher() { const std::string control_topic = this->declare_parameter("topics.control"); - auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + const auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); reference_sub_ = this->create_subscription( reference_topic, qos_sensor_data, - std::bind(&GripperControllerNode::reference_callback, this, - std::placeholders::_1)); + [this](const vortex_msgs::msg::GripperReferenceFilter::SharedPtr msg) { + reference_callback(msg); + }); state_sub_ = this->create_subscription( state_topic, qos_sensor_data, - std::bind(&GripperControllerNode::state_callback, this, - std::placeholders::_1)); + [this](const vortex_msgs::msg::GripperState::SharedPtr msg) { + state_callback(msg); + }); control_pub_ = this->create_publisher( @@ -89,13 +86,9 @@ void GripperControllerNode::set_subscribers_and_publisher() { // initialised before the first publish_control() fires. control_timer_ = this->create_wall_timer( time_step_, - std::bind(&GripperControllerNode::publish_control, this)); + [this]() { publish_control(); }); } -// --------------------------------------------------------------------------- -// CALLBACKS -// --------------------------------------------------------------------------- - void GripperControllerNode::reference_callback( const vortex_msgs::msg::GripperReferenceFilter::SharedPtr reference_msg) { std::lock_guard lock(state_mutex_); @@ -110,31 +103,28 @@ void GripperControllerNode::state_callback( pinch_measured_ = state_msg->pinch; } -// --------------------------------------------------------------------------- -// CONTROL LOOP -// --------------------------------------------------------------------------- - void GripperControllerNode::publish_control() { - types::GripperState measured_state; - types::GripperState reference_state; - - { + const auto [measured_state, reference_state] = [this] { std::lock_guard lock(state_mutex_); - measured_state.roll = roll_measured_; - measured_state.pinch = pinch_measured_; - reference_state.roll = roll_ref_; - reference_state.pinch = pinch_ref_; - } + types::GripperState measured; + types::GripperState reference; + measured.roll = roll_measured_; + measured.pinch = pinch_measured_; + reference.roll = roll_ref_; + reference.pinch = pinch_ref_; + return std::pair{measured, reference}; + }(); const types::Vector2d velocity_command = controller_.calculate_velocity(measured_state, reference_state); - vortex_msgs::msg::GripperStateVelocityCommand velocity_command_msg = - gripper_controller::translator::velocity_command_to_gripper_velocity_command_msg( - velocity_command); - velocity_command_msg.header.stamp = this->now(); + auto velocity_command_msg = + std::make_unique( + gripper_controller::translator::velocity_command_to_gripper_velocity_command_msg( + velocity_command)); + velocity_command_msg->header.stamp = this->now(); - control_pub_->publish(velocity_command_msg); + control_pub_->publish(std::move(velocity_command_msg)); } RCLCPP_COMPONENTS_REGISTER_NODE(GripperControllerNode) From 6a336219adf70f8b37afe4c443049a4f4d6df4ad Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Mon, 18 May 2026 16:36:16 +0200 Subject: [PATCH 19/24] controller(gripper): const-correct local variables in control law Addresses the PR review comment on gripper_controller/src/gripper_controller_ros.cpp L127 (jorgenfj): "These local state variables should preferably be a const. Should look into this for complex const initialization using lambdas". In calculate_velocity: * position_error is now const types::Vector2d, initialised via an IIFE lambda that constructs the local Eigen vector, writes the two components, and returns it. Prevents later writes that could desynchronise the error from the state inputs. * velocity_command is left non-const intentionally: std::clamp is used to saturate both components in place after the Kp_ * error multiplication, so the variable genuinely requires mutation. Marked here so the asymmetry is not mistaken for an oversight. --- gripper_controller/src/gripper_controller.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/gripper_controller/src/gripper_controller.cpp b/gripper_controller/src/gripper_controller.cpp index 617ff97..997582e 100644 --- a/gripper_controller/src/gripper_controller.cpp +++ b/gripper_controller/src/gripper_controller.cpp @@ -9,9 +9,12 @@ types::Vector2d GripperController::calculate_velocity( const types::GripperState& measured_state, const types::GripperState& reference_state) { - types::Vector2d position_error; - position_error(0) = reference_state.roll - measured_state.roll; - position_error(1) = reference_state.pinch - measured_state.pinch; + const types::Vector2d position_error = [&] { + types::Vector2d error; + error(0) = reference_state.roll - measured_state.roll; + error(1) = reference_state.pinch - measured_state.pinch; + return error; + }(); types::Vector2d velocity_command = Kp_ * position_error; From eeebeb60173d7db9d25033201eb57f8bea7868de Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Tue, 19 May 2026 11:48:11 +0200 Subject: [PATCH 20/24] gripper: rename time step variables to include unit suffix Addresses PR review comments on gripper_controller/src/gripper_controller_ros.cpp (jorgenfj, vortexntnu/vortex-gripper#10): L24: "this is also assigned in set_controller_params(). Also maybe call the variable time_step_ms_ to be more explicit" Removed the redundant default assignment of time_step_ms_ in the GripperControllerNode constructor. The ROS parameter declaration in set_controller_params() is the single source of truth. Renamed time_step_ to time_step_ms_ in GripperControllerNode to make the millisecond unit explicit in the member name. L28 on gripper_controller/include/gripper_controller/gripper_controller.hpp: "timestep_s_?" Renamed timestep_ to time_step_ms_ in GripperController. The member is not used in the P-controller math (velocity = Kp * error has no time dependency), so the unit rename has no effect on computed output. Setter renamed from set_time_step to set_time_step_ms accordingly. Caller now passes the millisecond integer directly rather than dividing by 1000. Companion: gripper_guidance/config/gripper_reference_filter_params.yaml gains time_step_ms: 10, matching the controller config convention. The reference filter ROS node declares this parameter in set_refererence_filter(); the hardcoded constructor default is removed. --- .../gripper_controller/gripper_controller.hpp | 20 +++++++++---------- .../gripper_controller_ros.hpp | 2 +- gripper_controller/src/gripper_controller.cpp | 6 +++--- .../src/gripper_controller_ros.cpp | 12 +++++------ .../test/test_gripper_controller.cpp | 2 +- .../gripper_reference_filter_params.yaml | 9 ++++++--- 6 files changed, 26 insertions(+), 25 deletions(-) diff --git a/gripper_controller/include/gripper_controller/gripper_controller.hpp b/gripper_controller/include/gripper_controller/gripper_controller.hpp index 4fde033..d969e7b 100644 --- a/gripper_controller/include/gripper_controller/gripper_controller.hpp +++ b/gripper_controller/include/gripper_controller/gripper_controller.hpp @@ -9,23 +9,23 @@ class GripperController { GripperController(); // @brief Calculate velocity command from position error - // @param state: struct containing measured gripper state [roll, pinch] - // @param reference: struct containing desired gripper state [roll, pinch] + // @param measured_state: struct containing measured gripper state [roll, pinch] + // @param reference_state: struct containing desired gripper state [roll, pinch] // @return 2D vector containing velocity commands [roll_vel, pinch_vel] - types::Vector2d calculate_velocity(const types::GripperState& state, - const types::GripperState& reference); + types::Vector2d calculate_velocity(const types::GripperState& measured_state, + const types::GripperState& reference_state); // @brief Set the proportional gain matrix - // @param Kp: 2x2 matrix containing the proportional gain matrix - void set_kp(const types::Matrix2d& Kp); + // @param proportional_gain_matrix: 2x2 matrix containing the proportional gain matrix + void set_kp(const types::Matrix2d& proportional_gain_matrix); - // @brief Set the time step - // @param dt: time step in seconds - void set_time_step(double timestep); + // @brief Set the controller loop period in milliseconds + // @param time_step_ms: time step in milliseconds + void set_time_step_ms(double time_step_ms); private: types::Matrix2d Kp_; - double timestep_; + double time_step_ms_; }; diff --git a/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp b/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp index 6069336..a39a7b8 100644 --- a/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp +++ b/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp @@ -49,7 +49,7 @@ class GripperControllerNode : public rclcpp::Node { control_pub_; rclcpp::TimerBase::SharedPtr control_timer_; - std::chrono::milliseconds time_step_; + std::chrono::milliseconds time_step_ms_; std::mutex state_mutex_; diff --git a/gripper_controller/src/gripper_controller.cpp b/gripper_controller/src/gripper_controller.cpp index 997582e..8f1a3ba 100644 --- a/gripper_controller/src/gripper_controller.cpp +++ b/gripper_controller/src/gripper_controller.cpp @@ -3,7 +3,7 @@ #include GripperController::GripperController() - : Kp_(types::Matrix2d::Identity()), timestep_(0.01) {} + : Kp_(types::Matrix2d::Identity()), time_step_ms_(10.0) {} types::Vector2d GripperController::calculate_velocity( const types::GripperState& measured_state, @@ -32,6 +32,6 @@ void GripperController::set_kp(const types::Matrix2d& proportional_gain_matrix) Kp_ = proportional_gain_matrix; } -void GripperController::set_time_step(double timestep) { - timestep_ = timestep; +void GripperController::set_time_step_ms(double time_step_ms) { + time_step_ms_ = time_step_ms; } diff --git a/gripper_controller/src/gripper_controller_ros.cpp b/gripper_controller/src/gripper_controller_ros.cpp index 5d4ae77..34aed9c 100644 --- a/gripper_controller/src/gripper_controller_ros.cpp +++ b/gripper_controller/src/gripper_controller_ros.cpp @@ -21,8 +21,6 @@ namespace vortex::controller { GripperControllerNode::GripperControllerNode(const rclcpp::NodeOptions & options) : Node("gripper_controller_node", options) { - time_step_ = std::chrono::milliseconds(10); - set_controller_params(); set_subscribers_and_publisher(); @@ -31,9 +29,9 @@ GripperControllerNode::GripperControllerNode(const rclcpp::NodeOptions & options } void GripperControllerNode::set_controller_params() { - const int time_step_ms = + const int time_step_ms_param = this->declare_parameter("time_step_ms", 10); - time_step_ = std::chrono::milliseconds(time_step_ms); + time_step_ms_ = std::chrono::milliseconds(time_step_ms_param); const double kp_roll = this->declare_parameter("kp.roll", 1.0); @@ -48,10 +46,10 @@ void GripperControllerNode::set_controller_params() { }(); controller_.set_kp(proportional_gain_matrix); - controller_.set_time_step(static_cast(time_step_ms) / 1000.0); + controller_.set_time_step_ms(static_cast(time_step_ms_param)); spdlog::info("GripperController: kp_roll={:.3f} kp_pinch={:.3f} dt={}ms", - kp_roll, kp_pinch, time_step_ms); + kp_roll, kp_pinch, time_step_ms_param); } void GripperControllerNode::set_subscribers_and_publisher() { @@ -85,7 +83,7 @@ void GripperControllerNode::set_subscribers_and_publisher() { // Timer is created last so that controller_ and the publisher are fully // initialised before the first publish_control() fires. control_timer_ = this->create_wall_timer( - time_step_, + time_step_ms_, [this]() { publish_control(); }); } diff --git a/gripper_controller/test/test_gripper_controller.cpp b/gripper_controller/test/test_gripper_controller.cpp index 00dd406..22f71ec 100644 --- a/gripper_controller/test/test_gripper_controller.cpp +++ b/gripper_controller/test/test_gripper_controller.cpp @@ -12,7 +12,7 @@ class GripperControllerTest : public ::testing::Test { void SetUp() override { types::Matrix2d identity_gain = types::Matrix2d::Identity(); controller_.set_kp(identity_gain); - controller_.set_time_step(0.01); + controller_.set_time_step_ms(10.0); } // @brief Helper: run calculate_velocity with explicit scalar inputs. diff --git a/gripper_guidance/config/gripper_reference_filter_params.yaml b/gripper_guidance/config/gripper_reference_filter_params.yaml index 03f5643..6e93944 100644 --- a/gripper_guidance/config/gripper_reference_filter_params.yaml +++ b/gripper_guidance/config/gripper_reference_filter_params.yaml @@ -1,14 +1,17 @@ /**: ros__parameters: + # Integration loop period in milliseconds + time_step_ms: 10 + # Filter dynamics parameters - zeta: [1.0, 1.0] - omega: [0.4, 0.4] + zeta: [1.0, 1.0] + omega: [0.4, 0.4] # Topics topics: guidance_gripper: "nautilus/gripper/guidance" gripper_state: "nautilus/gripper/state" - + # Action servers action_servers: gripper_reference_filter: "nautilus/gripper/reference_filter" From 9e0bca53f17e3f873277627702857c320afdcfba Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Tue, 19 May 2026 11:48:28 +0200 Subject: [PATCH 21/24] guidance(gripper): move filter state ownership into GripperReferenceFilter Addresses PR review comment on gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp (jorgenfj, vortexntnu/vortex-gripper#10): L73: "Maybe the reference filter class should own the filter_state_?" The 6D filter state [roll, pinch, roll_dot, pinch_dot, roll_dotdot, pinch_dotdot] is now a private member of GripperReferenceFilter rather than the ROS node. The Fossen third-order filter (Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen 2021 p. 336 eq. 12.5-6) requires the full 6D state internally to produce smooth output; only the first two components (roll, pinch) are ever published on the wire. New public API: reset(initial_reference) -- seeds position components, zeros higher order step(goal_reference, time_step_seconds) -- integrates one time step snap_to(goal_reference) -- pins position components at convergence reference_output() -- returns 2D [roll, pinch] for publication calculate_x_dot moved to private; calculate_Ad/calculate_Bd remain public for testability. Tests updated to use the new stateful API. --- .../gripper_reference_filter.hpp | 43 +++++++-- .../src/gripper_reference_filter.cpp | 38 ++++++-- .../test/test_gripper_reference_filter.cpp | 88 ++++++++++--------- 3 files changed, 111 insertions(+), 58 deletions(-) diff --git a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter.hpp b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter.hpp index 3f42390..c1ce725 100644 --- a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter.hpp +++ b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter.hpp @@ -14,14 +14,28 @@ class GripperReferenceFilter { public: explicit GripperReferenceFilter(const GripperReferenceFilterParams& params); - // @brief Calculate the state derivative - // @param x The state vector 6x1 - // @param r The reference vector 2x1 - // @return The state derivative 6x1 - // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen - // 2021 p. 336 eq: 12.5 - Eigen::Vector6d calculate_x_dot(const Eigen::Vector6d& x, - const Eigen::Vector2d& r); + // @brief Reset the internal 6D filter state. Position components are seeded + // from the supplied reference; velocity and acceleration components + // are zeroed. + // @param initial_reference 2D vector [roll, pinch] + void reset(const Eigen::Vector2d& initial_reference); + + // @brief Integrate the filter one time step toward the goal reference. + // @param goal_reference 2D vector [roll, pinch] + // @param time_step_seconds integration step in seconds + void step(const Eigen::Vector2d& goal_reference, double time_step_seconds); + + // @brief Snap the position components of the filter state to the goal + // reference. Used at convergence so that the published output + // exactly matches the requested goal. + // @param goal_reference 2D vector [roll, pinch] + void snap_to(const Eigen::Vector2d& goal_reference); + + // @brief Return the position output of the filter (roll, pinch). The + // higher-order state components are intentionally not exposed on + // the wire. + // @return 2D vector [roll, pinch] + Eigen::Vector2d reference_output() const; // @brief Calculate the state transition matrix // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen @@ -31,12 +45,23 @@ class GripperReferenceFilter { // @brief Calculate the input matrix // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen - // 2021 p. 336 eq: 12.6 + // 2021 p. 336 eq: 12.6 void calculate_Bd(const Eigen::Vector2d& omega); private: + // @brief Calculate the state derivative + // @param state The state vector 6x1 + // @param reference The reference vector 2x1 + // @return The state derivative 6x1 + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 336 eq: 12.5 + Eigen::Vector6d calculate_state_derivative( + const Eigen::Vector6d& state, + const Eigen::Vector2d& reference) const; + Eigen::Matrix6d Ad_; Eigen::Matrix6x2d Bd_; + Eigen::Vector6d filter_state_; }; } // namespace vortex::guidance diff --git a/gripper_guidance/src/gripper_reference_filter.cpp b/gripper_guidance/src/gripper_reference_filter.cpp index d87eddb..1235785 100644 --- a/gripper_guidance/src/gripper_reference_filter.cpp +++ b/gripper_guidance/src/gripper_reference_filter.cpp @@ -3,22 +3,42 @@ namespace vortex::guidance { GripperReferenceFilter::GripperReferenceFilter(const GripperReferenceFilterParams& params) { - Ad_.setZero(); - Bd_.setZero(); - calculate_Ad(params.omega, params.zeta); - calculate_Bd(params.omega); + Ad_.setZero(); + Bd_.setZero(); + filter_state_.setZero(); + calculate_Ad(params.omega, params.zeta); + calculate_Bd(params.omega); } -Eigen::Vector6d GripperReferenceFilter::calculate_x_dot(const Eigen::Vector6d& x, - const Eigen::Vector2d& r) { - const Eigen::Vector6d x_dot = Ad_ * x + Bd_ * r; +void GripperReferenceFilter::reset(const Eigen::Vector2d& initial_reference) { + filter_state_.setZero(); + filter_state_(0) = initial_reference(0); + filter_state_(1) = initial_reference(1); +} + +void GripperReferenceFilter::step(const Eigen::Vector2d& goal_reference, + double time_step_seconds) { + const Eigen::Vector6d state_derivative = + calculate_state_derivative(filter_state_, goal_reference); + filter_state_ += state_derivative * time_step_seconds; +} - return x_dot; +void GripperReferenceFilter::snap_to(const Eigen::Vector2d& goal_reference) { + filter_state_.head(2) = goal_reference; } +Eigen::Vector2d GripperReferenceFilter::reference_output() const { + return filter_state_.head(2); +} + +Eigen::Vector6d GripperReferenceFilter::calculate_state_derivative( + const Eigen::Vector6d& state, const Eigen::Vector2d& reference) const { + const Eigen::Vector6d state_derivative = Ad_ * state + Bd_ * reference; + return state_derivative; +} void GripperReferenceFilter::calculate_Ad(const Eigen::Vector2d& omega, - const Eigen::Vector2d& zeta) { + const Eigen::Vector2d& zeta) { const Eigen::Matrix2d omega_diag = omega.asDiagonal(); const Eigen::Matrix2d zeta_diag = zeta.asDiagonal(); const Eigen::Matrix2d omega_diag_squared = omega_diag * omega_diag; diff --git a/gripper_guidance/test/test_gripper_reference_filter.cpp b/gripper_guidance/test/test_gripper_reference_filter.cpp index 1390454..5dc5632 100644 --- a/gripper_guidance/test/test_gripper_reference_filter.cpp +++ b/gripper_guidance/test/test_gripper_reference_filter.cpp @@ -12,72 +12,80 @@ class ReferenceFilterTest : public ::testing::Test { filter = std::make_unique(params); } - Eigen::Vector6d integrate_steps(Eigen::Vector6d x, const Eigen::Vector2d& r, int steps, double dt = 0.01) { + Eigen::Vector2d integrate_to_steady_state(const Eigen::Vector2d& initial_reference, + const Eigen::Vector2d& goal_reference, + int steps, + double time_step_seconds = 0.01) { + filter->reset(initial_reference); for (int i = 0; i < steps; ++i) { - x += dt * filter->calculate_x_dot(x, r); + filter->step(goal_reference, time_step_seconds); } - return x; + return filter->reference_output(); } - bool position_converged_to_reference(const Eigen::Vector6d& x, const Eigen::Vector2d& r, double tol = 1e-2) { - return std::abs(x(0) - r(0)) < tol && std::abs(x(1) - r(1)) < tol; + bool position_converged_to_reference(const Eigen::Vector2d& output, + const Eigen::Vector2d& goal_reference, + double tol = 1e-2) { + return std::abs(output(0) - goal_reference(0)) < tol && + std::abs(output(1) - goal_reference(1)) < tol; } - bool velocity_within_bounds(const Eigen::Vector6d& x, double vel_bound = 10.0, double acc_bound = 100.0) { - return std::abs(x(2)) < vel_bound && std::abs(x(4)) < acc_bound; - } - - bool no_overshoot(const Eigen::Vector6d& x, const Eigen::Vector2d& r, double tol = 1e-6) { - return x(0) <= r(0) + tol && x(1) <= r(1) + tol; + bool no_overshoot(const Eigen::Vector2d& output, + const Eigen::Vector2d& goal_reference, + double tol = 1e-6) { + return output(0) <= goal_reference(0) + tol && + output(1) <= goal_reference(1) + tol; } std::unique_ptr filter; }; -// Zero input, zero state -> zero derivative -TEST_F(ReferenceFilterTest, ZeroInputZeroState) { - Eigen::Vector6d x_dot = filter->calculate_x_dot(Eigen::Vector6d::Zero(), Eigen::Vector2d::Zero()); - EXPECT_TRUE(x_dot.isZero(1e-9)); -} - -// Step response: x_d should converge to r over time (Fossen Eq. 12.11-12.12) -TEST_F(ReferenceFilterTest, StepResponseConverges) { - Eigen::Vector2d r; - r << 1.0, 0.5; +// Reset seeds the position output but produces no motion until step() is called. +TEST_F(ReferenceFilterTest, ResetSeedsPositionOutput) { + Eigen::Vector2d seed; + seed << 0.5, -0.25; - Eigen::Vector6d x = integrate_steps(Eigen::Vector6d::Zero(), r, 2000); + filter->reset(seed); - EXPECT_TRUE(position_converged_to_reference(x, r)); + EXPECT_TRUE(filter->reference_output().isApprox(seed, 1e-9)); } -// Velocity and acceleration states should stay bounded throughout transient -TEST_F(ReferenceFilterTest, VelocityAndAccelerationBounded) { - Eigen::Vector2d r; - r << 1.0, 1.0; - Eigen::Vector6d x = Eigen::Vector6d::Zero(); +// Step response: filter output converges to the goal reference over time +// (Fossen Eq. 12.11-12.12). +TEST_F(ReferenceFilterTest, StepResponseConverges) { + Eigen::Vector2d goal; + goal << 1.0, 0.5; + + const Eigen::Vector2d output = + integrate_to_steady_state(Eigen::Vector2d::Zero(), goal, 2000); - for (int i = 0; i < 1000; ++i) { - x += 0.01 * filter->calculate_x_dot(x, r); - EXPECT_TRUE(velocity_within_bounds(x)); - } + EXPECT_TRUE(position_converged_to_reference(output, goal)); } -// Critically damped (zeta=1) should produce no overshoot +// Critically damped (zeta=1) should produce no overshoot. TEST_F(ReferenceFilterTest, NoCriticallyDampedOvershoot) { - Eigen::Vector2d r; - r << 1.0, 1.0; - Eigen::Vector6d x = Eigen::Vector6d::Zero(); + Eigen::Vector2d goal; + goal << 1.0, 1.0; + filter->reset(Eigen::Vector2d::Zero()); for (int i = 0; i < 2000; ++i) { - x += 0.01 * filter->calculate_x_dot(x, r); - EXPECT_TRUE(no_overshoot(x, r)); + filter->step(goal, 0.01); + EXPECT_TRUE(no_overshoot(filter->reference_output(), goal)); } } -// namespace vortex::guidance +// snap_to forces the position output to match the goal exactly. +TEST_F(ReferenceFilterTest, SnapToMatchesGoal) { + Eigen::Vector2d goal; + goal << 3.14, -0.7; + + filter->reset(Eigen::Vector2d::Zero()); + filter->snap_to(goal); + + EXPECT_TRUE(filter->reference_output().isApprox(goal, 1e-9)); +} int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); } From 6ff86a2f5058c456143c81a732a2b62da232ca48 Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Tue, 19 May 2026 11:48:58 +0200 Subject: [PATCH 22/24] guidance(gripper): refactor reference filter ROS node per PR review Addresses PR review comments on gripper_guidance/src/gripper_reference_filter_ros.cpp (jorgenfj, vortexntnu/vortex-gripper#10): L158: "What is this used for?" Renamed publish_hold_timer to republish_held_reference_tick, hold_timer_ to held_reference_republish_timer_, hold_active_ to holding_reference_, hold_reference_msg_ to last_published_reference_, and publish_hold_reference to latch_current_state_as_held_reference. The doxygen on republish_held_reference_tick explains the invariant it maintains: downstream consumers keep receiving the last committed reference when no goal is active, preventing the convergence check from spuriously succeeding when a new goal targets a value near zero and the measured state defaults to zero. Bug fix (ApatShe, vortexntnu/vortex-gripper#10): "after serving a goal of roll to 3.14, if we send a goal of roll to 0 the action server reports success IMMEDIATELY likely due to assuming a default value of 0 if we DIDN'T hold previous value" execute() now seeds the filter from last_published_reference_ when holding_reference_ is true, falling back to measured_reference_ only on first use. Convergence is checked against the filter's own 2D output (gripper_reference_filter_->reference_output()), not the measured state, so a zero-valued hardware echo cannot trigger a false success. L272: "This function currently does too much. Could move the filter state step and the mode dependent error calculations into separate helpers" Mode-dependent error masking extracted to compute_convergence_error in gripper_reference_filter_ros_utils.hpp. The filter step is now a single call to gripper_reference_filter_->step(...) following the ownership refactor in the previous commit. L173: fill_reference_goal, fill_reference_msg, and apply_mode_logic updated to work with the 2D reference_output() API. Companion to vortex-msgs branch 584-task-gripper-controller which trims GripperReferenceFilter to roll/pinch only and GripperWaypoint to flat float64 fields (vortexntnu/vortex-gripper#10). --- .../gripper_reference_filter_ros.hpp | 29 +++-- .../gripper_reference_filter_ros_utils.hpp | 55 +++++++-- .../src/gripper_reference_filter_ros.cpp | 108 ++++++++---------- 3 files changed, 113 insertions(+), 79 deletions(-) diff --git a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp index 88ba7a1..88020e6 100644 --- a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp +++ b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp @@ -24,12 +24,17 @@ class GripperReferenceFilterNode : public rclcpp::Node { ~GripperReferenceFilterNode(); private: + // @brief Declare ROS parameters and create the subscription/publisher pair. void set_subscribers_and_publisher(); + // @brief Declare ROS parameters and create the action server. void set_action_server(); + // @brief Declare ROS parameters and construct the reference filter instance. void set_refererence_filter(); + // @brief Latch the measured gripper state for later use as the initial + // reference seed and for mode-masking the goal. void reference_callback( const vortex_msgs::msg::GripperState::SharedPtr state_msg); @@ -52,9 +57,17 @@ class GripperReferenceFilterNode : public rclcpp::Node { const std::shared_ptr> goal_handle); - void publish_hold_reference(); + // @brief Latch the filter's current 2D output as the held reference and + // publish it once. Called whenever a goal ends so that the next + // republish_held_reference_tick has a non-default value to send. + void latch_current_state_as_held_reference(); - void publish_hold_timer(); + // @brief Periodically republish the held reference when no goal is active. + // This keeps downstream consumers receiving the last committed + // reference rather than a default-zero message, which prevented + // the convergence check from spuriously succeeding on a new + // zero-valued goal. + void republish_held_reference_tick(); rclcpp_action::Server< vortex_msgs::action::GripperReferenceFilterWaypoint>::SharedPtr @@ -68,17 +81,15 @@ class GripperReferenceFilterNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr reference_sub_; - std::chrono::milliseconds time_step_{}; + std::chrono::milliseconds time_step_ms_{}; - Eigen::Vector6d filter_state_; - - Eigen::Vector2d reference_; + Eigen::Vector2d measured_reference_{Eigen::Vector2d::Zero()}; std::mutex mutex_; - rclcpp::TimerBase::SharedPtr hold_timer_; - vortex_msgs::msg::GripperReferenceFilter hold_reference_msg_; - bool hold_active_{false}; + rclcpp::TimerBase::SharedPtr held_reference_republish_timer_; + vortex_msgs::msg::GripperReferenceFilter last_published_reference_; + bool holding_reference_{false}; std::atomic preempted_{false}; std::mutex execute_mutex_; diff --git a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp index 96466a1..714e193 100644 --- a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp +++ b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp @@ -7,14 +7,9 @@ namespace vortex::guidance { -inline Eigen::Vector6d fill_reference_state( - const Eigen::Vector2d& current_reference) { - Eigen::Vector6d state_vector = Eigen::Vector6d::Zero(); - state_vector(0) = current_reference(0); - state_vector(1) = current_reference(1); - return state_vector; -} - +// @brief Extract the [roll, pinch] reference from a GripperWaypoint. +// @param waypoint_goal The action goal waypoint. +// @return 2D vector [roll, pinch]. inline Eigen::Vector2d fill_reference_goal( const vortex_msgs::msg::GripperWaypoint& waypoint_goal) { Eigen::Vector2d reference; @@ -22,14 +17,26 @@ inline Eigen::Vector2d fill_reference_goal( return reference; } +// @brief Build the wire-format GripperReferenceFilter message from a 2D +// reference output. Only the position components (roll, pinch) are +// published; higher-order filter state is intentionally not exposed. +// @param reference_output 2D vector [roll, pinch]. +// @return Message ready for publication. inline vortex_msgs::msg::GripperReferenceFilter fill_reference_msg( - const Eigen::Vector6d& state_vector) { + const Eigen::Vector2d& reference_output) { vortex_msgs::msg::GripperReferenceFilter reference_msg; - reference_msg.roll = state_vector(0); - reference_msg.pinch = state_vector(1); + reference_msg.roll = reference_output(0); + reference_msg.pinch = reference_output(1); return reference_msg; } +// @brief Apply mode logic to a requested reference. Axes excluded by the mode +// are pinned to the corresponding component of current_reference so +// that the unaffected axis does not drift. +// @param requested_reference 2D vector [roll, pinch] from the action goal. +// @param current_reference 2D vector [roll, pinch] currently being held. +// @param mode GripperWaypoint mode constant. +// @return The resolved reference after applying the mode mask. inline Eigen::Vector2d apply_mode_logic( const Eigen::Vector2d& requested_reference, const Eigen::Vector2d& current_reference, @@ -48,6 +55,32 @@ inline Eigen::Vector2d apply_mode_logic( return resolved_reference; } +// @brief Compute the convergence error between two references, with the axis +// excluded by the mode masked to zero so it does not contribute to the +// norm-based convergence check. +// @param current_reference 2D vector [roll, pinch] currently produced by the filter. +// @param goal_reference 2D vector [roll, pinch] requested by the goal. +// @param mode GripperWaypoint mode constant. +// @return 2D error vector with masked axes set to zero. +inline Eigen::Vector2d compute_convergence_error( + const Eigen::Vector2d& current_reference, + const Eigen::Vector2d& goal_reference, + uint8_t mode) { + Eigen::Vector2d error = current_reference - goal_reference; + switch (mode) { + case vortex_msgs::msg::GripperWaypoint::ONLY_ROLL: + error(1) = 0.0; + break; + case vortex_msgs::msg::GripperWaypoint::ONLY_PINCH: + error(0) = 0.0; + break; + case vortex_msgs::msg::GripperWaypoint::ROLL_AND_PINCH: + default: + break; + } + return error; +} + } // namespace vortex::guidance #endif // GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_UTILS_HPP_ diff --git a/gripper_guidance/src/gripper_reference_filter_ros.cpp b/gripper_guidance/src/gripper_reference_filter_ros.cpp index ac23c19..1ee7a43 100644 --- a/gripper_guidance/src/gripper_reference_filter_ros.cpp +++ b/gripper_guidance/src/gripper_reference_filter_ros.cpp @@ -20,17 +20,15 @@ namespace vortex::guidance { GripperReferenceFilterNode::GripperReferenceFilterNode(const rclcpp::NodeOptions& options) : Node("gripper_reference_filter_node", options) { - time_step_ = std::chrono::milliseconds(10); - set_subscribers_and_publisher(); set_action_server(); set_refererence_filter(); - hold_timer_ = this->create_wall_timer( - time_step_, - [this]() { publish_hold_timer(); }); + held_reference_republish_timer_ = this->create_wall_timer( + time_step_ms_, + [this]() { republish_held_reference_tick(); }); spdlog::info(start_message); } @@ -76,6 +74,10 @@ void GripperReferenceFilterNode::set_action_server() { } void GripperReferenceFilterNode::set_refererence_filter() { + const int time_step_ms_param = + this->declare_parameter("time_step_ms", 10); + time_step_ms_ = std::chrono::milliseconds(time_step_ms_param); + this->declare_parameter>("zeta"); this->declare_parameter>("omega"); @@ -92,7 +94,7 @@ void GripperReferenceFilterNode::set_refererence_filter() { void GripperReferenceFilterNode::reference_callback( const vortex_msgs::msg::GripperState::SharedPtr state_msg) { std::lock_guard lock(mutex_); - reference_ << state_msg->roll, state_msg->pinch; + measured_reference_ << state_msg->roll, state_msg->pinch; } rclcpp_action::GoalResponse GripperReferenceFilterNode::handle_goal( @@ -101,7 +103,7 @@ rclcpp_action::GoalResponse GripperReferenceFilterNode::handle_goal( /*goal*/) { { std::lock_guard lock(mutex_); - hold_active_ = false; + holding_reference_ = false; } spdlog::info("Accepted goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; @@ -128,33 +130,32 @@ void GripperReferenceFilterNode::handle_accepted( std::thread([this, goal_handle]() { execute(goal_handle); }); } -void GripperReferenceFilterNode::publish_hold_reference() { +void GripperReferenceFilterNode::latch_current_state_as_held_reference() { if (!reference_pub_) { return; } - auto hold_message = std::make_unique(); + auto held_message = std::make_unique( + fill_reference_msg(gripper_reference_filter_->reference_output())); { std::lock_guard lock(mutex_); - hold_message->roll = reference_(0); - hold_message->pinch = reference_(1); - hold_reference_msg_ = *hold_message; - hold_active_ = true; + last_published_reference_ = *held_message; + holding_reference_ = true; } - reference_pub_->publish(std::move(hold_message)); + reference_pub_->publish(std::move(held_message)); } -void GripperReferenceFilterNode::publish_hold_timer() { - auto hold_message = std::make_unique(); +void GripperReferenceFilterNode::republish_held_reference_tick() { + auto held_message = std::make_unique(); { std::lock_guard lock(mutex_); - if (!hold_active_ || !reference_pub_) { + if (!holding_reference_ || !reference_pub_) { return; } - *hold_message = hold_reference_msg_; + *held_message = last_published_reference_; } - reference_pub_->publish(std::move(hold_message)); + reference_pub_->publish(std::move(held_message)); } void GripperReferenceFilterNode::execute( @@ -162,10 +163,17 @@ void GripperReferenceFilterNode::execute( vortex_msgs::action::GripperReferenceFilterWaypoint>> goal_handle) { spdlog::info("Executing goal"); - { + const Eigen::Vector2d filter_seed = [this] { std::lock_guard lock(mutex_); - filter_state_ = fill_reference_state(reference_); - } + if (holding_reference_) { + Eigen::Vector2d seed; + seed << last_published_reference_.roll, + last_published_reference_.pinch; + return seed; + } + return measured_reference_; + }(); + gripper_reference_filter_->reset(filter_seed); const vortex_msgs::msg::GripperWaypoint waypoint_goal = goal_handle->get_goal()->waypoint; @@ -180,21 +188,19 @@ void GripperReferenceFilterNode::execute( "Using default 0.1"); } - Eigen::Vector2d goal_reference; - { - std::lock_guard lock(mutex_); - goal_reference = - apply_mode_logic(fill_reference_goal(waypoint_goal), reference_, mode); - } + const Eigen::Vector2d goal_reference = + apply_mode_logic(fill_reference_goal(waypoint_goal), filter_seed, mode); + + const double time_step_seconds = time_step_ms_.count() / 1000.0; auto result = std::make_shared< vortex_msgs::action::GripperReferenceFilterWaypoint::Result>(); - rclcpp::Rate loop_rate(1000.0 / time_step_.count()); + rclcpp::Rate loop_rate(1000.0 / time_step_ms_.count()); while (rclcpp::ok()) { if (preempted_.load()) { - publish_hold_reference(); + latch_current_state_as_held_reference(); result->success = false; goal_handle->abort(result); spdlog::info("Goal preempted by newer goal"); @@ -202,54 +208,38 @@ void GripperReferenceFilterNode::execute( } if (goal_handle->is_canceling()) { - publish_hold_reference(); + latch_current_state_as_held_reference(); result->success = false; goal_handle->canceled(result); spdlog::info("Goal canceled"); return; } - const Eigen::Vector6d state_derivative = - gripper_reference_filter_->calculate_x_dot(filter_state_, goal_reference); - filter_state_ += state_derivative * time_step_.count() / 1000.0; + gripper_reference_filter_->step(goal_reference, time_step_seconds); + + const Eigen::Vector2d filter_output = gripper_reference_filter_->reference_output(); auto reference_message = std::make_unique( - fill_reference_msg(filter_state_)); + fill_reference_msg(filter_output)); { std::lock_guard lock(mutex_); - hold_reference_msg_ = *reference_message; + last_published_reference_ = *reference_message; } reference_pub_->publish(std::move(reference_message)); - Eigen::Vector2d current_state; - { - std::lock_guard lock(mutex_); - current_state = reference_; - } - - Eigen::Vector2d error = current_state - goal_reference; - switch (mode) { - case vortex_msgs::msg::GripperWaypoint::ONLY_ROLL: - error(1) = 0.0; - break; - case vortex_msgs::msg::GripperWaypoint::ONLY_PINCH: - error(0) = 0.0; - break; - case vortex_msgs::msg::GripperWaypoint::ROLL_AND_PINCH: - default: - break; - } + const Eigen::Vector2d convergence_error = + compute_convergence_error(filter_output, goal_reference, mode); - if (error.norm() < convergence_threshold) { - filter_state_.head(2) = goal_reference; + if (convergence_error.norm() < convergence_threshold) { + gripper_reference_filter_->snap_to(goal_reference); auto final_message = std::make_unique( - fill_reference_msg(filter_state_)); + fill_reference_msg(gripper_reference_filter_->reference_output())); { std::lock_guard lock(mutex_); - hold_reference_msg_ = *final_message; - hold_active_ = true; + last_published_reference_ = *final_message; + holding_reference_ = true; } reference_pub_->publish(std::move(final_message)); result->success = true; From 4dd1980191dd84f9ec63b1aca5045a3b7d69a9c2 Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Tue, 19 May 2026 11:49:17 +0200 Subject: [PATCH 23/24] feat(gripper): add open-loop gripper controller package Addresses standalone PR review comment (jorgenfj, vortexntnu/vortex-gripper#10): "Would be nice to have a simple open loop gripper controller that owns an action server which accepts desired change or rotation, then the controller just publishes this velocity directly for a given amount of time before the action completes. This way we are still able to operate the gripper in the case that feedback is not available" GripperOpenLoopControllerNode accepts a GripperOpenLoopCommand goal (roll_velocity, pinch_velocity, duration_seconds, mode) and publishes GripperStateVelocityCommand at time_step_ms intervals until the duration elapses, then publishes a zero-velocity stop command and succeeds. Supports the same ROLL_AND_PINCH/ONLY_ROLL/ONLY_PINCH mode mask as the closed-loop path. Action type GripperOpenLoopCommand is defined in the companion vortex-msgs branch 584-task-gripper-controller. Package structure mirrors gripper_controller: component library + standalone executable, composable node registration, launch file, and YAML config. Preemption uses the same atomic+thread pattern as reference_filter_dp_quat and the updated gripper_reference_filter. Example goal (pinch only, 3 seconds): ros2 action send_goal /nautilus/gripper/open_loop_controller \ vortex_msgs/action/GripperOpenLoopCommand \ "{roll_velocity: 0.0, pinch_velocity: -0.5, duration_seconds: 3.0, mode: 2}" --- gripper_open_loop_controller/CMakeLists.txt | 77 +++++++ .../gripper_open_loop_controller_params.yaml | 13 ++ .../gripper_open_loop_controller_ros.hpp | 70 ++++++ .../gripper_open_loop_controller.launch.py | 22 ++ gripper_open_loop_controller/package.xml | 27 +++ .../src/gripper_open_loop_controller_node.cpp | 13 ++ .../src/gripper_open_loop_controller_ros.cpp | 207 ++++++++++++++++++ 7 files changed, 429 insertions(+) create mode 100644 gripper_open_loop_controller/CMakeLists.txt create mode 100644 gripper_open_loop_controller/config/gripper_open_loop_controller_params.yaml create mode 100644 gripper_open_loop_controller/include/gripper_open_loop_controller/gripper_open_loop_controller_ros.hpp create mode 100644 gripper_open_loop_controller/launch/gripper_open_loop_controller.launch.py create mode 100644 gripper_open_loop_controller/package.xml create mode 100644 gripper_open_loop_controller/src/gripper_open_loop_controller_node.cpp create mode 100644 gripper_open_loop_controller/src/gripper_open_loop_controller_ros.cpp diff --git a/gripper_open_loop_controller/CMakeLists.txt b/gripper_open_loop_controller/CMakeLists.txt new file mode 100644 index 0000000..c975b6b --- /dev/null +++ b/gripper_open_loop_controller/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.8) +project(gripper_open_loop_controller) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +include_directories(include) + +# --------------------------------------------------------------------------- +# 1. Component Library (Shared) +# --------------------------------------------------------------------------- +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED + src/gripper_open_loop_controller_ros.cpp +) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + rclcpp_action + vortex_msgs + vortex_utils + vortex_utils_ros + spdlog + fmt +) + +# --------------------------------------------------------------------------- +# 2. Component Registration +# --------------------------------------------------------------------------- +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "vortex::controller::GripperOpenLoopControllerNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +# --------------------------------------------------------------------------- +# 3. Standalone Executable +# --------------------------------------------------------------------------- +add_executable(${PROJECT_NAME}_standalone + src/gripper_open_loop_controller_node.cpp +) +target_link_libraries(${PROJECT_NAME}_standalone PUBLIC + ${LIB_NAME} +) +ament_target_dependencies(${PROJECT_NAME}_standalone PUBLIC + rclcpp +) + +ament_export_targets(export_${LIB_NAME}) +install(TARGETS ${LIB_NAME} ${PROJECT_NAME}_standalone + EXPORT export_${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ DESTINATION include) +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}/) + +ament_package() diff --git a/gripper_open_loop_controller/config/gripper_open_loop_controller_params.yaml b/gripper_open_loop_controller/config/gripper_open_loop_controller_params.yaml new file mode 100644 index 0000000..c351a5f --- /dev/null +++ b/gripper_open_loop_controller/config/gripper_open_loop_controller_params.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + # Publish loop period in milliseconds + time_step_ms: 10 + + # Topics + topics: + # Output: velocity command to the gripper interface node + control: "nautilus/gripper/control" + + # Action servers + action_servers: + gripper_open_loop_controller: "nautilus/gripper/open_loop_controller" diff --git a/gripper_open_loop_controller/include/gripper_open_loop_controller/gripper_open_loop_controller_ros.hpp b/gripper_open_loop_controller/include/gripper_open_loop_controller/gripper_open_loop_controller_ros.hpp new file mode 100644 index 0000000..c0e3d95 --- /dev/null +++ b/gripper_open_loop_controller/include/gripper_open_loop_controller/gripper_open_loop_controller_ros.hpp @@ -0,0 +1,70 @@ +#ifndef GRIPPER_OPEN_LOOP_CONTROLLER__GRIPPER_OPEN_LOOP_CONTROLLER_ROS_HPP_ +#define GRIPPER_OPEN_LOOP_CONTROLLER__GRIPPER_OPEN_LOOP_CONTROLLER_ROS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vortex::controller { + +// @brief Open-loop gripper controller. Owns an action server that accepts a +// velocity command for a fixed duration and republishes that command +// on a velocity topic at a fixed rate until the duration elapses. +// Intended as a feedback-free fallback for cases where the gripper +// state is not observable. +class GripperOpenLoopControllerNode : public rclcpp::Node { + public: + explicit GripperOpenLoopControllerNode(const rclcpp::NodeOptions& options); + + ~GripperOpenLoopControllerNode(); + + private: + void set_controller_params(); + + void set_publisher(); + + void set_action_server(); + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr + /*goal*/); + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr> /*goal_handle*/); + + void handle_accepted( + const std::shared_ptr> goal_handle); + + void execute( + const std::shared_ptr> goal_handle); + + // @brief Publish a single zero-velocity command. Called after the goal + // ends so that downstream consumers do not continue actuating. + void publish_zero_velocity(); + + rclcpp_action::Server::SharedPtr + action_server_; + + rclcpp::Publisher::SharedPtr + control_pub_; + + std::chrono::milliseconds time_step_ms_{}; + + std::atomic preempted_{false}; + std::mutex execute_mutex_; + std::thread execute_thread_; +}; + +} // namespace vortex::controller + +#endif // GRIPPER_OPEN_LOOP_CONTROLLER__GRIPPER_OPEN_LOOP_CONTROLLER_ROS_HPP_ diff --git a/gripper_open_loop_controller/launch/gripper_open_loop_controller.launch.py b/gripper_open_loop_controller/launch/gripper_open_loop_controller.launch.py new file mode 100644 index 0000000..bc2fb4c --- /dev/null +++ b/gripper_open_loop_controller/launch/gripper_open_loop_controller.launch.py @@ -0,0 +1,22 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + config_file_path = os.path.join( + get_package_share_directory("gripper_open_loop_controller"), + "config", + "gripper_open_loop_controller_params.yaml", + ) + + gripper_open_loop_controller_node = Node( + package="gripper_open_loop_controller", + executable="gripper_open_loop_controller_standalone", + name="gripper_open_loop_controller_node", + parameters=[config_file_path], + output="screen", + ) + + return LaunchDescription([gripper_open_loop_controller_node]) diff --git a/gripper_open_loop_controller/package.xml b/gripper_open_loop_controller/package.xml new file mode 100644 index 0000000..cb4c5e2 --- /dev/null +++ b/gripper_open_loop_controller/package.xml @@ -0,0 +1,27 @@ + + + + gripper_open_loop_controller + 0.0.0 + Open-loop gripper controller: executes a velocity command for a fixed duration via an action server. Intended as a feedback-free fallback for cases where measured gripper state is unavailable. + patricsh + MIT + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_components + vortex_msgs + vortex_utils + vortex_utils_ros + + spdlog + fmt + + ament_cmake_gtest + + + ament_cmake + + diff --git a/gripper_open_loop_controller/src/gripper_open_loop_controller_node.cpp b/gripper_open_loop_controller/src/gripper_open_loop_controller_node.cpp new file mode 100644 index 0000000..2b665fa --- /dev/null +++ b/gripper_open_loop_controller/src/gripper_open_loop_controller_node.cpp @@ -0,0 +1,13 @@ +#include +#include "gripper_open_loop_controller/gripper_open_loop_controller_ros.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + + auto node = std::make_shared( + rclcpp::NodeOptions()); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/gripper_open_loop_controller/src/gripper_open_loop_controller_ros.cpp b/gripper_open_loop_controller/src/gripper_open_loop_controller_ros.cpp new file mode 100644 index 0000000..ccc68c4 --- /dev/null +++ b/gripper_open_loop_controller/src/gripper_open_loop_controller_ros.cpp @@ -0,0 +1,207 @@ +#include "gripper_open_loop_controller/gripper_open_loop_controller_ros.hpp" + +#include +#include +#include +#include +#include + +const auto start_message = R"( + + ____ _ ___ _ ____ _ _ _ +/ ___|_ __ (_)_ __ _ __ ___ _ __ / _ \ _ __ ___ _ __ | | ___ ___ _ __ / ___|___ _ __ | |_ _ __ ___ | | | ___ _ __ +| | _| '_ \| | '_ \| '_ \ / _ \ '__|| | | | '_ \ / _ \ '_ \| | / _ \ / _ \| '_ \ | | / _ \| '_ \| __| '__/ _ \| | |/ _ \ '__| +| |_| | |_) | | |_) | |_) | __/ | | |_| | |_) | __/ | | | |__| (_) | (_) | |_) | | |__| (_) | | | | |_| | | (_) | | | __/ | + \____| .__/|_| .__/| .__/ \___|_| \___/| .__/ \___|_| |_|_____\___/ \___/| .__/ \____\___/|_| |_|\__|_| \___/|_|_|\___|_| + |_| |_| |_| |_| |_| + +)"; + +namespace vortex::controller { + +GripperOpenLoopControllerNode::GripperOpenLoopControllerNode( + const rclcpp::NodeOptions& options) +: Node("gripper_open_loop_controller_node", options) { + set_controller_params(); + + set_publisher(); + + set_action_server(); + + spdlog::info(start_message); +} + +GripperOpenLoopControllerNode::~GripperOpenLoopControllerNode() { + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } +} + +void GripperOpenLoopControllerNode::set_controller_params() { + const int time_step_ms_param = + this->declare_parameter("time_step_ms", 10); + time_step_ms_ = std::chrono::milliseconds(time_step_ms_param); + + spdlog::info("GripperOpenLoopController: dt={}ms", time_step_ms_param); +} + +void GripperOpenLoopControllerNode::set_publisher() { + const std::string control_topic = + this->declare_parameter("topics.control"); + + const auto qos_sensor_data = + vortex::utils::qos_profiles::sensor_data_profile(1); + + control_pub_ = + this->create_publisher( + control_topic, qos_sensor_data); +} + +void GripperOpenLoopControllerNode::set_action_server() { + const std::string action_server_name = + this->declare_parameter( + "action_servers.gripper_open_loop_controller"); + + action_server_ = rclcpp_action::create_server< + vortex_msgs::action::GripperOpenLoopCommand>( + this, action_server_name, + [this](const auto& uuid, auto goal) { + return handle_goal(uuid, std::move(goal)); + }, + [this](auto goal_handle) { return handle_cancel(goal_handle); }, + [this](auto goal_handle) { handle_accepted(goal_handle); }); +} + +rclcpp_action::GoalResponse GripperOpenLoopControllerNode::handle_goal( + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr + /*goal*/) { + spdlog::info("Accepted open-loop goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse GripperOpenLoopControllerNode::handle_cancel( + const std::shared_ptr> /*goal_handle*/) { + spdlog::info("Received request to cancel open-loop goal"); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void GripperOpenLoopControllerNode::handle_accepted( + const std::shared_ptr> goal_handle) { + std::lock_guard lock(execute_mutex_); + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } + preempted_ = false; + + execute_thread_ = + std::thread([this, goal_handle]() { execute(goal_handle); }); +} + +void GripperOpenLoopControllerNode::publish_zero_velocity() { + if (!control_pub_) { + return; + } + auto stop_message = + std::make_unique(); + stop_message->header.stamp = this->now(); + stop_message->roll_velocity = 0.0; + stop_message->pinch_velocity = 0.0; + control_pub_->publish(std::move(stop_message)); +} + +void GripperOpenLoopControllerNode::execute( + const std::shared_ptr> goal_handle) { + const auto goal = goal_handle->get_goal(); + + double requested_duration_seconds = goal->duration_seconds; + if (requested_duration_seconds <= 0.0) { + spdlog::warn( + "GripperOpenLoopController: Invalid duration_seconds received " + "(<= 0). Aborting goal."); + auto failure_result = + std::make_shared(); + failure_result->success = false; + goal_handle->abort(failure_result); + return; + } + + using GoalT = vortex_msgs::action::GripperOpenLoopCommand::Goal; + const double requested_roll_velocity = + (goal->mode == GoalT::ONLY_PINCH) ? 0.0 : goal->roll_velocity; + const double requested_pinch_velocity = + (goal->mode == GoalT::ONLY_ROLL) ? 0.0 : goal->pinch_velocity; + + spdlog::info( + "GripperOpenLoopController: roll_vel={:.3f} pinch_vel={:.3f} " + "duration={:.3f}s mode={}", + requested_roll_velocity, requested_pinch_velocity, + requested_duration_seconds, goal->mode); + + auto feedback = + std::make_shared(); + auto result = + std::make_shared(); + + rclcpp::Rate loop_rate(1000.0 / time_step_ms_.count()); + const auto start_time = this->now(); + + while (rclcpp::ok()) { + const double elapsed_seconds = + (this->now() - start_time).seconds(); + + if (preempted_.load()) { + publish_zero_velocity(); + result->success = false; + goal_handle->abort(result); + spdlog::info("Open-loop goal preempted by newer goal"); + return; + } + + if (goal_handle->is_canceling()) { + publish_zero_velocity(); + result->success = false; + goal_handle->canceled(result); + spdlog::info("Open-loop goal canceled"); + return; + } + + if (elapsed_seconds >= requested_duration_seconds) { + publish_zero_velocity(); + result->success = true; + goal_handle->succeed(result); + spdlog::info("Open-loop goal duration reached"); + return; + } + + auto velocity_command_msg = + std::make_unique(); + velocity_command_msg->header.stamp = this->now(); + velocity_command_msg->roll_velocity = requested_roll_velocity; + velocity_command_msg->pinch_velocity = requested_pinch_velocity; + control_pub_->publish(std::move(velocity_command_msg)); + + feedback->elapsed_seconds = elapsed_seconds; + goal_handle->publish_feedback(feedback); + + loop_rate.sleep(); + } + + if (!rclcpp::ok() && goal_handle->is_active()) { + result->success = false; + try { + goal_handle->abort(result); + } catch (...) { + // Ignore exceptions during shutdown + } + } +} + +} // namespace vortex::controller + +RCLCPP_COMPONENTS_REGISTER_NODE(vortex::controller::GripperOpenLoopControllerNode) From 685696264eb09fe8b0eb374881e4c6d866056643 Mon Sep 17 00:00:00 2001 From: Patrick Sheehan Date: Tue, 19 May 2026 11:53:23 +0200 Subject: [PATCH 24/24] gripper: fix short variable names per Code Complete ch. 6/11 gripper_controller.cpp: renamed error -> position_difference inside the IIFE that builds position_error. Single-letter and abbreviation-style names do not communicate intent (CC ch. 11: names should describe what the variable represents). gripper_controller_ros.cpp: renamed matrix -> gain_matrix inside the proportional gain IIFE; renamed measured/reference -> measured_gripper_state/reference_gripper_state inside the publish_control IIFE; renamed lambda params msg -> reference_filter_msg and gripper_state_msg to match the callback method signatures. gripper_controller_ros.hpp: updated callback declarations to match. gripper_reference_filter_ros_utils.hpp: renamed error -> convergence_error inside compute_convergence_error so the local variable name matches the function's documented purpose and does not shadow the semantic meaning of the return value. --- .../gripper_controller_ros.hpp | 4 +- gripper_controller/src/gripper_controller.cpp | 8 ++-- .../src/gripper_controller_ros.cpp | 42 +++++++++---------- .../gripper_reference_filter_ros_utils.hpp | 8 ++-- 4 files changed, 31 insertions(+), 31 deletions(-) diff --git a/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp b/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp index a39a7b8..ae1be93 100644 --- a/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp +++ b/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp @@ -28,10 +28,10 @@ class GripperControllerNode : public rclcpp::Node { private: void reference_callback( - const vortex_msgs::msg::GripperReferenceFilter::SharedPtr reference_msg); + const vortex_msgs::msg::GripperReferenceFilter::SharedPtr reference_filter_msg); void state_callback( - const vortex_msgs::msg::GripperState::SharedPtr state_msg); + const vortex_msgs::msg::GripperState::SharedPtr gripper_state_msg); void publish_control(); diff --git a/gripper_controller/src/gripper_controller.cpp b/gripper_controller/src/gripper_controller.cpp index 8f1a3ba..203f149 100644 --- a/gripper_controller/src/gripper_controller.cpp +++ b/gripper_controller/src/gripper_controller.cpp @@ -10,10 +10,10 @@ types::Vector2d GripperController::calculate_velocity( const types::GripperState& reference_state) { const types::Vector2d position_error = [&] { - types::Vector2d error; - error(0) = reference_state.roll - measured_state.roll; - error(1) = reference_state.pinch - measured_state.pinch; - return error; + types::Vector2d position_difference; + position_difference(0) = reference_state.roll - measured_state.roll; + position_difference(1) = reference_state.pinch - measured_state.pinch; + return position_difference; }(); types::Vector2d velocity_command = Kp_ * position_error; diff --git a/gripper_controller/src/gripper_controller_ros.cpp b/gripper_controller/src/gripper_controller_ros.cpp index 34aed9c..3936f5b 100644 --- a/gripper_controller/src/gripper_controller_ros.cpp +++ b/gripper_controller/src/gripper_controller_ros.cpp @@ -39,10 +39,10 @@ void GripperControllerNode::set_controller_params() { this->declare_parameter("kp.pinch", 1.0); const types::Matrix2d proportional_gain_matrix = [&] { - types::Matrix2d matrix = types::Matrix2d::Zero(); - matrix(0, 0) = kp_roll; - matrix(1, 1) = kp_pinch; - return matrix; + types::Matrix2d gain_matrix = types::Matrix2d::Zero(); + gain_matrix(0, 0) = kp_roll; + gain_matrix(1, 1) = kp_pinch; + return gain_matrix; }(); controller_.set_kp(proportional_gain_matrix); @@ -65,15 +65,15 @@ void GripperControllerNode::set_subscribers_and_publisher() { reference_sub_ = this->create_subscription( reference_topic, qos_sensor_data, - [this](const vortex_msgs::msg::GripperReferenceFilter::SharedPtr msg) { - reference_callback(msg); + [this](const vortex_msgs::msg::GripperReferenceFilter::SharedPtr reference_filter_msg) { + reference_callback(reference_filter_msg); }); state_sub_ = this->create_subscription( state_topic, qos_sensor_data, - [this](const vortex_msgs::msg::GripperState::SharedPtr msg) { - state_callback(msg); + [this](const vortex_msgs::msg::GripperState::SharedPtr gripper_state_msg) { + state_callback(gripper_state_msg); }); control_pub_ = @@ -88,29 +88,29 @@ void GripperControllerNode::set_subscribers_and_publisher() { } void GripperControllerNode::reference_callback( - const vortex_msgs::msg::GripperReferenceFilter::SharedPtr reference_msg) { + const vortex_msgs::msg::GripperReferenceFilter::SharedPtr reference_filter_msg) { std::lock_guard lock(state_mutex_); - roll_ref_ = reference_msg->roll; - pinch_ref_ = reference_msg->pinch; + roll_ref_ = reference_filter_msg->roll; + pinch_ref_ = reference_filter_msg->pinch; } void GripperControllerNode::state_callback( - const vortex_msgs::msg::GripperState::SharedPtr state_msg) { + const vortex_msgs::msg::GripperState::SharedPtr gripper_state_msg) { std::lock_guard lock(state_mutex_); - roll_measured_ = state_msg->roll; - pinch_measured_ = state_msg->pinch; + roll_measured_ = gripper_state_msg->roll; + pinch_measured_ = gripper_state_msg->pinch; } void GripperControllerNode::publish_control() { const auto [measured_state, reference_state] = [this] { std::lock_guard lock(state_mutex_); - types::GripperState measured; - types::GripperState reference; - measured.roll = roll_measured_; - measured.pinch = pinch_measured_; - reference.roll = roll_ref_; - reference.pinch = pinch_ref_; - return std::pair{measured, reference}; + types::GripperState measured_gripper_state; + types::GripperState reference_gripper_state; + measured_gripper_state.roll = roll_measured_; + measured_gripper_state.pinch = pinch_measured_; + reference_gripper_state.roll = roll_ref_; + reference_gripper_state.pinch = pinch_ref_; + return std::pair{measured_gripper_state, reference_gripper_state}; }(); const types::Vector2d velocity_command = diff --git a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp index 714e193..9314c34 100644 --- a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp +++ b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp @@ -66,19 +66,19 @@ inline Eigen::Vector2d compute_convergence_error( const Eigen::Vector2d& current_reference, const Eigen::Vector2d& goal_reference, uint8_t mode) { - Eigen::Vector2d error = current_reference - goal_reference; + Eigen::Vector2d convergence_error = current_reference - goal_reference; switch (mode) { case vortex_msgs::msg::GripperWaypoint::ONLY_ROLL: - error(1) = 0.0; + convergence_error(1) = 0.0; break; case vortex_msgs::msg::GripperWaypoint::ONLY_PINCH: - error(0) = 0.0; + convergence_error(0) = 0.0; break; case vortex_msgs::msg::GripperWaypoint::ROLL_AND_PINCH: default: break; } - return error; + return convergence_error; } } // namespace vortex::guidance