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

moveit version is not good

Browse Source
This commit is contained in:
Your Name
2026-01-17 12:59:34 +08:00
parent 84678115e2
commit 3b3bdc77a5
35 changed files with 2170 additions and 2043 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/robot_control/CMakeLists.txt
View File

@@ -56,6 +56,12 @@ set(SOURCES
src/core/remote_controller.cpp
src/core/motion_parameters.cpp
src/actions/action_manager.cpp
src/actions/follow_jt_executor.cpp
src/actions/move_home_action.cpp
src/actions/move_leg_action.cpp
src/actions/move_waist_action.cpp
src/actions/move_wheel_action.cpp
src/actions/dual_arm_action.cpp
src/controllers/control_base.cpp
src/controllers/leg_control.cpp
src/controllers/arm_control.cpp

3
src/robot_control/REFACTORING_GUIDE.md
View File

@@ -192,8 +192,7 @@ void RobotControlNode::ControlLoop()
{
robotControlManager_.SetJogWheel(false);
action_manager_->set_move_home_executing(false);
robotControlManager_.WheelReset();
robotControlManager_.ImuReset();
// Note: WheelReset/ImuReset were removed during refactor; state is handled internally now.
}
}
// ... 类似地处理其他 action ...

26
src/robot_control/include/actions/action_context.hpp Normal file
View File

@@ -0,0 +1,26 @@
#pragma once
#include <functional>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "control_msgs/action/follow_joint_trajectory.hpp"
namespace robot_control
{
class RobotControlManager;
struct ActionContext
{
using FollowJTA = control_msgs::action::FollowJointTrajectory;
rclcpp::Node* node{nullptr};
RobotControlManager* robot_control_manager{nullptr};
rclcpp_action::Client<FollowJTA>::SharedPtr follow_jt_client;
};
} // namespace robot_control

264
src/robot_control/include/actions/action_manager.hpp
View File

@@ -1,252 +1,62 @@
/**
* @file action_manager.hpp
* @brief Action管理器 - 统一管理所有ROS 2 Action服务器
* @author Ray
* @date 2025
*
* 该类负责：
* - 创建和管理所有Action服务器（MoveHome, MoveLeg, MoveWaist, MoveWheel）
* - 处理Action的目标请求、取消和执行
* - 提供Action执行状态查询接口
* @brief Thin orchestrator that wires per-action handlers.
*/
#pragma once
#include <memory>
#include <thread>
#include <atomic>
#include <functional>
#include <mutex>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "interfaces/action/move_home.hpp"
#include "interfaces/action/move_leg.hpp"
#include "interfaces/action/move_waist.hpp"
#include "interfaces/action/move_wheel.hpp"
#include "interfaces/action/dual_arm.hpp"
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "interfaces/msg/motor_cmd.hpp"
#include "interfaces/srv/motor_param.hpp"
using MoveHome = interfaces::action::MoveHome;
using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
using MoveLeg = interfaces::action::MoveLeg;
using GoalHandleMoveLeg = rclcpp_action::ServerGoalHandle<MoveLeg>;
using MoveWaist = interfaces::action::MoveWaist;
using GoalHandleMoveWaist = rclcpp_action::ServerGoalHandle<MoveWaist>;
using MoveWheel = interfaces::action::MoveWheel;
using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;
using DualArm = interfaces::action::DualArm;
using GoalHandleDualArm = rclcpp_action::ServerGoalHandle<DualArm>;
using MotorCmd = interfaces::msg::MotorCmd;
using MotorParam = interfaces::srv::MotorParam;
namespace robot_control
{
// 前向声明
class RobotControlManager;
class RobotControlManager;
/**
* @class ActionManager
* @brief Action管理器类
*
* 统一管理所有ROS 2 Action服务器，包括：
* - MoveHome: 回零动作
* - MoveLeg: 腿部运动动作
* - MoveWaist: 腰部运动动作
* - MoveWheel: 轮子运动动作
* - DualArm: 双臂运动动作
*/
class ActionManager
{
public:
/**
* @brief 构造函数
* @param node ROS 2节点指针
* @param robot_control_manager 机器人控制管理器引用
* @param is_jog_mode_func 获取Jog模式状态的函数
* @param motor_cmd_pub 电机命令发布器
* @param motor_param_client 电机参数客户端
*/
ActionManager(
rclcpp::Node* node,
RobotControlManager& robot_control_manager,
std::function<bool()> is_jog_mode_func,
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub,
rclcpp::Client<MotorParam>::SharedPtr motor_param_client);
/**
* @brief 析构函数
*/
~ActionManager();
class MoveHomeAction;
class MoveLegAction;
class MoveWaistAction;
class MoveWheelAction;
class DualArmAction;
/**
* @brief 初始化所有Action服务器
*/
void initialize();
class ActionManager
{
public:
using MotorCmd = interfaces::msg::MotorCmd;
using MotorParam = interfaces::srv::MotorParam;
using FollowJTA = control_msgs::action::FollowJointTrajectory;
// ==================== 执行状态查询接口 ====================
/**
* @brief 检查MoveHome是否正在执行
* @return true 正在执行，false 未执行
*/
bool is_move_home_executing() const { return move_home_executing_.load(); }
/**
* @brief 检查MoveLeg是否正在执行
* @return true 正在执行，false 未执行
*/
bool is_move_leg_executing() const { return move_leg_executing_.load(); }
/**
* @brief 检查MoveWaist是否正在执行
* @return true 正在执行，false 未执行
*/
bool is_move_waist_executing() const { return move_waist_executing_.load(); }
/**
* @brief 检查MoveWheel是否正在执行
* @return true 正在执行，false 未执行
*/
bool is_move_wheel_executing() const { return move_wheel_executing_.load(); }
/**
* @brief 检查DualArm是否正在执行
* @return true 正在执行，false 未执行
*/
bool is_dual_arm_executing() const { return dual_arm_executing_.load(); }
ActionManager(
rclcpp::Node* node,
RobotControlManager& robot_control_manager,
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub,
rclcpp::Client<MotorParam>::SharedPtr motor_param_client);
// ==================== 执行状态设置接口 ====================
/**
* @brief 设置MoveHome执行状态
* @param value true 设置为执行中，false 设置为未执行
*/
void set_move_home_executing(bool value) { move_home_executing_.store(value); }
/**
* @brief 设置MoveLeg执行状态
* @param value true 设置为执行中，false 设置为未执行
*/
void set_move_leg_executing(bool value) { move_leg_executing_.store(value); }
/**
* @brief 设置MoveWaist执行状态
* @param value true 设置为执行中，false 设置为未执行
*/
void set_move_waist_executing(bool value) { move_waist_executing_.store(value); }
/**
* @brief 设置MoveWheel执行状态
* @param value true 设置为执行中，false 设置为未执行
*/
void set_move_wheel_executing(bool value) { move_wheel_executing_.store(value); }
/**
* @brief 设置DualArm执行状态
* @param value true 设置为执行中，false 设置为未执行
*/
void set_dual_arm_executing(bool value) { dual_arm_executing_.store(value); }
~ActionManager();
private:
// ==================== MoveHome Action处理函数 ====================
/**
* @brief 处理MoveHome目标请求
* @param uuid 目标UUID
* @param goal 目标消息
* @return GoalResponse 接受或拒绝目标
*/
rclcpp_action::GoalResponse handle_move_home_goal(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveHome::Goal> goal);
/**
* @brief 处理MoveHome取消请求
* @param goal_handle 目标句柄
* @return CancelResponse 接受或拒绝取消
*/
rclcpp_action::CancelResponse handle_move_home_cancel(
const std::shared_ptr<GoalHandleMoveHome> goal_handle);
/**
* @brief 处理MoveHome目标接受
* @param goal_handle 目标句柄
*/
void handle_move_home_accepted(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
/**
* @brief 执行MoveHome动作
* @param goal_handle 目标句柄
*/
void move_home_execute(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
void initialize();
// ==================== MoveLeg Action处理函数 ====================
rclcpp_action::GoalResponse handle_move_leg_goal(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveLeg::Goal> goal);
rclcpp_action::CancelResponse handle_move_leg_cancel(
const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
void handle_move_leg_accepted(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
void move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
private:
rclcpp::Node* node_{nullptr};
RobotControlManager& robot_control_manager_;
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub_;
rclcpp::Client<MotorParam>::SharedPtr motor_param_client_;
// ==================== MoveWaist Action处理函数 ====================
rclcpp_action::GoalResponse handle_move_waist_goal(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveWaist::Goal> goal);
rclcpp_action::CancelResponse handle_move_waist_cancel(
const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
void handle_move_waist_accepted(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
void move_waist_execute(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
rclcpp_action::Client<FollowJTA>::SharedPtr follow_jt_client_;
// ==================== MoveWheel Action处理函数 ====================
rclcpp_action::GoalResponse handle_move_wheel_goal(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveWheel::Goal> goal);
rclcpp_action::CancelResponse handle_move_wheel_cancel(
const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
void handle_move_wheel_accepted(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
void move_wheel_execute(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
std::unique_ptr<MoveHomeAction> move_home_;
std::unique_ptr<MoveLegAction> move_leg_;
std::unique_ptr<MoveWaistAction> move_waist_;
std::unique_ptr<MoveWheelAction> move_wheel_;
std::unique_ptr<DualArmAction> dual_arm_;
};
} // namespace robot_control
// ==================== DualArm Action处理函数 ====================
rclcpp_action::GoalResponse handle_dual_arm_goal(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const DualArm::Goal> goal);
rclcpp_action::CancelResponse handle_dual_arm_cancel(
const std::shared_ptr<GoalHandleDualArm> goal_handle);
void handle_dual_arm_accepted(const std::shared_ptr<GoalHandleDualArm> goal_handle);
void dual_arm_execute(const std::shared_ptr<GoalHandleDualArm> goal_handle);
// ==================== 成员变量 ====================
rclcpp::Node* node_; ///< ROS 2节点指针
RobotControlManager& robot_control_manager_; ///< 机器人控制管理器引用
std::function<bool()> is_jog_mode_func_; ///< Jog模式状态查询函数
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub_; ///< 电机命令发布器
rclcpp::Client<MotorParam>::SharedPtr motor_param_client_; ///< 电机参数客户端
// Action服务器
rclcpp_action::Server<MoveHome>::SharedPtr move_home_action_server_; ///< MoveHome动作服务器
rclcpp_action::Server<MoveLeg>::SharedPtr move_leg_action_server_; ///< MoveLeg动作服务器
rclcpp_action::Server<MoveWaist>::SharedPtr move_waist_action_server_; ///< MoveWaist动作服务器
rclcpp_action::Server<MoveWheel>::SharedPtr move_wheel_action_server_; ///< MoveWheel动作服务器
rclcpp_action::Server<DualArm>::SharedPtr dual_arm_action_server_; ///< DualArm动作服务器
// Action执行状态标志（使用atomic确保线程安全）
std::atomic<bool> move_home_executing_; ///< MoveHome执行状态
std::atomic<bool> move_leg_executing_; ///< MoveLeg执行状态
std::atomic<bool> move_waist_executing_; ///< MoveWaist执行状态
std::atomic<bool> move_wheel_executing_; ///< MoveWheel执行状态
std::atomic<bool> dual_arm_executing_; ///< DualArm执行状态
};
}

79
src/robot_control/include/actions/dual_arm_action.hpp Normal file
View File

@@ -0,0 +1,79 @@
#pragma once
#include <atomic>
#include <memory>
#include <string>
#include <vector>
#include "rclcpp_action/rclcpp_action.hpp"
#include "interfaces/action/dual_arm.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "actions/action_context.hpp"
#include "actions/follow_jt_executor.hpp"
namespace robot_control
{
struct MotionParameters;
class DualArmAction
{
public:
using DualArm = interfaces::action::DualArm;
using GoalHandleDualArm = rclcpp_action::ServerGoalHandle<DualArm>;
using FollowJTA = control_msgs::action::FollowJointTrajectory;
explicit DualArmAction(const ActionContext& ctx);
void initialize();
bool is_executing() const { return executing_.load(); }
void set_executing(bool v) { executing_.store(v); }
private:
rclcpp_action::GoalResponse handle_goal_(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const DualArm::Goal> goal);
rclcpp_action::CancelResponse handle_cancel_(
const std::shared_ptr<GoalHandleDualArm> goal_handle);
void handle_accepted_(const std::shared_ptr<GoalHandleDualArm> goal_handle);
void execute_(const std::shared_ptr<GoalHandleDualArm> goal_handle);
bool collect_movej_targets_(
const DualArm::Goal& goal,
size_t arm_dof,
bool* need_left,
std::vector<double>* left_target,
bool* need_right,
std::vector<double>* right_target,
std::string* error_msg) const;
bool export_and_normalize_trajectory_(
const MotionParameters& motion_params,
trajectory_msgs::msg::JointTrajectory* out,
double* total_time_sec,
std::string* error_msg) const;
void smooth_trajectory_start_end_(
trajectory_msgs::msg::JointTrajectory* traj,
double max_acceleration = 2.0,
double smooth_duration = 0.15) const;
void fill_feedback_from_fjt_(
const FollowJTA::Feedback& fjt_fb,
const MotionParameters& motion_params,
size_t arm_dof,
double total_time_sec,
DualArm::Feedback* out) const;
ActionContext ctx_;
std::atomic<bool> executing_{false};
rclcpp_action::Server<DualArm>::SharedPtr server_;
std::unique_ptr<FollowJTExecutor> follow_executor_;
};
} // namespace robot_control

66
src/robot_control/include/actions/follow_jt_executor.hpp Normal file
View File

@@ -0,0 +1,66 @@
#pragma once
#include <atomic>
#include <functional>
#include <mutex>
#include <optional>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
namespace robot_control
{
class RobotControlManager;
class FollowJTExecutor
{
public:
using FollowJTA = control_msgs::action::FollowJointTrajectory;
using GoalHandleFollowJTA = rclcpp_action::ClientGoalHandle<FollowJTA>;
struct ExecResult
{
bool ok{false};
std::string message;
};
FollowJTExecutor(
rclcpp::Node* node,
RobotControlManager& robot_control_manager,
rclcpp_action::Client<FollowJTA>::SharedPtr follow_jt_client);
ExecResult send_and_wait(
const trajectory_msgs::msg::JointTrajectory& traj,
const std::function<bool()>& is_cancel_requested,
const std::function<void(const FollowJTA::Feedback&)>& on_feedback);
private:
double compute_soft_stop_duration_sec_(
const std::vector<double>& v0,
const std::vector<double>& amax) const;
trajectory_msgs::msg::JointTrajectory build_soft_stop_trajectory_(
const FollowJTA::Feedback& fb) const;
void save_trajectory_to_file_(
const trajectory_msgs::msg::JointTrajectory& traj) const;
rclcpp::Node* node_{nullptr};
RobotControlManager& robot_control_manager_;
rclcpp_action::Client<FollowJTA>::SharedPtr follow_jt_client_;
// Latest feedback cache for smooth stop on cancel (protected by mutex)
mutable std::mutex last_feedback_mutex_;
std::optional<FollowJTA::Feedback> last_feedback_;
// Trajectory saving switch
bool save_trajectory_enabled_{true};
};
} // namespace robot_control

43
src/robot_control/include/actions/move_home_action.hpp Normal file
View File

@@ -0,0 +1,43 @@
#pragma once
#include <atomic>
#include <memory>
#include "rclcpp_action/rclcpp_action.hpp"
#include "interfaces/action/move_home.hpp"
#include "actions/action_context.hpp"
namespace robot_control
{
class MoveHomeAction
{
public:
using MoveHome = interfaces::action::MoveHome;
using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
explicit MoveHomeAction(const ActionContext& ctx);
void initialize();
bool is_executing() const { return executing_.load(); }
void set_executing(bool v) { executing_.store(v); }
private:
rclcpp_action::GoalResponse handle_goal_(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveHome::Goal> goal);
rclcpp_action::CancelResponse handle_cancel_(
const std::shared_ptr<GoalHandleMoveHome> goal_handle);
void handle_accepted_(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
void execute_(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
ActionContext ctx_;
std::atomic<bool> executing_{false};
rclcpp_action::Server<MoveHome>::SharedPtr server_;
};
} // namespace robot_control

43
src/robot_control/include/actions/move_leg_action.hpp Normal file
View File

@@ -0,0 +1,43 @@
#pragma once
#include <atomic>
#include <memory>
#include "rclcpp_action/rclcpp_action.hpp"
#include "interfaces/action/move_leg.hpp"
#include "actions/action_context.hpp"
namespace robot_control
{
class MoveLegAction
{
public:
using MoveLeg = interfaces::action::MoveLeg;
using GoalHandleMoveLeg = rclcpp_action::ServerGoalHandle<MoveLeg>;
explicit MoveLegAction(const ActionContext& ctx);
void initialize();
bool is_executing() const { return executing_.load(); }
void set_executing(bool v) { executing_.store(v); }
private:
rclcpp_action::GoalResponse handle_goal_(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveLeg::Goal> goal);
rclcpp_action::CancelResponse handle_cancel_(
const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
void handle_accepted_(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
void execute_(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
ActionContext ctx_;
std::atomic<bool> executing_{false};
rclcpp_action::Server<MoveLeg>::SharedPtr server_;
};
} // namespace robot_control

43
src/robot_control/include/actions/move_waist_action.hpp Normal file
View File

@@ -0,0 +1,43 @@
#pragma once
#include <atomic>
#include <memory>
#include "rclcpp_action/rclcpp_action.hpp"
#include "interfaces/action/move_waist.hpp"
#include "actions/action_context.hpp"
namespace robot_control
{
class MoveWaistAction
{
public:
using MoveWaist = interfaces::action::MoveWaist;
using GoalHandleMoveWaist = rclcpp_action::ServerGoalHandle<MoveWaist>;
explicit MoveWaistAction(const ActionContext& ctx);
void initialize();
bool is_executing() const { return executing_.load(); }
void set_executing(bool v) { executing_.store(v); }
private:
rclcpp_action::GoalResponse handle_goal_(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveWaist::Goal> goal);
rclcpp_action::CancelResponse handle_cancel_(
const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
void handle_accepted_(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
void execute_(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
ActionContext ctx_;
std::atomic<bool> executing_{false};
rclcpp_action::Server<MoveWaist>::SharedPtr server_;
};
} // namespace robot_control

52
src/robot_control/include/actions/move_wheel_action.hpp Normal file
View File

@@ -0,0 +1,52 @@
#pragma once
#include <atomic>
#include <memory>
#include "rclcpp_action/rclcpp_action.hpp"
#include "interfaces/action/move_wheel.hpp"
#include "interfaces/msg/motor_cmd.hpp"
#include "interfaces/srv/motor_param.hpp"
#include "actions/action_context.hpp"
namespace robot_control
{
class MoveWheelAction
{
public:
using MoveWheel = interfaces::action::MoveWheel;
using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;
using MotorCmd = interfaces::msg::MotorCmd;
using MotorParam = interfaces::srv::MotorParam;
MoveWheelAction(
const ActionContext& ctx,
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub,
rclcpp::Client<MotorParam>::SharedPtr motor_param_client);
void initialize();
bool is_executing() const { return executing_.load(); }
void set_executing(bool v) { executing_.store(v); }
private:
rclcpp_action::GoalResponse handle_goal_(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveWheel::Goal> goal);
rclcpp_action::CancelResponse handle_cancel_(
const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
void handle_accepted_(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
void execute_(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
ActionContext ctx_;
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub_;
rclcpp::Client<MotorParam>::SharedPtr motor_param_client_;
std::atomic<bool> executing_{false};
rclcpp_action::Server<MoveWheel>::SharedPtr server_;
};
} // namespace robot_control

22
src/robot_control/include/controllers/arm_control.hpp
View File

@@ -14,6 +14,7 @@
#include <map>
#include "geometry_msgs/msg/pose.hpp"
#include "moveit/move_group_interface/move_group_interface.h"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
// 前向声明
namespace rclcpp {
@@ -80,6 +81,13 @@ namespace robot_control
double velocity_scaling,
double acceleration_scaling);
/**
* @brief 导出最近一次规划得到的 JointTrajectory（包含 time/pos/vel/acc）
* @param arm_id 0=左臂，1=右臂，2=双臂
* @param out 输出：规划轨迹
*/
bool ExportPlannedJointTrajectory(uint8_t arm_id, trajectory_msgs::msg::JointTrajectory& out) const;
/**
* @brief 规划笛卡尔空间运动（MOVEL）
*
@@ -223,17 +231,23 @@ namespace robot_control
};
TrajectoryState left_arm_trajectory_; ///< 左臂轨迹状态
TrajectoryState right_arm_trajectory_; ///< 右臂轨迹状态
TrajectoryState dual_arm_trajectory_; ///< 双臂轨迹状态（12关节）
// 同步保存为 JointTrajectory msg，便于 FollowJointTrajectory 一次性下发
trajectory_msgs::msg::JointTrajectory left_arm_traj_msg_;
trajectory_msgs::msg::JointTrajectory right_arm_traj_msg_;
trajectory_msgs::msg::JointTrajectory dual_arm_traj_msg_;
/**
* @brief 对 MoveIt 规划结果进行时间参数化 + 固定 dt 重采样，并写入对应臂的 TrajectoryState
* @brief 对 MoveIt 规划结果进行时间参数化，并写入对应臂的 TrajectoryState
* @param arm_id 手臂ID（0=左臂，1=右臂）
* @param move_group 对应 MoveGroupInterface
* @param plan MoveIt 规划结果（会被重采样后的轨迹覆盖）
* @param plan MoveIt 规划结果（会被写回带 time_from_start 的轨迹）
* @param velocity_scaling 速度缩放因子
* @param acceleration_scaling 加速度缩放因子
* @return true 成功
*/
bool timeParameterizeResampleAndStore_(
bool timeParameterizeAndStore_(
uint8_t arm_id,
moveit::planning_interface::MoveGroupInterface& move_group,
moveit::planning_interface::MoveGroupInterface::Plan& plan,
@@ -263,5 +277,7 @@ namespace robot_control
double alpha,
size_t num_joints,
const std::vector<double>& joint_positions);
public:
};
}

157
src/robot_control/include/core/robot_control_manager.hpp
View File

@@ -18,16 +18,14 @@
#include <vector>
#include <string>
#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 "sensor_msgs/msg/imu.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "core/motion_parameters.hpp"
#include "controllers/arm_control.hpp"
#include "controllers/leg_control.hpp"
#include "controllers/wheel_control.hpp"
#include "controllers/waist_control.hpp"
#include "utils/robot_model.hpp"
#include "core/common_enum.hpp"
#include "core/controller_factory.hpp"
@@ -63,47 +61,12 @@ namespace robot_control
// ==================== 运动控制接口 ====================
/**
* @brief 控制腿部运动
* @param dt 时间步长（秒）
* @return true 运动完成，false 运动进行中
*/
bool MoveLeg(double dt);
/**
* @brief 控制腰部运动
* @param dt 时间步长（秒）
* @return true 运动完成，false 运动进行中
*/
bool MoveWaist(double dt);
/**
* @brief 控制轮子运动
* @return true 运动完成，false 运动进行中
*/
bool MoveWheel();
/**
* @brief 停止所有运动
* @param dt 时间步长（秒）
* @return true 完全停止，false 正在停止
*/
bool Stop(double dt);
/**
* @brief 回零运动
* @param dt 时间步长（秒）
* @return true 回零完成，false 回零进行中
*/
bool GoHome(double dt);
/**
* @brief 点动控制指定轴
* @param axis 轴索引
* @param dir 方向（-1, 0, 1）
*/
void JogAxis(size_t axis, int dir);
/**
* @brief 设置轮子点动模式
* @param value true 启用点动，false 禁用点动
@@ -116,18 +79,6 @@ namespace robot_control
*/
bool GetJogWheel();
// ==================== 复位接口 ====================
/**
* @brief 重置轮子回零标志
*/
void WheelReset() { isWheelHomed_ = false; }
/**
* @brief 重置IMU初始化标志
*/
void ImuReset() { isGyroInited_ = false; }
// ==================== 参数设置接口 ====================
/**
@@ -152,14 +103,6 @@ namespace robot_control
* @return true 参数设置成功，false 参数设置失败
*/
bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle);
/**
* @brief 设置手臂关节命令（直接设置关节角度，通过topic发布）
* @param body_id 手臂ID（0=左臂，1=右臂）
* @param joint_angles 关节角度数组（弧度），大小为 arm_dof_
* @return true 设置成功，false 设置失败
*/
bool SetArmJointCommands(int8_t body_id, const std::vector<double>& joint_angles);
/**
* @brief 规划手臂运动（调用ArmControl的规划方法）
@@ -196,64 +139,43 @@ namespace robot_control
*/
bool InitializeArmMoveIt(rclcpp::Node* node);
/**
* @brief 获取当前插补点位（在主循环中调用）
* @param arm_id 手臂ID（0=左臂，1=右臂）
* @param dt 时间步长（秒）
* @param joint_positions 输出：当前插补点位（关节角度，弧度）
* @return true 还有更多点位，false 轨迹执行完成
*/
bool GetArmInterpolatedPoint(uint8_t arm_id, double dt, std::vector<double>& joint_positions);
/**
* @brief 检查是否有正在执行的轨迹
* @param arm_id 手臂ID（0=左臂，1=右臂）
* @return true 有轨迹在执行，false 没有轨迹
* @brief 规划双臂（或单臂保持另一臂）关节运动，生成一个 12关节 的 MoveIt 轨迹
* @param has_left 是否需要设置左臂目标
* @param left_target 左臂目标（size=arm_dof_，弧度）
* @param has_right 是否需要设置右臂目标
* @param right_target 右臂目标（size=arm_dof_，弧度）
*/
bool HasActiveArmTrajectory(uint8_t arm_id) const;
bool PlanDualArmJointMotion(
bool has_left,
const std::vector<double>& left_target,
bool has_right,
const std::vector<double>& right_target,
double velocity_scaling,
double acceleration_scaling);
/**
* @brief 获取该臂轨迹的末端关节位置（弧度）
* @brief 导出最近一次双臂规划轨迹（12关节，包含 time/pos/vel/acc）
*/
bool GetArmTrajectoryEndPositions(uint8_t arm_id, std::vector<double>& end_positions) const;
bool ExportDualArmPlannedTrajectory(trajectory_msgs::msg::JointTrajectory& out) const;
/**
* @brief 获取该臂轨迹的时间进度 [0,1]
*/
double GetArmTrajectoryProgress(uint8_t arm_id) const;
/**
* @brief 控制手臂运动（在主循环中调用）
* @param dt 时间步长（秒）
* @return true 运动完成，false 运动进行中
*/
bool MoveArm(double dt);
// ==================== 状态查询接口 ====================
/**
* @brief 获取关节命令
* @return 关节命令数组
* @brief 获取当前关节位置（内部顺序）
*/
std_msgs::msg::Float64MultiArray GetJointCommands();
const std::vector<double>& GetJointPositions() const;
/**
* @brief 获取关节反馈
* @return 关节反馈数组
* @brief 获取当前轮子位置（wheel joint order）
*/
std_msgs::msg::Float64MultiArray GetJointFeedback();
const std::vector<double>& GetWheelPositions() const;
/**
* @brief 获取轮子命令
* @return 轮子命令数组
* @brief 获取最近一次下发的轮子命令（wheel joint order）
*/
std_msgs::msg::Float64MultiArray GetWheelCommands();
/**
* @brief 获取轮子反馈
* @return 轮子反馈数组
*/
std_msgs::msg::Float64MultiArray GetWheelFeedback();
const std::vector<double>& GetWheelCommandPositions() const;
/**
* @brief 获取轮子速度比例
@@ -261,12 +183,6 @@ namespace robot_control
*/
double GetWheelRatio();
/**
* @brief 检查关节限位
* @param jointCommands 关节命令（会被修改）
*/
void CheckJointLimits(std_msgs::msg::Float64MultiArray& jointCommands);
/**
* @brief 更新关节状态（从JointState消息）
* @param msg 关节状态消息
@@ -286,16 +202,15 @@ namespace robot_control
void UpdateImuMsg(const ImuMsg::SharedPtr msg);
/**
* @brief 获取运动参数
* @return 运动参数结构
* @brief 获取运动参数（只读引用，避免拷贝）
*/
MotionParameters GetMotionParameters();
const MotionParameters& GetMotionParameters() const;
/**
* @brief 获取IMU差值
* @return IMU差值向量
* @return IMU yaw 差值（与内部 yaw 单位一致）
*/
std::vector<double> GetImuDifference();
double GetImuYawDifference() const;
/**
* @brief 检查指定部位是否在运动
@@ -316,16 +231,6 @@ namespace robot_control
*/
void init();
/**
* @brief 分配临时值（从关节命令中提取各控制器对应的值）
*/
void AssignTempValues();
/**
* @brief 更新关节命令（将各控制器的临时值合并到关节命令中）
*/
void UpdateJointCommands();
/**
* @brief 从关节名称列表获取索引列表（辅助函数）
* @param joint_names 关节名称列表
@@ -341,8 +246,6 @@ namespace robot_control
MotionParameters motionParams_; ///< 运动参数配置
bool isWaistHomed_; ///< 腰部是否已回零
bool isLegHomed_; ///< 腿部是否已回零
bool isWheelHomed_; ///< 轮子是否已回零
bool is_wheel_jog_; ///< 轮子点动模式标志
@@ -366,14 +269,11 @@ namespace robot_control
std::vector<double> wheelVelocities_; ///< 轮子速度（弧度/秒）
std::vector<double> jointEfforts_; ///< 关节力矩
// 关节命令和状态
std_msgs::msg::Float64MultiArray jointCommands_; ///< 关节命令
std_msgs::msg::Float64MultiArray wheelCommands_; ///< 轮子命令
// 状态
std::vector<double> wheelCommands_; ///< 轮子命令（wheel joint order）
sensor_msgs::msg::JointState jointStates_; ///< 关节状态
// 临时变量（用于在各控制器之间传递数据）
std::vector<double> tempWaistCmd_; ///< 腰部临时命令
std::vector<double> tempLegCmd_; ///< 腿部临时命令
// 临时变量（用于轮子控制器）
std::vector<double> tempWheelCmd_; ///< 轮子临时命令
// IMU相关
@@ -388,7 +288,6 @@ namespace robot_control
interfaces::msg::MotorPos motorPos_; ///< 电机位置消息
// 初始化相关
std::vector<bool> jointInited_; ///< 各关节是否已初始化
bool isJointInitValueSet_; ///< 关节初始化值是否已设置
bool joint_name_mapping_initialized_; ///< 关节名称映射是否已初始化
};

33
src/robot_control/include/core/robot_control_node.hpp
View File

@@ -13,13 +13,10 @@
#pragma once
#include <filesystem>
#include <fstream>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "std_msgs/msg/string.hpp"
#include "core/robot_control_manager.hpp"
#include "core/remote_controller.hpp"
@@ -33,8 +30,6 @@
#include "control_msgs/msg/dynamic_interface_group_values.hpp"
#include <controller_manager_msgs/srv/switch_controller.hpp>
#define RECORD_FLAG 1 ///< 数据记录标志（0=禁用，1=启用）
using MoveHome = interfaces::action::MoveHome;
using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
@@ -78,20 +73,9 @@ namespace robot_control
*/
~RobotControlNode();
/**
* @brief 控制循环（定时器回调）
*
* 在控制循环中：
* - 检查并处理停止请求
* - 执行正在进行的动作（MoveHome, MoveLeg, MoveWaist, MoveWheel）
* - 发布关节轨迹命令
*/
void ControlLoop();
private:
// ==================== ROS 2 通信接口 ====================
rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr jointTrajectoryPub_; ///< 关节轨迹发布器
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr gpioPub_; ///< GPIO发布器
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joyCommandSub_; ///< 手柄命令订阅器
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_; ///< 关节状态订阅器
@@ -107,25 +91,10 @@ namespace robot_control
std::unique_ptr<ActionManager> action_manager_; ///< 动作管理器
std::unique_ptr<RemoteController> remoteController_; ///< 遥控器控制器
// ==================== 数据记录 ====================
std::ofstream data_file_; ///< 数据文件流（用于记录关节数据）
std::string data_file_path_; ///< 数据文件路径
// ==================== 定时器和时间 ====================
rclcpp::TimerBase::SharedPtr controlTimer_; ///< 控制循环定时器
rclcpp::Time lastTime_; ///< 上次控制循环时间
// ==================== 控制状态 ====================
bool is_robot_enabled_; ///< 机器人是否启用
// ==================== 回调函数 ====================
/**
* @brief 发布关节轨迹
*/
void Publish_joint_trajectory();
/**
* @brief 关节状态回调函数

10
src/robot_control/scripts/dual_arm_action_client.py
View File

@@ -223,14 +223,14 @@ def example_left_and_right_arm_joint_motion():
arm_id = ArmMotionParams.ARM_LEFT
# 6关节 home（rad）
home = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
home = [0.0, 1.0, 0.0, 0.0, 0.0, 0.0]
home2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
amplitude = 1.0 # 单关节摆动幅度（rad）
cycles = 1 # 每个关节 up/down 循环次数
velocity_scaling = 0.5
acceleration_scaling = 0.5
cycles = 3 # 每个关节 up/down 循环次数
velocity_scaling = 0.8
acceleration_scaling = 0.8
result_timeout_sec = 30.0
sleep_between_sec = 5.0
sleep_between_sec = 2.0
dof = 6

802
src/robot_control/src/actions/action_manager.cpp
View File

@@ -1,784 +1,58 @@
/**
* @file action_manager.cpp
* @brief Action管理器实现
* @author Ray
* @date 2025
* @brief Thin orchestrator that wires per-action handlers.
*/
#include "actions/action_manager.hpp"
#include "actions/action_context.hpp"
#include "actions/move_home_action.hpp"
#include "actions/move_leg_action.hpp"
#include "actions/move_waist_action.hpp"
#include "actions/move_wheel_action.hpp"
#include "actions/dual_arm_action.hpp"
#include "core/robot_control_manager.hpp"
#include "core/common_enum.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include <thread>
#include <cmath>
#include <chrono>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
// DualArm action - 使用 DualArm.action 中定义的常量
using namespace robot_control;
namespace robot_control
{
ActionManager::ActionManager(
rclcpp::Node* node,
RobotControlManager& robot_control_manager,
std::function<bool()> is_jog_mode_func,
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub,
rclcpp::Client<MotorParam>::SharedPtr motor_param_client)
: node_(node)
, robot_control_manager_(robot_control_manager)
, is_jog_mode_func_(is_jog_mode_func)
, motor_cmd_pub_(motor_cmd_pub)
, motor_param_client_(motor_param_client)
, move_home_executing_(false)
, move_leg_executing_(false)
, move_waist_executing_(false)
, move_wheel_executing_(false)
, dual_arm_executing_(false)
rclcpp::Node* node,
RobotControlManager& robot_control_manager,
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub,
rclcpp::Client<MotorParam>::SharedPtr motor_param_client)
: node_(node)
, robot_control_manager_(robot_control_manager)
, motor_cmd_pub_(std::move(motor_cmd_pub))
, motor_param_client_(std::move(motor_param_client))
{
}
ActionManager::~ActionManager()
{
}
ActionManager::~ActionManager() = default;
void ActionManager::initialize()
{
using namespace std::placeholders;
// Shared FollowJointTrajectory client to ros2_control trajectory controller (driver layer)
follow_jt_client_ = rclcpp_action::create_client<FollowJTA>(
node_, "/trajectory_controller/follow_joint_trajectory");
// 创建 MoveHome Action 服务器
move_home_action_server_ = rclcpp_action::create_server<MoveHome>(
node_,
"MoveHome",
std::bind(&ActionManager::handle_move_home_goal, this, _1, _2),
std::bind(&ActionManager::handle_move_home_cancel, this, _1),
std::bind(&ActionManager::handle_move_home_accepted, this, _1));
RCLCPP_INFO(node_->get_logger(), "MoveHome action server is ready");
ActionContext ctx;
ctx.node = node_;
ctx.robot_control_manager = &robot_control_manager_;
ctx.follow_jt_client = follow_jt_client_;
// 创建 MoveLeg Action 服务器
move_leg_action_server_ = rclcpp_action::create_server<MoveLeg>(
node_,
"MoveLeg",
std::bind(&ActionManager::handle_move_leg_goal, this, _1, _2),
std::bind(&ActionManager::handle_move_leg_cancel, this, _1),
std::bind(&ActionManager::handle_move_leg_accepted, this, _1));
RCLCPP_INFO(node_->get_logger(), "MoveLeg action server is ready");
move_home_ = std::make_unique<MoveHomeAction>(ctx);
move_leg_ = std::make_unique<MoveLegAction>(ctx);
move_waist_ = std::make_unique<MoveWaistAction>(ctx);
move_wheel_ = std::make_unique<MoveWheelAction>(ctx, motor_cmd_pub_, motor_param_client_);
dual_arm_ = std::make_unique<DualArmAction>(ctx);
// 创建 MoveWaist Action 服务器
move_waist_action_server_ = rclcpp_action::create_server<MoveWaist>(
node_,
"MoveWaist",
std::bind(&ActionManager::handle_move_waist_goal, this, _1, _2),
std::bind(&ActionManager::handle_move_waist_cancel, this, _1),
std::bind(&ActionManager::handle_move_waist_accepted, this, _1));
RCLCPP_INFO(node_->get_logger(), "MoveWaist action server is ready");
// 创建 MoveWheel Action 服务器
move_wheel_action_server_ = rclcpp_action::create_server<MoveWheel>(
node_,
"MoveWheel",
std::bind(&ActionManager::handle_move_wheel_goal, this, _1, _2),
std::bind(&ActionManager::handle_move_wheel_cancel, this, _1),
std::bind(&ActionManager::handle_move_wheel_accepted, this, _1));
RCLCPP_INFO(node_->get_logger(), "MoveWheel action server is ready");
// 创建 DualArm Action 服务器
dual_arm_action_server_ = rclcpp_action::create_server<interfaces::action::DualArm>(
node_,
"DualArm",
std::bind(&ActionManager::handle_dual_arm_goal, this, _1, _2),
std::bind(&ActionManager::handle_dual_arm_cancel, this, _1),
std::bind(&ActionManager::handle_dual_arm_accepted, this, _1));
RCLCPP_INFO(node_->get_logger(), "DualArm action server is ready");
}
// MoveHome Action 处理函数
rclcpp_action::GoalResponse ActionManager::handle_move_home_goal(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveHome::Goal> goal)
{
(void)uuid;
(void)goal;
if (robot_control_manager_.IsMoving(MovementPart::ALL))
{
RCLCPP_ERROR(node_->get_logger(), "Robot is moving");
return rclcpp_action::GoalResponse::REJECT;
}
if (move_home_executing_.load())
{
RCLCPP_ERROR(node_->get_logger(), "Another move home goal is executing");
return rclcpp_action::GoalResponse::REJECT;
}
if (is_jog_mode_func_())
{
RCLCPP_ERROR(node_->get_logger(), "Jog mode is enabled");
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse ActionManager::handle_move_home_cancel(
const std::shared_ptr<GoalHandleMoveHome> goal_handle)
{
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void ActionManager::handle_move_home_accepted(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
{
move_home_executing_.store(true);
using namespace std::placeholders;
std::thread{std::bind(&ActionManager::move_home_execute, this, _1), goal_handle}.detach();
}
void ActionManager::move_home_execute(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
{
rclcpp::Rate loop_rate(10);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<MoveHome::Feedback>();
auto result = std::make_shared<MoveHome::Result>();
while (move_home_executing_.load() && rclcpp::ok())
{
if (goal_handle->is_canceling())
{
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(node_->get_logger(), "Move home canceled");
move_home_executing_.store(false);
return;
}
auto joint_commands = robot_control_manager_.GetJointFeedback();
feedback->joint_values.clear();
for (double val : joint_commands.data)
{
feedback->joint_values.push_back(static_cast<int64_t>(val));
}
goal_handle->publish_feedback(feedback);
loop_rate.sleep();
}
if (rclcpp::ok())
{
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(node_->get_logger(), "move home succeeded.");
}
}
// MoveLeg Action 处理函数
rclcpp_action::GoalResponse ActionManager::handle_move_leg_goal(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveLeg::Goal> goal)
{
(void)uuid;
if (robot_control_manager_.IsMoving(MovementPart::LEG))
{
RCLCPP_ERROR(node_->get_logger(), "Robot leg is moving");
return rclcpp_action::GoalResponse::REJECT;
}
if (move_leg_executing_.load())
{
RCLCPP_ERROR(node_->get_logger(), "Another move leg goal is executing");
return rclcpp_action::GoalResponse::REJECT;
}
if (is_jog_mode_func_())
{
RCLCPP_ERROR(node_->get_logger(), "Jog mode is enabled");
return rclcpp_action::GoalResponse::REJECT;
}
if (!robot_control_manager_.SetMoveLegParameters(goal->move_up_distance))
{
RCLCPP_ERROR(node_->get_logger(), "Failed to set move leg parameters");
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse ActionManager::handle_move_leg_cancel(
const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
{
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void ActionManager::handle_move_leg_accepted(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
{
move_leg_executing_.store(true);
using namespace std::placeholders;
std::thread{std::bind(&ActionManager::move_leg_execute, this, _1), goal_handle}.detach();
}
void ActionManager::move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
{
rclcpp::Rate loop_rate(10);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<MoveLeg::Feedback>();
auto result = std::make_shared<MoveLeg::Result>();
while (move_leg_executing_.load() && rclcpp::ok())
{
if (goal_handle->is_canceling())
{
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(node_->get_logger(), "move leg canceled");
move_leg_executing_.store(false);
return;
}
auto joint_commands = robot_control_manager_.GetJointFeedback();
feedback->joint_values.clear();
for (double val : joint_commands.data)
{
feedback->joint_values.push_back(static_cast<int64_t>(val));
}
goal_handle->publish_feedback(feedback);
loop_rate.sleep();
}
if (rclcpp::ok())
{
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(node_->get_logger(), "move leg succeeded.");
}
}
// MoveWaist Action 处理函数
rclcpp_action::GoalResponse ActionManager::handle_move_waist_goal(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveWaist::Goal> goal)
{
(void)uuid;
if (robot_control_manager_.IsMoving(MovementPart::WAIST))
{
RCLCPP_ERROR(node_->get_logger(), "Robot waist is moving");
return rclcpp_action::GoalResponse::REJECT;
}
if (move_waist_executing_.load())
{
RCLCPP_ERROR(node_->get_logger(), "Another move waist goal is executing");
return rclcpp_action::GoalResponse::REJECT;
}
if (is_jog_mode_func_())
{
RCLCPP_ERROR(node_->get_logger(), "Jog mode is enabled");
return rclcpp_action::GoalResponse::REJECT;
}
if (!robot_control_manager_.SetMoveWaistParameters(goal->move_pitch_degree, goal->move_yaw_degree))
{
RCLCPP_ERROR(node_->get_logger(), "Invalid move waist request");
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse ActionManager::handle_move_waist_cancel(
const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
{
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void ActionManager::handle_move_waist_accepted(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
{
move_waist_executing_.store(true);
using namespace std::placeholders;
std::thread{std::bind(&ActionManager::move_waist_execute, this, _1), goal_handle}.detach();
}
void ActionManager::move_waist_execute(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
{
rclcpp::Rate loop_rate(10);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<MoveWaist::Feedback>();
auto result = std::make_shared<MoveWaist::Result>();
while (move_waist_executing_.load() && rclcpp::ok())
{
if (goal_handle->is_canceling())
{
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(node_->get_logger(), "move waist canceled");
move_waist_executing_.store(false);
return;
}
auto joint_commands = robot_control_manager_.GetJointFeedback();
feedback->joint_values.clear();
for (double val : joint_commands.data)
{
feedback->joint_values.push_back(static_cast<int64_t>(val));
}
goal_handle->publish_feedback(feedback);
loop_rate.sleep();
}
if (rclcpp::ok())
{
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(node_->get_logger(), "move waist succeeded.");
}
}
// MoveWheel Action 处理函数
rclcpp_action::GoalResponse ActionManager::handle_move_wheel_goal(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveWheel::Goal> goal)
{
(void)uuid;
if (robot_control_manager_.IsMoving(MovementPart::WHEEL))
{
RCLCPP_ERROR(node_->get_logger(), "Robot wheel is moving");
return rclcpp_action::GoalResponse::REJECT;
}
if (move_wheel_executing_.load())
{
RCLCPP_ERROR(node_->get_logger(), "Another move wheel goal is executing");
return rclcpp_action::GoalResponse::REJECT;
}
if (goal->move_distance > 2.1 || goal->move_distance < -1.0 || goal->move_angle < -10 || goal->move_angle > 10)
{
RCLCPP_ERROR(node_->get_logger(), "exceed limit");
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse ActionManager::handle_move_wheel_cancel(
const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
{
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void ActionManager::handle_move_wheel_accepted(const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
{
move_wheel_executing_.store(true);
RCLCPP_INFO(node_->get_logger(), "Goal accepted");
using namespace std::placeholders;
std::thread{std::bind(&ActionManager::move_wheel_execute, this, _1), goal_handle}.detach();
}
void ActionManager::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
{
// 注意：这里需要从原始的 robot_control_node.cpp 中的 move_wheel_execute 函数复制完整实现
// 由于代码较长且涉及 motorCmdPub_ 和 motorParamClient_，这里提供框架
// 完整实现需要从原文件中迁移
rclcpp::Rate loop_rate(10);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<MoveWheel::Feedback>();
auto result = std::make_shared<MoveWheel::Result>();
double wheelAngle = 0;
if (abs(goal->move_angle) > 0)
{
robot_control_manager_.SetJogWheel(true);
wheelAngle = goal->move_angle;
}
if (abs(goal->move_angle) == 0 && abs(goal->move_distance) == 0)
{
robot_control_manager_.SetJogWheel(false);
}
if (!robot_control_manager_.GetJogWheel())
{
double tempValue = robot_control_manager_.GetImuDifference()[2];
wheelAngle = abs(tempValue) > 40.0 ? 0.0 : tempValue;
}
robot_control_manager_.SetMoveWheelParameters(goal->move_distance, wheelAngle);
double ratio = robot_control_manager_.GetWheelRatio();
auto request = std::make_shared<MotorParam::Request>();
if ((goal->move_distance > 0.1) && !robot_control_manager_.GetJogWheel())
{
request->motor_id = 1;
request->max_speed = static_cast<uint16_t>(round((ratio) * 51));
request->add_acc = 8;
request->dec_acc = 8;
motor_param_client_->async_send_request(request);
std::cout << "set motor 1 max speed : " << request->max_speed << std::endl;
rclcpp::sleep_for(std::chrono::milliseconds(50));
}
robot_control_manager_.MoveWheel();
std_msgs::msg::Float64MultiArray wheel_commands = robot_control_manager_.GetWheelCommands();
MotorCmd wheel_commands_msg;
wheel_commands_msg.target = "rs485";
wheel_commands_msg.type = "bm";
wheel_commands_msg.position = "";
wheel_commands_msg.motor_id = {1, 2};
wheel_commands_msg.motor_angle = {(float)(wheel_commands.data[0]), (float)(wheel_commands.data[1])};
motor_cmd_pub_->publish(wheel_commands_msg);
while (move_wheel_executing_.load() && rclcpp::ok())
{
if (goal_handle->is_canceling())
{
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(node_->get_logger(), "move wheel canceled");
move_wheel_executing_.store(false);
return;
}
auto joint_feedback = robot_control_manager_.GetWheelFeedback();
if (abs(joint_feedback.data[0] - wheel_commands.data[0]) < 20.0 &&
abs(joint_feedback.data[1] - wheel_commands.data[1]) < 20.0)
{
move_wheel_executing_.store(false);
}
feedback->current_pos = joint_feedback.data[0];
goal_handle->publish_feedback(feedback);
loop_rate.sleep();
}
if ((goal->move_distance > 0.0) && !robot_control_manager_.GetJogWheel())
{
request->motor_id = 1;
request->max_speed = 51;
request->add_acc = 8;
request->dec_acc = 8;
motor_param_client_->async_send_request(request);
}
if (rclcpp::ok())
{
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(node_->get_logger(), "move wheel succeeded.");
}
}
// DualArm Action 处理函数
rclcpp_action::GoalResponse ActionManager::handle_dual_arm_goal(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const DualArm::Goal> goal)
{
(void)uuid;
RCLCPP_INFO(node_->get_logger(),
"Received DualArm goal: arm_type=%d, velocity_scaling=%.2f, acceleration_scaling=%.2f, arm_motion_params_count=%zu",
goal->arm_type, goal->velocity_scaling, goal->acceleration_scaling, goal->arm_motion_params.size());
if (robot_control_manager_.IsMoving(MovementPart::ALL))
{
RCLCPP_ERROR(node_->get_logger(), "Robot is moving");
return rclcpp_action::GoalResponse::REJECT;
}
if (dual_arm_executing_.load())
{
RCLCPP_ERROR(node_->get_logger(), "Another dual arm goal is executing");
return rclcpp_action::GoalResponse::REJECT;
}
if (is_jog_mode_func_())
{
RCLCPP_ERROR(node_->get_logger(), "Jog mode is enabled");
return rclcpp_action::GoalResponse::REJECT;
}
// 在规划之前检查 MOVEJ 的关节目标是否超过限位（单位：rad）
for (const auto& arm_param : goal->arm_motion_params) {
if (arm_param.motion_type == arm_param.MOVEJ) {
std::string err;
if (!robot_control_manager_.ValidateArmJointTarget(
arm_param.arm_id, arm_param.target_joints, &err))
{
RCLCPP_ERROR(node_->get_logger(),
"DualArm goal rejected: MOVEJ target_joints exceed limits for arm_id %d: %s",
arm_param.arm_id, err.c_str());
return rclcpp_action::GoalResponse::REJECT;
}
}
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse ActionManager::handle_dual_arm_cancel(
const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
(void)goal_handle;
RCLCPP_INFO(node_->get_logger(), "Received request to cancel DualArm goal");
return rclcpp_action::CancelResponse::ACCEPT;
}
void ActionManager::handle_dual_arm_accepted(const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
using namespace std::placeholders;
std::thread{std::bind(&ActionManager::dual_arm_execute, this, _1), goal_handle}.detach();
}
void ActionManager::dual_arm_execute(const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
rclcpp::Rate loop_rate(10);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<DualArm::Feedback>();
auto result = std::make_shared<DualArm::Result>();
result->success = false;
result->message = "DualArm action failed";
const MotionParameters& motionParams = robot_control_manager_.GetMotionParameters();
const size_t ARM_DOF = motionParams.arm_dof_; // 每条手臂的自由度数
try {
// 发送规划阶段反馈
feedback->status = 0; // STATUS_PLANNING = 0
feedback->progress = 0.0;
feedback->joints_left.clear();
feedback->joints_right.clear();
goal_handle->publish_feedback(feedback);
// 使用RobotControlManager的统一规划接口
for (const auto& arm_param : goal->arm_motion_params) {
// 检查参数有效性
if (arm_param.motion_type == arm_param.MOVEJ) {
if (arm_param.target_joints.empty()) {
RCLCPP_ERROR(node_->get_logger(), "MOVEJ requested but target_joints is empty for arm_id %d", arm_param.arm_id);
result->message = "MOVEJ target_joints empty";
goal_handle->abort(result);
dual_arm_executing_.store(false);
return;
}
if (arm_param.target_joints.size() != ARM_DOF) {
RCLCPP_ERROR(node_->get_logger(), "MOVEJ requested but target_joints size (%zu) does not match arm_dof (%zu) for arm_id %d",
arm_param.target_joints.size(), ARM_DOF, arm_param.arm_id);
result->message = "MOVEJ target_joints size mismatch";
goal_handle->abort(result);
dual_arm_executing_.store(false);
return;
}
// 再次校验关节限位（防止执行阶段被直接调用/绕过 goal 阶段）
std::string err;
if (!robot_control_manager_.ValidateArmJointTarget(
arm_param.arm_id, arm_param.target_joints, &err))
{
RCLCPP_ERROR(node_->get_logger(),
"MOVEJ target_joints exceed limits for arm_id %d: %s",
arm_param.arm_id, err.c_str());
result->message = std::string("MOVEJ target_joints exceed limits: ") + err;
goal_handle->abort(result);
dual_arm_executing_.store(false);
return;
}
}
std::cout << "Plan Arm Motion" << std::endl;
std::cout << "arm_param.arm_id: " << arm_param.arm_id << std::endl;
std::cout << "arm_param.motion_type: " << arm_param.motion_type << std::endl;
std::cout << "goal->velocity_scaling: " << goal->velocity_scaling << std::endl;
std::cout << "goal->acceleration_scaling: " << goal->acceleration_scaling << std::endl;
std::cout << "arm_param.target_joints: " << arm_param.target_joints.size() << std::endl;
std::cout << "arm_param.target_joints: " << arm_param.target_joints[0] << std::endl;
std::cout << "arm_param.target_joints: " << arm_param.target_joints[1] << std::endl;
std::cout << "arm_param.target_joints: " << arm_param.target_joints[2] << std::endl;
std::cout << "arm_param.target_joints: " << arm_param.target_joints[3] << std::endl;
std::cout << "arm_param.target_joints: " << arm_param.target_joints[4] << std::endl;
std::cout << "arm_param.target_joints: " << arm_param.target_joints[5] << std::endl;
// 调用RobotControlManager的规划接口（统一管理）
if (robot_control_manager_.PlanArmMotion(
arm_param,
goal->velocity_scaling,
goal->acceleration_scaling)) {
RCLCPP_INFO(node_->get_logger(),
"Arm motion planning successful for arm_id %d, motion_type %d",
arm_param.arm_id, arm_param.motion_type);
result->success = true;
result->message = "DualArm planning succeeded";
} else {
RCLCPP_ERROR(node_->get_logger(),
"Arm motion planning failed for arm_id %d, motion_type %d",
arm_param.arm_id, arm_param.motion_type);
result->success = false;
result->message = "Arm motion planning failed";
goal_handle->abort(result);
dual_arm_executing_.store(false);
return;
}
}
} catch (const std::exception& e) {
RCLCPP_ERROR(node_->get_logger(), "Exception during DualArm execution: %s", e.what());
result->success = false;
result->message = std::string("Exception: ") + e.what();
goal_handle->abort(result);
dual_arm_executing_.store(false);
return;
}
// 执行阶段：循环发送反馈，直到轨迹执行完成
feedback->status = 1; // STATUS_EXECUTING = 1
dual_arm_executing_.store(true);
// 哪些手臂需要达成目标（根据本次 goal 的 arm_motion_params 判断）
bool need_left = false;
bool need_right = false;
for (const auto& arm_param : goal->arm_motion_params) {
if (arm_param.arm_id == 0) need_left = true;
if (arm_param.arm_id == 1) need_right = true;
}
const double kJointTolRad = 0.002; // 目标到达阈值（弧度）
const rclcpp::Time exec_start_time = node_->now();
const double kMaxWaitSec = 15.0; // 安全超时，防止永不结束
while (dual_arm_executing_.load() && rclcpp::ok())
{
// 检查取消请求
if (goal_handle->is_canceling()) {
result->success = false;
result->message = "DualArm goal canceled";
goal_handle->canceled(result);
RCLCPP_INFO(node_->get_logger(), "DualArm goal canceled");
dual_arm_executing_.store(false);
return;
}
// 检查是否有轨迹在执行
bool left_arm_active = robot_control_manager_.HasActiveArmTrajectory(0);
bool right_arm_active = robot_control_manager_.HasActiveArmTrajectory(1);
// 基于时间的进度：读取 ArmControl 内部 elapsed_time / total_time
double p_sum = 0.0;
int p_cnt = 0;
if (need_left) { p_sum += robot_control_manager_.GetArmTrajectoryProgress(0); ++p_cnt; }
if (need_right){ p_sum += robot_control_manager_.GetArmTrajectoryProgress(1); ++p_cnt; }
feedback->progress = (p_cnt > 0) ? (p_sum / static_cast<double>(p_cnt)) : 1.0;
// 获取关节反馈并提取左右臂关节（单位：弧度）
auto joint_feedback = robot_control_manager_.GetJointFeedback();
feedback->joints_left.clear();
feedback->joints_right.clear();
// 提取左臂关节（dual_arm_joint_names_ 的前6个）
if (motionParams.dual_arm_joint_names_.size() >= ARM_DOF) {
for (size_t i = 0; i < ARM_DOF; ++i) {
const std::string& joint_name = motionParams.dual_arm_joint_names_[i];
auto it = motionParams.joint_name_to_index_.find(joint_name);
if (it != motionParams.joint_name_to_index_.end() && it->second < joint_feedback.data.size()) {
double angle_rad = joint_feedback.data[it->second];
feedback->joints_left.push_back(angle_rad);
} else {
feedback->joints_left.push_back(0.0);
}
}
}
// 提取右臂关节（dual_arm_joint_names_ 的后6个）
if (motionParams.dual_arm_joint_names_.size() >= 2 * ARM_DOF) {
for (size_t i = ARM_DOF; i < 2 * ARM_DOF; ++i) {
const std::string& joint_name = motionParams.dual_arm_joint_names_[i];
auto it = motionParams.joint_name_to_index_.find(joint_name);
if (it != motionParams.joint_name_to_index_.end() && it->second < joint_feedback.data.size()) {
double angle_rad = joint_feedback.data[it->second];
feedback->joints_right.push_back(angle_rad);
} else {
feedback->joints_right.push_back(0.0);
}
}
}
// 发布反馈
goal_handle->publish_feedback(feedback);
// 判断是否“接近目标值才结束”
auto all_close = [&](uint8_t arm_id, const std::vector<double>& current, double tol) -> bool {
std::vector<double> target;
if (!robot_control_manager_.GetArmTrajectoryEndPositions(arm_id, target)) {
return true; // 没有轨迹，认为不需要判断
}
if (current.size() != target.size()) {
return false;
}
for (size_t i = 0; i < current.size(); ++i) {
if (std::abs(current[i] - target[i]) > tol) return false;
}
return true;
};
const bool left_close = (!need_left) ? true : all_close(0, feedback->joints_left, kJointTolRad);
const bool right_close = (!need_right) ? true : all_close(1, feedback->joints_right, kJointTolRad);
// 安全超时：避免永远等不到反馈到位
if ((node_->now() - exec_start_time).seconds() > kMaxWaitSec) {
RCLCPP_ERROR(node_->get_logger(), "DualArm execute timeout (%.1fs), aborting", kMaxWaitSec);
result->success = false;
result->message = "DualArm execute timeout";
goal_handle->abort(result);
dual_arm_executing_.store(false);
return;
}
// 当轨迹已结束（不再 active）且反馈已接近目标，才结束
const bool traj_done = (!left_arm_active && !right_arm_active);
if (traj_done && left_close && right_close) {
feedback->progress = 1.0;
feedback->status = 2; // STATUS_DONE = 2
goal_handle->publish_feedback(feedback);
std::cout << "轨迹执行完成且到位 , action internal finished" << std::endl;
break;
}
loop_rate.sleep();
}
// 检查取消请求
if (goal_handle->is_canceling()) {
result->success = false;
result->message = "DualArm goal canceled";
goal_handle->canceled(result);
RCLCPP_INFO(node_->get_logger(), "DualArm goal canceled");
dual_arm_executing_.store(false);
return;
}
if (rclcpp::ok()) {
result->final_progress = 1.0;
goal_handle->succeed(result);
RCLCPP_INFO(node_->get_logger(), "DualArm goal %s.", result->success ? "succeeded" : "failed");
}
dual_arm_executing_.store(false);
move_home_->initialize();
move_leg_->initialize();
move_waist_->initialize();
move_wheel_->initialize();
dual_arm_->initialize();
}
} // namespace robot_control

539
src/robot_control/src/actions/dual_arm_action.cpp Normal file
View File

@@ -0,0 +1,539 @@
#include "actions/dual_arm_action.hpp"
#include <thread>
#include <unordered_map>
#include "core/robot_control_manager.hpp"
#include "core/common_enum.hpp"
namespace robot_control
{
DualArmAction::DualArmAction(const ActionContext& ctx)
: ctx_(ctx)
{
}
void DualArmAction::initialize()
{
using namespace std::placeholders;
// FJT executor (shared client created by ActionManager)
if (ctx_.node && ctx_.robot_control_manager && ctx_.follow_jt_client) {
follow_executor_ = std::make_unique<FollowJTExecutor>(
ctx_.node, *ctx_.robot_control_manager, ctx_.follow_jt_client);
}
server_ = rclcpp_action::create_server<DualArm>(
ctx_.node,
"DualArm",
std::bind(&DualArmAction::handle_goal_, this, _1, _2),
std::bind(&DualArmAction::handle_cancel_, this, _1),
std::bind(&DualArmAction::handle_accepted_, this, _1));
RCLCPP_INFO(ctx_.node->get_logger(), "DualArm action server is ready");
}
rclcpp_action::GoalResponse DualArmAction::handle_goal_(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const DualArm::Goal> goal)
{
(void)uuid;
if (!ctx_.robot_control_manager) return rclcpp_action::GoalResponse::REJECT;
RCLCPP_INFO(ctx_.node->get_logger(),
"Received DualArm goal: arm_type=%d, velocity_scaling=%.2f, acceleration_scaling=%.2f, arm_motion_params_count=%zu",
goal->arm_type, goal->velocity_scaling, goal->acceleration_scaling, goal->arm_motion_params.size());
if (ctx_.robot_control_manager->IsMoving(MovementPart::ALL)) {
RCLCPP_ERROR(ctx_.node->get_logger(), "Robot is moving");
return rclcpp_action::GoalResponse::REJECT;
}
if (executing_.load()) {
RCLCPP_ERROR(ctx_.node->get_logger(), "Another dual arm goal is executing");
return rclcpp_action::GoalResponse::REJECT;
}
// MOVEJ-only for now (dual_arm group planning + single FJT)
for (const auto& arm_param : goal->arm_motion_params) {
if (arm_param.motion_type != arm_param.MOVEJ) {
RCLCPP_ERROR(ctx_.node->get_logger(),
"DualArm goal rejected: only MOVEJ is supported for now (got motion_type=%d for arm_id=%d)",
arm_param.motion_type, arm_param.arm_id);
return rclcpp_action::GoalResponse::REJECT;
}
std::string err;
if (!ctx_.robot_control_manager->ValidateArmJointTarget(arm_param.arm_id, arm_param.target_joints, &err)) {
RCLCPP_ERROR(ctx_.node->get_logger(),
"DualArm goal rejected: MOVEJ target_joints exceed limits for arm_id %d: %s",
arm_param.arm_id, err.c_str());
return rclcpp_action::GoalResponse::REJECT;
}
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse DualArmAction::handle_cancel_(
const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
(void)goal_handle;
RCLCPP_INFO(ctx_.node->get_logger(), "Received request to cancel DualArm goal");
return rclcpp_action::CancelResponse::ACCEPT;
}
void DualArmAction::handle_accepted_(const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
using namespace std::placeholders;
std::thread{std::bind(&DualArmAction::execute_, this, _1), goal_handle}.detach();
}
bool DualArmAction::collect_movej_targets_(
const DualArm::Goal& goal,
size_t arm_dof,
bool* need_left,
std::vector<double>* left_target,
bool* need_right,
std::vector<double>* right_target,
std::string* error_msg) const
{
if (!need_left || !left_target || !need_right || !right_target) {
if (error_msg) *error_msg = "Null output pointer";
return false;
}
*need_left = false;
*need_right = false;
left_target->clear();
right_target->clear();
for (const auto& arm_param : goal.arm_motion_params) {
if (arm_param.motion_type != arm_param.MOVEJ) {
if (error_msg) *error_msg = "Only MOVEJ is supported for DualArm";
return false;
}
if (arm_param.target_joints.size() != arm_dof) {
if (error_msg) {
*error_msg =
"target_joints size mismatch for arm_id " + std::to_string(arm_param.arm_id) +
" (expected " + std::to_string(arm_dof) +
", got " + std::to_string(arm_param.target_joints.size()) + ")";
}
return false;
}
if (arm_param.arm_id == 0) {
*need_left = true;
*left_target = arm_param.target_joints;
} else if (arm_param.arm_id == 1) {
*need_right = true;
*right_target = arm_param.target_joints;
} else {
if (error_msg) *error_msg = "Invalid arm_id (expected 0=left, 1=right)";
return false;
}
}
if (!(*need_left) && !(*need_right)) {
if (error_msg) *error_msg = "No arm_motion_params provided";
return false;
}
return true;
}
bool DualArmAction::export_and_normalize_trajectory_(
const MotionParameters& motion_params,
trajectory_msgs::msg::JointTrajectory* out,
double* total_time_sec,
std::string* error_msg) const
{
if (!out || !total_time_sec) {
if (error_msg) *error_msg = "Null output pointer";
return false;
}
*total_time_sec = 0.0;
out->points.clear();
out->joint_names.clear();
out->header.stamp.sec = 0;
out->header.stamp.nanosec = 0;
trajectory_msgs::msg::JointTrajectory raw;
if (!ctx_.robot_control_manager->ExportDualArmPlannedTrajectory(raw)) {
if (error_msg) *error_msg = "ExportDualArmPlannedTrajectory failed";
return false;
}
if (raw.joint_names.empty() || raw.points.empty()) {
if (error_msg) *error_msg = "Raw trajectory is empty";
return false;
}
out->joint_names = motion_params.dual_arm_joint_names_;
std::unordered_map<std::string, size_t> raw_idx;
raw_idx.reserve(raw.joint_names.size());
for (size_t i = 0; i < raw.joint_names.size(); ++i) raw_idx[raw.joint_names[i]] = i;
size_t start_k = 0;
{
const double t0 =
static_cast<double>(raw.points.front().time_from_start.sec) +
static_cast<double>(raw.points.front().time_from_start.nanosec) * 1e-9;
if (t0 <= 1e-9 && raw.points.size() > 1) start_k = 1;
}
out->points.reserve(raw.points.size() - start_k);
for (size_t k = start_k; k < raw.points.size(); ++k) {
const auto& rp = raw.points[k];
trajectory_msgs::msg::JointTrajectoryPoint p;
p.positions.assign(out->joint_names.size(), 0.0);
p.velocities.assign(out->joint_names.size(), 0.0);
p.accelerations.assign(out->joint_names.size(), 0.0);
for (size_t j = 0; j < out->joint_names.size(); ++j) {
auto it = raw_idx.find(out->joint_names[j]);
if (it == raw_idx.end()) continue;
const size_t ridx = it->second;
if (ridx < rp.positions.size()) p.positions[j] = rp.positions[ridx];
if (ridx < rp.velocities.size()) p.velocities[j] = rp.velocities[ridx];
if (ridx < rp.accelerations.size()) p.accelerations[j] = rp.accelerations[ridx];
}
p.time_from_start = rp.time_from_start;
const double t =
static_cast<double>(p.time_from_start.sec) +
static_cast<double>(p.time_from_start.nanosec) * 1e-9;
if (t > *total_time_sec) *total_time_sec = t;
out->points.push_back(std::move(p));
}
if (out->points.empty()) {
if (error_msg) *error_msg = "Empty trajectory after normalization";
return false;
}
return true;
}
void DualArmAction::smooth_trajectory_start_end_(
trajectory_msgs::msg::JointTrajectory* traj,
double max_acceleration,
double smooth_duration) const
{
if (!traj || traj->points.empty() || traj->joint_names.empty()) {
return;
}
const size_t num_joints = traj->joint_names.size();
const double dt = 0.01; // 采样时间间隔
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> smoothed_points;
// ==================== 处理启动平滑 ====================
const auto& first_point = traj->points.front();
// 检查第一个点是否有非零速度或加速度
bool need_start_smooth = false;
for (size_t i = 0; i < num_joints; ++i) {
double v0 = (i < first_point.velocities.size()) ? first_point.velocities[i] : 0.0;
double a0 = (i < first_point.accelerations.size()) ? first_point.accelerations[i] : 0.0;
if (std::abs(v0) > 1e-6 || std::abs(a0) > 1e-6) {
need_start_smooth = true;
break;
}
}
if (need_start_smooth) {
// 计算从静止加速到第一个点速度所需的时间
double max_v0 = 0.0;
for (size_t i = 0; i < num_joints; ++i) {
double v0 = (i < first_point.velocities.size()) ? std::abs(first_point.velocities[i]) : 0.0;
if (v0 > max_v0) max_v0 = v0;
}
double t_accel = (max_v0 > 1e-6) ? std::min(smooth_duration, max_v0 / max_acceleration) : smooth_duration;
if (t_accel < dt) t_accel = dt;
// 生成启动段（从静止加速到第一个点的速度）
size_t n_start = static_cast<size_t>(std::ceil(t_accel / dt));
for (size_t k = 0; k < n_start; ++k) {
double t = static_cast<double>(k) * dt;
if (t > t_accel) t = t_accel;
trajectory_msgs::msg::JointTrajectoryPoint p;
p.positions.resize(num_joints, 0.0);
p.velocities.resize(num_joints, 0.0);
p.accelerations.resize(num_joints, 0.0);
for (size_t i = 0; i < num_joints; ++i) {
double v_target = (i < first_point.velocities.size()) ? first_point.velocities[i] : 0.0;
double a_target = (i < first_point.accelerations.size()) ? first_point.accelerations[i] : 0.0;
// 使用匀加速模型：v(t) = a*t, s(t) = 0.5*a*t^2
double accel_dir = (std::abs(v_target) > 1e-6) ? ((v_target > 0) ? max_acceleration : -max_acceleration) : 0.0;
double alpha = (t_accel > 1e-6) ? (t / t_accel) : 1.0;
alpha = std::min(1.0, alpha);
p.accelerations[i] = accel_dir * (1.0 - alpha) + a_target * alpha;
p.velocities[i] = accel_dir * t;
p.positions[i] = first_point.positions[i] - 0.5 * accel_dir * t_accel * t_accel + 0.5 * accel_dir * t * t;
}
p.time_from_start = rclcpp::Duration::from_seconds(t);
smoothed_points.push_back(std::move(p));
}
}
// ==================== 添加原始轨迹点（调整时间偏移） ====================
double time_offset = 0.0;
if (need_start_smooth && !smoothed_points.empty()) {
const auto& last_smooth = smoothed_points.back();
time_offset =
static_cast<double>(last_smooth.time_from_start.sec) +
static_cast<double>(last_smooth.time_from_start.nanosec) * 1e-9;
}
for (size_t k = 0; k < traj->points.size(); ++k) {
const auto& orig = traj->points[k];
trajectory_msgs::msg::JointTrajectoryPoint p = orig;
double orig_time =
static_cast<double>(orig.time_from_start.sec) +
static_cast<double>(orig.time_from_start.nanosec) * 1e-9;
// 如果是第一个点且已添加启动段，跳过（避免重复）
if (k == 0 && need_start_smooth) {
continue;
}
double new_time = orig_time + time_offset;
p.time_from_start = rclcpp::Duration::from_seconds(new_time);
smoothed_points.push_back(std::move(p));
}
// ==================== 处理停止平滑 ====================
if (!smoothed_points.empty()) {
const auto& last_point = smoothed_points.back();
// 检查最后一个点是否有非零速度或加速度
bool need_stop_smooth = false;
for (size_t i = 0; i < num_joints; ++i) {
double v_end = (i < last_point.velocities.size()) ? last_point.velocities[i] : 0.0;
double a_end = (i < last_point.accelerations.size()) ? last_point.accelerations[i] : 0.0;
if (std::abs(v_end) > 1e-6 || std::abs(a_end) > 1e-6) {
need_stop_smooth = true;
break;
}
}
if (need_stop_smooth) {
// 计算从最后一个点速度减速到静止所需的时间
double max_v_end = 0.0;
for (size_t i = 0; i < num_joints; ++i) {
double v_end = (i < last_point.velocities.size()) ? std::abs(last_point.velocities[i]) : 0.0;
if (v_end > max_v_end) max_v_end = v_end;
}
double t_decel = (max_v_end > 1e-6) ? std::min(smooth_duration, max_v_end / max_acceleration) : smooth_duration;
if (t_decel < dt) t_decel = dt;
// 获取最后一个点的时间
double last_time =
static_cast<double>(last_point.time_from_start.sec) +
static_cast<double>(last_point.time_from_start.nanosec) * 1e-9;
// 生成停止段（从最后一个点的速度减速到静止）
size_t n_stop = static_cast<size_t>(std::ceil(t_decel / dt));
for (size_t k = 1; k <= n_stop; ++k) {
double t = static_cast<double>(k) * dt;
if (t > t_decel) t = t_decel;
trajectory_msgs::msg::JointTrajectoryPoint p;
p.positions.resize(num_joints, 0.0);
p.velocities.resize(num_joints, 0.0);
p.accelerations.resize(num_joints, 0.0);
for (size_t i = 0; i < num_joints; ++i) {
double v_start = (i < last_point.velocities.size()) ? last_point.velocities[i] : 0.0;
double pos_start = last_point.positions[i];
// 使用匀减速模型：v(t) = v0 - a*t, s(t) = s0 + v0*t - 0.5*a*t^2
double accel_dir = (std::abs(v_start) > 1e-6) ? ((v_start > 0) ? -max_acceleration : max_acceleration) : 0.0;
double alpha = (t_decel > 1e-6) ? (t / t_decel) : 1.0;
alpha = std::min(1.0, alpha);
p.velocities[i] = v_start + accel_dir * t;
p.accelerations[i] = accel_dir;
p.positions[i] = pos_start + v_start * t + 0.5 * accel_dir * t * t;
// 确保最终速度为0
if (std::abs(p.velocities[i]) < 1e-6) {
p.velocities[i] = 0.0;
}
}
p.time_from_start = rclcpp::Duration::from_seconds(last_time + t);
smoothed_points.push_back(std::move(p));
}
}
}
// 替换原始轨迹点
if (!smoothed_points.empty()) {
traj->points = std::move(smoothed_points);
}
}
void DualArmAction::fill_feedback_from_fjt_(
const FollowJTA::Feedback& fjt_fb,
const MotionParameters& motion_params,
size_t arm_dof,
double total_time_sec,
DualArm::Feedback* out) const
{
if (!out) return;
out->status = 1;
const double t_des =
static_cast<double>(fjt_fb.desired.time_from_start.sec) +
static_cast<double>(fjt_fb.desired.time_from_start.nanosec) * 1e-9;
if (total_time_sec > 1e-9) {
double p = t_des / total_time_sec;
if (p < 0.0) p = 0.0;
if (p > 1.0) p = 1.0;
out->progress = p;
}
out->joints_left.assign(arm_dof, 0.0);
out->joints_right.assign(arm_dof, 0.0);
std::unordered_map<std::string, size_t> dual_idx;
dual_idx.reserve(motion_params.dual_arm_joint_names_.size());
for (size_t i = 0; i < motion_params.dual_arm_joint_names_.size(); ++i) {
dual_idx[motion_params.dual_arm_joint_names_[i]] = i;
}
for (size_t k = 0; k < fjt_fb.joint_names.size() && k < fjt_fb.actual.positions.size(); ++k) {
auto it = dual_idx.find(fjt_fb.joint_names[k]);
if (it == dual_idx.end()) continue;
const size_t idx = it->second;
if (idx < arm_dof) {
out->joints_left[idx] = fjt_fb.actual.positions[k];
} else if (idx < 2 * arm_dof) {
out->joints_right[idx - arm_dof] = fjt_fb.actual.positions[k];
}
}
}
void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
if (!ctx_.robot_control_manager || !follow_executor_) {
auto result = std::make_shared<DualArm::Result>();
result->success = false;
result->message = "DualArm not initialized (missing robot_control_manager or follow_executor)";
goal_handle->abort(result);
return;
}
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<DualArm::Feedback>();
auto result = std::make_shared<DualArm::Result>();
result->success = false;
result->message = "DualArm action failed";
const auto& motionParams = ctx_.robot_control_manager->GetMotionParameters();
const size_t ARM_DOF = motionParams.arm_dof_;
// planning feedback
feedback->status = 0;
feedback->progress = 0.0;
feedback->joints_left.clear();
feedback->joints_right.clear();
goal_handle->publish_feedback(feedback);
bool need_left = false;
bool need_right = false;
std::vector<double> left_target;
std::vector<double> right_target;
std::string err;
if (!collect_movej_targets_(*goal, ARM_DOF, &need_left, &left_target, &need_right, &right_target, &err)) {
result->success = false;
result->message = std::string("DualArm goal invalid: ") + err;
goal_handle->abort(result);
return;
}
if (!ctx_.robot_control_manager->PlanDualArmJointMotion(
need_left, left_target, need_right, right_target,
goal->velocity_scaling, goal->acceleration_scaling))
{
result->success = false;
result->message = "DualArm MoveIt planning failed (dual_arm group)";
goal_handle->abort(result);
return;
}
feedback->status = 1;
trajectory_msgs::msg::JointTrajectory traj;
double total_time = 0.0;
if (!export_and_normalize_trajectory_(motionParams, &traj, &total_time, &err)) {
result->success = false;
result->message = std::string("Export/normalize trajectory failed: ") + err;
goal_handle->abort(result);
return;
}
// Smooth trajectory start and end to reduce impact
//smooth_trajectory_start_end_(&traj);
// Recalculate total_time after smoothing
if (!traj.points.empty()) {
const auto& last_point = traj.points.back();
total_time =
static_cast<double>(last_point.time_from_start.sec) +
static_cast<double>(last_point.time_from_start.nanosec) * 1e-9;
}
feedback->progress = 0.0;
feedback->joints_left.clear();
feedback->joints_right.clear();
goal_handle->publish_feedback(feedback);
executing_.store(true);
auto on_fjt_feedback =
[this, motionParams, ARM_DOF, total_time, feedback, goal_handle](const FollowJTA::Feedback& fjt_fb) {
fill_feedback_from_fjt_(fjt_fb, motionParams, ARM_DOF, total_time, feedback.get());
goal_handle->publish_feedback(feedback);
};
auto exec_res = follow_executor_->send_and_wait(
traj,
[&]() { return goal_handle->is_canceling(); },
on_fjt_feedback);
executing_.store(false);
if (goal_handle->is_canceling()) {
result->success = false;
result->message = "DualArm goal canceled";
goal_handle->canceled(result);
return;
}
if (!exec_res.ok) {
result->success = false;
result->message = std::string("FollowJointTrajectory failed: ") + exec_res.message;
goal_handle->abort(result);
return;
}
feedback->status = 2;
feedback->progress = 1.0;
goal_handle->publish_feedback(feedback);
result->success = true;
result->message = "DualArm succeeded";
result->final_progress = 1.0;
goal_handle->succeed(result);
}
} // namespace robot_control

255
src/robot_control/src/actions/follow_jt_executor.cpp Normal file
View File

@@ -0,0 +1,255 @@
#include "actions/follow_jt_executor.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <ctime>
#include <future>
#include <fstream>
#include <iomanip>
#include <sstream>
#include "core/robot_control_manager.hpp"
namespace robot_control
{
FollowJTExecutor::FollowJTExecutor(
rclcpp::Node* node,
RobotControlManager& robot_control_manager,
rclcpp_action::Client<FollowJTA>::SharedPtr follow_jt_client)
: node_(node)
, robot_control_manager_(robot_control_manager)
, follow_jt_client_(std::move(follow_jt_client))
{
}
FollowJTExecutor::ExecResult FollowJTExecutor::send_and_wait(
const trajectory_msgs::msg::JointTrajectory& traj,
const std::function<bool()>& is_cancel_requested,
const std::function<void(const FollowJTA::Feedback&)>& on_feedback)
{
ExecResult out;
if (!follow_jt_client_) {
out.ok = false;
out.message = "follow_jt_client is null";
return out;
}
if (!follow_jt_client_->wait_for_action_server(std::chrono::seconds(3))) {
out.ok = false;
out.message = "FollowJointTrajectory action server not available: /trajectory_controller/follow_joint_trajectory";
return out;
}
FollowJTA::Goal goal;
goal.trajectory = traj;
// Save trajectory to file if enabled
if (save_trajectory_enabled_) {
save_trajectory_to_file_(goal.trajectory);
}
typename rclcpp_action::Client<FollowJTA>::SendGoalOptions options;
options.feedback_callback =
[this, on_feedback](GoalHandleFollowJTA::SharedPtr,
const std::shared_ptr<const FollowJTA::Feedback> feedback) {
if (feedback) {
std::lock_guard<std::mutex> lk(last_feedback_mutex_);
last_feedback_ = *feedback;
}
if (on_feedback && feedback) on_feedback(*feedback);
};
auto goal_handle_future = follow_jt_client_->async_send_goal(goal, options);
if (goal_handle_future.wait_for(std::chrono::seconds(5)) != std::future_status::ready) {
out.ok = false;
out.message = "Timeout waiting FollowJointTrajectory goal acceptance";
return out;
}
GoalHandleFollowJTA::SharedPtr goal_handle = goal_handle_future.get();
if (!goal_handle) {
out.ok = false;
out.message = "FollowJointTrajectory goal rejected";
return out;
}
auto result_future = follow_jt_client_->async_get_result(goal_handle);
while (rclcpp::ok()) {
if (is_cancel_requested && is_cancel_requested()) {
(void)follow_jt_client_->async_cancel_goal(goal_handle);
// Send a short deceleration trajectory to avoid an abrupt hold “impact”
std::optional<FollowJTA::Feedback> fb;
{
std::lock_guard<std::mutex> lk(last_feedback_mutex_);
fb = last_feedback_;
}
if (fb) {
auto stop_traj = build_soft_stop_trajectory_(*fb);
FollowJTA::Goal stop_goal;
stop_goal.trajectory = stop_traj;
(void)follow_jt_client_->async_send_goal(stop_goal);
}
out.ok = false;
out.message = "Canceled by outer action";
return out;
}
auto st = result_future.wait_for(std::chrono::milliseconds(50));
if (st == std::future_status::ready) break;
}
if (!rclcpp::ok()) {
out.ok = false;
out.message = "ROS shutdown";
return out;
}
auto wrapped = result_future.get();
if (wrapped.code != rclcpp_action::ResultCode::SUCCEEDED) {
out.ok = false;
out.message = "FollowJointTrajectory failed, code=" + std::to_string(static_cast<int>(wrapped.code));
return out;
}
out.ok = true;
out.message = "OK";
return out;
}
double FollowJTExecutor::compute_soft_stop_duration_sec_(
const std::vector<double>& v0,
const std::vector<double>& amax) const
{
const size_t dof = std::min(v0.size(), amax.size());
double T = 0.1;
for (size_t i = 0; i < dof; ++i) {
const double a = std::max(0.1, amax[i]) * 0.5;
if (a > 1e-6) {
T = std::max(T, std::abs(v0[i]) / a);
}
}
return std::clamp(T, 0.1, 0.5);
}
trajectory_msgs::msg::JointTrajectory FollowJTExecutor::build_soft_stop_trajectory_(
const FollowJTA::Feedback& fb) const
{
trajectory_msgs::msg::JointTrajectory stop;
stop.header.stamp.sec = 0;
stop.header.stamp.nanosec = 0;
stop.joint_names = fb.joint_names;
const auto& mp = robot_control_manager_.GetMotionParameters();
const size_t dof = fb.joint_names.size();
std::vector<double> q0(dof, 0.0), v0(dof, 0.0), amax(dof, 1.0);
for (size_t i = 0; i < dof; ++i) {
if (i < fb.actual.positions.size()) q0[i] = fb.actual.positions[i];
if (i < fb.actual.velocities.size()) v0[i] = fb.actual.velocities[i];
auto it = mp.joints_info_map_.find(fb.joint_names[i]);
if (it != mp.joints_info_map_.end()) {
amax[i] = std::max(0.1, std::abs(it->second.max_acceleration));
} else {
amax[i] = 1.0;
}
}
const double T = compute_soft_stop_duration_sec_(v0, amax);
const double dt = 0.01;
const size_t N = static_cast<size_t>(std::ceil(T / dt));
stop.points.reserve(std::max<size_t>(N, 1));
for (size_t k = 1; k <= std::max<size_t>(N, 1); ++k) {
const double t = std::min(T, static_cast<double>(k) * dt);
trajectory_msgs::msg::JointTrajectoryPoint p;
p.positions.resize(dof, 0.0);
p.velocities.resize(dof, 0.0);
p.accelerations.resize(dof, 0.0);
for (size_t i = 0; i < dof; ++i) {
const double s = (v0[i] >= 0.0) ? 1.0 : -1.0;
const double a = std::max(0.1, amax[i]) * 0.5;
const double t_stop = (a > 1e-6) ? (std::abs(v0[i]) / a) : 0.0;
if (t_stop <= 1e-6 || std::abs(v0[i]) <= 1e-6) {
p.positions[i] = q0[i];
p.velocities[i] = 0.0;
p.accelerations[i] = 0.0;
continue;
}
if (t < t_stop) {
p.positions[i] = q0[i] + v0[i] * t - 0.5 * s * a * t * t;
p.velocities[i] = v0[i] - s * a * t;
p.accelerations[i] = -s * a;
} else {
p.positions[i] = q0[i] + 0.5 * v0[i] * t_stop;
p.velocities[i] = 0.0;
p.accelerations[i] = 0.0;
}
}
p.time_from_start = rclcpp::Duration::from_seconds(t);
stop.points.push_back(std::move(p));
}
return stop;
}
void FollowJTExecutor::save_trajectory_to_file_(
const trajectory_msgs::msg::JointTrajectory& traj) const
{
if (traj.points.empty() || traj.joint_names.empty()) {
return;
}
// Generate filename with timestamp
auto now = std::chrono::system_clock::now();
auto time_t = std::chrono::system_clock::to_time_t(now);
std::stringstream ss;
ss << "/tmp/trajectory_" << std::put_time(std::localtime(&time_t), "%Y%m%d_%H%M%S") << ".csv";
std::string filename = ss.str();
std::ofstream file(filename);
if (!file.is_open()) {
RCLCPP_WARN(node_->get_logger(), "Failed to open file for trajectory saving: %s", filename.c_str());
return;
}
// Write header
file << "time";
for (const auto& joint_name : traj.joint_names) {
file << "," << joint_name << "_position";
}
file << "\n";
// Write trajectory points
for (const auto& point : traj.points) {
// Convert time_from_start to seconds
double time_sec = point.time_from_start.sec + point.time_from_start.nanosec * 1e-9;
file << std::fixed << std::setprecision(6) << time_sec;
// Write positions for each joint
for (size_t i = 0; i < traj.joint_names.size(); ++i) {
double pos = (i < point.positions.size()) ? point.positions[i] : 0.0;
file << "," << std::fixed << std::setprecision(8) << pos;
}
file << "\n";
file.flush();
}
file.close();
RCLCPP_INFO(node_->get_logger(), "Trajectory saved to file: %s", filename.c_str());
}
} // namespace robot_control

95
src/robot_control/src/actions/move_home_action.cpp Normal file
View File

@@ -0,0 +1,95 @@
#include "actions/move_home_action.hpp"
#include <thread>
#include "core/robot_control_manager.hpp"
#include "core/common_enum.hpp"
namespace robot_control
{
MoveHomeAction::MoveHomeAction(const ActionContext& ctx)
: ctx_(ctx)
{
}
void MoveHomeAction::initialize()
{
using namespace std::placeholders;
server_ = rclcpp_action::create_server<MoveHome>(
ctx_.node,
"MoveHome",
std::bind(&MoveHomeAction::handle_goal_, this, _1, _2),
std::bind(&MoveHomeAction::handle_cancel_, this, _1),
std::bind(&MoveHomeAction::handle_accepted_, this, _1));
RCLCPP_INFO(ctx_.node->get_logger(), "MoveHome action server is ready");
}
rclcpp_action::GoalResponse MoveHomeAction::handle_goal_(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveHome::Goal> goal)
{
(void)uuid;
(void)goal;
if (!ctx_.robot_control_manager) return rclcpp_action::GoalResponse::REJECT;
if (ctx_.robot_control_manager->IsMoving(MovementPart::ALL)) {
RCLCPP_ERROR(ctx_.node->get_logger(), "Robot is moving");
return rclcpp_action::GoalResponse::REJECT;
}
if (executing_.load()) {
RCLCPP_ERROR(ctx_.node->get_logger(), "Another move home goal is executing");
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse MoveHomeAction::handle_cancel_(
const std::shared_ptr<GoalHandleMoveHome> goal_handle)
{
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void MoveHomeAction::handle_accepted_(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
{
executing_.store(true);
using namespace std::placeholders;
std::thread{std::bind(&MoveHomeAction::execute_, this, _1), goal_handle}.detach();
}
void MoveHomeAction::execute_(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
{
rclcpp::Rate loop_rate(10);
auto feedback = std::make_shared<MoveHome::Feedback>();
auto result = std::make_shared<MoveHome::Result>();
while (executing_.load() && rclcpp::ok()) {
if (goal_handle->is_canceling()) {
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(ctx_.node->get_logger(), "MoveHome canceled");
executing_.store(false);
return;
}
const auto& joint_fb = ctx_.robot_control_manager->GetJointPositions();
feedback->joint_values.clear();
for (double val : joint_fb) {
feedback->joint_values.push_back(static_cast<int64_t>(val));
}
goal_handle->publish_feedback(feedback);
loop_rate.sleep();
}
if (rclcpp::ok()) {
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(ctx_.node->get_logger(), "MoveHome succeeded.");
}
}
} // namespace robot_control

99
src/robot_control/src/actions/move_leg_action.cpp Normal file
View File

@@ -0,0 +1,99 @@
#include "actions/move_leg_action.hpp"
#include <thread>
#include "core/robot_control_manager.hpp"
#include "core/common_enum.hpp"
namespace robot_control
{
MoveLegAction::MoveLegAction(const ActionContext& ctx)
: ctx_(ctx)
{
}
void MoveLegAction::initialize()
{
using namespace std::placeholders;
server_ = rclcpp_action::create_server<MoveLeg>(
ctx_.node,
"MoveLeg",
std::bind(&MoveLegAction::handle_goal_, this, _1, _2),
std::bind(&MoveLegAction::handle_cancel_, this, _1),
std::bind(&MoveLegAction::handle_accepted_, this, _1));
RCLCPP_INFO(ctx_.node->get_logger(), "MoveLeg action server is ready");
}
rclcpp_action::GoalResponse MoveLegAction::handle_goal_(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveLeg::Goal> goal)
{
(void)uuid;
if (!ctx_.robot_control_manager) return rclcpp_action::GoalResponse::REJECT;
if (ctx_.robot_control_manager->IsMoving(MovementPart::LEG)) {
RCLCPP_ERROR(ctx_.node->get_logger(), "Robot leg is moving");
return rclcpp_action::GoalResponse::REJECT;
}
if (executing_.load()) {
RCLCPP_ERROR(ctx_.node->get_logger(), "Another move leg goal is executing");
return rclcpp_action::GoalResponse::REJECT;
}
if (!ctx_.robot_control_manager->SetMoveLegParameters(goal->move_up_distance)) {
RCLCPP_ERROR(ctx_.node->get_logger(), "Failed to set move leg parameters");
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse MoveLegAction::handle_cancel_(
const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
{
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void MoveLegAction::handle_accepted_(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
{
executing_.store(true);
using namespace std::placeholders;
std::thread{std::bind(&MoveLegAction::execute_, this, _1), goal_handle}.detach();
}
void MoveLegAction::execute_(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
{
rclcpp::Rate loop_rate(10);
auto feedback = std::make_shared<MoveLeg::Feedback>();
auto result = std::make_shared<MoveLeg::Result>();
while (executing_.load() && rclcpp::ok()) {
if (goal_handle->is_canceling()) {
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(ctx_.node->get_logger(), "MoveLeg canceled");
executing_.store(false);
return;
}
const auto& joint_fb = ctx_.robot_control_manager->GetJointPositions();
feedback->joint_values.clear();
for (double val : joint_fb) {
feedback->joint_values.push_back(static_cast<int64_t>(val));
}
goal_handle->publish_feedback(feedback);
loop_rate.sleep();
}
if (rclcpp::ok()) {
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(ctx_.node->get_logger(), "MoveLeg succeeded.");
}
}
} // namespace robot_control

99
src/robot_control/src/actions/move_waist_action.cpp Normal file
View File

@@ -0,0 +1,99 @@
#include "actions/move_waist_action.hpp"
#include <thread>
#include "core/robot_control_manager.hpp"
#include "core/common_enum.hpp"
namespace robot_control
{
MoveWaistAction::MoveWaistAction(const ActionContext& ctx)
: ctx_(ctx)
{
}
void MoveWaistAction::initialize()
{
using namespace std::placeholders;
server_ = rclcpp_action::create_server<MoveWaist>(
ctx_.node,
"MoveWaist",
std::bind(&MoveWaistAction::handle_goal_, this, _1, _2),
std::bind(&MoveWaistAction::handle_cancel_, this, _1),
std::bind(&MoveWaistAction::handle_accepted_, this, _1));
RCLCPP_INFO(ctx_.node->get_logger(), "MoveWaist action server is ready");
}
rclcpp_action::GoalResponse MoveWaistAction::handle_goal_(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveWaist::Goal> goal)
{
(void)uuid;
if (!ctx_.robot_control_manager) return rclcpp_action::GoalResponse::REJECT;
if (ctx_.robot_control_manager->IsMoving(MovementPart::WAIST)) {
RCLCPP_ERROR(ctx_.node->get_logger(), "Robot waist is moving");
return rclcpp_action::GoalResponse::REJECT;
}
if (executing_.load()) {
RCLCPP_ERROR(ctx_.node->get_logger(), "Another move waist goal is executing");
return rclcpp_action::GoalResponse::REJECT;
}
if (!ctx_.robot_control_manager->SetMoveWaistParameters(goal->move_pitch_degree, goal->move_yaw_degree)) {
RCLCPP_ERROR(ctx_.node->get_logger(), "Invalid move waist request");
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse MoveWaistAction::handle_cancel_(
const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
{
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void MoveWaistAction::handle_accepted_(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
{
executing_.store(true);
using namespace std::placeholders;
std::thread{std::bind(&MoveWaistAction::execute_, this, _1), goal_handle}.detach();
}
void MoveWaistAction::execute_(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
{
rclcpp::Rate loop_rate(10);
auto feedback = std::make_shared<MoveWaist::Feedback>();
auto result = std::make_shared<MoveWaist::Result>();
while (executing_.load() && rclcpp::ok()) {
if (goal_handle->is_canceling()) {
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(ctx_.node->get_logger(), "MoveWaist canceled");
executing_.store(false);
return;
}
const auto& joint_fb = ctx_.robot_control_manager->GetJointPositions();
feedback->joint_values.clear();
for (double val : joint_fb) {
feedback->joint_values.push_back(static_cast<int64_t>(val));
}
goal_handle->publish_feedback(feedback);
loop_rate.sleep();
}
if (rclcpp::ok()) {
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(ctx_.node->get_logger(), "MoveWaist succeeded.");
}
}
} // namespace robot_control

161
src/robot_control/src/actions/move_wheel_action.cpp Normal file
View File

@@ -0,0 +1,161 @@
#include "actions/move_wheel_action.hpp"
#include <cmath>
#include <thread>
#include "core/robot_control_manager.hpp"
#include "core/common_enum.hpp"
namespace robot_control
{
MoveWheelAction::MoveWheelAction(
const ActionContext& ctx,
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub,
rclcpp::Client<MotorParam>::SharedPtr motor_param_client)
: ctx_(ctx)
, motor_cmd_pub_(std::move(motor_cmd_pub))
, motor_param_client_(std::move(motor_param_client))
{
}
void MoveWheelAction::initialize()
{
using namespace std::placeholders;
server_ = rclcpp_action::create_server<MoveWheel>(
ctx_.node,
"MoveWheel",
std::bind(&MoveWheelAction::handle_goal_, this, _1, _2),
std::bind(&MoveWheelAction::handle_cancel_, this, _1),
std::bind(&MoveWheelAction::handle_accepted_, this, _1));
RCLCPP_INFO(ctx_.node->get_logger(), "MoveWheel action server is ready");
}
rclcpp_action::GoalResponse MoveWheelAction::handle_goal_(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveWheel::Goal> goal)
{
(void)uuid;
if (!ctx_.robot_control_manager) return rclcpp_action::GoalResponse::REJECT;
if (ctx_.robot_control_manager->IsMoving(MovementPart::WHEEL)) {
RCLCPP_ERROR(ctx_.node->get_logger(), "Robot wheel is moving");
return rclcpp_action::GoalResponse::REJECT;
}
if (executing_.load()) {
RCLCPP_ERROR(ctx_.node->get_logger(), "Another move wheel goal is executing");
return rclcpp_action::GoalResponse::REJECT;
}
if (goal->move_distance > 2.1 || goal->move_distance < -1.0 || goal->move_angle < -10 || goal->move_angle > 10) {
RCLCPP_ERROR(ctx_.node->get_logger(), "MoveWheel exceed limit");
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse MoveWheelAction::handle_cancel_(
const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
{
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void MoveWheelAction::handle_accepted_(const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
{
executing_.store(true);
using namespace std::placeholders;
std::thread{std::bind(&MoveWheelAction::execute_, this, _1), goal_handle}.detach();
}
void MoveWheelAction::execute_(const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
{
rclcpp::Rate loop_rate(10);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<MoveWheel::Feedback>();
auto result = std::make_shared<MoveWheel::Result>();
double wheelAngle = 0.0;
if (std::abs(goal->move_angle) > 0.0) {
ctx_.robot_control_manager->SetJogWheel(true);
wheelAngle = goal->move_angle;
}
if (std::abs(goal->move_angle) == 0.0 && std::abs(goal->move_distance) == 0.0) {
ctx_.robot_control_manager->SetJogWheel(false);
}
if (!ctx_.robot_control_manager->GetJogWheel()) {
const double tempValue = ctx_.robot_control_manager->GetImuYawDifference();
wheelAngle = std::abs(tempValue) > 40.0 ? 0.0 : tempValue;
}
ctx_.robot_control_manager->SetMoveWheelParameters(goal->move_distance, wheelAngle);
double ratio = ctx_.robot_control_manager->GetWheelRatio();
auto request = std::make_shared<MotorParam::Request>();
if ((goal->move_distance > 0.1) && !ctx_.robot_control_manager->GetJogWheel()) {
request->motor_id = 1;
request->max_speed = static_cast<uint16_t>(std::round((ratio) * 51));
request->add_acc = 8;
request->dec_acc = 8;
if (motor_param_client_) motor_param_client_->async_send_request(request);
rclcpp::sleep_for(std::chrono::milliseconds(50));
}
ctx_.robot_control_manager->MoveWheel();
const auto& wheel_commands = ctx_.robot_control_manager->GetWheelCommandPositions();
MotorCmd wheel_commands_msg;
wheel_commands_msg.target = "rs485";
wheel_commands_msg.type = "bm";
wheel_commands_msg.position = "";
wheel_commands_msg.motor_id = {1, 2};
if (wheel_commands.size() >= 2) {
wheel_commands_msg.motor_angle = {(float)wheel_commands[0], (float)wheel_commands[1]};
}
if (motor_cmd_pub_) motor_cmd_pub_->publish(wheel_commands_msg);
while (executing_.load() && rclcpp::ok()) {
if (goal_handle->is_canceling()) {
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(ctx_.node->get_logger(), "MoveWheel canceled");
executing_.store(false);
return;
}
const auto& joint_feedback = ctx_.robot_control_manager->GetWheelPositions();
if (joint_feedback.size() >= 2 && wheel_commands.size() >= 2) {
if (std::abs(joint_feedback[0] - wheel_commands[0]) < 20.0 &&
std::abs(joint_feedback[1] - wheel_commands[1]) < 20.0) {
executing_.store(false);
}
}
feedback->current_pos = joint_feedback.empty() ? 0.0 : joint_feedback[0];
goal_handle->publish_feedback(feedback);
loop_rate.sleep();
}
if ((goal->move_distance > 0.0) && !ctx_.robot_control_manager->GetJogWheel()) {
request->motor_id = 1;
request->max_speed = 51;
request->add_acc = 8;
request->dec_acc = 8;
if (motor_param_client_) motor_param_client_->async_send_request(request);
}
if (rclcpp::ok()) {
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(ctx_.node->get_logger(), "MoveWheel succeeded.");
}
}
} // namespace robot_control

199
src/robot_control/src/controllers/arm_control.cpp
View File

@@ -1,5 +1,4 @@
#include "controllers/arm_control.hpp"
#include <iostream>
#include <stdexcept>
#include <vector>
#include <memory>
@@ -31,7 +30,6 @@ ArmControl::ArmControl(
, node_(nullptr)
, current_arm_id_(0)
{
std::cout << "ArmControl Constructor: total_joints=" << joint_names.size() << std::endl;
// MoveIt配置从SRDF/URDF文件加载，但基础参数从MotionParameters获取
}
@@ -46,7 +44,7 @@ ArmControl::~ArmControl()
bool ArmControl::InitializeMoveIt(rclcpp::Node* node)
{
if (!node) {
std::cerr << "ArmControl::InitializeMoveIt: node is null" << std::endl;
RCLCPP_ERROR(rclcpp::get_logger("robot_control.arm_control"), "InitializeMoveIt: node is null");
return false;
}
@@ -68,16 +66,15 @@ bool ArmControl::InitializeMoveIt(rclcpp::Node* node)
move_group_left_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
node_shared, "arm_left");
if (!move_group_left_) {
std::cerr << "ArmControl::InitializeMoveIt: Failed to create MoveGroupInterface for arm_left" << std::endl;
RCLCPP_ERROR(node_->get_logger(), "InitializeMoveIt: failed to create MoveGroupInterface for arm_left");
return false;
}
std::cout << "ArmControl::InitializeMoveIt: MoveGroupInterface initialized for arm_left" << std::endl;
} catch (const std::exception& e) {
std::cerr << "ArmControl::InitializeMoveIt: Exception creating arm_left MoveGroupInterface: " << e.what() << std::endl;
RCLCPP_ERROR(node_->get_logger(), "InitializeMoveIt: exception creating arm_left MoveGroupInterface: %s", e.what());
move_group_left_.reset();
return false;
} catch (...) {
std::cerr << "ArmControl::InitializeMoveIt: Unknown exception creating arm_left MoveGroupInterface" << std::endl;
RCLCPP_ERROR(node_->get_logger(), "InitializeMoveIt: unknown exception creating arm_left MoveGroupInterface");
move_group_left_.reset();
return false;
}
@@ -87,18 +84,17 @@ bool ArmControl::InitializeMoveIt(rclcpp::Node* node)
move_group_right_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
node_shared, "arm_right");
if (!move_group_right_) {
std::cerr << "ArmControl::InitializeMoveIt: Failed to create MoveGroupInterface for arm_right" << std::endl;
RCLCPP_ERROR(node_->get_logger(), "InitializeMoveIt: failed to create MoveGroupInterface for arm_right");
move_group_left_.reset();
return false;
}
std::cout << "ArmControl::InitializeMoveIt: MoveGroupInterface initialized for arm_right" << std::endl;
} catch (const std::exception& e) {
std::cerr << "ArmControl::InitializeMoveIt: Exception creating arm_right MoveGroupInterface: " << e.what() << std::endl;
RCLCPP_ERROR(node_->get_logger(), "InitializeMoveIt: exception creating arm_right MoveGroupInterface: %s", e.what());
move_group_left_.reset();
move_group_right_.reset();
return false;
} catch (...) {
std::cerr << "ArmControl::InitializeMoveIt: Unknown exception creating arm_right MoveGroupInterface" << std::endl;
RCLCPP_ERROR(node_->get_logger(), "InitializeMoveIt: unknown exception creating arm_right MoveGroupInterface");
move_group_left_.reset();
move_group_right_.reset();
return false;
@@ -109,20 +105,19 @@ bool ArmControl::InitializeMoveIt(rclcpp::Node* node)
move_group_dual_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
node_shared, "dual_arm");
if (!move_group_dual_) {
std::cerr << "ArmControl::InitializeMoveIt: Failed to create MoveGroupInterface for dual_arm" << std::endl;
RCLCPP_ERROR(node_->get_logger(), "InitializeMoveIt: failed to create MoveGroupInterface for dual_arm");
move_group_left_.reset();
move_group_right_.reset();
return false;
}
std::cout << "ArmControl::InitializeMoveIt: MoveGroupInterface initialized for dual_arm" << std::endl;
} catch (const std::exception& e) {
std::cerr << "ArmControl::InitializeMoveIt: Exception creating dual_arm MoveGroupInterface: " << e.what() << std::endl;
RCLCPP_ERROR(node_->get_logger(), "InitializeMoveIt: exception creating dual_arm MoveGroupInterface: %s", e.what());
move_group_left_.reset();
move_group_right_.reset();
move_group_dual_.reset();
return false;
} catch (...) {
std::cerr << "ArmControl::InitializeMoveIt: Unknown exception creating dual_arm MoveGroupInterface" << std::endl;
RCLCPP_ERROR(node_->get_logger(), "InitializeMoveIt: unknown exception creating dual_arm MoveGroupInterface");
move_group_left_.reset();
move_group_right_.reset();
move_group_dual_.reset();
@@ -131,13 +126,13 @@ bool ArmControl::InitializeMoveIt(rclcpp::Node* node)
return true;
} catch (const std::exception& e) {
std::cerr << "ArmControl::InitializeMoveIt: Exception: " << e.what() << std::endl;
RCLCPP_ERROR(node_->get_logger(), "InitializeMoveIt: exception: %s", e.what());
move_group_left_.reset();
move_group_right_.reset();
move_group_dual_.reset();
return false;
} catch (...) {
std::cerr << "ArmControl::InitializeMoveIt: Unknown exception" << std::endl;
RCLCPP_ERROR(node_->get_logger(), "InitializeMoveIt: unknown exception");
move_group_left_.reset();
move_group_right_.reset();
move_group_dual_.reset();
@@ -155,7 +150,9 @@ std::shared_ptr<moveit::planning_interface::MoveGroupInterface> ArmControl::GetM
case 2: // ARM_DUAL
return move_group_dual_;
default:
std::cerr << "ArmControl::GetMoveGroup: Invalid arm_id " << static_cast<int>(arm_id) << std::endl;
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "GetMoveGroup: invalid arm_id %d", static_cast<int>(arm_id));
}
return nullptr;
}
}
@@ -198,14 +195,17 @@ bool ArmControl::PlanJointMotion(
moveit::core::MoveItErrorCode error_code = move_group->plan(plan);
if (error_code != moveit::core::MoveItErrorCode::SUCCESS) {
std::cerr << " Planning failed with error code: "
<< error_code.val << " for arm_id " << static_cast<int>(arm_id) << std::endl;
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "PlanJointMotion failed: error_code=%d arm_id=%d", error_code.val, static_cast<int>(arm_id));
}
return false;
}
return timeParameterizeResampleAndStore_(
return timeParameterizeAndStore_(
arm_id, *move_group, plan, velocity_scaling, acceleration_scaling);
} catch (const std::exception& e) {
std::cerr << "ArmControl::PlanJointMotion: Exception: " << e.what() << std::endl;
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "PlanJointMotion exception: %s", e.what());
}
return false;
}
}
@@ -218,7 +218,6 @@ bool ArmControl::PlanCartesianMotion(
{
auto move_group = GetMoveGroup(arm_id);
if (!move_group) {
std::cerr << "ArmControl::PlanCartesianMotion: Invalid arm_id or MoveIt not initialized" << std::endl;
return false;
}
@@ -241,111 +240,79 @@ bool ArmControl::PlanCartesianMotion(
moveit::core::MoveItErrorCode error_code = move_group->plan(plan);
if (error_code != moveit::core::MoveItErrorCode::SUCCESS) {
std::cerr << "ArmControl::PlanCartesianMotion: Planning failed with error code: "
<< error_code.val << " for arm_id " << static_cast<int>(arm_id) << std::endl;
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "PlanCartesianMotion failed: error_code=%d arm_id=%d", error_code.val, static_cast<int>(arm_id));
}
return false;
}
return timeParameterizeResampleAndStore_(
return timeParameterizeAndStore_(
arm_id, *move_group, plan, velocity_scaling, acceleration_scaling);
} catch (const std::exception& e) {
std::cerr << "ArmControl::PlanCartesianMotion: Exception: " << e.what() << std::endl;
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "PlanCartesianMotion exception: %s", e.what());
}
return false;
}
}
bool ArmControl::timeParameterizeResampleAndStore_(
bool ArmControl::timeParameterizeAndStore_(
uint8_t arm_id,
moveit::planning_interface::MoveGroupInterface& move_group,
moveit::planning_interface::MoveGroupInterface::Plan& plan,
double velocity_scaling,
double acceleration_scaling)
{
// 固定采样时间间隔（建议与你的控制周期一致或为其整数倍）
// CYCLE_TIME 单位是毫秒(ms)，这里转换为秒(s)
constexpr double kFixedTimeStep = static_cast<double>(CYCLE_TIME) / 1000.0;
// 1) RobotTrajectory：从 plan.trajectory_ 构建
robot_trajectory::RobotTrajectory original_traj(
move_group.getRobotModel(), move_group.getName());
original_traj.setRobotTrajectoryMsg(*move_group.getCurrentState(), plan.trajectory_);
// 2) 先时间参数化，得到合理的 total_time
// 2) 时间参数化：为每个点生成 time_from_start（FollowJointTrajectory 直接执行即可）
trajectory_processing::IterativeParabolicTimeParameterization time_param;
if (!time_param.computeTimeStamps(original_traj, velocity_scaling, acceleration_scaling))
{
std::cerr << "ArmControl: computeTimeStamps failed for arm_id "
<< static_cast<int>(arm_id) << std::endl;
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "computeTimeStamps failed for arm_id %d", static_cast<int>(arm_id));
}
return false;
}
const size_t waypoint_count = original_traj.getWayPointCount();
if (waypoint_count == 0)
{
std::cerr << "ArmControl: Empty trajectory after time parameterization for arm_id "
<< static_cast<int>(arm_id) << std::endl;
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "Empty trajectory after time parameterization for arm_id %d", static_cast<int>(arm_id));
}
return false;
}
const double total_time = original_traj.getWayPointDurationFromStart(waypoint_count - 1);
// 3) 固定 dt 重采样：注意 addSuffixWayPoint 的 dt 参数是“相邻点间隔”，不是绝对时间
robot_trajectory::RobotTrajectory resampled_traj(
move_group.getRobotModel(), move_group.getName());
// 3) 写回 plan.trajectory_（带时间戳的原始轨迹，不做固定 dt 重采样）
original_traj.getRobotTrajectoryMsg(plan.trajectory_);
moveit::core::RobotStatePtr interpolated_state =
std::make_shared<moveit::core::RobotState>(move_group.getRobotModel());
double prev_t = 0.0;
bool first = true;
for (double current_time = 0.0; current_time <= total_time + 1e-9; current_time += kFixedTimeStep)
{
const double t = std::min(current_time, total_time);
if (!original_traj.getStateAtDurationFromStart(t, interpolated_state))
{
std::cerr << "ArmControl: getStateAtDurationFromStart failed at t=" << t
<< " for arm_id " << static_cast<int>(arm_id) << std::endl;
return false;
}
const double dt_from_prev = first ? 0.0 : (t - prev_t);
resampled_traj.addSuffixWayPoint(*interpolated_state, dt_from_prev);
first = false;
prev_t = t;
if (t >= total_time - 1e-9) break;
}
// 4) 写回 plan.trajectory_
resampled_traj.getRobotTrajectoryMsg(plan.trajectory_);
// 5) 存入内部 TrajectoryState（使用 msg 内的 time_from_start，应该已经是均匀 dt）
// 4) 存入内部 TrajectoryState（使用 msg 内的 time_from_start，并保留 vel/acc）
const auto& jt = plan.trajectory_.joint_trajectory;
if (jt.points.empty())
{
std::cerr << "ArmControl: Trajectory is empty after resampling for arm_id "
<< static_cast<int>(arm_id) << std::endl;
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "Trajectory is empty after time parameterization for arm_id %d", static_cast<int>(arm_id));
}
return false;
}
TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
TrajectoryState* traj_state =
(arm_id == 0) ? &left_arm_trajectory_ :
(arm_id == 1) ? &right_arm_trajectory_ :
&dual_arm_trajectory_;
traj_state->trajectory_points.clear();
const size_t num_joints = jt.points[0].positions.size();
traj_state->trajectory_points.reserve(jt.points.size());
for (const auto& point : jt.points)
{
TrajectoryPoint traj_point;
traj_point.positions = point.positions;
if (point.velocities.size() == num_joints)
traj_point.velocities = point.velocities;
else
traj_point.velocities.assign(num_joints, 0.0);
if (point.accelerations.size() == num_joints)
traj_point.accelerations = point.accelerations;
else
traj_point.accelerations.assign(num_joints, 0.0);
traj_point.velocities = point.velocities;
traj_point.accelerations = point.accelerations;
traj_point.time_from_start =
static_cast<double>(point.time_from_start.sec) +
@@ -358,7 +325,18 @@ bool ArmControl::timeParameterizeResampleAndStore_(
traj_state->elapsed_time = 0.0;
traj_state->is_active = true;
std::cout << "ArmControl::timeParameterizeResampleAndStore_: Resampled points: " << traj_state->trajectory_points.size() << std::endl;
if (node_) {
RCLCPP_DEBUG(node_->get_logger(), "Time-parameterized points: %zu", traj_state->trajectory_points.size());
}
// 同步保存 JointTrajectory msg（用于 action 一次性下发）
if (arm_id == 0) {
left_arm_traj_msg_ = jt;
} else if (arm_id == 1) {
right_arm_traj_msg_ = jt;
} else {
dual_arm_traj_msg_ = jt;
}
return true;
}
@@ -390,8 +368,8 @@ void ArmControl::UpdateJointsInfo(uint8_t arm_id, const TrajectoryPoint& point)
// 辅助函数：更新 joints_info_map_ 从插补后的值（区分左右臂）
void ArmControl::UpdateJointsInfoInterpolated(
uint8_t arm_id,
const TrajectoryPoint& p1,
const TrajectoryPoint& p2,
const TrajectoryPoint& p1,
const TrajectoryPoint& p2,
double alpha,
size_t num_joints,
const std::vector<double>& joint_positions)
@@ -406,29 +384,41 @@ void ArmControl::UpdateJointsInfoInterpolated(
}
auto& joint_info = joints_info_map_[joint_names_[idx]];
joint_info.current_position = joint_positions[i];
// 插补速度
// 插补速度/加速度（如果存在）
if (i < p1.velocities.size() && i < p2.velocities.size()) {
double vel = p1.velocities[i] + alpha * (p2.velocities[i] - p1.velocities[i]);
joint_info.current_velocity = vel;
joint_info.command_velocity = vel;
} else if (i < p1.velocities.size()) {
joint_info.current_velocity = p1.velocities[i];
joint_info.command_velocity = p1.velocities[i];
const double v = p1.velocities[i] + alpha * (p2.velocities[i] - p1.velocities[i]);
joint_info.current_velocity = v;
joint_info.command_velocity = v;
}
// 插补加速度
if (i < p1.accelerations.size() && i < p2.accelerations.size()) {
double acc = p1.accelerations[i] + alpha * (p2.accelerations[i] - p1.accelerations[i]);
joint_info.current_acceleration = acc;
joint_info.command_acceleration = acc;
} else if (i < p1.accelerations.size()) {
joint_info.current_acceleration = p1.accelerations[i];
joint_info.command_acceleration = p1.accelerations[i];
const double a = p1.accelerations[i] + alpha * (p2.accelerations[i] - p1.accelerations[i]);
joint_info.current_acceleration = a;
joint_info.command_acceleration = a;
}
}
}
bool ArmControl::ExportPlannedJointTrajectory(uint8_t arm_id, trajectory_msgs::msg::JointTrajectory& out) const
{
if (arm_id == 0) {
if (left_arm_traj_msg_.points.empty()) return false;
out = left_arm_traj_msg_;
return true;
}
if (arm_id == 1) {
if (right_arm_traj_msg_.points.empty()) return false;
out = right_arm_traj_msg_;
return true;
}
if (arm_id == 2) {
if (dual_arm_traj_msg_.points.empty()) return false;
out = dual_arm_traj_msg_;
return true;
}
return false;
}
bool ArmControl::GetInterpolatedPoint(uint8_t arm_id, double dt, std::vector<double>& joint_positions)
{
TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
@@ -439,11 +429,9 @@ bool ArmControl::GetInterpolatedPoint(uint8_t arm_id, double dt, std::vector<dou
// 累加已执行时间
traj_state->elapsed_time += dt;
// std::cout << "ArmControl::GetInterpolatedPoint: elapsed_time = " << traj_state->elapsed_time << std::endl;
// 获取轨迹的总时间（最后一个点的时间）
const double total_time = traj_state->trajectory_points.back().time_from_start;
// std::cout << "ArmControl::GetInterpolatedPoint: total_time = " << total_time << std::endl;
// 如果已执行时间超过总时间，轨迹执行完成
if (traj_state->elapsed_time >= total_time - 1e-6) {
@@ -547,8 +535,9 @@ void ArmControl::ClearTrajectory(uint8_t arm_id)
traj_state->elapsed_time = 0.0;
traj_state->is_active = false;
std::cout << "ArmControl::ClearTrajectory: Cleared trajectory for arm_id "
<< static_cast<int>(arm_id) << std::endl;
if (node_) {
RCLCPP_DEBUG(node_->get_logger(), "Cleared trajectory for arm_id %d", static_cast<int>(arm_id));
}
}
// ==================== 重写ControlBase的方法 ====================

3
src/robot_control/src/controllers/control_base.cpp
View File

@@ -2,7 +2,6 @@
#include "core/motion_parameters.hpp"
#include <stdexcept>
#include <vector>
#include <iostream>
namespace robot_control
{
@@ -253,7 +252,7 @@ void ControlBase::UpdateJointStates(
joints_info_map_[joint_names_[i]].command_position = joint_positions[i];
}
is_joint_position_initialized_ = true;
std::cout << "joint commands initialized" << std::endl;
// joint positions initialized
}
}

59
src/robot_control/src/controllers/leg_control.cpp
View File

@@ -2,7 +2,6 @@
#include "core/motion_parameters.hpp"
#include <stdexcept>
#include <vector>
#include <iostream>
#include <cmath>
#ifndef M_PI
@@ -19,26 +18,20 @@ LegControl::LegControl(
) : ControlBase(joint_names, joints_info_map, lengthParameters)
, target_joint_positions_(joint_names.size(), 0.0)
{
std::cout << "LegControl Constructor" << std::endl;
}
LegControl::~LegControl()
{
delete trapezoidalTrajectory_;
// ControlBase owns and deletes trajectory planners
}
// MoveToTargetPoint removed - not declared in header
bool LegControl::MoveUp(std::vector<double>& joint_positions, double dt)
{
if (!is_moving_)
{
if (!is_target_set_)
{
{
std::cout << "Target position not set!" << std::endl;
return true;
}
return true;
}
}
@@ -89,10 +82,10 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
std::vector<double> tempPosition;
tempPosition.resize(total_joints_, 0.0);
// 使用默认零位置 0.0（已删除 joint_zero_positions_）
double backJoint1 = DEG2RAD(joints_info_map_[joint_names_[0]].current_position);
double frountJoint1 = DEG2RAD(joints_info_map_[joint_names_[1]].current_position);
double frountJoint2 = DEG2RAD(joints_info_map_[joint_names_[2]].current_position);
// 内部统一单位：rad（JointState 也是 rad）
double backJoint1 = joints_info_map_[joint_names_[0]].current_position;
double frountJoint1 = joints_info_map_[joint_names_[1]].current_position;
double frountJoint2 = joints_info_map_[joint_names_[2]].current_position;
double currentBackLegHeight = lengthParameters_[0] * abs(std::sin(backJoint1));
double currentFrountLegHeight = lengthParameters_[1] * abs(std::sin(frountJoint1)) + lengthParameters_[2] * abs(std::sin(frountJoint1 + frountJoint2));
@@ -102,10 +95,6 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
{
if ((moveDistance + currentFrountLegHeight) > lengthParameters_[3])
{
std::cout << "Move distance is too large! " << std::endl;
std::cout << " Current Front Leg Length: " << currentFrountLegHeight << std::endl;
std::cout << " Current Back Leg Length: " << currentBackLegHeight << std::endl;
std::cout << " Move Distance: " << moveDistance << std::endl;
return false;
}
}
@@ -113,10 +102,6 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
{
if (moveDistance + currentFrountLegHeight < lengthParameters_[4])
{
std::cout << "Move distance is too large!" << std::endl;
std::cout << " Current Front Leg Length: " << currentFrountLegHeight << std::endl;
std::cout << " Current Back Leg Length: " << currentBackLegHeight << std::endl;
std::cout << " Move Distance: " << moveDistance << std::endl;
return false;
}
}
@@ -124,31 +109,27 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
double finalFrountLegHeight = currentFrountLegHeight + moveDistance;
double finalBackLegHeight = currentBackLegHeight + moveDistance;
double backFinalAngle = RAD2DEG(std::acos(finalBackLegHeight / lengthParameters_[0]));
double backFinalAngle = std::acos(finalBackLegHeight / lengthParameters_[0]);
double tempFrountLegLength = std::sqrt(finalFrountLegHeight * finalFrountLegHeight + currentFrountLegDistance * currentFrountLegDistance);
double frountAngle1_part1 = calculate_angle_from_links(finalFrountLegHeight, tempFrountLegLength, currentFrountLegDistance, true);
double frountAngle1_part2 = calculate_angle_from_links(tempFrountLegLength, lengthParameters_[1], lengthParameters_[2], true);
std::cout << "frountAngle1_part1 : " << frountAngle1_part1 << std::endl;
std::cout << "frountAngle1_part2 : " << frountAngle1_part2 << std::endl;
double frountAngle1_part1 = calculate_angle_from_links(finalFrountLegHeight, tempFrountLegLength, currentFrountLegDistance, false);
double frountAngle1_part2 = calculate_angle_from_links(tempFrountLegLength, lengthParameters_[1], lengthParameters_[2], false);
double frountFinalAngle1 = frountAngle1_part1 + frountAngle1_part2;
double frountFinalAngle2 = calculate_angle_from_links(lengthParameters_[1], lengthParameters_[2], tempFrountLegLength, true);
double frountFinalAngle2 = calculate_angle_from_links(lengthParameters_[1], lengthParameters_[2], tempFrountLegLength, false);
// 使用默认方向 1（正方向）
// 零点为水平方向：把原来的 90/180 度常数替换为 π/2, π（弧度）
tempPosition[0] = backFinalAngle;
tempPosition[1] = (90.0 - frountFinalAngle1); //零点为水平方向
tempPosition[2] = (180.0 - frountFinalAngle2);
tempPosition[3] = (90.0 - frountFinalAngle1);
tempPosition[4] = (180.0 - frountFinalAngle2);
tempPosition[1] = (M_PI / 2.0 - frountFinalAngle1);
tempPosition[2] = (M_PI - frountFinalAngle2);
tempPosition[3] = (M_PI / 2.0 - frountFinalAngle1);
tempPosition[4] = (M_PI - frountFinalAngle2);
tempPosition[5] = backFinalAngle;
if (!checkJointLimits(tempPosition))
{
std::cout << "Joint limits exceeded!" << std::endl;
return false;
}
@@ -158,16 +139,6 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
target_joint_positions_[i] = tempPosition[i];
}
std::cout << "Target relative joint Position: "
<< tempPosition[0] << ", " << tempPosition[1] << ", "
<< tempPosition[2] << ", " << tempPosition[3] << ", "
<< tempPosition[4] << ", " << tempPosition[5] << std::endl;
std::cout << "Target abs Joint Position: "
<< target_joint_positions_[0] << ", " << target_joint_positions_[1] << ", "
<< target_joint_positions_[2] << ", " << target_joint_positions_[3] << ", "
<< target_joint_positions_[4] << ", " << target_joint_positions_[5] << std::endl;
is_target_set_ = true;
return true;

44
src/robot_control/src/controllers/waist_control.cpp
View File

@@ -1,6 +1,5 @@
#include "controllers/waist_control.hpp"
#include "core/motion_parameters.hpp"
#include <iostream>
#include <stdexcept>
#include <vector>
@@ -14,24 +13,18 @@ WaistControl::WaistControl(
): ControlBase(joint_names, joints_info_map, lengthParameters)
, target_joint_positions_(joint_names.size(), 0.0)
{
std::cout << "WaistControl Constructor" << std::endl;
}
WaistControl::~WaistControl()
{
delete trapezoidalTrajectory_;
// ControlBase owns and deletes trajectory planners
}
// MoveToTargetPoint removed - not declared in header
bool WaistControl::MoveWaist(std::vector<double>& joint_positions, double dt)
{
if (!is_moving_)
{
if (!is_target_set_)
{
std::cout << "Target position not set!" << std::endl;
return true;
}
}
@@ -44,7 +37,6 @@ bool WaistControl::MoveWaist(std::vector<double>& joint_positions, double dt)
{
if (IsReached(target_joint_positions_) || (time_out_count_ > TIME_OUT_COUNT))
{
std::cout << "Waist reached target position!" << std::endl;
is_target_set_ = false;
is_cmd_send_finished_ = false;
time_out_count_ = 0;
@@ -53,7 +45,6 @@ bool WaistControl::MoveWaist(std::vector<double>& joint_positions, double dt)
else
{
time_out_count_ ++;
// std::cout << "Waist not reached target position!" << std::endl;
return false;
}
}
@@ -61,32 +52,26 @@ bool WaistControl::MoveWaist(std::vector<double>& joint_positions, double dt)
return false;
}
// void WaistControl::SetHomePositions(const std::vector<double>& home_positions)
// {
// if (home_positions.size() != total_joints_)
// {
// throw std::invalid_argument("Home positions size does not match total joints!");
// }
// // if (home_positions[1] < 0)
// // {
// // joint_home_positions_[1] = joint_home_positions_[1] - 360.0;
// // }
// std::cout << "Home positions set to: " << joint_home_positions_[0] << ", " << joint_home_positions_[1] << std::endl;
// }
bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double moveyaw)
{
std::vector<double> tempPosition;
tempPosition.resize(total_joints_, 0.0);
tempPosition[0] = joints_info_map_[joint_names_[0]].command_position - joints_info_map_[joint_names_[0]].home_position + movepitch;
tempPosition[1] = joints_info_map_[joint_names_[1]].command_position - joints_info_map_[joint_names_[1]].home_position + moveyaw;
// 输入是 degree（来自 MoveWaist.action 字段），内部统一使用 rad
const double movepitch_rad = DEG2RAD(movepitch);
const double moveyaw_rad = DEG2RAD(moveyaw);
tempPosition[0] =
joints_info_map_[joint_names_[0]].command_position -
joints_info_map_[joint_names_[0]].home_position +
movepitch_rad;
tempPosition[1] =
joints_info_map_[joint_names_[1]].command_position -
joints_info_map_[joint_names_[1]].home_position +
moveyaw_rad;
if (!checkJointLimits(tempPosition))
{
std::cout << "Joint limits exceeded!" << std::endl;
return false;
}
@@ -95,9 +80,6 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey
target_joint_positions_[i] = tempPosition[i] + joints_info_map_[joint_names_[i]].home_position;
}
std::cout << "Waist Target Joint Position: " << target_joint_positions_[0] << ", " << target_joint_positions_[1] << std::endl;
// std::cout << "Waist Target Joint Position: " << joint_position_desired_[0] << std::endl;
is_target_set_ = true;
return true;

29
src/robot_control/src/controllers/wheel_control.cpp
View File

@@ -1,6 +1,5 @@
#include "controllers/wheel_control.hpp"
#include "core/motion_parameters.hpp"
#include <iostream>
#include <vector>
namespace robot_control
@@ -15,12 +14,11 @@ WheelControl::WheelControl(
, moveRatio_(0.0)
, target_joint_positions_(joint_names.size(), 0.0)
{
std::cout << "WheelControl Constructor" << std::endl;
}
WheelControl::~WheelControl()
{
delete trapezoidalTrajectory_;
// ControlBase owns and deletes trajectory planners
}
int tempAdjustCount = 0;
@@ -110,7 +108,6 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
}
else
{
std::cout << "change distance " << std::endl;
for (size_t i = 0; i < total_joints_; i++)
{
target_joint_positions_[i] = tempDesiredPositions[i];
@@ -118,13 +115,6 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
}
}
std::cout << "motor 1 distance : " << target_joint_positions_[0] - original1 << std::endl;
std::cout << "motor 2 distance : " << target_joint_positions_[1] - original2 << std::endl;
std::cout << "Set move ratio : " << moveRatio_ << std::endl;
// std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] << " , " << joint_position_desired_[1] << std::endl;
lastMoveDistance_ = moveWheelDistance;
is_target_set_ = true;
@@ -134,9 +124,6 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, double moveWheelAngle)
{
double original1 = target_joint_positions_[0];
double original2 = target_joint_positions_[1];
std::vector<double> tempHomePositions;
std::vector<double> tempDesiredPositions;
@@ -174,13 +161,6 @@ bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, d
}
std::cout << "motor 1 distance : " << target_joint_positions_[0] - original1 << std::endl;
std::cout << "motor 2 distance : " << target_joint_positions_[1] - original2 << std::endl;
std::cout << "Set move ratio : " << moveRatio_ << std::endl;
// std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] << " , " << joint_position_desired_[1] << std::endl;
is_target_set_ = true;
return true;
@@ -188,15 +168,12 @@ bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, d
// MoveToTargetPoint removed - not declared in header
bool WheelControl::MoveWheel(std::vector<double>& joint_positions)
{
if (!is_moving_)
{
if (!is_target_set_)
{
std::cout << "Target position not set!" << std::endl;
return true;
}
}
@@ -211,10 +188,6 @@ bool WheelControl::MoveWheel(std::vector<double>& joint_positions)
return true;
}
// GoHome removed - not declared in header
double WheelControl::GetWheelRatioInternal()
{
return moveRatio_;

39
src/robot_control/src/core/motion_parameters.cpp
View File

@@ -59,10 +59,6 @@ MotionParameters::MotionParameters()
"left_arm_joint_4", "left_arm_joint_5", "left_arm_joint_6",
"right_arm_joint_1", "right_arm_joint_2", "right_arm_joint_3",
"right_arm_joint_4", "right_arm_joint_5", "right_arm_joint_6"};
// dual_arm_joint_names_ = {"joint_1", "joint_2", " joint_3",
// "joint_4", "joint_5", "joint_6",
// "joint_7", "joint_8", "joint_9",
// "joint_10", "joint_11", "joint_12"};
// ==================== 手臂参数 ====================
// 每条手臂的自由度数（关节数）
@@ -77,26 +73,39 @@ MotionParameters::MotionParameters()
joints_info_map_[wheel_joint_names_[i]] = JointInfo(0.0, 0.0, LimitType::POSITION, 5.0, 25.0, home_pos);
}
// 腰部关节：默认限位±190度，速度50度/秒，加速度100度/秒²
std::vector<double> waist_limits = {40.0, 190.0};
// 腰部关节（统一单位：rad）
// 限位/速度/加速度原始配置是度制，这里转换为弧度制，保证与 ROS JointState/ros2_control 一致
std::vector<double> waist_limits = {DEG2RAD(40.0), DEG2RAD(190.0)};
std::vector<double> waist_home = {0.0, 0.0};
for (size_t i = 0; i < waist_joint_names_.size(); i++)
{
double max_limit = (i < waist_limits.size()) ? waist_limits[i] : 190.0;
double max_limit = (i < waist_limits.size()) ? waist_limits[i] : DEG2RAD(190.0);
double home_pos = (i < waist_home.size()) ? waist_home[i] : 0.0;
joints_info_map_[waist_joint_names_[i]] = JointInfo(max_limit, -max_limit, LimitType::POSITION, 50.0, 100.0, home_pos);
joints_info_map_[waist_joint_names_[i]] = JointInfo(
max_limit, -max_limit, LimitType::POSITION,
DEG2RAD(50.0), DEG2RAD(100.0), home_pos);
}
// 腿部关节：默认限位根据关节类型设置，速度50度/秒，加速度100度/秒²
std::vector<double> leg_max_limits = {40.0, 190.0, 0.0, 90.0, 60.0, 0.0};
std::vector<double> leg_min_limits = {-40.0, -190.0, -90.0, 0.0, 0.0, -90.0};
std::vector<double> leg_home = {217.52 - 65.0, 120.84 + 41.0, 108.7 + 40.63, 221.95 - 41.0, 234.14 - 29.504, 125.39 + 65.0};
// 腿部关节（统一单位：rad）
// 注意：这些默认值看起来来自旧版本“度制”标定，这里整体转换为弧度制。
std::vector<double> leg_max_limits = {DEG2RAD(40.0), DEG2RAD(190.0), DEG2RAD(0.0), DEG2RAD(90.0), DEG2RAD(60.0), DEG2RAD(0.0)};
std::vector<double> leg_min_limits = {DEG2RAD(-40.0), DEG2RAD(-190.0), DEG2RAD(-90.0), DEG2RAD(0.0), DEG2RAD(0.0), DEG2RAD(-90.0)};
std::vector<double> leg_home = {
DEG2RAD(217.52 - 65.0),
DEG2RAD(120.84 + 41.0),
DEG2RAD(108.7 + 40.63),
DEG2RAD(221.95 - 41.0),
DEG2RAD(234.14 - 29.504),
DEG2RAD(125.39 + 65.0)
};
for (size_t i = 0; i < leg_joint_names_.size(); i++)
{
double max_limit = (i < leg_max_limits.size()) ? leg_max_limits[i] : 180.0;
double min_limit = (i < leg_min_limits.size()) ? leg_min_limits[i] : -180.0;
double max_limit = (i < leg_max_limits.size()) ? leg_max_limits[i] : DEG2RAD(180.0);
double min_limit = (i < leg_min_limits.size()) ? leg_min_limits[i] : DEG2RAD(-180.0);
double home_pos = (i < leg_home.size()) ? leg_home[i] : 0.0;
joints_info_map_[leg_joint_names_[i]] = JointInfo(max_limit, min_limit, LimitType::POSITION, 50.0, 100.0, home_pos);
joints_info_map_[leg_joint_names_[i]] = JointInfo(
max_limit, min_limit, LimitType::POSITION,
DEG2RAD(50.0), DEG2RAD(100.0), home_pos);
}
// 双臂关节（单位：弧度）

3
src/robot_control/src/core/remote_controller.cpp
View File

@@ -159,8 +159,7 @@ void RemoteController::HandleEmergencyStop()
if (!isJogMode_) {
isStopping_ = true;
RCLCPP_WARN(node_->get_logger(), "Emergency stop requested");
// 注意：停止动作的执行由robot_control_node的ControlLoop处理
// 这里只设置停止标志
// Stop is handled by the action layer / higher-level logic; we only set the flag here.
}
}

487
src/robot_control/src/core/robot_control_manager.cpp
View File

@@ -11,8 +11,13 @@
#include <string>
using namespace robot_control;
using namespace std;
using namespace std_msgs::msg;
namespace {
inline rclcpp::Logger logger()
{
return rclcpp::get_logger("robot_control.robot_control_manager");
}
} // namespace
RobotControlManager::RobotControlManager()
@@ -22,14 +27,6 @@ RobotControlManager::RobotControlManager()
void RobotControlManager::init()
{
jointInited_.resize(motionParams_.total_joints_, false);
// Initialize the joint commands
for (size_t i = 0; i < motionParams_.total_joints_; i++)
{
jointCommands_.data.push_back(0.0);
}
gyroValues_.resize(4, 0.0);
gyroVelocities_.resize(3, 0.0);
gyroAccelerations_.resize(3, 0.0);
@@ -37,10 +34,7 @@ void RobotControlManager::init()
is_wheel_jog_ = false;
// Initialize the wheel commands
for (size_t i = 0; i < motionParams_.wheel_joint_names_.size(); i++)
{
wheelCommands_.data.push_back(0.0);
}
wheelCommands_.assign(motionParams_.wheel_joint_names_.size(), 0.0);
// 初始化控制器启用标志
leg_controller_enabled_ = motionParams_.leg_controller_enabled_;
@@ -80,8 +74,6 @@ void RobotControlManager::init()
}
}
isWaistHomed_ = false;
isLegHomed_ = false;
isWheelHomed_ = false;
isJointInitValueSet_ = false;
@@ -170,49 +162,6 @@ bool RobotControlManager::SetMoveWaistParameters(double movePitchAngle, double m
return waist_controller_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle);
}
bool RobotControlManager::SetArmJointCommands(int8_t body_id, const std::vector<double>& joint_angles)
{
// 从 dual_arm_joint_names_ 获取对应的关节名称（body_id 0=左臂前arm_dof_个，1=右臂后arm_dof_个）
if (!joint_name_mapping_initialized_ || motionParams_.dual_arm_joint_names_.empty()) {
std::cout << "Joint name mapping not initialized or dual_arm_joint_names_ is empty" << std::endl;
return false;
}
const size_t ARM_DOF = motionParams_.arm_dof_; // 每条手臂的自由度数
const size_t start_idx = (body_id == 0) ? 0 : ARM_DOF; // 左臂从0开始，右臂从ARM_DOF开始
if (joint_angles.size() < ARM_DOF) {
std::cout << "Joint angles size (" << joint_angles.size() << ") is less than required (" << ARM_DOF << ")" << std::endl;
return false;
}
if (start_idx + ARM_DOF > motionParams_.dual_arm_joint_names_.size()) {
std::cout << "Arm joint names out of range for body_id: " << static_cast<int>(body_id) << std::endl;
return false;
}
// 确保jointCommands_数组足够大
if (jointCommands_.data.size() < motionParams_.total_joints_) {
jointCommands_.data.resize(motionParams_.total_joints_, 0.0);
}
// 设置关节命令（内部统一单位：弧度）
for (size_t i = 0; i < ARM_DOF; ++i) {
const std::string& joint_name = motionParams_.dual_arm_joint_names_[start_idx + i];
auto it = motionParams_.joint_name_to_index_.find(joint_name);
if (it != motionParams_.joint_name_to_index_.end()) {
size_t idx = it->second;
if (idx < jointCommands_.data.size()) {
jointCommands_.data[idx] = joint_angles[i];
}
} else {
std::cout << "Joint name not found in mapping: " << joint_name << std::endl;
}
}
return true;
}
bool RobotControlManager::InitializeArmMoveIt(rclcpp::Node* node)
{
if (!arm_controller_enabled_ || !arm_controller_) {
@@ -239,25 +188,22 @@ bool RobotControlManager::PlanArmMotion(
if (arm_params.motion_type == arm_params.MOVEJ) {
// 关节空间运动
if (arm_params.target_joints.empty()) {
std::cout << "Target joints empty" << std::endl;
RCLCPP_WARN(logger(), "PlanArmMotion MOVEJ rejected: target_joints is empty (arm_id=%d)", arm_params.arm_id);
return false;
}
{
std::string err;
if (!ValidateArmJointTarget(arm_params.arm_id, arm_params.target_joints, &err)) {
std::cout << "PlanArmMotion: target_joints exceed limits: " << err << std::endl;
RCLCPP_WARN(logger(), "PlanArmMotion MOVEJ rejected: %s", err.c_str());
return false; // 在规划之前直接失败
}
}
std::cout << "Plan Joint Motion" << std::endl;
return arm_controller_->PlanJointMotion(
arm_params.arm_id,
arm_params.target_joints, // 已经是弧度
velocity_scaling,
acceleration_scaling);
} else if (arm_params.motion_type == arm_params.MOVEL) {
// 笛卡尔空间运动
std::cout << "Plan Cartesian Motion" << std::endl;
return arm_controller_->PlanCartesianMotion(
arm_params.arm_id,
arm_params.target_pose,
@@ -268,191 +214,74 @@ bool RobotControlManager::PlanArmMotion(
return false;
}
bool RobotControlManager::GetArmInterpolatedPoint(
uint8_t arm_id,
double dt,
std::vector<double>& joint_positions)
bool RobotControlManager::PlanDualArmJointMotion(
bool has_left,
const std::vector<double>& left_target,
bool has_right,
const std::vector<double>& right_target,
double velocity_scaling,
double acceleration_scaling)
{
if (!arm_controller_enabled_ || !arm_controller_) return false;
if (!arm_controller_->IsMoveItInitialized()) return false;
if (!arm_controller_enabled_ || !arm_controller_) {
return false;
}
const size_t ARM_DOF = motionParams_.arm_dof_;
return arm_controller_->GetInterpolatedPoint(arm_id, dt, joint_positions);
}
bool RobotControlManager::HasActiveArmTrajectory(uint8_t arm_id) const
{
if (!arm_controller_enabled_ || !arm_controller_) {
return false;
}
return arm_controller_->HasActiveTrajectory(arm_id);
}
bool RobotControlManager::GetArmTrajectoryEndPositions(uint8_t arm_id, std::vector<double>& end_positions) const
{
if (!arm_controller_enabled_ || !arm_controller_) {
return false;
}
return arm_controller_->GetTrajectoryEndPositions(arm_id, end_positions);
}
double RobotControlManager::GetArmTrajectoryProgress(uint8_t arm_id) const
{
if (!arm_controller_enabled_ || !arm_controller_) {
return 1.0;
}
return arm_controller_->GetTrajectoryProgress(arm_id);
}
bool RobotControlManager::MoveArm(double dt)
{
if (!arm_controller_enabled_ || !arm_controller_) {
return true; // 没有arm控制器，认为已完成
}
// 检查是否有左臂轨迹在执行
bool left_arm_active = HasActiveArmTrajectory(0);
bool right_arm_active = HasActiveArmTrajectory(1);
if (!left_arm_active && !right_arm_active) {
return true; // 没有轨迹在执行，认为已完成
}
// 处理左臂轨迹
if (left_arm_active) {
std::vector<double> left_joint_positions;
if (GetArmInterpolatedPoint(0, dt, left_joint_positions)) {
SetArmJointCommands(0, left_joint_positions);
// 构建 12 关节目标：默认保持当前 jointPositions_（如未初始化，则保持 0）
std::vector<double> target(2 * ARM_DOF, 0.0);
if (joint_name_mapping_initialized_) {
for (size_t i = 0; i < 2 * ARM_DOF; ++i) {
const std::string& jn = motionParams_.dual_arm_joint_names_[i];
auto it = motionParams_.joint_name_to_index_.find(jn);
if (it != motionParams_.joint_name_to_index_.end() && it->second < jointPositions_.size()) {
target[i] = jointPositions_[it->second];
}
}
}
// 处理右臂轨迹
if (right_arm_active) {
std::vector<double> right_joint_positions;
if (GetArmInterpolatedPoint(1, dt, right_joint_positions)) {
SetArmJointCommands(1, right_joint_positions);
}
if (has_left) {
for (size_t i = 0; i < ARM_DOF; ++i) target[i] = left_target[i];
}
if (has_right) {
for (size_t i = 0; i < ARM_DOF; ++i) target[ARM_DOF + i] = right_target[i];
}
// 检查是否还有轨迹在执行
return !HasActiveArmTrajectory(0) && !HasActiveArmTrajectory(1);
// arm_id=2 表示使用 dual_arm MoveGroup 规划 12关节轨迹
return arm_controller_->PlanJointMotion(2, target, velocity_scaling, acceleration_scaling);
}
Float64MultiArray RobotControlManager::GetWheelCommands()
bool RobotControlManager::ExportDualArmPlannedTrajectory(trajectory_msgs::msg::JointTrajectory& out) const
{
if (!arm_controller_enabled_ || !arm_controller_) return false;
return arm_controller_->ExportPlannedJointTrajectory(2, out);
}
const std::vector<double>& RobotControlManager::GetJointPositions() const
{
return jointPositions_;
}
const std::vector<double>& RobotControlManager::GetWheelPositions() const
{
return wheelPositions_;
}
const std::vector<double>& RobotControlManager::GetWheelCommandPositions() const
{
return wheelCommands_;
}
Float64MultiArray RobotControlManager::GetJointFeedback()
{
Float64MultiArray tempValues;
tempValues.data.resize(motionParams_.total_joints_);
for (size_t i = 0; i < jointPositions_.size(); i++)
{
tempValues.data[i] = jointPositions_[i];
}
return tempValues;
}
Float64MultiArray RobotControlManager::GetWheelFeedback()
{
Float64MultiArray tempValues;
tempValues.data.resize(motionParams_.wheel_joint_names_.size());
for (size_t i = 0; i < wheelPositions_.size(); i++)
{
double angle = wheelPositions_[i];
tempValues.data[i] = angle;
}
return tempValues;
}
double RobotControlManager::GetWheelRatio()
{
return wheel_controller_ ? wheel_controller_->GetWheelRatioInternal() : 1.0;
}
Float64MultiArray RobotControlManager::GetJointCommands()
double RobotControlManager::GetImuYawDifference() const
{
Float64MultiArray tempValues;
tempValues.data.resize(motionParams_.total_joints_);
for (size_t i = 0; i < jointCommands_.data.size(); i++)
{
tempValues.data[i] = jointCommands_.data[i];
if (gyroValues_.size() <= 2 || gyroInitValues_.size() <= 2) {
return 0.0;
}
return tempValues;
}
void RobotControlManager::CheckJointLimits(Float64MultiArray &tempJointValues)
{
// Check the joint limits using joint names
for (size_t i = 0; i < tempJointValues.data.size(); i++)
{
double max_limit = std::numeric_limits<double>::max();
double min_limit = std::numeric_limits<double>::lowest();
// 使用名称映射访问关节信息（如果映射已初始化）
if (joint_name_mapping_initialized_ && i < motionParams_.all_joint_names_.size())
{
const std::string& joint_name = motionParams_.all_joint_names_[i];
if (motionParams_.joints_info_map_.find(joint_name) != motionParams_.joints_info_map_.end())
{
const auto& info = motionParams_.joints_info_map_[joint_name];
max_limit = info.max_limit;
min_limit = info.min_limit;
}
}
else
{
// 如果映射未初始化，使用默认值（向后兼容）
// 注意：这应该不会发生，因为映射应该在首次收到JointState时初始化
max_limit = std::numeric_limits<double>::max();
min_limit = std::numeric_limits<double>::lowest();
}
if (tempJointValues.data[i] > max_limit)
{
tempJointValues.data[i] = max_limit;
}
if (tempJointValues.data[i] < min_limit)
{
tempJointValues.data[i] = min_limit;
}
}
}
bool isAllTrueManual(const std::vector<bool>& vec) {
for (bool element : vec) { // 范围 for 循环遍历每个元素
if (!element) { // 若存在 false，直接返回 false
return false;
}
}
return true; // 所有元素都是 true
}
std::vector<double> RobotControlManager::GetImuDifference()
{
std::vector<double> result;
result.resize(gyroValues_.size(), 0.0);
for (size_t i = 0; i < gyroValues_.size(); i++)
{
result[i] = gyroValues_[i] - gyroInitValues_[i];
}
std::cout << "get gyro init value : " << gyroInitValues_[2] << std::endl;
std::cout << "get gyro value : " << gyroValues_[2] << std::endl;
return result;
return gyroValues_[2] - gyroInitValues_[2];
}
void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg)
@@ -484,8 +313,6 @@ void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg)
{
gyroInitValues_ = gyroValues_;
isGyroInited_ = true;
std::cout << "IMU values : " << gyroInitValues_[0] << " " << gyroInitValues_[1] << " " << gyroInitValues_[2] << std::endl;
}
@@ -497,8 +324,6 @@ void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg)
gyroAccelerations_[1] = tempMsg.linear_acceleration.y;
gyroAccelerations_[2] = tempMsg.linear_acceleration.z;
// std::cout << "gyroValues_ : " << gyroValues_[0] << " " << gyroValues_[1] << " " << gyroValues_[2] << std::endl;
}
void RobotControlManager::QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
@@ -541,7 +366,8 @@ void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg)
if (motorPos_.motor_angle.size() != motionParams_.wheel_joint_names_.size())
{
std::cout << "wheel states size is not equal to wheel numbles" << std::endl;
RCLCPP_WARN(logger(), "Wheel states size mismatch (got %zu, expected %zu)",
motorPos_.motor_angle.size(), motionParams_.wheel_joint_names_.size());
return;
}
@@ -567,14 +393,14 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
{
if (!msg) {
std::cout << "UpdateJointStates: null msg" << std::endl;
RCLCPP_WARN(logger(), "UpdateJointStates: null msg");
return;
}
// JointState 基本一致性检查：name/position 必须等长
if (msg->name.size() != msg->position.size()) {
std::cout << "UpdateJointStates: name.size(" << msg->name.size()
<< ") != position.size(" << msg->position.size() << ")" << std::endl;
RCLCPP_WARN(logger(), "UpdateJointStates: name.size(%zu) != position.size(%zu)",
msg->name.size(), msg->position.size());
return;
}
@@ -585,9 +411,7 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
{
motionParams_.InitializeJointNameMapping(jointStates_.name);
joint_name_mapping_initialized_ = true;
jointInited_.resize(motionParams_.total_joints_, false);
jointCommands_.data.resize(motionParams_.total_joints_, 0.0);
std::cout << "Joint name mapping initialized with " << motionParams_.total_joints_ << " joints" << std::endl;
RCLCPP_INFO(logger(), "Joint name mapping initialized with %zu joints", motionParams_.total_joints_);
jointPositions_.resize(motionParams_.total_joints_);
jointVelocities_.resize(motionParams_.total_joints_);
jointEfforts_.resize(motionParams_.total_joints_);
@@ -623,22 +447,9 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
{
if (count > 500)
{
for (size_t i = 0; i<jointPositions_.size(); i++)
{
jointPositions_[i] = 0.0;
}
// 用已映射后的 jointPositions_ 初始化 command，并裁剪到关节限位内
jointCommands_.data = jointPositions_;
CheckJointLimits(jointCommands_);
isJointInitValueSet_ = 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 << jointStates_.name[i] << " " << jointStates_.position[i] << std::endl;
}
RCLCPP_INFO(logger(), "All joints are initialized");
}
count++;
}
@@ -716,7 +527,7 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
}
}
MotionParameters RobotControlManager::GetMotionParameters()
const MotionParameters& RobotControlManager::GetMotionParameters() const
{
return motionParams_;
}
@@ -748,64 +559,6 @@ bool RobotControlManager::RobotInitFinished()
return isJointInitValueSet_;
}
bool RobotControlManager::Stop(double dt)
{
AssignTempValues();
bool waistStopped = true;
bool legStopped = true;
bool wheelStopped = true;
if (waist_controller_)
{
waistStopped = waist_controller_->Stop(tempWaistCmd_, dt);
}
if (leg_controller_)
{
legStopped = leg_controller_->Stop(tempLegCmd_, dt);
}
if (wheel_controller_)
{
wheelStopped = wheel_controller_->Stop(tempWheelCmd_, dt);
}
UpdateJointCommands();
return waistStopped && legStopped && wheelStopped;
}
bool RobotControlManager::MoveLeg(double dt)
{
if (!leg_controller_)
{
return false;
}
AssignTempValues();
bool result = leg_controller_->MoveUp(tempLegCmd_, dt);
UpdateJointCommands();
return result;
}
bool RobotControlManager::MoveWaist(double dt)
{
if (!waist_controller_)
{
return false;
}
AssignTempValues();
bool result = waist_controller_->MoveWaist(tempWaistCmd_, dt);
UpdateJointCommands();
return result;
}
bool RobotControlManager::MoveWheel()
{
if (!wheel_controller_)
@@ -817,95 +570,12 @@ bool RobotControlManager::MoveWheel()
bool result = wheel_controller_->MoveWheel(tempWheelCmd_);
if (joint_name_mapping_initialized_) {
std::vector<size_t> wheel_indices = GetJointIndicesFromNames(motionParams_.wheel_joint_names_);
for (size_t i = 0; i < wheel_indices.size() && i < tempWheelCmd_.size(); i++)
{
if (wheel_indices[i] < wheelCommands_.data.size()) {
wheelCommands_.data[wheel_indices[i]] = tempWheelCmd_[i];
}
}
}
// wheelCommands_ is wheel-joint-order array
wheelCommands_ = tempWheelCmd_;
return result;
}
bool RobotControlManager::GoHome(double dt)
{
AssignTempValues();
if (waist_controller_ && !isWaistHomed_)
{
isWaistHomed_ = waist_controller_->GoHome(tempWaistCmd_, dt);
}
else if (!waist_controller_)
{
isWaistHomed_ = true; // 如果没有控制器，认为已完成
}
if (leg_controller_ && !isLegHomed_)
{
isLegHomed_ = leg_controller_->GoHome(tempLegCmd_, dt);
}
else if (!leg_controller_)
{
isLegHomed_ = true; // 如果没有控制器，认为已完成
}
UpdateJointCommands();
if (isWaistHomed_ && isLegHomed_)
{
isWaistHomed_ = false;
isLegHomed_ = false;
return true;
}
else
{
return false;
}
}
void RobotControlManager::JogAxis(size_t axis, int direction)
{
if(axis > motionParams_.total_joints_)
{
return;
}
// jog_step_size_ 已删除，使用默认值 1.0 度
const double JOG_STEP_SIZE = 1.0;
jointCommands_.data[axis] += direction * JOG_STEP_SIZE;
}
void RobotControlManager::AssignTempValues()
{
if (!joint_name_mapping_initialized_) {
return;
}
std::vector<size_t> waist_indices = GetJointIndicesFromNames(motionParams_.waist_joint_names_);
tempWaistCmd_.resize(waist_indices.size());
for (size_t i = 0; i < waist_indices.size(); i++)
{
if (waist_indices[i] < jointCommands_.data.size()) {
tempWaistCmd_[i] = jointCommands_.data[waist_indices[i]];
}
}
std::vector<size_t> leg_indices = GetJointIndicesFromNames(motionParams_.leg_joint_names_);
tempLegCmd_.resize(leg_indices.size());
for (size_t i = 0; i < leg_indices.size(); i++)
{
if (leg_indices[i] < jointCommands_.data.size()) {
tempLegCmd_[i] = jointCommands_.data[leg_indices[i]];
}
}
}
std::vector<size_t> RobotControlManager::GetJointIndicesFromNames(const std::vector<std::string>& joint_names) const
{
std::vector<size_t> indices;
@@ -917,26 +587,3 @@ std::vector<size_t> RobotControlManager::GetJointIndicesFromNames(const std::vec
}
return indices;
}
void RobotControlManager::UpdateJointCommands()
{
if (!joint_name_mapping_initialized_) {
return;
}
std::vector<size_t> waist_indices = GetJointIndicesFromNames(motionParams_.waist_joint_names_);
for (size_t i = 0; i < waist_indices.size() && i < tempWaistCmd_.size(); i++)
{
if (waist_indices[i] < jointCommands_.data.size()) {
jointCommands_.data[waist_indices[i]] = tempWaistCmd_[i];
}
}
std::vector<size_t> leg_indices = GetJointIndicesFromNames(motionParams_.leg_joint_names_);
for (size_t i = 0; i < leg_indices.size() && i < tempLegCmd_.size(); i++)
{
if (leg_indices[i] < jointCommands_.data.size()) {
jointCommands_.data[leg_indices[i]] = tempLegCmd_[i];
}
}
}

231
src/robot_control/src/core/robot_control_node.cpp
View File

@@ -5,72 +5,42 @@
#include "core/motion_parameters.hpp"
#include "core/common_enum.hpp"
#include <chrono>
#include <future>
using namespace robot_control;
using namespace std;
using namespace std_msgs::msg;
namespace fs = std::filesystem;
RobotControlNode::RobotControlNode() : Node("robot_control_node")
{
RCLCPP_INFO(this->get_logger(), "robot_control_node Node is creating");
is_robot_enabled_ = false;
// 初始化数据文件（设置路径，确保目录存在）
#if RECORD_FLAG
data_file_path_ = "/home/nvidia/disk/ros2_joint_data.txt"; // 数据保存路径
fs::path file_path(data_file_path_);
fs::create_directories(file_path.parent_path()); // 自动创建父目录（如果不存在）
data_file_.open(data_file_path_, std::ios::trunc);
if (!data_file_.is_open()) {
} else {
// 写入表头（仅在文件为空时）
data_file_.seekp(0, std::ios::end); // 移动到文件末尾
}
#endif
// 创建发布器和客户端（ActionManager 需要）
motorCmdPub_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
motorParamClient_ = this->create_client<MotorParam>("/motor_param");
jointTrajectoryPub_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>("/trajectory_controller/joint_trajectory", 10);
gpioPub_ = this->create_publisher<control_msgs::msg::DynamicInterfaceGroupValues>("/gpio_command_controller/commands", 10);
switch_client_ = create_client<controller_manager_msgs::srv::SwitchController>("/controller_manager/switch_controller");
imuMsgSub_ = this->create_subscription<ImuMsg>("/openzen/imu_msg", 10,std::bind(&RobotControlNode::ImuMsgCallback, this, std::placeholders::_1));
jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1));
wheelStatesSub_ = this->create_subscription<MotorPos>("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, this, std::placeholders::_1));
joyCommandSub_ = this->create_subscription<std_msgs::msg::String>("/gamepad_msg", 10,std::bind(&RobotControlNode::JoyCommandCallback, this, std::placeholders::_1));
controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(CYCLE_TIME),std::bind(&RobotControlNode::ControlLoop, this));
// 初始化 RemoteController（需要在 ActionManager 之前，因为 ActionManager 需要 is_jog_mode_func）
// 初始化 RemoteController（用于 gamepad/jog 相关逻辑）
remoteController_ = std::make_unique<RemoteController>(
this,
robotControlManager_);
// 初始化 ActionManager（在创建发布器和客户端之后）
// 注意：is_jog_mode_func 使用一个临时的 lambda，稍后会被 RemoteController 覆盖
auto is_jog_mode_func = [this]() -> bool {
return remoteController_ ? remoteController_->IsJogMode() : false;
};
action_manager_ = std::make_unique<ActionManager>(
this,
robotControlManager_,
is_jog_mode_func,
motorCmdPub_,
motorParamClient_);
action_manager_->initialize();
bool ret=activateController("trajectory_controller");
if(ret)
{
std::cout << "切换控制器成功!" << std::endl;
}
else
{
std::cout << "切换控制器失败!" << std::endl;
const bool ret = activateController("trajectory_controller");
if (ret) {
RCLCPP_INFO(this->get_logger(), "Activated controller: trajectory_controller");
} else {
RCLCPP_ERROR(this->get_logger(), "Failed to activate controller: trajectory_controller");
}
motor_enable(0, 1);
@@ -80,184 +50,11 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
} else {
RCLCPP_WARN(this->get_logger(), "MoveIt initialization failed or arm controller not enabled");
}
lastTime_ = this->now(); // 初始化时间
std::cout << "RobotFsm Node is created finished!" << std::endl;
RCLCPP_INFO(this->get_logger(), "robot_control_node created");
}
RobotControlNode::~RobotControlNode()
{
if (data_file_.is_open()) {
data_file_.close();
}
}
// ==================== 控制循环 ====================
void RobotControlNode::ControlLoop()
{
// 计算时间步长
rclcpp::Time currentTime = this->now();
double dt_sec = (currentTime - lastTime_).seconds();
lastTime_ = currentTime;
// 处理停止请求
if (remoteController_ && remoteController_->HasStopRequest())
{
action_manager_->set_move_home_executing(false);
action_manager_->set_move_leg_executing(false);
action_manager_->set_move_waist_executing(false);
action_manager_->set_move_wheel_executing(false);
remoteController_->SetJogMode(false);
if (robotControlManager_.Stop(dt_sec))
{
remoteController_->ClearStopRequest();
}
}
if (action_manager_->is_move_home_executing())
{
if(robotControlManager_.GoHome(dt_sec))
{
robotControlManager_.SetJogWheel(false);
action_manager_->set_move_home_executing(false);
robotControlManager_.WheelReset();
robotControlManager_.ImuReset();
}
}
if (action_manager_->is_move_leg_executing())
{
if(robotControlManager_.MoveLeg(dt_sec))
{
action_manager_->set_move_leg_executing(false);
}
}
if (action_manager_->is_move_waist_executing())
{
if(robotControlManager_.MoveWaist(dt_sec))
{
action_manager_->set_move_waist_executing(false);
}
}
if (action_manager_->is_move_wheel_executing())
{
if(robotControlManager_.MoveWheel())
{
action_manager_->set_move_wheel_executing(false);
}
}
if (action_manager_->is_dual_arm_executing())
{
if(robotControlManager_.MoveArm(dt_sec))
{
action_manager_->set_dual_arm_executing(false);
}
}
if (robotControlManager_.RobotInitFinished())
{
if(!is_robot_enabled_)
{
motor_reset_fault_all();
is_robot_enabled_ = true;
motor_enable(0, 1);
}
else
{
Publish_joint_trajectory();
}
}
}
void RobotControlNode::Publish_joint_trajectory()
{
std_msgs::msg::Float64MultiArray cmd_msg;
if (remoteController_ && remoteController_->IsJogMode())
{
robotControlManager_.JogAxis(remoteController_->GetJogIndex(), remoteController_->GetJogDirection());
cmd_msg = robotControlManager_.GetJointCommands();
}
else
{
cmd_msg = robotControlManager_.GetJointCommands();
}
#if RECORD_FLAG
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(); // 强制刷新
}
#endif
// 创建 JointTrajectory 消息
trajectory_msgs::msg::JointTrajectory trajectory_msg;
// joint_trajectory_controller 要求：要么 header.stamp=0 表示立即开始，
// 要么 header.stamp 是未来时间；同时 points[0].time_from_start 必须为正且单调递增。
trajectory_msg.header.stamp.sec = 0;
trajectory_msg.header.stamp.nanosec = 0; // start ASAP
const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
size_t total_joints = motion_params.total_joints_;
if (!motion_params.all_joint_names_.empty() && motion_params.all_joint_names_.size() == total_joints)
{
trajectory_msg.joint_names = motion_params.all_joint_names_;
}
else
{
std::cout << "joint names not set!" << std::endl;
return;
}
// 创建轨迹点
trajectory_msgs::msg::JointTrajectoryPoint point;
point.positions.resize(total_joints);
point.velocities.resize(total_joints, 0.0);
point.accelerations.resize(total_joints, 0.0);
for (size_t i = 0; i < total_joints; ++i)
{
point.positions[i] = cmd_msg.data[i];
}
// 单点轨迹也需要一个 >0 的 time_from_start（用控制周期）
const double dt = static_cast<double>(CYCLE_TIME) / 1000.0;
point.time_from_start.sec = static_cast<int32_t>(dt);
point.time_from_start.nanosec =
static_cast<uint32_t>((dt - static_cast<double>(point.time_from_start.sec)) * 1e9);
trajectory_msg.points.push_back(point);
// std::cout << "Publish Joint Trajectory" << std::endl;
// std::cout << "trajectory_msg.points.size(): " << trajectory_msg.points.size() << std::endl;
// std::cout << "trajectory_msg.points[0].positions.size(): " << trajectory_msg.points[0].positions.size() << std::endl;
// std::cout << "trajectory_msg.points[0].positions[0]: " << trajectory_msg.points[0].positions[0] << std::endl;
// std::cout << "trajectory_msg.points[0].positions[1]: " << trajectory_msg.points[0].positions[1] << std::endl;
// std::cout << "trajectory_msg.points[0].positions[2]: " << trajectory_msg.points[0].positions[2] << std::endl;
// std::cout << "trajectory_msg.points[0].positions[3]: " << trajectory_msg.points[0].positions[3] << std::endl;
// std::cout << "trajectory_msg.points[0].positions[4]: " << trajectory_msg.points[0].positions[4] << std::endl;
// std::cout << "trajectory_msg.points[0].positions[5]: " << trajectory_msg.points[0].positions[5] << std::endl;
// std::cout << "trajectory_msg.points[0].positions[6]: " << trajectory_msg.points[0].positions[6] << std::endl;
// std::cout << "trajectory_msg.points[0].positions[7]: " << trajectory_msg.points[0].positions[7] << std::endl;
// std::cout << "trajectory_msg.points[0].positions[8]: " << trajectory_msg.points[0].positions[8] << std::endl;
// std::cout << "trajectory_msg.points[0].positions[9]: " << trajectory_msg.points[0].positions[9] << std::endl;
// std::cout << "trajectory_msg.points[0].positions[10]: " << trajectory_msg.points[0].positions[10] << std::endl;
// std::cout << "trajectory_msg.points[0].positions[11]: " << trajectory_msg.points[0].positions[11] << std::endl;
// 发布关节轨迹
jointTrajectoryPub_->publish(trajectory_msg);
}
void RobotControlNode::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg)
@@ -271,17 +68,25 @@ void RobotControlNode::JoyCommandCallback(const std_msgs::msg::String::SharedPtr
void RobotControlNode::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
{
if (!msg) { // 检查消息是否有效
std::cout << "get null joint states!" << std::endl;
RCLCPP_WARN(this->get_logger(), "Received null JointState, ignoring");
return;
}
robotControlManager_.UpdateJointStates(msg);
// 去掉周期发布链路：当关节初始化完成后，单次执行电机 fault reset + enable
if (!is_robot_enabled_ && robotControlManager_.RobotInitFinished())
{
motor_reset_fault_all();
motor_enable(0, 1);
is_robot_enabled_ = true;
}
}
void RobotControlNode::ImuMsgCallback(const ImuMsg::SharedPtr cmd_msg)
{
if (!cmd_msg) { // 检查消息是否有效
std::cout << "get null imu msg!" << std::endl;
RCLCPP_WARN(this->get_logger(), "Received null IMU msg, ignoring");
return;
}
@@ -291,7 +96,7 @@ void RobotControlNode::ImuMsgCallback(const ImuMsg::SharedPtr cmd_msg)
void RobotControlNode::WheelStatesCallback(const MotorPos::SharedPtr cmd_msg)
{
if (!cmd_msg) { // 检查消息是否有效
std::cout << "get null wheel states!" << std::endl;
RCLCPP_WARN(this->get_logger(), "Received null wheel states, ignoring");
return;
}

17
src/robot_control/src/main.cpp
View File

@@ -1,26 +1,15 @@
#include <rclcpp/rclcpp.hpp>
#include <iostream>
#include "core/robot_control_node.hpp"
/**
* @brief 程序入口函数
* 功能：初始化ROS 2，创建状态机节点，启动自旋
*/
using namespace robot_control;
int main(int argc, char *argv[]) {
// 初始化ROS 2
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
// 创建状态机节点（智能指针管理）
auto robot_control_node = std::make_shared<RobotControlNode>();
auto robot_control_node = std::make_shared<robot_control::RobotControlNode>();
// 启动节点自旋（阻塞，处理回调和定时器）
rclcpp::spin(robot_control_node);
// 关闭ROS 2
rclcpp::shutdown();
return 0;
}

2
src/robot_control/src/utils/plot_joint_trajectory.py
View File

@@ -22,7 +22,7 @@ def plot_joint_trajectories(file_path):
line=line[:-1]
parts = list(map(float, line.split(',')))
timestamp = parts[0]
joints = parts[1:] # 第0个是时间戳，后面是关节数据
joints = parts[0:] # 第0个是时间戳，后面是关节数据
###parts = line.split(',')
###timestamp = parts[0]
####joints = parts[1:] # 第0个是时间戳，后面是关节数据

130
src/robot_control/src/utils/plot_trajectory_csv.py Normal file
View File

@@ -0,0 +1,130 @@
#!/usr/bin/env python3
"""
Plot joint trajectory from CSV file saved by follow_jt_executor.cpp
The CSV file contains time and position information for each joint.
"""
import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
import sys
import os
import glob
def plot_trajectory_from_csv(file_path):
"""
Read trajectory CSV file and plot time vs position for each joint.
Args:
file_path: Path to the CSV file
"""
# Check if file exists
if not os.path.exists(file_path):
print(f"Error: File does not exist - {file_path}")
return
# Read CSV file using pandas
try:
df = pd.read_csv(file_path)
except Exception as e:
print(f"Error reading CSV file: {e}")
return
# Check if required columns exist
if 'time' not in df.columns:
print("Error: CSV file must contain 'time' column")
return
# Get time column
time = df['time'].values
# Find all position columns (columns ending with '_position')
position_columns = [col for col in df.columns if col.endswith('_position')]
if not position_columns:
print("Warning: No position columns found in CSV file")
return
# Create figure with subplots
num_joints = len(position_columns)
# If many joints, use a grid layout
if num_joints <= 4:
rows, cols = num_joints, 1
fig, axes = plt.subplots(rows, cols, figsize=(10, 4*num_joints))
if num_joints == 1:
axes = [axes]
elif num_joints <= 8:
rows, cols = 2, 4
fig, axes = plt.subplots(rows, cols, figsize=(16, 8))
axes = axes.flatten()
else:
rows = (num_joints + 3) // 4
cols = 4
fig, axes = plt.subplots(rows, cols, figsize=(16, 4*rows))
axes = axes.flatten()
# Plot each joint
for idx, col in enumerate(position_columns):
joint_name = col.replace('_position', '')
position = df[col].values
ax = axes[idx] if idx < len(axes) else None
if ax is None:
continue
ax.plot(time, position, linewidth=2, label=joint_name)
ax.set_xlabel('Time (seconds)', fontsize=10)
ax.set_ylabel('Position (rad)', fontsize=10)
ax.set_title(f'{joint_name}', fontsize=12, fontweight='bold')
ax.grid(True, linestyle='--', alpha=0.7)
ax.legend(loc='best')
# Hide unused subplots
for idx in range(len(position_columns), len(axes)):
axes[idx].set_visible(False)
plt.suptitle('Joint Trajectory - Time vs Position', fontsize=14, fontweight='bold', y=0.995)
plt.tight_layout()
plt.show()
# Also create a combined plot with all joints on one figure
plt.figure(figsize=(12, 8))
for col in position_columns:
joint_name = col.replace('_position', '')
position = df[col].values
plt.plot(time, position, linewidth=2, label=joint_name, marker='o', markersize=2)
plt.xlabel('Time (seconds)', fontsize=12)
plt.ylabel('Position (rad)', fontsize=12)
plt.title('All Joints Trajectory - Combined View', fontsize=14, fontweight='bold')
plt.grid(True, linestyle='--', alpha=0.7)
plt.legend(bbox_to_anchor=(1.05, 1), loc='upper left', fontsize=10)
plt.tight_layout()
plt.show()
def main():
"""Main function"""
if len(sys.argv) > 1:
# Use file path from command line argument
file_path = sys.argv[1]
else:
# Try to find the most recent trajectory file in /tmp
pattern = "/tmp/trajectory_20260117_121313.csv"
files = glob.glob(pattern)
if not files:
print(f"Error: No trajectory CSV files found in /tmp")
print(f"Usage: {sys.argv[0]} <csv_file_path>")
print(f"Or place a trajectory_*.csv file in /tmp/")
return
# Get the most recent file
file_path = max(files, key=os.path.getmtime)
print(f"Using most recent file: {file_path}")
plot_trajectory_from_csv(file_path)
if __name__ == "__main__":
main()

65
src/robot_control/src/utils/plot_trajectory_new.py Normal file
View File

@@ -0,0 +1,65 @@
import numpy as np
import matplotlib.pyplot as plt
import os
def plot_joint_trajectories(file_path):
# 检查文件是否存在
if not os.path.exists(file_path):
print(f"错误：文件不存在 - {file_path}")
return
# 读取数据
timestamps = []
joint_data = [] # 存储所有关节数据，格式：[ [关节1数据], [关节2数据], ... ]
with open(file_path, 'r') as f:
for line in f:
line = line.strip()
if not line:
continue
# 解析一行数据（时间戳 + 所有关节值）
parts = list(map(float, line.split(',')))
timestamp = parts[0]
joints = parts[1:] # 第0个是时间戳，后面是关节数据
timestamps.append(timestamp)
# 初始化关节数据列表（首次读取时）
if not joint_data:
joint_data = [[] for _ in range(len(joints))]
# 按关节索引存储数据
for i, val in enumerate(joints):
if i < len(joint_data): # 避免索引越界
joint_data[i].append(val)
# 转换为相对时间（以第一个时间戳为起点）
if not timestamps:
print("警告：未读取到有效数据")
return
start_time = timestamps[0]
relative_times = [t - start_time for t in timestamps]
# 绘制所有关节轨迹（最多显示12个关节，避免图过于拥挤）
num_joints = len(joint_data)
num_plots = min(num_joints, 12) # 超过12个关节只显示前12个
plt.figure(figsize=(12, 8))
for i in range(num_plots):
plt.plot(joint_data[i], label=f'关节 {i+1}')
# 图形配置
plt.xlabel('时间 (秒)')
plt.ylabel('关节位置')
plt.title('所有关节轨迹曲线')
plt.grid(True, linestyle='--', alpha=0.7)
plt.legend(bbox_to_anchor=(1.05, 1), loc='upper left') # 图例放右侧
plt.tight_layout() # 自动调整布局
plt.show()
if __name__ == "__main__":
# 数据文件路径（与C++代码中的路径一致）
data_file = "/home/nvidia/disk/ros2_joint_data.txt"
plot_joint_trajectories(data_file)

8
src/robot_control/src/utils/trapezoidal_trajectory.cpp
View File

@@ -150,11 +150,6 @@ void TrapezoidalTrajectory::calculateTrajectoryParams()
acceleration_time_ = axis_accel_time[i];
deceleration_time_ = axis_decel_time[i];
cruise_time_ = axis_cruise_time[i];
// std::cout << "total time : " << total_time_ << std::endl;
// std::cout << "acceleration_time_ : " << acceleration_time_ << std::endl;
// std::cout << "deceleration_time_ : " << deceleration_time_ << std::endl;
// std::cout << "cruise_time_ : " << cruise_time_ << std::endl;
}
}
@@ -177,7 +172,8 @@ void TrapezoidalTrajectory::calculateTrajectoryParams()
continue;
}
//TODO: 最大加速度需要保持一致,否则这里的计算会有问题.
// Note: This computes per-axis cruise velocities under a synchronized time base.
// If per-axis acceleration limits differ widely, the profile may be conservative or violate assumptions.
cruise_velocity_[i] = dist / (acceleration_time_ + cruise_time_);
acceleration_[i] = cruise_velocity_[i] / acceleration_time_;
deceleration_[i] = cruise_velocity_[i] / deceleration_time_;