Chassis follow mode and hardware support for DJI and MI motors

.github/workflows/ros-humble.yml

@@ -15,6 +15,10 @@ jobs:
     - uses: actions/checkout@v4
       with:
         submodules: recursive
+    - name: Upgrade cmake
+      uses: jwlawson/actions-setup-cmake@v2
+      with:
+        cmake-version: '3.23.x'
     - name: Ignore scara_moveit
       run: touch decomposition/scara_moveit/AMENT_IGNORE
     - name: Install dependencies

.gitmodules

@@ -14,3 +14,6 @@
 [submodule "decomposition/wheel_leg_rl"]
 	path = decomposition/wheel_leg_rl
 	url = https://github.com/Yao-Xinchen/Meta-WL
+[submodule "execution/metav_hardware/libsockcanpp"]
+	path = execution/metav_hardware/libsockcanpp
+	url = https://github.com/SimonCahill/libsockcanpp.git

decomposition/gimbal_controller/include/gimbal_controller/gimbal_controller.hpp

@@ -30,95 +30,92 @@ static constexpr size_t STATE_MY_ITFS = 0;
 static constexpr size_t CMD_MY_ITFS = 0;
 
 enum class gimbal_role : std::uint8_t {
-  PITCH = 0,
-  YAW = 1,
+    PITCH = 0,
+    YAW = 1,
 };
 
 class GimbalController
     : public controller_interface::ChainableControllerInterface {
-public:
-  GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
-  GimbalController();
-
-  GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
-  controller_interface::CallbackReturn on_init() override;
-
-  GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
-  controller_interface::InterfaceConfiguration
-  command_interface_configuration() const override;
-
-  GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
-  controller_interface::InterfaceConfiguration
-  state_interface_configuration() const override;
-
-  GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
-  controller_interface::CallbackReturn
-  on_configure(const rclcpp_lifecycle::State &previous_state) override;
-
-  GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
-  controller_interface::CallbackReturn
-  on_activate(const rclcpp_lifecycle::State &previous_state) override;
-
-  GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
-  controller_interface::CallbackReturn
-  on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
-
-  GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
-  controller_interface::return_type
-  update_reference_from_subscribers() override;
-
-  GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
-  controller_interface::return_type
-  update_and_write_commands(const rclcpp::Time &time,
-                            const rclcpp::Duration &period) override;
-
-  using ControllerFeedbackMsg = sensor_msgs::msg::Imu;
-  using ControllerReferenceMsg = behavior_interface::msg::Aim;
-  using ControllerModeSrvType = std_srvs::srv::SetBool;
-  using ControllerStateMsg = control_msgs::msg::MultiDOFStateStamped;
-
-protected:
-  std::shared_ptr<gimbal_controller::ParamListener> param_listener_;
-  gimbal_controller::Params params_;
-
-  std::vector<std::string> state_joints_;
-
-  // Command subscribers
-  rclcpp::Duration ref_timeout_ = rclcpp::Duration(0, 0);
-  rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr
-      ref_subscriber_ = nullptr;
-  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>>
-      input_ref_;
-
-  // Feedback subscribers
-  rclcpp::Subscription<ControllerFeedbackMsg>::SharedPtr feedback_subscriber_ =
-      nullptr;
-  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerFeedbackMsg>>
-      input_feedback_;
-
-  std::vector<std::shared_ptr<control_toolbox::PidROS>> pos2vel_pids_;
-  std::vector<std::shared_ptr<control_toolbox::PidROS>> vel2eff_pids_;
-
-  using ControllerStatePublisher =
-      realtime_tools::RealtimePublisher<ControllerStateMsg>;
-
-  rclcpp::Publisher<ControllerStateMsg>::SharedPtr s_publisher_;
-  std::unique_ptr<ControllerStatePublisher> state_publisher_;
-
-  // override methods from ChainableControllerInterface
-  std::vector<hardware_interface::CommandInterface>
-  on_export_reference_interfaces() override;
-
-  bool on_set_chained_mode(bool chained_mode) override;
-
-  std::vector<gimbal_role> gimbal_roles_;
-
-private:
-  // callback for topic interface
-  GIMBAL_CONTROLLER__VISIBILITY_LOCAL
-  void reference_callback(
-      const std::shared_ptr<ControllerReferenceMsg> msg);
-  void feedback_callback(const std::shared_ptr<ControllerFeedbackMsg> msg);
+  public:
+    GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
+    GimbalController();
+
+    GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
+    controller_interface::CallbackReturn on_init() override;
+
+    GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
+    controller_interface::InterfaceConfiguration
+    command_interface_configuration() const override;
+
+    GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
+    controller_interface::InterfaceConfiguration
+    state_interface_configuration() const override;
+
+    GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
+    controller_interface::CallbackReturn
+    on_configure(const rclcpp_lifecycle::State &previous_state) override;
+
+    GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
+    controller_interface::CallbackReturn
+    on_activate(const rclcpp_lifecycle::State &previous_state) override;
+
+    GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
+    controller_interface::CallbackReturn
+    on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
+
+    GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
+    controller_interface::return_type
+    update_reference_from_subscribers() override;
+
+    GIMBAL_CONTROLLER__VISIBILITY_PUBLIC
+    controller_interface::return_type
+    update_and_write_commands(const rclcpp::Time &time,
+                              const rclcpp::Duration &period) override;
+
+    using ControllerFeedbackMsg = sensor_msgs::msg::Imu;
+    using ControllerReferenceMsg = behavior_interface::msg::Aim;
+    using ControllerModeSrvType = std_srvs::srv::SetBool;
+    using ControllerStateMsg = control_msgs::msg::MultiDOFStateStamped;
+
+  protected:
+    std::shared_ptr<gimbal_controller::ParamListener> param_listener_;
+    gimbal_controller::Params params_;
+
+    // Command subscribers
+    rclcpp::Duration ref_timeout_ = rclcpp::Duration(0, 0);
+    rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ =
+        nullptr;
+    realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>>
+        input_ref_;
+
+    // Feedback subscribers
+    rclcpp::Subscription<ControllerFeedbackMsg>::SharedPtr
+        feedback_subscriber_ = nullptr;
+    realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerFeedbackMsg>>
+        input_feedback_;
+
+    std::vector<std::shared_ptr<control_toolbox::PidROS>> pos2vel_pids_;
+    std::vector<std::shared_ptr<control_toolbox::PidROS>> vel2eff_pids_;
+
+    using ControllerStatePublisher =
+        realtime_tools::RealtimePublisher<ControllerStateMsg>;
+
+    rclcpp::Publisher<ControllerStateMsg>::SharedPtr s_publisher_;
+    std::unique_ptr<ControllerStatePublisher> state_publisher_;
+
+    // override methods from ChainableControllerInterface
+    std::vector<hardware_interface::CommandInterface>
+    on_export_reference_interfaces() override;
+
+    bool on_set_chained_mode(bool chained_mode) override;
+
+    std::vector<gimbal_role> gimbal_roles_;
+
+  private:
+    // callback for topic interface
+    GIMBAL_CONTROLLER__VISIBILITY_LOCAL
+    void reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg);
+    void feedback_callback(const std::shared_ptr<ControllerFeedbackMsg> msg);
 };
 
 } // namespace gimbal_controller