Optimized chassis base

rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h

@@ -48,6 +48,14 @@
 #include <geometry_msgs/Vector3Stamped.h>
 #include <nav_msgs/Odometry.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <rm_common/ros_utilities.h>
+#include <rm_common/math_utilities.h>
+#include <rm_common/ori_tool.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <angles/angles.h>
+#include <hardware_interface/imu_sensor_interface.h>
+#include <cmath>
+#include <string>
 
 namespace rm_chassis_controllers
 {
@@ -140,46 +148,79 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
    * @param msg This expresses velocity in free space broken into its linear and angular parts.
    */
   void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg);
-  void outsideOdomCallback(const nav_msgs::Odometry::ConstPtr& msg);
+
+  void initialize_parameters(ros::NodeHandle& controller_nh);
+  void slamCallback(const nav_msgs::Odometry::ConstPtr& msg);
+  void localizationCallback(const geometry_msgs::TransformStamped::ConstPtr& msg);
 
   rm_control::RobotStateHandle robot_state_handle_{};
   hardware_interface::EffortJointInterface* effort_joint_interface_{};
   std::vector<hardware_interface::JointHandle> 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_{};
+
+  rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{};
+  rm_common::TfRtBroadcaster brcst4robot_odom2robot_base_{};
+
+  geometry_msgs::TransformStamped global_map2robot_odom_{};
+  geometry_msgs::TransformStamped robot_odom2robot_base_{};
+  geometry_msgs::TransformStamped robot_base2lidar_base_{};
+
+  tf2::Transform T_global_map2robot_odom_{};
+  tf2::Transform T_robot_odom_2robot_base_{};
+  tf2::Transform T_lidar_odom2lidar_base_{};
+  tf2::Transform T_robot_base2lidar_base_{};
+  tf2::Transform T_global_map2lidar_odom_{};
+
+  ros::Subscriber cmd_vel_sub_;
+  ros::Subscriber cmd_chassis_sub_;
+  ros::Subscriber slam_sub_;
+  ros::Subscriber localization_sub_;
+
+  std::unique_ptr<RampFilter<double>> ramp_x_{ nullptr };
+  std::unique_ptr<RampFilter<double>> ramp_y_{ nullptr };
+  std::unique_ptr<RampFilter<double>> ramp_w_{ nullptr };
+
+  double publish_rate_{ 100.0 };
+  bool publish_map_tf_{ false };
+  bool publish_odom_tf_{ false };
+
+  double velocity_coeff_{ 0.0 };
+  double effort_coeff_{ 0.0 };
+  double power_offset_{ 0.0 };
 
-  double wheel_radius_{}, publish_rate_{}, twist_angular_{}, timeout_{}, effort_coeff_{}, velocity_coeff_{},
-      power_offset_{};
-  double roll_ = 0., pitch_ = 0., yaw_ = 0.;
-  double pitch_angle_threshold_ = 0., scale_ = 0.;
-  double max_odom_vel_;
-  bool enable_uphill_acceleration_ = false;
-  bool enable_odom_tf_ = false;
-  bool topic_update_ = false;
-  bool publish_odom_tf_ = false;
-  bool state_changed_ = true;
+  double wheel_radius_{ 0.02 };
+  double twist_angular_{ M_PI / 6 };
+  double max_odom_vel_{ 10.0 };
+  double timeout_{ 0.1 };
+
+  bool odom_initialized_{ false };
+  bool slam_updated_{ false };
+  bool localization_updated_{ false };
+  bool state_changed_{ true };
+  int state_{ RAW };
+
+  std::string follow_source_frame_{};
+  std::string command_source_frame_{};
+  std::string global_map_frame_id_{ "map" };
+  std::string robot_odom_frame_id_{ "odom" };
+  std::string robot_base_frame_id_{ "base_link" };
+  std::string lidar_base_frame_id_{ "livox_frame" };
+  std::string slam_topic_{ "/Odometry" };
+  std::string localization_topic_{ "/hdl_global_localization/result" };
+
+  ros::Time last_publish_time_{};
+  geometry_msgs::Vector3 vel_cmd_{};  // x, y
+  control_toolbox::Pid pid_follow_{};
+
+  Command cmd_struct_{};
   enum
   {
     RAW,
     FOLLOW,
     TWIST
   };
-  int state_ = RAW;
-  RampFilter<double>*ramp_x_{}, *ramp_y_{}, *ramp_w_{};
-  std::string follow_source_frame_{}, command_source_frame_{};
-
-  ros::Time last_publish_time_;
-  geometry_msgs::TransformStamped odom2base_{};
-  tf2::Transform world2odom_;
-  geometry_msgs::Vector3 vel_cmd_{};  // x, y
-  control_toolbox::Pid pid_follow_;
-
-  std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
-  rm_common::TfRtBroadcaster tf_broadcaster_{};
-  ros::Subscriber outside_odom_sub_;
-  ros::Subscriber cmd_chassis_sub_;
-  ros::Subscriber cmd_vel_sub_;
-  Command cmd_struct_;
-  realtime_tools::RealtimeBuffer<Command> cmd_rt_buffer_;
-  realtime_tools::RealtimeBuffer<nav_msgs::Odometry> odom_buffer_;
 };
 
 }  // namespace rm_chassis_controllers

rm_chassis_controllers/include/rm_chassis_controllers/swerve.h

@@ -41,13 +41,16 @@
 
 #include <rm_common/eigen_types.h>
 #include <effort_controllers/joint_position_controller.h>
+#include "rm_msgs/PowerHeatData.h"
+#include <sensor_msgs/JointState.h>
 
 namespace rm_chassis_controllers
 {
 struct Module
 {
   Vec2<double> position_;
-  double pivot_offset_, wheel_radius_;
+  double pivot_offset_, pivot_buffer_threshold_, pivot_effort_threshold_, pivot_position_error_threshold_,
+      pivot_max_reduce_cnt_, wheel_radius_;
   effort_controllers::JointPositionController* ctrl_pivot_;
   effort_controllers::JointVelocityController* ctrl_wheel_;
 };
@@ -60,8 +63,14 @@ class SwerveController : public ChassisBase<rm_control::RobotStateInterface, har
 
 private:
   void moveJoint(const ros::Time& time, const ros::Duration& period) override;
+  void powerManagerCallback(const rm_msgs::PowerHeatData::ConstPtr& data);
+  bool isPivotBlock(double cur_effort, double position_error, Module module);
+  void reduceTargetPosition(double& target_pos, double& position_error, Module module);
   geometry_msgs::Twist odometry() override;
   std::vector<Module> modules_;
+  ros::Subscriber power_manager_sub_;
+  int chassis_power_buffer_ = 60;
+  int pivot_block_cnt_ = 0;
 };
 
 }  // namespace rm_chassis_controllers