Merge pull request #21 from StanfordASL/alvin/thruster_ctrl

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
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
@@ -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

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,23 +36,41 @@ 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:
         """
         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:
         """

ff_control/include/ff_control/ll_ctrl.hpp

@@ -24,7 +24,6 @@
 #pragma once
 
 #include <array>
-#include <string>
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -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<double, 8> & duty_cycle);
+  void SetAllThrusts(const std::array<bool, 8> & 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<bool, 8> & 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<ff_msgs::msg::ThrusterCommand>::SharedPtr thruster_pub_;
+  rclcpp::Publisher<ff_msgs::msg::ThrusterCommand>::SharedPtr thrust_pub_;
   rclcpp::Publisher<ff_msgs::msg::WheelVelCommand>::SharedPtr wheel_pub_;
+  ff_msgs::msg::ThrusterCommand thrust_cmd_{};
 };
 
 }  // namespace ff

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 <array>
+#include <chrono>
+
+#include <rclcpp/rclcpp.hpp>
+
+#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<double, 8> & duty_cycle);
+
+private:
+  std::array<double, 8> duty_cycle_ = {0};
+  std::array<rclcpp::TimerBase::SharedPtr, 8> switch_timers_;
+  std::chrono::duration<double> period_ = std::chrono::seconds(0);
+  rclcpp::TimerBase::SharedPtr period_timer_;
+
+  void PeriodCallback();
+  void SwitchCallback(size_t idx);
+};
+
+}  // namespace ff

ff_control/include/ff_control/wrench_ctrl.hpp

@@ -25,14 +25,14 @@
 
 #include <string>
 
-#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();

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

ff_control/src/ll_ctrl.cpp

@@ -33,17 +33,34 @@ LowLevelController::LowLevelController()
 : rclcpp::Node("ll_ctrl_node"),
   p_(this)
 {
-  thruster_pub_ = this->create_publisher<ThrusterCommand>("ctrl/duty_cycle", 10);
+  thrust_pub_ = this->create_publisher<ThrusterCommand>("ctrl/binary_thrust", 10);
   wheel_pub_ = this->create_publisher<WheelVelCommand>("ctrl/velocity", 10);
 }
 
-void LowLevelController::SetThrustDutyCycle(const std::array<double, 8> & duty_cycle)
+void LowLevelController::SetAllThrusts(const std::array<bool, 8> & 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<bool, 8> & LowLevelController::GetThrust() const
+{
+  return thrust_cmd_.switches;
 }
 
 void LowLevelController::SetWheelVelocity(const double & velocity)

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 <atomic>
+#include <functional>
+#include <memory>
+
+#include <rclcpp/rclcpp.hpp>
+
+#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<ThrusterPWMCommand>(
+      "ctrl/pwm_thrust", 10, std::bind(&PWMControlNode::PWMThrustCallback, this, _1));
+  }
+
+private:
+  rclcpp::Subscription<ThrusterPWMCommand>::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<PWMControlNode>());
+  rclcpp::shutdown();
+  return 0;
+}