diff --git a/ff_control/CMakeLists.txt b/ff_control/CMakeLists.txt index 3f9408a..0213d44 100644 --- a/ff_control/CMakeLists.txt +++ b/ff_control/CMakeLists.txt @@ -16,7 +16,13 @@ find_package(rclcpp REQUIRED) find_package(ff_msgs REQUIRED) find_package(ff_params REQUIRED) -add_library(ctrl_lib src/ll_ctrl.cpp src/wrench_ctrl.cpp src/keyboard_ctrl.cpp src/linear_ctrl.cpp) +add_library(ctrl_lib + src/ll_ctrl.cpp + src/pwm_ctrl.cpp + src/wrench_ctrl.cpp + src/keyboard_ctrl.cpp + src/linear_ctrl.cpp +) target_include_directories(ctrl_lib PUBLIC $ $) @@ -42,6 +48,9 @@ install( INCLUDES DESTINATION include ) +add_executable(pwm_ctrl_cpp_node src/nodes/pwm_ctrl_node.cpp) +target_link_libraries(pwm_ctrl_cpp_node ctrl_lib) + add_executable(wrench_ctrl_cpp_node src/nodes/wrench_ctrl_node.cpp) target_link_libraries(wrench_ctrl_cpp_node ctrl_lib) @@ -53,7 +62,7 @@ add_executable(key_teleop_cpp_node src/nodes/key_teleop_node.cpp) target_link_libraries(key_teleop_cpp_node ctrl_lib) # install CPP nodes -install(TARGETS wrench_ctrl_cpp_node pd_ctrl_cpp_node key_teleop_cpp_node +install(TARGETS pwm_ctrl_cpp_node wrench_ctrl_cpp_node pd_ctrl_cpp_node key_teleop_cpp_node DESTINATION lib/${PROJECT_NAME}) # install Python libraries diff --git a/ff_control/ff_control/ll_ctrl.py b/ff_control/ff_control/ll_ctrl.py index 5cfabfa..0700f42 100644 --- a/ff_control/ff_control/ll_ctrl.py +++ b/ff_control/ff_control/ll_ctrl.py @@ -20,12 +20,12 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. -from ff_msgs.msg import ThrusterCommand -from ff_msgs.msg import WheelVelCommand -from ff_params import RobotParams - import numpy as np +import typing as T +from ff_msgs.msg import ThrusterPWMCommand, ThrusterCommand +from ff_msgs.msg import WheelVelCommand +from ff_params import RobotParams from rclpy.node import Node @@ -36,8 +36,10 @@ def __init__(self, node_name: str = "ll_ctrl_node") -> None: # robot parameters that can be accessed by sub-classes self.p = RobotParams(self) - # low level control publishers - self._thruster_pub = self.create_publisher(ThrusterCommand, "ctrl/duty_cycle", 10) + # low level thruster control publishers + self._thruster_binary_pub = self.create_publisher(ThrusterCommand, "ctrl/binary_thrust", 10) + self._thruster_pwm_pub = self.create_publisher(ThrusterPWMCommand, "ctrl/pwm_thrust", 10) + self._wheel_pub = self.create_publisher(WheelVelCommand, "ctrl/velocity", 10) def set_thrust_duty_cycle(self, duty_cycle: np.ndarray) -> None: @@ -45,14 +47,30 @@ def set_thrust_duty_cycle(self, duty_cycle: np.ndarray) -> None: Send command to set the thrusters duty cycles. :param duty_cycle: duty cycle for each thrust (in [0, 1]) + + Note: this function works only when pwm_ctrl_cpp_node is running + """ + if len(duty_cycle) != len(ThrusterPWMCommand().duty_cycles): + self.get_logger().error("Incompatible thruster length sent.") + return + msg = ThrusterPWMCommand() + msg.header.stamp = self.get_clock().now().to_msg() + msg.duty_cycles = duty_cycle + self._thruster_pwm_pub.publish(msg) + + def set_thrust_binary(self, switches: T.Sequence[bool]) -> None: + """ + Send command to set the thrusters binary output. + + :param switches: binary switch for each thrust """ - if len(duty_cycle) != len(ThrusterCommand().duty_cycle): + if len(switches) != len(ThrusterCommand().switches): self.get_logger().error("Incompatible thruster length sent.") return msg = ThrusterCommand() msg.header.stamp = self.get_clock().now().to_msg() - msg.duty_cycle = duty_cycle - self._thruster_pub.publish(msg) + msg.switches = switches + self._thruster_binary_pub.publish(msg) def set_wheel_velocity(self, velocity: float) -> None: """ diff --git a/ff_control/include/ff_control/ll_ctrl.hpp b/ff_control/include/ff_control/ll_ctrl.hpp index 68acd80..b4ffbaf 100644 --- a/ff_control/include/ff_control/ll_ctrl.hpp +++ b/ff_control/include/ff_control/ll_ctrl.hpp @@ -24,7 +24,6 @@ #pragma once #include -#include #include @@ -47,11 +46,26 @@ class LowLevelController : virtual public rclcpp::Node const ff::RobotParams p_; /** - * @brief send command to set the thrusters duty cycles + * @brief send binary switching command to the thrusters * - * @param duty_cycle duty cycle for each thruster (in [0, 1]) + * @param thrusts boolean switches for each thruster (True is on, False is off) */ - void SetThrustDutyCycle(const std::array & duty_cycle); + void SetAllThrusts(const std::array & thrusts); + + /** + * @brief set single thrust command + * + * @param idx index of the target thruster + * @param on set to true to turn on thruster, false to turn off + */ + void SetThrust(size_t idx, bool on); + + /** + * @brief obtain the current state of thrusters + * + * @return boolean states for each thruster + */ + const std::array & GetThrust() const; /** * @brief send command to set the inertial wheel velocity @@ -62,8 +76,9 @@ class LowLevelController : virtual public rclcpp::Node void SetWheelVelocity(const double & velocity); private: - rclcpp::Publisher::SharedPtr thruster_pub_; + rclcpp::Publisher::SharedPtr thrust_pub_; rclcpp::Publisher::SharedPtr wheel_pub_; + ff_msgs::msg::ThrusterCommand thrust_cmd_{}; }; } // namespace ff diff --git a/ff_control/include/ff_control/pwm_ctrl.hpp b/ff_control/include/ff_control/pwm_ctrl.hpp new file mode 100644 index 0000000..448fcc7 --- /dev/null +++ b/ff_control/include/ff_control/pwm_ctrl.hpp @@ -0,0 +1,59 @@ +// 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 + +#include + +#include "ff_control/ll_ctrl.hpp" + +namespace ff +{ + +class PWMController : public LowLevelController +{ +public: + PWMController(); + +protected: + /** + * @brief send binary switching command to the thrusters + * + * @param thrusts boolean switches for each thruster (True is on, False is off) + */ + void SetThrustDutyCycle(const std::array & duty_cycle); + +private: + std::array duty_cycle_ = {0}; + std::array switch_timers_; + std::chrono::duration period_ = std::chrono::seconds(0); + rclcpp::TimerBase::SharedPtr period_timer_; + + void PeriodCallback(); + void SwitchCallback(size_t idx); +}; + +} // namespace ff diff --git a/ff_control/include/ff_control/wrench_ctrl.hpp b/ff_control/include/ff_control/wrench_ctrl.hpp index efc442a..717bfb4 100644 --- a/ff_control/include/ff_control/wrench_ctrl.hpp +++ b/ff_control/include/ff_control/wrench_ctrl.hpp @@ -25,14 +25,14 @@ #include -#include "ff_control/ll_ctrl.hpp" +#include "ff_control/pwm_ctrl.hpp" #include "ff_msgs/msg/wrench2_d.hpp" namespace ff { -class WrenchController : public LowLevelController +class WrenchController : public PWMController { public: WrenchController(); diff --git a/ff_control/scripts/safety_filter b/ff_control/scripts/safety_filter index 30eeac1..ae86307 100755 --- a/ff_control/scripts/safety_filter +++ b/ff_control/scripts/safety_filter @@ -29,8 +29,7 @@ from rclpy.duration import Duration import numpy as np import time -from ff_msgs.msg import ThrusterCommand -from ff_msgs.msg import WheelVelCommand +from ff_msgs.msg import ThrusterCommand, WheelVelCommand from std_msgs.msg import Bool class SafetyFilter(Node): @@ -44,20 +43,20 @@ class SafetyFilter(Node): self.kill_state = False # Create publishers for thruster and wheel commands - self.thrust_pub = self.create_publisher(ThrusterCommand, 'commands/duty_cycle', 10) self.wheel_pub = self.create_publisher(WheelVelCommand, 'commands/velocity', 10) + self.thrust_pub = self.create_publisher(ThrusterCommand, 'commands/binary_thrust', 10) # Create subscribers for thruster and wheel commands - self.thrust_sub = self.create_subscription( - ThrusterCommand, - 'ctrl/duty_cycle', - self.thrust_callback, - 10) self.wheel_sub = self.create_subscription( WheelVelCommand, 'ctrl/velocity', self.wheel_callback, 10) + self.thrust_sub = self.create_subscription( + ThrusterCommand, + 'ctrl/binary_thrust', + self.thrust_callback, + 10) # Create timer to check if we've received any messages in the last 'wait_period' seconds self.timer = self.create_timer(0.1, self.check_timer_callback) @@ -95,6 +94,15 @@ class SafetyFilter(Node): # Store time last message was published self.last_thrust_time = self.get_clock().now() + def thrust_callback(self, msg): + if self.kill_state: + self.get_logger().info("Kill state is active. Ignoring thrust command.") + return + # Publish the message to thrust pub + self.thrust_pub.publish(msg) + # Store time last message was published + self.last_thrust_time = self.get_clock().now() + def wheel_callback(self, msg): if self.kill_state: self.get_logger().info("Kill state is active. Ignoring wheel command.") @@ -107,8 +115,9 @@ class SafetyFilter(Node): def send_zero_thrust(self): zero_thrust_msg = ThrusterCommand() zero_thrust_msg.header.stamp = self.get_clock().now().to_msg() - zero_thrust_msg.duty_cycle = np.zeros(8) - self.thrust_pub.publish(ThrusterCommand()) + zero_thrust_msg.switches = [False] * 8 + + self.thrust_pub.publish(zero_thrust_msg) def send_zero_wheel(self): zero_wheel_msg = WheelVelCommand() diff --git a/ff_control/src/ll_ctrl.cpp b/ff_control/src/ll_ctrl.cpp index 74de847..2ae12d0 100644 --- a/ff_control/src/ll_ctrl.cpp +++ b/ff_control/src/ll_ctrl.cpp @@ -33,17 +33,34 @@ LowLevelController::LowLevelController() : rclcpp::Node("ll_ctrl_node"), p_(this) { - thruster_pub_ = this->create_publisher("ctrl/duty_cycle", 10); + thrust_pub_ = this->create_publisher("ctrl/binary_thrust", 10); wheel_pub_ = this->create_publisher("ctrl/velocity", 10); } -void LowLevelController::SetThrustDutyCycle(const std::array & duty_cycle) +void LowLevelController::SetAllThrusts(const std::array & thrusts) { - ThrusterCommand msg{}; - msg.header.stamp = this->get_clock()->now(); - msg.duty_cycle = duty_cycle; + thrust_cmd_.header.stamp = this->get_clock()->now(); + thrust_cmd_.switches = thrusts; + + thrust_pub_->publish(thrust_cmd_); +} + +void LowLevelController::SetThrust(size_t idx, bool on) +{ + if (idx > thrust_cmd_.switches.size()) { + RCLCPP_ERROR(this->get_logger(), "thrust index out of range"); + return; + } - thruster_pub_->publish(msg); + thrust_cmd_.header.stamp = this->get_clock()->now(); + thrust_cmd_.switches[idx] = on; + + thrust_pub_->publish(thrust_cmd_); +} + +const std::array & LowLevelController::GetThrust() const +{ + return thrust_cmd_.switches; } void LowLevelController::SetWheelVelocity(const double & velocity) diff --git a/ff_control/src/nodes/pwm_ctrl_node.cpp b/ff_control/src/nodes/pwm_ctrl_node.cpp new file mode 100644 index 0000000..2c64ab9 --- /dev/null +++ b/ff_control/src/nodes/pwm_ctrl_node.cpp @@ -0,0 +1,62 @@ +// 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 +#include +#include + +#include + +#include "ff_control/pwm_ctrl.hpp" +#include "ff_msgs/msg/thruster_pwm_command.hpp" + +using namespace std::placeholders; +using ff_msgs::msg::ThrusterPWMCommand; + +class PWMControlNode : public ff::PWMController +{ +public: + PWMControlNode() + : rclcpp::Node("pwm_control_node"), + ff::PWMController() + { + thruster_pwm_sub_ = this->create_subscription( + "ctrl/pwm_thrust", 10, std::bind(&PWMControlNode::PWMThrustCallback, this, _1)); + } + +private: + rclcpp::Subscription::SharedPtr thruster_pwm_sub_; + + void PWMThrustCallback(const ThrusterPWMCommand::SharedPtr msg) + { + SetThrustDutyCycle(msg->duty_cycles); + } +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/ff_control/src/pwm_ctrl.cpp b/ff_control/src/pwm_ctrl.cpp new file mode 100644 index 0000000..3fe83e3 --- /dev/null +++ b/ff_control/src/pwm_ctrl.cpp @@ -0,0 +1,76 @@ +// 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_control/pwm_ctrl.hpp" + +using namespace std::chrono_literals; + +namespace ff +{ + +PWMController::PWMController() +: rclcpp::Node("pwm_ctrl_node"), + period_(1.0s / this->declare_parameter("pwm_frequency", 10.0)) +{ + period_timer_ = this->create_wall_timer(period_, std::bind(&PWMController::PeriodCallback, this)); +} + +void PWMController::SetThrustDutyCycle(const std::array & duty_cycle) +{ + duty_cycle_ = duty_cycle; +} + +void PWMController::PeriodCallback() +{ + std::array thrusts = {0}; + + for (size_t i = 0; i < thrusts.size(); ++i) { + if (duty_cycle_[i] > 0) { + thrusts[i] = true; + switch_timers_[i] = this->create_wall_timer( + duty_cycle_[i] * period_, + [this, i]() {SwitchCallback(i);} + ); + } + } + + SetAllThrusts(thrusts); +} + +void PWMController::SwitchCallback(size_t idx) +{ + if (idx >= duty_cycle_.size()) { + RCLCPP_ERROR(this->get_logger(), "PWM idx out of range"); + return; + } + + if (!switch_timers_[idx]) { + RCLCPP_ERROR(this->get_logger(), "encountered unitialized pointer"); + return; + } + + switch_timers_[idx]->cancel(); + SetThrust(idx, false); +} + +} // namespace ff diff --git a/ff_control/src/wrench_ctrl.cpp b/ff_control/src/wrench_ctrl.cpp index 6d2a9a1..7608ab4 100644 --- a/ff_control/src/wrench_ctrl.cpp +++ b/ff_control/src/wrench_ctrl.cpp @@ -32,7 +32,7 @@ namespace ff WrenchController::WrenchController() : rclcpp::Node("wrench_ctrl_node"), - LowLevelController() {} + PWMController() {} void WrenchController::SetBodyWrench(const Wrench2D & wrench_body, bool use_wheel) { diff --git a/ff_drivers/CMakeLists.txt b/ff_drivers/CMakeLists.txt index 734adbf..ec08039 100644 --- a/ff_drivers/CMakeLists.txt +++ b/ff_drivers/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED) 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 @@ -18,20 +19,20 @@ target_include_directories(pwm PUBLIC target_compile_features(pwm PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 ament_target_dependencies(pwm rclcpp) -# nodes -add_executable(thruster_node src/thruster_node.cpp) -target_link_libraries(thruster_node pwm) -ament_target_dependencies(thruster_node ff_msgs) +# 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) # tests add_executable(test_single src/tests/test_single.cpp) target_link_libraries(test_single pwm) add_executable(test_all_thrusters src/tests/test_all_thrusters.cpp) -ament_target_dependencies(test_all_thrusters rclcpp ff_msgs) +ament_target_dependencies(test_all_thrusters rclcpp ff_msgs ff_control) # install nodes -install(TARGETS thruster_node test_single test_all_thrusters +install(TARGETS test_single test_all_thrusters DESTINATION lib/${PROJECT_NAME}) # install launch files diff --git a/ff_drivers/package.xml b/ff_drivers/package.xml index dce8506..e0bc0e4 100644 --- a/ff_drivers/package.xml +++ b/ff_drivers/package.xml @@ -12,7 +12,8 @@ rclcpp ff_msgs - ff_control + ff_control + ff_params realsense2_camera vrpn_mocap diff --git a/ff_drivers/src/tests/test_all_thrusters.cpp b/ff_drivers/src/tests/test_all_thrusters.cpp index afa2e40..1540968 100644 --- a/ff_drivers/src/tests/test_all_thrusters.cpp +++ b/ff_drivers/src/tests/test_all_thrusters.cpp @@ -25,25 +25,23 @@ #include -#include "ff_msgs/msg/thruster_command.hpp" +#include "ff_control/pwm_ctrl.hpp" using namespace std::chrono_literals; -using ff_msgs::msg::ThrusterCommand; -class TestAllThrustersNode : public rclcpp::Node +class TestAllThrustersNode : public ff::PWMController { public: TestAllThrustersNode() - : rclcpp::Node("test_all_thrusters_node") + : rclcpp::Node("test_all_thrusters_node"), + ff::PWMController() { - thrust_cmd_pub_ = this->create_publisher("commands/duty_cycle", 10); timer_ = this->create_wall_timer(5s, std::bind(&TestAllThrustersNode::TimerCallback, this)); this->declare_parameter("duty_cycle", .2); } private: - rclcpp::Publisher::SharedPtr thrust_cmd_pub_; rclcpp::TimerBase::SharedPtr timer_; int th_idx_ = 0; @@ -51,17 +49,17 @@ class TestAllThrustersNode : public rclcpp::Node { double duty_cycle = this->get_parameter("duty_cycle").as_double(); + std::array duty_cycles; // populate thrust msg - ThrusterCommand msg{}; for (int i = 0; i < 8; ++i) { - msg.duty_cycle[i] = 0.; + duty_cycles[i] = 0.; if (i == th_idx_) { - msg.duty_cycle[i] = duty_cycle; + duty_cycles[i] = duty_cycle; } } // publish thrust msg - this->thrust_cmd_pub_->publish(msg); + SetThrustDutyCycle(duty_cycles); RCLCPP_INFO(this->get_logger(), "opening valve %d", th_idx_); // increment th_idx diff --git a/ff_drivers/src/thruster_node.cpp b/ff_drivers/src/thruster_node.cpp index d29005b..8e9fe7e 100644 --- a/ff_drivers/src/thruster_node.cpp +++ b/ff_drivers/src/thruster_node.cpp @@ -27,7 +27,7 @@ #include #include "ff_drivers/pwm.hpp" -#include "ff_msgs/msg/thruster_command.hpp" +#include "ff_msgs/msg/thruster_pwm_command.hpp" #define NUM_THRUSTERS 8 @@ -46,7 +46,7 @@ static constexpr int THRUSTER_PINS[NUM_THRUSTERS] = { using namespace std::chrono_literals; -using ff_msgs::msg::ThrusterCommand; +using ff_msgs::msg::ThrusterPWMCommand; class ThrusterNode : public ff::PWMManager { @@ -72,20 +72,20 @@ class ThrusterNode : public ff::PWMManager this->EnableAll(); // listen to commands - sub_duty_cycle_ = this->create_subscription( + sub_duty_cycle_ = this->create_subscription( "commands/duty_cycle", - 10, [this](const ThrusterCommand::SharedPtr msg) {DutyCycleCallback(msg);}); + 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_; + rclcpp::Subscription::SharedPtr sub_duty_cycle_; - void DutyCycleCallback(const ThrusterCommand::SharedPtr msg) + void DutyCycleCallback(const ThrusterPWMCommand::SharedPtr msg) { for (size_t i = 0; i < NUM_THRUSTERS; ++i) { - this->SetDutyCycle(i, msg->duty_cycle[i]); + this->SetDutyCycle(i, msg->duty_cycles[i]); } } }; diff --git a/ff_msgs/CMakeLists.txt b/ff_msgs/CMakeLists.txt index 0ef3928..a889b37 100644 --- a/ff_msgs/CMakeLists.txt +++ b/ff_msgs/CMakeLists.txt @@ -19,8 +19,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Wrench2DStamped.msg" "msg/FreeFlyerState.msg" "msg/FreeFlyerStateStamped.msg" - "msg/ThrusterCommand.msg" "msg/WheelVelCommand.msg" + "msg/ThrusterCommand.msg" + "msg/ThrusterPWMCommand.msg" DEPENDENCIES std_msgs ) diff --git a/ff_msgs/msg/ThrusterCommand.msg b/ff_msgs/msg/ThrusterCommand.msg index 0179c9e..86cd72a 100644 --- a/ff_msgs/msg/ThrusterCommand.msg +++ b/ff_msgs/msg/ThrusterCommand.msg @@ -1,2 +1,2 @@ std_msgs/Header header -float64[8] duty_cycle +bool[8] switches diff --git a/ff_msgs/msg/ThrusterPWMCommand.msg b/ff_msgs/msg/ThrusterPWMCommand.msg new file mode 100644 index 0000000..31cd9e5 --- /dev/null +++ b/ff_msgs/msg/ThrusterPWMCommand.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +float64[8] duty_cycles diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index 4096ae5..7503257 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -24,7 +24,7 @@ """ Simulates the freeflyer. -Maps thrusters duty cycle + wheel control input +Maps thrusters + wheel control input to a state evolution over time ----------------------------------------------- """ @@ -38,7 +38,12 @@ from rclpy.node import Node from geometry_msgs.msg import PoseStamped -from ff_msgs.msg import FreeFlyerStateStamped, Wrench2DStamped, ThrusterCommand, WheelVelCommand +from ff_msgs.msg import ( + FreeFlyerStateStamped, + Wrench2DStamped, + WheelVelCommand, + ThrusterCommand, +) from ff_params import RobotParams @@ -134,7 +139,7 @@ def __init__(self): p_sim = self.declare_parameters( "", [ - ("sim_dt", 0.01), # update period in [s] + ("sim_dt", 0.001), # update period in [s] ("discretization", "Euler"), # discretization scheme from {"Euler", "RungeKutta"} ("x_0", [0.6, 2.0, 0.0, 0.0, 0.0, 0.0]), # initial state ("B_sim_contacts", True), # if True, simulates contacts @@ -149,7 +154,7 @@ def __init__(self): self.F_cmd_body = np.zeros(2) # (x,y) self.theta_dot_cmd = 0.0 # commands - self.thrusters_dutycycle_cmd = np.zeros(8) + self.thrusters = np.zeros(8) self.wheel_vel_cmd = 0.0 # current states of the freeflyer, in world frame self.x_cur = self.x_0.copy() # (x, y, theta, v_x, v_y, theta_dot) @@ -159,8 +164,8 @@ def __init__(self): self.sub_wheel_cmd_vel = self.create_subscription( WheelVelCommand, "commands/velocity", self.update_wheel_cmd_vel_cb, 10 ) - self.sub_thrusters_cmd_dutycycle = self.create_subscription( - ThrusterCommand, "commands/duty_cycle", self.update_thrusters_dutycycle_cmd_cb, 10 + self.sub_thrusters_cmd_binary = self.create_subscription( + ThrusterCommand, "commands/binary_thrust", self.update_thrusters_cb, 10 ) self.sub_state_init = self.create_subscription( FreeFlyerStateStamped, "state_init", self.update_state_init_cb, 10 @@ -185,13 +190,13 @@ def sim_loop(self) -> None: now = self.get_clock().now().to_msg() # Get thrusters forces - # Note: assume perfect mapping from duty cycle to true forces. (ToDo: simulate missmatch) - F_bodyFrame, M = self.thrusters_dutycycle_to_body_wrench(self.thrusters_dutycycle_cmd) + # Note: assume perfect mapping from thrust to true forces. (ToDo: simulate missmatch) + F_bodyFrame, M = self.thrusters_to_body_wrench(self.thrusters) # Get true torque - # Note: assume perfect mapping from duty cycle to true forces. (ToDo: simulate missmatch) + # Note: assume perfect mapping from thrust to true forces. (ToDo: simulate missmatch) self.F_cmd_body = F_bodyFrame - # Publish true exerted force on freeflyer, as a mapping of the thrusters command duty cycle + # Publish true exerted force on freeflyer, as a mapping of the thrusters command R = self.get_rotmatrix_body_to_world(self.x_cur[2]) F_worldFrame = np.matmul(R, F_bodyFrame) wrench_msg = Wrench2DStamped() @@ -257,9 +262,8 @@ def sim_loop(self) -> None: def update_wheel_cmd_vel_cb(self, msg: WheelVelCommand) -> None: self.wheel_vel_cmd = msg.velocity - def update_thrusters_dutycycle_cmd_cb(self, msg: ThrusterCommand) -> None: - # Saturate controls so within [0,1] (in %) - self.thrusters_dutycycle_cmd = np.clip(msg.duty_cycle, 0.0, 1.0) + def update_thrusters_cb(self, msg: ThrusterCommand) -> None: + self.thrusters = np.array(msg.switches, dtype=float) def update_state_init_cb(self, msg: FreeFlyerStateStamped) -> None: self.x_cur = np.array( @@ -273,7 +277,7 @@ def update_state_init_cb(self, msg: FreeFlyerStateStamped) -> None: ] ) - def thrusters_dutycycle_to_body_wrench(self, thrusters_dutycycles_vec): + def thrusters_to_body_wrench(self, thrusters_vec): # Thrusters Configuration # (2) e_y (1) ___ # <-- ^ --> / \ @@ -290,7 +294,7 @@ def thrusters_dutycycle_to_body_wrench(self, thrusters_dutycycles_vec): Fmax = self.p.actuators["F_max_per_thruster"] dist = self.p.actuators["thrusters_lever_arm"] - u_dc = thrusters_dutycycles_vec + u_dc = thrusters_vec F_x = Fmax * ((u_dc[2] + u_dc[5]) - (u_dc[1] + u_dc[6])) F_y = Fmax * ((u_dc[4] + u_dc[7]) - (u_dc[0] + u_dc[3])) diff --git a/freeflyer/launch/sim_pd.launch.py b/freeflyer/launch/sim_pd.launch.py index 53fc139..6eb10e8 100644 --- a/freeflyer/launch/sim_pd.launch.py +++ b/freeflyer/launch/sim_pd.launch.py @@ -23,7 +23,8 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -67,6 +68,13 @@ def generate_launch_description(): name="pd_ctrl_node", namespace=robot_name, ), + Node( + package="ff_control", + executable=["pwm_ctrl_cpp_node"], + name="pwm_ctrl_node", + namespace=robot_name, + condition=IfCondition(PythonExpression(["'", impl, "'", " == 'py'"])), + ), Node( package="ff_estimate", executable="moving_avg_estimator_node", diff --git a/freeflyer/test/pd_ctrl_launch_test.py b/freeflyer/test/pd_ctrl_launch_test.py index 2949c13..bcf9134 100644 --- a/freeflyer/test/pd_ctrl_launch_test.py +++ b/freeflyer/test/pd_ctrl_launch_test.py @@ -28,7 +28,8 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch_testing.actions import ReadyToTest @@ -58,6 +59,13 @@ def generate_test_description(): name="pd_ctrl_node", namespace=ROBOT_NAME, ) + pwm_ctrl_node = Node( + package="ff_control", + executable=["pwm_ctrl_cpp_node"], + name="pwm_ctrl_node", + namespace=ROBOT_NAME, + condition=IfCondition(PythonExpression(["'", impl, "'", " == 'py'"])), + ) estimator = Node( package="ff_estimate", executable="moving_avg_estimator_node", @@ -70,6 +78,7 @@ def generate_test_description(): DeclareLaunchArgument("impl", default_value="cpp", choices=["cpp", "py"]), sim_launch, pd_ctrl_node, + pwm_ctrl_node, estimator, ReadyToTest(), ]