From 4f480116e06394ddd2930b3f8c15c8ad07b22b1f Mon Sep 17 00:00:00 2001 From: AphroraC <108052453@qq.com> Date: Sun, 4 Oct 2026 16:25:38 +0800 Subject: [PATCH 1/2] Optimize chassis_base for localization and power limit. --- .../rm_chassis_controllers/chassis_base.h | 156 +++++-- rm_chassis_controllers/src/chassis_base.cpp | 437 ++++++++++-------- 2 files changed, 360 insertions(+), 233 deletions(-) mode change 100644 => 100755 rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h mode change 100644 => 100755 rm_chassis_controllers/src/chassis_base.cpp diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h old mode 100644 new mode 100755 index faca1374..18f0a457 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -47,9 +47,18 @@ #include #include #include -#include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace rm_chassis_controllers { @@ -57,9 +66,30 @@ 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 class ChassisBase : public controller_interface::MultiInterfaceController { @@ -114,7 +144,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController 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 * * 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 * @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 joint_handles_{}; - - 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; + std::vector wheel_joint_handles_{}; + realtime_tools::RealtimeBuffer cmd_rt_buffer_{}; + realtime_tools::RealtimeBuffer slam_rt_buffer_{}; + realtime_tools::RealtimeBuffer localization_rt_buffer_{}; + std::unique_ptr> odom_pub_; + std::unique_ptr> cpower_pub_; // command power publisher + std::unique_ptr> epower_pub_; // estimated power publisher + std::unique_ptr> 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> ramp_x_{ nullptr }; + std::unique_ptr> ramp_y_{ nullptr }; + std::unique_ptr> 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 twist_covariance_{}; + control_toolbox::Pid pid_follow_{}; + + Command cmd_struct_{}; + PowerLimitor wheel_power_limitor_{}; + enum { RAW, FOLLOW, TWIST }; - int state_ = RAW; - RampFilter*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* power_limit_srv_{}; - realtime_tools::RealtimeBuffer power_limit_rt_buffer_; - std::shared_ptr > 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 cmd_rt_buffer_; - realtime_tools::RealtimeBuffer odom_buffer_; }; -} // namespace rm_chassis_controllers +} // namespace rm_chassis_controllers \ No newline at end of file diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp old mode 100644 new mode 100755 index 1e744ebd..f1c13625 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -35,12 +35,6 @@ // Created by huakang on 2021/3/21. // #include "rm_chassis_controllers/chassis_base.h" -#include -#include -#include -#include -#include -#include namespace rm_chassis_controllers { @@ -48,81 +42,109 @@ template class ChassisBase; +template +void ChassisBase::initialize_parameters(ros::NodeHandle& controller_nh) +{ + try + { + controller_nh.getParam("publish_rate", publish_rate_); + controller_nh.getParam("publish_map_tf", publish_map_tf_); + controller_nh.getParam("publish_odom_tf", publish_odom_tf_); + controller_nh.getParam("gravity_estimation_offset", gravity_estimation_offset_); + controller_nh.getParam("slam_topic", slam_topic_); + controller_nh.getParam("localization_topic", localization_topic_); + controller_nh.getParam("wheel_radius", wheel_radius_); + controller_nh.getParam("twist_angular", twist_angular_); + controller_nh.getParam("max_odom_vel", max_odom_vel_); + controller_nh.getParam("timeout", timeout_); + + if (controller_nh.hasParam("pid_follow")) + pid_follow_.init(ros::NodeHandle(controller_nh, "pid_follow")); + } + catch (std::exception& e) + { + ROS_ERROR("Chassis parameter initialization failed: %s", e.what()); + } +} template bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { - if (!controller_nh.getParam("publish_rate", publish_rate_) || !controller_nh.getParam("timeout", timeout_) || - !controller_nh.getParam("power/vel_coeff", velocity_coeff_) || - !controller_nh.getParam("power/effort_coeff", effort_coeff_) || - !controller_nh.getParam("power/power_offset", power_offset_)) + initialize_parameters(controller_nh); + odom_pub_ = std::make_unique>(root_nh, "odom", 100); + odom_pub_->msg_.header.frame_id = robot_odom_frame_id_; + odom_pub_->msg_.child_frame_id = robot_base_frame_id_; + XmlRpc::XmlRpcValue twist_covariance_list; + if (controller_nh.getParam("twist_covariance_diagonal", twist_covariance_list)) { - ROS_ERROR("Some chassis params doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; + if (twist_covariance_list.getType() != XmlRpc::XmlRpcValue::TypeArray || twist_covariance_list.size() != 6) + { + ROS_ERROR("twist_covariance_diagonal must be a 6-element array."); + return false; + } + twist_covariance_.fill(0.0); + for (int i = 0; i < 6; i++) + { + if (twist_covariance_list[i].getType() != XmlRpc::XmlRpcValue::TypeDouble && + twist_covariance_list[i].getType() != XmlRpc::XmlRpcValue::TypeInt) + { + ROS_ERROR("twist_covariance_diagonal[%d] must be numeric.", i); + return false; + } + twist_covariance_[i * 7] = static_cast(twist_covariance_list[i]); + } + for (size_t i = 0; i < twist_covariance_.size(); ++i) + odom_pub_->msg_.twist.covariance[i] = twist_covariance_[i]; } - pitch_angle_threshold_ = getParam(controller_nh, "pitch_angle_threshold", -0.25); - scale_ = getParam(controller_nh, "scale", 1.); - enable_uphill_acceleration_ = getParam(controller_nh, "enable_uphill_acceleration", false); - wheel_radius_ = getParam(controller_nh, "wheel_radius", 0.02); - twist_angular_ = getParam(controller_nh, "twist_angular", M_PI / 6); - max_odom_vel_ = getParam(controller_nh, "max_odom_vel", 0); - enable_odom_tf_ = getParam(controller_nh, "enable_odom_tf", true); - publish_odom_tf_ = getParam(controller_nh, "publish_odom_tf", false); - - // Get and check params for covariances - XmlRpc::XmlRpcValue twist_cov_list; - controller_nh.getParam("twist_covariance_diagonal", twist_cov_list); - ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray); - ROS_ASSERT(twist_cov_list.size() == 6); - for (int i = 0; i < twist_cov_list.size(); ++i) - ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); - robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); effort_joint_interface_ = robot_hw->get(); - // Setup odometry realtime publisher + odom message constant fields - odom_pub_.reset(new realtime_tools::RealtimePublisher(root_nh, "odom", 100)); - odom_pub_->msg_.header.frame_id = "odom"; - odom_pub_->msg_.child_frame_id = "base_link"; - odom_pub_->msg_.twist.covariance = { static_cast(twist_cov_list[0]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[1]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[2]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[3]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[4]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[5]) }; - - ramp_x_ = new RampFilter(0, 0.001); - ramp_y_ = new RampFilter(0, 0.001); - ramp_w_ = new RampFilter(0, 0.001); - - // init odom tf - if (enable_odom_tf_) - { - odom2base_.header.frame_id = "odom"; - odom2base_.header.stamp = ros::Time::now(); - odom2base_.child_frame_id = "base_link"; - odom2base_.transform.rotation.w = 1; - tf_broadcaster_.init(root_nh); - tf_broadcaster_.sendTransform(odom2base_); - } - world2odom_.setRotation(tf2::Quaternion::getIdentity()); - - outside_odom_sub_ = - controller_nh.subscribe("/odometry", 10, &ChassisBase::outsideOdomCallback, this); + cmd_vel_sub_ = root_nh.subscribe("cmd_vel", 1, &ChassisBase::cmdVelCallback, this); cmd_chassis_sub_ = controller_nh.subscribe("/cmd_chassis", 1, &ChassisBase::cmdChassisCallback, this); - cmd_vel_sub_ = root_nh.subscribe("cmd_vel", 1, &ChassisBase::cmdVelCallback, this); + slam_sub_ = controller_nh.subscribe(slam_topic_, 10, &ChassisBase::slamCallback, this); + localization_sub_ = controller_nh.subscribe( + localization_topic_, 10, &ChassisBase::localizationCallback, this); + capacity_sub_ = root_nh.subscribe(capacity_topic_, 10, + &ChassisBase::capacityCallback, this); + + // Setup real_power from capacity publishers. + auto chassis_power_publisher = + std::make_unique>(controller_nh, "power/chassis_power", 100); + this->chassis_power_pub_ = std::move(chassis_power_publisher); + + ramp_x_ = std::make_unique>(0, 0.001); + ramp_y_ = std::make_unique>(0, 0.001); + ramp_w_ = std::make_unique>(0, 0.001); + + if (publish_map_tf_) + { + global_map2robot_odom_.header.stamp = ros::Time::now(); + global_map2robot_odom_.header.frame_id = global_map_frame_id_; + global_map2robot_odom_.child_frame_id = robot_odom_frame_id_; + global_map2robot_odom_.transform.rotation.w = 1; + brcst4global_map2robot_odom_.init(root_nh); + brcst4global_map2robot_odom_.sendTransform(global_map2robot_odom_); + + global_map2camera_init_.header.stamp = ros::Time::now(); + global_map2camera_init_.header.frame_id = global_map_frame_id_; + global_map2camera_init_.child_frame_id = "camera_init"; + global_map2camera_init_.transform.rotation.w = 1; + brcst4global_map2camera_init_.init(root_nh); + brcst4global_map2camera_init_.sendTransform(global_map2camera_init_); + } - if (controller_nh.hasParam("pid_follow")) - if (!pid_follow_.init(ros::NodeHandle(controller_nh, "pid_follow"))) - return false; + robot_odom2robot_base_.header.stamp = ros::Time::now(); + robot_odom2robot_base_.header.frame_id = robot_odom_frame_id_; + robot_odom2robot_base_.child_frame_id = robot_base_frame_id_; + robot_odom2robot_base_.transform.rotation.w = 1; + robot_state_handle_.setTransform(robot_odom2robot_base_, "rm_chassis_controllers"); - // dynamic reconfigure - power_limit_srv_ = new dynamic_reconfigure::Server( - ros::NodeHandle(controller_nh, "power")); - dynamic_reconfigure::Server::CallbackType cb = - boost::bind(&ChassisBase::powerLimitReconfigCB, this, _1, _2); - power_limit_srv_->setCallback(cb); + if (publish_odom_tf_) + { + brcst4robot_odom2robot_base_.init(root_nh); + brcst4robot_odom2robot_base_.sendTransform(robot_odom2robot_base_); + } return true; } @@ -204,8 +226,9 @@ void ChassisBase::follow(const ros::Time& time, const ros::Duration& perio try { double roll{}, pitch{}, yaw{}; - quatToRPY(robot_state_handle_.lookupTransform("base_link", follow_source_frame_, ros::Time(0)).transform.rotation, - roll, pitch, yaw); + quatToRPY( + robot_state_handle_.lookupTransform(robot_base_frame_id_, follow_source_frame_, ros::Time(0)).transform.rotation, + roll, pitch, yaw); double follow_error = angles::shortest_angular_distance(yaw, 0); pid_follow_.computeCommand(-follow_error, period); vel_cmd_.z = pid_follow_.getCurrentCmd() + cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_vel_des; @@ -231,7 +254,8 @@ void ChassisBase::twist(const ros::Time& time, const ros::Duration& period try { double roll{}, pitch{}, yaw{}; - quatToRPY(robot_state_handle_.lookupTransform("base_link", command_source_frame_, ros::Time(0)).transform.rotation, + quatToRPY(robot_state_handle_.lookupTransform(robot_base_frame_id_, command_source_frame_, ros::Time(0)) + .transform.rotation, roll, pitch, yaw); double angle[4] = { -0.785, 0.785, 2.355, -2.355 }; @@ -272,110 +296,161 @@ void ChassisBase::raw() template void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& period) { - geometry_msgs::Twist vel_base = odometry(); // on base_link frame - if (enable_odom_tf_) + const geometry_msgs::Twist vel_base = odometry(); // on base_link frame + if (publish_map_tf_) { - geometry_msgs::Vector3 linear_vel_odom, angular_vel_odom; - try + if (!odom_initialized_) { - odom2base_ = robot_state_handle_.lookupTransform("odom", "base_link", ros::Time(0)); - tf2::Quaternion q; - tf2::fromMsg(odom2base_.transform.rotation, q); - tf2::Matrix3x3(q).getEulerYPR(yaw_, pitch_, roll_); + try + { + robot_base2lidar_base_ = + robot_state_handle_.lookupTransform(robot_base_frame_id_, lidar_base_frame_id_, ros::Time(0)); + T_global_map2lidar_odom_.setOrigin(tf2::Vector3(robot_base2lidar_base_.transform.translation.x, + robot_base2lidar_base_.transform.translation.y, + robot_base2lidar_base_.transform.translation.z)); + if (gravity_estimation_offset_) + { + T_global_map2lidar_odom_.setRotation(tf2::Quaternion(0, 0, 0, 1)); + global_map2camera_init_.transform.translation.x = T_global_map2lidar_odom_.getOrigin().x(); + global_map2camera_init_.transform.translation.y = T_global_map2lidar_odom_.getOrigin().y(); + global_map2camera_init_.transform.translation.z = T_global_map2lidar_odom_.getOrigin().z(); + } + else + { + T_global_map2lidar_odom_.setRotation(tf2::Quaternion( + robot_base2lidar_base_.transform.rotation.x, robot_base2lidar_base_.transform.rotation.y, + robot_base2lidar_base_.transform.rotation.z, robot_base2lidar_base_.transform.rotation.w)); + global_map2camera_init_.transform = tf2::toMsg(T_global_map2lidar_odom_); + } + odom_initialized_ = true; + } + catch (...) + { + ROS_WARN("Failed to init robot_odom2lidar_odom."); + } } - catch (tf2::TransformException& ex) + + if (localization_updated_) { - tf_broadcaster_.sendTransform(odom2base_); // TODO: For some reason, the sendTransform in init sometime not work? - ROS_WARN("%s", ex.what()); - return; + try + { + localization_updated_ = false; + const auto& localization = localization_rt_buffer_.readFromRT(); + T_global_map2lidar_odom_.setOrigin(tf2::Vector3(localization->transform.translation.x, + localization->transform.translation.y, + localization->transform.translation.z)); + T_global_map2lidar_odom_.setRotation( + tf2::Quaternion(localization->transform.rotation.x, localization->transform.rotation.y, + localization->transform.rotation.z, localization->transform.rotation.w)); + global_map2camera_init_.transform = tf2::toMsg(T_global_map2lidar_odom_); + } + catch (...) + { + ROS_WARN("Failed to update localization offset."); + } + } + + if (slam_updated_) + { + try + { + slam_updated_ = false; + const auto& slam = slam_rt_buffer_.readFromRT(); + T_lidar_odom2lidar_base_.setOrigin( + tf2::Vector3(slam->pose.pose.position.x, slam->pose.pose.position.y, slam->pose.pose.position.z)); + T_lidar_odom2lidar_base_.setRotation( + tf2::Quaternion(slam->pose.pose.orientation.x, slam->pose.pose.orientation.y, slam->pose.pose.orientation.z, + slam->pose.pose.orientation.w)); + + T_robot_base2lidar_base_.setOrigin(tf2::Vector3(robot_base2lidar_base_.transform.translation.x, + robot_base2lidar_base_.transform.translation.y, + robot_base2lidar_base_.transform.translation.z)); + T_robot_base2lidar_base_.setRotation( + tf2::Quaternion(robot_base2lidar_base_.transform.rotation.x, robot_base2lidar_base_.transform.rotation.y, + robot_base2lidar_base_.transform.rotation.z, robot_base2lidar_base_.transform.rotation.w)); + + T_robot_odom_2robot_base_.setOrigin(tf2::Vector3(robot_odom2robot_base_.transform.translation.x, + robot_odom2robot_base_.transform.translation.y, + robot_odom2robot_base_.transform.translation.z)); + T_robot_odom_2robot_base_.setRotation( + tf2::Quaternion(robot_odom2robot_base_.transform.rotation.x, robot_odom2robot_base_.transform.rotation.y, + robot_odom2robot_base_.transform.rotation.z, robot_odom2robot_base_.transform.rotation.w)); + + T_global_map2robot_odom_ = T_global_map2lidar_odom_ * T_lidar_odom2lidar_base_ * + T_robot_base2lidar_base_.inverse() * T_robot_odom_2robot_base_.inverse(); + global_map2robot_odom_.transform = tf2::toMsg(T_global_map2robot_odom_); + } + catch (...) + { + ROS_WARN("Failed to update global_map2robot_odom."); + } } - odom2base_.header.stamp = time; - // integral vel to pos and angle - tf2::doTransform(vel_base.linear, linear_vel_odom, odom2base_); - tf2::doTransform(vel_base.angular, angular_vel_odom, odom2base_); + global_map2robot_odom_.header.stamp = time; + global_map2camera_init_.header.stamp = time; + } + + try + { + robot_odom2robot_base_ = + robot_state_handle_.lookupTransform(robot_odom_frame_id_, robot_base_frame_id_, ros::Time(0)); + robot_odom2robot_base_.header.stamp = time; + geometry_msgs::Vector3 linear_vel_odom, angular_vel_odom; + tf2::doTransform(vel_base.linear, linear_vel_odom, robot_odom2robot_base_); + tf2::doTransform(vel_base.angular, angular_vel_odom, robot_odom2robot_base_); + double length = std::sqrt(std::pow(linear_vel_odom.x, 2) + std::pow(linear_vel_odom.y, 2) + std::pow(linear_vel_odom.z, 2)); if (length < max_odom_vel_) - { - // avoid nan vel - odom2base_.transform.translation.x += linear_vel_odom.x * period.toSec(); - odom2base_.transform.translation.y += linear_vel_odom.y * period.toSec(); - odom2base_.transform.translation.z += linear_vel_odom.z * period.toSec(); + { // avoid nan vel + robot_odom2robot_base_.transform.translation.x += linear_vel_odom.x * period.toSec(); + robot_odom2robot_base_.transform.translation.y += linear_vel_odom.y * period.toSec(); + robot_odom2robot_base_.transform.translation.z += linear_vel_odom.z * period.toSec(); } length = std::sqrt(std::pow(angular_vel_odom.x, 2) + std::pow(angular_vel_odom.y, 2) + std::pow(angular_vel_odom.z, 2)); if (length > 0.001) { // avoid nan quat tf2::Quaternion odom2base_quat, trans_quat; - tf2::fromMsg(odom2base_.transform.rotation, odom2base_quat); + tf2::fromMsg(robot_odom2robot_base_.transform.rotation, odom2base_quat); trans_quat.setRotation(tf2::Vector3(angular_vel_odom.x / length, angular_vel_odom.y / length, angular_vel_odom.z / length), length * period.toSec()); odom2base_quat = trans_quat * odom2base_quat; odom2base_quat.normalize(); - odom2base_.transform.rotation = tf2::toMsg(odom2base_quat); + robot_odom2robot_base_.transform.rotation = tf2::toMsg(odom2base_quat); } + quatToRPY(robot_odom2robot_base_.transform.rotation, roll_, pitch_, yaw_); + robot_state_handle_.setTransform(robot_odom2robot_base_, "rm_chassis_controllers"); } - - if (topic_update_) + catch (...) { - auto* odom_msg = odom_buffer_.readFromRT(); - - tf2::Transform world2sensor; - world2sensor.setOrigin( - tf2::Vector3(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z)); - world2sensor.setRotation(tf2::Quaternion(odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y, - odom_msg->pose.pose.orientation.z, odom_msg->pose.pose.orientation.w)); - - if (world2odom_.getRotation() == tf2::Quaternion::getIdentity()) // First received - { - tf2::Transform odom2sensor; - try - { - geometry_msgs::TransformStamped tf_msg = - robot_state_handle_.lookupTransform("odom", "livox_frame", odom_msg->header.stamp); - tf2::fromMsg(tf_msg.transform, odom2sensor); - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); - return; - } - world2odom_ = world2sensor * odom2sensor.inverse(); - } - tf2::Transform base2sensor; - try - { - geometry_msgs::TransformStamped tf_msg = - robot_state_handle_.lookupTransform("base_link", "livox_frame", odom_msg->header.stamp); - tf2::fromMsg(tf_msg.transform, base2sensor); - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); - return; - } - tf2::Transform odom2base = world2odom_.inverse() * world2sensor * base2sensor.inverse(); - odom2base_.transform.translation.x = odom2base.getOrigin().x(); - odom2base_.transform.translation.y = odom2base.getOrigin().y(); - odom2base_.transform.translation.z = odom2base.getOrigin().z(); - topic_update_ = false; + ROS_WARN("Failed to update robot_odom2robot_base."); } - robot_state_handle_.setTransform(odom2base_, "rm_chassis_controllers"); - if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { - if (odom_pub_->trylock()) + if (odom_pub_ && odom_pub_->trylock()) { odom_pub_->msg_.header.stamp = time; - odom_pub_->msg_.twist.twist.linear.x = vel_base.linear.x; - odom_pub_->msg_.twist.twist.linear.y = vel_base.linear.y; - odom_pub_->msg_.twist.twist.angular.z = vel_base.angular.z; + odom_pub_->msg_.header.frame_id = robot_odom_frame_id_; + odom_pub_->msg_.child_frame_id = robot_base_frame_id_; + odom_pub_->msg_.pose.pose.position.x = robot_odom2robot_base_.transform.translation.x; + odom_pub_->msg_.pose.pose.position.y = robot_odom2robot_base_.transform.translation.y; + odom_pub_->msg_.pose.pose.position.z = robot_odom2robot_base_.transform.translation.z; + odom_pub_->msg_.pose.pose.orientation = robot_odom2robot_base_.transform.rotation; + odom_pub_->msg_.twist.twist = vel_base; odom_pub_->unlockAndPublish(); } - if (enable_odom_tf_ && publish_odom_tf_) - tf_broadcaster_.sendTransform(odom2base_); + + if (publish_map_tf_) + { + brcst4global_map2camera_init_.sendTransform(global_map2camera_init_); + brcst4global_map2robot_odom_.sendTransform(global_map2robot_odom_); + } + + if (publish_odom_tf_) + brcst4robot_odom2robot_base_.sendTransform(robot_odom2robot_base_); + last_publish_time_ = time; } } @@ -391,49 +466,11 @@ void ChassisBase::recovery() template void ChassisBase::powerLimit() { - double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; - const auto& power_config = *power_limit_rt_buffer_.readFromRT(); - - double vel_coeff = power_config.vel_coeff; - double effort_coeff = power_config.effort_coeff; - double power_offset = power_config.power_offset; +} - // Three coefficients of a quadratic equation in one variable - double a = 0., b = 0., c = 0.; - for (const auto& joint : joint_handles_) - { - double cmd_effort = joint.getCommand(); - double real_vel = joint.getVelocity(); - if (joint.getName().find("wheel") != std::string::npos) // The pivot joint of swerve drive doesn't need power limit - { - a += square(cmd_effort); - b += std::abs(cmd_effort * real_vel); - c += square(real_vel); - } - } - a *= effort_coeff; - c = c * vel_coeff - power_offset - power_limit; - // Root formula for quadratic equation in one variable - double zoom_coeff = (square(b) - 4 * a * c) > 0 ? ((-b + sqrt(square(b) - 4 * a * c)) / (2 * a)) : 0.; - for (auto joint : joint_handles_) - if (joint.getName().find("wheel") != std::string::npos) - { - if (pitch_ < pitch_angle_threshold_ && enable_uphill_acceleration_) - { - if (joint.getName().find("back") != std::string::npos) - { - joint.setCommand(zoom_coeff > 1 ? joint.getCommand() : joint.getCommand() * zoom_coeff * scale_); - } - if (joint.getName().find("front") != std::string::npos) - { - joint.setCommand(zoom_coeff > 1 ? joint.getCommand() : joint.getCommand() * zoom_coeff); - } - } - else - { - joint.setCommand(zoom_coeff > 1 ? joint.getCommand() : joint.getCommand() * zoom_coeff); - } - } +template +void ChassisBase::updatePowerStatus() +{ } template @@ -465,17 +502,29 @@ void ChassisBase::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg } template -void ChassisBase::outsideOdomCallback(const nav_msgs::Odometry::ConstPtr& msg) +void ChassisBase::slamCallback(const nav_msgs::Odometry::ConstPtr& msg) { - odom_buffer_.writeFromNonRT(*msg); - topic_update_ = true; + slam_rt_buffer_.writeFromNonRT(*msg); + slam_updated_ = true; } template -void ChassisBase::powerLimitReconfigCB(rm_chassis_controllers::PowerLimitConfig& config, uint32_t /*level*/) +void ChassisBase::localizationCallback(const geometry_msgs::TransformStamped::ConstPtr& msg) { - ROS_INFO("[Power Limit] Dynamic params change"); - power_limit_rt_buffer_.writeFromNonRT(config); + localization_rt_buffer_.writeFromNonRT(*msg); + localization_updated_ = true; +} + +template +void ChassisBase::capacityCallback(const rm_msgs::PowerManagementSampleAndStatusData::ConstPtr& msg) +{ + chassis_power_ = msg->chassis_power + msg->capacity_discharge_power; + capacity_update_flag_ = true; + if (chassis_power_pub_->trylock()) + { + chassis_power_pub_->msg_.data = chassis_power_; + chassis_power_pub_->unlockAndPublish(); + } } -} // namespace rm_chassis_controllers +} // namespace rm_chassis_controllers \ No newline at end of file From a9ac576e830a2fb35b95ae1fc8b1fc766bea388e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=8F=81=E6=80=A1?= <3099568863@qq.com> Date: Sun, 4 Oct 2026 17:13:18 +0800 Subject: [PATCH 2/2] Revert wheel_joint_handles_ back to joint_handles_ to fix build. --- .../include/rm_chassis_controllers/chassis_base.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index 18f0a457..9a95c74c 100755 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -182,7 +182,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController rm_control::RobotStateHandle robot_state_handle_{}; hardware_interface::EffortJointInterface* effort_joint_interface_{}; - std::vector wheel_joint_handles_{}; + std::vector joint_handles_{}; realtime_tools::RealtimeBuffer cmd_rt_buffer_{}; realtime_tools::RealtimeBuffer slam_rt_buffer_{}; realtime_tools::RealtimeBuffer localization_rt_buffer_{}; @@ -266,4 +266,4 @@ class ChassisBase : public controller_interface::MultiInterfaceController }; }; -} // namespace rm_chassis_controllers \ No newline at end of file +} // namespace rm_chassis_controllers