Update standard file.

rm_chassis_controllers/CMakeLists.txt

@@ -28,7 +28,6 @@ find_package(Eigen3 REQUIRED)
 
 generate_dynamic_reconfigure_options(
   cfg/LQRWeight.cfg
-  cfg/PowerLimit.cfg
 )
 
 ###################################

rm_chassis_controllers/README.md

@@ -119,6 +119,10 @@ sudo rosdep install --from-paths src
 
   Allowed period (in s) between two commands. If the time is exceed this period, the speed of chassis will be set 0.
 
+* **`raw_yaw_feedforward_k`** (double, default: 0.0)
+
+  Yaw feedforward time constant in seconds. Only used in `raw` mode, where the command-frame rotation uses `yaw + k * w` to compensate direction-change lag.
+
 * **`power_offset`** (double)
 
   Fix the difference between theoretical power and actual power.
@@ -245,6 +249,7 @@ sudo rosdep install --from-paths src
       power_offset: -8.41
     twist_angular: 0.5233
     timeout: 0.1
+    raw_yaw_feedforward_k: 0.0
     pid_follow: { p: 5.0, i: 0, d: 0.3, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true }
     twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ]
 

rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h

@@ -48,7 +48,6 @@
 #include <geometry_msgs/Vector3Stamped.h>
 #include <nav_msgs/Odometry.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <rm_chassis_controllers/PowerLimitConfig.h>
 #include <rm_common/ros_utilities.h>
 #include <rm_common/math_utilities.h>
 #include <rm_common/ori_tool.h>
@@ -153,11 +152,12 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
   /** @brief Set chassis velocity to zero.
    */
   void recovery();
+  void fallen();
   /** @brief Transform tf velocity to base link frame.
    *
    * @param from The father frame.
    */
-  void tfVelToBase(const std::string& from);
+  void tfVelToBase(const std::string& from, double yaw_offset = 0.);
   /** @brief To limit the chassis power according to current power limit.
    *
    * Receive power limit from command. Set max_effort command to chassis to avoid exceed power limit.
@@ -182,7 +182,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
 
   rm_control::RobotStateHandle robot_state_handle_{};
   hardware_interface::EffortJointInterface* effort_joint_interface_{};
-  std::vector<hardware_interface::JointHandle> joint_handles_{};
+  std::vector<hardware_interface::JointHandle> wheel_joint_handles_{};
   realtime_tools::RealtimeBuffer<Command> cmd_rt_buffer_{};
   realtime_tools::RealtimeBuffer<nav_msgs::Odometry> slam_rt_buffer_{};
   realtime_tools::RealtimeBuffer<geometry_msgs::TransformStamped> localization_rt_buffer_{};
@@ -228,6 +228,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
   double twist_angular_{ M_PI / 6 };
   double max_odom_vel_{ 10.0 };
   double timeout_{ 0.1 };
+  double raw_yaw_feedforward_k_{ 0.0 };
   double chassis_power_{ 0.0 };
   bool capacity_update_flag_{ false };
   bool use_rls_{ false };
@@ -260,9 +261,10 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
 
   enum
   {
-    RAW,
-    FOLLOW,
-    TWIST
+    RAW = 0,
+    FOLLOW = 1,
+    TWIST = 2,
+    FALLEN = 4
   };
 };
 

rm_chassis_controllers/include/rm_chassis_controllers/omni.h

@@ -5,6 +5,8 @@
 #pragma once
 
 #include <Eigen/Dense>
+#include <rm_common/filters/lp_filter.h>
+#include <rm_common/rls.h>
 
 #include "rm_chassis_controllers/chassis_base.h"
 
@@ -16,14 +18,20 @@ class OmniController : public ChassisBase<rm_control::RobotStateInterface, hardw
   OmniController() = default;
   bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;
 
+protected:
+  void moveJoint(const ros::Time& time, const ros::Duration& period) override;
+
 private:
   geometry_msgs::Twist odometry() override;
 
   std::vector<std::shared_ptr<effort_controllers::JointVelocityController>> joints_;
   Eigen::MatrixXd chassis2joints_;
+  void powerLimit() override;
+  void updatePowerStatus() override;
+  virtual void stateJudge();
 
-protected:
-  void moveJoint(const ros::Time& time, const ros::Duration& period) override;
+  std::array<LowPassFilter*, 4> motor_lp_filters_{};
+  std::unique_ptr<Rls<double>> rls_{};
 };
 
 }  // namespace rm_chassis_controllers

rm_chassis_controllers/include/rm_chassis_controllers/swerve.h

@@ -39,8 +39,15 @@
 
 #include "rm_chassis_controllers/chassis_base.h"
 
+#include <rm_common/rls.h>
+#include <rm_msgs/PowerManagementSampleAndStatusData.h>
 #include <rm_common/eigen_types.h>
 #include <effort_controllers/joint_position_controller.h>
+#include <rm_common/filters/lp_filter.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <array>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 namespace rm_chassis_controllers
 {
@@ -52,7 +59,8 @@ struct Module
   effort_controllers::JointVelocityController* ctrl_wheel_;
 };
 
-class SwerveController : public ChassisBase<rm_control::RobotStateInterface, hardware_interface::EffortJointInterface>
+class SwerveController : public ChassisBase<rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface,
+                                            hardware_interface::EffortJointInterface>
 {
 public:
   SwerveController() = default;
@@ -61,7 +69,18 @@ class SwerveController : public ChassisBase<rm_control::RobotStateInterface, har
 private:
   void moveJoint(const ros::Time& time, const ros::Duration& period) override;
   geometry_msgs::Twist odometry() override;
-  std::vector<Module> modules_;
+  void powerLimit() override;
+  void updatePowerStatus() override;
+  void getBaseGyro();
+  void stateJudge();
+
+  std::vector<Module> modules_{};
+  std::vector<hardware_interface::JointHandle> pivot_joint_handles_{};
+  std::unique_ptr<realtime_tools::RealtimePublisher<geometry_msgs::Vector3Stamped>> base_gyro_pub_;
+
+  PowerLimitor pivot_power_limitor_{};
+  std::array<std::array<LowPassFilter*, 4>, 2> motor_lp_filters_{};
+  std::unique_ptr<Rls<double>> rls_{};
 };
 
 }  // namespace rm_chassis_controllers

rm_chassis_controllers/src/balance.cpp

@@ -32,8 +32,8 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan
   }
   left_wheel_joint_handle_ = robot_hw->get<hardware_interface::EffortJointInterface>()->getHandle(left_wheel_joint);
   right_wheel_joint_handle_ = robot_hw->get<hardware_interface::EffortJointInterface>()->getHandle(right_wheel_joint);
-  joint_handles_.push_back(left_wheel_joint_handle_);
-  joint_handles_.push_back(right_wheel_joint_handle_);
+  wheel_joint_handles_.push_back(left_wheel_joint_handle_);
+  wheel_joint_handles_.push_back(right_wheel_joint_handle_);
   left_momentum_block_joint_handle_ =
       robot_hw->get<hardware_interface::EffortJointInterface>()->getHandle(left_momentum_block_joint);
   right_momentum_block_joint_handle_ =

rm_chassis_controllers/src/chassis_base.cpp

@@ -57,6 +57,7 @@ void ChassisBase<T...>::initialize_parameters(ros::NodeHandle& controller_nh)
     controller_nh.getParam("twist_angular", twist_angular_);
     controller_nh.getParam("max_odom_vel", max_odom_vel_);
     controller_nh.getParam("timeout", timeout_);
+    raw_yaw_feedforward_k_ = getParam(controller_nh, "raw_yaw_feedforward_k", 0.0);
 
     if (controller_nh.hasParam("pid_follow"))
       pid_follow_.init(ros::NodeHandle(controller_nh, "pid_follow"));
@@ -200,6 +201,9 @@ void ChassisBase<T...>::update(const ros::Time& time, const ros::Duration& perio
     case TWIST:
       twist(time, period);
       break;
+    case FALLEN:
+      fallen();
+      break;
   }
 
   ramp_w_->setAcc(cmd_chassis.accel.angular.z);
@@ -290,7 +294,29 @@ void ChassisBase<T...>::raw()
 
     recovery();
   }
-  tfVelToBase(command_source_frame_);
+  double yaw_offset;
+  if (command_source_frame_ == "yaw")
+    yaw_offset = raw_yaw_feedforward_k_ * vel_cmd_.z;
+  else
+    yaw_offset = 0.;
+  tfVelToBase(command_source_frame_, yaw_offset);
+}
+
+template <typename... T>
+void ChassisBase<T...>::fallen()
+{
+  if (state_changed_)
+  {
+    state_changed_ = false;
+    ROS_INFO("[Chassis] Enter FALLEN");
+  }
+
+  ramp_x_->clear();
+  ramp_y_->clear();
+  ramp_w_->clear();
+  vel_cmd_.x = 0.;
+  vel_cmd_.y = 0.;
+  vel_cmd_.z = 0.;
 }
 
 template <typename... T>
@@ -474,11 +500,22 @@ void ChassisBase<T...>::updatePowerStatus()
 }
 
 template <typename... T>
-void ChassisBase<T...>::tfVelToBase(const std::string& from)
+void ChassisBase<T...>::tfVelToBase(const std::string& from, double yaw_offset)
 {
   try
   {
-    tf2::doTransform(vel_cmd_, vel_cmd_, robot_state_handle_.lookupTransform("base_link", from, ros::Time(0)));
+    geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform("base_link", from, ros::Time(0));
+    if (std::abs(yaw_offset) > 1e-9)
+    {
+      tf2::Quaternion rotation;
+      tf2::fromMsg(transform.transform.rotation, rotation);
+      tf2::Quaternion yaw_feedforward;
+      yaw_feedforward.setRPY(0., 0., yaw_offset);
+      rotation *= yaw_feedforward;
+      rotation.normalize();
+      transform.transform.rotation = tf2::toMsg(rotation);
+    }
+    tf2::doTransform(vel_cmd_, vel_cmd_, transform);
   }
   catch (tf2::TransformException& ex)
   {