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

fix some issues

Browse Source
This commit is contained in:
NuoDaJia
2025-09-19 15:28:51 +08:00
parent b0e8767353
commit 6c68ade5c7
21 changed files with 589 additions and 178 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/src/ethercat_driver.cpp
View File

@@ -302,8 +302,6 @@ CallbackReturn EthercatDriver::on_activate(
}
// start EC and wait until state operative
control_frequency_=100.0f;
std::cout<<"force set freq"<<std::endl;
master_.setCtrlFrequency(control_frequency_);
std::cout << "DRV: ----------abc-------------" << master_.getInterval() << std::endl;
for (auto i = 0ul; i < ec_modules_.size(); i++) {

2
HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp
View File

@@ -239,7 +239,9 @@ uint16_t EcCiA402Drive::transition(DeviceState state, uint16_t control_word)
case STATE_FAULT_REACTION_ACTIVE: // -> STATE_FAULT (automatic)
return control_word;
case STATE_FAULT: // -> STATE_SWITCH_ON_DISABLED
// std::cout << "EcCiA402Drive: Fault detected. Resetting drive." << std::endl;
if (auto_fault_reset_ || fault_reset_) {
// if (false || fault_reset_) {
fault_reset_ = false;
return (control_word & 0b11111111) | 0b10000000; // automatic reset
} else {

2
HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ec_master.hpp
View File

@@ -83,9 +83,7 @@ public:
void setCtrlFrequency(double frequency)
{
std::cout << "set control frequency to " << frequency << std::endl;
interval_ = 1000000000.0 / frequency;
std::cout << "new interval_ " << interval_<< std::endl;
}
uint32_t getInterval() {return interval_;}

12
HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/config/controllers.yaml
View File

@@ -1,6 +1,6 @@
controller_manager:
ros__parameters:
update_rate: 100 # Hz
update_rate: 1000 # Hz
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
@@ -20,6 +20,16 @@ controller_manager:
# - position
# joints:
# - joint_1
# - joint_2
# - joint_3
# - joint_4
# - joint_5
# - joint_6
# - joint_7
# - joint_8
# - joint_9
# - joint_10
# - joint_11
gpio_command_controller:
ros__parameters:

2
HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/config/zeroErr_config.yaml
View File

@@ -2,7 +2,7 @@
vendor_id: 0x5a65726f
product_id: 0x00029252
assign-activate: 0x0300 #DC synch register
auto_fault_reset: true
auto_fault_reset: false
auto_enable_set: false
# sdo: # sdo data to be transferred at drive startup

2
HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/description/ros2_control/motor_drive.ros2_control.xacro
View File

@@ -3,7 +3,7 @@
<!-- 公共参数定义（一次修改，全局生效） -->
<xacro:property name="master_id" value="0" />
<xacro:property name="control_frequency" value="100" />
<xacro:property name="control_frequency" value="1000" />
<xacro:property name="ec_plugin" value="ethercat_generic_plugins/EcCiA402Drive" />
<xacro:property name="alias" value="0" />
<xacro:property name="mode_of_operation" value="8" />

11
HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/launch/motor_drive.launch.py
View File

@@ -89,11 +89,11 @@ def generate_launch_description():
arguments=["gpio_command_controller", "-c", "/controller_manager"],
)
# trajectory_controller_spawner = Node(
# package="controller_manager",
# executable="spawner",
# arguments=["trajectory_controller", "-c", "/controller_manager"],
# )
trajectory_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["trajectory_controller", "-c", "/controller_manager"],
)
# velocity_controller_spawner = Node(
# package="controller_manager",
@@ -111,6 +111,7 @@ def generate_launch_description():
control_node,
robot_state_pub_node,
joint_state_broadcaster_spawner,
# trajectory_controller_spawner,
gpio_controller_spawner,
]

28
HiveCoreR0/src/joy_control/joy_control/joy_control_node.py
View File

@@ -59,9 +59,21 @@ class JoyControlNode(Node):
if msg.buttons[0] == 1:
command = 'A'
self.command_publisher.publish(String(data=command))
if msg.buttons[1] == 1:
elif msg.buttons[1] == 1:
command = 'B'
self.command_publisher.publish(String(data=command))
elif msg.buttons[2] == 1:
command = 'X'
self.command_publisher.publish(String(data=command))
elif msg.buttons[3] == 1:
command = 'Y'
self.command_publisher.publish(String(data=command))
elif msg.buttons[4] == 1:
command = 'LB'
self.command_publisher.publish(String(data=command))
elif msg.buttons[5] == 1:
command = 'RB'
self.command_publisher.publish(String(data=command))
# 处理摇杆（死区0.1）
deadzone = 0.1
@@ -80,6 +92,14 @@ class JoyControlNode(Node):
if msg.axes[1] != self.previous_axes[1] and msg.axes[1] < - deadzone :
command = '后退'
self.command_publisher.publish(String(data=command))
if msg.axes[6] != self.previous_axes[6] and msg.axes[6] > deadzone :
command = '抬手'
self.command_publisher.publish(String(data=command))
if msg.axes[6] != self.previous_axes[6] and msg.axes[6] < - deadzone :
command = '放手'
self.command_publisher.publish(String(data=command))
if msg.axes[7] != self.previous_axes[7] and msg.axes[7] > deadzone :
command = '上升'
@@ -89,8 +109,8 @@ class JoyControlNode(Node):
command = '下降'
self.command_publisher.publish(String(data=command))
if msg.axes[0] != self.previous_axes[0] or msg.axes[1] != self.previous_axes[1] or msg.axes[7] != self.previous_axes[7]:
if msg.axes[0] == 0.0 and msg.axes[1] == 0.0 and msg.axes[7] == 0.0:
if msg.axes[0] != self.previous_axes[0] or msg.axes[1] != self.previous_axes[1] or msg.axes[7] != self.previous_axes[7] or msg.axes[6] != self.previous_axes[6]:
if msg.axes[0] == 0.0 and msg.axes[1] == 0.0 and msg.axes[7] == 0.0 and msg.axes[6] == 0.0:
command = '停止'
self.command_publisher.publish(String(data=command))
@@ -109,4 +129,4 @@ def main(args=None):
if __name__ == '__main__':
main()

4
HiveCoreR0/src/robot_control/include/arm_control.hpp
View File

@@ -31,6 +31,10 @@ namespace robot_control
bool Stop(std::vector<double>& joint_positions, double dt);
bool MoveUp(std::vector<double>& joint_positions, double dt);
bool MoveDown(std::vector<double>& joint_positions, double dt);
void UpdateJointStates(const std::vector<double>& joint_positions, const std::vector<double>& joint_velocities, const std::vector<double>& joint_torques);
bool IsMoving();

4
HiveCoreR0/src/robot_control/include/common_enum.hpp
View File

@@ -16,6 +16,8 @@ namespace robot_control
TURNING_RIGHT, // 右转
MOVING_UP, // 上升（腰部）
MOVING_DOWN, // 下降（腰部）
ARM_MOVING_UP, // 手臂上升
ARM_MOVING_DOWN, // 手臂下降
HOME_MOVING // 回零
};
@@ -30,6 +32,8 @@ namespace robot_control
RIGHT, // 右转
UP, // 上升
DOWN, // 下降
HAND_UP, // 手臂上升
HAND_DOWN, // 手臂下降
STOP, // 停止
ARM_GRAB, // 手臂抓取
GO_HOME, // 关节回零

27
HiveCoreR0/src/robot_control/include/motion_parameters.hpp
View File

@@ -45,26 +45,26 @@ namespace robot_control
arm_length_l2_ = 0.219;
// 轮子速度参数
max_linear_speed_x_ = 10;
max_linear_speed_x_ = 100;
max_linear_speed_z_ = 10;
max_angular_speed_z_ = 50;
// 关节速度参数
max_joint_velocity_ = 10; // 10 度/秒
max_joint_acceleration_ = 100; // 10 度/秒^2
max_joint_velocity_ = 5; // 10 度/秒
max_joint_acceleration_ = 25; // 10 度/秒^2
// 初始化关节索引
wheel_joint_indices_ = {0, 1, 2, 3};
pitch_body_indices_ = {0};
right_leg_joint_indices_ = {1};
left_leg_joint_indices_ = {2};
right_arm_joint_indices_ = {3, 4, 5, 6};
left_arm_joint_indices_ = {7, 8, 9, 10};
wheel_joint_indices_ = {0, 1};
pitch_body_indices_ = {8};
right_leg_joint_indices_ = {9};
left_leg_joint_indices_ = {10};
right_arm_joint_indices_ = {0, 1, 2, 3};
left_arm_joint_indices_ = {4, 5, 6, 7};
total_joints_ = 11 ; //left_arm_joint_indices_.size() + right_arm_joint_indices_.size() + pitch_body_indices_.size() + left_leg_joint_indices_.size() + right_leg_joint_indices_.size();
total_joints_ = 11 ; //left_arm_joint_indices_.size() + right_arm_joint_indices_.size() + pitch_body_indices_.size() + left_leg_joint_indices_.size() + right_leg_joint_indices_.size();
// 初始化关节方向
wheel_joint_directions_ = {1, -1, 1, -1};
wheel_joint_directions_ = {-1, 1};
left_arm_joint_directions_ = {1, 1, 1, 1};
right_arm_joint_directions_ = {1, 1, 1, 1};
pitch_body_directions_ = {1};
@@ -152,6 +152,8 @@ namespace robot_control
pulse_to_radian_ = 2.0 * M_PI / 524288.0;
radian_to_pulse_ = 524288.0 / (2.0 * M_PI);
jog_step_size_ = 10 * pulse_to_degree_;
serial_port_ = "/dev/ttyUSB0";
baud_rate_ = 115200;
@@ -210,6 +212,7 @@ namespace robot_control
double max_joint_velocity_;
double max_joint_acceleration_;
double jog_step_size_;
// 运动状态
std::vector<double> joint_positions_; // 关节位置（弧度）

11
HiveCoreR0/src/robot_control/include/robot_control_manager.hpp
View File

@@ -26,12 +26,19 @@ namespace robot_control
void TurnRight();
void MoveUp(double dt);
void MoveDown(double dt);
void ArmMoveUp(double dt);
void ArmMoveDown(double dt);
bool Stop(double dt);
bool GoHome(double dt);
void JogAxis(int axis, int dir);
// void ArmGrab(double dt);
void UpdateInitCommand();
// 机器人关节指令
std_msgs::msg::Float64MultiArray GetJointCommands();
std_msgs::msg::Float64MultiArray GetJointInitCommands();
void CheckJointLimits(std_msgs::msg::Float64MultiArray& jointCommands);
void UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg);
@@ -66,6 +73,10 @@ namespace robot_control
sensor_msgs::msg::JointState jointStates_;
std_msgs::msg::Float64MultiArray jointCommands_;
std_msgs::msg::Float64MultiArray jointCommandsInit_;
void AssignTempValues();
void UpdateJointCommands();
};

18
HiveCoreR0/src/robot_control/include/robot_fsm.hpp
View File

@@ -36,7 +36,9 @@ namespace robot_control
// 状态转换为字符串
std::string StateToString(RobotState state) const;
void EnableRobot();
void EnableRobot(double value);
void ResetFault(double value);
int motor_dst(std::string name,double dst);//add by hehe
private:
rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr cmdPub_;
@@ -62,10 +64,20 @@ namespace robot_control
bool isPaused_;
bool isFinished_;
bool isRobotEnabled_;
bool isFaultReset_;
bool isJogMode_;
bool enableCommandExecuted_;
// bool resetFault_;
int jogDirection_;
int jogIndex_;
double jogValue_;
//add by hehe
std::unordered_map<std::string,double> curMap_;
std::unordered_map<std::string,double> dstMap_;
control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
sensor_msgs::msg::JointState curJointSta_;
//add by hehe end
// 状态转换表
std::map<std::pair<RobotState, ControlCommand>, RobotState> transitionTable_;
@@ -88,4 +100,4 @@ namespace robot_control
} // namespace robot_control
#endif // ROBOT_FSM_H
#endif // ROBOT_FSM_H

4
HiveCoreR0/src/robot_control/include/rs485_driver.hpp
View File

@@ -10,7 +10,7 @@ namespace robot_control
class RS485Driver
{
public:
RS485Driver(size_t max_motors = 4);
RS485Driver(size_t max_motors = 2);
~RS485Driver();
// 打开串口
@@ -45,6 +45,8 @@ namespace robot_control
bool is_open_;
struct termios original_termios_;
size_t max_motors_;
size_t control_index_;
// 转换波特率
speed_t convertBaudRate(int baud_rate);

4
HiveCoreR0/src/robot_control/include/wheel_control.hpp
View File

@@ -20,7 +20,9 @@ namespace robot_control
void setLinearAngularSpeed(double linearSpeed, double angularSpeed);
void ExecuteCommand();
bool ExecuteCommand();
double GetLinearSpeed() {return linearSpeed_;}
private:

42
HiveCoreR0/src/robot_control/src/arm_control.cpp
View File

@@ -121,6 +121,46 @@ bool ArmControl::MoveToTargetJoint(std::vector<double>& joint_positions, const s
return false;
}
bool ArmControl::MoveUp(std::vector<double>& joint_positions, double dt)
{
if (!is_moving_)
{
for (size_t i = 0; i < joint_position_.size(); i++)
{
if (joint_directions_[i] == 1)
{
joint_position_desired_[i] = joint_position_[i] + 20;
}
else
{
joint_position_desired_[i] = joint_position_[i] - 20;
}
}
}
return MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
}
bool ArmControl::MoveDown(std::vector<double>& joint_positions, double dt)
{
if (!is_moving_)
{
for (size_t i = 0; i < joint_position_.size(); i++)
{
if (joint_directions_[i] == 1)
{
joint_position_desired_[i] = joint_position_[i] - 20;
}
else
{
joint_position_desired_[i] = joint_position_[i] + 20;
}
}
}
return MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
}
bool ArmControl::GoHome(std::vector<double>& joint_positions, double dt)
{
bool all_joints_at_home = true; // 标记是否所有关节都在零点
@@ -195,9 +235,7 @@ bool ArmControl::Stop(std::vector<double>& joint_positions, double dt)
if (is_moving_ && !is_stopping_)
{
is_stopping_ = true;
std::cout << "arm control stop1" << std::endl;
trapezoidalTrajectory_->StopCalculate();
std::cout << "arm control stop2" << std::endl;
stopDurationTime_ = 0.0;
}

8
HiveCoreR0/src/robot_control/src/leg_control.cpp
View File

@@ -108,11 +108,11 @@ bool LegControl::MoveUp(std::vector<double>& joint_positions, double dt)
{
if (joint_directions_[i] == 1)
{
joint_position_desired_[i] = maxLimits_[i];
joint_position_desired_[i] = joint_position_[i] + 20;
}
else
{
joint_position_desired_[i] = minLimits_[i];
joint_position_desired_[i] = joint_position_[i] - 20;
}
}
}
@@ -128,11 +128,11 @@ bool LegControl::MoveDown(std::vector<double>& joint_positions, double dt)
{
if (joint_directions_[i] == 1)
{
joint_position_desired_[i] = minLimits_[i];
joint_position_desired_[i] = joint_position_[i] - 20;
}
else
{
joint_position_desired_[i] = maxLimits_[i];
joint_position_desired_[i] = joint_position_[i] + 20;
}
}
}

234
HiveCoreR0/src/robot_control/src/robot_control_manager.cpp
View File

@@ -21,6 +21,7 @@ void RobotControlManager::init()
for (size_t i = 0; i < motionParams_.total_joints_; i++)
{
jointCommands_.data.push_back(0.0);
jointCommandsInit_.data.push_back(0.0);
}
leftArmController_ = new ArmControl(
@@ -112,6 +113,31 @@ Float64MultiArray RobotControlManager::GetJointCommands()
return tempValues;
}
Float64MultiArray RobotControlManager::GetJointInitCommands()
{
Float64MultiArray tempValues;
tempValues.data.resize(motionParams_.total_joints_);
// Convert the joint commands to motor values and limit to encoder range
for (size_t i = 0; i < jointCommandsInit_.data.size(); i++)
{
double angle = jointCommandsInit_.data[i];
double normalizedAngle = fmod(angle, 360.0);
// 计算原始脉冲值
double pulseValue = normalizedAngle * motionParams_.degree_to_pulse_;
tempValues.data[i] = pulseValue;
}
return tempValues;
}
void RobotControlManager::UpdateInitCommand()
{
jointCommandsInit_ = jointCommands_;
}
void RobotControlManager::CheckJointLimits(Float64MultiArray &tempJointValues)
{
@@ -139,8 +165,7 @@ bool isAllTrueManual(const std::vector<bool>& vec) {
return true; // 所有元素都是 true
}
bool isPrint = false;
bool isValueSet = false;
void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg)
{
jointStates_ = *msg;
@@ -150,36 +175,66 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
for (size_t i = 0; i < motionParams_.total_joints_; i++)
{
jointPositions_[i] = jointStates_.position[i] * motionParams_.pulse_to_degree_; // convert to degree
jointVelocities_[i] = jointStates_.velocity[i] * motionParams_.pulse_to_degree_;
jointEfforts_[i] = 0.0; //jointStates_.effort[i] ; // TODO: check the factory
std::string joint_name = jointStates_.name[i];
int index = -1;
size_t underscore_pos = joint_name.find_last_of('_');
if (underscore_pos != std::string::npos && underscore_pos < joint_name.size() - 1)
{
std::string num_str = joint_name.substr(underscore_pos + 1);
try
{
index = std::stoi(num_str) - 1;
}
catch (const std::invalid_argument& e)
{
std::cerr << "关节名称 " << joint_name << " 格式错误：下划线后非数字" << std::endl;
}
}
if (index != -1 && index < motionParams_.total_joints_)
{
jointPositions_[index] = jointStates_.position[i] * motionParams_.pulse_to_degree_;
jointVelocities_[index] = jointStates_.velocity[i] * motionParams_.pulse_to_degree_;
jointEfforts_[index] = 0.0;
}
// else do not update.
}
// TODO: This block can be deleted
for (size_t i = 0; i < motionParams_.total_joints_; i++)
{
if(!jointInited_[i])
{
jointCommands_.data[i] = jointStates_.position[i]* motionParams_.pulse_to_degree_;
if(jointCommands_.data[i] != 0)
if(jointPositions_[i] != 0)
{
jointInited_[i] = true;
}
}
}
if (isAllTrueManual(jointInited_) && !isPrint)
static int count = 0;
if (isAllTrueManual(jointInited_) && !isValueSet)
{
isPrint = true;
std::cout << "All joints are initialized" << std::endl;
for (size_t i = 0; i < motionParams_.total_joints_; i++)
if (count > 5000)
{
std::cout << "Joint positions: " << i + 1 << " " << jointCommands_.data[i] << std::endl;
jointCommands_.data = jointPositions_;
jointCommandsInit_.data = jointPositions_;
isValueSet = true;
count = 0;
std::cout << "Joint commands set to joint positions" << std::endl;
std::cout << "All joints are initialized" << std::endl;
for (size_t i = 0; i < motionParams_.total_joints_; i++)
{
std::cout << "Joint positions: " << i + 1 << " " << jointCommands_.data[i] << std::endl;
}
}
count++;
}
// Update the arm controller
size_t armJointSize = motionParams_.left_arm_joint_indices_.size();
std::vector<double> armPositions(jointPositions_.begin() + motionParams_.left_arm_joint_indices_[0], jointPositions_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize);
@@ -225,85 +280,96 @@ void RobotControlManager::MoveBackward()
void RobotControlManager::TurnLeft()
{
wheelController_ -> setLinearAngularSpeed(0.0, - motionParams_.max_angular_speed_z_);
wheelController_ -> setLinearAngularSpeed(0.0, motionParams_.max_angular_speed_z_);
wheelController_ -> ExecuteCommand();
}
void RobotControlManager::TurnRight()
{
wheelController_ -> setLinearAngularSpeed(0.0, motionParams_.max_angular_speed_z_);
wheelController_ -> setLinearAngularSpeed(0.0, - motionParams_.max_angular_speed_z_);
wheelController_ -> ExecuteCommand();
}
bool executeFinished = true;
bool RobotControlManager::Stop(double dt)
{
wheelController_ -> setLinearAngularSpeed(0.0, 0.0);
wheelController_ -> ExecuteCommand();
double currentSpeed = wheelController_ -> GetLinearSpeed();
bool wheelStopped = abs(currentSpeed) > 0 ? false : true;
if(executeFinished)
{
double tempSpeed = 0.0;
if(currentSpeed < -1.0)
{
tempSpeed = currentSpeed + 1.0;
}
else if(currentSpeed > 5.0)
{
tempSpeed = currentSpeed - 1.0;
}
wheelController_ -> setLinearAngularSpeed(tempSpeed, 0.0);
// std::cout << "set speed : " << tempSpeed << std::endl;
}
executeFinished = wheelController_ -> ExecuteCommand();
AssignTempValues();
bool leftLegStopped = leftLegController_->Stop(tempLeftLegCmd, dt);
bool rightLegStopped = rightLegController_->Stop(tempRightLegCmd, dt);
bool leftArmStopped = leftArmController_->Stop(tempLeftArmCmd, dt);
bool rightArmStopped = rightArmController_->Stop(tempRightArmCmd, dt);
UpdateJointCommands();
for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i];
}
for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i];
}
//TODO: there is no data from arm
// for (size_t i = 0; i < motionParams_.left_arm_joint_indices_.size(); i++)
// {
// jointCommands_.data[motionParams_.left_arm_joint_indices_[i]] = motionParams_.left_arm_joint_directions_[i] * tempLeftArmCmd[i];
// }
// for (size_t i = 0; i < motionParams_.right_arm_joint_indices_.size(); i++)
// {
// jointCommands_.data[motionParams_.right_arm_joint_indices_[i]] = motionParams_.right_arm_joint_directions_[i] * tempRightArmCmd[i];
// }
return leftLegStopped && rightLegStopped && leftArmStopped && rightArmStopped;
return leftLegStopped && rightLegStopped && leftArmStopped && rightArmStopped && wheelStopped && executeFinished;
}
void RobotControlManager::MoveUp(double dt)
{
AssignTempValues();
leftLegController_ -> MoveUp(tempLeftLegCmd, dt);
rightLegController_ -> MoveUp(tempRightLegCmd, dt);
wheelController_-> setLinearAngularSpeed(5.0, 0.0); //TODO: value need to be changed
wheelController_ -> ExecuteCommand();
for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i];
}
for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i];
}
UpdateJointCommands();
}
void RobotControlManager::MoveDown(double dt)
{
AssignTempValues();
leftLegController_ -> MoveDown(tempLeftLegCmd, dt);
rightLegController_ -> MoveDown(tempRightLegCmd, dt);
wheelController_-> setLinearAngularSpeed(-5.0, 0.0); //TODO: value need to be changed
wheelController_ -> ExecuteCommand();
for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i];
}
for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i];
}
UpdateJointCommands();
}
void RobotControlManager::ArmMoveUp(double dt)
{
AssignTempValues();
leftArmController_ -> MoveUp(tempLeftArmCmd, dt);
rightArmController_ -> MoveUp(tempRightArmCmd, dt);
UpdateJointCommands();
}
void RobotControlManager::ArmMoveDown(double dt)
{
AssignTempValues();
leftArmController_ -> MoveDown(tempLeftArmCmd, dt);
rightArmController_ -> MoveDown(tempRightArmCmd, dt);
UpdateJointCommands();
}
bool RobotControlManager::GoHome(double dt)
@@ -346,3 +412,59 @@ bool RobotControlManager::GoHome(double dt)
return robotHomed_[0] && robotHomed_[1];// && homeFinished[2] && homeFinished[3];
}
void RobotControlManager::JogAxis(int axis, int direction)
{
if(axis < 0 || axis > motionParams_.total_joints_)
{
return;
}
jointCommands_.data[axis] += direction * motionParams_.jog_step_size_;
}
void RobotControlManager::AssignTempValues()
{
tempLeftArmCmd.resize(motionParams_.left_arm_joint_indices_.size());
tempRightArmCmd.resize(motionParams_.right_arm_joint_indices_.size());
tempLeftLegCmd.resize(motionParams_.left_leg_joint_indices_.size());
tempRightLegCmd.resize(motionParams_.right_leg_joint_indices_.size());
for (size_t i = 0; i < motionParams_.left_arm_joint_indices_.size(); i++)
{
tempLeftArmCmd[i] = jointCommands_.data[motionParams_.left_arm_joint_indices_[i]];
}
for (size_t i = 0; i < motionParams_.right_arm_joint_indices_.size(); i++)
{
tempRightArmCmd[i] = jointCommands_.data[motionParams_.right_arm_joint_indices_[i]];
}
for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++)
{
tempLeftLegCmd[i] = jointCommands_.data[motionParams_.left_leg_joint_indices_[i]];
}
for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++)
{
tempRightLegCmd[i] = jointCommands_.data[motionParams_.right_leg_joint_indices_[i]];
}
}
void RobotControlManager::UpdateJointCommands()
{
// TODO: first joint will conflict with righ arm, so we start from the second one
for (size_t i = 1; i < motionParams_.left_arm_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.left_arm_joint_indices_[i]] = tempLeftArmCmd[i];
}
for (size_t i = 1; i < motionParams_.right_arm_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.right_arm_joint_indices_[i]] = tempRightArmCmd[i];
}
for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i];
}
for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i];
}
}

257
HiveCoreR0/src/robot_control/src/robot_fsm.cpp
View File

@@ -14,9 +14,12 @@ RobotFsm::RobotFsm() : Node("robot_Fsm_node")
previousState_ = RobotState::INIT;
isRobotEnabled_ = false;
isFaultReset_ = false;
isJogMode_ = false;
enableCommandExecuted_ = false;
jogDirection_ = 0;
jogIndex_ = 5; //default axis 6
// 初始化数据文件（设置路径，确保目录存在）
data_file_path_ = "/home/demo/ros2_joint_data.txt"; // 数据保存路径
fs::path file_path(data_file_path_);
@@ -57,8 +60,9 @@ RobotFsm::RobotFsm() : Node("robot_Fsm_node")
lastTime_ = this->now(); // 初始化时间
// 创建定时器，每10ms执行一次控制逻辑（频率100Hz）
controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(10),std::bind(&RobotFsm::ControlLoop, this)); // 绑定到新的控制函数);
controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(1),std::bind(&RobotFsm::ControlLoop, this)); // 绑定到新的控制函数);
curMap_.clear();
dstMap_.clear();
std::cout << "RobotFsm Node is created finished!" << std::endl;
}
@@ -70,14 +74,34 @@ RobotFsm::~RobotFsm()
std::cout << "Robot controller stopped." << std::endl;
}
void PrintMessage(control_msgs::msg::DynamicInterfaceGroupValues msg) {
std::cout << "publish joint trajectory" << std::endl;
std::cout << msg.header.stamp.sec << std::endl;
std::cout << msg.header.stamp.nanosec << std::endl;
std::cout << msg.header.frame_id << std::endl;
for (size_t i = 0; i < msg.interface_groups.size(); i++)
{
std::cout << msg.interface_groups[i] << std::endl;
}
for (size_t i = 0; i < msg.interface_values.size(); i++)
{
std::cout << msg.interface_values[i].interface_names[0];
std::cout << msg.interface_values[i].values[0] << std::endl;
}
}
void RobotFsm::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
{
if (!msg) { // 检查消息是否有效
std::cout << "get null joint states!" << std::endl;
return;
}
// 更新机器人状态
for(size_t i=0; i < msg-> name.size();i++)
{
curMap_[msg->name[i]] = msg->position[i];
}
robotControlManager_.UpdateJointStates(msg);
}
@@ -124,11 +148,32 @@ void RobotFsm::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg) {
// 解析消息内容
std::string command = msg->data;
static std::string last_msg="";
if(last_msg!=command)
last_msg=command;
else
return;
if (command == "前进") {
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::FORWARD);
if (isJogMode_ && jogDirection_ == 0)
{
jogIndex_ ++;
std::cout << "switch to jog axis: " << jogIndex_ + 1 << std::endl;
}
else
{
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::FORWARD);
}
}
else if (command == "后退") {
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::BACKWARD);
if (isJogMode_ && jogDirection_ == 0)
{
jogIndex_ --;
std::cout << "switch to jog axis: " << jogIndex_ + 1 << std::endl;
}
else
{
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::BACKWARD);
}
}
else if (command == "左转") {
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::LEFT);
@@ -137,57 +182,129 @@ void RobotFsm::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg) {
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::RIGHT);
}
else if (command == "上升") {
jogDirection_ = 1;
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::UP);
if (isJogMode_)
{
jogDirection_ = 1;
std::cout << "jog axis in positive direction: " << jogIndex_ + 1 << std::endl;
}
else
{
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::UP);
}
}
else if (command == "下降") {
jogDirection_ = -1;
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::DOWN);
if (isJogMode_)
{
jogDirection_ = -1;
std::cout << "jog axis in negative direction: " << jogIndex_ + 1 << std::endl;
}
else
{
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::DOWN);
}
}
else if (command == "抬手") {
if (!isJogMode_)
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::HAND_UP);
}
else if (command == "放手") {
if (!isJogMode_)
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::HAND_DOWN);
}
else if (command == "停止") {
jogDirection_ = 0;
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::STOP);
if (isJogMode_)
{
jogDirection_ = 0;
std::cout << "jog axis stopped: " << jogIndex_ + 1 << std::endl;
}
else
{
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::STOP);
}
}
else if (command == "A") {
// TODO: need to be checked.
// inputDevices_[InputType::JOY]->SetCommand(ControlCommand::GO_HOME);
if (enableCommandExecuted_)
{
enableCommandExecuted_ = false;
EnableRobot(0);
std::cout<<"enable all motor"<<std::endl;
}
}
else if (command == "Y") {
ResetFault(1);
std::cout<<"reset fault"<<std::endl;
}
else if (command == "X") {
ResetFault(0);
std::cout<<"reset fault"<<std::endl;
}
else if (command == "B") {
if (!enableCommandExecuted_)
{
enableCommandExecuted_ = true;
EnableRobot();
EnableRobot(1);
std::cout<<"enable all motor"<<std::endl;
}
}
else if (command == "LB")
{
isJogMode_ = true;
std::cout << "jog mode enabled" << std::endl;
}
else if (command == "RB")
{
isJogMode_ = false;
std::cout << "jog mode disabled" << std::endl;
}
else {
RCLCPP_WARN(this->get_logger(), "未知的命令: %s", command.c_str());
}
}
void RobotFsm::EnableRobot()
void RobotFsm::ResetFault(double value)
{
control_msgs::msg::DynamicInterfaceGroupValues enableMsg;
for (size_t i = 0; i < robotControlManager_.GetMotionParameters().total_joints_; i++)
{
control_msgs::msg::DynamicInterfaceGroupValues enableMsg;
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
enableMsg.interface_groups.push_back(tempInterfaceGroup);
control_msgs::msg::InterfaceValue tempValue;
tempValue.interface_names = {"reset_fault"};
tempValue.values = {value};
enableMsg.interface_values.push_back(tempValue);
}
PrintMessage(enableMsg);
gpioPub_->publish(enableMsg);
if (value == 1)
{
std::cout << "Fault reset" << std::endl;
isFaultReset_ = true;
}
}
void RobotFsm::EnableRobot(double value)
{
control_msgs::msg::DynamicInterfaceGroupValues enableMsg;
for (size_t i = 0; i < robotControlManager_.GetMotionParameters().total_joints_; i++)
{
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
enableMsg.interface_groups.push_back(tempInterfaceGroup);
control_msgs::msg::InterfaceValue tempValue;
tempValue.interface_names = {"enable"};
tempValue.values = {1};
tempValue.values = {value};
enableMsg.interface_values.push_back(tempValue);
std::cout << "Enable robot " << i + 1 << std::endl;
gpioPub_->publish(enableMsg);
sleep(1);
}
sleep(2);
isRobotEnabled_ = true;
gpioPub_->publish(enableMsg);
isRobotEnabled_ = value == 1? true: false;
}
void RobotFsm::VoiceCommandCallback(const std_msgs::msg::String::SharedPtr msg) {
@@ -233,6 +350,8 @@ void RobotFsm::InitTransitionTable() {
transitionTable_[{RobotState::TURNING_RIGHT, ControlCommand::STOP}] = RobotState::STOPPING;
transitionTable_[{RobotState::MOVING_UP, ControlCommand::STOP}] = RobotState::STOPPING;
transitionTable_[{RobotState::MOVING_DOWN, ControlCommand::STOP}] = RobotState::STOPPING;
transitionTable_[{RobotState::ARM_MOVING_UP, ControlCommand::STOP}] = RobotState::STOPPING;
transitionTable_[{RobotState::ARM_MOVING_DOWN, ControlCommand::STOP}] = RobotState::STOPPING;
transitionTable_[{RobotState::HOME_MOVING, ControlCommand::STOP}] = RobotState::STOPPING;
transitionTable_[{RobotState::STOPPED, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
@@ -241,6 +360,8 @@ void RobotFsm::InitTransitionTable() {
transitionTable_[{RobotState::STOPPED, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT;
transitionTable_[{RobotState::STOPPED, ControlCommand::UP}] = RobotState::MOVING_UP;
transitionTable_[{RobotState::STOPPED, ControlCommand::DOWN}] = RobotState::MOVING_DOWN;
transitionTable_[{RobotState::STOPPED, ControlCommand::HAND_UP}] = RobotState::ARM_MOVING_UP;
transitionTable_[{RobotState::STOPPED, ControlCommand::HAND_DOWN}] = RobotState::ARM_MOVING_DOWN;
transitionTable_[{RobotState::STOPPED, ControlCommand::GO_HOME}] = RobotState::HOME_MOVING;
transitionTable_[{RobotState::INIT, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
@@ -249,6 +370,8 @@ void RobotFsm::InitTransitionTable() {
transitionTable_[{RobotState::INIT, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT;
transitionTable_[{RobotState::INIT, ControlCommand::UP}] = RobotState::MOVING_UP;
transitionTable_[{RobotState::INIT, ControlCommand::DOWN}] = RobotState::MOVING_DOWN;
transitionTable_[{RobotState::INIT, ControlCommand::HAND_UP}] = RobotState::ARM_MOVING_UP;
transitionTable_[{RobotState::INIT, ControlCommand::HAND_DOWN}] = RobotState::ARM_MOVING_DOWN;
transitionTable_[{RobotState::INIT, ControlCommand::GO_HOME}] = RobotState::HOME_MOVING;
}
@@ -258,10 +381,30 @@ void RobotFsm::AddInputDevice(InputType type, std::unique_ptr<InputDevice> devic
std::cout << "Added input device: " << inputDevices_[type]->GetDeviceName() << std::endl;
}
int RobotFsm::motor_dst(std::string name,double dst){
double val=curMap_[name];
double diff=dst-val;
double tempJointValue=0;
float delta=0.0;
if(diff>600){
delta=120.0;
tempJointValue=val+delta;
}else if(diff<-600){
delta=-120.0;
tempJointValue=val+delta;
}else{
return 0;
}
posMsg_.interface_groups.push_back(name);
control_msgs::msg::InterfaceValue tempValue;
tempValue.interface_names = {"position"};
tempValue.values = {tempJointValue};
posMsg_.interface_values.push_back(tempValue);
return 0;
}
// 状态机主循环
void RobotFsm::ControlLoop() {
// 检查所有输入设备
ControlCommand cmd = ControlCommand::NONE;
for (const auto& [type, device] : inputDevices_) {
@@ -323,39 +466,38 @@ void RobotFsm::Publish_joint_trajectory()
void RobotFsm::Gpio_publish_joint_trajectory()
{
static size_t jointIndex = 5;
static auto jointValue = robotControlManager_.GetJointCommands().data;
static double jogValue = jointValue[jointIndex -1];
std::cout << jogValue << std::endl;
if (true)
// jog
if (isJogMode_)
{
robotControlManager_.JogAxis(jogIndex_, jogDirection_);
auto cmd_msg = robotControlManager_.GetJointCommands();
data_file_ << 0;
// 2. 保存整个数组数据到txt文件
if (data_file_.is_open()) {
for (const auto& val : cmd_msg.data) {
data_file_ << "," << val; // 用逗号分隔每个元素
}
data_file_ << std::endl; // 换行
data_file_.flush(); // 强制刷新
}
control_msgs::msg::DynamicInterfaceGroupValues positionMsg;
for (size_t i = 0; i < robotControlManager_.GetMotionParameters().total_joints_; i++)
{
std::string tempInterfaceGroup = "joint_" + std::to_string(i + 1);
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
positionMsg.interface_groups.push_back(tempInterfaceGroup);
double tempJointValue;
if (i + 1 == jointIndex)
{
jogValue = jogValue + jogDirection_ * 10;
tempJointValue = jogValue;
}
else
{
tempJointValue = jointValue[i];
}
control_msgs::msg::InterfaceValue tempValue;
tempValue.interface_names = {"position"};
tempValue.values = {tempJointValue};
tempValue.values = {cmd_msg.data[i]};
positionMsg.interface_values.push_back(tempValue);
}
// 发布消息
// PrintMessage(positionMsg);
gpioPub_->publish(positionMsg);
}
else
@@ -387,7 +529,7 @@ void RobotFsm::Gpio_publish_joint_trajectory()
}
// 发布消息
//gpioPub_->publish(positionMsg);
gpioPub_->publish(positionMsg);
}
}
@@ -438,7 +580,12 @@ void RobotFsm::ExecuteStateAction(double dt) {
break;
case RobotState::MOVING_DOWN:
robotControlManager_.MoveDown(dt);
// 发送下降命令
break;
case RobotState::ARM_MOVING_UP:
robotControlManager_.ArmMoveUp(dt);
break;
case RobotState::ARM_MOVING_DOWN:
robotControlManager_.ArmMoveDown(dt);
break;
case RobotState::HOME_MOVING:
if(robotControlManager_.GoHome(dt))
@@ -462,8 +609,10 @@ std::string RobotFsm::StateToString(RobotState state) const {
case RobotState::TURNING_LEFT: return "TURNING_LEFT";
case RobotState::TURNING_RIGHT: return "TURNING_RIGHT";
case RobotState::MOVING_UP: return "MOVING_UP";
case RobotState::ARM_MOVING_UP: return "ARM_MOVING_UP";
case RobotState::ARM_MOVING_DOWN: return "ARM_MOVING_DOWN";
case RobotState::MOVING_DOWN: return "MOVING_DOWN";
case RobotState::HOME_MOVING: return "HOME_MOVING";
default: return "UNKNOWN";
}
}
}

85
HiveCoreR0/src/robot_control/src/rs485_driver.cpp
View File

@@ -47,7 +47,7 @@ namespace robot_control
RS485Driver::RS485Driver(size_t maxMotors) : max_motors_(maxMotors), serial_port_(-1), is_open_(false)
{
control_index_ = 0;
}
RS485Driver::~RS485Driver()
@@ -339,35 +339,70 @@ namespace robot_control
std::vector<uint8_t> command;
// 限制速度范围
int32_t clamped_speed = speeds[i - 1];
if (clamped_speed > 1000) clamped_speed = 1000;
if (clamped_speed < -1000) clamped_speed = -1000;
if (i < 5)
{
control_index_ = 1;
// 限制速度范围
int32_t clamped_speed = speeds[control_index_ - 1];
if (clamped_speed > 1000) clamped_speed = 1000;
if (clamped_speed < -1000) clamped_speed = -1000;
uint8_t high_byte = (clamped_speed >> 8) & 0xFF; // 高8位
uint8_t low_byte = clamped_speed & 0xFF; // 低8位
command.push_back(i);
command.push_back(CONTROL_CMD);
command.push_back(high_byte);
command.push_back(low_byte);
command.push_back(ZERO_CMD);
command.push_back(ZERO_CMD);
command.push_back(DEFAULT_TIME);
command.push_back(ZERO_CMD);
command.push_back(ZERO_CMD);
uint8_t high_byte = (clamped_speed >> 8) & 0xFF; // 高8位
uint8_t low_byte = clamped_speed & 0xFF; // 低8位
unsigned char crc8 = CRC8_Table(command.data(), 9);
command.push_back(2);
command.push_back(CONTROL_CMD);
command.push_back(high_byte);
command.push_back(low_byte);
command.push_back(ZERO_CMD);
command.push_back(ZERO_CMD);
command.push_back(DEFAULT_TIME);
command.push_back(ZERO_CMD);
command.push_back(ZERO_CMD);
unsigned char crc8 = CRC8_Table(command.data(), 9);
command.push_back(crc8);
// 发送命令
success = sendData(command);
// std::cout << "已发送命令到电机 " << static_cast<int>(control_index_) << ", 速度: " << clamped_speed << std::endl;
}
else if ( i > 5)
{
control_index_ = 2;
// 限制速度范围
int32_t clamped_speed = speeds[control_index_ - 1];
if (clamped_speed > 1000) clamped_speed = 1000;
if (clamped_speed < -1000) clamped_speed = -1000;
uint8_t high_byte = (clamped_speed >> 8) & 0xFF; // 高8位
uint8_t low_byte = clamped_speed & 0xFF; // 低8位
command.push_back(3);
command.push_back(CONTROL_CMD);
command.push_back(high_byte);
command.push_back(low_byte);
command.push_back(ZERO_CMD);
command.push_back(ZERO_CMD);
command.push_back(DEFAULT_TIME);
command.push_back(ZERO_CMD);
command.push_back(ZERO_CMD);
unsigned char crc8 = CRC8_Table(command.data(), 9);
command.push_back(crc8);
// 发送命令
success = sendData(command);
// std::cout << "已发送命令到电机 " << static_cast<int>(control_index_) << ", 速度: " << clamped_speed << std::endl;
}
command.push_back(crc8);
// 发送命令
success = sendData(command);
std::cout << "已发送命令到电机 " << static_cast<int>(i) << ", 速度: " << clamped_speed << std::endl;
++i;
if (i > max_motors_)
// std::cout << "i = " << static_cast<int>(i) << std::endl;
if (i > 8)
{
i = 1;
return success;

8
HiveCoreR0/src/robot_control/src/wheel_control.cpp
View File

@@ -60,9 +60,7 @@ namespace robot_control
int32_t rightOmega = rightSpeed;
wheelSpeed_[0] = wheelDirection_[0] * leftOmega; // left_wheel1
wheelSpeed_[1] = wheelDirection_[1] * rightOmega; // right_wheel1
wheelSpeed_[2] = wheelDirection_[2] * leftOmega; // left_wheel2
wheelSpeed_[3] = wheelDirection_[3] * rightOmega; // right_wheel2
wheelSpeed_[1] = wheelDirection_[1] * rightOmega; // right_wheel
}
@@ -94,7 +92,7 @@ namespace robot_control
}
}
void WheelControl::ExecuteCommand()
bool WheelControl::ExecuteCommand()
{
CalculateWheelSpeeds();
@@ -106,6 +104,8 @@ namespace robot_control
previousWheelSpeed_ = wheelSpeed_;
}
}
return isSendFinished_;
}
}