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

add robot control module

Browse Source
This commit is contained in:
Ray
2025-08-15 19:58:35 +08:00
parent f4b83db868
commit 3a963cfd85
26 changed files with 992 additions and 800 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
HiveCoreR0/src/keyboard_control/include/keyboard_control.hpp
View File

@@ -2,7 +2,7 @@
#define KEY_BOARD_CONTROL_NODE__HPP_
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "std_msgs/msg/string.hpp"
class KeyboardControlNode : public rclcpp::Node
{
@@ -15,10 +15,8 @@ private:
void timer_callback();
// 成员变量
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr twist_pub_;
rclcpp::TimerBase::SharedPtr timer_;
double linear_speed_; // 线速度（m/s）
double angular_speed_; // 角速度（rad/s）
};
#endif // KEY_BOARD_CONTROL_NODE__HPP_

43
HiveCoreR0/src/keyboard_control/src/keyboard_control.cpp
View File

@@ -36,18 +36,8 @@ int kbhit()
KeyboardControlNode::KeyboardControlNode() : Node("keyboard_control_node")
{
// 声明参数：移动速度、旋转速度
this->declare_parameter("linear_speed", 0.2); // 线速度（m/s）
this->declare_parameter("angular_speed", 0.5); // 角速度（rad/s）
// 读取参数
linear_speed_ = this->get_parameter("linear_speed").as_double();
angular_speed_ = this->get_parameter("angular_speed").as_double();
// 创建发布者（发送速度指令）
twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
"/cmd_vel", 10
);
twist_pub_ = this->create_publisher<std_msgs::msg::String>("keyboard_command", 10);
// 打印控制说明
print_help();
@@ -68,12 +58,14 @@ void KeyboardControlNode::print_help()
RCLCPP_INFO(this->get_logger(), " a : 左转");
RCLCPP_INFO(this->get_logger(), " d : 右转");
RCLCPP_INFO(this->get_logger(), " x : 停止");
RCLCPP_INFO(this->get_logger(), " u : 上升");
RCLCPP_INFO(this->get_logger(), " j : 下降");
RCLCPP_INFO(this->get_logger(), " q : 退出");
}
void KeyboardControlNode::timer_callback()
{
geometry_msgs::msg::Twist twist_msg;
std_msgs::msg::String twist_msg;
if(kbhit())
{
@@ -81,37 +73,40 @@ void KeyboardControlNode::timer_callback()
switch(key)
{
case 'w': // 前进
twist_msg.linear.x = linear_speed_;
twist_msg.angular.z = 0.0;
twist_msg.data = "w";
RCLCPP_INFO(this->get_logger(), "前进");
break;
case 's': // 后退
twist_msg.linear.x = -linear_speed_;
twist_msg.angular.z = 0.0;
twist_msg.data = "s";
RCLCPP_INFO(this->get_logger(), "后退");
break;
case 'a': // 左转
twist_msg.linear.x = 0.0;
twist_msg.angular.z = angular_speed_;
twist_msg.data = "a";
RCLCPP_INFO(this->get_logger(), "左转");
break;
case 'd': // 右转
twist_msg.linear.x = 0.0;
twist_msg.angular.z = -angular_speed_;
twist_msg.data = "d";
RCLCPP_INFO(this->get_logger(), "右转");
break;
case 'x': // 停止
twist_msg.linear.x = 0.0;
twist_msg.angular.z = 0.0;
twist_msg.data = "x";
RCLCPP_INFO(this->get_logger(), "停止");
break;
case 'q': // 退出
twist_msg.data = "q";
RCLCPP_INFO(this->get_logger(), "退出控制");
rclcpp::shutdown();
return;
case 'u':
twist_msg.data = "u";
RCLCPP_INFO(this->get_logger(), "上升");
break;
case 'j':
twist_msg.data = "j";
RCLCPP_INFO(this->get_logger(), "下降");
break;
default:
twist_msg.linear.x = 0.0;
twist_msg.angular.z = 0.0;
twist_msg.data = "停止";
print_help();
break;
}

17
HiveCoreR0/src/mujoco_simulation/launch/mujoco_simulation.launch.py
View File

@@ -1,28 +1,13 @@
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
return LaunchDescription([
DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='是否使用模拟时间'
),
DeclareLaunchArgument(
'rotation_time',
default_value='10.0',
description='旋转一圈的目标时间(秒)'
),
Node(
package='mujoco_simulation',
executable='mujoco_sim_node',
name='mujoco_sim_node',
output='screen',
parameters=[
{'use_sim_time': LaunchConfiguration('use_sim_time')},
{'rotation_time': LaunchConfiguration('rotation_time')}
]
parameters=[{"use_sim_time": False}] # 启用模拟时间
)
])

23
HiveCoreR0/src/robot_control/CMakeLists.txt
View File

@@ -8,19 +8,34 @@ endif()
# 依赖
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(Eigen3 REQUIRED) # 查找Eigen3库
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
# 关键：添加Eigen3的头文件目录
include_directories(
include
${EIGEN3_INCLUDE_DIRS} # 引入Eigen3的头文件路径
)
add_executable(robot_control_node src/robot_control.cpp)
ament_target_dependencies(robot_control_node rclcpp geometry_msgs sensor_msgs)
add_executable(robot_fsm
src/robot_fsm.cpp
src/robot_control_manager.cpp
src/input_device.cpp
src/arm_control.cpp
src/leg_control.cpp
src/robot_model.cpp
src/wheel_control.cpp
)
ament_target_dependencies(robot_fsm
rclcpp
geometry_msgs
sensor_msgs
)
install(TARGETS
robot_control_node
robot_fsm
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch

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

@@ -4,35 +4,39 @@
#include "rclcpp/rclcpp.hpp"
#include <Eigen/Dense>
class ArmControl
namespace robot_control
{
public:
ArmControl(int total_joints);
~ArmControl() = default;
class ArmControl
{
public:
ArmControl(int total_joints);
~ArmControl();
void MoveToZeroPoint(double duration);
void MoveToZeroPoint(double duration);
void MoveToTargetPoint(const Eigen::Vector3d& target_pos, double duration);
void MoveToTargetPoint(const Eigen::Vector3d& target_pos, double duration);
void MoveToTargetJoint(const std::vector<double>& target_joint, double duration);
void MoveToTargetJoint(const std::vector<double>& target_joint, double duration);
void DefaultArmMovement(std::vector<double>& joint_position_desired, double duration);
void DefaultArmMovement(std::vector<double>& joint_position_desired, double duration);
private:
private:
int total_joints_; // 总关节数
int total_joints_; // 总关节数
std::vector<double> joint_position_; // 机械臂关节位置
std::vector<double> joint_velocity_; // 机械臂关节速度
std::vector<double> joint_acceleration_; // 机械臂关节加速度
std::vector<double> joint_torque_; // 机械臂关节力矩
std::vector<double> joint_angle_; // 机械臂关节角度
std::vector<double> joint_position_; // 机械臂关节位置
std::vector<double> joint_velocity_; // 机械臂关节速度
std::vector<double> joint_acceleration_; // 机械臂关节加速度
std::vector<double> joint_torque_; // 机械臂关节力矩
std::vector<double> joint_angle_; // 机械臂关节角度
std::vector<double> joint_position_desired_; // 机械臂关节期望位置
std::vector<double> joint_position_desired_; // 机械臂关节期望位置
double amplitude_rad_; // 振幅 （弧度）
double amplitude_deg_; // 振幅 （角度）
double frequency_; // 频率
};
double amplitude_rad_; // 振幅 （弧度）
double amplitude_deg_; // 振幅 （角度）
double frequency_; // 频率
};
}
#endif // ROBOT_CONTROL__ARMCONTROL_HPP_

77
HiveCoreR0/src/robot_control/include/input_device.hpp Normal file
View File

@@ -0,0 +1,77 @@
#ifndef INPUT_DEVICE_H
#define INPUT_DEVICE_H
#include <string>
// 控制命令枚举
enum class ControlCommand {
NONE,
STOP,
FORWARD,
BACKWARD,
LEFT,
RIGHT,
UP,
DOWN
};
// 输入设备类型
enum class InputType {
KEYBOARD,
GAMEPAD,
VOICE
};
// 输入设备接口
class InputDevice {
protected:
InputType type;
ControlCommand command;
public:
InputDevice(InputType t) : type(t) {}
virtual ~InputDevice() = default;
InputType GetType() const { return type; }
virtual ControlCommand GetCommand() = 0;
virtual void SetCommand(ControlCommand command) = 0;
virtual std::string GetDeviceName() const = 0;
};
// 键盘输入设备
class KeyboardInput : public InputDevice {
public:
KeyboardInput() : InputDevice(InputType::KEYBOARD) {
command = ControlCommand::NONE;
}
ControlCommand GetCommand() override;
void SetCommand(ControlCommand command) override;
std::string GetDeviceName() const override { return "Keyboard"; }
};
// 手柄输入设备
class GamepadInput : public InputDevice {
public:
GamepadInput() : InputDevice(InputType::GAMEPAD) {
command = ControlCommand::NONE;
}
ControlCommand GetCommand() override;
void SetCommand(ControlCommand command) override;
std::string GetDeviceName() const override { return "Gamepad"; }
};
// 语音输入设备
class VoiceInput : public InputDevice {
public:
VoiceInput() : InputDevice(InputType::VOICE) {
command = ControlCommand::NONE;
}
ControlCommand GetCommand() override;
void SetCommand(ControlCommand command) override;
std::string GetDeviceName() const override { return "Voice Control"; }
};
#endif // INPUT_DEVICE_H

40
HiveCoreR0/src/robot_control/include/leg_control.hpp
View File

@@ -2,22 +2,42 @@
#define ROBOT_CONTROL__LEGCONTROL_HPP_
#include "rclcpp/rclcpp.hpp"
#include <cmath>
namespace robot_control
{
class LegControl
{
public:
LegControl();
~LegControl();
/**
* @brief LegControl 类用于控制腿部运动
*
* 该类提供了腿部关节的控制功能，包括关节重置、移动和计算目标位置等操作。
*/
class LegControl
{
public:
LegControl(int totalJointNumber, double maxSpeed, double LegLength);
~LegControl();
void set_goal(double x, double y);
private:
void ResetJoints();
};
void Move(std::vector<double>& joint_positions, double dt_sec);
void CalculateWheelTargets(std::vector<double> &joint_targets, double dt_sec);
void setLegSpeed(double speed);
private:
int totalJointNumber_;
double maxSpeed_;
double speed_;
double LegLength_;
std::vector<double> joint_positions_;
};
} // namespace robot_control

95
HiveCoreR0/src/robot_control/include/motion_parameters.hpp Normal file
View File

@@ -0,0 +1,95 @@
#ifndef MOTION_PARAMETERS_HPP
#define MOTION_PARAMETERS_HPP
#include <vector>
#include "sensor_msgs/msg/joint_state.hpp"
namespace robot_control
{
class MotionParameters
{
public:
MotionParameters()
{
// 初始化参数
arm_amplitude_rad_ = 0.0;
arm_frequency_ = 0.0;
total_joints_ = 15;
phase1_notified_ = false;
wheel_radius_ = 0.09;
wheel_separation_ = 0.26;
max_linear_speed_x_ = 0.2;
max_linear_speed_z_ = 0.2;
max_angular_speed_z_ = 0.2;
linear_speed_x_ = 0.1;
linear_speed_z_ = 0.1;
angular_speed_z_ = 0.1;
// 初始化关节索引
leg_joint_indices_ = {0, 1, 2, 3}; // 腿部关节索引
wheel_joint_indices_ = {4, 5, 6, 7};
pitch_body_indices_ = {8}; // 身体关节索引
arm_joint_indices_ = {9, 10, 11, 12, 13, 14}; // 手臂关节索引
wheel_joint_directions_ = {1, -1, 1, -1}; // 轮子方向
leg_joint_directions_ = {-1, -1, 1, 1}; // 腿部方向
leg_wheel_directions_ = {1, -1, -1, 1}; // 腿部 和 腰部的耦合方向
joint_directions_.clear();
leg_length_ = 50.0;
// 初始化关节状态
joint_positions_.clear();
joint_velocities_.clear();
joint_accelerations_.clear();
joint_efforts_.clear();
};
// 运动参数
double leg_target_rad_; // 腿部目标角度（弧度）
double arm_amplitude_rad_; // 手臂摆动振幅（弧度）
double arm_frequency_; // 手臂摆动频率（Hz）
int total_joints_; // 总关节数
bool phase1_notified_; // 第一阶段结束提示标记
double duringTime_; // 运动持续时间
// 关节索引
std::vector<int> leg_joint_indices_; // 腿部关节索引
std::vector<int> arm_joint_indices_; // 手臂关节索引
std::vector<int> wheel_joint_indices_; // 轮子关节索引
std::vector<int> pitch_body_indices_; // 腿部轮子关节索引
std::vector<int> joint_directions_; // 关节方向
std::vector<int> wheel_joint_directions_; // 轮子关节方向
std::vector<int> leg_joint_directions_; // 腿部关节方向
std::vector<int> leg_wheel_directions_; // 腿部 和 腰部的耦合方向
// 轮子控制相关
double wheel_radius_; // 轮子半径（米）
double wheel_separation_; // 轮距（米）
std::vector<double> wheel_targets_; // 轮子目标角度（弧度）
double max_linear_speed_x_; // 线速度（m/s）
double max_linear_speed_z_; // 线速度（m/s）
double max_angular_speed_z_; // 角速度（rad/s）
double linear_speed_x_; // 线速度（m/s）
double linear_speed_z_; // 线速度（m/s）
double angular_speed_z_; // 角速度（rad/s）
// 腿相关
double leg_length_; // 腿部摆动振幅（弧度）
// 运动状态
std::vector<double> joint_positions_; // 关节位置（弧度）
std::vector<double> joint_velocities_; // 关节速度（弧度/秒）
std::vector<double> joint_accelerations_; // 关节加速度（弧度/秒^2）
std::vector<double> joint_efforts_; // 关节力矩（牛顿米）
};
}
#endif // MOTION_PARAMETERS_HPP

72
HiveCoreR0/src/robot_control/include/robot_control.hpp
View File

@@ -1,72 +0,0 @@
#ifndef ROBOT_CONTROL__HPP_
#define ROBOT_CONTROL__HPP_
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
namespace robot_control
{
class RobotControlNode : public rclcpp::Node
{
public:
RobotControlNode();
~RobotControlNode();
// 关节复位
void resetJoints();
private:
void print_help();
void joint_states_callback(const sensor_msgs::msg::JointState::SharedPtr msg);
void twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
void timer_callback();
void handle_phase_motion(std_msgs::msg::Float64MultiArray& cmd_msg, double current_time);
void handle_wheel_control(std_msgs::msg::Float64MultiArray& cmd_msg, double dt_sec);
// 成员变量
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr cmd_pub_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_states_sub_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time start_time_;
rclcpp::Time last_time_;
// 运动参数
double leg_target_rad_; // 腿部目标角度（弧度）
double arm_amplitude_rad_; // 手臂摆动振幅（弧度）
double arm_frequency_; // 手臂摆动频率（Hz）
int total_joints_; // 总关节数
bool phase1_notified_ = false; // 第一阶段结束提示标记
// 关节索引
std::vector<int> leg_joint_indices_; // 腿部关节索引
std::vector<int> arm_joint_indices_; // 手臂关节索引
std::vector<int> wheel_joint_indices_; // 轮子关节索引
std::vector<int> pitch_body_indices_; // 腿部轮子关节索引
std::vector<int> joint_directions_; // 关节方向
std::vector<int> wheel_joint_directions_; // 轮子关节方向
// 轮子控制相关
double wheel_radius_; // 轮子半径（米）
double wheel_separation_; // 轮距（米）
std::vector<double> wheel_targets_; // 轮子目标角度（弧度）
double linear_x_ = 0.0; // 线速度（m/s）
double angular_z_ = 0.0; // 角速度（rad/s）
double linear_speed_; // 最大线速度（m/s）
double angular_speed_; // 最大角速度（rad/s）
// 运动状态
std::vector<double> joint_positions_; // 关节位置（弧度）
std::vector<double> joint_velocities_; // 关节速度（弧度/秒）
std::vector<double> joint_accelerations_; // 关节加速度（弧度/秒^2）
std::vector<double> joint_efforts_; // 关节力矩（牛顿米）
sensor_msgs::msg::JointState joint_states_;
};
} // namespace robot_control
#endif // ROBOT_CONTROL__HPP_

74
HiveCoreR0/src/robot_control/include/robot_control_manager.hpp Normal file
View File

@@ -0,0 +1,74 @@
#ifndef ROBOT_CONTROL_MANAGER_HPP_
#define ROBOT_CONTROL_MANAGER_HPP_
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "motion_parameters.hpp"
#include "arm_control.hpp"
#include "leg_control.hpp"
#include "wheel_control.hpp"
#include "robot_model.hpp"
namespace robot_control
{
class RobotControlManager
{
public:
RobotControlManager();
~RobotControlManager();
void UpdateJointStates(sensor_msgs::msg::JointState jointStates_);
// 关节复位
void ResetJoints();
// 控制机器人运动
void MoveForward(double dt);
void MoveBackward(double dt);
void TurnLeft(double dt);
void TurnRight(double dt);
void MoveUp(double dt);
void MoveDown(double dt);
void Stop();
// 机器人关节指令
std_msgs::msg::Float64MultiArray GetJointCommands();
void SetJointCommands(std_msgs::msg::Float64MultiArray jointCommands);
private:
void init();
void cleanup();
MotionParameters motionParams_;
int MoveCounter_; // 移动计数器
double MoveTime_; // 移动时间
// 控制器
ArmControl* armController_;
LegControl* legController_;
WheelControl* wheelController_;
// 传感器
// TODO: add sensor class
// 机器人模型
RobotModel* robotModel_;
// 运动状态
std::vector<double> jointPositions_; // 关节位置（弧度）
std::vector<double> jointVelocities_; // 关节速度（弧度/秒）
std::vector<double> jointAccelerations_; // 关节加速度（弧度/秒^2）
std::vector<double> jointEfforts_; // 关节力矩（牛顿米）
sensor_msgs::msg::JointState jointStates_;
std_msgs::msg::Float64MultiArray jointCommands_;
};
} // namespace robot_control_manager
#endif // ROBOT_CONTROL_MANAGER_HPP_

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

@@ -1,15 +1,11 @@
#ifndef ROBOT_FSM_H
#define ROBOT_FSM_H
#include <iostream>
#include <string>
#include <map>
#include <memory>
#include <chrono>
#include <thread>
// 前向声明
class InputDevice;
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "std_msgs/msg/string.hpp"
#include "input_device.hpp"
#include "robot_control_manager.hpp"
// 机器人运动状态枚举
enum class RobotState {
@@ -22,99 +18,57 @@ enum class RobotState {
MOVING_DOWN // 下降
};
// 控制命令枚举
enum class ControlCommand {
NONE,
STOP,
FORWARD,
BACKWARD,
LEFT,
RIGHT,
UP,
DOWN
class RobotFsm : public rclcpp::Node
{
public:
RobotFsm();
~RobotFsm();
// 添加输入设备
void AddInputDevice(InputType type, std::unique_ptr<InputDevice> device);
// 状态机主循环
void ControlLoop();
// 处理命令并转换状态
void ProcessCommand(ControlCommand cmd);
// 获取当前状态
RobotState GetCurrentState() const { return currentState_; }
// 状态转换为字符串
std::string StateToString(RobotState state) const;
private:
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr cmdPub_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr keyboadCommandSub_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr voiceCommandSub_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;
RobotState currentState_;
robot_control::RobotControlManager robotControlManager_;
std::map<InputType, std::unique_ptr<InputDevice>> inputDevices_;
rclcpp::TimerBase::SharedPtr controlTimer_;
rclcpp::Time lastTime_; // 移至类成员
double executionTime_;
bool isRunning_;
bool isPaused_;
bool isFinished_;
// 状态转换表
std::map<std::pair<RobotState, ControlCommand>, RobotState> transitionTable_;
// 初始化状态转换规则
void InitTransitionTable();
// 执行当前状态对应的动作
void ExecuteStateAction(double dt);
void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
void KeyboardCommandCallback(const std_msgs::msg::String::SharedPtr msg);
void VoiceCommandCallback(const std_msgs::msg::String::SharedPtr msg);
};
// 输入设备类型
enum class InputType {
KEYBOARD,
GAMEPAD,
VOICE
};
class RobotFsm {
private:
RobotState currentState;
std::map<InputType, std::unique_ptr<InputDevice>> inputDevices;
// 状态转换表
std::map<std::pair<RobotState, ControlCommand>, RobotState> transitionTable;
// 初始化状态转换规则
void initTransitionTable();
// 执行当前状态对应的动作
void executeStateAction();
public:
RobotFsm();
// 添加输入设备
void addInputDevice(InputType type, std::unique_ptr<InputDevice> device);
// 状态机主循环
void run();
// 处理命令并转换状态
void processCommand(ControlCommand cmd);
// 获取当前状态
RobotState getCurrentState() const { return currentState; }
// 状态转换为字符串
std::string stateToString(RobotState state) const;
};
// 输入设备接口
class InputDevice {
protected:
InputType type;
public:
InputDevice(InputType t) : type(t) {}
virtual ~InputDevice() = default;
InputType getType() const { return type; }
virtual ControlCommand getCommand() = 0;
virtual std::string getDeviceName() const = 0;
};
// 键盘输入设备
class KeyboardInput : public InputDevice {
public:
KeyboardInput() : InputDevice(InputType::KEYBOARD) {}
ControlCommand getCommand() override;
std::string getDeviceName() const override { return "Keyboard"; }
};
// 手柄输入设备
class GamepadInput : public InputDevice {
public:
GamepadInput() : InputDevice(InputType::GAMEPAD) {}
ControlCommand getCommand() override;
std::string getDeviceName() const override { return "Gamepad"; }
};
// 语音输入设备
class VoiceInput : public InputDevice {
public:
VoiceInput() : InputDevice(InputType::VOICE) {}
ControlCommand getCommand() override;
std::string getDeviceName() const override { return "Voice Control"; }
};
#endif // ROBOT_FSM_H
#endif // ROBOT_FSM_H

2
HiveCoreR0/src/robot_control/include/robot_kinematics.hpp
View File

@@ -6,7 +6,7 @@
#include <vector>
#include <memory>
namespace robot_model
namespace robot_control
{
class RobotKinematics

10
HiveCoreR0/src/robot_control/include/robot_model.hpp
View File

@@ -1,11 +1,11 @@
#ifndef ROBOT_MODEL__ROBOT_MODEL_HPP_
#define ROBOT_MODEL__ROBOT_MODEL_HPP_
#ifndef ROBOT_CONTROL__ROBOT_MODEL_HPP_
#define ROBOT_CONTROL__ROBOT_MODEL_HPP_
#include <vector>
#include <string>
#include <memory>
namespace robot_model
namespace robot_control
{
class UrdfParser;
@@ -109,6 +109,6 @@ private:
std::vector<Leg> legs_; // 腿部集合（4条腿）
};
} // namespace robot_model
} // namespace robot_CONTROL
#endif // ROBOT_MODEL__ROBOT_MODEL_HPP_
#endif // ROBOT_CONTROL__ROBOT_MODEL_HPP_

41
HiveCoreR0/src/robot_control/include/wheel_control.hpp Normal file
View File

@@ -0,0 +1,41 @@
#ifndef ROBOT_CONTROL__WHEELCONTROL_HPP_
#define ROBOT_CONTROL__WHEELCONTROL_HPP_
#include "rclcpp/rclcpp.hpp"
#include <Eigen/Dense>
namespace robot_control
{
class WheelControl
{
public:
WheelControl(int totalJoints, double wheelRadius, double wheelSeparation, double linearSpeed, double angularSpeed);
~WheelControl();
void ResetJoints();
void Move(std::vector<double>& jointPositions, double dtSec);
void CalculateWheelTargets(std::vector<double> &jointTargets, double dtSec);
void setLinearAngularSpeed(double linearSpeed, double angularSpeed);
private:
int totalJoints_; // 总关节数
// 轮子控制相关
double wheelRadius_; // 轮子半径（米）
double wheelSeparation_; // 轮距（米）
std::vector<double> wheelTargets_; // 轮子目标角度（弧度）
double linearSpeed_ = 0.0; // 线速度（m/s）
double angularSpeed_ = 0.0; // 角速度（rad/s）
double maxLinearSpeed_; // 最大线速度（m/s）
double maxAngularSpeed_; // 最大角速度（rad/s）
};
}
#endif // ROBOT_CONTROL__ARMCONTROL_HPP_

13
HiveCoreR0/src/robot_control/launch/robot_control.launch.py
View File

@@ -4,19 +4,20 @@ from launch.actions import TimerAction
def generate_launch_description():
# 创建节点启动描述
robot_control_node = Node(
robot_fsm_node = Node(
package='robot_control', # 功能包名称
executable='robot_control_node', # 可执行文件名称
name='robot_control_node', # 节点名称（可自定义）
executable='robot_fsm', # 可执行文件名称
name='robot_fsm', # 节点名称（可自定义）
output='screen', # 输出到屏幕
parameters=[{"use_sim_time": False}] # 启用模拟时间
)
start_robot_control_node = TimerAction(
start_robot_fsm_node = TimerAction(
period=1.0,
actions=[robot_control_node],
actions=[robot_fsm_node],
)
# 组装launch描述
return LaunchDescription([
start_robot_control_node
start_robot_fsm_node
])

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

@@ -5,6 +5,7 @@
#include "arm_control.hpp"
using namespace std::chrono_literals;
using namespace robot_control;
ArmControl::ArmControl(int total_joints)
{
@@ -36,7 +37,7 @@ void ArmControl::DefaultArmMovement(std::vector<double>& joint_position_desired,
{
double sine_pos = amplitude_rad_ * std::sin(2 * M_PI * frequency_ * duration);
for (size_t i = 0; i < total_joints_; i++)
for (int i = 0; i < total_joints_; i++)
{
joint_position_desired_[i] = sine_pos;
joint_position_desired = joint_position_desired_;
@@ -46,17 +47,38 @@ void ArmControl::DefaultArmMovement(std::vector<double>& joint_position_desired,
void ArmControl::MoveToZeroPoint(double duration)
{
std::vector<double> joint_position_desired(total_joints_, 0.0);
MoveToTargetPoint(joint_position_desired, duration);
MoveToTargetJoint(joint_position_desired, duration);
}
void ArmControl::MoveToTargetPoint(const Eigen::Vector3d& target_pos, double duration)
{
// 计算每个关节的目标位置
for (size_t i = 0; i < total_joints_; i++)
for (int i = 0; i < total_joints_; i++)
{
joint_position_desired_[i] = joint_position_desired[i];
// TODO: Inverse kinematic calculation
joint_position_desired_[i] = 0.0; // Placeholder value
}
// 计算每个关节的所需时间
}
void ArmControl::MoveToTargetJoint(const std::vector<double>& joint_position_desired, double duration)
{
// 计算每个关节的所需时间
double time_step = 0.01; // 时间步长（秒）
int num_steps = duration / time_step;
// 发布目标关节位置
for (int i = 0; i < num_steps; i++)
{
// 计算当前时间
double current_time = i * time_step;
// 计算每个关节的当前位置
for (size_t j = 0; j < total_joints_; j++)
{
joint_position_[j] = joint_position_desired[j] * current_time / duration;
}
}
}

29
HiveCoreR0/src/robot_control/src/input_device.cpp Normal file
View File

@@ -0,0 +1,29 @@
#include "input_device.hpp"
// 键盘输入处理
ControlCommand KeyboardInput::GetCommand() {
return command;
}
void KeyboardInput::SetCommand(ControlCommand cmd) {
command = cmd;
}
// 手柄输入处理（模拟实现）
ControlCommand GamepadInput::GetCommand() {
return command;
}
void GamepadInput::SetCommand(ControlCommand cmd) {
command = cmd;
}
// 语音输入处理（模拟实现）
ControlCommand VoiceInput::GetCommand() {
return command;
}
void VoiceInput::SetCommand(ControlCommand cmd) {
command = cmd;
}

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

@@ -1,139 +1,42 @@
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include <cmath>
#include "leg_control.hpp"
class MotionController : public rclcpp::Node
{
public:
MotionController() : Node("motion_controller")
{
// 声明参数
this->declare_parameter<double>("wheel_radius", 0.05);
this->declare_parameter<double>("wheel_separation", 0.2);
// 获取参数
this->get_parameter("wheel_radius", wheel_radius_);
this->get_parameter("wheel_separation", wheel_separation_);
// 创建发布者和订阅者
cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
"/odom", 10,
std::bind(&MotionController::odom_callback, this, std::placeholders::_1)
);
// 创建定时器用于控制循环
control_timer_ = this->create_wall_timer(
std::chrono::milliseconds(100),
std::bind(&MotionController::control_loop, this)
);
RCLCPP_INFO(this->get_logger(), "Motion controller initialized");
}
private:
void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
// 保存当前里程计信息
current_pose_ = msg->pose.pose;
current_twist_ = msg->twist.twist;
}
void control_loop()
{
// 这里可以实现更复杂的控制算法
// 简单示例：如果设置了目标位置，则向目标位置移动
if (goal_set_)
{
// 计算当前位置与目标位置的差异
double dx = goal_x_ - current_pose_.position.x;
double dy = goal_y_ - current_pose_.position.y;
double distance = std::sqrt(dx*dx + dy*dy);
// 计算目标方向
double target_yaw = std::atan2(dy, dx);
// 从四元数获取当前偏航角
double current_yaw = get_yaw_from_quaternion(current_pose_.orientation);
// 计算角度差
double angle_diff = target_yaw - current_yaw;
// 归一化到[-π, π]
angle_diff = std::atan2(std::sin(angle_diff), std::cos(angle_diff));
// 控制参数
const double angle_tolerance = 0.1; // 弧度
const double distance_tolerance = 0.05; // 米
geometry_msgs::msg::Twist cmd_vel;
// 如果到达目标位置，停止
if (distance < distance_tolerance)
{
cmd_vel.linear.x = 0.0;
cmd_vel.angular.z = 0.0;
goal_set_ = false;
RCLCPP_INFO(this->get_logger(), "Reached goal position");
}
// 否则先旋转到目标方向
else if (std::abs(angle_diff) > angle_tolerance)
{
cmd_vel.linear.x = 0.0;
cmd_vel.angular.z = std::clamp(angle_diff, -0.5, 0.5); // 限制最大角速度
}
// 然后前进
else
{
cmd_vel.linear.x = std::clamp(distance, 0.0, 0.3); // 限制最大线速度
cmd_vel.angular.z = 0.0;
}
cmd_vel_pub_->publish(cmd_vel);
}
}
// 从四元数计算偏航角
double get_yaw_from_quaternion(const geometry_msgs::msg::Quaternion& quat)
{
double w = quat.w;
double x = quat.x;
double y = quat.y;
double z = quat.z;
return std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
}
// 设置目标位置（可以通过服务或动作接口实现）
void set_goal(double x, double y)
{
goal_x_ = x;
goal_y_ = y;
goal_set_ = true;
RCLCPP_INFO(this->get_logger(), "New goal set: (%f, %f)", x, y);
}
// 成员变量
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
rclcpp::TimerBase::SharedPtr control_timer_;
geometry_msgs::msg::Pose current_pose_;
geometry_msgs::msg::Twist current_twist_;
double wheel_radius_;
double wheel_separation_;
double goal_x_ = 0.0;
double goal_y_ = 0.0;
bool goal_set_ = false;
};
using namespace robot_control;
int main(int argc, char * argv[])
LegControl::LegControl(int totalJointNumber, double maxSpeed, double LegLength)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MotionController>());
rclcpp::shutdown();
return 0;
this->totalJointNumber_ = totalJointNumber;
this->maxSpeed_ = maxSpeed;
this->LegLength_ = LegLength;
this->joint_positions_.resize(totalJointNumber);
}
LegControl::~LegControl()
{
}
void LegControl::setLegSpeed(double speed)
{
if (speed > maxSpeed_)
{
speed = maxSpeed_;
}
else if (speed < -maxSpeed_)
{
speed = -maxSpeed_;
}
this->speed_ = speed;
}
void LegControl::Move(std::vector<double>& deltaJointPositions, double dt_sec)
{
for (int i = 0; i < this->totalJointNumber_; i++)
{
joint_positions_[i] = speed_ * dt_sec;
}
deltaJointPositions = joint_positions_;
}

251
HiveCoreR0/src/robot_control/src/robot_control.cpp
View File

@@ -1,251 +0,0 @@
#include "robot_control.hpp"
#include <cmath>
#include <chrono>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
namespace robot_control
{
using namespace std::chrono_literals;
RobotControlNode::RobotControlNode(): Node("robot_phase_motion_node")
{
// 声明并获取参数
this->declare_parameter("leg_target_deg", 0.0); // 腿部目标角度（度）
this->declare_parameter("arm_amplitude_deg", 30.0); // 手臂摆动振幅（度）
this->declare_parameter("arm_frequency", 0.5); // 手臂摆动频率（Hz）
this->declare_parameter("control_period_ms", 10); // 控制周期（毫秒）
this->declare_parameter("wheel_radius", 0.05); // 轮子半径（米）
this->declare_parameter("wheel_separation", 0.3); // 轮距（米）
this->declare_parameter("linear_speed", 0.2); // 线速度（m/s）
this->declare_parameter("angular_speed", 0.5); // 角速度（rad/s）
// 读取参数并转换单位
double leg_target_deg = this->get_parameter("leg_target_deg").as_double();
leg_target_rad_ = leg_target_deg * M_PI / 180.0;
double arm_amplitude_deg = this->get_parameter("arm_amplitude_deg").as_double();
arm_amplitude_rad_ = arm_amplitude_deg * M_PI / 180.0;
arm_frequency_ = this->get_parameter("arm_frequency").as_double();
int period_ms = this->get_parameter("control_period_ms").as_int();
wheel_radius_ = this->get_parameter("wheel_radius").as_double();
wheel_separation_ = this->get_parameter("wheel_separation").as_double();
linear_speed_ = this->get_parameter("linear_speed").as_double();
angular_speed_ = this->get_parameter("angular_speed").as_double();
joint_directions_ = {1, -1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; // 关节方向
total_joints_ = 15; // 总关节数
// 关节索引定义
leg_joint_indices_ = {0, 1, 2, 3}; // 腿部关节索引
wheel_joint_indices_ = {4, 5, 6, 7};
pitch_body_indices_ = {8}; // 身体关节索引
arm_joint_indices_ = {9, 10, 11, 12, 13, 14}; // 手臂关节索引
wheel_joint_directions_ = {1, -1, 1, -1}; // 轮子方向
// 初始化轮子目标角度
wheel_targets_.resize(4, 0.0);
// 初始化关节状态
joint_positions_.resize(total_joints_, 0.0);
joint_velocities_.resize(total_joints_, 0.0);
// 创建发布者
cmd_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
"joint_commands", 10
);
joint_states_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
"joint_states", 10,
std::bind(&RobotControlNode::joint_states_callback, this, std::placeholders::_1)
);
// 创建订阅者（接收来自速度指令）
twist_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel", 10,
std::bind(&RobotControlNode::twist_callback, this, std::placeholders::_1)
);
// 创建定时器
timer_ = this->create_wall_timer(
std::chrono::milliseconds(period_ms),
std::bind(&RobotControlNode::timer_callback, this)
);
// 记录起始时间
start_time_ = this->now();
last_time_ = this->now();
}
RobotControlNode::~RobotControlNode()
{
// 关闭节点时复位关节
resetJoints();
}
// 关节复位
void RobotControlNode::resetJoints()
{
auto reset_msg = std_msgs::msg::Float64MultiArray();
reset_msg.data.resize(total_joints_, 0.0);
cmd_pub_->publish(reset_msg);
}
void RobotControlNode::joint_states_callback(const sensor_msgs::msg::JointState::SharedPtr msg)
{
// 保存当前关节角度
joint_states_ = *msg;
for (size_t i = 0; i < msg->position.size(); i++)
{
// joint_positions_[i] = msg->position[i];
// joint_velocities_[i] = msg->velocity[i];
}
}
void RobotControlNode::twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
if (!msg) { // 检查消息是否有效
RCLCPP_WARN(this->get_logger(), "收到空的Twist消息，忽略");
return;
}
// 接收外部速度指令
linear_x_ = msg->linear.x;
angular_z_ = msg->angular.z;
}
void RobotControlNode::timer_callback()
{
// 计算时间差
rclcpp::Time current_time = this->now();
rclcpp::Duration dt = current_time - last_time_;
double dt_sec = dt.seconds();
last_time_ = current_time;
// 构建指令数组
auto cmd_msg = std_msgs::msg::Float64MultiArray();
cmd_msg.data.resize(total_joints_, 0.0);
// 分阶段控制腿部和手臂
double phase_time = (current_time - start_time_).seconds();
handle_phase_motion(cmd_msg, phase_time);
// 计算并设置轮子角度
handle_wheel_control(cmd_msg, dt_sec);
// 发送指令
cmd_pub_->publish(cmd_msg);
RCLCPP_INFO(this->get_logger(), "Joint 1 command: %f", cmd_msg.data[1]);
}
void RobotControlNode::handle_phase_motion(std_msgs::msg::Float64MultiArray& cmd_msg, double current_time)
{
// 第一阶段：前5秒，腿部关节缓慢张开
if (current_time <= 5.0)
{
double progress = current_time / 5.0;
double current_leg_angle = leg_target_rad_ * progress;
// 设置腿部关节角度
for (int idx : leg_joint_indices_)
{
if (idx == 0 || idx == 4) // 左腿1、左腿2关节
{
cmd_msg.data[idx] = -current_leg_angle;
}
else
{
cmd_msg.data[idx] = current_leg_angle;
}
}
// 手臂保持初始位置
for (int idx : arm_joint_indices_)
{
cmd_msg.data[idx] = 0.0;
}
if (current_time >= 4.9 && !phase1_notified_)
{
RCLCPP_INFO(this->get_logger(), "第一阶段即将结束，准备进入手臂摆动阶段");
phase1_notified_ = true;
}
}
// 第二阶段：5秒后，双臂做摆手动作
else
{
// 腿部保持在目标角度
for (int idx : leg_joint_indices_)
{
if (idx == 0 || idx == 4) // 左腿1、左腿2关节
{
cmd_msg.data[idx] = -leg_target_rad_;
}
else
{
cmd_msg.data[idx] = leg_target_rad_;
}
}
// 计算手臂摆动角度
double arm_phase_time = current_time - 5.0;
double arm_angle = arm_amplitude_rad_ * std::sin(2 * M_PI * arm_frequency_ * arm_phase_time);
// 设置手臂关节角度（左右臂对称）
for (size_t i = 0; i < arm_joint_indices_.size(); ++i)
{
if (i >= arm_joint_indices_.size() / 2)
{
cmd_msg.data[arm_joint_indices_[i]] = -arm_angle;
}
else
{
cmd_msg.data[arm_joint_indices_[i]] = arm_angle;
}
cmd_msg.data[arm_joint_indices_[i]] = 0;
}
}
}
void RobotControlNode::handle_wheel_control(std_msgs::msg::Float64MultiArray& cmd_msg, double dt_sec)
{
// 计算左右轮目标线速度
double left_speed = linear_x_ + (angular_z_ * wheel_separation_) / 2.0;
double right_speed = linear_x_ - (angular_z_ * wheel_separation_) / 2.0;
// 计算轮子角速度（rad/s）
double left_omega = left_speed / wheel_radius_;
double right_omega = right_speed / wheel_radius_;
// 积分计算目标角度（弧度）
wheel_targets_[0] += left_omega * dt_sec; // left_wheel1
wheel_targets_[1] += right_omega * dt_sec; // right_wheel1
wheel_targets_[2] += left_omega * dt_sec; // left_wheel2
wheel_targets_[3] += right_omega * dt_sec; // right_wheel2
// 赋值轮子目标角度到指令数组
for (size_t i = 0; i < wheel_joint_indices_.size(); ++i)
{
cmd_msg.data[wheel_joint_indices_[i]] = wheel_joint_directions_[i] * wheel_targets_[i];
}
}
}
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<robot_control::RobotControlNode>();
rclcpp::spin(node);
node->resetJoints();
RCLCPP_INFO(node->get_logger(), "节点退出，发送复位指令");
rclcpp::shutdown();
return 0;
}

186
HiveCoreR0/src/robot_control/src/robot_control_manager.cpp Normal file
View File

@@ -0,0 +1,186 @@
#include "robot_control_manager.hpp"
using namespace robot_control;
RobotControlManager::RobotControlManager()
{
this->init();
}
void RobotControlManager::init()
{
// Initialize the robot control manager
std::cout << "Initializing robot control manager..." << std::endl;
// Initialize the robot control manager
motionParams_ = MotionParameters();
// Initialize the joint commands
for (int i = 0; i < motionParams_.total_joints_; i++)
{
jointCommands_.data.push_back(0.0);
}
armController_ = new ArmControl(motionParams_.arm_joint_indices_.size());
legController_ = new LegControl(motionParams_.leg_joint_indices_.size(), motionParams_.max_linear_speed_z_, motionParams_.leg_length_);
wheelController_ = new WheelControl(
motionParams_.wheel_joint_indices_.size(),
motionParams_.wheel_radius_,
motionParams_.wheel_separation_,
motionParams_.max_linear_speed_x_,
motionParams_.max_angular_speed_z_
);
MoveCounter_ = 0;
MoveTime_ = 0.0;
}
RobotControlManager::~RobotControlManager()
{
// Clean up the robot control manager
this->cleanup();
delete armController_;
delete legController_;
delete wheelController_;
delete robotModel_;
}
void RobotControlManager::cleanup()
{
// Clean up the robot control manager
std::cout << "Cleaning up robot control manager..." << std::endl;
}
void RobotControlManager::UpdateJointStates(sensor_msgs::msg::JointState joint_states)
{
// Update the joint states
jointStates_ = joint_states;
}
std_msgs::msg::Float64MultiArray RobotControlManager::GetJointCommands()
{
// Get the joint commands
return jointCommands_;
}
void RobotControlManager::SetJointCommands(std_msgs::msg::Float64MultiArray joint_commands)
{
// Set the joint commands
jointCommands_ = joint_commands;
}
void RobotControlManager::ResetJoints()
{
// Reset the joints
std::cout << "Resetting joints..." << std::endl;
for (int i = 0; i < motionParams_.total_joints_; i++)
{
jointCommands_.data[i] = 0.0;
}
}
void RobotControlManager::MoveForward(double dt)
{
// Move forward
wheelController_ -> setLinearAngularSpeed(motionParams_.linear_speed_x_, 0);
std::vector<double> tempWheelCmd;
wheelController_ -> Move(tempWheelCmd, dt);
for (int i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.wheel_joint_indices_[i]] += motionParams_.wheel_joint_directions_[i] * tempWheelCmd[i];
}
}
void RobotControlManager::MoveBackward(double dt)
{
// Move forward
wheelController_ -> setLinearAngularSpeed( - motionParams_.linear_speed_x_, 0.0);
std::vector<double> tempDeltaWheelCmd;
wheelController_ -> Move(tempDeltaWheelCmd, dt);
for (int i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.wheel_joint_indices_[i]] += motionParams_.wheel_joint_directions_[i] * tempDeltaWheelCmd[i];
}
}
void RobotControlManager::TurnLeft(double dt)
{
// Turn left
wheelController_ -> setLinearAngularSpeed(0.0, - motionParams_.angular_speed_z_);
std::vector<double> tempDeltaWheelCmd;
wheelController_ -> Move(tempDeltaWheelCmd, dt);
for (int i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.wheel_joint_indices_[i]] += motionParams_.wheel_joint_directions_[i] * tempDeltaWheelCmd[i];
}
}
void RobotControlManager::TurnRight(double dt)
{
// Turn right
wheelController_ -> setLinearAngularSpeed(0.0, motionParams_.angular_speed_z_);
std::vector<double> tempDeltaWheelCmd;
wheelController_ -> Move(tempDeltaWheelCmd, dt);
for (int i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.wheel_joint_indices_[i]] += motionParams_.wheel_joint_directions_[i] * tempDeltaWheelCmd[i];
}
}
void RobotControlManager::Stop()
{
// Stop
wheelController_ -> setLinearAngularSpeed(0.0, 0.0);
std::vector<double> tempDeltaWheelCmd;
wheelController_ -> Move(tempDeltaWheelCmd, 0.0);
}
void RobotControlManager::MoveUp(double dt)
{
legController_ -> setLegSpeed(motionParams_.linear_speed_z_);
std::vector<double> tempDeltaLegCmd;
legController_ -> Move(tempDeltaLegCmd, dt);
for (int i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.leg_joint_indices_[i]] -= motionParams_.leg_joint_directions_[i] * tempDeltaLegCmd[i];
}
for (int i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.wheel_joint_indices_[i]] -= motionParams_.leg_wheel_directions_[i] * tempDeltaLegCmd[i];
}
}
void RobotControlManager::MoveDown(double dt)
{
legController_ -> setLegSpeed( - motionParams_.linear_speed_z_);
std::vector<double> tempDeltaLegCmd;
legController_ -> Move(tempDeltaLegCmd, dt);
for (int i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.leg_joint_indices_[i]] -= motionParams_.leg_joint_directions_[i] * tempDeltaLegCmd[i];
}
for (int i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.wheel_joint_indices_[i]] -= motionParams_.leg_wheel_directions_[i] * tempDeltaLegCmd[i];
}
}

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

@@ -1,133 +1,217 @@
#include "rclcpp/rclcpp.hpp"
#include "robot_fsm.hpp"
#include <conio.h> // 用于键盘输入 (Windows)
#include <random> // 用于模拟一些输入
// 机器人控制器构造函数
RobotFsm::RobotFsm() : currentState(RobotState::STOPPED) {
initTransitionTable();
RobotFsm::RobotFsm() : Node("keyboard_control_node"){
InitTransitionTable();
executionTime_ = 0.0;
// 添加输入设备
AddInputDevice(InputType::KEYBOARD, std::make_unique<KeyboardInput>());
AddInputDevice(InputType::VOICE, std::make_unique<VoiceInput>());
AddInputDevice(InputType::GAMEPAD, std::make_unique<GamepadInput>());
// 创建发布者
cmdPub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("joint_commands", 10);
// 订阅键盘输出节点数据
jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("joint_states", 10,std::bind(&RobotFsm::JointStatesCallback, this, std::placeholders::_1));
// 创建订阅者（接收来自速度指令）
keyboadCommandSub_ = this->create_subscription<std_msgs::msg::String>("keyboard_command", 10,std::bind(&RobotFsm::KeyboardCommandCallback, this, std::placeholders::_1));
// 创建订阅者（接收来自语音指令）
voiceCommandSub_ = this->create_subscription<std_msgs::msg::String>("voice_command", 10,std::bind(&RobotFsm::VoiceCommandCallback, this, std::placeholders::_1));
lastTime_ = this->now(); // 初始化时间
// 创建定时器，每10ms执行一次控制逻辑（频率100Hz）
controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(10),std::bind(&RobotFsm::ControlLoop, this)); // 绑定到新的控制函数);
}
RobotFsm::~RobotFsm()
{
std::cout << "Robot controller stopped." << std::endl;
}
void RobotFsm::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg) {
// 处理接收到的关节状态数据
// ...
}
void RobotFsm::KeyboardCommandCallback(const std_msgs::msg::String::SharedPtr msg) {
if (!msg) { // 检查消息是否有效
RCLCPP_WARN(this->get_logger(), "收到空的keyboard消息,忽略");
return;
}
// 解析消息内容
std::string command = msg->data;
RCLCPP_WARN(this->get_logger(), "收到键盘指令： %s", command.c_str());
if (command == "w") {
inputDevices_[InputType::KEYBOARD]->SetCommand(ControlCommand::FORWARD);
}
else if (command == "s") {
inputDevices_[InputType::KEYBOARD]->SetCommand(ControlCommand::BACKWARD);
}
else if (command == "a") {
inputDevices_[InputType::KEYBOARD]->SetCommand(ControlCommand::LEFT);
}
else if (command == "d") {
inputDevices_[InputType::KEYBOARD]->SetCommand(ControlCommand::RIGHT);
}
else if (command == "u") {
inputDevices_[InputType::KEYBOARD]->SetCommand(ControlCommand::UP);
}
else if (command == "j") {
inputDevices_[InputType::KEYBOARD]->SetCommand(ControlCommand::DOWN);
}
else if (command == "x") {
inputDevices_[InputType::KEYBOARD]->SetCommand(ControlCommand::STOP);
}
else {
RCLCPP_WARN(this->get_logger(), "未知的命令: %s", command.c_str());
}
}
void RobotFsm::VoiceCommandCallback(const std_msgs::msg::String::SharedPtr msg) {
if (!msg) { // 检查消息是否有效
RCLCPP_WARN(this->get_logger(), "收到空的keyboard消息,忽略");
return;
}
// 解析消息内容
std::string command = msg->data;
if (command == "前进。") {
inputDevices_[InputType::VOICE]->SetCommand(ControlCommand::FORWARD);
}
else if (command == "后退。") {
inputDevices_[InputType::VOICE]->SetCommand(ControlCommand::BACKWARD);
}
else if (command == "左转。") {
inputDevices_[InputType::VOICE]->SetCommand(ControlCommand::LEFT);
}
else if (command == "右转。") {
inputDevices_[InputType::VOICE]->SetCommand(ControlCommand::RIGHT);
}
else if (command == "上升。") {
inputDevices_[InputType::VOICE]->SetCommand(ControlCommand::UP);
}
else if (command == "下降。") {
inputDevices_[InputType::VOICE]->SetCommand(ControlCommand::DOWN);
}
else if (command == "停止。") {
inputDevices_[InputType::VOICE]->SetCommand(ControlCommand::STOP);
}
else {
RCLCPP_WARN(this->get_logger(), "未知的命令: %s", command.c_str());
}
}
// 初始化状态转换表
void RobotFsm::initTransitionTable() {
void RobotFsm::InitTransitionTable() {
// 从任意状态都可以转换到停止状态
transitionTable[{RobotState::STOPPED, ControlCommand::STOP}] = RobotState::STOPPED;
transitionTable[{RobotState::MOVING_FORWARD, ControlCommand::STOP}] = RobotState::STOPPED;
transitionTable[{RobotState::MOVING_BACKWARD, ControlCommand::STOP}] = RobotState::STOPPED;
transitionTable[{RobotState::TURNING_LEFT, ControlCommand::STOP}] = RobotState::STOPPED;
transitionTable[{RobotState::TURNING_RIGHT, ControlCommand::STOP}] = RobotState::STOPPED;
transitionTable[{RobotState::MOVING_UP, ControlCommand::STOP}] = RobotState::STOPPED;
transitionTable[{RobotState::MOVING_DOWN, ControlCommand::STOP}] = RobotState::STOPPED;
transitionTable_[{RobotState::STOPPED, ControlCommand::STOP}] = RobotState::STOPPED;
transitionTable_[{RobotState::MOVING_FORWARD, ControlCommand::STOP}] = RobotState::STOPPED;
transitionTable_[{RobotState::MOVING_BACKWARD, ControlCommand::STOP}] = RobotState::STOPPED;
transitionTable_[{RobotState::TURNING_LEFT, ControlCommand::STOP}] = RobotState::STOPPED;
transitionTable_[{RobotState::TURNING_RIGHT, ControlCommand::STOP}] = RobotState::STOPPED;
transitionTable_[{RobotState::MOVING_UP, ControlCommand::STOP}] = RobotState::STOPPED;
transitionTable_[{RobotState::MOVING_DOWN, ControlCommand::STOP}] = RobotState::STOPPED;
// 从停止状态转换到其他状态
transitionTable[{RobotState::STOPPED, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
transitionTable[{RobotState::STOPPED, ControlCommand::BACKWARD}] = RobotState::MOVING_BACKWARD;
transitionTable[{RobotState::STOPPED, ControlCommand::LEFT}] = RobotState::TURNING_LEFT;
transitionTable[{RobotState::STOPPED, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT;
transitionTable[{RobotState::STOPPED, ControlCommand::UP}] = RobotState::MOVING_UP;
transitionTable[{RobotState::STOPPED, ControlCommand::DOWN}] = RobotState::MOVING_DOWN;
// 从运动状态转换到其他运动状态 TODO: 不支持直接切换
// transitionTable[{RobotState::MOVING_FORWARD, ControlCommand::BACKWARD}] = RobotState::MOVING_BACKWARD;
// transitionTable[{RobotState::MOVING_FORWARD, ControlCommand::LEFT}] = RobotState::TURNING_LEFT;
// transitionTable[{RobotState::MOVING_FORWARD, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT;
// transitionTable[{RobotState::MOVING_BACKWARD, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
// transitionTable[{RobotState::MOVING_BACKWARD, ControlCommand::LEFT}] = RobotState::TURNING_LEFT;
// transitionTable[{RobotState::MOVING_BACKWARD, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT;
// 其他可能的状态转换...
// transitionTable[{RobotState::TURNING_LEFT, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
// transitionTable[{RobotState::TURNING_RIGHT, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
transitionTable_[{RobotState::STOPPED, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
transitionTable_[{RobotState::STOPPED, ControlCommand::BACKWARD}] = RobotState::MOVING_BACKWARD;
transitionTable_[{RobotState::STOPPED, ControlCommand::LEFT}] = RobotState::TURNING_LEFT;
transitionTable_[{RobotState::STOPPED, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT;
transitionTable_[{RobotState::STOPPED, ControlCommand::UP}] = RobotState::MOVING_UP;
transitionTable_[{RobotState::STOPPED, ControlCommand::DOWN}] = RobotState::MOVING_DOWN;
}
// 添加输入设备
void RobotFsm::addInputDevice(InputType type, std::unique_ptr<InputDevice> device) {
inputDevices[type] = std::move(device);
std::cout << "Added input device: " << inputDevices[type]->getDeviceName() << std::endl;
void RobotFsm::AddInputDevice(InputType type, std::unique_ptr<InputDevice> device) {
inputDevices_[type] = std::move(device);
std::cout << "Added input device: " << inputDevices_[type]->GetDeviceName() << std::endl;
}
// 状态机主循环
void RobotFsm::run() {
std::cout << "Robot controller started. Press 'q' to quit." << std::endl;
std::cout << "Keyboard controls: WASD to move, Q to quit, U to up, D to down" << std::endl;
while (true) {
void RobotFsm::ControlLoop() {
// 检查所有输入设备
ControlCommand cmd = ControlCommand::NONE;
for (const auto& [type, device] : inputDevices) {
ControlCommand deviceCmd = device->getCommand();
for (const auto& [type, device] : inputDevices_) {
ControlCommand deviceCmd = device->GetCommand();
if (deviceCmd != ControlCommand::NONE) {
cmd = deviceCmd;
break; // 优先级：先获取到的命令先执行
}
}
// 处理退出命令
if (cmd == ControlCommand::NONE) {
// 检查键盘的退出命令（特殊处理）
if (_kbhit()) {
char key = _getch();
if (key == 'q' || key == 'Q') {
std::cout << "Exiting robot controller..." << std::endl;
processCommand(ControlCommand::STOP);
break;
}
}
} else {
processCommand(cmd);
}
ProcessCommand(cmd);
// 计算时间差
rclcpp::Time current_time = this->now();
rclcpp::Duration dt = current_time - lastTime_;
double dt_sec = dt.seconds();
lastTime_ = current_time;
// 执行当前状态的动作
executeStateAction();
// 控制循环频率
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
ExecuteStateAction(dt_sec);
auto cmd_msg = robotControlManager_.GetJointCommands();
cmdPub_->publish(cmd_msg);
}
// 处理命令并转换状态
void RobotController::processCommand(ControlCommand cmd) {
void RobotFsm::ProcessCommand(ControlCommand cmd) {
if (cmd == ControlCommand::NONE) return;
auto it = transitionTable.find({currentState, cmd});
if (it != transitionTable.end()) {
auto it = transitionTable_.find({currentState_, cmd});
if (it != transitionTable_.end()) {
RobotState newState = it->second;
if (newState != currentState) {
std::cout << "State changed: " << stateToString(currentState)
<< " -> " << stateToString(newState) << std::endl;
currentState = newState;
if (newState != currentState_) {
std::cout << "State changed: " << StateToString(currentState_)
<< " -> " << StateToString(newState) << std::endl;
currentState_ = newState;
}
}
}
// 执行当前状态对应的动作
void RobotController::executeStateAction() {
// 在实际应用中，这里会发送相应的控制信号给机器人
switch (currentState) {
void RobotFsm::ExecuteStateAction(double dt) {
switch (currentState_) {
case RobotState::STOPPED:
// 发送停止命令
robotControlManager_.Stop();
break;
case RobotState::MOVING_FORWARD:
// 发送前进命令
robotControlManager_.MoveForward(dt);
break;
case RobotState::MOVING_BACKWARD:
// 发送后退命令
robotControlManager_.MoveBackward(dt);
break;
case RobotState::TURNING_LEFT:
robotControlManager_.TurnLeft(dt);
// 发送左转命令
break;
case RobotState::TURNING_RIGHT:
robotControlManager_.TurnRight(dt);
// 发送右转命令
break;
case RobotState::MOVING_UP:
// 发送上升命令
robotControlManager_.MoveUp(dt);
break;
case RobotState::MOVING_DOWN:
robotControlManager_.MoveDown(dt);
// 发送下降命令
break;
}
}
// 状态转换为字符串
std::string RobotController::stateToString(RobotState state) const {
std::string RobotFsm::StateToString(RobotState state) const {
switch (state) {
case RobotState::STOPPED: return "STOPPED";
case RobotState::MOVING_FORWARD: return "MOVING_FORWARD";
@@ -140,61 +224,10 @@ std::string RobotController::stateToString(RobotState state) const {
}
}
// 键盘输入处理
ControlCommand KeyboardInput::getCommand() {
if (_kbhit()) {
char key = _getch();
switch (key) {
case 'w': case 'W': return ControlCommand::FORWARD;
case 's': case 'S': return ControlCommand::BACKWARD;
case 'a': case 'A': return ControlCommand::LEFT;
case 'd': case 'D': return ControlCommand::RIGHT;
case 'u': case 'U': return ControlCommand::UP;
case 'j': case 'J': return ControlCommand::DOWN; // 使用J代替D避免与后退冲突
case ' ': return ControlCommand::STOP; // 空格键停止
default: return ControlCommand::NONE;
}
}
return ControlCommand::NONE;
}
// 手柄输入处理（模拟实现）
ControlCommand GamepadInput::getCommand() {
// 实际应用中这里会读取游戏手柄的输入
// 这里使用随机值模拟手柄输入，仅作示例
static std::random_device rd;
static std::mt19937 gen(rd());
static std::uniform_int_distribution<> dist(0, 100);
int val = dist(gen);
if (val < 10) return ControlCommand::FORWARD;
else if (val < 20) return ControlCommand::BACKWARD;
else if (val < 30) return ControlCommand::LEFT;
else if (val < 40) return ControlCommand::RIGHT;
else if (val < 45) return ControlCommand::UP;
else if (val < 50) return ControlCommand::DOWN;
else if (val < 55) return ControlCommand::STOP;
else return ControlCommand::NONE;
}
// 语音输入处理（模拟实现）
ControlCommand VoiceInput::getCommand() {
// 实际应用中这里会处理语音识别结果
// 这里使用随机值模拟语音输入，仅作示例
static std::random_device rd;
static std::mt19937 gen(rd());
static std::uniform_int_distribution<> dist(0, 100);
int val = dist(gen);
if (val < 5) return ControlCommand::FORWARD;
else if (val < 10) return ControlCommand::BACKWARD;
else if (val < 15) return ControlCommand::LEFT;
else if (val < 20) return ControlCommand::RIGHT;
else if (val < 23) return ControlCommand::UP;
else if (val < 26) return ControlCommand::DOWN;
else if (val < 29) return ControlCommand::STOP;
else return ControlCommand::NONE;
}
int main(int argc, char * argv[]) {
rclcpp::init(argc, argv);
auto node = std::make_shared<RobotFsm>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

6
HiveCoreR0/src/robot_control/src/robot_kinematics.cpp
View File

@@ -3,7 +3,7 @@
#include <stdexcept>
#include <iostream>
namespace robot_model
namespace robot_control
{
RobotKinematics::RobotKinematics(std::shared_ptr<RobotModel> robot_model)
@@ -44,7 +44,7 @@ Eigen::Matrix3d RobotKinematics::rotate_x(double angle)
Eigen::Vector3d RobotKinematics::arm_forward_kinematics(const std::vector<double>& joint_angles)
{
const auto& arm = robot_model_->get_arm();
const auto& arm = robot_model_->get_arm(0);
// 检查关节角度数量是否正确
if (joint_angles.size() != arm.joints.size())
@@ -76,7 +76,7 @@ Eigen::Vector3d RobotKinematics::arm_forward_kinematics(const std::vector<double
bool RobotKinematics::arm_inverse_kinematics(const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles)
{
const auto& arm = robot_model_->get_arm();
const auto& arm = robot_model_->get_arm(0);
joint_angles.resize(4, 0.0); // 4个关节
// 连杆长度

14
HiveCoreR0/src/robot_control/src/robot_model.cpp
View File

@@ -2,7 +2,7 @@
#include <iostream>
#include <cmath>
namespace robot_model
namespace robot_control
{
RobotModel::RobotModel()
@@ -14,14 +14,14 @@ namespace robot_model
{
// 初始化机械臂（4个关节，2个连杆）
// 添加关节
arm_.joints.push_back(std::make_shared<Joint>("arm_base_joint", -M_PI/2, M_PI/2, 10.0));
arm_.joints.push_back(std::make_shared<Joint>("arm_shoulder_joint", -M_PI/2, M_PI/2, 8.0));
arm_.joints.push_back(std::make_shared<Joint>("arm_elbow_joint", 0, M_PI/2, 6.0));
arm_.joints.push_back(std::make_shared<Joint>("arm_wrist_joint", -M_PI/4, M_PI/4, 4.0));
arms_[0].joints.push_back(std::make_shared<Joint>("arm_base_joint", -M_PI/2, M_PI/2, 10.0));
arms_[0].joints.push_back(std::make_shared<Joint>("arm_shoulder_joint", -M_PI/2, M_PI/2, 8.0));
arms_[0].joints.push_back(std::make_shared<Joint>("arm_elbow_joint", 0, M_PI/2, 6.0));
arms_[0].joints.push_back(std::make_shared<Joint>("arm_wrist_joint", -M_PI/4, M_PI/4, 4.0));
// 添加连杆
arm_.links.push_back(std::make_shared<Link>("arm_upper_link", 0.3, 1.5));
arm_.links.push_back(std::make_shared<Link>("arm_lower_link", 0.25, 1.0));
arms_[0].links.push_back(std::make_shared<Link>("arm_upper_link", 0.3, 1.5));
arms_[0].links.push_back(std::make_shared<Link>("arm_lower_link", 0.25, 1.0));
// 初始化腿部（4条腿，每条腿2个连杆）
std::vector<std::string> leg_sides = {"front_left", "front_right", "rear_left", "rear_right"};

83
HiveCoreR0/src/robot_control/src/wheel_control.cpp Normal file
View File

@@ -0,0 +1,83 @@
#include "wheel_control.hpp"
namespace robot_control
{
using namespace std::chrono_literals;
WheelControl::WheelControl(int totalJoints, double wheelRadius, double wheelSeparation, double maxLinearSpeed, double maxAngularSpeed)
{
totalJoints_ = totalJoints; // 总关节数
wheelRadius_ = wheelRadius; // 轮子半径
wheelSeparation_ = wheelSeparation; // 轮子间距
maxLinearSpeed_ = maxLinearSpeed; // 线速度
maxAngularSpeed_ = maxAngularSpeed; // 角速度
// 初始化轮子目标角度
wheelTargets_.resize(totalJoints_, 0.0);
}
WheelControl::~WheelControl()
{
// 关闭节点时复位关节
ResetJoints();
}
// 关节复位
void WheelControl::ResetJoints()
{
wheelTargets_.resize(totalJoints_, 0.0);
}
void WheelControl::Move(std::vector<double>& deltaJointPositions, double dt)
{
CalculateWheelTargets(deltaJointPositions, dt);
}
void WheelControl::CalculateWheelTargets(std::vector<double>& deltaJointPositions, double dt_sec)
{
// 计算左右轮目标线速度
double leftSpeed = linearSpeed_ + (angularSpeed_ * wheelSeparation_) / 2.0;
double rightSpeed = linearSpeed_ - (angularSpeed_ * wheelSeparation_) / 2.0;
// 计算轮子角速度（rad/s）
double leftOmega = leftSpeed / wheelRadius_;
double rightOmega = rightSpeed / wheelRadius_;
// 积分计算目标角度（弧度）
wheelTargets_[0] = leftOmega * dt_sec; // left_wheel1
wheelTargets_[1] = rightOmega * dt_sec; // right_wheel1
wheelTargets_[2] = leftOmega * dt_sec; // left_wheel2
wheelTargets_[3] = rightOmega * dt_sec; // right_wheel2
deltaJointPositions = wheelTargets_;
}
void WheelControl::setLinearAngularSpeed(double linearSpeed, double angularSpeed)
{
// 检查是否超限
if (linearSpeed > maxLinearSpeed_ )
{
linearSpeed = maxLinearSpeed_;
}
else if (linearSpeed < -maxLinearSpeed_)
{
linearSpeed = -maxLinearSpeed_;
}
if (angularSpeed > maxAngularSpeed_)
{
angularSpeed = maxAngularSpeed_;
}
else if (angularSpeed < -maxAngularSpeed_)
{
angularSpeed = -maxAngularSpeed_;
}
if (linearSpeed != linearSpeed_ || angularSpeed != angularSpeed_)
{
linearSpeed_ = linearSpeed;
angularSpeed_ = angularSpeed;
}
}
}

2
HiveCoreR0/src/voice_control/src/iat_publish.cpp
View File

@@ -96,7 +96,7 @@ int main(int argc, char* argv[])
rclcpp::Rate loop_rate(10);
// 发布语音识别结果
auto voiceWordsPub = node->create_publisher<std_msgs::msg::String>("voiceWords", 1000);
auto voiceWordsPub = node->create_publisher<std_msgs::msg::String>("voice_command", 1000);
RCLCPP_INFO(node->get_logger(), "Sleeping...");

2
HiveCoreR1/src/robot_simulation/launch/gazebo_launch.py
View File

@@ -8,7 +8,7 @@ from launch.substitutions import LaunchConfiguration
def generate_launch_description():
# 设置参数
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
world_file_name = 'empty.world'
world_path = os.path.join(get_package_share_directory('robot_simulation'), 'worlds', world_file_name)