From 65797e9a52a46edb0cabac3cd34fcd917103189f Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 9 Jan 2024 15:38:47 -0800 Subject: [PATCH 01/15] cherry pick from joshua's PR --- ff_control/ff_control/ll_binary_ctrl.py | 33 +++++++++++++++++++++++++ ff_control/scripts/safety_filter | 25 ++++++++++++++++++- ff_msgs/CMakeLists.txt | 1 + ff_msgs/msg/BinaryCommand.msg | 2 ++ ff_sim/ff_sim/simulator_node.py | 8 +++++- 5 files changed, 67 insertions(+), 2 deletions(-) create mode 100644 ff_control/ff_control/ll_binary_ctrl.py create mode 100644 ff_msgs/msg/BinaryCommand.msg diff --git a/ff_control/ff_control/ll_binary_ctrl.py b/ff_control/ff_control/ll_binary_ctrl.py new file mode 100644 index 0000000..cb8a53b --- /dev/null +++ b/ff_control/ff_control/ll_binary_ctrl.py @@ -0,0 +1,33 @@ +from ff_msgs.msg import ThrusterCommand +from ff_msgs.msg import BinaryCommand +from ff_params import RobotParams + +import numpy as np +import typing as T + +from rclpy.node import Node + + +class LowLevelBinaryController(Node): + def __init__(self, node_name: str = "ll_ctrl_node") -> None: + super().__init__(node_name) + + # robot parameters that can be accessed by sub-classes + self.p = RobotParams(self) + + # low level control publishers + self._binary_thruster_pub = self.create_publisher(BinaryCommand, "ctrl/binary_command", 10) + + def set_thrust_binary(self, binary_command: T.Sequence[bool]) -> None: + """ + Send command to set the thrusters binary output. + + :param binary_command: binary switch for each thrust + """ + if len(binary_command) != len(BinaryCommand().binary_command): + self.get_logger().error("Incompatible thruster length sent.") + return + msg = BinaryCommand() + msg.header.stamp = self.get_clock().now().to_msg() + msg.binary_command = binary_command + self._binary_thruster_pub.publish(msg) \ No newline at end of file diff --git a/ff_control/scripts/safety_filter b/ff_control/scripts/safety_filter index 30eeac1..3490aba 100755 --- a/ff_control/scripts/safety_filter +++ b/ff_control/scripts/safety_filter @@ -31,6 +31,7 @@ import time from ff_msgs.msg import ThrusterCommand from ff_msgs.msg import WheelVelCommand +from ff_msgs.msg import BinaryCommand from std_msgs.msg import Bool class SafetyFilter(Node): @@ -46,6 +47,7 @@ class SafetyFilter(Node): # 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.bool_thrust_pub = self.create_publisher(BinaryCommand, 'commands/binary', 10) # Create subscribers for thruster and wheel commands self.thrust_sub = self.create_subscription( @@ -58,6 +60,11 @@ class SafetyFilter(Node): 'ctrl/velocity', self.wheel_callback, 10) + self.bool_thrust_sub = self.create_subscription( + BinaryCommand, + 'ctrl/binary', + self.bool_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 +102,15 @@ class SafetyFilter(Node): # Store time last message was published self.last_thrust_time = self.get_clock().now() + def bool_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.bool_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.") @@ -108,7 +124,14 @@ class SafetyFilter(Node): 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_binary_msg = BinaryCommand() + zero_thrust_binary_msg.header.stamp = self.get_clock().now().to_msg() + zero_thrust_binary_msg.binary_command = [False] * 8 + + self.thrust_pub.publish(zero_thrust_msg) + self.bool_thrust_pub.publish(zero_thrust_binary_msg) + def send_zero_wheel(self): zero_wheel_msg = WheelVelCommand() diff --git a/ff_msgs/CMakeLists.txt b/ff_msgs/CMakeLists.txt index 0ef3928..41e55d2 100644 --- a/ff_msgs/CMakeLists.txt +++ b/ff_msgs/CMakeLists.txt @@ -21,6 +21,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/FreeFlyerStateStamped.msg" "msg/ThrusterCommand.msg" "msg/WheelVelCommand.msg" + "msg/BinaryCommand.msg" DEPENDENCIES std_msgs ) diff --git a/ff_msgs/msg/BinaryCommand.msg b/ff_msgs/msg/BinaryCommand.msg new file mode 100644 index 0000000..ed287e0 --- /dev/null +++ b/ff_msgs/msg/BinaryCommand.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +bool[8] binary_command \ No newline at end of file diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index 4096ae5..9d7e5b8 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -38,7 +38,7 @@ 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, ThrusterCommand, WheelVelCommand, BinaryCommand from ff_params import RobotParams @@ -162,6 +162,9 @@ def __init__(self): 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( + BinaryCommand, "commands/binary", self.update_thrusters_binary_cmd_cb, 10 + ) self.sub_state_init = self.create_subscription( FreeFlyerStateStamped, "state_init", self.update_state_init_cb, 10 ) @@ -260,6 +263,9 @@ def update_wheel_cmd_vel_cb(self, msg: WheelVelCommand) -> None: 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_binary_cmd_cb(self, msg: BinaryCommand) -> None: + self.thrusters_dutycycle_cmd = np.array(msg.binary_command, dtype = float) def update_state_init_cb(self, msg: FreeFlyerStateStamped) -> None: self.x_cur = np.array( From 5d48dffdd0d71a839c838d51e1f64312fa54cca5 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 9 Jan 2024 15:57:38 -0800 Subject: [PATCH 02/15] rename thruster messages --- ff_control/ff_control/ll_binary_ctrl.py | 4 ++-- ff_control/ff_control/ll_ctrl.py | 10 +++++----- ff_control/include/ff_control/ll_ctrl.hpp | 4 ++-- ff_control/scripts/safety_filter | 14 +++++++------- ff_control/src/ll_ctrl.cpp | 8 ++++---- ff_drivers/src/tests/test_all_thrusters.cpp | 14 +++++++------- ff_drivers/src/thruster_node.cpp | 14 +++++++------- ff_msgs/CMakeLists.txt | 4 ++-- ...BinaryCommand.msg => ThrusterBinaryCommand.msg} | 2 +- ...{ThrusterCommand.msg => ThrusterPWMCommand.msg} | 2 +- ff_sim/ff_sim/simulator_node.py | 10 +++++----- 11 files changed, 43 insertions(+), 43 deletions(-) rename ff_msgs/msg/{BinaryCommand.msg => ThrusterBinaryCommand.msg} (51%) rename ff_msgs/msg/{ThrusterCommand.msg => ThrusterPWMCommand.msg} (50%) diff --git a/ff_control/ff_control/ll_binary_ctrl.py b/ff_control/ff_control/ll_binary_ctrl.py index cb8a53b..f4d8a65 100644 --- a/ff_control/ff_control/ll_binary_ctrl.py +++ b/ff_control/ff_control/ll_binary_ctrl.py @@ -1,4 +1,4 @@ -from ff_msgs.msg import ThrusterCommand +from ff_msgs.msg import ThrusterPWMCommand from ff_msgs.msg import BinaryCommand from ff_params import RobotParams @@ -30,4 +30,4 @@ def set_thrust_binary(self, binary_command: T.Sequence[bool]) -> None: msg = BinaryCommand() msg.header.stamp = self.get_clock().now().to_msg() msg.binary_command = binary_command - self._binary_thruster_pub.publish(msg) \ No newline at end of file + self._binary_thruster_pub.publish(msg) diff --git a/ff_control/ff_control/ll_ctrl.py b/ff_control/ff_control/ll_ctrl.py index 5cfabfa..fe71ba6 100644 --- a/ff_control/ff_control/ll_ctrl.py +++ b/ff_control/ff_control/ll_ctrl.py @@ -20,7 +20,7 @@ # 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 ThrusterPWMCommand from ff_msgs.msg import WheelVelCommand from ff_params import RobotParams @@ -37,7 +37,7 @@ def __init__(self, node_name: str = "ll_ctrl_node") -> None: self.p = RobotParams(self) # low level control publishers - self._thruster_pub = self.create_publisher(ThrusterCommand, "ctrl/duty_cycle", 10) + self._thruster_pub = self.create_publisher(ThrusterPWMCommand, "ctrl/duty_cycle", 10) self._wheel_pub = self.create_publisher(WheelVelCommand, "ctrl/velocity", 10) def set_thrust_duty_cycle(self, duty_cycle: np.ndarray) -> None: @@ -46,12 +46,12 @@ def set_thrust_duty_cycle(self, duty_cycle: np.ndarray) -> None: :param duty_cycle: duty cycle for each thrust (in [0, 1]) """ - if len(duty_cycle) != len(ThrusterCommand().duty_cycle): + if len(duty_cycle) != len(ThrusterPWMCommand().duty_cycles): self.get_logger().error("Incompatible thruster length sent.") return - msg = ThrusterCommand() + msg = ThrusterPWMCommand() msg.header.stamp = self.get_clock().now().to_msg() - msg.duty_cycle = duty_cycle + msg.duty_cycles = duty_cycle self._thruster_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..7bf37b6 100644 --- a/ff_control/include/ff_control/ll_ctrl.hpp +++ b/ff_control/include/ff_control/ll_ctrl.hpp @@ -28,7 +28,7 @@ #include -#include "ff_msgs/msg/thruster_command.hpp" +#include "ff_msgs/msg/thruster_pwm_command.hpp" #include "ff_msgs/msg/wheel_vel_command.hpp" #include "ff_params/robot_params.hpp" @@ -62,7 +62,7 @@ class LowLevelController : virtual public rclcpp::Node void SetWheelVelocity(const double & velocity); private: - rclcpp::Publisher::SharedPtr thruster_pub_; + rclcpp::Publisher::SharedPtr thruster_pub_; rclcpp::Publisher::SharedPtr wheel_pub_; }; diff --git a/ff_control/scripts/safety_filter b/ff_control/scripts/safety_filter index 3490aba..905ce74 100755 --- a/ff_control/scripts/safety_filter +++ b/ff_control/scripts/safety_filter @@ -29,7 +29,7 @@ from rclpy.duration import Duration import numpy as np import time -from ff_msgs.msg import ThrusterCommand +from ff_msgs.msg import ThrusterPWMCommand from ff_msgs.msg import WheelVelCommand from ff_msgs.msg import BinaryCommand from std_msgs.msg import Bool @@ -45,13 +45,13 @@ 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.thrust_pub = self.create_publisher(ThrusterPWMCommand, 'commands/duty_cycle', 10) self.wheel_pub = self.create_publisher(WheelVelCommand, 'commands/velocity', 10) self.bool_thrust_pub = self.create_publisher(BinaryCommand, 'commands/binary', 10) # Create subscribers for thruster and wheel commands self.thrust_sub = self.create_subscription( - ThrusterCommand, + ThrusterPWMCommand, 'ctrl/duty_cycle', self.thrust_callback, 10) @@ -121,17 +121,17 @@ class SafetyFilter(Node): self.last_wheel_time = self.get_clock().now() def send_zero_thrust(self): - zero_thrust_msg = ThrusterCommand() + zero_thrust_msg = ThrusterPWMCommand() zero_thrust_msg.header.stamp = self.get_clock().now().to_msg() - zero_thrust_msg.duty_cycle = np.zeros(8) + zero_thrust_msg.duty_cycles = np.zeros(8) zero_thrust_binary_msg = BinaryCommand() zero_thrust_binary_msg.header.stamp = self.get_clock().now().to_msg() zero_thrust_binary_msg.binary_command = [False] * 8 - + self.thrust_pub.publish(zero_thrust_msg) self.bool_thrust_pub.publish(zero_thrust_binary_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..87937bf 100644 --- a/ff_control/src/ll_ctrl.cpp +++ b/ff_control/src/ll_ctrl.cpp @@ -23,7 +23,7 @@ #include "ff_control/ll_ctrl.hpp" -using ff_msgs::msg::ThrusterCommand; +using ff_msgs::msg::ThrusterPWMCommand; using ff_msgs::msg::WheelVelCommand; namespace ff @@ -33,15 +33,15 @@ LowLevelController::LowLevelController() : rclcpp::Node("ll_ctrl_node"), p_(this) { - thruster_pub_ = this->create_publisher("ctrl/duty_cycle", 10); + thruster_pub_ = this->create_publisher("ctrl/duty_cycle", 10); wheel_pub_ = this->create_publisher("ctrl/velocity", 10); } void LowLevelController::SetThrustDutyCycle(const std::array & duty_cycle) { - ThrusterCommand msg{}; + ThrusterPWMCommand msg{}; msg.header.stamp = this->get_clock()->now(); - msg.duty_cycle = duty_cycle; + msg.duty_cycles = duty_cycle; thruster_pub_->publish(msg); } diff --git a/ff_drivers/src/tests/test_all_thrusters.cpp b/ff_drivers/src/tests/test_all_thrusters.cpp index afa2e40..3491469 100644 --- a/ff_drivers/src/tests/test_all_thrusters.cpp +++ b/ff_drivers/src/tests/test_all_thrusters.cpp @@ -25,10 +25,10 @@ #include -#include "ff_msgs/msg/thruster_command.hpp" +#include "ff_msgs/msg/thruster_pwm_command.hpp" using namespace std::chrono_literals; -using ff_msgs::msg::ThrusterCommand; +using ff_msgs::msg::ThrusterPWMCommand; class TestAllThrustersNode : public rclcpp::Node @@ -37,13 +37,13 @@ class TestAllThrustersNode : public rclcpp::Node TestAllThrustersNode() : rclcpp::Node("test_all_thrusters_node") { - thrust_cmd_pub_ = this->create_publisher("commands/duty_cycle", 10); + 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::Publisher::SharedPtr thrust_cmd_pub_; rclcpp::TimerBase::SharedPtr timer_; int th_idx_ = 0; @@ -52,11 +52,11 @@ class TestAllThrustersNode : public rclcpp::Node double duty_cycle = this->get_parameter("duty_cycle").as_double(); // populate thrust msg - ThrusterCommand msg{}; + ThrusterPWMCommand msg{}; for (int i = 0; i < 8; ++i) { - msg.duty_cycle[i] = 0.; + msg.duty_cycles[i] = 0.; if (i == th_idx_) { - msg.duty_cycle[i] = duty_cycle; + msg.duty_cycles[i] = duty_cycle; } } 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 41e55d2..f01b54a 100644 --- a/ff_msgs/CMakeLists.txt +++ b/ff_msgs/CMakeLists.txt @@ -19,9 +19,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Wrench2DStamped.msg" "msg/FreeFlyerState.msg" "msg/FreeFlyerStateStamped.msg" - "msg/ThrusterCommand.msg" + "msg/ThrusterPWMCommand.msg" "msg/WheelVelCommand.msg" - "msg/BinaryCommand.msg" + "msg/ThrusterBinaryCommand.msg" DEPENDENCIES std_msgs ) diff --git a/ff_msgs/msg/BinaryCommand.msg b/ff_msgs/msg/ThrusterBinaryCommand.msg similarity index 51% rename from ff_msgs/msg/BinaryCommand.msg rename to ff_msgs/msg/ThrusterBinaryCommand.msg index ed287e0..86cd72a 100644 --- a/ff_msgs/msg/BinaryCommand.msg +++ b/ff_msgs/msg/ThrusterBinaryCommand.msg @@ -1,2 +1,2 @@ std_msgs/Header header -bool[8] binary_command \ No newline at end of file +bool[8] switches diff --git a/ff_msgs/msg/ThrusterCommand.msg b/ff_msgs/msg/ThrusterPWMCommand.msg similarity index 50% rename from ff_msgs/msg/ThrusterCommand.msg rename to ff_msgs/msg/ThrusterPWMCommand.msg index 0179c9e..31cd9e5 100644 --- a/ff_msgs/msg/ThrusterCommand.msg +++ b/ff_msgs/msg/ThrusterPWMCommand.msg @@ -1,2 +1,2 @@ std_msgs/Header header -float64[8] duty_cycle +float64[8] duty_cycles diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index 9d7e5b8..165cafb 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -38,7 +38,7 @@ from rclpy.node import Node from geometry_msgs.msg import PoseStamped -from ff_msgs.msg import FreeFlyerStateStamped, Wrench2DStamped, ThrusterCommand, WheelVelCommand, BinaryCommand +from ff_msgs.msg import FreeFlyerStateStamped, Wrench2DStamped, ThrusterPWMCommand, WheelVelCommand, BinaryCommand from ff_params import RobotParams @@ -160,7 +160,7 @@ def __init__(self): 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 + ThrusterPWMCommand, "commands/duty_cycle", self.update_thrusters_dutycycle_cmd_cb, 10 ) self.sub_thrusters_cmd_binary = self.create_subscription( BinaryCommand, "commands/binary", self.update_thrusters_binary_cmd_cb, 10 @@ -260,10 +260,10 @@ 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: + def update_thrusters_dutycycle_cmd_cb(self, msg: ThrusterPWMCommand) -> None: # Saturate controls so within [0,1] (in %) - self.thrusters_dutycycle_cmd = np.clip(msg.duty_cycle, 0.0, 1.0) - + self.thrusters_dutycycle_cmd = np.clip(msg.duty_cycles, 0.0, 1.0) + def update_thrusters_binary_cmd_cb(self, msg: BinaryCommand) -> None: self.thrusters_dutycycle_cmd = np.array(msg.binary_command, dtype = float) From bdd8a38482f17aa48ffd5d10ce51484b5220e1ba Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 9 Jan 2024 16:18:18 -0800 Subject: [PATCH 03/15] fix tests --- ff_control/ff_control/ll_binary_ctrl.py | 33 ------------------------- ff_control/ff_control/ll_ctrl.py | 32 ++++++++++++++++++------ ff_control/ff_control/wrench_ctrl.py | 2 +- ff_control/scripts/safety_filter | 10 ++++---- ff_sim/ff_sim/simulator_node.py | 16 ++++++++---- 5 files changed, 42 insertions(+), 51 deletions(-) delete mode 100644 ff_control/ff_control/ll_binary_ctrl.py diff --git a/ff_control/ff_control/ll_binary_ctrl.py b/ff_control/ff_control/ll_binary_ctrl.py deleted file mode 100644 index f4d8a65..0000000 --- a/ff_control/ff_control/ll_binary_ctrl.py +++ /dev/null @@ -1,33 +0,0 @@ -from ff_msgs.msg import ThrusterPWMCommand -from ff_msgs.msg import BinaryCommand -from ff_params import RobotParams - -import numpy as np -import typing as T - -from rclpy.node import Node - - -class LowLevelBinaryController(Node): - def __init__(self, node_name: str = "ll_ctrl_node") -> None: - super().__init__(node_name) - - # robot parameters that can be accessed by sub-classes - self.p = RobotParams(self) - - # low level control publishers - self._binary_thruster_pub = self.create_publisher(BinaryCommand, "ctrl/binary_command", 10) - - def set_thrust_binary(self, binary_command: T.Sequence[bool]) -> None: - """ - Send command to set the thrusters binary output. - - :param binary_command: binary switch for each thrust - """ - if len(binary_command) != len(BinaryCommand().binary_command): - self.get_logger().error("Incompatible thruster length sent.") - return - msg = BinaryCommand() - msg.header.stamp = self.get_clock().now().to_msg() - msg.binary_command = binary_command - self._binary_thruster_pub.publish(msg) diff --git a/ff_control/ff_control/ll_ctrl.py b/ff_control/ff_control/ll_ctrl.py index fe71ba6..e538a03 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 ThrusterPWMCommand -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, ThrusterBinaryCommand +from ff_msgs.msg import WheelVelCommand +from ff_params import RobotParams from rclpy.node import Node @@ -36,8 +36,12 @@ 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(ThrusterPWMCommand, "ctrl/duty_cycle", 10) + # low level thruster control publishers + self._thruster_pwm_pub = self.create_publisher(ThrusterPWMCommand, "ctrl/duty_cycle", 10) + self._thruster_binary_pub = self.create_publisher( + ThrusterBinaryCommand, "ctrl/binary_command", 10 + ) + self._wheel_pub = self.create_publisher(WheelVelCommand, "ctrl/velocity", 10) def set_thrust_duty_cycle(self, duty_cycle: np.ndarray) -> None: @@ -52,7 +56,21 @@ def set_thrust_duty_cycle(self, duty_cycle: np.ndarray) -> None: msg = ThrusterPWMCommand() msg.header.stamp = self.get_clock().now().to_msg() msg.duty_cycles = duty_cycle - self._thruster_pub.publish(msg) + self._thruster_pwm_pub.publish(msg) + + def set_thrust_binary(self, binary_command: T.Sequence[bool]) -> None: + """ + Send command to set the thrusters binary output. + + :param binary_command: binary switch for each thrust + """ + if len(binary_command) != len(ThrusterBinaryCommand().switches): + self.get_logger().error("Incompatible thruster length sent.") + return + msg = ThrusterBinaryCommand() + msg.header.stamp = self.get_clock().now().to_msg() + msg.switches = binary_command + self._thruster_binary_pub.publish(msg) def set_wheel_velocity(self, velocity: float) -> None: """ 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_control/scripts/safety_filter b/ff_control/scripts/safety_filter index 905ce74..6796628 100755 --- a/ff_control/scripts/safety_filter +++ b/ff_control/scripts/safety_filter @@ -31,7 +31,7 @@ import time from ff_msgs.msg import ThrusterPWMCommand from ff_msgs.msg import WheelVelCommand -from ff_msgs.msg import BinaryCommand +from ff_msgs.msg import ThrusterBinaryCommand from std_msgs.msg import Bool class SafetyFilter(Node): @@ -47,7 +47,7 @@ class SafetyFilter(Node): # Create publishers for thruster and wheel commands self.thrust_pub = self.create_publisher(ThrusterPWMCommand, 'commands/duty_cycle', 10) self.wheel_pub = self.create_publisher(WheelVelCommand, 'commands/velocity', 10) - self.bool_thrust_pub = self.create_publisher(BinaryCommand, 'commands/binary', 10) + self.bool_thrust_pub = self.create_publisher(ThrusterBinaryCommand, 'commands/binary', 10) # Create subscribers for thruster and wheel commands self.thrust_sub = self.create_subscription( @@ -61,7 +61,7 @@ class SafetyFilter(Node): self.wheel_callback, 10) self.bool_thrust_sub = self.create_subscription( - BinaryCommand, + ThrusterBinaryCommand, 'ctrl/binary', self.bool_thrust_callback, 10) @@ -125,9 +125,9 @@ class SafetyFilter(Node): zero_thrust_msg.header.stamp = self.get_clock().now().to_msg() zero_thrust_msg.duty_cycles = np.zeros(8) - zero_thrust_binary_msg = BinaryCommand() + zero_thrust_binary_msg = ThrusterBinaryCommand() zero_thrust_binary_msg.header.stamp = self.get_clock().now().to_msg() - zero_thrust_binary_msg.binary_command = [False] * 8 + zero_thrust_binary_msg.switches = [False] * 8 self.thrust_pub.publish(zero_thrust_msg) self.bool_thrust_pub.publish(zero_thrust_binary_msg) diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index 165cafb..7f227e9 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -38,7 +38,13 @@ from rclpy.node import Node from geometry_msgs.msg import PoseStamped -from ff_msgs.msg import FreeFlyerStateStamped, Wrench2DStamped, ThrusterPWMCommand, WheelVelCommand, BinaryCommand +from ff_msgs.msg import ( + FreeFlyerStateStamped, + Wrench2DStamped, + ThrusterPWMCommand, + WheelVelCommand, + ThrusterBinaryCommand, +) from ff_params import RobotParams @@ -163,7 +169,7 @@ def __init__(self): ThrusterPWMCommand, "commands/duty_cycle", self.update_thrusters_dutycycle_cmd_cb, 10 ) self.sub_thrusters_cmd_binary = self.create_subscription( - BinaryCommand, "commands/binary", self.update_thrusters_binary_cmd_cb, 10 + ThrusterBinaryCommand, "commands/binary", self.update_thrusters_binary_cmd_cb, 10 ) self.sub_state_init = self.create_subscription( FreeFlyerStateStamped, "state_init", self.update_state_init_cb, 10 @@ -264,8 +270,8 @@ def update_thrusters_dutycycle_cmd_cb(self, msg: ThrusterPWMCommand) -> None: # Saturate controls so within [0,1] (in %) self.thrusters_dutycycle_cmd = np.clip(msg.duty_cycles, 0.0, 1.0) - def update_thrusters_binary_cmd_cb(self, msg: BinaryCommand) -> None: - self.thrusters_dutycycle_cmd = np.array(msg.binary_command, dtype = float) + def update_thrusters_binary_cmd_cb(self, msg: ThrusterBinaryCommand) -> None: + self.thrusters_dutycycle_cmd = np.array(msg.switches, dtype=float) def update_state_init_cb(self, msg: FreeFlyerStateStamped) -> None: self.x_cur = np.array( @@ -377,7 +383,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 dfb7af40a9de7483dd26882a9356f9ea40f97d6f Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 9 Jan 2024 16:29:40 -0800 Subject: [PATCH 04/15] make sim run faster --- ff_sim/ff_sim/simulator_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index 7f227e9..433600e 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -140,7 +140,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 From 3442c3b852217c453bba660d12452f8d543e57b3 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 9 Jan 2024 20:01:01 -0800 Subject: [PATCH 05/15] black 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 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 433600e..eff835a 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -383,7 +383,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 71deb0e67b287520894798ecc33d792f5959ffe8 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 9 Jan 2024 20:18:27 -0800 Subject: [PATCH 06/15] also update cpp API --- ff_control/include/ff_control/ll_ctrl.hpp | 11 ++++++++++- ff_control/src/ll_ctrl.cpp | 14 ++++++++++++-- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/ff_control/include/ff_control/ll_ctrl.hpp b/ff_control/include/ff_control/ll_ctrl.hpp index 7bf37b6..fec42e4 100644 --- a/ff_control/include/ff_control/ll_ctrl.hpp +++ b/ff_control/include/ff_control/ll_ctrl.hpp @@ -29,6 +29,7 @@ #include #include "ff_msgs/msg/thruster_pwm_command.hpp" +#include "ff_msgs/msg/thruster_binary_command.hpp" #include "ff_msgs/msg/wheel_vel_command.hpp" #include "ff_params/robot_params.hpp" @@ -53,6 +54,13 @@ class LowLevelController : virtual public rclcpp::Node */ void SetThrustDutyCycle(const std::array & duty_cycle); + /** + * @brief send binary switching command to the thrusters + * + * @param switches boolean switches for each thruster (True is on, False is off) + */ + void SetBinaryThrust(const std::array & switches); + /** * @brief send command to set the inertial wheel velocity * @TODO(alvin): support this or remove? @@ -62,7 +70,8 @@ class LowLevelController : virtual public rclcpp::Node void SetWheelVelocity(const double & velocity); private: - rclcpp::Publisher::SharedPtr thruster_pub_; + rclcpp::Publisher::SharedPtr thrust_pwm_pub_; + rclcpp::Publisher::SharedPtr thrust_binary_pub_; rclcpp::Publisher::SharedPtr wheel_pub_; }; diff --git a/ff_control/src/ll_ctrl.cpp b/ff_control/src/ll_ctrl.cpp index 87937bf..fa9bc66 100644 --- a/ff_control/src/ll_ctrl.cpp +++ b/ff_control/src/ll_ctrl.cpp @@ -24,6 +24,7 @@ #include "ff_control/ll_ctrl.hpp" using ff_msgs::msg::ThrusterPWMCommand; +using ff_msgs::msg::ThrusterBinaryCommand; using ff_msgs::msg::WheelVelCommand; namespace ff @@ -33,7 +34,8 @@ LowLevelController::LowLevelController() : rclcpp::Node("ll_ctrl_node"), p_(this) { - thruster_pub_ = this->create_publisher("ctrl/duty_cycle", 10); + thrust_pwm_pub_ = this->create_publisher("ctrl/duty_cycle", 10); + thrust_binary_pub_ = this->create_publisher("ctrl/binary_command", 10); wheel_pub_ = this->create_publisher("ctrl/velocity", 10); } @@ -43,7 +45,15 @@ void LowLevelController::SetThrustDutyCycle(const std::array & duty_c msg.header.stamp = this->get_clock()->now(); msg.duty_cycles = duty_cycle; - thruster_pub_->publish(msg); + thrust_pwm_pub_->publish(msg); +} + +void LowLevelController::SetBinaryThrust(const std::array & switches) { + ThrusterBinaryCommand msg{}; + msg.header.stamp = this->get_clock()->now(); + msg.switches = switches; + + thrust_binary_pub_->publish(msg); } void LowLevelController::SetWheelVelocity(const double & velocity) From ed5889c3a9262f7219d0d6872bc0a35f54d3f6b0 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 9 Jan 2024 20:43:39 -0800 Subject: [PATCH 07/15] change binary thurst topic name --- ff_control/ff_control/ll_ctrl.py | 10 +++++----- ff_control/src/ll_ctrl.cpp | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ff_control/ff_control/ll_ctrl.py b/ff_control/ff_control/ll_ctrl.py index e538a03..e89690c 100644 --- a/ff_control/ff_control/ll_ctrl.py +++ b/ff_control/ff_control/ll_ctrl.py @@ -39,7 +39,7 @@ def __init__(self, node_name: str = "ll_ctrl_node") -> None: # low level thruster control publishers self._thruster_pwm_pub = self.create_publisher(ThrusterPWMCommand, "ctrl/duty_cycle", 10) self._thruster_binary_pub = self.create_publisher( - ThrusterBinaryCommand, "ctrl/binary_command", 10 + ThrusterBinaryCommand, "ctrl/binary_thrust", 10 ) self._wheel_pub = self.create_publisher(WheelVelCommand, "ctrl/velocity", 10) @@ -58,18 +58,18 @@ def set_thrust_duty_cycle(self, duty_cycle: np.ndarray) -> None: msg.duty_cycles = duty_cycle self._thruster_pwm_pub.publish(msg) - def set_thrust_binary(self, binary_command: T.Sequence[bool]) -> None: + def set_thrust_binary(self, switches: T.Sequence[bool]) -> None: """ Send command to set the thrusters binary output. - :param binary_command: binary switch for each thrust + :param switches: binary switch for each thrust """ - if len(binary_command) != len(ThrusterBinaryCommand().switches): + if len(switches) != len(ThrusterBinaryCommand().switches): self.get_logger().error("Incompatible thruster length sent.") return msg = ThrusterBinaryCommand() msg.header.stamp = self.get_clock().now().to_msg() - msg.switches = binary_command + msg.switches = switches self._thruster_binary_pub.publish(msg) def set_wheel_velocity(self, velocity: float) -> None: diff --git a/ff_control/src/ll_ctrl.cpp b/ff_control/src/ll_ctrl.cpp index fa9bc66..ddf56bb 100644 --- a/ff_control/src/ll_ctrl.cpp +++ b/ff_control/src/ll_ctrl.cpp @@ -35,7 +35,7 @@ LowLevelController::LowLevelController() p_(this) { thrust_pwm_pub_ = this->create_publisher("ctrl/duty_cycle", 10); - thrust_binary_pub_ = this->create_publisher("ctrl/binary_command", 10); + thrust_binary_pub_ = this->create_publisher("ctrl/binary_thrust", 10); wheel_pub_ = this->create_publisher("ctrl/velocity", 10); } From ea454d1ca704cd27541160e7161275a16c4ea0e0 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 9 Jan 2024 20:45:32 -0800 Subject: [PATCH 08/15] uncrusitify format --- ff_control/src/ll_ctrl.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ff_control/src/ll_ctrl.cpp b/ff_control/src/ll_ctrl.cpp index ddf56bb..bdb07dc 100644 --- a/ff_control/src/ll_ctrl.cpp +++ b/ff_control/src/ll_ctrl.cpp @@ -48,7 +48,8 @@ void LowLevelController::SetThrustDutyCycle(const std::array & duty_c thrust_pwm_pub_->publish(msg); } -void LowLevelController::SetBinaryThrust(const std::array & switches) { +void LowLevelController::SetBinaryThrust(const std::array & switches) +{ ThrusterBinaryCommand msg{}; msg.header.stamp = this->get_clock()->now(); msg.switches = switches; From 08afea83bdf547097bec0afee4409b8f14d51105 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 9 Jan 2024 20:54:40 -0800 Subject: [PATCH 09/15] remove pwm from ff_sim --- ff_control/scripts/safety_filter | 4 ++-- ff_sim/ff_sim/simulator_node.py | 29 +++++++++++------------------ 2 files changed, 13 insertions(+), 20 deletions(-) diff --git a/ff_control/scripts/safety_filter b/ff_control/scripts/safety_filter index 6796628..f33f106 100755 --- a/ff_control/scripts/safety_filter +++ b/ff_control/scripts/safety_filter @@ -47,7 +47,7 @@ class SafetyFilter(Node): # Create publishers for thruster and wheel commands self.thrust_pub = self.create_publisher(ThrusterPWMCommand, 'commands/duty_cycle', 10) self.wheel_pub = self.create_publisher(WheelVelCommand, 'commands/velocity', 10) - self.bool_thrust_pub = self.create_publisher(ThrusterBinaryCommand, 'commands/binary', 10) + self.bool_thrust_pub = self.create_publisher(ThrusterBinaryCommand, 'commands/thrust_binary', 10) # Create subscribers for thruster and wheel commands self.thrust_sub = self.create_subscription( @@ -62,7 +62,7 @@ class SafetyFilter(Node): 10) self.bool_thrust_sub = self.create_subscription( ThrusterBinaryCommand, - 'ctrl/binary', + 'ctrl/thrust_binary', self.bool_thrust_callback, 10) diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index eff835a..9b65e1e 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 ----------------------------------------------- """ @@ -155,7 +155,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) @@ -165,11 +165,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( - ThrusterPWMCommand, "commands/duty_cycle", self.update_thrusters_dutycycle_cmd_cb, 10 - ) self.sub_thrusters_cmd_binary = self.create_subscription( - ThrusterBinaryCommand, "commands/binary", self.update_thrusters_binary_cmd_cb, 10 + ThrusterBinaryCommand, "commands/thrust_binary", self.update_thrusters_binary_cmd_cb, 10 ) self.sub_state_init = self.create_subscription( FreeFlyerStateStamped, "state_init", self.update_state_init_cb, 10 @@ -194,13 +191,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() @@ -266,12 +263,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: ThrusterPWMCommand) -> None: - # Saturate controls so within [0,1] (in %) - self.thrusters_dutycycle_cmd = np.clip(msg.duty_cycles, 0.0, 1.0) - - def update_thrusters_binary_cmd_cb(self, msg: ThrusterBinaryCommand) -> None: - self.thrusters_dutycycle_cmd = np.array(msg.switches, dtype=float) + def update_thrusters_cb(self, msg: ThrusterBinaryCommand) -> None: + self.thrusters = np.array(msg.switches, dtype=float) def update_state_init_cb(self, msg: FreeFlyerStateStamped) -> None: self.x_cur = np.array( @@ -285,7 +278,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) ___ # <-- ^ --> / \ @@ -302,7 +295,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])) From 5a5bd28c4eb28587a0c31ca53ed50d379b468ea1 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 9 Jan 2024 22:59:10 -0800 Subject: [PATCH 10/15] move pwm ctrl above ll binary ctrl --- ff_control/CMakeLists.txt | 8 ++- ff_control/include/ff_control/ll_ctrl.hpp | 28 +++++---- ff_control/include/ff_control/pwm_ctrl.hpp | 59 +++++++++++++++++++ ff_control/include/ff_control/wrench_ctrl.hpp | 4 +- ff_control/src/ll_ctrl.cpp | 34 ++++++----- ff_control/src/pwm_ctrl.cpp | 49 +++++++++++++++ ff_control/src/wrench_ctrl.cpp | 2 +- ff_drivers/CMakeLists.txt | 13 ++-- ff_drivers/package.xml | 3 +- ff_drivers/src/tests/test_all_thrusters.cpp | 15 ++--- ff_msgs/CMakeLists.txt | 3 +- ...rBinaryCommand.msg => ThrusterCommand.msg} | 0 ff_msgs/msg/ThrusterPWMCommand.msg | 2 - 13 files changed, 171 insertions(+), 49 deletions(-) create mode 100644 ff_control/include/ff_control/pwm_ctrl.hpp create mode 100644 ff_control/src/pwm_ctrl.cpp rename ff_msgs/msg/{ThrusterBinaryCommand.msg => ThrusterCommand.msg} (100%) delete mode 100644 ff_msgs/msg/ThrusterPWMCommand.msg diff --git a/ff_control/CMakeLists.txt b/ff_control/CMakeLists.txt index 3f9408a..4ed29cb 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 $ $) diff --git a/ff_control/include/ff_control/ll_ctrl.hpp b/ff_control/include/ff_control/ll_ctrl.hpp index fec42e4..b4ffbaf 100644 --- a/ff_control/include/ff_control/ll_ctrl.hpp +++ b/ff_control/include/ff_control/ll_ctrl.hpp @@ -24,12 +24,10 @@ #pragma once #include -#include #include -#include "ff_msgs/msg/thruster_pwm_command.hpp" -#include "ff_msgs/msg/thruster_binary_command.hpp" +#include "ff_msgs/msg/thruster_command.hpp" #include "ff_msgs/msg/wheel_vel_command.hpp" #include "ff_params/robot_params.hpp" @@ -48,18 +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 send binary switching command to the thrusters + * @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 * - * @param switches boolean switches for each thruster (True is on, False is off) + * @return boolean states for each thruster */ - void SetBinaryThrust(const std::array & switches); + const std::array & GetThrust() const; /** * @brief send command to set the inertial wheel velocity @@ -70,9 +76,9 @@ class LowLevelController : virtual public rclcpp::Node void SetWheelVelocity(const double & velocity); private: - rclcpp::Publisher::SharedPtr thrust_pwm_pub_; - rclcpp::Publisher::SharedPtr thrust_binary_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/src/ll_ctrl.cpp b/ff_control/src/ll_ctrl.cpp index bdb07dc..2ae12d0 100644 --- a/ff_control/src/ll_ctrl.cpp +++ b/ff_control/src/ll_ctrl.cpp @@ -23,8 +23,7 @@ #include "ff_control/ll_ctrl.hpp" -using ff_msgs::msg::ThrusterPWMCommand; -using ff_msgs::msg::ThrusterBinaryCommand; +using ff_msgs::msg::ThrusterCommand; using ff_msgs::msg::WheelVelCommand; namespace ff @@ -34,27 +33,34 @@ LowLevelController::LowLevelController() : rclcpp::Node("ll_ctrl_node"), p_(this) { - thrust_pwm_pub_ = this->create_publisher("ctrl/duty_cycle", 10); - thrust_binary_pub_ = this->create_publisher("ctrl/binary_thrust", 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) { - ThrusterPWMCommand msg{}; - msg.header.stamp = this->get_clock()->now(); - msg.duty_cycles = duty_cycle; + thrust_cmd_.header.stamp = this->get_clock()->now(); + thrust_cmd_.switches = thrusts; - thrust_pwm_pub_->publish(msg); + thrust_pub_->publish(thrust_cmd_); } -void LowLevelController::SetBinaryThrust(const std::array & switches) +void LowLevelController::SetThrust(size_t idx, bool on) { - ThrusterBinaryCommand msg{}; - msg.header.stamp = this->get_clock()->now(); - msg.switches = switches; + if (idx > thrust_cmd_.switches.size()) { + RCLCPP_ERROR(this->get_logger(), "thrust index out of range"); + return; + } + + thrust_cmd_.header.stamp = this->get_clock()->now(); + thrust_cmd_.switches[idx] = on; - thrust_binary_pub_->publish(msg); + 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/pwm_ctrl.cpp b/ff_control/src/pwm_ctrl.cpp new file mode 100644 index 0000000..059bc82 --- /dev/null +++ b/ff_control/src/pwm_ctrl.cpp @@ -0,0 +1,49 @@ +#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 3491469..ac86c2d 100644 --- a/ff_drivers/src/tests/test_all_thrusters.cpp +++ b/ff_drivers/src/tests/test_all_thrusters.cpp @@ -25,25 +25,22 @@ #include -#include "ff_msgs/msg/thruster_pwm_command.hpp" +#include "ff_control/pwm_ctrl.hpp" using namespace std::chrono_literals; -using ff_msgs::msg::ThrusterPWMCommand; -class TestAllThrustersNode : public rclcpp::Node +class TestAllThrustersNode : public ff::PWMController { public: TestAllThrustersNode() : rclcpp::Node("test_all_thrusters_node") { - 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 +48,17 @@ class TestAllThrustersNode : public rclcpp::Node { double duty_cycle = this->get_parameter("duty_cycle").as_double(); + std::array duty_cycles; // populate thrust msg - ThrusterPWMCommand msg{}; for (int i = 0; i < 8; ++i) { - msg.duty_cycles[i] = 0.; + duty_cycles[i] = 0.; if (i == th_idx_) { - msg.duty_cycles[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_msgs/CMakeLists.txt b/ff_msgs/CMakeLists.txt index f01b54a..e6bc241 100644 --- a/ff_msgs/CMakeLists.txt +++ b/ff_msgs/CMakeLists.txt @@ -19,9 +19,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Wrench2DStamped.msg" "msg/FreeFlyerState.msg" "msg/FreeFlyerStateStamped.msg" - "msg/ThrusterPWMCommand.msg" "msg/WheelVelCommand.msg" - "msg/ThrusterBinaryCommand.msg" + "msg/ThrusterCommand.msg" DEPENDENCIES std_msgs ) diff --git a/ff_msgs/msg/ThrusterBinaryCommand.msg b/ff_msgs/msg/ThrusterCommand.msg similarity index 100% rename from ff_msgs/msg/ThrusterBinaryCommand.msg rename to ff_msgs/msg/ThrusterCommand.msg diff --git a/ff_msgs/msg/ThrusterPWMCommand.msg b/ff_msgs/msg/ThrusterPWMCommand.msg deleted file mode 100644 index 31cd9e5..0000000 --- a/ff_msgs/msg/ThrusterPWMCommand.msg +++ /dev/null @@ -1,2 +0,0 @@ -std_msgs/Header header -float64[8] duty_cycles From 57ee21465002cc16e2d78631b3691d8241b94380 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 9 Jan 2024 23:09:19 -0800 Subject: [PATCH 11/15] cpp pd sim working --- ff_control/scripts/safety_filter | 34 ++++++++++---------------------- ff_sim/ff_sim/simulator_node.py | 7 +++---- 2 files changed, 13 insertions(+), 28 deletions(-) diff --git a/ff_control/scripts/safety_filter b/ff_control/scripts/safety_filter index f33f106..ae86307 100755 --- a/ff_control/scripts/safety_filter +++ b/ff_control/scripts/safety_filter @@ -29,9 +29,7 @@ from rclpy.duration import Duration import numpy as np import time -from ff_msgs.msg import ThrusterPWMCommand -from ff_msgs.msg import WheelVelCommand -from ff_msgs.msg import ThrusterBinaryCommand +from ff_msgs.msg import ThrusterCommand, WheelVelCommand from std_msgs.msg import Bool class SafetyFilter(Node): @@ -45,25 +43,19 @@ class SafetyFilter(Node): self.kill_state = False # Create publishers for thruster and wheel commands - self.thrust_pub = self.create_publisher(ThrusterPWMCommand, 'commands/duty_cycle', 10) self.wheel_pub = self.create_publisher(WheelVelCommand, 'commands/velocity', 10) - self.bool_thrust_pub = self.create_publisher(ThrusterBinaryCommand, 'commands/thrust_binary', 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( - ThrusterPWMCommand, - 'ctrl/duty_cycle', - self.thrust_callback, - 10) self.wheel_sub = self.create_subscription( WheelVelCommand, 'ctrl/velocity', self.wheel_callback, 10) - self.bool_thrust_sub = self.create_subscription( - ThrusterBinaryCommand, - 'ctrl/thrust_binary', - self.bool_thrust_callback, + 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 @@ -102,12 +94,12 @@ class SafetyFilter(Node): # Store time last message was published self.last_thrust_time = self.get_clock().now() - def bool_thrust_callback(self, msg): + 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.bool_thrust_pub.publish(msg) + self.thrust_pub.publish(msg) # Store time last message was published self.last_thrust_time = self.get_clock().now() @@ -121,17 +113,11 @@ class SafetyFilter(Node): self.last_wheel_time = self.get_clock().now() def send_zero_thrust(self): - zero_thrust_msg = ThrusterPWMCommand() + zero_thrust_msg = ThrusterCommand() zero_thrust_msg.header.stamp = self.get_clock().now().to_msg() - zero_thrust_msg.duty_cycles = np.zeros(8) - - zero_thrust_binary_msg = ThrusterBinaryCommand() - zero_thrust_binary_msg.header.stamp = self.get_clock().now().to_msg() - zero_thrust_binary_msg.switches = [False] * 8 + zero_thrust_msg.switches = [False] * 8 self.thrust_pub.publish(zero_thrust_msg) - self.bool_thrust_pub.publish(zero_thrust_binary_msg) - def send_zero_wheel(self): zero_wheel_msg = WheelVelCommand() diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index 9b65e1e..7503257 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -41,9 +41,8 @@ from ff_msgs.msg import ( FreeFlyerStateStamped, Wrench2DStamped, - ThrusterPWMCommand, WheelVelCommand, - ThrusterBinaryCommand, + ThrusterCommand, ) from ff_params import RobotParams @@ -166,7 +165,7 @@ def __init__(self): WheelVelCommand, "commands/velocity", self.update_wheel_cmd_vel_cb, 10 ) self.sub_thrusters_cmd_binary = self.create_subscription( - ThrusterBinaryCommand, "commands/thrust_binary", self.update_thrusters_binary_cmd_cb, 10 + 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 @@ -263,7 +262,7 @@ def sim_loop(self) -> None: def update_wheel_cmd_vel_cb(self, msg: WheelVelCommand) -> None: self.wheel_vel_cmd = msg.velocity - def update_thrusters_cb(self, msg: ThrusterBinaryCommand) -> None: + 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: From 7c3d9777285903cde5dca38be633d419f5330222 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 9 Jan 2024 23:55:23 -0800 Subject: [PATCH 12/15] all test passes --- ff_control/CMakeLists.txt | 5 ++- ff_control/ff_control/ll_ctrl.py | 11 +++-- ff_control/src/nodes/pwm_ctrl_node.cpp | 62 ++++++++++++++++++++++++++ ff_control/src/pwm_ctrl.cpp | 37 ++++++++++++--- ff_msgs/CMakeLists.txt | 1 + ff_msgs/msg/ThrusterPWMCommand.msg | 2 + freeflyer/launch/sim_pd.launch.py | 10 ++++- freeflyer/test/pd_ctrl_launch_test.py | 11 ++++- 8 files changed, 125 insertions(+), 14 deletions(-) create mode 100644 ff_control/src/nodes/pwm_ctrl_node.cpp create mode 100644 ff_msgs/msg/ThrusterPWMCommand.msg diff --git a/ff_control/CMakeLists.txt b/ff_control/CMakeLists.txt index 4ed29cb..0213d44 100644 --- a/ff_control/CMakeLists.txt +++ b/ff_control/CMakeLists.txt @@ -48,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) @@ -59,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 e89690c..bbae560 100644 --- a/ff_control/ff_control/ll_ctrl.py +++ b/ff_control/ff_control/ll_ctrl.py @@ -23,7 +23,7 @@ import numpy as np import typing as T -from ff_msgs.msg import ThrusterPWMCommand, ThrusterBinaryCommand +from ff_msgs.msg import ThrusterPWMCommand, ThrusterCommand from ff_msgs.msg import WheelVelCommand from ff_params import RobotParams from rclpy.node import Node @@ -38,9 +38,8 @@ def __init__(self, node_name: str = "ll_ctrl_node") -> None: # low level thruster control publishers self._thruster_pwm_pub = self.create_publisher(ThrusterPWMCommand, "ctrl/duty_cycle", 10) - self._thruster_binary_pub = self.create_publisher( - ThrusterBinaryCommand, "ctrl/binary_thrust", 10 - ) + 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) @@ -64,10 +63,10 @@ def set_thrust_binary(self, switches: T.Sequence[bool]) -> None: :param switches: binary switch for each thrust """ - if len(switches) != len(ThrusterBinaryCommand().switches): + if len(switches) != len(ThrusterCommand().switches): self.get_logger().error("Incompatible thruster length sent.") return - msg = ThrusterBinaryCommand() + msg = ThrusterCommand() msg.header.stamp = self.get_clock().now().to_msg() msg.switches = switches self._thruster_binary_pub.publish(msg) 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 index 059bc82..3fe83e3 100644 --- a/ff_control/src/pwm_ctrl.cpp +++ b/ff_control/src/pwm_ctrl.cpp @@ -1,12 +1,37 @@ +// 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 { +namespace ff +{ PWMController::PWMController() - : rclcpp::Node("pwm_ctrl_node"), - period_(1.0s / this->declare_parameter("pwm_frequency", 10.0)) { +: 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)); } @@ -15,7 +40,8 @@ void PWMController::SetThrustDutyCycle(const std::array & duty_cycle) duty_cycle_ = duty_cycle; } -void PWMController::PeriodCallback() { +void PWMController::PeriodCallback() +{ std::array thrusts = {0}; for (size_t i = 0; i < thrusts.size(); ++i) { @@ -31,7 +57,8 @@ void PWMController::PeriodCallback() { SetAllThrusts(thrusts); } -void PWMController::SwitchCallback(size_t idx) { +void PWMController::SwitchCallback(size_t idx) +{ if (idx >= duty_cycle_.size()) { RCLCPP_ERROR(this->get_logger(), "PWM idx out of range"); return; diff --git a/ff_msgs/CMakeLists.txt b/ff_msgs/CMakeLists.txt index e6bc241..a889b37 100644 --- a/ff_msgs/CMakeLists.txt +++ b/ff_msgs/CMakeLists.txt @@ -21,6 +21,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/FreeFlyerStateStamped.msg" "msg/WheelVelCommand.msg" "msg/ThrusterCommand.msg" + "msg/ThrusterPWMCommand.msg" DEPENDENCIES std_msgs ) 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/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(), ] From 827f3480b4f57e4a7b0a32eef9518d898522d6cb Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Tue, 9 Jan 2024 23:56:45 -0800 Subject: [PATCH 13/15] add doc --- ff_control/ff_control/ll_ctrl.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ff_control/ff_control/ll_ctrl.py b/ff_control/ff_control/ll_ctrl.py index bbae560..354c922 100644 --- a/ff_control/ff_control/ll_ctrl.py +++ b/ff_control/ff_control/ll_ctrl.py @@ -48,6 +48,8 @@ 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.") From 6355257c3160803e87c379424d8cad6242429aa5 Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 10 Jan 2024 12:09:37 -0800 Subject: [PATCH 14/15] remove unused variable --- ff_control/ff_control/ll_ctrl.py | 1 - 1 file changed, 1 deletion(-) diff --git a/ff_control/ff_control/ll_ctrl.py b/ff_control/ff_control/ll_ctrl.py index 354c922..0700f42 100644 --- a/ff_control/ff_control/ll_ctrl.py +++ b/ff_control/ff_control/ll_ctrl.py @@ -37,7 +37,6 @@ def __init__(self, node_name: str = "ll_ctrl_node") -> None: self.p = RobotParams(self) # low level thruster control publishers - self._thruster_pwm_pub = self.create_publisher(ThrusterPWMCommand, "ctrl/duty_cycle", 10) self._thruster_binary_pub = self.create_publisher(ThrusterCommand, "ctrl/binary_thrust", 10) self._thruster_pwm_pub = self.create_publisher(ThrusterPWMCommand, "ctrl/pwm_thrust", 10) From c4c455a2fe0a8c38f612fe6742e448dafdb861ba Mon Sep 17 00:00:00 2001 From: Alvin Sun Date: Wed, 10 Jan 2024 12:20:28 -0800 Subject: [PATCH 15/15] fix constructor --- ff_drivers/src/tests/test_all_thrusters.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ff_drivers/src/tests/test_all_thrusters.cpp b/ff_drivers/src/tests/test_all_thrusters.cpp index ac86c2d..1540968 100644 --- a/ff_drivers/src/tests/test_all_thrusters.cpp +++ b/ff_drivers/src/tests/test_all_thrusters.cpp @@ -34,7 +34,8 @@ class TestAllThrustersNode : public ff::PWMController { public: TestAllThrustersNode() - : rclcpp::Node("test_all_thrusters_node") + : rclcpp::Node("test_all_thrusters_node"), + ff::PWMController() { timer_ = this->create_wall_timer(5s, std::bind(&TestAllThrustersNode::TimerCallback, this)); this->declare_parameter("duty_cycle", .2);