From a984aa1fe4b8db1e883f17a0d9be138501f4bbe6 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Fri, 2 Feb 2024 12:09:08 -0800 Subject: [PATCH 1/5] change driver to work with binary thrust message --- ff_drivers/CMakeLists.txt | 18 ++--- ff_drivers/include/ff_drivers/gpio.hpp | 51 +++++++++++++ ff_drivers/src/gpio.cpp | 87 ++++++++++++++++++++++ ff_drivers/src/thruster_node.cpp | 50 +++++-------- ff_drivers/src/thruster_pwm_node.cpp | 99 ++++++++++++++++++++++++++ 5 files changed, 265 insertions(+), 40 deletions(-) create mode 100644 ff_drivers/include/ff_drivers/gpio.hpp create mode 100644 ff_drivers/src/gpio.cpp create mode 100644 ff_drivers/src/thruster_pwm_node.cpp diff --git a/ff_drivers/CMakeLists.txt b/ff_drivers/CMakeLists.txt index ec08039..40dd586 100644 --- a/ff_drivers/CMakeLists.txt +++ b/ff_drivers/CMakeLists.txt @@ -12,21 +12,21 @@ find_package(rclcpp REQUIRED) find_package(ff_msgs REQUIRED) find_package(ff_control REQUIRED) -add_library(pwm src/pwm.cpp) -target_include_directories(pwm PUBLIC +add_library(driver_lib src/pwm.cpp src/gpio.cpp) +target_include_directories(driver_lib PUBLIC $ $) -target_compile_features(pwm PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -ament_target_dependencies(pwm rclcpp) +target_compile_features(driver_lib PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies(driver_lib rclcpp) -# nodes (TODO: needs to be updated) -# add_executable(thruster_node src/thruster_node.cpp) -# target_link_libraries(thruster_node pwm) -# ament_target_dependencies(thruster_node ff_msgs) +add_executable(thruster_node src/thruster_node.cpp) +target_link_libraries(thruster_node driver_lib) +ament_target_dependencies(thruster_node ff_msgs) # tests +# TODO(alvin): remove pwm stuff add_executable(test_single src/tests/test_single.cpp) -target_link_libraries(test_single pwm) +target_link_libraries(test_single driver_lib) add_executable(test_all_thrusters src/tests/test_all_thrusters.cpp) ament_target_dependencies(test_all_thrusters rclcpp ff_msgs ff_control) diff --git a/ff_drivers/include/ff_drivers/gpio.hpp b/ff_drivers/include/ff_drivers/gpio.hpp new file mode 100644 index 0000000..1330ee3 --- /dev/null +++ b/ff_drivers/include/ff_drivers/gpio.hpp @@ -0,0 +1,51 @@ +// MIT License +// +// Copyright (c) 2024 Stanford Autonomous Systems Lab +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + + +#pragma once + +#include +#include + +namespace ff +{ + +class GPIO +{ +public: + explicit GPIO(int pin); + ~GPIO(); + + void SetState(bool state); + void SetPolarity(bool normal = true); + bool GetState() const; + +private: + int pin_; + bool state_; + std::ofstream f_value_; + std::ofstream f_active_low_; + + std::string BasePath() const; +}; + +} // namespace ff diff --git a/ff_drivers/src/gpio.cpp b/ff_drivers/src/gpio.cpp new file mode 100644 index 0000000..367b839 --- /dev/null +++ b/ff_drivers/src/gpio.cpp @@ -0,0 +1,87 @@ +// MIT License +// +// Copyright (c) 2024 Stanford Autonomous Systems Lab +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + + +#include "ff_drivers/gpio.hpp" + +namespace ff +{ + +GPIO::GPIO(int pin) +: pin_(pin) +{ + // request gpio device + { + std::ofstream f_export("/sys/class/gpio/export"); + f_export << pin; + } + + // configure GPIO to be an output + { + std::ofstream f_direction(BasePath() + "direction"); + f_direction << "out"; + } + + f_value_.open(BasePath() + "value"); + f_active_low_.open(BasePath() + "active_low"); + + SetPolarity(true); + SetState(false); +} + +GPIO::~GPIO() +{ + SetState(false); + + // close files + f_value_.close(); + f_active_low_.close(); + + // free gpio device + std::ofstream f_unexport("/sys/class/gpio/unexport"); + f_unexport << pin_; +} + +void GPIO::SetState(bool state) +{ + f_value_ << static_cast(state); + f_value_.flush(); + state_ = state; +} + +bool GPIO::GetState() const +{ + return state_; +} + +void GPIO::SetPolarity(bool normal) +{ + f_active_low_ << static_cast(!normal); + f_active_low_.flush(); +} + +std::string GPIO::BasePath() const +{ + return "/sys/class/gpio/gpio" + std::to_string(pin_) + "/"; +} + +} // namespace ff diff --git a/ff_drivers/src/thruster_node.cpp b/ff_drivers/src/thruster_node.cpp index 8e9fe7e..28e7815 100644 --- a/ff_drivers/src/thruster_node.cpp +++ b/ff_drivers/src/thruster_node.cpp @@ -1,6 +1,6 @@ // MIT License // -// Copyright (c) 2023 Stanford Autonomous Systems Lab +// Copyright (c) 2024 Stanford Autonomous Systems Lab // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -21,13 +21,12 @@ // SOFTWARE. -#include #include - +#include #include -#include "ff_drivers/pwm.hpp" -#include "ff_msgs/msg/thruster_pwm_command.hpp" +#include "ff_drivers/gpio.hpp" +#include "ff_msgs/msg/thruster_command.hpp" #define NUM_THRUSTERS 8 @@ -45,47 +44,36 @@ static constexpr int THRUSTER_PINS[NUM_THRUSTERS] = { }; -using namespace std::chrono_literals; -using ff_msgs::msg::ThrusterPWMCommand; +using ff_msgs::msg::ThrusterCommand; -class ThrusterNode : public ff::PWMManager +class ThrusterNode : public rclcpp::Node { public: ThrusterNode() - : ff::PWMManager("thruster_driver_node") + : rclcpp::Node("thruster_node") { - // add all PWMs + // initialize all GPIO for (size_t i = 0; i < NUM_THRUSTERS; ++i) { - this->AddSoftPWM(THRUSTER_PINS[i]); + gpios_.push_back(std::make_unique(THRUSTER_PINS[i])); } - // set period (default to 10Hz) - double period = this->declare_parameter("period", .1); - this->SetPeriodAll(period * 1s); - // update period on the fly - sub_params_ = std::make_shared(this); - cb_period_ = sub_params_->add_parameter_callback( - "period", - [this](const rclcpp::Parameter & p) {SetPeriodAll(p.as_double() * 1s);}); - - // start all PWMs - this->EnableAll(); - // listen to commands - sub_duty_cycle_ = this->create_subscription( - "commands/duty_cycle", - 10, [this](const ThrusterPWMCommand::SharedPtr msg) {DutyCycleCallback(msg);}); + sub_thruster_ = this->create_subscription( + "commands/binary_thruster", + 10, + [this](const ThrusterCommand::SharedPtr msg) {ThrusterCommandCallback(msg);}); } private: - std::shared_ptr sub_params_; - std::shared_ptr cb_period_; - rclcpp::Subscription::SharedPtr sub_duty_cycle_; + std::vector> gpios_; + rclcpp::Subscription::SharedPtr sub_thruster_; - void DutyCycleCallback(const ThrusterPWMCommand::SharedPtr msg) + void ThrusterCommandCallback(const ThrusterCommand::SharedPtr msg) { for (size_t i = 0; i < NUM_THRUSTERS; ++i) { - this->SetDutyCycle(i, msg->duty_cycles[i]); + if (gpios_[i]->GetState() != msg->switches[i]) { + gpios_[i]->SetState(msg->switches[i]); + } } } }; diff --git a/ff_drivers/src/thruster_pwm_node.cpp b/ff_drivers/src/thruster_pwm_node.cpp new file mode 100644 index 0000000..8e9fe7e --- /dev/null +++ b/ff_drivers/src/thruster_pwm_node.cpp @@ -0,0 +1,99 @@ +// MIT License +// +// Copyright (c) 2023 Stanford Autonomous Systems Lab +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + + +#include +#include + +#include + +#include "ff_drivers/pwm.hpp" +#include "ff_msgs/msg/thruster_pwm_command.hpp" + +#define NUM_THRUSTERS 8 + +// thruster pin connection +// @see: https://wiki.odroid.com/odroid-n2l/application_note/gpio/pwm#tab__odroid-n2 +static constexpr int THRUSTER_PINS[NUM_THRUSTERS] = { + 476, // thruster pin 1 -> odroid pin 16 + 477, // thruster pin 2 -> odroid pin 18 + 484, // thruster pin 3 -> odroid pin 19 + 485, // thruster pin 4 -> odroid pin 21 + 478, // thruster pin 5 -> odroid pin 22 + 487, // thruster pin 6 -> odroid pin 23 + 486, // thruster pin 7 -> odroid pin 24 + 464, // thruster pin 8 -> odroid pin 26 +}; + + +using namespace std::chrono_literals; +using ff_msgs::msg::ThrusterPWMCommand; + +class ThrusterNode : public ff::PWMManager +{ +public: + ThrusterNode() + : ff::PWMManager("thruster_driver_node") + { + // add all PWMs + for (size_t i = 0; i < NUM_THRUSTERS; ++i) { + this->AddSoftPWM(THRUSTER_PINS[i]); + } + + // set period (default to 10Hz) + double period = this->declare_parameter("period", .1); + this->SetPeriodAll(period * 1s); + // update period on the fly + sub_params_ = std::make_shared(this); + cb_period_ = sub_params_->add_parameter_callback( + "period", + [this](const rclcpp::Parameter & p) {SetPeriodAll(p.as_double() * 1s);}); + + // start all PWMs + this->EnableAll(); + + // listen to commands + sub_duty_cycle_ = this->create_subscription( + "commands/duty_cycle", + 10, [this](const ThrusterPWMCommand::SharedPtr msg) {DutyCycleCallback(msg);}); + } + +private: + std::shared_ptr sub_params_; + std::shared_ptr cb_period_; + rclcpp::Subscription::SharedPtr sub_duty_cycle_; + + void DutyCycleCallback(const ThrusterPWMCommand::SharedPtr msg) + { + for (size_t i = 0; i < NUM_THRUSTERS; ++i) { + this->SetDutyCycle(i, msg->duty_cycles[i]); + } + } +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From b46e1d3bfb59d698fbc0c0495f20bc33e1227e01 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Fri, 2 Feb 2024 13:07:19 -0800 Subject: [PATCH 2/5] fix python format --- ff_control/ff_control/wrench_ctrl.py | 2 +- ff_sim/ff_sim/simulator_node.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ff_control/ff_control/wrench_ctrl.py b/ff_control/ff_control/wrench_ctrl.py index 55c4143..60024da 100644 --- a/ff_control/ff_control/wrench_ctrl.py +++ b/ff_control/ff_control/wrench_ctrl.py @@ -93,7 +93,7 @@ def set_world_wrench(self, wrench_world: Wrench2D, theta: float) -> None: def clip_wrench(self, wrench: Wrench2D) -> Wrench2D: wrench_clipped = Wrench2D() - force = np.sqrt(wrench.fx**2 + wrench.fy**2) + force = np.sqrt(wrench.fx ** 2 + wrench.fy ** 2) force_scale = max(force / self.p.actuators["F_body_max"], 1.0) torque_scale = max(abs(wrench.tz) / self.p.actuators["M_body_max"], 1.0) diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index 7503257..fa58a11 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -375,7 +375,7 @@ def f_dynamics_continuous_time(self, x, u): f[2] = thetadot thetaddot = (M - F[1] * p0[0] + F[0] * p0[1]) / J f[3:5] = np.matmul( - R, (F / m - (thetaddot * np.array([-p0[1], p0[0]]) - thetadot**2 * p0)) + R, (F / m - (thetaddot * np.array([-p0[1], p0[0]]) - thetadot ** 2 * p0)) ) f[5] = thetaddot From 237bafb400a2aa3585ecb8b8a2bc265b7be1bdb4 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Fri, 2 Feb 2024 13:14:17 -0800 Subject: [PATCH 3/5] fix python format --- ff_control/ff_control/wrench_ctrl.py | 2 +- ff_sim/ff_sim/simulator_node.py | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/ff_control/ff_control/wrench_ctrl.py b/ff_control/ff_control/wrench_ctrl.py index 60024da..55c4143 100644 --- a/ff_control/ff_control/wrench_ctrl.py +++ b/ff_control/ff_control/wrench_ctrl.py @@ -93,7 +93,7 @@ def set_world_wrench(self, wrench_world: Wrench2D, theta: float) -> None: def clip_wrench(self, wrench: Wrench2D) -> Wrench2D: wrench_clipped = Wrench2D() - force = np.sqrt(wrench.fx ** 2 + wrench.fy ** 2) + force = np.sqrt(wrench.fx**2 + wrench.fy**2) force_scale = max(force / self.p.actuators["F_body_max"], 1.0) torque_scale = max(abs(wrench.tz) / self.p.actuators["M_body_max"], 1.0) diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index fa58a11..018e5c7 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -374,9 +374,7 @@ def f_dynamics_continuous_time(self, x, u): f[0:2] = v f[2] = thetadot thetaddot = (M - F[1] * p0[0] + F[0] * p0[1]) / J - f[3:5] = np.matmul( - R, (F / m - (thetaddot * np.array([-p0[1], p0[0]]) - thetadot ** 2 * p0)) - ) + f[3:5] = np.matmul(R, (F / m - (thetaddot * np.array([-p0[1], p0[0]]) - thetadot**2 * p0))) f[5] = thetaddot # add constant force due to table tilt From 589430e6b806d61bd4aecb84260925a6759bbaa7 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Fri, 2 Feb 2024 16:12:08 -0800 Subject: [PATCH 4/5] fix typo --- ff_drivers/launch/hardware_bringup.launch.py | 2 +- ff_drivers/src/thruster_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ff_drivers/launch/hardware_bringup.launch.py b/ff_drivers/launch/hardware_bringup.launch.py index df549b8..f30f09f 100644 --- a/ff_drivers/launch/hardware_bringup.launch.py +++ b/ff_drivers/launch/hardware_bringup.launch.py @@ -53,7 +53,7 @@ def generate_launch_description(): # ASL optitrack IP and port parameters=[ { - "server": "192.168.1.8", + "server": "192.168.1.2", "port": 3883, } ], diff --git a/ff_drivers/src/thruster_node.cpp b/ff_drivers/src/thruster_node.cpp index 28e7815..7a1e9a3 100644 --- a/ff_drivers/src/thruster_node.cpp +++ b/ff_drivers/src/thruster_node.cpp @@ -59,7 +59,7 @@ class ThrusterNode : public rclcpp::Node // listen to commands sub_thruster_ = this->create_subscription( - "commands/binary_thruster", + "commands/binary_thrust", 10, [this](const ThrusterCommand::SharedPtr msg) {ThrusterCommandCallback(msg);}); } From fb468aa0de1341bf6d7b6772f8850e606825f1db Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Mon, 5 Feb 2024 14:11:59 -0800 Subject: [PATCH 5/5] install thruster node back --- ff_drivers/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ff_drivers/CMakeLists.txt b/ff_drivers/CMakeLists.txt index 40dd586..433c296 100644 --- a/ff_drivers/CMakeLists.txt +++ b/ff_drivers/CMakeLists.txt @@ -32,7 +32,7 @@ add_executable(test_all_thrusters src/tests/test_all_thrusters.cpp) ament_target_dependencies(test_all_thrusters rclcpp ff_msgs ff_control) # install nodes -install(TARGETS test_single test_all_thrusters +install(TARGETS thruster_node test_single test_all_thrusters DESTINATION lib/${PROJECT_NAME}) # install launch files