Optimize chassis_base for localization and power limit.

rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h

@@ -47,19 +47,49 @@
 #include <geometry_msgs/TwistStamped.h>
 #include <geometry_msgs/Vector3Stamped.h>
 #include <nav_msgs/Odometry.h>
-#include <rm_msgs/ChassisActiveSusCmd.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>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <angles/angles.h>
+#include <hardware_interface/imu_sensor_interface.h>
+#include <cmath>
+#include <string>
+#include <std_msgs/Float64.h>
+#include <rm_msgs/PowerManagementSampleAndStatusData.h>
 
 namespace rm_chassis_controllers
 {
 struct Command
 {
   geometry_msgs::Twist cmd_vel_;
   rm_msgs::ChassisCmd cmd_chassis_;
-  rm_msgs::ChassisActiveSusCmd cmd_active_sus_;
   ros::Time stamp_;
 };
+
+struct PowerLimitor
+{
+  double vel_coeff{};
+  double effort_coeff{};
+  double power_offset{};
+  double max_power{};
+  double power_in[4]{};
+  double cmd_power{};
+  double estimated_power{};
+  double power_limit[4]{};
+  double ratio{ 1.0 };
+  double err[4]{};
+  double err_upper{};
+  double err_lower{};
+  double err_sum{};
+  double omiga[4]{};
+  double torque[4]{};
+  double K{};
+  double K_angle[4]{};
+};
+
 template <typename... T>
 class ChassisBase : public controller_interface::MultiInterfaceController<T...>
 {
@@ -114,7 +144,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
   void twist(const ros::Time& time, const ros::Duration& period);
   virtual void moveJoint(const ros::Time& time, const ros::Duration& period) = 0;
   virtual geometry_msgs::Twist odometry() = 0;
-  /** @brief Init frame on base_link. Integral vel to pos and angle.
+  /** @brief Init frame on base_link. Integral vel to current_pos_ and angle.
    *
    * @param time The current time.
    * @param period The time passed since the last call to update.
@@ -132,7 +162,8 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
    *
    * Receive power limit from command. Set max_effort command to chassis to avoid exceed power limit.
    */
-  void powerLimit();
+  virtual void powerLimit();
+  virtual void updatePowerStatus();
   /** @brief Write current command from rm_msgs::ChassisCmd.
    *
    * @param msg This message contains various state parameter settings for basic chassis control
@@ -143,49 +174,96 @@ 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 powerLimitReconfigCB(rm_chassis_controllers::PowerLimitConfig& config, uint32_t level);
+
+  void initialize_parameters(ros::NodeHandle& controller_nh);
+  void slamCallback(const nav_msgs::Odometry::ConstPtr& msg);
+  void localizationCallback(const geometry_msgs::TransformStamped::ConstPtr& msg);
+  void capacityCallback(const rm_msgs::PowerManagementSampleAndStatusData::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_{};
+  std::unique_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry>> odom_pub_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64>> cpower_pub_;  // command power publisher
+  std::unique_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64>> epower_pub_;  // estimated power publisher
+  std::unique_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64>> chassis_power_pub_;  // chassis power publisher
+
+  rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{};
+  rm_common::TfRtBroadcaster brcst4global_map2camera_init_{};
+  rm_common::TfRtBroadcaster brcst4robot_odom2robot_base_{};
+
+  geometry_msgs::TransformStamped global_map2robot_odom_{};
+  geometry_msgs::TransformStamped global_map2camera_init_{};
+  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_;
+  ros::Subscriber capacity_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 };
+  bool gravity_estimation_offset_{ false };
+
+  double roll_{ 0.0 };
+  double pitch_{ 0.0 };
+  double yaw_{ 0.0 };
+  double wheel_radius_{ 0.02 };
+  double twist_angular_{ M_PI / 6 };
+  double max_odom_vel_{ 10.0 };
+  double timeout_{ 0.1 };
+  double chassis_power_{ 0.0 };
+  bool capacity_update_flag_{ false };
+  bool use_rls_{ false };
+  bool use_K_angle_{ false };
+
+  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_{ "/slam/odometry" };
+  std::string localization_topic_{ "/shinji/result" };
+
+  std::string capacity_topic_{ "/rm_referee/power_management/sample_and_status" };
+
+  ros::Time last_publish_time_{};
+  geometry_msgs::Vector3 vel_cmd_{};  // x, y
+  std::array<double, 36> twist_covariance_{};
+  control_toolbox::Pid pid_follow_{};
+
+  Command cmd_struct_{};
+  PowerLimitor wheel_power_limitor_{};
 
-  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;
   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_;
-
-  dynamic_reconfigure::Server<rm_chassis_controllers::PowerLimitConfig>* power_limit_srv_{};
-  realtime_tools::RealtimeBuffer<rm_chassis_controllers::PowerLimitConfig> power_limit_rt_buffer_;
-  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