132 lines
3.8 KiB
C++
132 lines
3.8 KiB
C++
#ifndef ROBOT_MOTION_ACTION_H
|
||
#define ROBOT_MOTION_ACTION_H
|
||
|
||
#include <action_msgs/msg/goal_status.hpp>
|
||
#include <rclcpp/rclcpp.hpp>
|
||
#include <rclcpp_action/rclcpp_action.hpp>
|
||
#include <Eigen/Dense>
|
||
#include <string>
|
||
|
||
// 定义Action消息类型
|
||
namespace robot_control {
|
||
namespace ros {
|
||
|
||
// 运动目标
|
||
struct MotionGoal {
|
||
// 运动类型：移动、操作、姿态调整等
|
||
enum class Type {
|
||
MOVE_BASE, // 移动基座
|
||
ARM_MOTION, // 手臂运动
|
||
LEG_MOTION, // 腿部运动
|
||
BODY_POSE, // 身体姿态
|
||
WHOLE_BODY_MOTION // 全身运动
|
||
};
|
||
|
||
Type type;
|
||
|
||
// 基础移动目标 (用于MOVE_BASE类型)
|
||
struct BaseTarget {
|
||
double x; // x位置 (m)
|
||
double y; // y位置 (m)
|
||
double yaw; // 偏航角 (rad)
|
||
double speed; // 移动速度 (m/s)
|
||
};
|
||
|
||
// 手臂运动目标 (用于ARM_MOTION类型)
|
||
struct ArmTarget {
|
||
bool leftArm; // 是否是左臂
|
||
Eigen::Vector3d position; // 位置目标 (m)
|
||
Eigen::Vector3d orientation; // 姿态目标 (rad)
|
||
bool relative; // 是否是相对运动
|
||
};
|
||
|
||
// 腿部运动目标 (用于LEG_MOTION类型)
|
||
struct LegTarget {
|
||
int legIndex; // 腿索引 (0-3)
|
||
Eigen::Vector3d position; // 位置目标 (m)
|
||
bool relative; // 是否是相对运动
|
||
};
|
||
|
||
// 身体姿态目标 (用于BODY_POSE类型)
|
||
struct BodyPoseTarget {
|
||
double roll; // 横滚角 (rad)
|
||
double pitch; // 俯仰角 (rad)
|
||
double yaw; // 偏航角 (rad)
|
||
double height; // 高度 (m)
|
||
};
|
||
|
||
// 根据运动类型选择对应的目标数据
|
||
union {
|
||
BaseTarget base;
|
||
ArmTarget arm;
|
||
LegTarget leg;
|
||
BodyPoseTarget bodyPose;
|
||
} target;
|
||
|
||
// 运动参数
|
||
double duration; // 期望运动时间 (s)，0表示使用默认
|
||
bool blocking; // 是否阻塞等待完成
|
||
};
|
||
|
||
// 运动反馈
|
||
struct MotionFeedback {
|
||
double progress; // 进度 (0.0-1.0)
|
||
Eigen::VectorXd currentState; // 当前状态
|
||
std::string status; // 状态描述
|
||
};
|
||
|
||
// 运动结果
|
||
struct MotionResult {
|
||
bool success; // 是否成功
|
||
std::string message; // 结果消息
|
||
double actualDuration; // 实际运动时间 (s)
|
||
};
|
||
|
||
// Action接口类
|
||
class RobotMotionAction {
|
||
public:
|
||
using Ptr = std::shared_ptr<RobotMotionAction>;
|
||
|
||
using GoalHandle = rclcpp_action::ServerGoalHandle<robot_control_msgs::action::RobotMotion>;
|
||
|
||
RobotMotionAction(rclcpp::Node::SharedPtr node);
|
||
|
||
// 发送目标
|
||
void sendGoal(const MotionGoal& goal);
|
||
|
||
// 取消目标
|
||
void cancelGoal();
|
||
|
||
// 注册目标回调
|
||
using GoalCallback = std::function<void(const MotionGoal&)>;
|
||
void setGoalCallback(GoalCallback callback);
|
||
|
||
// 注册取消回调
|
||
using CancelCallback = std::function<void()>;
|
||
void setCancelCallback(CancelCallback callback);
|
||
|
||
// 发布反馈
|
||
void publishFeedback(const MotionFeedback& feedback);
|
||
|
||
// 发布结果
|
||
void publishResult(GoalHandle::SharedPtr goalHandle, const MotionResult& result);
|
||
|
||
private:
|
||
rclcpp::Node::SharedPtr node_;
|
||
rclcpp_action::Server<robot_control_msgs::action::RobotMotion>::SharedPtr actionServer_;
|
||
|
||
GoalCallback goalCallback_;
|
||
CancelCallback cancelCallback_;
|
||
|
||
// Action回调函数
|
||
rclcpp_action::GoalResponse handleGoal(const rclcpp_action::GoalUUID& uuid,
|
||
std::shared_ptr<const robot_control_msgs::action::RobotMotion::Goal> goal);
|
||
rclcpp_action::CancelResponse handleCancel(const std::shared_ptr<GoalHandle> goalHandle);
|
||
void handleAccepted(const std::shared_ptr<GoalHandle> goalHandle);
|
||
};
|
||
|
||
} // namespace ros
|
||
} // namespace robot_control
|
||
|
||
#endif // ROBOT_MOTION_ACTION_H
|