HiveCoreRD/hivecore_robot_motion
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wheel optimization

Browse Source
This commit is contained in:
NuoDaJia02
2025-11-02 15:43:08 +08:00
parent 9520d275a8
commit 55dd17e055
3 changed files with 32 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/robot_control/src/robot_control_manager.cpp
View File

@@ -194,6 +194,9 @@ std::vector<double> RobotControlManager::GetImuDifference()
{
result[i] = gyroValues_[i] - gyroInitValues_[i];
}
//TODO: optimization
gyroInitValues_[2] = gyroInitValues_[2] - 0.1;
return result;
}

116
src/robot_control/src/robot_control_node.cpp
View File

@@ -19,6 +19,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
jogIndex_ = 0;
// 初始化数据文件（设置路径，确保目录存在）
#if 0
data_file_path_ = "/home/demo/ros2_joint_data.txt"; // 数据保存路径
fs::path file_path(data_file_path_);
fs::create_directories(file_path.parent_path()); // 自动创建父目录（如果不存在）
@@ -29,6 +30,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
// 写入表头（仅在文件为空时）
data_file_.seekp(0, std::ios::end); // 移动到文件末尾
}
#endif
using namespace std::placeholders;
@@ -97,8 +99,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_home_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const MoveHome::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received move home request");
(void)goal;
if (robotControlManager_.IsMoving(MovementPart::ALL))
@@ -127,7 +127,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_home_goal(
rclcpp_action::CancelResponse RobotControlNode::handle_move_home_cancel(
const std::shared_ptr<GoalHandleMoveHome> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
@@ -136,7 +135,6 @@ rclcpp_action::CancelResponse RobotControlNode::handle_move_home_cancel(
void RobotControlNode::handle_move_home_accepted(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
{
move_home_executing_ = true;
RCLCPP_INFO(this->get_logger(), "Goal accepted");
using namespace std::placeholders;
std::thread{std::bind(&RobotControlNode::move_home_execute, this, _1), goal_handle}.detach();
@@ -188,8 +186,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_leg_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const MoveLeg::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received move leg request");
if (robotControlManager_.IsMoving(MovementPart::LEG))
{
RCLCPP_ERROR(this->get_logger(), "Robot is moving");
@@ -223,7 +219,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_leg_goal(
rclcpp_action::CancelResponse RobotControlNode::handle_move_leg_cancel(
const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
@@ -232,7 +227,6 @@ rclcpp_action::CancelResponse RobotControlNode::handle_move_leg_cancel(
void RobotControlNode::handle_move_leg_accepted(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
{
move_leg_executing_ = true;
RCLCPP_INFO(this->get_logger(), "Goal accepted");
using namespace std::placeholders;
std::thread{std::bind(&RobotControlNode::move_leg_execute, this, _1), goal_handle}.detach();
@@ -284,8 +278,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_waist_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const MoveWaist::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received move waist request");
if (robotControlManager_.IsMoving(MovementPart::WAIST))
{
RCLCPP_ERROR(this->get_logger(), "Robot is moving");
@@ -319,7 +311,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_waist_goal(
rclcpp_action::CancelResponse RobotControlNode::handle_move_waist_cancel(
const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
@@ -328,7 +319,6 @@ rclcpp_action::CancelResponse RobotControlNode::handle_move_waist_cancel(
void RobotControlNode::handle_move_waist_accepted(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
{
move_waist_executing_ = true;
RCLCPP_INFO(this->get_logger(), "Goal accepted");
using namespace std::placeholders;
std::thread{std::bind(&RobotControlNode::move_waist_execute, this, _1), goal_handle}.detach();
}
@@ -379,8 +369,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_wheel_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const MoveWheel::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received move wheel request");
if (robotControlManager_.IsMoving(MovementPart::WHEEL))
{
RCLCPP_ERROR(this->get_logger(), "Robot is moving");
@@ -408,7 +396,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_wheel_goal(
rclcpp_action::CancelResponse RobotControlNode::handle_move_wheel_cancel(
const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
@@ -431,25 +418,26 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
auto feedback = std::make_shared<MoveWheel::Feedback>();
auto result = std::make_shared<MoveWheel::Result>();
double wheelAngle = abs(robotControlManager_.GetImuDifference()[2]) > 5.0 ? 0.0 : robotControlManager_.GetImuDifference()[2];
double tempValue = robotControlManager_.GetImuDifference()[2];
double wheelAngle = abs(tempValue) > 40.0 ? 0.0 : tempValue;
robotControlManager_.SetMoveWheelParameters(goal->move_distance, wheelAngle);
if (goal->move_distance > 0)
// if (true)
{
double ratio = robotControlManager_.GetWheelRatio();
double ratio = robotControlManager_.GetWheelRatio();
auto request = std::make_shared<MotorParam::Request>();
request->motor_id = 1;
request->max_speed = (uint16_t)(ratio * 20);
request->add_acc = 5;
request->dec_acc = 5;
auto request = std::make_shared<MotorParam::Request>();
motorParamClient_->async_send_request(request);
request->motor_id = 1;
request->max_speed = static_cast<uint16_t>(round(ratio * 40));
request->add_acc = 10;
request->dec_acc = 10;
motorParamClient_->async_send_request(request);
rclcpp::sleep_for(std::chrono::milliseconds(1000));
}
std::cout << "request->max_speed : " << request->max_speed << std::endl;
rclcpp::sleep_for(std::chrono::milliseconds(1000));
robotControlManager_.MoveWheel();
@@ -490,76 +478,21 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
feedback->current_pos = joint_feedback.data[0];
//TODO: get the angle from lidar.
feedback->current_angle = 0.0;
//feedback->current_angle = robotControlManager_.GetImuDifference()[2];
goal_handle->publish_feedback(feedback);
loop_rate.sleep();
}
// if (goal->move_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();
// }
// }
request->motor_id = 1;
request->max_speed = 40;
request->add_acc = 10;
request->dec_acc = 10;
if (goal->move_distance > 0)
// if (true)
{
auto request = std::make_shared<MotorParam::Request>();
request->motor_id = 1;
request->max_speed = 20;
request->add_acc = 5;
request->dec_acc = 5;
motorParamClient_->async_send_request(request);
motorParamClient_->async_send_request(request);
}
std::cout << "request->max_speed : " << request->max_speed << std::endl;
// 检查目标是否仍在活跃状态
if (rclcpp::ok())
@@ -635,6 +568,7 @@ void RobotControlNode::Publish_joint_trajectory()
cmd_msg = robotControlManager_.GetJointCommands();
}
#if 0
data_file_ << 0;
// 2. 保存整个数组数据到txt文件
@@ -647,6 +581,8 @@ void RobotControlNode::Publish_joint_trajectory()
data_file_.flush(); // 强制刷新
}
#endif
std_msgs::msg::String positionMsg;
std::stringstream msg_stream;
size_t total_joints = robotControlManager_.GetMotionParameters().total_joints_;

12
src/robot_control/src/wheel_control.cpp
View File

@@ -44,10 +44,12 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
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;
std::cout << "moveWheelDistance: " << moveWheelDistance << std::endl;
if(moveWheelDistance > lastMoveDistance && beta != 0)
{
std::cout << " beta: " << beta << std::endl;
for (size_t i = 0; i < joint_directions_.size(); i++)
{
joint_home_positions_[i] = joint_home_positions_[i] - beta;
@@ -71,8 +73,6 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
}
}
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);
@@ -85,12 +85,6 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
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;