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 e979da0c..76f0f406 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -48,6 +48,14 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include namespace rm_chassis_controllers { @@ -140,46 +148,79 @@ 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 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 joint_handles_{}; + realtime_tools::RealtimeBuffer cmd_rt_buffer_{}; + realtime_tools::RealtimeBuffer slam_rt_buffer_{}; + realtime_tools::RealtimeBuffer 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> 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 }; + + 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*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 > 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 diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h index 23ddfb94..2aabec90 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h @@ -41,13 +41,16 @@ #include #include +#include "rm_msgs/PowerHeatData.h" +#include namespace rm_chassis_controllers { struct Module { Vec2 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 modules_; + ros::Subscriber power_manager_sub_; + int chassis_power_buffer_ = 60; + int pivot_block_cnt_ = 0; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 627b4831..a8292fdf 100644 --- 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 { @@ -49,73 +43,71 @@ template class ChassisBase; template -bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, - ros::NodeHandle& controller_nh) +void ChassisBase::initialize_parameters(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_)) + try { - ROS_ERROR("Some chassis params doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; + 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("slam_topic", slam_topic_); + controller_nh.getParam("localization_topic", localization_topic_); + + controller_nh.getParam("power/vel_coeff", velocity_coeff_); + controller_nh.getParam("power/effort_coeff", effort_coeff_); + controller_nh.getParam("power/power_offset", power_offset_); + + 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")); } - 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_) + catch (std::exception& e) { - 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_); + ROS_ERROR("Chassis parameter initialization failed: %s", e.what()); } - world2odom_.setRotation(tf2::Quaternion::getIdentity()); +} +template +bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + initialize_parameters(controller_nh); + robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); + effort_joint_interface_ = robot_hw->get(); - 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); + + 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_); + } - if (controller_nh.hasParam("pid_follow")) - if (!pid_follow_.init(ros::NodeHandle(controller_nh, "pid_follow"))) - return false; + if (publish_odom_tf_) + { + 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_; + global_map2robot_odom_.transform.rotation.w = 1; + brcst4robot_odom2robot_base_.init(root_nh); + brcst4robot_odom2robot_base_.sendTransform(robot_odom2robot_base_); + } return true; } @@ -197,8 +189,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; @@ -224,7 +217,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 }; @@ -265,110 +259,136 @@ 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_) + if (publish_map_tf_) { - geometry_msgs::Vector3 linear_vel_odom, angular_vel_odom; - try - { - 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_); - } - catch (tf2::TransformException& ex) + if (!odom_initialized_) { - tf_broadcaster_.sendTransform(odom2base_); // TODO: For some reason, the sendTransform in init sometime not work? - ROS_WARN("%s", ex.what()); - return; + try + { + geometry_msgs::TransformStamped global_map2lidar_odom = + robot_state_handle_.lookupTransform(robot_base_frame_id_, lidar_base_frame_id_, ros::Time(0)); + T_global_map2lidar_odom_.setOrigin(tf2::Vector3(global_map2lidar_odom.transform.translation.x, + global_map2lidar_odom.transform.translation.y, + global_map2lidar_odom.transform.translation.z)); + T_global_map2lidar_odom_.setRotation( + tf2::Quaternion(global_map2lidar_odom.transform.rotation.x, global_map2lidar_odom.transform.rotation.y, + global_map2lidar_odom.transform.rotation.z, global_map2lidar_odom.transform.rotation.w)); + odom_initialized_ = true; + } + catch (...) + { + ROS_WARN("Failed to init robot_odom2lidar_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_); - 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_) + + if (localization_updated_) { - // 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(); - } - 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); - 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); + 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)); + } + catch (...) + { + ROS_WARN("Failed to update localization offset."); + } } - } - - if (topic_update_) - { - 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 + if (slam_updated_) { - 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); + 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)); + + robot_base2lidar_base_ = + robot_state_handle_.lookupTransform(robot_base_frame_id_, lidar_base_frame_id_, ros::Time(0)); + 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 (tf2::TransformException& ex) + catch (...) { - ROS_WARN("%s", ex.what()); - return; + ROS_WARN("Failed to update global_map2robot_odom."); } - world2odom_ = world2sensor * odom2sensor.inverse(); } - tf2::Transform base2sensor; + global_map2robot_odom_.header.stamp = time; + } + + if (publish_odom_tf_) + { try { - geometry_msgs::TransformStamped tf_msg = - robot_state_handle_.lookupTransform("base_link", "livox_frame", odom_msg->header.stamp); - tf2::fromMsg(tf_msg.transform, base2sensor); + 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::Twist vel_base = odometry(); // on base_link frame + 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 + 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(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(); + robot_odom2robot_base_.transform.rotation = tf2::toMsg(odom2base_quat); + } + robot_state_handle_.setTransform(robot_odom2robot_base_, "rm_chassis_controllers"); } - catch (tf2::TransformException& ex) + catch (...) { - ROS_WARN("%s", ex.what()); - return; + ROS_WARN("Failed to update robot_odom2robot_base."); } - 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; } - 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()) - { - 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_->unlockAndPublish(); - } - if (enable_odom_tf_ && publish_odom_tf_) - tf_broadcaster_.sendTransform(odom2base_); + if (publish_map_tf_) + brcst4global_map2robot_odom_.sendTransform(global_map2robot_odom_); + + if (publish_odom_tf_) + brcst4robot_odom2robot_base_.sendTransform(robot_odom2robot_base_); + last_publish_time_ = time; } } @@ -405,21 +425,7 @@ void ChassisBase::powerLimit() 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); - } + joint.setCommand(zoom_coeff > 1 ? joint.getCommand() : joint.getCommand() * zoom_coeff); } } @@ -452,10 +458,17 @@ 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) +{ + slam_rt_buffer_.writeFromNonRT(*msg); + slam_updated_ = true; +} + +template +void ChassisBase::localizationCallback(const geometry_msgs::TransformStamped::ConstPtr& msg) { - odom_buffer_.writeFromNonRT(*msg); - topic_update_ = true; + localization_rt_buffer_.writeFromNonRT(*msg); + localization_updated_ = true; } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 69c21077..5f762394 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -63,6 +63,10 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand Module m{ .position_ = Vec2((double)module.second["position"][0], (double)module.second["position"][1]), .pivot_offset_ = module.second["pivot"]["offset"], + .pivot_buffer_threshold_ = module.second["pivot"]["buffer_threshold"], + .pivot_effort_threshold_ = module.second["pivot"]["effort_threshold"], + .pivot_position_error_threshold_ = module.second["pivot"]["position_error_threshold"], + .pivot_max_reduce_cnt_ = module.second["pivot"]["max_reduce_cnt"], .wheel_radius_ = module.second["wheel"]["radius"], .ctrl_pivot_ = new effort_controllers::JointPositionController(), .ctrl_wheel_ = new effort_controllers::JointVelocityController() }; @@ -77,6 +81,8 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand joint_handles_.push_back(m.ctrl_wheel_->joint_); modules_.push_back(m); } + power_manager_sub_ = root_nh.subscribe("/rm_referee/power_manager", 10, + &SwerveController::powerManagerCallback, this); return true; } @@ -92,13 +98,45 @@ void SwerveController::moveJoint(const ros::Time& time, const ros::Duration& per // Direction flipping and Stray module mitigation double a = angles::shortest_angular_distance(module.ctrl_pivot_->joint_.getPosition(), vel_angle); double b = angles::shortest_angular_distance(module.ctrl_pivot_->joint_.getPosition(), vel_angle + M_PI); - module.ctrl_pivot_->setCommand(std::abs(a) < std::abs(b) ? vel_angle : vel_angle + M_PI); + double target_pos = std::abs(a) < std::abs(b) ? vel_angle : vel_angle + M_PI; + double pos_error = angles::shortest_angular_distance(module.ctrl_pivot_->joint_.getPosition(), target_pos); + if (chassis_power_buffer_ > module.pivot_buffer_threshold_ && + !isPivotBlock(module.ctrl_pivot_->joint_.getEffort(), pos_error, module)) + { + pivot_block_cnt_ = 0; + module.ctrl_pivot_->setCommand(target_pos); + } + else + { + reduceTargetPosition(target_pos, pos_error, module); + module.ctrl_pivot_->setCommand(target_pos); + } + module.ctrl_wheel_->setCommand(vel.norm() / module.wheel_radius_ * std::cos(a)); module.ctrl_pivot_->update(time, period); module.ctrl_wheel_->update(time, period); } } +bool SwerveController::isPivotBlock(double cur_effort, double position_error, Module module) +{ + return abs(cur_effort) > module.pivot_effort_threshold_ && + abs(position_error) > module.pivot_position_error_threshold_; +} + +void SwerveController::reduceTargetPosition(double& target_pos, double& position_error, Module module) +{ + pivot_block_cnt_++; + if (pivot_block_cnt_ > module.pivot_max_reduce_cnt_) + pivot_block_cnt_ = module.pivot_max_reduce_cnt_; + double reduce_step_size = position_error / module.pivot_max_reduce_cnt_; + + if (abs(position_error) > abs(pivot_block_cnt_ * reduce_step_size)) + target_pos -= pivot_block_cnt_ * reduce_step_size; + else + target_pos = module.ctrl_pivot_->joint_.getPosition(); +} + geometry_msgs::Twist SwerveController::odometry() { geometry_msgs::Twist vel_data{}; @@ -125,5 +163,10 @@ geometry_msgs::Twist SwerveController::odometry() return vel_data; } +void SwerveController::powerManagerCallback(const rm_msgs::PowerHeatData::ConstPtr& data) +{ + chassis_power_buffer_ = data->chassis_power_buffer; +} + PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) } // namespace rm_chassis_controllers