From 9520d275a86232c205cdd67a003426300d3cc5e6 Mon Sep 17 00:00:00 2001 From: NuoDaJia02 Date: Sat, 1 Nov 2025 22:02:57 +0800 Subject: [PATCH] add new changes --- src/robot_control/include/control_base.hpp | 5 + .../include/motion_parameters.hpp | 8 +- .../include/robot_control_manager.hpp | 5 + .../include/robot_control_node.hpp | 4 + src/robot_control/include/wheel_control.hpp | 6 ++ src/robot_control/src/control_base.cpp | 14 +++ .../src/robot_control_manager.cpp | 32 ++++++- src/robot_control/src/robot_control_node.cpp | 96 ++++++++++++++++++- src/robot_control/src/waist_control.cpp | 8 +- src/robot_control/src/wheel_control.cpp | 65 +++++++++---- 10 files changed, 212 insertions(+), 31 deletions(-) diff --git a/src/robot_control/include/control_base.hpp b/src/robot_control/include/control_base.hpp index d56bfe7..e3dabe5 100644 --- a/src/robot_control/include/control_base.hpp +++ b/src/robot_control/include/control_base.hpp @@ -6,6 +6,7 @@ #include "motion_parameters.hpp" #define POSITION_TOLERANCE 1.0 +#define TIME_OUT_COUNT 10000 namespace robot_control { @@ -27,7 +28,9 @@ namespace robot_control std::vector joint_zero_positions_; // 零点位置 std::vector joint_directions_; // 关节运动方向 std::vector joint_position_; // 当前关节位置 + std::vector joint_commands_; // 当前关节指令 std::vector joint_velocity_; // 当前关节速度 + std::vector joint_velocity_commands_; // 当前关节速度指令 std::vector joint_acceleration_; // 当前关节加速度 std::vector joint_torque_; // 当前关节力矩 std::vector minLimits_; // 关节位置下限 @@ -38,7 +41,9 @@ namespace robot_control bool is_stopping_; // 是否停止中 bool is_target_set_; // 是否已设置目标点 bool is_cmd_send_finished_; + bool is_joint_position_initialized_; // 是否已初始化关节位置 + int time_out_count_; // 超时时间 public: // 构造函数（子类需调用此构造函数初始化父类） ControlBase( diff --git a/src/robot_control/include/motion_parameters.hpp b/src/robot_control/include/motion_parameters.hpp index 36eeb2a..4e4c135 100644 --- a/src/robot_control/include/motion_parameters.hpp +++ b/src/robot_control/include/motion_parameters.hpp @@ -56,11 +56,11 @@ namespace robot_control // 初始化结构参数 unit m //TODO: 修改为实际参数 wheel_radius_ = 0.09; - wheel_separation_ = 0.26; + wheel_separation_ = 0.55; wheel_length_ = { wheel_radius_, - wheel_radius_ + wheel_separation_ }; // 腿长参数 unit m @@ -80,8 +80,8 @@ namespace robot_control // 关节速度参数 max_joint_velocity_ = { - 5, - 25, + 30, + 30, 50, 50, 50, diff --git a/src/robot_control/include/robot_control_manager.hpp b/src/robot_control/include/robot_control_manager.hpp index 0d9f1e3..e619b13 100644 --- a/src/robot_control/include/robot_control_manager.hpp +++ b/src/robot_control/include/robot_control_manager.hpp @@ -46,6 +46,7 @@ namespace robot_control std_msgs::msg::Float64MultiArray GetJointFeedback(); std_msgs::msg::Float64MultiArray GetWheelCommands(); std_msgs::msg::Float64MultiArray GetWheelFeedback(); + double GetWheelRatio(); // 机器人状态 void CheckJointLimits(std_msgs::msg::Float64MultiArray& jointCommands); @@ -55,6 +56,8 @@ namespace robot_control MotionParameters GetMotionParameters(); + std::vector GetImuDifference(); + bool IsMoving(MovementPart part); bool RobotInitFinished(); @@ -91,7 +94,9 @@ namespace robot_control // 陀螺仪状态 ImuMsg imuMsg_; + bool isGyroInited_; std::vector gyroValues_; // 陀螺仪数据（弧度/秒） + std::vector gyroInitValues_; // 陀螺仪初始位置 std::vector gyroVelocities_; // 陀螺仪速度（弧度/秒） std::vector gyroAccelerations_; // 陀螺仪加速度（弧度/秒^2） diff --git a/src/robot_control/include/robot_control_node.hpp b/src/robot_control/include/robot_control_node.hpp index bee59e3..645b116 100644 --- a/src/robot_control/include/robot_control_node.hpp +++ b/src/robot_control/include/robot_control_node.hpp @@ -16,6 +16,7 @@ #include "interfaces/action/move_waist.hpp" #include "interfaces/action/move_wheel.hpp" #include "interfaces/msg/motor_cmd.hpp" +#include "interfaces/srv/motor_param.hpp" using MoveHome = interfaces::action::MoveHome; using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle; @@ -31,6 +32,8 @@ using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle; using MotorCmd = interfaces::msg::MotorCmd; +using MotorParam = interfaces::srv::MotorParam; + namespace robot_control { class RobotControlNode : public rclcpp::Node @@ -81,6 +84,7 @@ namespace robot_control rclcpp::Publisher::SharedPtr motorCmdPub_; rclcpp::Subscription::SharedPtr wheelStatesSub_; rclcpp::Subscription::SharedPtr imuMsgSub_; + rclcpp::Client::SharedPtr motorParamClient_; RobotControlManager robotControlManager_; diff --git a/src/robot_control/include/wheel_control.hpp b/src/robot_control/include/wheel_control.hpp index d3c7ef0..7721436 100644 --- a/src/robot_control/include/wheel_control.hpp +++ b/src/robot_control/include/wheel_control.hpp @@ -22,6 +22,12 @@ namespace robot_control ~WheelControl() override; + double moveRatio_= 1.0; + + double lastMoveDistance = 0.0; + + double GetWheelRatioInternal(){ return moveRatio_ ;}; + bool SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle); bool MoveToTargetPoint(std::vector& joint_positions, const std::vector& target_pos, double duration) override; diff --git a/src/robot_control/src/control_base.cpp b/src/robot_control/src/control_base.cpp index a8096c3..b915a74 100644 --- a/src/robot_control/src/control_base.cpp +++ b/src/robot_control/src/control_base.cpp @@ -35,6 +35,8 @@ ControlBase::ControlBase( joint_acceleration_.resize(total_joints_, 0.0); joint_torque_.resize(total_joints_, 0.0); joint_position_desired_.resize(total_joints_, 0.0); + joint_commands_.resize(total_joints_, 0.0); + joint_velocity_commands_.resize(total_joints_, 0.0); // 初始化梯形轨迹规划器 trapezoidalTrajectory_ = new TrapezoidalTrajectory(maxSpeed, maxAcc); @@ -43,9 +45,12 @@ ControlBase::ControlBase( is_stopping_ = false; is_target_set_ = false; is_cmd_send_finished_ = false; + is_joint_position_initialized_ = false; stopDurationTime_ = 0.0; movingDurationTime_ = 0.0; + + time_out_count_ = 0; } ControlBase::~ControlBase() @@ -56,6 +61,7 @@ ControlBase::~ControlBase() void ControlBase::SetHomePositions(const std::vector& home_positions) { joint_home_positions_ = home_positions; + joint_position_desired_ = home_positions; } bool ControlBase::MoveToTargetJoint(std::vector& joint_positions, const std::vector& target_joint, double dt) @@ -88,12 +94,14 @@ bool ControlBase::MoveToTargetJoint(std::vector& joint_positions, const if (trapezoidalTrajectory_->isFinished(movingDurationTime_)) { joint_positions = target_joint; + joint_commands_ = target_joint; is_moving_ = false; movingDurationTime_ = 0.0; return true; } + joint_commands_ = desired_pos; joint_positions = desired_pos; joint_velocity_ = trapezoidalTrajectory_->getVelocity(); joint_acceleration_ = trapezoidalTrajectory_->getAcceleration(); @@ -146,6 +154,12 @@ void ControlBase::UpdateJointStates( // 2. 更新关节位置（同时同步关节角度，假设位置与角度单位一致） joint_position_ = joint_positions; + if( !is_joint_position_initialized_) + { + joint_commands_ = joint_position_; + is_joint_position_initialized_ = true; + } + // 3. 更新关节速度 joint_velocity_ = joint_velocities; diff --git a/src/robot_control/src/robot_control_manager.cpp b/src/robot_control/src/robot_control_manager.cpp index 21df672..2847acf 100644 --- a/src/robot_control/src/robot_control_manager.cpp +++ b/src/robot_control/src/robot_control_manager.cpp @@ -72,6 +72,7 @@ void RobotControlManager::init() isWheelHomed_ = false; isJointInitValueSet_ = false; + isGyroInited_ = false; } RobotControlManager::~RobotControlManager() @@ -129,6 +130,11 @@ Float64MultiArray RobotControlManager::GetWheelFeedback() return tempValues; } +double RobotControlManager::GetWheelRatio() +{ + return wheelController_->GetWheelRatioInternal(); +} + Float64MultiArray RobotControlManager::GetJointCommands() { Float64MultiArray tempValues; @@ -177,6 +183,21 @@ bool isAllTrueManual(const std::vector& vec) { return true; // 所有元素都是 true } + + +std::vector RobotControlManager::GetImuDifference() +{ + std::vector result; + result.resize(gyroValues_.size(), 0.0); + + for (size_t i = 0; i < gyroValues_.size(); i++) + { + result[i] = gyroValues_[i] - gyroInitValues_[i]; + } + + return result; +} + void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg) { imuMsg_ = *msg; @@ -202,6 +223,15 @@ void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg) gyroValues_[2] ); + if (!isGyroInited_) + { + gyroInitValues_ = gyroValues_; + isGyroInited_ = true; + + std::cout << "IMU values : " << gyroInitValues_[0] << " " << gyroInitValues_[1] << " " << gyroInitValues_[2] << std::endl; + } + + gyroVelocities_[0] = tempMsg.angular_velocity.x; gyroVelocities_[1] = tempMsg.angular_velocity.y; gyroVelocities_[2] = tempMsg.angular_velocity.z; @@ -210,7 +240,7 @@ void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg) gyroAccelerations_[1] = tempMsg.linear_acceleration.y; gyroAccelerations_[2] = tempMsg.linear_acceleration.z; - std::cout << "gyroValues_ : " << gyroValues_[0] << " " << gyroValues_[1] << " " << gyroValues_[2] << std::endl; + // std::cout << "gyroValues_ : " << gyroValues_[0] << " " << gyroValues_[1] << " " << gyroValues_[2] << std::endl; } diff --git a/src/robot_control/src/robot_control_node.cpp b/src/robot_control/src/robot_control_node.cpp index c30e5ea..ded0ade 100644 --- a/src/robot_control/src/robot_control_node.cpp +++ b/src/robot_control/src/robot_control_node.cpp @@ -80,6 +80,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node") wheelStatesSub_ = this->create_subscription("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, this, std::placeholders::_1)); joyCommandSub_ = this->create_subscription("/gamepad_msg", 10,std::bind(&RobotControlNode::JoyCommandCallback, this, std::placeholders::_1)); controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(CYCLE_TIME),std::bind(&RobotControlNode::ControlLoop, this)); // 绑定到新的控制函数); + motorParamClient_ = this->create_client("/motor_param"); lastTime_ = this->now(); // 初始化时间 std::cout << "RobotFsm Node is created finished!" << std::endl; @@ -392,11 +393,11 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_wheel_goal( return rclcpp_action::GoalResponse::REJECT; } - if (!robotControlManager_.SetMoveWheelParameters(goal->move_distance, goal->move_angle)) - { - RCLCPP_ERROR(this->get_logger(), "Invalid move wheel request"); - return rclcpp_action::GoalResponse::REJECT; - } + // if (!robotControlManager_.SetMoveWheelParameters(goal->move_distance, goal->move_angle)) + // { + // RCLCPP_ERROR(this->get_logger(), "Invalid move wheel request"); + // return rclcpp_action::GoalResponse::REJECT; + // } (void)uuid; @@ -430,6 +431,26 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr(); auto result = std::make_shared(); + double wheelAngle = abs(robotControlManager_.GetImuDifference()[2]) > 5.0 ? 0.0 : robotControlManager_.GetImuDifference()[2]; + + robotControlManager_.SetMoveWheelParameters(goal->move_distance, wheelAngle); + + if (goal->move_distance > 0) + // if (true) + { + double ratio = robotControlManager_.GetWheelRatio(); + + auto request = std::make_shared(); + request->motor_id = 1; + request->max_speed = (uint16_t)(ratio * 20); + request->add_acc = 5; + request->dec_acc = 5; + + motorParamClient_->async_send_request(request); + + rclcpp::sleep_for(std::chrono::milliseconds(1000)); + } + robotControlManager_.MoveWheel(); std_msgs::msg::Float64MultiArray wheel_commands; @@ -475,6 +496,71 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptrmove_distance > 0) + // // if (true) + // { + // robotControlManager_.SetMoveWheelParameters(goal->move_distance, robotControlManager_.GetImuDifference()[2] * 2.0 ); + + // std::cout << " IMU different : " << robotControlManager_.GetImuDifference()[2] << std::endl; + + // robotControlManager_.MoveWheel(); + + // wheel_commands = robotControlManager_.GetWheelCommands(); + + // wheel_commands_msg.target = "rs485"; + // wheel_commands_msg.type = "bm"; + // wheel_commands_msg.position = ""; + // wheel_commands_msg.motor_id = {1,2}; + // wheel_commands_msg.motor_angle = {(float)(wheel_commands.data[0]),(float)(wheel_commands.data[1])}; + + // std::cout << "wheel_commands.data[0] " << wheel_commands.data[0] << std::endl; + // std::cout << "wheel_commands.data[1] " << wheel_commands.data[1] << std::endl; + + // motorCmdPub_->publish(wheel_commands_msg); + + // while (move_wheel_executing_ && rclcpp::ok()) + // { + // if (goal_handle->is_canceling()) + // { + // result->success = false; + // goal_handle->canceled(result); + // RCLCPP_INFO(this->get_logger(), "move wheel canceled"); + // move_wheel_executing_ = false; + // //TODO: ADD STOP LOGIC + // return; + // } + + // auto joint_feedback = robotControlManager_.GetWheelFeedback(); + + // if (joint_feedback.data[0] - wheel_commands.data[0] < 7.0 && joint_feedback.data[1] - wheel_commands.data[1] < 7.0) + // { + // move_wheel_executing_ = false; + // } + + // feedback->current_pos = joint_feedback.data[0]; + + // //TODO: get the angle from lidar. + // feedback->current_angle = 0.0; + + // goal_handle->publish_feedback(feedback); + + // loop_rate.sleep(); + // } + // } + + if (goal->move_distance > 0) + // if (true) + { + auto request = std::make_shared(); + request->motor_id = 1; + request->max_speed = 20; + request->add_acc = 5; + request->dec_acc = 5; + + motorParamClient_->async_send_request(request); + } + // 检查目标是否仍在活跃状态 if (rclcpp::ok()) { diff --git a/src/robot_control/src/waist_control.cpp b/src/robot_control/src/waist_control.cpp index 528228b..47afba9 100644 --- a/src/robot_control/src/waist_control.cpp +++ b/src/robot_control/src/waist_control.cpp @@ -69,15 +69,17 @@ bool WaistControl::MoveWaist(std::vector& joint_positions, double dt) } else { - if (IsReached(joint_position_desired_)) + if (IsReached(joint_position_desired_) || (time_out_count_ > TIME_OUT_COUNT)) { std::cout << "Waist reached target position!" << std::endl; is_target_set_ = false; is_cmd_send_finished_ = false; + time_out_count_ = 0; return true; } else { + time_out_count_ ++; // std::cout << "Waist not reached target position!" << std::endl; return false; } @@ -106,8 +108,8 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey std::vector tempPosition; tempPosition.resize(total_joints_, 0.0); - tempPosition[0] = joint_position_[0] - joint_home_positions_[0] + movepitch; - tempPosition[1] = joint_position_[1] - joint_home_positions_[1] + moveyaw; + tempPosition[0] = joint_commands_[0] - joint_home_positions_[0] + movepitch; + tempPosition[1] = joint_commands_[1] - joint_home_positions_[1] + moveyaw; if (!checkJointLimits(tempPosition)) { diff --git a/src/robot_control/src/wheel_control.cpp b/src/robot_control/src/wheel_control.cpp index 1317da6..cc8a625 100644 --- a/src/robot_control/src/wheel_control.cpp +++ b/src/robot_control/src/wheel_control.cpp @@ -34,40 +34,69 @@ WheelControl::~WheelControl() bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle) { + double original1 = joint_position_desired_[0]; + double original2 = joint_position_desired_[1]; + + std::cout << "Original joint_position_desired_: " << original1 << " , " << original2 << std::endl; + // 设置移动轮参数 double theta = moveWheelDistance / lengthParameters_[0] * 180.0 / M_PI; + double beta = moveWheelAngle / 360.0 * M_PI * 2 * lengthParameters_[1] / lengthParameters_[0] * 180.0 / M_PI; + + std::cout << "Move Wheel theta: " << theta << " beta: " << beta << std::endl; + + if(moveWheelDistance > lastMoveDistance && beta != 0) + { + for (size_t i = 0; i < joint_directions_.size(); i++) + { + joint_home_positions_[i] = joint_home_positions_[i] - beta; + } + + double additionDistance = 0.001 / lengthParameters_[0] * 180.0 / M_PI; + + joint_home_positions_[0] = joint_home_positions_[0] + additionDistance; + joint_home_positions_[1] = joint_home_positions_[1] - additionDistance; + } + for (size_t i = 0; i < joint_directions_.size(); i++) { if (joint_directions_[i] == 1) { - if (theta < 0.0) - { - joint_position_desired_[i] = joint_home_positions_[i] + theta; - } - else - { - joint_position_desired_[i] = joint_home_positions_[i] + theta; // * 1.02; - } + joint_position_desired_[i] = joint_home_positions_[i] + theta; } else { - if (theta < 0.0) - { - joint_position_desired_[i] = joint_home_positions_[i] - theta; - } - else - { - joint_position_desired_[i] = joint_home_positions_[i] - theta; // * 0.98 * 1.02; - } + joint_position_desired_[i] = joint_home_positions_[i] - theta; // * 0.98 * 1.02; } } - //TODO: 设置移动轮角度 - (void) moveWheelAngle; + std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] << " , " << joint_position_desired_[1] << std::endl; + + double a1 = abs(joint_position_desired_[0] - original1); + double a2 = abs(joint_position_desired_[1] - original2); + + if (a2 > 0.0 && a1 > 0.0) + { + moveRatio_ = a1 / a2; + } + else + { + moveRatio_ = 1.0; + } + + if (moveRatio_ > 1.3 || moveRatio_ < 0.7) + { + moveRatio_ = 1.0; + } + + + std::cout << "Set move ratio : " << moveRatio_ << std::endl; std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] << " , " << joint_position_desired_[1] << std::endl; + lastMoveDistance = moveWheelDistance; + is_target_set_ = true; return true;