add gazebo and right arm test
This commit is contained in:
NuoDaJia
@@ -0,0 +1,95 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import Command, PathJoinSubstitution
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# 包路径
|
||||
desc_share = get_package_share_directory('hc_dual_arm_description')
|
||||
moveit_share = get_package_share_directory('hc_dual_arm_moveit_config')
|
||||
gazebo_share = get_package_share_directory('gazebo_ros')
|
||||
|
||||
# 使用 Gazebo 版本 URDF xacro 生成 robot_description
|
||||
urdf_xacro = PathJoinSubstitution(
|
||||
[moveit_share, 'config', 'hc_dual_arm_gazebo.urdf.xacro']
|
||||
)
|
||||
initial_positions_yaml = PathJoinSubstitution(
|
||||
[moveit_share, 'config', 'initial_positions.yaml']
|
||||
)
|
||||
robot_description = Command([
|
||||
'xacro ', urdf_xacro,
|
||||
' initial_positions_file:=', initial_positions_yaml
|
||||
])
|
||||
|
||||
# 1) 启动 Gazebo 空世界
|
||||
gazebo_launch = PythonLaunchDescriptionSource(
|
||||
[gazebo_share, '/launch/gazebo.launch.py']
|
||||
)
|
||||
gazebo = IncludeLaunchDescription(gazebo_launch)
|
||||
|
||||
# 2) robot_state_publisher（发布 TF + robot_description）
|
||||
rsp = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
parameters=[{'robot_description': robot_description}],
|
||||
)
|
||||
|
||||
# 3) ros2_control_node（使用同一份 robot_description + ros2_controllers.yaml）
|
||||
controllers_yaml = PathJoinSubstitution(
|
||||
[moveit_share, 'config', 'ros2_controllers.yaml']
|
||||
)
|
||||
ros2_control_node = Node(
|
||||
package='controller_manager',
|
||||
executable='ros2_control_node',
|
||||
parameters=[{'robot_description': robot_description},
|
||||
controllers_yaml],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# 4) 在 Gazebo 中 spawn 机器人（从 robot_description topic）
|
||||
spawn_robot = Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
arguments=[
|
||||
'-topic', 'robot_description',
|
||||
'-entity', 'hc_dual_arm'
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# 5) 启动 joint_state_broadcaster 和 左右臂控制器
|
||||
jsb_spawner = Node(
|
||||
package='controller_manager',
|
||||
executable='spawner.py',
|
||||
arguments=['joint_state_broadcaster'],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
right_arm_spawner = Node(
|
||||
package='controller_manager',
|
||||
executable='spawner.py',
|
||||
arguments=['arm_right_controller'],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
left_arm_spawner = Node(
|
||||
package='controller_manager',
|
||||
executable='spawner.py',
|
||||
arguments=['arm_left_controller'],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
gazebo,
|
||||
rsp,
|
||||
ros2_control_node,
|
||||
spawn_robot,
|
||||
jsb_spawner,
|
||||
right_arm_spawner,
|
||||
left_arm_spawner,
|
||||
])
|
||||
@@ -33,9 +33,21 @@ ament_target_dependencies(hc_dual_arm_motion_server
|
||||
hc_dual_arm_interfaces
|
||||
)
|
||||
|
||||
# 编译测试节点
|
||||
add_executable(right_arm_joint_load_test
|
||||
src/right_arm_joint_load_test.cpp
|
||||
)
|
||||
ament_target_dependencies(right_arm_joint_load_test
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
moveit_ros_planning_interface
|
||||
std_msgs
|
||||
)
|
||||
|
||||
# 安装可执行文件
|
||||
install(TARGETS
|
||||
hc_dual_arm_motion_server
|
||||
right_arm_joint_load_test
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
install(DIRECTORY launch
|
||||
|
||||
158
src/hc_dual_arm_motion/README_JOINT_LOAD_TEST.md
Normal file
158
src/hc_dual_arm_motion/README_JOINT_LOAD_TEST.md
Normal file
@@ -0,0 +1,158 @@
|
||||
# 右臂关节负载测试节点
|
||||
|
||||
## 功能说明
|
||||
|
||||
`right_arm_joint_load_test` 节点用于测试右臂机器人在各个关节负载状态下的运行情况。该节点会对右臂的每个关节单独执行上下运动轨迹，并记录运动过程中的关节状态数据（位置、速度、力矩等）。
|
||||
|
||||
## 主要特性
|
||||
|
||||
- **单关节测试**：依次测试右臂的每个关节（right_arm_joint_1 到 right_arm_joint_6）
|
||||
- **上下运动**：每个关节执行上下往复运动
|
||||
- **数据记录**：记录运动过程中的关节位置、速度、力矩等数据到CSV文件
|
||||
- **可配置参数**：支持通过参数配置运动幅度、周期数、速度等
|
||||
|
||||
## 使用方法
|
||||
|
||||
### 1. 编译
|
||||
|
||||
```bash
|
||||
cd ~/hive_core_workspace/hivecore_robot_dual_arms
|
||||
colcon build --packages-select hc_dual_arm_motion
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
### 2. 启动测试
|
||||
|
||||
#### 基本启动（使用默认参数）
|
||||
|
||||
```bash
|
||||
ros2 launch hc_dual_arm_motion right_arm_joint_load_test.launch.py
|
||||
```
|
||||
|
||||
#### 自定义参数启动
|
||||
|
||||
```bash
|
||||
ros2 launch hc_dual_arm_motion right_arm_joint_load_test.launch.py \
|
||||
motion_amplitude:=0.3 \
|
||||
motion_cycles:=5 \
|
||||
velocity_scaling:=0.2 \
|
||||
output_file:=/tmp/joint_test_data.csv
|
||||
```
|
||||
|
||||
### 3. 参数说明
|
||||
|
||||
| 参数名 | 类型 | 默认值 | 说明 |
|
||||
|--------|------|--------|------|
|
||||
| `planning_group` | string | `arm_right` | MoveIt规划组名称 |
|
||||
| `motion_amplitude` | double | `0.5` | 运动幅度（弧度） |
|
||||
| `motion_cycles` | int | `3` | 每个关节的运动周期数 |
|
||||
| `velocity_scaling` | double | `0.3` | 速度缩放因子 [0,1] |
|
||||
| `acceleration_scaling` | double | `0.3` | 加速度缩放因子 [0,1] |
|
||||
| `cycle_duration` | double | `5.0` | 每个周期的时间（秒，仅供参考） |
|
||||
| `output_file` | string | `joint_load_test_data.csv` | 数据输出文件路径 |
|
||||
| `enable_data_logging` | bool | `true` | 是否启用数据记录 |
|
||||
|
||||
### 4. 数据输出
|
||||
|
||||
测试过程中会生成CSV格式的数据文件，包含以下列：
|
||||
|
||||
- `timestamp`: 时间戳（秒.纳秒）
|
||||
- `joint_name`: 关节名称
|
||||
- `joint_index`: 关节索引（0-5）
|
||||
- `position`: 关节位置（弧度）
|
||||
- `velocity`: 关节速度（弧度/秒）
|
||||
- `effort`: 关节力矩（N·m）
|
||||
- `cycle`: 当前周期编号
|
||||
- `phase`: 运动阶段（"up"或"down"）
|
||||
|
||||
### 5. 测试流程
|
||||
|
||||
1. 节点启动后等待2秒，确保MoveGroup就绪
|
||||
2. 获取当前关节位置作为初始位置
|
||||
3. 依次测试每个关节：
|
||||
- 对于每个周期：
|
||||
- 向上运动：关节从初始位置移动到 `初始位置 + motion_amplitude`
|
||||
- 向下运动：关节从当前位置回到初始位置
|
||||
- 每个关节测试完成后等待2秒
|
||||
4. 所有关节测试完成后，节点自动关闭
|
||||
|
||||
## 示例输出
|
||||
|
||||
```
|
||||
[INFO] [right_arm_joint_load_test]: Right Arm Joint Load Test Node started
|
||||
[INFO] [right_arm_joint_load_test]: Waiting for MoveGroup to be ready...
|
||||
[INFO] [right_arm_joint_load_test]: Right arm has 6 joints
|
||||
[INFO] [right_arm_joint_load_test]: Joint 0: right_arm_joint_1
|
||||
[INFO] [right_arm_joint_load_test]: Joint 1: right_arm_joint_2
|
||||
...
|
||||
[INFO] [right_arm_joint_load_test]: ========================================
|
||||
[INFO] [right_arm_joint_load_test]: Starting Right Arm Joint Load Test
|
||||
[INFO] [right_arm_joint_load_test]: Motion amplitude: 0.500 rad
|
||||
[INFO] [right_arm_joint_load_test]: Motion cycles per joint: 3
|
||||
[INFO] [right_arm_joint_load_test]: Cycle duration: 5.00 s
|
||||
[INFO] [right_arm_joint_load_test]: ========================================
|
||||
[INFO] [right_arm_joint_load_test]:
|
||||
[INFO] [right_arm_joint_load_test]: ========================================
|
||||
[INFO] [right_arm_joint_load_test]: Testing joint 0: right_arm_joint_1
|
||||
[INFO] [right_arm_joint_load_test]: ========================================
|
||||
[INFO] [right_arm_joint_load_test]: Cycle 1/3
|
||||
[INFO] [right_arm_joint_load_test]: Moving up: right_arm_joint_1 = 0.000 -> 0.500 rad
|
||||
[INFO] [right_arm_joint_load_test]: Moving down: right_arm_joint_1 = 0.500 -> 0.000 rad
|
||||
...
|
||||
```
|
||||
|
||||
## 注意事项
|
||||
|
||||
1. **安全第一**：测试前确保机器人周围没有障碍物，运动空间充足
|
||||
2. **运动幅度**：根据实际关节限制调整 `motion_amplitude`，避免超出关节限位
|
||||
3. **速度设置**：建议从较低的速度开始测试（如0.2-0.3），逐步增加
|
||||
4. **数据文件**：确保有写入权限，文件路径正确
|
||||
5. **MoveIt配置**：确保MoveIt配置正确，规划组 `arm_right` 可用
|
||||
|
||||
## 数据分析
|
||||
|
||||
可以使用Python、MATLAB等工具分析生成的CSV数据：
|
||||
|
||||
```python
|
||||
import pandas as pd
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# 读取数据
|
||||
df = pd.read_csv('joint_load_test_data.csv')
|
||||
|
||||
# 分析特定关节的数据
|
||||
joint_data = df[df['joint_name'] == 'right_arm_joint_1']
|
||||
|
||||
# 绘制位置-时间曲线
|
||||
plt.figure()
|
||||
plt.plot(joint_data['timestamp'], joint_data['position'])
|
||||
plt.xlabel('Time (s)')
|
||||
plt.ylabel('Position (rad)')
|
||||
plt.title('Joint Position vs Time')
|
||||
plt.show()
|
||||
|
||||
# 绘制力矩-时间曲线
|
||||
plt.figure()
|
||||
plt.plot(joint_data['timestamp'], joint_data['effort'])
|
||||
plt.xlabel('Time (s)')
|
||||
plt.ylabel('Effort (N·m)')
|
||||
plt.title('Joint Effort vs Time')
|
||||
plt.show()
|
||||
```
|
||||
|
||||
## 故障排除
|
||||
|
||||
1. **规划失败**：检查关节限制配置，减小运动幅度
|
||||
2. **执行失败**：检查控制器状态，确保机器人处于可控制状态
|
||||
3. **数据未记录**：检查文件路径权限，确认 `enable_data_logging` 为 true
|
||||
4. **MoveGroup未就绪**：确保MoveIt和机器人状态发布器正常运行
|
||||
|
||||
## 扩展功能
|
||||
|
||||
可以根据需要扩展以下功能：
|
||||
|
||||
- 添加负载检测和报警
|
||||
- 实时绘制关节状态曲线
|
||||
- 支持自定义运动轨迹
|
||||
- 添加测试结果统计和报告生成
|
||||
|
||||
@@ -0,0 +1,84 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# MoveIt demo.launch（提供 robot_description、move_group 等）
|
||||
moveit_config_share = FindPackageShare(package="hc_dual_arm_moveit_config")
|
||||
moveit_demo_launch = PathJoinSubstitution(
|
||||
[moveit_config_share, "launch", "demo.launch.py"]
|
||||
)
|
||||
|
||||
moveit_demo = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(moveit_demo_launch)
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
# 声明启动参数
|
||||
DeclareLaunchArgument(
|
||||
'planning_group',
|
||||
default_value='arm_right',
|
||||
description='MoveIt planning group name'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'motion_amplitude',
|
||||
default_value='0.5',
|
||||
description='Motion amplitude in radians'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'motion_cycles',
|
||||
default_value='3',
|
||||
description='Number of motion cycles per joint'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'velocity_scaling',
|
||||
default_value='0.3',
|
||||
description='Velocity scaling factor'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'acceleration_scaling',
|
||||
default_value='0.3',
|
||||
description='Acceleration scaling factor'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'cycle_duration',
|
||||
default_value='5.0',
|
||||
description='Duration of each cycle in seconds'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'output_file',
|
||||
default_value='joint_load_test_data.csv',
|
||||
description='Output file for data logging'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'enable_data_logging',
|
||||
default_value='true',
|
||||
description='Enable data logging to file'
|
||||
),
|
||||
|
||||
# 启动 MoveIt demo（提供 robot_description）
|
||||
moveit_demo,
|
||||
|
||||
# 测试节点
|
||||
Node(
|
||||
package='hc_dual_arm_motion',
|
||||
executable='right_arm_joint_load_test',
|
||||
name='right_arm_joint_load_test',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'planning_group': LaunchConfiguration('planning_group'),
|
||||
'motion_amplitude': LaunchConfiguration('motion_amplitude'),
|
||||
'motion_cycles': LaunchConfiguration('motion_cycles'),
|
||||
'velocity_scaling': LaunchConfiguration('velocity_scaling'),
|
||||
'acceleration_scaling': LaunchConfiguration('acceleration_scaling'),
|
||||
'cycle_duration': LaunchConfiguration('cycle_duration'),
|
||||
'output_file': LaunchConfiguration('output_file'),
|
||||
'enable_data_logging': LaunchConfiguration('enable_data_logging'),
|
||||
}]
|
||||
),
|
||||
])
|
||||
|
||||
97
src/hc_dual_arm_motion/scripts/run_joint_load_test.sh
Executable file
97
src/hc_dual_arm_motion/scripts/run_joint_load_test.sh
Executable file
@@ -0,0 +1,97 @@
|
||||
#!/bin/bash
|
||||
|
||||
# 右臂关节负载测试启动脚本
|
||||
# 使用方法: ./run_joint_load_test.sh [参数]
|
||||
|
||||
# 默认参数
|
||||
PLANNING_GROUP="arm_right"
|
||||
MOTION_AMPLITUDE="0.5"
|
||||
MOTION_CYCLES="3"
|
||||
VELOCITY_SCALING="0.3"
|
||||
ACCELERATION_SCALING="0.3"
|
||||
OUTPUT_FILE="joint_load_test_data.csv"
|
||||
ENABLE_LOGGING="true"
|
||||
|
||||
# 解析命令行参数
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case $1 in
|
||||
--amplitude)
|
||||
MOTION_AMPLITUDE="$2"
|
||||
shift 2
|
||||
;;
|
||||
--cycles)
|
||||
MOTION_CYCLES="$2"
|
||||
shift 2
|
||||
;;
|
||||
--velocity)
|
||||
VELOCITY_SCALING="$2"
|
||||
shift 2
|
||||
;;
|
||||
--output)
|
||||
OUTPUT_FILE="$2"
|
||||
shift 2
|
||||
;;
|
||||
--no-logging)
|
||||
ENABLE_LOGGING="false"
|
||||
shift
|
||||
;;
|
||||
--help)
|
||||
echo "Usage: $0 [OPTIONS]"
|
||||
echo ""
|
||||
echo "Options:"
|
||||
echo " --amplitude VALUE Motion amplitude in radians (default: 0.5)"
|
||||
echo " --cycles VALUE Number of motion cycles per joint (default: 3)"
|
||||
echo " --velocity VALUE Velocity scaling factor (default: 0.3)"
|
||||
echo " --output FILE Output file path (default: joint_load_test_data.csv)"
|
||||
echo " --no-logging Disable data logging"
|
||||
echo " --help Show this help message"
|
||||
exit 0
|
||||
;;
|
||||
*)
|
||||
echo "Unknown option: $1"
|
||||
echo "Use --help for usage information"
|
||||
exit 1
|
||||
;;
|
||||
esac
|
||||
done
|
||||
|
||||
# 检查ROS2环境
|
||||
if [ -z "$ROS_DISTRO" ]; then
|
||||
echo "Error: ROS2 environment not set. Please source your ROS2 setup file."
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# 检查工作空间是否已构建
|
||||
if [ ! -d "install/hc_dual_arm_motion" ]; then
|
||||
echo "Warning: Package not found in install directory. Building..."
|
||||
colcon build --packages-select hc_dual_arm_motion
|
||||
if [ $? -ne 0 ]; then
|
||||
echo "Error: Build failed"
|
||||
exit 1
|
||||
fi
|
||||
fi
|
||||
|
||||
# Source工作空间
|
||||
source install/setup.bash
|
||||
|
||||
echo "=========================================="
|
||||
echo "Right Arm Joint Load Test"
|
||||
echo "=========================================="
|
||||
echo "Motion amplitude: $MOTION_AMPLITUDE rad"
|
||||
echo "Motion cycles: $MOTION_CYCLES"
|
||||
echo "Velocity scaling: $VELOCITY_SCALING"
|
||||
echo "Output file: $OUTPUT_FILE"
|
||||
echo "Data logging: $ENABLE_LOGGING"
|
||||
echo "=========================================="
|
||||
echo ""
|
||||
|
||||
# 启动测试
|
||||
ros2 launch hc_dual_arm_motion right_arm_joint_load_test.launch.py \
|
||||
planning_group:="$PLANNING_GROUP" \
|
||||
motion_amplitude:="$MOTION_AMPLITUDE" \
|
||||
motion_cycles:="$MOTION_CYCLES" \
|
||||
velocity_scaling:="$VELOCITY_SCALING" \
|
||||
acceleration_scaling:="$ACCELERATION_SCALING" \
|
||||
output_file:="$OUTPUT_FILE" \
|
||||
enable_data_logging:="$ENABLE_LOGGING"
|
||||
|
||||
362
src/hc_dual_arm_motion/src/right_arm_joint_load_test.cpp
Normal file
362
src/hc_dual_arm_motion/src/right_arm_joint_load_test.cpp
Normal file
@@ -0,0 +1,362 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <moveit/move_group_interface/move_group_interface.h>
|
||||
#include <sensor_msgs/msg/joint_state.hpp>
|
||||
#include <std_msgs/msg/float64_multi_array.hpp>
|
||||
#include <chrono>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <algorithm>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
class RightArmJointLoadTest : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
RightArmJointLoadTest()
|
||||
: Node("right_arm_joint_load_test")
|
||||
{
|
||||
// 声明参数
|
||||
this->declare_parameter<std::string>("planning_group", "arm_right");
|
||||
this->declare_parameter<double>("motion_amplitude", 0.5); // 运动幅度（弧度）
|
||||
this->declare_parameter<int>("motion_cycles", 3); // 每个关节的运动周期数
|
||||
this->declare_parameter<double>("velocity_scaling", 0.3);
|
||||
this->declare_parameter<double>("acceleration_scaling", 0.3);
|
||||
this->declare_parameter<double>("cycle_duration", 5.0); // 每个周期的时间（秒）
|
||||
this->declare_parameter<std::string>("output_file", "joint_load_test_data.csv");
|
||||
this->declare_parameter<bool>("enable_data_logging", true);
|
||||
|
||||
// 获取参数
|
||||
this->get_parameter("planning_group", planning_group_);
|
||||
this->get_parameter("motion_amplitude", motion_amplitude_);
|
||||
this->get_parameter("motion_cycles", motion_cycles_);
|
||||
this->get_parameter("velocity_scaling", velocity_scaling_);
|
||||
this->get_parameter("acceleration_scaling", acceleration_scaling_);
|
||||
this->get_parameter("cycle_duration", cycle_duration_);
|
||||
this->get_parameter("output_file", output_file_);
|
||||
this->get_parameter("enable_data_logging", enable_data_logging_);
|
||||
|
||||
// 初始化MoveGroup
|
||||
move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
|
||||
std::shared_ptr<rclcpp::Node>(this, [](auto*) {}), planning_group_);
|
||||
|
||||
move_group_->setMaxVelocityScalingFactor(velocity_scaling_);
|
||||
move_group_->setMaxAccelerationScalingFactor(acceleration_scaling_);
|
||||
move_group_->setPlanningTime(5.0);
|
||||
move_group_->setNumPlanningAttempts(10);
|
||||
|
||||
// 获取关节名称
|
||||
joint_names_ = move_group_->getJointNames();
|
||||
RCLCPP_INFO(this->get_logger(), "Right arm has %zu joints", joint_names_.size());
|
||||
for (size_t i = 0; i < joint_names_.size(); ++i) {
|
||||
RCLCPP_INFO(this->get_logger(), " Joint %zu: %s", i, joint_names_[i].c_str());
|
||||
}
|
||||
|
||||
// 订阅关节状态
|
||||
joint_state_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"/joint_states", 10,
|
||||
std::bind(&RightArmJointLoadTest::joint_state_callback, this, std::placeholders::_1));
|
||||
|
||||
// 存储最新的关节状态
|
||||
latest_joint_state_ = nullptr;
|
||||
joint_state_received_ = false;
|
||||
|
||||
// 初始化数据记录
|
||||
if (enable_data_logging_) {
|
||||
data_file_.open(output_file_, std::ios::out);
|
||||
if (data_file_.is_open()) {
|
||||
// 写入CSV头部
|
||||
data_file_ << "timestamp,joint_name,joint_index,position,velocity,effort,cycle,phase\n";
|
||||
RCLCPP_INFO(this->get_logger(), "Data logging enabled, output file: %s", output_file_.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to open output file: %s", output_file_.c_str());
|
||||
enable_data_logging_ = false;
|
||||
}
|
||||
}
|
||||
|
||||
// 启动测试
|
||||
test_timer_ = this->create_wall_timer(
|
||||
1s, std::bind(&RightArmJointLoadTest::start_test, this));
|
||||
test_started_ = false;
|
||||
}
|
||||
|
||||
~RightArmJointLoadTest()
|
||||
{
|
||||
if (data_file_.is_open()) {
|
||||
data_file_.close();
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
void start_test()
|
||||
{
|
||||
if (test_started_) {
|
||||
return;
|
||||
}
|
||||
test_started_ = true;
|
||||
test_timer_->cancel();
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "========================================");
|
||||
RCLCPP_INFO(this->get_logger(), "Starting Right Arm Joint Load Test");
|
||||
RCLCPP_INFO(this->get_logger(), "Motion amplitude: %.3f rad", motion_amplitude_);
|
||||
RCLCPP_INFO(this->get_logger(), "Motion cycles per joint: %d", motion_cycles_);
|
||||
RCLCPP_INFO(this->get_logger(), "Cycle duration: %.2f s", cycle_duration_);
|
||||
RCLCPP_INFO(this->get_logger(), "========================================");
|
||||
|
||||
// 等待MoveGroup就绪
|
||||
std::this_thread::sleep_for(2s);
|
||||
|
||||
// 执行测试
|
||||
execute_test();
|
||||
}
|
||||
|
||||
void execute_test()
|
||||
{
|
||||
// 等待关节状态就绪
|
||||
RCLCPP_INFO(this->get_logger(), "Waiting for joint states...");
|
||||
int wait_count = 0;
|
||||
while (!joint_state_received_ && wait_count < 50) {
|
||||
rclcpp::spin_some(shared_from_this());
|
||||
std::this_thread::sleep_for(100ms);
|
||||
wait_count++;
|
||||
if (wait_count % 10 == 0) {
|
||||
RCLCPP_INFO(this->get_logger(), "Still waiting for joint states... (%d/50)", wait_count);
|
||||
}
|
||||
}
|
||||
|
||||
if (!joint_state_received_) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Timeout waiting for joint states!");
|
||||
return;
|
||||
}
|
||||
|
||||
// 从关节状态消息中获取初始关节位置
|
||||
std::vector<double> initial_joints;
|
||||
initial_joints.resize(joint_names_.size(), 0.0);
|
||||
|
||||
if (latest_joint_state_) {
|
||||
for (size_t i = 0; i < joint_names_.size(); ++i) {
|
||||
auto it = std::find(latest_joint_state_->name.begin(),
|
||||
latest_joint_state_->name.end(),
|
||||
joint_names_[i]);
|
||||
if (it != latest_joint_state_->name.end()) {
|
||||
size_t idx = std::distance(latest_joint_state_->name.begin(), it);
|
||||
if (idx < latest_joint_state_->position.size()) {
|
||||
initial_joints[i] = latest_joint_state_->position[idx];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 如果从joint_states获取失败，尝试从MoveGroup获取
|
||||
if (initial_joints.empty() ||
|
||||
std::all_of(initial_joints.begin(), initial_joints.end(),
|
||||
[](double v) { return v == 0.0; })) {
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to get joints from joint_states, trying MoveGroup...");
|
||||
std::this_thread::sleep_for(2s);
|
||||
|
||||
try {
|
||||
initial_joints = move_group_->getCurrentJointValues();
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to get current joint values: %s", e.what());
|
||||
RCLCPP_ERROR(this->get_logger(), "Using zero positions as fallback");
|
||||
initial_joints.resize(joint_names_.size(), 0.0);
|
||||
}
|
||||
}
|
||||
|
||||
if (initial_joints.size() != joint_names_.size()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Joint count mismatch! Expected %zu, got %zu",
|
||||
joint_names_.size(), initial_joints.size());
|
||||
return;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Initial joint positions:");
|
||||
for (size_t i = 0; i < joint_names_.size(); ++i) {
|
||||
RCLCPP_INFO(this->get_logger(), " %s: %.3f rad", joint_names_[i].c_str(), initial_joints[i]);
|
||||
}
|
||||
|
||||
// 对每个关节进行测试
|
||||
for (size_t joint_idx = 0; joint_idx < joint_names_.size(); ++joint_idx) {
|
||||
RCLCPP_INFO(this->get_logger(), "\n========================================");
|
||||
RCLCPP_INFO(this->get_logger(), "Testing joint %zu: %s", joint_idx, joint_names_[joint_idx].c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "========================================");
|
||||
|
||||
// 测试单个关节
|
||||
test_single_joint(joint_idx, initial_joints);
|
||||
|
||||
// 关节测试之间的等待时间
|
||||
std::this_thread::sleep_for(2s);
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "\n========================================");
|
||||
RCLCPP_INFO(this->get_logger(), "All joint tests completed!");
|
||||
RCLCPP_INFO(this->get_logger(), "========================================");
|
||||
|
||||
if (enable_data_logging_ && data_file_.is_open()) {
|
||||
data_file_.flush();
|
||||
RCLCPP_INFO(this->get_logger(), "Data saved to: %s", output_file_.c_str());
|
||||
}
|
||||
|
||||
// 测试完成后关闭节点
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
|
||||
void test_single_joint(size_t joint_idx, const std::vector<double>& initial_joints)
|
||||
{
|
||||
// 为每个周期创建上下运动轨迹
|
||||
for (int cycle = 0; cycle < motion_cycles_; ++cycle) {
|
||||
RCLCPP_INFO(this->get_logger(), " Cycle %d/%d", cycle + 1, motion_cycles_);
|
||||
|
||||
// 创建目标关节位置（其他关节保持初始位置）
|
||||
std::vector<double> target_joints = initial_joints;
|
||||
|
||||
// 向上运动
|
||||
current_cycle_ = cycle;
|
||||
current_phase_ = "up";
|
||||
target_joints[joint_idx] = initial_joints[joint_idx] + motion_amplitude_;
|
||||
RCLCPP_INFO(this->get_logger(), " Moving up: %s = %.3f -> %.3f rad",
|
||||
joint_names_[joint_idx].c_str(),
|
||||
initial_joints[joint_idx],
|
||||
target_joints[joint_idx]);
|
||||
|
||||
if (!execute_joint_motion(target_joints, joint_idx)) {
|
||||
RCLCPP_ERROR(this->get_logger(), " Failed to move up!");
|
||||
continue;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(500ms);
|
||||
|
||||
// 向下运动（回到初始位置）
|
||||
current_phase_ = "down";
|
||||
target_joints[joint_idx] = initial_joints[joint_idx];
|
||||
RCLCPP_INFO(this->get_logger(), " Moving down: %s = %.3f -> %.3f rad",
|
||||
joint_names_[joint_idx].c_str(),
|
||||
initial_joints[joint_idx] + motion_amplitude_,
|
||||
target_joints[joint_idx]);
|
||||
|
||||
if (!execute_joint_motion(target_joints, joint_idx)) {
|
||||
RCLCPP_ERROR(this->get_logger(), " Failed to move down!");
|
||||
continue;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(500ms);
|
||||
}
|
||||
}
|
||||
|
||||
bool execute_joint_motion(const std::vector<double>& target_joints, size_t joint_idx)
|
||||
{
|
||||
// 设置目标关节值
|
||||
move_group_->setJointValueTarget(target_joints);
|
||||
|
||||
// 规划运动
|
||||
moveit::planning_interface::MoveGroupInterface::Plan plan;
|
||||
bool success = (move_group_->plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
|
||||
|
||||
if (!success) {
|
||||
RCLCPP_WARN(this->get_logger(), " Planning failed, trying again...");
|
||||
// 重试一次
|
||||
success = (move_group_->plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
|
||||
}
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 执行运动
|
||||
moveit::core::MoveItErrorCode execute_result = move_group_->execute(plan);
|
||||
|
||||
if (execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
|
||||
RCLCPP_WARN(this->get_logger(), " Execution result: %d", execute_result.val);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg)
|
||||
{
|
||||
// 保存最新的关节状态
|
||||
latest_joint_state_ = msg;
|
||||
if (!joint_state_received_) {
|
||||
joint_state_received_ = true;
|
||||
RCLCPP_INFO(this->get_logger(), "Received first joint state message");
|
||||
}
|
||||
|
||||
if (!enable_data_logging_ || !data_file_.is_open()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// 查找右臂关节的状态
|
||||
for (size_t i = 0; i < joint_names_.size(); ++i) {
|
||||
// 在joint_states消息中查找对应的关节
|
||||
auto it = std::find(msg->name.begin(), msg->name.end(), joint_names_[i]);
|
||||
if (it != msg->name.end()) {
|
||||
size_t idx = std::distance(msg->name.begin(), it);
|
||||
|
||||
// 记录数据
|
||||
auto now = std::chrono::system_clock::now();
|
||||
auto time_since_epoch = now.time_since_epoch();
|
||||
auto seconds = std::chrono::duration_cast<std::chrono::seconds>(time_since_epoch).count();
|
||||
auto nanoseconds = std::chrono::duration_cast<std::chrono::nanoseconds>(time_since_epoch).count() % 1000000000;
|
||||
|
||||
double position = (idx < msg->position.size()) ? msg->position[idx] : 0.0;
|
||||
double velocity = (idx < msg->velocity.size()) ? msg->velocity[idx] : 0.0;
|
||||
double effort = (idx < msg->effort.size()) ? msg->effort[idx] : 0.0;
|
||||
|
||||
data_file_ << seconds << "." << std::setfill('0') << std::setw(9) << nanoseconds << ","
|
||||
<< joint_names_[i] << ","
|
||||
<< i << ","
|
||||
<< std::fixed << std::setprecision(6) << position << ","
|
||||
<< velocity << ","
|
||||
<< effort << ","
|
||||
<< current_cycle_ << ","
|
||||
<< current_phase_ << "\n";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 参数
|
||||
std::string planning_group_;
|
||||
double motion_amplitude_;
|
||||
int motion_cycles_;
|
||||
double velocity_scaling_;
|
||||
double acceleration_scaling_;
|
||||
double cycle_duration_;
|
||||
std::string output_file_;
|
||||
bool enable_data_logging_;
|
||||
|
||||
// MoveIt接口
|
||||
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
|
||||
std::vector<std::string> joint_names_;
|
||||
|
||||
// 订阅
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
|
||||
sensor_msgs::msg::JointState::SharedPtr latest_joint_state_;
|
||||
bool joint_state_received_;
|
||||
|
||||
// 数据记录
|
||||
std::ofstream data_file_;
|
||||
int current_cycle_ = -1;
|
||||
std::string current_phase_ = "idle";
|
||||
|
||||
// 定时器
|
||||
rclcpp::TimerBase::SharedPtr test_timer_;
|
||||
bool test_started_ = false;
|
||||
};
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
auto node = std::make_shared<RightArmJointLoadTest>();
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "Right Arm Joint Load Test Node started");
|
||||
RCLCPP_INFO(node->get_logger(), "Waiting for MoveGroup to be ready...");
|
||||
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hc_dual_arm">
|
||||
|
||||
<!-- 初始关节位置配置 -->
|
||||
<xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
|
||||
|
||||
<!-- 几何和惯性等描述，直接复用 description 包里的 urdf -->
|
||||
<xacro:include filename="$(find hc_dual_arm_description)/urdf/hc_dual_arm.urdf" />
|
||||
|
||||
<!-- Gazebo 版本 ros2_control 描述 -->
|
||||
<xacro:include filename="hc_dual_arm_gazebo.ros2_control.xacro" />
|
||||
|
||||
<!-- 实例化 ros2_control（GazeboSystem） -->
|
||||
<xacro:hc_dual_arm_ros2_control_gazebo
|
||||
name="GazeboSystem"
|
||||
initial_positions_file="$(arg initial_positions_file)" />
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,114 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<!-- Gazebo 版本 ros2_control 描述 -->
|
||||
<xacro:macro name="hc_dual_arm_ros2_control_gazebo" params="name initial_positions_file">
|
||||
<xacro:property name="initial_positions"
|
||||
value="${xacro.load_yaml(initial_positions_file)['initial_positions']}"/>
|
||||
|
||||
<ros2_control name="${name}" type="system">
|
||||
<hardware>
|
||||
<!-- Gazebo 作为硬件执行层 -->
|
||||
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
|
||||
</hardware>
|
||||
|
||||
<!-- 左臂关节 -->
|
||||
<joint name="left_arm_joint_1">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_arm_joint_1']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
<joint name="left_arm_joint_2">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_arm_joint_2']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
<joint name="left_arm_joint_3">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_arm_joint_3']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
<joint name="left_arm_joint_4">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_arm_joint_4']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
<joint name="left_arm_joint_5">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_arm_joint_5']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
<joint name="left_arm_joint_6">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_arm_joint_6']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
<!-- 右臂关节 -->
|
||||
<joint name="right_arm_joint_1">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_arm_joint_1']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
<joint name="right_arm_joint_2">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_arm_joint_2']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
<joint name="right_arm_joint_3">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_arm_joint_3']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
<joint name="right_arm_joint_4">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_arm_joint_4']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
<joint name="right_arm_joint_5">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_arm_joint_5']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
<joint name="right_arm_joint_6">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_arm_joint_6']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
</ros2_control>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
Reference in New Issue
Block a user