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

dual arm motion control optimization

Browse Source
This commit is contained in:
Your Name
2026-02-26 09:17:34 +08:00
parent a721198fe0
commit 64ee485100
11 changed files with 821 additions and 1238 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/robot_control/CMakeLists.txt
View File

@@ -1,6 +1,6 @@
cmake_minimum_required(VERSION 3.16) # 提升最低版本以匹配ROS 2推荐版本
project(robot_control)
set(ROS2_HUMBLE_PATH /home/nvidia/disk/ros/humble)
set(ROS2_HUMBLE_PATH /opt/ros/humble)
# 手动指定 eigen_stl_containers 的cmake配置文件路径
set(eigen_stl_containers_DIR ${ROS2_HUMBLE_PATH}/share/eigen_stl_containers/cmake)
# 手动指定 eigen3 相关依赖路径 (moveit_core同时依赖这个)
@@ -57,7 +57,6 @@ set(SOURCES
src/services/motor_service.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

257
src/robot_control/REFACTORING_GUIDE.md
View File

@@ -1,257 +0,0 @@
# 重构指南
## 已完成的工作
1. ✅ 创建了新的目录结构（include/actions, include/core, include/controllers, include/utils）
2. ✅ 创建了 ActionManager 类（头文件和实现文件）
3. ✅ 创建了 ControllerFactory 类（头文件和实现文件）
## 待完成的工作
### 步骤 1: 移动文件到新目录结构
由于文件依赖关系复杂，建议按以下顺序移动：
#### 1.1 移动工具类文件（utils）
这些文件依赖最少：
```bash
# 移动到 utils 目录
mv include/trapezoidal_trajectory.hpp include/utils/
mv src/trapezoidal_trajectory.cpp src/utils/
mv include/robot_model.hpp include/utils/
mv src/robot_model.cpp src/utils/
mv include/robot_kinematics.hpp include/utils/
mv src/robot_kinematics.cpp src/utils/
mv include/urdf_parser.hpp include/utils/
mv src/urdf_parser.cpp src/utils/
mv include/extended_kalman_filter.hpp include/utils/
mv src/extended_kalman_filter.cpp src/utils/
mv include/deceleration_planner.hpp include/utils/
```
#### 1.2 移动核心文件（core）
```bash
# 移动核心枚举和参数
mv include/common_enum.hpp include/core/
mv include/motion_parameters.hpp include/core/
# 移动管理器
mv include/robot_control_manager.hpp include/core/
mv src/robot_control_manager.cpp src/core/
# 移动节点（稍后修改）
# mv include/robot_control_node.hpp include/core/
# mv src/robot_control_node.cpp src/core/
```
#### 1.3 移动控制器文件（controllers）
```bash
# 移动控制器基类
mv include/control_base.hpp include/controllers/
mv src/control_base.cpp src/controllers/
# 移动具体控制器
mv include/leg_control.hpp include/controllers/
mv src/leg_control.cpp src/controllers/
mv include/arm_control.hpp include/controllers/
mv src/arm_control.cpp src/controllers/
mv include/wheel_control.hpp include/controllers/
mv src/wheel_control.cpp src/controllers/
mv include/waist_control.hpp include/controllers/
mv src/waist_control.cpp src/controllers/
```
### 步骤 2: 更新包含路径
所有文件移动后，需要更新包含路径。使用以下脚本批量替换：
```bash
# 在项目根目录执行
find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"control_base\.hpp"|"controllers/control_base.hpp"|g' {} +
find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"leg_control\.hpp"|"controllers/leg_control.hpp"|g' {} +
find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"arm_control\.hpp"|"controllers/arm_control.hpp"|g' {} +
find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"wheel_control\.hpp"|"controllers/wheel_control.hpp"|g' {} +
find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"waist_control\.hpp"|"controllers/waist_control.hpp"|g' {} +
find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"motion_parameters\.hpp"|"core/motion_parameters.hpp"|g' {} +
find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"common_enum\.hpp"|"core/common_enum.hpp"|g' {} +
find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"robot_control_manager\.hpp"|"core/robot_control_manager.hpp"|g' {} +
find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"trapezoidal_trajectory\.hpp"|"utils/trapezoidal_trajectory.hpp"|g' {} +
find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"robot_model\.hpp"|"utils/robot_model.hpp"|g' {} +
```
### 步骤 3: 修改 RobotControlManager 支持动态控制器加载
在 `include/core/robot_control_manager.hpp` 中：
1. 添加控制器工厂的包含：
```cpp
#include "core/controller_factory.hpp"
```
2. 修改私有成员，使用智能指针和可选控制器：
```cpp
private:
// 控制器（使用智能指针，支持可选加载）
std::unique_ptr<LegControl> leg_controller_;
std::unique_ptr<WheelControl> wheel_controller_;
std::unique_ptr<WaistControl> waist_controller_;
std::unique_ptr<ArmControl> arm_controller_; // 可选
// 控制器启用标志
bool leg_controller_enabled_;
bool wheel_controller_enabled_;
bool waist_controller_enabled_;
bool arm_controller_enabled_;
```
3. 修改构造函数，添加控制器配置参数：
```cpp
RobotControlManager(const std::vector<std::string>& enabled_controllers = {"leg", "wheel", "waist"});
```
4. 在 `init()` 函数中使用 ControllerFactory 创建控制器：
```cpp
void RobotControlManager::init(const std::vector<std::string>& enabled_controllers)
{
// ... 其他初始化代码 ...
// 动态加载控制器
if (ControllerFactory::is_controller_enabled(ControllerType::LEG, enabled_controllers))
{
leg_controller_ = std::unique_ptr<LegControl>(
static_cast<LegControl*>(ControllerFactory::create_controller(
ControllerType::LEG, motionParams_).release()));
leg_controller_enabled_ = true;
}
if (ControllerFactory::is_controller_enabled(ControllerType::WHEEL, enabled_controllers))
{
wheel_controller_ = std::unique_ptr<WheelControl>(
static_cast<WheelControl*>(ControllerFactory::create_controller(
ControllerType::WHEEL, motionParams_).release()));
wheel_controller_enabled_ = true;
}
// ... 类似地处理其他控制器 ...
}
```
### 步骤 4: 修改 RobotControlNode 使用 ActionManager
在 `include/core/robot_control_node.hpp` 中：
1. 添加 ActionManager 包含：
```cpp
#include "actions/action_manager.hpp"
```
2. 移除所有 action 相关的成员变量和方法声明
3. 添加 ActionManager 成员：
```cpp
private:
std::unique_ptr<ActionManager> action_manager_;
```
4. 在构造函数中初始化 ActionManager：
```cpp
RobotControlNode::RobotControlNode() : Node("robot_control_node")
{
// ... 其他初始化代码 ...
// 初始化 ActionManager
auto is_jog_mode_func = [this]() -> bool { return isJogMode_; };
action_manager_ = std::make_unique<ActionManager>(
this,
robotControlManager_,
is_jog_mode_func,
motorCmdPub_,
motorParamClient_);
action_manager_->initialize();
}
```
5. 在 ControlLoop 中使用 ActionManager 的状态：
```cpp
void RobotControlNode::ControlLoop()
{
// ...
if (action_manager_->is_move_home_executing())
{
if(robotControlManager_.GoHome(dt_sec))
{
robotControlManager_.SetJogWheel(false);
action_manager_->set_move_home_executing(false);
// Note: WheelReset/ImuReset were removed during refactor; state is handled internally now.
}
}
// ... 类似地处理其他 action ...
}
```
### 步骤 5: 更新 CMakeLists.txt
1. 更新源文件路径：
```cmake
set(SOURCES
src/core/robot_control_node.cpp
src/core/robot_control_manager.cpp
src/core/controller_factory.cpp
src/actions/action_manager.cpp
src/controllers/control_base.cpp
src/controllers/leg_control.cpp
src/controllers/arm_control.cpp
src/controllers/wheel_control.cpp
src/controllers/waist_control.cpp
src/utils/trapezoidal_trajectory.cpp
src/main.cpp
)
```
2. 更新头文件包含目录（如果需要）：
```cmake
include_directories(
include
${EIGEN3_INCLUDE_DIRS}
)
```
### 步骤 6: 测试和调试
1. 编译项目：
```bash
cd /home/demo/hive_core_workspace/hivecore_robot_os1
colcon build --packages-select robot_control
```
2. 修复编译错误（主要是包含路径问题）
3. 运行测试确保功能正常
## 注意事项
1. **文件移动顺序很重要**：先移动依赖少的文件，再移动依赖多的文件
2. **包含路径更新**：移动文件后必须更新所有包含路径
3. **控制器动态加载**：确保在使用控制器前检查是否启用
4. **测试**：每完成一步都要测试编译，避免错误累积
## 建议的重构顺序
1. 移动 utils 文件并更新路径
2. 移动 controllers 文件并更新路径
3. 移动 core 文件（除了 robot_control_node）并更新路径
4. 修改 RobotControlManager 支持动态加载
5. 移动和修改 robot_control_node
6. 更新 CMakeLists.txt
7. 测试编译和运行

54
src/robot_control/include/actions/dual_arm_action.hpp
View File

@@ -1,6 +1,8 @@
#pragma once
#include <atomic>
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <vector>
@@ -10,7 +12,6 @@
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "actions/action_context.hpp"
#include "actions/follow_jt_executor.hpp"
namespace robot_control
{
@@ -22,6 +23,7 @@ public:
using DualArm = interfaces::action::DualArm;
using GoalHandleDualArm = rclcpp_action::ServerGoalHandle<DualArm>;
using FollowJTA = control_msgs::action::FollowJointTrajectory;
using GoalHandleFollowJTA = rclcpp_action::ClientGoalHandle<FollowJTA>;
explicit DualArmAction(const ActionContext& ctx);
@@ -56,17 +58,48 @@ private:
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 publish_planning_feedback_(
const std::shared_ptr<GoalHandleDualArm>& goal_handle,
const std::shared_ptr<DualArm::Feedback>& feedback) const;
void fill_feedback_from_fjt_(
const FollowJTA::Feedback& fjt_fb,
double trajectory_total_time_sec_(
const trajectory_msgs::msg::JointTrajectory& traj) const;
void split_dual_arm_trajectory_(
const MotionParameters& motion_params,
size_t arm_dof,
const trajectory_msgs::msg::JointTrajectory& traj,
trajectory_msgs::msg::JointTrajectory* left,
trajectory_msgs::msg::JointTrajectory* right) const;
void execute_single_arm_(
const std::shared_ptr<GoalHandleDualArm>& goal_handle,
const std::shared_ptr<const DualArm::Goal>& goal,
const std::shared_ptr<DualArm::Feedback>& feedback,
const std::shared_ptr<DualArm::Result>& result,
double acceleration_scaling);
void execute_dual_arm_(
const std::shared_ptr<GoalHandleDualArm>& goal_handle,
const std::shared_ptr<const DualArm::Goal>& goal,
const std::shared_ptr<DualArm::Feedback>& feedback,
const std::shared_ptr<DualArm::Result>& result,
const MotionParameters& motion_params,
double acceleration_scaling);
bool wait_for_execution_(
const std::shared_ptr<GoalHandleDualArm>& goal_handle,
const std::shared_ptr<DualArm::Feedback>& feedback,
const std::shared_ptr<DualArm::Result>& result,
double total_time_sec,
DualArm::Feedback* out) const;
const std::function<bool()>& is_done,
const std::function<void()>& cancel_actions,
const std::string& cancel_message,
bool* canceled) const;
void save_trajectory_to_file_(
const trajectory_msgs::msg::JointTrajectory& traj,
const std::string& tag) const;
ActionContext ctx_;
// 整体 DualArm 执行标志（用于双臂协同运动场景）
@@ -83,8 +116,9 @@ private:
rclcpp_action::Client<FollowJTA>::SharedPtr left_fjt_client_;
rclcpp_action::Client<FollowJTA>::SharedPtr right_fjt_client_;
// 如需更复杂的执行逻辑（软停止等），可以复用 FollowJTExecutor；当前实现未使用。
std::unique_ptr<FollowJTExecutor> follow_executor_;
bool save_trajectory_enabled_{true};
bool trajectory_execution_enabled_{false};
};
} // namespace robot_control

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

@@ -1,66 +0,0 @@
#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

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

@@ -15,6 +15,7 @@
#include "geometry_msgs/msg/pose.hpp"
#include "moveit/move_group_interface/move_group_interface.h"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "utils/s_curve_trajectory.hpp"
// 前向声明
namespace rclcpp {
@@ -249,11 +250,26 @@ namespace robot_control
*/
bool timeParameterizeAndStore_(
uint8_t arm_id,
moveit::planning_interface::MoveGroupInterface& move_group,
moveit::planning_interface::MoveGroupInterface::Plan& plan,
double velocity_scaling,
double acceleration_scaling);
/**
* @brief 基于S型曲线对 MoveIt 轨迹进行时间参数化并重采样
* @param arm_id 手臂ID
* @param in_traj 输入轨迹（仅位置）
* @param out_traj 输出轨迹（包含 time/pos/vel/acc）
* @param velocity_scaling 速度缩放因子
* @param acceleration_scaling 加速度缩放因子
* @return true 成功生成 S 型参数化轨迹
*/
bool sCurveTimeParameterize_(
uint8_t arm_id,
const trajectory_msgs::msg::JointTrajectory& in_traj,
trajectory_msgs::msg::JointTrajectory& out_traj,
double velocity_scaling,
double acceleration_scaling) const;
/**
* @brief 辅助函数：更新 joints_info_map_ 从单个轨迹点
* @param arm_id 手臂ID（0=左臂，1=右臂）

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

@@ -55,7 +55,8 @@ namespace robot_control
private:
// ==================== ROS 2 通信接口 ====================
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr gpioPub_; ///< GPIO发布器
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr left_gpio_pub_; ///< 左臂GPIO发布器
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr right_gpio_pub_; ///< 右臂GPIO发布器
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joyCommandSub_; ///< 手柄命令订阅器
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_; ///< 关节状态订阅器
rclcpp::Subscription<MotorPos>::SharedPtr wheelStatesSub_; ///< 轮子状态订阅器
@@ -118,17 +119,42 @@ namespace robot_control
/**
* @brief 设置电机 fault（参考 test_motor.cpp）
* @param id 电机/关节序号（0=全部；否则按 all_joint_names_ 的 index+1 语义）
* @param id 关节序号（0=全部；否则按 dual_arm_joint_names_ 的 index+1 语义）
* @param fault fault 值（通常 1 表示复位脉冲，0 表示释放）
*/
void motor_fault(int id, double fault);
/**
* @brief 设置电机 enable（参考 test_motor.cpp）
* @param id 电机/关节序号（0=全部；否则按 all_joint_names_ 的 index+1 语义）
* @param id 关节序号（0=全部；否则按 dual_arm_joint_names_ 的 index+1 语义）
* @param enable 使能值（1=使能，0=禁用）
*/
void motor_enable(int id, double enable);
/**
* @brief 根据 dual_arm_joint_names_ 拆分左右臂关节名称
*/
bool split_arm_joint_names_(
const MotionParameters& motion_params,
std::vector<std::string>& left,
std::vector<std::string>& right) const;
/**
* @brief 发布GPIO命令（fault/enable）
* @param pub 目标GPIO控制器发布器
* @param joints 关节名称列表（对应 interface_groups）
* @param interface_name 接口名称（fault 或 enable）
* @param id 关节序号（0=全部）
* @param value 指令值
* @param offset 该列表在全局双臂数组中的起始偏移
*/
void publish_gpio_command_(
const rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr& pub,
const std::vector<std::string>& joints,
const std::string& interface_name,
int id,
double value,
int offset) const;
};
}

855
src/robot_control/src/actions/dual_arm_action.cpp
View File

@@ -2,7 +2,14 @@
#include <thread>
#include <unordered_map>
#include <chrono>
#include <ctime>
#include <algorithm>
#include <fstream>
#include <iomanip>
#include <sstream>
#include "rclcpp/rclcpp.hpp"
#include "core/robot_control_manager.hpp"
#include "core/common_enum.hpp"
@@ -23,6 +30,28 @@ void DualArmAction::initialize()
ctx_.node, "/left_arm_controller/follow_joint_trajectory");
right_fjt_client_ = rclcpp_action::create_client<FollowJTA>(
ctx_.node, "/right_arm_controller/follow_joint_trajectory");
const std::string save_param = "save_trajectory_enabled";
try {
ctx_.node->declare_parameter<bool>(save_param, save_trajectory_enabled_);
} catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException&) {
// Parameter already declared elsewhere; keep going.
}
if (ctx_.node->get_parameter(save_param, save_trajectory_enabled_)) {
RCLCPP_INFO(ctx_.node->get_logger(),
"save_trajectory_enabled=%s", save_trajectory_enabled_ ? "true" : "false");
}
const std::string exec_param = "arm_trajectory_execution_enabled";
try {
ctx_.node->declare_parameter<bool>(exec_param, trajectory_execution_enabled_);
} catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException&) {
// Parameter already declared elsewhere; keep going.
}
if (ctx_.node->get_parameter(exec_param, trajectory_execution_enabled_)) {
RCLCPP_INFO(ctx_.node->get_logger(),
"arm_trajectory_execution_enabled=%s", trajectory_execution_enabled_ ? "true" : "false");
}
}
server_ = rclcpp_action::create_server<DualArm>(
@@ -285,406 +314,271 @@ bool DualArmAction::export_and_normalize_trajectory_(
return true;
}
void DualArmAction::smooth_trajectory_start_end_(
trajectory_msgs::msg::JointTrajectory* traj,
double max_acceleration,
double smooth_duration) const
void DualArmAction::publish_planning_feedback_(
const std::shared_ptr<GoalHandleDualArm>& goal_handle,
const std::shared_ptr<DualArm::Feedback>& feedback) 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) {
auto result = std::make_shared<DualArm::Result>();
result->success = false;
result->message = "DualArm not initialized (missing robot_control_manager)";
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_;
const double acceleration_scaling = 0.5;
// planning feedback
feedback->status = 0;
feedback->progress = 0.0;
feedback->joints_left.clear();
feedback->joints_right.clear();
goal_handle->publish_feedback(feedback);
}
// ==================== 单臂模式 ====================
const size_t arm_count = goal->arm_motion_params.size();
if (arm_count == 1) {
const auto& arm_param = goal->arm_motion_params[0];
const uint8_t arm_id = arm_param.arm_id;
if (arm_id != ArmMotionParams::ARM_LEFT && arm_id != ArmMotionParams::ARM_RIGHT) {
result->success = false;
result->message = "Invalid arm_id for single-arm goal";
goal_handle->abort(result);
return;
double DualArmAction::trajectory_total_time_sec_(
const trajectory_msgs::msg::JointTrajectory& traj) const
{
if (traj.points.empty()) {
return 0.0;
}
const auto& last_point = traj.points.back();
return static_cast<double>(last_point.time_from_start.sec) +
static_cast<double>(last_point.time_from_start.nanosec) * 1e-9;
}
void DualArmAction::split_dual_arm_trajectory_(
const MotionParameters& motion_params,
size_t arm_dof,
const trajectory_msgs::msg::JointTrajectory& traj,
trajectory_msgs::msg::JointTrajectory* left,
trajectory_msgs::msg::JointTrajectory* right) const
{
if (!left || !right) return;
left->header = traj.header;
right->header = traj.header;
left->joint_names.assign(
motion_params.dual_arm_joint_names_.begin(),
motion_params.dual_arm_joint_names_.begin() + arm_dof);
right->joint_names.assign(
motion_params.dual_arm_joint_names_.begin() + arm_dof,
motion_params.dual_arm_joint_names_.begin() + 2 * arm_dof);
left->points.clear();
right->points.clear();
left->points.reserve(traj.points.size());
right->points.reserve(traj.points.size());
for (const auto& p : traj.points) {
trajectory_msgs::msg::JointTrajectoryPoint lp;
trajectory_msgs::msg::JointTrajectoryPoint rp;
if (p.positions.size() >= 2 * arm_dof) {
lp.positions.assign(p.positions.begin(), p.positions.begin() + arm_dof);
rp.positions.assign(p.positions.begin() + arm_dof, p.positions.begin() + 2 * arm_dof);
}
if ((arm_id == 0 && !left_fjt_client_) || (arm_id == 1 && !right_fjt_client_)) {
result->success = false;
result->message = "FJT client for target arm is null";
goal_handle->abort(result);
return;
if (p.velocities.size() >= 2 * arm_dof) {
lp.velocities.assign(p.velocities.begin(), p.velocities.begin() + arm_dof);
rp.velocities.assign(p.velocities.begin() + arm_dof, p.velocities.begin() + 2 * arm_dof);
}
// 规划器占用检查（非阻塞）
bool expected = false;
if (!planner_busy_.compare_exchange_strong(expected, true)) {
result->success = false;
result->message = "MoveIt planner is busy, cannot plan another arm motion";
goal_handle->abort(result);
return;
if (p.accelerations.size() >= 2 * arm_dof) {
lp.accelerations.assign(p.accelerations.begin(), p.accelerations.begin() + arm_dof);
rp.accelerations.assign(p.accelerations.begin() + arm_dof, p.accelerations.begin() + 2 * arm_dof);
}
lp.time_from_start = p.time_from_start;
rp.time_from_start = p.time_from_start;
// 使用单臂规划接口
ArmMotionParams arm_params = arm_param;
left->points.push_back(std::move(lp));
right->points.push_back(std::move(rp));
}
}
if (!ctx_.robot_control_manager->PlanArmMotion(
arm_params,
goal->velocity_scaling,
acceleration_scaling)) {
planner_busy_.store(false);
result->success = false;
result->message = "PlanArmMotion failed for single arm";
goal_handle->abort(result);
return;
}
planner_busy_.store(false);
trajectory_msgs::msg::JointTrajectory traj_single;
if (!ctx_.robot_control_manager->ExportArmPlannedTrajectory(arm_id, traj_single)) {
result->success = false;
result->message = "ExportArmPlannedTrajectory failed";
goal_handle->abort(result);
return;
}
auto& fjt_client = (arm_id == 0) ? left_fjt_client_ : right_fjt_client_;
if (!fjt_client->wait_for_action_server(std::chrono::seconds(3))) {
result->success = false;
result->message = "FollowJointTrajectory action server not available for target arm";
goal_handle->abort(result);
return;
}
FollowJTA::Goal goal_msg;
goal_msg.trajectory = traj_single;
auto send_future = fjt_client->async_send_goal(goal_msg);
if (send_future.wait_for(std::chrono::seconds(5)) != std::future_status::ready) {
result->success = false;
result->message = "Timeout waiting FollowJointTrajectory goal acceptance for target arm";
goal_handle->abort(result);
return;
}
auto goal_handle_arm = send_future.get();
if (!goal_handle_arm) {
result->success = false;
result->message = "FollowJointTrajectory goal rejected for target arm";
goal_handle->abort(result);
return;
}
auto res_future = fjt_client->async_get_result(goal_handle_arm);
// 标记对应手臂正在执行
if (arm_id == 0) executing_left_.store(true);
else executing_right_.store(true);
// 使用时间进度估计执行完成
double total_time = 0.0;
if (!traj_single.points.empty()) {
const auto& last_point = traj_single.points.back();
total_time =
static_cast<double>(last_point.time_from_start.sec) +
static_cast<double>(last_point.time_from_start.nanosec) * 1e-9;
}
double total_time_sec = total_time;
rclcpp::Rate loop_rate(50.0);
auto start_time = ctx_.node->now();
while (rclcpp::ok()) {
if (goal_handle->is_canceling()) {
(void)fjt_client->async_cancel_goal(goal_handle_arm);
if (arm_id == 0) executing_left_.store(false);
else executing_right_.store(false);
result->success = false;
result->message = "Single-arm goal canceled";
goal_handle->canceled(result);
return;
}
auto now = ctx_.node->now();
const double elapsed =
(now.seconds() - start_time.seconds());
double prog = 1.0;
if (total_time_sec > 1e-6) {
prog = std::min(1.0, std::max(0.0, elapsed / total_time_sec));
}
feedback->status = 1;
feedback->progress = prog;
goal_handle->publish_feedback(feedback);
bool done = (res_future.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready);
if (elapsed >= total_time_sec && done) {
break;
}
loop_rate.sleep();
}
if (arm_id == 0) executing_left_.store(false);
else executing_right_.store(false);
if (!rclcpp::ok()) {
result->success = false;
result->message = "ROS shutdown during single-arm execution";
goal_handle->abort(result);
return;
}
auto wrapped = res_future.get();
if (wrapped.code != rclcpp_action::ResultCode::SUCCEEDED) {
result->success = false;
result->message = "FollowJointTrajectory failed for target arm";
goal_handle->abort(result);
return;
}
feedback->status = 2;
feedback->progress = 1.0;
goal_handle->publish_feedback(feedback);
result->success = true;
result->message = "Single-arm motion succeeded";
result->final_progress = 1.0;
goal_handle->succeed(result);
void DualArmAction::save_trajectory_to_file_(
const trajectory_msgs::msg::JointTrajectory& traj,
const std::string& tag) const
{
if (!save_trajectory_enabled_) {
return;
}
if (traj.points.empty() || traj.joint_names.empty()) {
return;
}
// ==================== 双臂模式 ====================
if (arm_count != 2) {
auto now = std::chrono::system_clock::now();
auto time_t = std::chrono::system_clock::to_time_t(now);
std::stringstream ss;
ss << "/tmp/trajectory_" << tag << "_"
<< std::put_time(std::localtime(&time_t), "%Y%m%d_%H%M%S") << ".csv";
const std::string filename = ss.str();
std::ofstream file(filename);
if (!file.is_open()) {
RCLCPP_WARN(ctx_.node->get_logger(), "Failed to open file for trajectory saving: %s", filename.c_str());
return;
}
file << "time";
for (const auto& joint_name : traj.joint_names) {
file << "," << joint_name << "_position";
}
file << "\n";
for (const auto& point : traj.points) {
const double time_sec = point.time_from_start.sec + point.time_from_start.nanosec * 1e-9;
file << std::fixed << std::setprecision(6) << time_sec;
for (size_t i = 0; i < traj.joint_names.size(); ++i) {
const double pos = (i < point.positions.size()) ? point.positions[i] : 0.0;
file << "," << std::fixed << std::setprecision(8) << pos;
}
file << "\n";
}
file.close();
RCLCPP_INFO(ctx_.node->get_logger(), "Trajectory saved to file: %s", filename.c_str());
}
void DualArmAction::execute_single_arm_(
const std::shared_ptr<GoalHandleDualArm>& goal_handle,
const std::shared_ptr<const DualArm::Goal>& goal,
const std::shared_ptr<DualArm::Feedback>& feedback,
const std::shared_ptr<DualArm::Result>& result,
double acceleration_scaling)
{
const auto& arm_param = goal->arm_motion_params[0];
const uint8_t arm_id = arm_param.arm_id;
if (arm_id != ArmMotionParams::ARM_LEFT && arm_id != ArmMotionParams::ARM_RIGHT) {
result->success = false;
result->message = "Dual-arm mode requires two ArmMotionParams";
result->message = "Invalid arm_id for single-arm goal";
goal_handle->abort(result);
return;
}
if ((arm_id == 0 && !left_fjt_client_) || (arm_id == 1 && !right_fjt_client_)) {
result->success = false;
result->message = "FJT client for target arm is null";
goal_handle->abort(result);
return;
}
// 规划器占用检查（非阻塞）
bool expected = false;
if (!planner_busy_.compare_exchange_strong(expected, true)) {
result->success = false;
result->message = "MoveIt planner is busy, cannot plan another arm motion";
goal_handle->abort(result);
return;
}
// 使用单臂规划接口
ArmMotionParams arm_params = arm_param;
if (!ctx_.robot_control_manager->PlanArmMotion(
arm_params,
goal->velocity_scaling,
acceleration_scaling)) {
planner_busy_.store(false);
result->success = false;
result->message = "PlanArmMotion failed for single arm";
goal_handle->abort(result);
return;
}
planner_busy_.store(false);
trajectory_msgs::msg::JointTrajectory traj_single;
if (!ctx_.robot_control_manager->ExportArmPlannedTrajectory(arm_id, traj_single)) {
result->success = false;
result->message = "ExportArmPlannedTrajectory failed";
goal_handle->abort(result);
return;
}
save_trajectory_to_file_(traj_single, (arm_id == 0) ? "left_arm" : "right_arm");
if (!trajectory_execution_enabled_) {
result->success = false;
result->message = "Arm trajectory execution disabled by parameter: arm_trajectory_execution_enabled";
goal_handle->abort(result);
return;
}
auto& fjt_client = (arm_id == 0) ? left_fjt_client_ : right_fjt_client_;
if (!fjt_client->wait_for_action_server(std::chrono::seconds(3))) {
result->success = false;
result->message = "FollowJointTrajectory action server not available for target arm";
goal_handle->abort(result);
return;
}
FollowJTA::Goal goal_msg;
goal_msg.trajectory = traj_single;
auto send_future = fjt_client->async_send_goal(goal_msg);
if (send_future.wait_for(std::chrono::seconds(5)) != std::future_status::ready) {
result->success = false;
result->message = "Timeout waiting FollowJointTrajectory goal acceptance for target arm";
goal_handle->abort(result);
return;
}
auto goal_handle_arm = send_future.get();
if (!goal_handle_arm) {
result->success = false;
result->message = "FollowJointTrajectory goal rejected for target arm";
goal_handle->abort(result);
return;
}
auto res_future = fjt_client->async_get_result(goal_handle_arm);
// 标记对应手臂正在执行
if (arm_id == 0) executing_left_.store(true);
else executing_right_.store(true);
const double total_time_sec = trajectory_total_time_sec_(traj_single);
bool canceled = false;
if (!wait_for_execution_(
goal_handle,
feedback,
result,
total_time_sec,
[&res_future]() {
return res_future.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready;
},
[&fjt_client, &goal_handle_arm, &arm_id, this]() {
(void)fjt_client->async_cancel_goal(goal_handle_arm);
if (arm_id == 0) executing_left_.store(false);
else executing_right_.store(false);
},
"Single-arm goal canceled",
&canceled)) {
if (canceled) return;
result->success = false;
result->message = "ROS shutdown during single-arm execution";
goal_handle->abort(result);
return;
}
if (arm_id == 0) executing_left_.store(false);
else executing_right_.store(false);
auto wrapped = res_future.get();
if (wrapped.code != rclcpp_action::ResultCode::SUCCEEDED) {
result->success = false;
result->message = "FollowJointTrajectory failed for target arm";
goal_handle->abort(result);
return;
}
feedback->status = 2;
feedback->progress = 1.0;
goal_handle->publish_feedback(feedback);
result->success = true;
result->message = "Single-arm motion succeeded";
result->final_progress = 1.0;
goal_handle->succeed(result);
}
void DualArmAction::execute_dual_arm_(
const std::shared_ptr<GoalHandleDualArm>& goal_handle,
const std::shared_ptr<const DualArm::Goal>& goal,
const std::shared_ptr<DualArm::Feedback>& feedback,
const std::shared_ptr<DualArm::Result>& result,
const MotionParameters& motion_params,
double acceleration_scaling)
{
const size_t ARM_DOF = motion_params.arm_dof_;
if (!left_fjt_client_ || !right_fjt_client_) {
result->success = false;
result->message = "FJT clients for left/right arm are null";
@@ -738,48 +632,23 @@ void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handl
trajectory_msgs::msg::JointTrajectory traj;
double total_time = 0.0;
if (!export_and_normalize_trajectory_(motionParams, &traj, &total_time, &err)) {
if (!export_and_normalize_trajectory_(motion_params, &traj, &total_time, &err)) {
result->success = false;
result->message = std::string("Export/normalize trajectory failed: ") + err;
goal_handle->abort(result);
return;
}
// 将 12 关节轨迹拆分为左右臂两条 6 关节轨迹
trajectory_msgs::msg::JointTrajectory traj_left;
trajectory_msgs::msg::JointTrajectory traj_right;
traj_left.header = traj.header;
traj_right.header = traj.header;
traj_left.joint_names.assign(
motionParams.dual_arm_joint_names_.begin(),
motionParams.dual_arm_joint_names_.begin() + ARM_DOF);
traj_right.joint_names.assign(
motionParams.dual_arm_joint_names_.begin() + ARM_DOF,
motionParams.dual_arm_joint_names_.begin() + 2 * ARM_DOF);
for (const auto& p : traj.points) {
trajectory_msgs::msg::JointTrajectoryPoint lp;
trajectory_msgs::msg::JointTrajectoryPoint rp;
if (p.positions.size() >= 2 * ARM_DOF) {
lp.positions.assign(p.positions.begin(), p.positions.begin() + ARM_DOF);
rp.positions.assign(p.positions.begin() + ARM_DOF, p.positions.begin() + 2 * ARM_DOF);
}
if (p.velocities.size() >= 2 * ARM_DOF) {
lp.velocities.assign(p.velocities.begin(), p.velocities.begin() + ARM_DOF);
rp.velocities.assign(p.velocities.begin() + ARM_DOF, p.velocities.begin() + 2 * ARM_DOF);
}
if (p.accelerations.size() >= 2 * ARM_DOF) {
lp.accelerations.assign(p.accelerations.begin(), p.accelerations.begin() + ARM_DOF);
rp.accelerations.assign(p.accelerations.begin() + ARM_DOF, p.accelerations.begin() + 2 * ARM_DOF);
}
lp.time_from_start = p.time_from_start;
rp.time_from_start = p.time_from_start;
traj_left.points.push_back(std::move(lp));
traj_right.points.push_back(std::move(rp));
split_dual_arm_trajectory_(motion_params, ARM_DOF, traj, &traj_left, &traj_right);
save_trajectory_to_file_(traj_left, "dual_left");
save_trajectory_to_file_(traj_right, "dual_right");
if (!trajectory_execution_enabled_) {
result->success = false;
result->message = "Arm trajectory execution disabled by parameter: arm_trajectory_execution_enabled";
goal_handle->abort(result);
return;
}
// 通过左右臂各自的 FollowJointTrajectory action 下发
@@ -830,56 +699,39 @@ void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handl
executing_right_.store(true);
const double total_time_sec = total_time;
rclcpp::Rate loop_rate(50.0);
auto start_time = ctx_.node->now();
while (rclcpp::ok()) {
if (goal_handle->is_canceling()) {
(void)left_fjt_client_->async_cancel_goal(left_handle);
(void)right_fjt_client_->async_cancel_goal(right_handle);
executing_.store(false);
executing_left_.store(false);
executing_right_.store(false);
result->success = false;
result->message = "DualArm goal canceled";
goal_handle->canceled(result);
return;
}
auto now = ctx_.node->now();
const double elapsed =
(now.seconds() - start_time.seconds());
double prog = 1.0;
if (total_time_sec > 1e-6) {
prog = std::min(1.0, std::max(0.0, elapsed / total_time_sec));
}
feedback->status = 1;
feedback->progress = prog;
goal_handle->publish_feedback(feedback);
// 同时检查左右臂 action 是否都完成
bool left_done = (left_res_future.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready);
bool right_done = (right_res_future.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready);
if (elapsed >= total_time_sec && left_done && right_done) {
break;
}
loop_rate.sleep();
}
executing_.store(false);
executing_left_.store(false);
executing_right_.store(false);
// 获取左右臂执行结果
if (!rclcpp::ok()) {
bool canceled = false;
if (!wait_for_execution_(
goal_handle,
feedback,
result,
total_time_sec,
[&left_res_future, &right_res_future]() {
const bool left_done =
(left_res_future.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready);
const bool right_done =
(right_res_future.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready);
return left_done && right_done;
},
[&left_handle, &right_handle, this]() {
(void)left_fjt_client_->async_cancel_goal(left_handle);
(void)right_fjt_client_->async_cancel_goal(right_handle);
executing_.store(false);
executing_left_.store(false);
executing_right_.store(false);
},
"DualArm goal canceled",
&canceled)) {
if (canceled) return;
result->success = false;
result->message = "ROS shutdown during DualArm execution";
goal_handle->abort(result);
return;
}
executing_.store(false);
executing_left_.store(false);
executing_right_.store(false);
auto left_wrapped = left_res_future.get();
auto right_wrapped = right_res_future.get();
if (left_wrapped.code != rclcpp_action::ResultCode::SUCCEEDED ||
@@ -899,6 +751,91 @@ void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handl
result->final_progress = 1.0;
goal_handle->succeed(result);
}
bool DualArmAction::wait_for_execution_(
const std::shared_ptr<GoalHandleDualArm>& goal_handle,
const std::shared_ptr<DualArm::Feedback>& feedback,
const std::shared_ptr<DualArm::Result>& result,
double total_time_sec,
const std::function<bool()>& is_done,
const std::function<void()>& cancel_actions,
const std::string& cancel_message,
bool* canceled) const
{
if (canceled) *canceled = false;
rclcpp::Rate loop_rate(50.0);
auto start_time = ctx_.node->now();
while (rclcpp::ok()) {
if (goal_handle->is_canceling()) {
if (cancel_actions) {
cancel_actions();
}
if (canceled) *canceled = true;
result->success = false;
result->message = cancel_message;
goal_handle->canceled(result);
return false;
}
auto now = ctx_.node->now();
const double elapsed =
(now.seconds() - start_time.seconds());
double prog = 1.0;
if (total_time_sec > 1e-6) {
prog = std::min(1.0, std::max(0.0, elapsed / total_time_sec));
}
feedback->status = 1;
feedback->progress = prog;
goal_handle->publish_feedback(feedback);
if (elapsed >= total_time_sec && is_done && is_done()) {
break;
}
loop_rate.sleep();
}
return rclcpp::ok();
}
void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
if (!ctx_.robot_control_manager) {
auto result = std::make_shared<DualArm::Result>();
result->success = false;
result->message = "DualArm not initialized (missing robot_control_manager)";
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";
publish_planning_feedback_(goal_handle, feedback);
const auto& motionParams = ctx_.robot_control_manager->GetMotionParameters();
const double acceleration_scaling = 0.5;
const size_t arm_count = goal->arm_motion_params.size();
if (arm_count == 1) {
execute_single_arm_(goal_handle, goal, feedback, result, acceleration_scaling);
return;
}
if (arm_count == 2) {
execute_dual_arm_(goal_handle, goal, feedback, result, motionParams, acceleration_scaling);
return;
}
result->success = false;
result->message = "Dual-arm mode requires two ArmMotionParams";
goal_handle->abort(result);
}
} // namespace robot_control

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

@@ -1,255 +0,0 @@
#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

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

@@ -4,13 +4,10 @@
#include <memory>
#include <cmath>
#include <algorithm>
#include <limits>
#include "rclcpp/rclcpp.hpp"
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_interface/planning_interface.h"
#include "moveit_msgs/msg/move_it_error_codes.hpp"
#include "moveit/robot_trajectory/robot_trajectory.h"
#include "moveit/robot_state/robot_state.h"
#include "moveit/trajectory_processing/iterative_time_parameterization.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -201,7 +198,7 @@ bool ArmControl::PlanJointMotion(
return false;
}
return timeParameterizeAndStore_(
arm_id, *move_group, plan, velocity_scaling, acceleration_scaling);
arm_id, plan, velocity_scaling, acceleration_scaling);
} catch (const std::exception& e) {
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "PlanJointMotion exception: %s", e.what());
@@ -246,7 +243,7 @@ bool ArmControl::PlanCartesianMotion(
return false;
}
return timeParameterizeAndStore_(
arm_id, *move_group, plan, velocity_scaling, acceleration_scaling);
arm_id, plan, velocity_scaling, acceleration_scaling);
} catch (const std::exception& e) {
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "PlanCartesianMotion exception: %s", e.what());
@@ -257,39 +254,34 @@ bool ArmControl::PlanCartesianMotion(
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)
{
// 1) RobotTrajectory：从 plan.trajectory_ 构建
robot_trajectory::RobotTrajectory original_traj(
move_group.getRobotModel(), move_group.getName());
original_traj.setRobotTrajectoryMsg(*move_group.getCurrentState(), plan.trajectory_);
// 2) 时间参数化：为每个点生成 time_from_start（FollowJointTrajectory 直接执行即可）
trajectory_processing::IterativeParabolicTimeParameterization time_param;
if (!time_param.computeTimeStamps(original_traj, velocity_scaling, acceleration_scaling))
{
// 1) 使用 S 型曲线参数化（MoveIt 路径 + S-curve time scaling）
bool s_curve_ok = false;
if (!plan.trajectory_.joint_trajectory.points.empty()) {
trajectory_msgs::msg::JointTrajectory s_curve_traj;
s_curve_ok = sCurveTimeParameterize_(
arm_id,
plan.trajectory_.joint_trajectory,
s_curve_traj,
velocity_scaling,
acceleration_scaling);
if (s_curve_ok) {
plan.trajectory_.joint_trajectory = s_curve_traj;
}
}
if (!s_curve_ok) {
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "computeTimeStamps failed for arm_id %d", static_cast<int>(arm_id));
RCLCPP_ERROR(node_->get_logger(),
"S-curve time parameterization failed for arm_id %d",
static_cast<int>(arm_id));
}
return false;
}
const size_t waypoint_count = original_traj.getWayPointCount();
if (waypoint_count == 0)
{
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "Empty trajectory after time parameterization for arm_id %d", static_cast<int>(arm_id));
}
return false;
}
// 3) 写回 plan.trajectory_（带时间戳的原始轨迹，不做固定 dt 重采样）
original_traj.getRobotTrajectoryMsg(plan.trajectory_);
// 4) 存入内部 TrajectoryState（使用 msg 内的 time_from_start，并保留 vel/acc）
// 2) 存入内部 TrajectoryState（使用 msg 内的 time_from_start，并保留 vel/acc）
const auto& jt = plan.trajectory_.joint_trajectory;
if (jt.points.empty())
{
@@ -340,6 +332,192 @@ bool ArmControl::timeParameterizeAndStore_(
return true;
}
bool ArmControl::sCurveTimeParameterize_(
uint8_t arm_id,
const trajectory_msgs::msg::JointTrajectory& in_traj,
trajectory_msgs::msg::JointTrajectory& out_traj,
double velocity_scaling,
double acceleration_scaling) const
{
(void)arm_id;
if (in_traj.joint_names.empty() || in_traj.points.empty()) {
return false;
}
const size_t dof = in_traj.joint_names.size();
std::vector<std::vector<double>> waypoints;
waypoints.reserve(in_traj.points.size());
for (const auto& p : in_traj.points) {
if (p.positions.size() < dof) {
return false;
}
waypoints.push_back(p.positions);
}
if (waypoints.size() == 1) {
out_traj = in_traj;
out_traj.points.clear();
trajectory_msgs::msg::JointTrajectoryPoint point;
point.positions = waypoints.front();
point.velocities.assign(dof, 0.0);
point.accelerations.assign(dof, 0.0);
point.time_from_start = rclcpp::Duration::from_seconds(0.0);
out_traj.points.push_back(std::move(point));
return true;
}
// 1) 计算路径累计长度与各关节的 |dq/ds| 上界
const double kEps = 1e-9;
std::vector<double> s_cum(waypoints.size(), 0.0);
std::vector<double> max_abs_dqds(dof, 0.0);
double total_length = 0.0;
for (size_t i = 1; i < waypoints.size(); ++i) {
double seg_sq = 0.0;
for (size_t j = 0; j < dof; ++j) {
const double dq = waypoints[i][j] - waypoints[i - 1][j];
seg_sq += dq * dq;
}
const double seg_len = std::sqrt(seg_sq);
total_length += seg_len;
s_cum[i] = total_length;
if (seg_len > kEps) {
for (size_t j = 0; j < dof; ++j) {
const double dq = waypoints[i][j] - waypoints[i - 1][j];
max_abs_dqds[j] = std::max(max_abs_dqds[j], std::abs(dq) / seg_len);
}
}
}
if (total_length <= kEps) {
out_traj = in_traj;
out_traj.points.clear();
trajectory_msgs::msg::JointTrajectoryPoint point;
point.positions = waypoints.back();
point.velocities.assign(dof, 0.0);
point.accelerations.assign(dof, 0.0);
point.time_from_start = rclcpp::Duration::from_seconds(0.0);
out_traj.points.push_back(std::move(point));
return true;
}
// 2) 读取关节限速/限加速度，映射到路径参数 s 的标量限制
std::vector<double> v_limits(dof, 0.0);
std::vector<double> a_limits(dof, 0.0);
for (size_t j = 0; j < dof; ++j) {
auto it = joints_info_map_.find(in_traj.joint_names[j]);
if (it == joints_info_map_.end()) {
return false;
}
const double vmax = std::abs(it->second.max_velocity) * velocity_scaling;
const double amax = std::abs(it->second.max_acceleration) * acceleration_scaling;
if (vmax <= 0.0 || amax <= 0.0) {
return false;
}
v_limits[j] = vmax;
a_limits[j] = amax;
}
double s_max_vel = std::numeric_limits<double>::infinity();
double s_max_acc = std::numeric_limits<double>::infinity();
for (size_t j = 0; j < dof; ++j) {
if (max_abs_dqds[j] <= kEps) {
continue;
}
s_max_vel = std::min(s_max_vel, v_limits[j] / max_abs_dqds[j]);
s_max_acc = std::min(s_max_acc, a_limits[j] / max_abs_dqds[j]);
}
if (!std::isfinite(s_max_vel) || !std::isfinite(s_max_acc) ||
s_max_vel <= 0.0 || s_max_acc <= 0.0) {
return false;
}
// 3) 使用单轴 S 型曲线对路径参数 s 进行时间标定
S_CurveTrajectory s_curve(
std::vector<double>{s_max_vel},
std::vector<double>{s_max_acc},
std::vector<double>{s_max_acc * 10.0});
s_curve.init(std::vector<double>{0.0}, std::vector<double>{total_length});
const double dt = std::max(0.001, static_cast<double>(CYCLE_TIME) / 1000.0);
out_traj.header = in_traj.header;
out_traj.joint_names = in_traj.joint_names;
out_traj.points.clear();
size_t seg_idx = 0;
double t = 0.0;
bool done = false;
const double min_time = total_length / std::max(1e-6, s_max_vel);
const size_t max_iter =
static_cast<size_t>(std::ceil((min_time * 5.0) / dt)) + 1000;
for (size_t iter = 0; iter < max_iter; ++iter) {
const std::vector<double> s_vec = s_curve.update(t);
double s = s_vec.empty() ? 0.0 : s_vec[0];
if (s > total_length) s = total_length;
const double s_dot = s_curve.getVelocity()[0];
const double s_ddot = s_curve.getAcceleration()[0];
while (seg_idx + 1 < s_cum.size() && s > s_cum[seg_idx + 1]) {
++seg_idx;
}
if (seg_idx + 1 >= waypoints.size()) {
seg_idx = waypoints.size() - 2;
}
const double seg_len = s_cum[seg_idx + 1] - s_cum[seg_idx];
double alpha = 0.0;
if (seg_len > kEps) {
alpha = (s - s_cum[seg_idx]) / seg_len;
alpha = std::clamp(alpha, 0.0, 1.0);
}
trajectory_msgs::msg::JointTrajectoryPoint point;
point.positions.resize(dof, 0.0);
point.velocities.resize(dof, 0.0);
point.accelerations.resize(dof, 0.0);
for (size_t j = 0; j < dof; ++j) {
const double q0 = waypoints[seg_idx][j];
const double q1 = waypoints[seg_idx + 1][j];
const double dq = q1 - q0;
point.positions[j] = q0 + alpha * dq;
if (seg_len > kEps) {
const double dqds = dq / seg_len;
point.velocities[j] = s_dot * dqds;
point.accelerations[j] = s_ddot * dqds;
}
}
point.time_from_start = rclcpp::Duration::from_seconds(t);
out_traj.points.push_back(std::move(point));
if (s_curve.isFinished(t) || (total_length - s) <= 1e-6) {
done = true;
break;
}
t += dt;
}
if (!done || out_traj.points.empty()) {
return false;
}
// 确保末点速度/加速度为 0
auto& last = out_traj.points.back();
last.positions = waypoints.back();
last.velocities.assign(dof, 0.0);
last.accelerations.assign(dof, 0.0);
return true;
}
// 辅助函数：更新 joints_info_map_ 从单个轨迹点（区分左右臂）
void ArmControl::UpdateJointsInfo(uint8_t arm_id, const TrajectoryPoint& point)
{

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

@@ -14,7 +14,10 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
is_robot_enabled_ = false;
// 创建发布器和客户端
gpioPub_ = this->create_publisher<control_msgs::msg::DynamicInterfaceGroupValues>("/gpio_command_controller/commands", 10);
left_gpio_pub_ = this->create_publisher<control_msgs::msg::DynamicInterfaceGroupValues>(
"/left_arm_gpio_controller/commands", 10);
right_gpio_pub_ = this->create_publisher<control_msgs::msg::DynamicInterfaceGroupValues>(
"/right_arm_gpio_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));
@@ -32,12 +35,23 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
robotControlManager_);
action_manager_->initialize();
const bool ret = activateController("trajectory_controller");
if (ret) {
RCLCPP_INFO(this->get_logger(), "Activated controller: trajectory_controller");
const bool ret1 = activateController("left_arm_controller");
if (ret1) {
RCLCPP_INFO(this->get_logger(), "Activated left_arm_controller: trajectory_controller");
} else {
RCLCPP_ERROR(this->get_logger(), "Failed to activate controller: trajectory_controller");
RCLCPP_ERROR(this->get_logger(), "Failed to activate left_arm_controller: trajectory_controller");
}
const bool ret2 = activateController("right_arm_controller");
if (ret2) {
RCLCPP_INFO(this->get_logger(), "Activated right_arm_controller: trajectory_controller");
} else {
RCLCPP_ERROR(this->get_logger(), "Failed to activate right_arm_controller: trajectory_controller");
}
motor_enable(0, 1);
// 初始化MoveIt（需要在ROS节点创建后调用）
@@ -137,52 +151,87 @@ bool RobotControlNode::activateController(const std::string& controller_name)
void RobotControlNode::motor_fault(int id, double fault)
{
if (!gpioPub_) return;
control_msgs::msg::DynamicInterfaceGroupValues msg;
const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
// id == 0: apply to all joints
// id != 0: apply only to that joint index (1-based), others set to 0
for (size_t i = 0; i < motion_params.all_joint_names_.size(); ++i) {
const std::string& group = motion_params.all_joint_names_[i];
msg.interface_groups.push_back(group);
control_msgs::msg::InterfaceValue v;
v.interface_names = {"fault"};
if (id == 0) {
v.values = {fault};
} else {
v.values = {(static_cast<int>(i) + 1 == id) ? fault : 0.0};
}
msg.interface_values.push_back(v);
std::vector<std::string> left;
std::vector<std::string> right;
if (!split_arm_joint_names_(motion_params, left, right)) {
return;
}
gpioPub_->publish(msg);
const size_t arm_dof = motion_params.arm_dof_;
const int target_index = id - 1;
(void)target_index;
publish_gpio_command_(left_gpio_pub_, left, "fault", id, fault, 0);
publish_gpio_command_(right_gpio_pub_, right, "fault", id, fault, static_cast<int>(arm_dof));
}
void RobotControlNode::motor_enable(int id, double enable)
{
if (!gpioPub_) return;
const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
std::vector<std::string> left;
std::vector<std::string> right;
if (!split_arm_joint_names_(motion_params, left, right)) {
return;
}
const size_t arm_dof = motion_params.arm_dof_;
const int target_index = id - 1;
(void)target_index;
publish_gpio_command_(left_gpio_pub_, left, "enable", id, enable, 0);
publish_gpio_command_(right_gpio_pub_, right, "enable", id, enable, static_cast<int>(arm_dof));
}
bool RobotControlNode::split_arm_joint_names_(
const MotionParameters& motion_params,
std::vector<std::string>& left,
std::vector<std::string>& right) const
{
left.clear();
right.clear();
if (motion_params.dual_arm_joint_names_.empty() || motion_params.arm_dof_ == 0) {
return false;
}
const size_t arm_dof = motion_params.arm_dof_;
if (motion_params.dual_arm_joint_names_.size() < 2 * arm_dof) {
return false;
}
left.assign(
motion_params.dual_arm_joint_names_.begin(),
motion_params.dual_arm_joint_names_.begin() + arm_dof);
right.assign(
motion_params.dual_arm_joint_names_.begin() + arm_dof,
motion_params.dual_arm_joint_names_.begin() + 2 * arm_dof);
return true;
}
void RobotControlNode::publish_gpio_command_(
const rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr& pub,
const std::vector<std::string>& joints,
const std::string& interface_name,
int id,
double value,
int offset) const
{
if (!pub) return;
control_msgs::msg::DynamicInterfaceGroupValues msg;
const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
for (size_t i = 0; i < motion_params.all_joint_names_.size(); ++i) {
const std::string& group = motion_params.all_joint_names_[i];
msg.interface_groups.push_back(group);
for (size_t i = 0; i < joints.size(); ++i) {
msg.interface_groups.push_back(joints[i]);
control_msgs::msg::InterfaceValue v;
v.interface_names = {"enable"};
v.interface_names = {interface_name};
if (id == 0) {
v.values = {enable};
v.values = {value};
} else {
v.values = {(static_cast<int>(i) + 1 == id) ? enable : 0.0};
const int global_idx = static_cast<int>(offset + i);
v.values = {(global_idx == (id - 1)) ? value : 0.0};
}
msg.interface_values.push_back(v);
}
gpioPub_->publish(msg);
pub->publish(msg);
}
void RobotControlNode::motor_reset_fault_all()

160
src/robot_control/src/utils/plot_trajectory_csv.py
View File

@@ -1,129 +1,51 @@
#!/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 csv
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:
import matplotlib.pyplot as plt
def load_csv(path):
with open(path, "r", newline="") as f:
reader = csv.reader(f)
rows = list(reader)
if not rows or len(rows) < 2:
raise ValueError("CSV is empty or missing data rows")
header = rows[0]
if len(header) < 2 or header[0] != "time":
raise ValueError("Invalid header, expected: time,<joint>_position...")
time = []
series = [[] for _ in header[1:]]
for row in rows[1:]:
if not row:
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()
time.append(float(row[0]))
for i, val in enumerate(row[1:]):
series[i].append(float(val))
return time, header[1:], series
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 len(sys.argv) < 2:
print("Usage: plot_trajectory_csv.py /tmp/trajectory_*.csv")
sys.exit(1)
path = sys.argv[1]
time, labels, series = load_csv(path)
for label, data in zip(labels, series):
plt.plot(time, data, label=label)
plt.xlabel("time (s)")
plt.ylabel("position (rad)")
plt.title(path)
plt.grid(True)
plt.legend()
plt.show()
if __name__ == "__main__":
main()