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

code optimization

Browse Source
This commit is contained in:
NuoDaJia
2026-03-05 17:48:37 +08:00
parent 900869515c
commit 773ce04f37
63 changed files with 1273 additions and 1228 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
.gitignore vendored
View File

@@ -1,8 +0,0 @@
# 忽略所有层级的 build 文件夹及其内容
**/build/
# 忽略所有层级的 install 文件夹及其内容
**/install/
# 忽略所有层级的 log 文件夹及其内容
**/log/

73
LICENSE
View File

@@ -1,73 +0,0 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files.
"Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions:
(a) You must give any other recipients of the Work or Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License.
You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives.
Copyright 2025 HiveCoreRD
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

109
src/dual_arm_description/urdf/dual_arm.urdf
View File

@@ -741,4 +741,113 @@
effort="30.0"
velocity="3.0" />
</joint>
<!-- ros2_control: mock_components/GenericSystem for fake hardware simulation.
Joints teleport instantly to commanded position (no physics).
Provides /joint_states via joint_state_broadcaster and
FollowJointTrajectory action servers via joint_trajectory_controller. -->
<ros2_control name="dual_arm_mock" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
<joint name="left_arm_joint_1">
<command_interface name="position">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
</joint>
<joint name="left_arm_joint_2">
<command_interface name="position">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
</joint>
<joint name="left_arm_joint_3">
<command_interface name="position">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
</joint>
<joint name="left_arm_joint_4">
<command_interface name="position">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
</joint>
<joint name="left_arm_joint_5">
<command_interface name="position">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
</joint>
<joint name="left_arm_joint_6">
<command_interface name="position">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
</joint>
<joint name="right_arm_joint_1">
<command_interface name="position">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
</joint>
<joint name="right_arm_joint_2">
<command_interface name="position">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
</joint>
<joint name="right_arm_joint_3">
<command_interface name="position">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
</joint>
<joint name="right_arm_joint_4">
<command_interface name="position">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
</joint>
<joint name="right_arm_joint_5">
<command_interface name="position">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
</joint>
<joint name="right_arm_joint_6">
<command_interface name="position">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
</joint>
</ros2_control>
</robot>

2
src/robot_control/CMakeLists.txt
View File

@@ -124,9 +124,7 @@ install(DIRECTORY launch
install(PROGRAMS
scripts/dual_arm_action_client.py
scripts/mock_dual_arm_hardware.py
scripts/run_dual_arm_mock_tests.py
scripts/analyze_trajectory_metrics.py
DESTINATION lib/${PROJECT_NAME}
)

5
src/robot_control/include/actions/action_context.hpp
View File

@@ -1,3 +1,8 @@
/**
* @file action_context.hpp
* @brief 动作上下文结构体，聚合控制器与服务引用
*/
#pragma once
#include <memory>

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

@@ -24,14 +24,26 @@ class SafetyService;
class ActionManager
{
public:
/**
* @brief 构造函数，初始化动作管理器并持有所需依赖
* @param node ROS 2 节点指针
* @param robot_control_manager 机器人控制管理器引用
* @param safety_service 安全服务共享指针
*/
ActionManager(
rclcpp::Node* node,
RobotControlManager& robot_control_manager,
const std::shared_ptr<SafetyService>& safety_service);
/**
* @brief 析构函数
*/
~ActionManager();
void initialize();
/**
* @brief 初始化所有动作服务端
*/
void Initialize();
private:
rclcpp::Node* node_{nullptr};

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

@@ -1,3 +1,8 @@
/**
* @file dual_arm_action.hpp
* @brief 双臂协调动作服务端接口
*/
#pragma once
#include <atomic>
@@ -27,12 +32,28 @@ public:
using FollowJTA = control_msgs::action::FollowJointTrajectory;
using GoalHandleFollowJTA = rclcpp_action::ClientGoalHandle<FollowJTA>;
/**
* @brief 构造函数，初始化双臂协调动作服务端
* @param ctx 动作上下文（包含节点和控制器引用）
*/
explicit DualArmAction(const ActionContext& ctx);
void initialize();
/**
* @brief 创建并注册 ROS 2 Action 服务端及左右臂 FJT 客户端
*/
void Initialize();
bool is_executing() const { return executing_.load(); }
void set_executing(bool v) { executing_.store(v); }
/**
* @brief 查询当前是否正在执行双臂协调动作
* @return true 正在执行，false 未执行
*/
bool IsExecuting() const { return executing_.load(); }
/**
* @brief 设置整体执行状态标志
* @param v true 表示正在执行，false 表示未执行
*/
void SetExecuting(bool v) { executing_.store(v); }
private:
rclcpp_action::GoalResponse handle_goal_(

27
src/robot_control/include/actions/move_home_action.hpp
View File

@@ -1,3 +1,8 @@
/**
* @file move_home_action.hpp
* @brief 归零动作服务端接口
*/
#pragma once
#include <atomic>
@@ -16,12 +21,28 @@ public:
using MoveHome = interfaces::action::MoveHome;
using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
/**
* @brief 构造函数，初始化归零动作服务端
* @param ctx 动作上下文（包含节点和控制器引用）
*/
explicit MoveHomeAction(const ActionContext& ctx);
void initialize();
/**
* @brief 创建并注册 ROS 2 Action 服务端
*/
void Initialize();
bool is_executing() const { return executing_.load(); }
void set_executing(bool v) { executing_.store(v); }
/**
* @brief 查询当前是否正在执行归零动作
* @return true 正在执行，false 未执行
*/
bool IsExecuting() const { return executing_.load(); }
/**
* @brief 设置执行状态标志
* @param v true 表示正在执行，false 表示未执行
*/
void SetExecuting(bool v) { executing_.store(v); }
private:
rclcpp_action::GoalResponse handle_goal_(

27
src/robot_control/include/actions/move_leg_action.hpp
View File

@@ -1,3 +1,8 @@
/**
* @file move_leg_action.hpp
* @brief 腿部运动动作服务端接口
*/
#pragma once
#include <atomic>
@@ -16,12 +21,28 @@ public:
using MoveLeg = interfaces::action::MoveLeg;
using GoalHandleMoveLeg = rclcpp_action::ServerGoalHandle<MoveLeg>;
/**
* @brief 构造函数，初始化腿部运动动作服务端
* @param ctx 动作上下文（包含节点和控制器引用）
*/
explicit MoveLegAction(const ActionContext& ctx);
void initialize();
/**
* @brief 创建并注册 ROS 2 Action 服务端
*/
void Initialize();
bool is_executing() const { return executing_.load(); }
void set_executing(bool v) { executing_.store(v); }
/**
* @brief 查询当前是否正在执行腿部运动动作
* @return true 正在执行，false 未执行
*/
bool IsExecuting() const { return executing_.load(); }
/**
* @brief 设置执行状态标志
* @param v true 表示正在执行，false 表示未执行
*/
void SetExecuting(bool v) { executing_.store(v); }
private:
rclcpp_action::GoalResponse handle_goal_(

27
src/robot_control/include/actions/move_waist_action.hpp
View File

@@ -1,3 +1,8 @@
/**
* @file move_waist_action.hpp
* @brief 腰部运动动作服务端接口
*/
#pragma once
#include <atomic>
@@ -16,12 +21,28 @@ public:
using MoveWaist = interfaces::action::MoveWaist;
using GoalHandleMoveWaist = rclcpp_action::ServerGoalHandle<MoveWaist>;
/**
* @brief 构造函数，初始化腰部运动动作服务端
* @param ctx 动作上下文（包含节点和控制器引用）
*/
explicit MoveWaistAction(const ActionContext& ctx);
void initialize();
/**
* @brief 创建并注册 ROS 2 Action 服务端
*/
void Initialize();
bool is_executing() const { return executing_.load(); }
void set_executing(bool v) { executing_.store(v); }
/**
* @brief 查询当前是否正在执行腰部运动动作
* @return true 正在执行，false 未执行
*/
bool IsExecuting() const { return executing_.load(); }
/**
* @brief 设置执行状态标志
* @param v true 表示正在执行，false 表示未执行
*/
void SetExecuting(bool v) { executing_.store(v); }
private:
rclcpp_action::GoalResponse handle_goal_(

28
src/robot_control/include/actions/move_wheel_action.hpp
View File

@@ -1,3 +1,8 @@
/**
* @file move_wheel_action.hpp
* @brief 轮式运动动作服务端接口
*/
#pragma once
#include <atomic>
@@ -19,14 +24,31 @@ public:
using MoveWheel = interfaces::action::MoveWheel;
using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;
/**
* @brief 构造函数，初始化轮式运动动作服务端
* @param ctx 动作上下文（包含节点和控制器引用）
* @param motor_service 电机服务共享指针
*/
MoveWheelAction(
const ActionContext& ctx,
std::shared_ptr<MotorService> motor_service);
void initialize();
/**
* @brief 创建并注册 ROS 2 Action 服务端
*/
void Initialize();
bool is_executing() const { return executing_.load(); }
void set_executing(bool v) { executing_.store(v); }
/**
* @brief 查询当前是否正在执行轮式运动动作
* @return true 正在执行，false 未执行
*/
bool IsExecuting() const { return executing_.load(); }
/**
* @brief 设置执行状态标志
* @param v true 表示正在执行，false 表示未执行
*/
void SetExecuting(bool v) { executing_.store(v); }
private:
rclcpp_action::GoalResponse handle_goal_(

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

@@ -8,15 +8,16 @@
#pragma once
#include "controllers/control_base.hpp"
#include <memory>
#include <string>
#include <vector>
#include <map>
#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"
#include <map>
#include <memory>
#include <string>
#include <vector>
// 前向声明
namespace rclcpp {
class Node;

15
src/robot_control/include/controllers/control_base.hpp
View File

@@ -13,14 +13,15 @@
#pragma once
#include <vector>
#include <memory>
#include <string>
#include <map>
#include "utils/trapezoidal_trajectory.hpp"
#include "utils/s_curve_trajectory.hpp"
#include "core/motion_parameters.hpp"
#include <map>
#include <memory>
#include <string>
#include <vector>
namespace robot_control
{
/**
@@ -112,7 +113,7 @@ namespace robot_control
* @brief 检查关节限位
* @param joint_positions 关节位置（会被修改以满足限位）
*/
bool checkJointLimits(std::vector<double>& joint_positions);
bool CheckJointLimits(std::vector<double>& joint_positions);
/**
* @brief 设置回零位置
@@ -148,8 +149,8 @@ namespace robot_control
bool is_moving_; ///< 是否在运动
bool is_stopping_; ///< 是否在停止
double movingDurationTime_; ///< 运动持续时间
double stopDurationTime_; ///< 停止持续时间
double moving_duration_time_; ///< 运动持续时间
double stop_duration_time_; ///< 停止持续时间
bool is_joint_position_initialized_; ///< 关节位置是否已初始化

2
src/robot_control/include/controllers/leg_control.hpp
View File

@@ -51,7 +51,7 @@ namespace robot_control
bool SetMoveLegParametersInternal(double moveLegDistance);
private:
double calculate_angle_from_links(double side1, double side2, double side_opposite, bool is_degree);
double CalculateAngleFromLinks_(double side1, double side2, double side_opposite, bool is_degree);
std::vector<double> target_joint_positions_; ///< 目标关节位置
};
}

4
src/robot_control/include/controllers/wheel_control.hpp
View File

@@ -59,8 +59,8 @@ namespace robot_control
private:
// 成员变量（特定于轮子控制器）
double lastMoveDistance_; ///< 上次移动距离
double moveRatio_; ///< 移动比例
double last_move_distance_; ///< 上次移动距离
double move_ratio_; ///< 移动比例
std::vector<double> target_joint_positions_; ///< 目标关节位置
};
}

8
src/robot_control/include/core/controller_factory.hpp
View File

@@ -41,7 +41,7 @@ namespace robot_control
* @param motion_params 运动参数
* @return 控制器智能指针，如果类型无效返回nullptr
*/
static std::unique_ptr<ControlBase> create_controller(
static std::unique_ptr<ControlBase> CreateController(
ControllerType type,
const MotionParameters& motion_params);
@@ -51,7 +51,7 @@ namespace robot_control
* @param enabled_controllers 启用的控制器名称列表
* @return true 已启用，false 未启用
*/
static bool is_controller_enabled(
static bool IsControllerEnabled(
ControllerType type,
const std::vector<std::string>& enabled_controllers);
@@ -60,13 +60,13 @@ namespace robot_control
* @param type 控制器类型
* @return 控制器类型字符串（小写）
*/
static std::string controller_type_to_string(ControllerType type);
static std::string ControllerTypeToString(ControllerType type);
/**
* @brief 将字符串转换为控制器类型
* @param type_str 控制器类型字符串
* @return 控制器类型，如果无效返回UNKNOWN
*/
static ControllerType string_to_controller_type(const std::string& type_str);
static ControllerType StringToControllerType(const std::string& type_str);
};
}

11
src/robot_control/include/core/motion_parameters.hpp
View File

@@ -14,11 +14,11 @@
#pragma once
#include <vector>
#include <iostream>
#include "core/common_enum.hpp" // 包含 LimitType 枚举定义
#include <map>
#include <string>
#include "core/common_enum.hpp" // 包含 LimitType 枚举定义
#include <vector>
// ==================== 宏定义 ====================
@@ -92,10 +92,9 @@ namespace robot_control
current_position(0.0), current_velocity(0.0), current_acceleration(0.0), current_torque(0.0),
command_position(0.0), command_velocity(0.0), command_acceleration(0.0), command_torque(0.0)
{
// 如果最大值小于最小值，交换它们并输出警告
if (max_limit < min_limit)
if (max_limit < min_limit)
{
std::cerr << "[Warning] JointInfo: max_limit (" << max_limit << ") < min_limit (" << min_limit << "), swapping values!" << std::endl;
// max_limit < min_limit: swap silently to ensure valid range
std::swap(max_limit, min_limit);
}
}

28
src/robot_control/include/core/remote_controller.hpp
View File

@@ -60,56 +60,56 @@ namespace robot_control
* @brief 检查是否处于点动模式
* @return true 点动模式开启，false 点动模式关闭
*/
bool IsJogMode() const { return isJogMode_; }
bool IsJogMode() const { return is_jog_mode_; }
/**
* @brief 获取当前点动的关节索引
* @return 关节索引
*/
size_t GetJogIndex() const { return jogIndex_; }
size_t GetJogIndex() const { return jog_index_; }
/**
* @brief 获取当前点动方向
* @return 方向（-1, 0, 1）
*/
int GetJogDirection() const { return jogDirection_; }
int GetJogDirection() const { return jog_direction_; }
/**
* @brief 设置点动模式状态（用于外部控制）
* @param enabled true 启用点动模式，false 禁用点动模式
*/
void SetJogMode(bool enabled) { isJogMode_ = enabled; }
void SetJogMode(bool enabled) { is_jog_mode_ = enabled; }
/**
* @brief 检查是否有停止请求
* @return true 有停止请求，false 无停止请求
*/
bool HasStopRequest() const { return isStopping_; }
bool HasStopRequest() const { return is_stopping_; }
/**
* @brief 清除停止请求标志
*/
void ClearStopRequest() { isStopping_ = false; }
void ClearStopRequest() { is_stopping_ = false; }
private:
// ==================== 核心组件引用 ====================
rclcpp::Node* node_; ///< ROS 2节点指针（用于日志）
RobotControlManager& robotControlManager_; ///< 机器人控制管理器引用
RobotControlManager& robot_control_manager_; ///< 机器人控制管理器引用
// ==================== Jog 控制状态 ====================
bool isJogMode_; ///< 是否处于点动模式
int jogDirection_; ///< 点动方向（-1, 0, 1）
size_t jogIndex_; ///< 当前点动的关节索引
bool is_jog_mode_; ///< 是否处于点动模式
int jog_direction_; ///< 点动方向（-1, 0, 1）
size_t jog_index_; ///< 当前点动的关节索引
// ==================== 运动控制状态 ====================
bool isStopping_; ///< 是否有停止请求
bool is_stopping_; ///< 是否有停止请求
// ==================== 命令去重 ====================
std::string lastCommand_; ///< 上次处理的命令（用于去重）
std::string last_command_; ///< 上次处理的命令（用于去重）
// ==================== Jog 控制方法 ====================
@@ -151,13 +151,13 @@ namespace robot_control
* @param command 命令字符串
* @return true 重复命令，false 新命令
*/
bool IsDuplicateCommand(const std::string& command);
bool IsDuplicateCommand_(const std::string& command);
/**
* @brief 更新最后处理的命令
* @param command 命令字符串
*/
void UpdateLastCommand(const std::string& command);
void UpdateLastCommand_(const std::string& command);
};
}

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

@@ -13,10 +13,6 @@
#pragma once
#include <cmath>
#include <memory>
#include <vector>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "sensor_msgs/msg/imu.hpp"
@@ -34,6 +30,11 @@
#include "interfaces/msg/imu_msg.hpp"
#include "interfaces/msg/arm_motion_params.hpp"
#include <cmath>
#include <memory>
#include <string>
#include <vector>
using MotorPos = interfaces::msg::MotorPos;
using ImuMsg = sensor_msgs::msg::Imu;
using ArmMotionParams = interfaces::msg::ArmMotionParams;
@@ -294,14 +295,14 @@ namespace robot_control
/**
* @brief 初始化函数
*/
void init();
void Init_();
/**
* @brief 从关节名称列表获取索引列表（辅助函数）
* @param joint_names 关节名称列表
* @return 索引列表
*/
std::vector<size_t> GetJointIndicesFromNames(const std::vector<std::string>& joint_names) const;
std::vector<size_t> GetJointIndicesFromNames_(const std::vector<std::string>& joint_names) const;
/**
* @brief 按关节名构建子控制器所需的 positions/velocities/efforts
@@ -311,21 +312,21 @@ namespace robot_control
* @param out_efforts 输出力矩
* @return true 构建成功且有至少一个关节数据，false 失败或无有效数据
*/
bool BuildControllerJointStatesByNames(
bool BuildControllerJointStatesByNames_(
const std::vector<std::string>& joint_names,
std::vector<double>* out_positions,
std::vector<double>* out_velocities,
std::vector<double>* out_efforts) const;
// Quaternion conversion utilities (private helpers)
void QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
void QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
void QuaternionToRpyRad_(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
void QuaternionToRpyDeg_(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
// ==================== 成员变量 ====================
MotionParameters motionParams_; ///< 运动参数配置
MotionParameters motion_params_; ///< 运动参数配置
bool isWheelHomed_; ///< 轮子是否已回零
bool is_wheel_homed_; ///< 轮子是否已回零
bool is_wheel_jog_; ///< 轮子点动模式标志
@@ -342,32 +343,32 @@ namespace robot_control
bool arm_controller_enabled_; ///< 手臂控制器是否启用
// 运动状态
std::vector<double> wheelPositions_; ///< 轮子位置
std::vector<double> jointPositions_; ///< 关节位置（弧度）
std::vector<double> jointVelocities_; ///< 关节速度（弧度/秒）
std::vector<double> wheelVelocities_; ///< 轮子速度（弧度/秒）
std::vector<double> jointEfforts_; ///< 关节力矩
std::vector<double> wheel_positions_; ///< 轮子位置
std::vector<double> joint_positions_; ///< 关节位置（弧度）
std::vector<double> joint_velocities_; ///< 关节速度（弧度/秒）
std::vector<double> wheel_velocities_; ///< 轮子速度（弧度/秒）
std::vector<double> joint_efforts_; ///< 关节力矩
// 状态
std::vector<double> wheelCommands_; ///< 轮子命令（wheel joint order）
sensor_msgs::msg::JointState jointStates_; ///< 关节状态
std::vector<double> wheel_commands_; ///< 轮子命令（wheel joint order）
sensor_msgs::msg::JointState joint_states_; ///< 关节状态
// 临时变量（用于轮子控制器）
std::vector<double> tempWheelCmd_; ///< 轮子临时命令
std::vector<double> temp_wheel_cmd_; ///< 轮子临时命令
// IMU相关
std::vector<double> gyroValues_; ///< 陀螺仪值
std::vector<double> gyroInitValues_; ///< 陀螺仪初始值
std::vector<double> gyroVelocities_; ///< 陀螺仪速度
std::vector<double> gyroAccelerations_; ///< 陀螺仪加速度
sensor_msgs::msg::Imu imuMsg_; ///< IMU消息
bool isGyroInited_; ///< 陀螺仪是否已初始化
std::vector<double> gyro_values_; ///< 陀螺仪值
std::vector<double> gyro_init_values_; ///< 陀螺仪初始值
std::vector<double> gyro_velocities_; ///< 陀螺仪速度
std::vector<double> gyro_accelerations_; ///< 陀螺仪加速度
sensor_msgs::msg::Imu imu_msg_; ///< IMU消息
bool is_gyro_inited_; ///< 陀螺仪是否已初始化
// 轮子状态相关
interfaces::msg::MotorPos motorPos_; ///< 电机位置消息
interfaces::msg::MotorPos motor_pos_; ///< 电机位置消息
// 初始化相关
bool isJointInitValueSet_; ///< 关节初始化值是否已设置
bool is_joint_init_value_set_; ///< 关节初始化值是否已设置
bool joint_name_mapping_initialized_; ///< 关节名称映射是否已初始化
};
}

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

@@ -13,9 +13,6 @@
#pragma once
#include <memory>
#include <cstdint>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
@@ -35,6 +32,10 @@
#include "interfaces/srv/clear_arm_error.hpp"
#include "interfaces/msg/robot_arm_status.hpp"
#include <cstdint>
#include <memory>
#include <vector>
namespace robot_control
{
// 前向声明
@@ -69,11 +70,11 @@ namespace robot_control
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr left_gpio_pub_; ///< 左臂GPIO发布器
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr right_gpio_pub_; ///< 右臂GPIO发布器
rclcpp::Publisher<interfaces::msg::RobotArmStatus>::SharedPtr robot_arm_status_pub_; ///< 机械臂状态发布器
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joyCommandSub_; ///< 手柄命令订阅器
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_; ///< 关节状态订阅器
rclcpp::Subscription<MotorPos>::SharedPtr wheelStatesSub_; ///< 轮子状态订阅器
rclcpp::Subscription<ImuMsg>::SharedPtr imuMsgSub_; ///< IMU消息订阅器
rclcpp::Subscription<control_msgs::msg::DynamicJointState>::SharedPtr dynamicJointStatesSub_; ///< 动态关节状态订阅器
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joy_command_sub_; ///< 手柄命令订阅器
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_states_sub_; ///< 关节状态订阅器
rclcpp::Subscription<MotorPos>::SharedPtr wheel_states_sub_; ///< 轮子状态订阅器
rclcpp::Subscription<ImuMsg>::SharedPtr imu_msg_sub_; ///< IMU消息订阅器
rclcpp::Subscription<control_msgs::msg::DynamicJointState>::SharedPtr dynamic_joint_states_sub_; ///< 动态关节状态订阅器
rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_client_; ///< 控制器切换客户端
rclcpp::Service<interfaces::srv::InverseKinematics>::SharedPtr inverse_kinematics_srv_; ///< 单臂逆解服务
rclcpp::Service<interfaces::srv::SetArmEnable>::SharedPtr set_arm_enable_srv_; ///< 上下使能服务
@@ -82,9 +83,9 @@ namespace robot_control
// ==================== 核心组件 ====================
RobotControlManager robotControlManager_; ///< 机器人控制管理器
RobotControlManager robot_control_manager_; ///< 机器人控制管理器
std::unique_ptr<ActionManager> action_manager_; ///< 动作管理器
std::unique_ptr<RemoteController> remoteController_; ///< 遥控器控制器
std::unique_ptr<RemoteController> remote_controller_; ///< 遥控器控制器
std::unique_ptr<ArmHardwareService> arm_hardware_service_; ///< 机械臂GPIO服务实现
std::unique_ptr<KinematicsService> kinematics_service_; ///< 运动学服务实现
std::unique_ptr<ArmStatusService> arm_status_service_; ///< 机械臂状态服务实现
@@ -125,12 +126,12 @@ namespace robot_control
* @param controller_name 控制器名称
* @return 激活是否成功
*/
bool activateController(const std::string& controller_name);
bool ActivateController_(const std::string& controller_name);
/**
* @brief 重置所有电机故障
*/
void motor_reset_fault_all();
void MotorResetFaultAll_();
/**
* @brief 启用所有电机
@@ -143,18 +144,18 @@ namespace robot_control
* @param id 关节序号（0=全部；否则按 dual_arm_joint_names_ 的 index+1 语义）
* @param fault fault 值（通常 1 表示复位脉冲，0 表示释放）
*/
void motor_fault(int id, double fault);
void MotorFault_(int id, double fault);
/**
* @brief 设置电机 enable（参考 test_motor.cpp）
* @param id 关节序号（0=全部；否则按 dual_arm_joint_names_ 的 index+1 语义）
* @param enable 使能值（1=使能，0=禁用）
*/
void motor_enable(int id, double enable);
void MotorEnable_(int id, double enable);
/**
* @brief 根据 dual_arm_joint_names_ 拆分左右臂关节名称
*/
bool split_arm_joint_names_(
bool SplitArmJointNames_(
const MotionParameters& motion_params,
std::vector<std::string>& left,
std::vector<std::string>& right) const;
@@ -168,7 +169,7 @@ namespace robot_control
* @param value 指令值
* @param offset 该列表在全局双臂数组中的起始偏移
*/
void publish_gpio_command_(
void PublishGpioCommand_(
const rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr& pub,
const std::vector<std::string>& joints,
const std::string& interface_name,

24
src/robot_control/include/services/arm_hardware_service.hpp
View File

@@ -1,3 +1,8 @@
/**
* @file arm_hardware_service.hpp
* @brief 手臂硬件服务，提供使能与错误清除接口
*/
#pragma once
#include <cstdint>
@@ -16,6 +21,15 @@ namespace robot_control
class ArmHardwareService
{
public:
/**
* @brief 构造函数，初始化手臂硬件服务
* @param node ROS 2 节点指针
* @param manager 机器人控制管理器指针
* @param left_gpio_pub 左臂 GPIO 发布器
* @param right_gpio_pub 右臂 GPIO 发布器
* @param joint_enabled_cache 关节使能缓存指针
* @param joint_error_code_cache 关节错误码缓存指针
*/
ArmHardwareService(
rclcpp::Node* node,
RobotControlManager* manager,
@@ -24,10 +38,20 @@ public:
std::vector<bool>* joint_enabled_cache,
std::vector<int32_t>* joint_error_code_cache);
/**
* @brief 处理设置手臂使能服务请求
* @param request 服务请求（包含手臂 ID、关节编号及使能标志）
* @param response 服务响应（包含操作成功标志）
*/
void HandleSetArmEnable(
const std::shared_ptr<interfaces::srv::SetArmEnable::Request> request,
std::shared_ptr<interfaces::srv::SetArmEnable::Response> response);
/**
* @brief 处理清除手臂错误服务请求
* @param request 服务请求（包含手臂 ID 和关节编号）
* @param response 服务响应（包含操作成功标志）
*/
void HandleClearArmError(
const std::shared_ptr<interfaces::srv::ClearArmError::Request> request,
std::shared_ptr<interfaces::srv::ClearArmError::Response> response);

23
src/robot_control/include/services/arm_status_service.hpp
View File

@@ -1,3 +1,8 @@
/**
* @file arm_status_service.hpp
* @brief 手臂状态服务，周期性发布末端位姿
*/
#pragma once
#include <cstdint>
@@ -16,6 +21,15 @@ namespace robot_control
class ArmStatusService
{
public:
/**
* @brief 构造函数，初始化手臂状态服务
* @param node ROS 2 节点指针
* @param manager 机器人控制管理器指针
* @param safety_service 安全服务指针
* @param status_pub 手臂状态发布器
* @param joint_enabled_cache 关节使能缓存指针
* @param joint_error_code_cache 关节错误码缓存指针
*/
ArmStatusService(
rclcpp::Node* node,
RobotControlManager* manager,
@@ -24,7 +38,15 @@ public:
std::vector<bool>* joint_enabled_cache,
std::vector<int32_t>* joint_error_code_cache);
/**
* @brief 处理动态关节状态消息回调
* @param msg 动态关节状态消息
*/
void OnDynamicJointStates(const control_msgs::msg::DynamicJointState::SharedPtr msg);
/**
* @brief 发布机器人手臂状态
*/
void PublishRobotArmStatus();
private:
@@ -34,6 +56,7 @@ private:
rclcpp::Publisher<interfaces::msg::RobotArmStatus>::SharedPtr status_pub_;
std::vector<bool>* joint_enabled_cache_{nullptr};
std::vector<int32_t>* joint_error_code_cache_{nullptr};
bool publish_arm_pose_{true};
};
} // namespace robot_control

15
src/robot_control/include/services/kinematics_service.hpp
View File

@@ -1,3 +1,8 @@
/**
* @file kinematics_service.hpp
* @brief 运动学服务，提供 IK 解算 ROS 接口
*/
#pragma once
#include <memory>
@@ -12,8 +17,18 @@ namespace robot_control
class KinematicsService
{
public:
/**
* @brief 构造函数，初始化运动学服务
* @param node ROS 2 节点指针
* @param manager 机器人控制管理器指针
*/
KinematicsService(rclcpp::Node* node, RobotControlManager* manager);
/**
* @brief 处理逆运动学求解服务请求
* @param request 服务请求（包含目标位姿和手臂 ID）
* @param response 服务响应（包含关节角解和成功标志）
*/
void HandleInverseKinematics(
const std::shared_ptr<interfaces::srv::InverseKinematics::Request> request,
std::shared_ptr<interfaces::srv::InverseKinematics::Response> response);

8
src/robot_control/include/services/motor_service.hpp
View File

@@ -28,6 +28,10 @@ public:
using MotorCmd = interfaces::msg::MotorCmd;
using MotorParam = interfaces::srv::MotorParam;
/**
* @brief 构造函数，初始化电机服务并创建发布器与服务客户端
* @param node ROS 2 节点指针
*/
explicit MotorService(rclcpp::Node* node);
/**
@@ -36,13 +40,13 @@ public:
* @param ratio 速度比例（由 RobotControlManager 提供）
* @param is_jog_mode 当前是否处于点动模式
*/
void configure_wheel_speed(double move_distance, double ratio, bool is_jog_mode);
void ConfigureWheelSpeed(double move_distance, double ratio, bool is_jog_mode);
/**
* @brief 发布轮子角度命令（封装 MotorCmd 发布逻辑）
* @param wheel_commands 轮子目标角度（至少包含两个元素）
*/
void publish_wheel_command(const std::vector<double>& wheel_commands);
void PublishWheelCommand(const std::vector<double>& wheel_commands);
private:
rclcpp::Node* node_{nullptr};

28
src/robot_control/include/services/safety_service.hpp
View File

@@ -1,3 +1,8 @@
/**
* @file safety_service.hpp
* @brief 安全服务，管理急停状态与解锁
*/
#pragma once
#include <atomic>
@@ -16,14 +21,37 @@ public:
using EStopState = interfaces::msg::EStopState;
using ResetEStop = interfaces::srv::ResetEStop;
/**
* @brief 构造函数，初始化安全服务并创建订阅与服务端
* @param node ROS 2 节点指针
*/
explicit SafetyService(rclcpp::Node* node);
/**
* @brief 处理急停状态消息回调
* @param msg 急停状态消息
*/
void OnEStopState(const EStopState::SharedPtr msg);
/**
* @brief 处理急停复位服务请求
* @param request 服务请求
* @param response 服务响应
*/
void HandleResetEStop(
const std::shared_ptr<ResetEStop::Request> request,
std::shared_ptr<ResetEStop::Response> response);
/**
* @brief 查询当前急停是否已锁定
* @return true 急停已锁定，false 未锁定
*/
bool IsEstopLatched() const { return estop_latched_.load(); }
/**
* @brief 获取当前急停生成计数（每次复位后递增）
* @return 急停生成计数
*/
uint32_t GetEstopGeneration() const { return estop_generation_.load(); }
private:

11
src/robot_control/include/utils/deceleration_planner.hpp
View File

@@ -1,3 +1,8 @@
/**
* @file deceleration_planner.hpp
* @brief 减速规划工具函数
*/
#pragma once
#include <vector>
@@ -5,6 +10,12 @@
#include <cmath>
#include <stdexcept>
/**
* @brief 计算从给定初速度以最大减速度减速至零所需的时间
* @param initial_velocity 初始速度（m/s 或 rad/s）
* @param max_decel 最大减速度（正数，m/s² 或 rad/s²）
* @return 所需减速时间（秒）；初速度为零时返回 0
*/
inline double calculate_decel_time(double initial_velocity, double max_decel) {
if (std::fabs(initial_velocity) < 1e-9) { // 速度为0，无需减速
return 0.0;

4
src/robot_control/include/utils/duration_utils.hpp
View File

@@ -5,10 +5,10 @@
#pragma once
#include <cmath>
#include "builtin_interfaces/msg/duration.hpp"
#include <cmath>
namespace robot_control
{

37
src/robot_control/include/utils/s_curve_trajectory.hpp
View File

@@ -1,3 +1,8 @@
/**
* @file s_curve_trajectory.hpp
* @brief S 曲线轨迹生成器（支持多轴同步）
*/
#pragma once
#include <vector>
@@ -45,98 +50,98 @@ public:
* @param start_pos 起始位置（向量，长度=轴数）
* @param target_pos 目标位置（向量，长度=轴数）
*/
void init(const std::vector<double>& start_pos, const std::vector<double>& target_pos);
void Init(const std::vector<double>& start_pos, const std::vector<double>& target_pos);
/**
* @brief 初始化单轴轨迹规划
* @param start_pos 起始位置
* @param target_pos 目标位置
*/
void init(double start_pos, double target_pos);
void Init(double start_pos, double target_pos);
/**
* @brief 更新多轴轨迹，计算当前时间的位置
* @param time 已运动时间（s）
* @return 当前位置向量
*/
std::vector<double> update(double time);
std::vector<double> Update(double time);
/**
* @brief 更新单轴轨迹，计算当前时间的位置
* @param time 已运动时间（s）
* @return 当前位置
*/
double updateSingle(double time);
double UpdateSingle(double time);
/**
* @brief 获取当前速度（多轴）
*/
std::vector<double> getVelocity() const { return current_velocity_; }
std::vector<double> GetVelocity() const { return current_velocity_; }
/**
* @brief 获取当前速度（单轴）
*/
double getSingleVelocity() const { return current_velocity_[0]; }
double GetSingleVelocity() const { return current_velocity_[0]; }
/**
* @brief 获取当前加速度（多轴）
*/
std::vector<double> getAcceleration() const { return current_acceleration_; }
std::vector<double> GetAcceleration() const { return current_acceleration_; }
/**
* @brief 获取当前加速度（单轴）
*/
double getSingleAcceleration() const { return current_acceleration_[0]; }
double GetSingleAcceleration() const { return current_acceleration_[0]; }
/**
* @brief 检查轨迹是否已完成
* @param time 已运动时间（s）
* @return 完成返回true
*/
bool isFinished(double time) const { return time >= total_time_; }
bool IsFinished(double time) const { return time >= total_time_; }
/**
* @brief 设置各轴最大速度
* @param max_velocities 新的最大速度向量（长度=轴数）
*/
void setMaxVelocities(const std::vector<double>& max_velocities);
void SetMaxVelocities(const std::vector<double>& max_velocities);
/**
* @brief 设置单轴最大速度
* @param max_velocity 新的最大速度
*/
void setMaxVelocity(double max_velocity);
void SetMaxVelocity(double max_velocity);
/**
* @brief 设置各轴最大加速度
* @param max_accelerations 新的最大加速度向量（长度=轴数）
*/
void setMaxAccelerations(const std::vector<double>& max_accelerations);
void SetMaxAccelerations(const std::vector<double>& max_accelerations);
/**
* @brief 设置单轴最大加速度
* @param max_acceleration 新的最大加速度
*/
void setMaxAcceleration(double max_acceleration);
void SetMaxAcceleration(double max_acceleration);
/**
* @brief 计算急停轨迹参数（当前状态下减速到停止）
*/
void calculateStopTrajectory();
void CalculateStopTrajectory();
/**
* @brief 更新急停轨迹
* @param dt 从急停开始的时间（s）
* @return 当前位置
*/
std::vector<double> updateStop(double dt);
std::vector<double> UpdateStop(double dt);
/**
* @brief 检查急停是否完成
* @param dt 从急停开始的时间（s）
* @return 完成返回true
*/
bool isStopFinished(double dt) const { return dt >= stop_total_time_; }
bool IsStopFinished(double dt) const { return dt >= stop_total_time_; }
private:
/**

40
src/robot_control/include/utils/trapezoidal_trajectory.hpp
View File

@@ -1,8 +1,14 @@
/**
* @file trapezoidal_trajectory.hpp
* @brief 梯形速度轨迹生成器（支持多轴同步）
*/
#pragma once
#include <vector>
#include <Eigen/Dense>
#include <vector>
namespace robot_control
{
@@ -39,7 +45,7 @@ public:
* @param target_pos 目标位置（向量，长度=轴数）
* @param duration 期望运动时间（若>0且>最小时间则强制使用该时间）
*/
void init(const std::vector<double>& start_pos, const std::vector<double>& target_pos);
void Init(const std::vector<double>& start_pos, const std::vector<double>& target_pos);
/**
* @brief 初始化单轴轨迹规划
@@ -47,91 +53,91 @@ public:
* @param target_pos 目标位置
* @param duration 期望运动时间（若>0且>最小时间则强制使用该时间）
*/
void init(double start_pos, double target_pos);
void Init(double start_pos, double target_pos);
/**
* @brief 更新多轴轨迹，计算当前时间的位置
* @param time 已运动时间（s）
* @return 当前位置向量
*/
std::vector<double> update(double time);
std::vector<double> Update(double time);
/**
* @brief 更新单轴轨迹，计算当前时间的位置
* @param time 已运动时间（s）
* @return 当前位置
*/
double updateSingle(double time);
double UpdateSingle(double time);
/**
* @brief 获取当前速度（多轴）
*/
std::vector<double> getVelocity() const { return current_velocity_; }
std::vector<double> GetVelocity() const { return current_velocity_; }
/**
* @brief 获取当前速度（单轴）
*/
double getSingleVelocity() const { return current_velocity_[0]; }
double GetSingleVelocity() const { return current_velocity_[0]; }
/**
* @brief 获取当前加速度（多轴）
*/
std::vector<double> getAcceleration() const { return current_acceleration_; }
std::vector<double> GetAcceleration() const { return current_acceleration_; }
/**
* @brief 获取当前加速度（单轴）
*/
double getSingleAcceleration() const { return current_acceleration_[0]; }
double GetSingleAcceleration() const { return current_acceleration_[0]; }
/**
* @brief 检查轨迹是否已完成
* @param time 已运动时间（s）
* @return 完成返回true
*/
bool isFinished(double time) const { return time >= total_time_; }
bool IsFinished(double time) const { return time >= total_time_; }
/**
* @brief 设置各轴最大速度
* @param max_velocities 新的最大速度向量（长度=轴数）
*/
void setMaxVelocities(const std::vector<double>& max_velocities);
void SetMaxVelocities(const std::vector<double>& max_velocities);
/**
* @brief 设置单轴最大速度
* @param max_velocity 新的最大速度
*/
void setMaxVelocity(double max_velocity);
void SetMaxVelocity(double max_velocity);
/**
* @brief 设置各轴最大加速度
* @param max_accelerations 新的最大加速度向量（长度=轴数）
*/
void setMaxAccelerations(const std::vector<double>& max_accelerations);
void SetMaxAccelerations(const std::vector<double>& max_accelerations);
/**
* @brief 设置单轴最大加速度
* @param max_acceleration 新的最大加速度
*/
void setMaxAcceleration(double max_acceleration);
void SetMaxAcceleration(double max_acceleration);
/**
* @brief 计算急停轨迹参数（当前状态下减速到停止）
*/
void calculateStopTrajectory();
void CalculateStopTrajectory();
/**
* @brief 更新急停轨迹
* @param dt 从急停开始的时间（s）
* @return 当前位置
*/
std::vector<double> updateStop(double dt);
std::vector<double> UpdateStop(double dt);
/**
* @brief 检查急停是否完成
* @param dt 从急停开始的时间（s）
* @return 完成返回true
*/
bool isStopFinished(double dt) const { return dt >= stop_total_time_; }
bool IsStopFinished(double dt) const { return dt >= stop_total_time_; }
private:
/**

BIN
src/robot_control/launch/__pycache__/dual_arm_mock_test.launch.cpython-310.pyc
View File

Binary file not shown.

BIN
src/robot_control/launch/__pycache__/robot_control.launch.cpython-310.pyc Normal file
View File

Binary file not shown.

175
src/robot_control/launch/dual_arm_mock_test.launch.py
View File

@@ -1,36 +1,139 @@
"""Launch robot_control + mock dual-arm hardware for no-hardware tests."""
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_prefix, get_package_share_directory
import os
def generate_launch_description() -> LaunchDescription:
"""Generate launch description for DualArm no-hardware tests."""
robot_control_share = get_package_share_directory("robot_control")
robot_control_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(robot_control_share, "launch", "robot_control.launch.py")
)
)
mock_script = os.path.join(
get_package_prefix("robot_control"),
"lib",
"robot_control",
"mock_dual_arm_hardware.py",
)
mock_hw = ExecuteProcess(
cmd=["python3", mock_script],
output="screen",
)
return LaunchDescription(
[
robot_control_launch,
mock_hw,
]
)
"""Launch robot_control + ros2_control fake hardware for no-hardware integration tests.
Architecture
------------
Fake hardware layer (ros2_control, t=0s):
- ros2_control_node with mock_components/GenericSystem
→ joints teleport instantly to commanded position (no physics)
- joint_state_broadcaster → publishes /joint_states
- left_arm_controller ┐
- right_arm_controller ┘ → FollowJointTrajectory action servers
- robot_state_publisher → TF for /tf, /tf_static
Control layer (robot_control.launch.py, t=3s):
- move_group (MoveIt motion planning)
- robot_control_node (delayed another 8s = t≈11s total)
No MuJoCo or extra Python nodes required.
"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
OpaqueFunction,
TimerAction,
)
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
# mock hardware (ros2_control) must be stable before robot_control stack starts
_ROBOT_CONTROL_STARTUP_DELAY_SEC = 3.0
def _build_hardware_nodes(context, *args, **kwargs):
"""Read URDF and create ros2_control_node + robot_state_publisher."""
use_sim_time_str = LaunchConfiguration("use_sim_time").perform(context)
use_sim_time = use_sim_time_str.lower() == "true"
dual_arm_description_share = get_package_share_directory("dual_arm_description")
dual_arm_moveit_share = get_package_share_directory("dual_arm_moveit_config")
urdf_path = os.path.join(dual_arm_description_share, "urdf", "dual_arm.urdf")
with open(urdf_path, "r") as f:
robot_description = f.read()
ros2_controllers_yaml = os.path.join(
dual_arm_moveit_share, "config", "ros2_controllers.yaml"
)
# ros2_control_node: loads mock_components/GenericSystem from <ros2_control> in URDF
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[
{"robot_description": robot_description, "use_sim_time": use_sim_time},
ros2_controllers_yaml,
],
output="screen",
)
# robot_state_publisher: publishes TF from /joint_states + robot_description
rsp_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[
{"robot_description": robot_description, "use_sim_time": use_sim_time}
],
output="screen",
)
return [ros2_control_node, rsp_node]
def _spawn_controllers(context, *args, **kwargs):
"""Spawn joint_state_broadcaster and arm trajectory controllers."""
controllers = [
"joint_state_broadcaster",
"left_arm_controller",
"right_arm_controller",
]
return [
Node(
package="controller_manager",
executable="spawner",
arguments=[ctrl],
output="screen",
)
for ctrl in controllers
]
def generate_launch_description() -> LaunchDescription:
"""Generate launch description for DualArm no-hardware tests.
Launch arguments:
use_sim_time (bool, default false) Use ROS sim clock.
Usage:
ros2 launch robot_control dual_arm_mock_test.launch.py
ros2 launch robot_control dual_arm_mock_test.launch.py use_sim_time:=false
"""
use_sim_time_arg = DeclareLaunchArgument(
"use_sim_time",
default_value="false",
description="Use simulation clock if true.",
)
# t=0: fake hardware layer
hardware_nodes = OpaqueFunction(function=_build_hardware_nodes)
controller_spawners = OpaqueFunction(function=_spawn_controllers)
# t=3s: MoveIt (move_group) + robot_control_node
robot_control_share = get_package_share_directory("robot_control")
robot_control_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(robot_control_share, "launch", "robot_control.launch.py")
),
launch_arguments={
"use_sim_time": LaunchConfiguration("use_sim_time"),
"publish_arm_pose": "false",
}.items(),
)
robot_control_delayed = TimerAction(
period=_ROBOT_CONTROL_STARTUP_DELAY_SEC,
actions=[robot_control_launch],
)
return LaunchDescription(
[
use_sim_time_arg,
hardware_nodes, # t=0: ros2_control_node + rsp
controller_spawners, # t=0: joint_state_broadcaster + arm controllers
robot_control_delayed, # t=3s: move_group → robot_control_node (t≈11s)
]
)

42
src/robot_control/launch/right_arm_joint_test.launch.py
View File

@@ -1,42 +0,0 @@
"""
Launch file for right arm joint test node
"""
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
# 声明启动参数
use_action_arg = DeclareLaunchArgument(
'use_action',
default_value='true',
description='Whether to use FollowJointTrajectory action (true) or MoveGroupInterface::execute() (false)'
)
action_name_arg = DeclareLaunchArgument(
'action_name',
default_value='/right_arm_controller/follow_joint_trajectory',
description='Action server name for FollowJointTrajectory'
)
# 创建节点
right_arm_joint_test_node = Node(
package='robot_control',
executable='right_arm_joint_test',
name='right_arm_joint_test',
output='screen',
parameters=[{
'use_action': LaunchConfiguration('use_action'),
'action_name': LaunchConfiguration('action_name'),
}]
)
return LaunchDescription([
use_action_arg,
action_name_arg,
right_arm_joint_test_node,
])

45
src/robot_control/launch/robot_control.launch.py
View File

@@ -1,13 +1,17 @@
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import TimerAction
from launch.actions import IncludeLaunchDescription, OpaqueFunction
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from ament_index_python.packages import get_package_share_directory
import os
# move_group needs time to load URDF/SRDF and start internal services before
# robot_control_node calls MoveGroupInterface() in its constructor.
_ROBOT_CONTROL_NODE_STARTUP_DELAY_SEC = 8.0
def load_urdf_srdf(context, *args, **kwargs):
"""加载URDF和SRDF文件内容"""
nodes = []
@@ -27,13 +31,16 @@ def load_urdf_srdf(context, *args, **kwargs):
srdf_content = f.read()
# 创建robot_control_node，并传递URDF和SRDF参数
use_sim_time = LaunchConfiguration('use_sim_time').perform(context).lower() == 'true'
publish_arm_pose = LaunchConfiguration('publish_arm_pose').perform(context).lower() == 'true'
robot_control_node = Node(
package='robot_control',
executable='robot_control_node',
name='robot_control_node',
output='screen',
parameters=[{
'use_sim_time': False,
'use_sim_time': use_sim_time,
'arm_status.publish_arm_pose': publish_arm_pose,
'robot_description': urdf_content,
'robot_description_semantic': srdf_content
}]
@@ -43,6 +50,19 @@ def load_urdf_srdf(context, *args, **kwargs):
return nodes
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time')
publish_arm_pose = LaunchConfiguration('publish_arm_pose')
use_sim_time_arg = DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation clock if true.'
)
publish_arm_pose_arg = DeclareLaunchArgument(
'publish_arm_pose',
default_value='true',
description='Publish arm end-effector pose in robot arm status.'
)
# 获取dual_arm_moveit_config包的路径
dual_arm_moveit_config_path = get_package_share_directory('dual_arm_moveit_config')
@@ -52,15 +72,26 @@ def generate_launch_description():
move_group_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
dual_arm_moveit_config_path, '/launch/move_group.launch.py'
])
]),
launch_arguments={
'use_sim_time': use_sim_time
}.items()
)
# 使用OpaqueFunction来加载URDF和SRDF并创建节点
robot_control_nodes = OpaqueFunction(function=load_urdf_srdf)
# 延迟启动 robot_control_node，等待 move_group 完成初始化
robot_control_nodes_delayed = TimerAction(
period=_ROBOT_CONTROL_NODE_STARTUP_DELAY_SEC,
actions=[robot_control_nodes],
)
# 组装launch描述
# move_group需要在robot_control_node之前启动
return LaunchDescription([
move_group_launch, # 先启动move_group
robot_control_nodes # 然后启动robot_control_node
use_sim_time_arg,
publish_arm_pose_arg,
move_group_launch, # 先启动move_group
robot_control_nodes_delayed, # 延迟启动robot_control_node
])

BIN
src/robot_control/scripts/__pycache__/mock_dual_arm_hardware.cpython-310.pyc
View File

Binary file not shown.

BIN
src/robot_control/scripts/__pycache__/run_dual_arm_mock_tests.cpython-310.pyc
View File

Binary file not shown.

151
src/robot_control/scripts/analyze_trajectory_metrics.py
View File

@@ -1,151 +0,0 @@
#!/usr/bin/env python3
# Copyright (c) 2026 HiveCore. All rights reserved.
#
# @file analyze_trajectory_metrics.py
# @brief 轨迹平稳性/平滑性分析工具（基于 DualArm 导出的 CSV）
"""Analyze trajectory smoothness metrics from robot_control CSV outputs."""
from __future__ import annotations
import argparse
import csv
import glob
import math
import os
from dataclasses import dataclass
from typing import Dict, List, Sequence, Tuple
@dataclass
class JointMetrics:
"""Per-joint smoothness metrics."""
max_abs_vel: float
max_abs_acc: float
max_abs_jerk: float
rms_jerk: float
def finite_diff(values: Sequence[float], times: Sequence[float]) -> List[float]:
"""Compute first-order finite difference with non-uniform dt."""
if len(values) < 2:
return []
out: List[float] = []
for i in range(len(values) - 1):
dt = max(times[i + 1] - times[i], 1e-9)
out.append((values[i + 1] - values[i]) / dt)
return out
def rms(values: Sequence[float]) -> float:
"""Root-mean-square value."""
if not values:
return 0.0
return math.sqrt(sum(v * v for v in values) / float(len(values)))
def load_csv(path: str) -> Tuple[List[float], Dict[str, List[float]]]:
"""Load trajectory csv: time + joint_position columns."""
times: List[float] = []
columns: Dict[str, List[float]] = {}
with open(path, "r", encoding="utf-8") as file:
reader = csv.DictReader(file)
if reader.fieldnames is None:
raise ValueError(f"Invalid CSV header: {path}")
if "time" not in reader.fieldnames:
raise ValueError(f"CSV missing 'time' column: {path}")
joint_cols = [name for name in reader.fieldnames if name != "time"]
for joint in joint_cols:
columns[joint] = []
for row in reader:
times.append(float(row["time"]))
for joint in joint_cols:
columns[joint].append(float(row[joint]))
return times, columns
def analyze_joint(times: Sequence[float], positions: Sequence[float]) -> JointMetrics:
"""Analyze one joint trajectory."""
vel = finite_diff(positions, times)
if len(vel) < 2:
return JointMetrics(0.0, 0.0, 0.0, 0.0)
t_vel = list(times[1:])
acc = finite_diff(vel, t_vel)
if len(acc) < 2:
return JointMetrics(max(abs(v) for v in vel), 0.0, 0.0, 0.0)
t_acc = t_vel[1:]
jerk = finite_diff(acc, t_acc)
max_vel = max(abs(v) for v in vel) if vel else 0.0
max_acc = max(abs(v) for v in acc) if acc else 0.0
max_jerk = max(abs(v) for v in jerk) if jerk else 0.0
return JointMetrics(max_vel, max_acc, max_jerk, rms(jerk))
def analyze_file(path: str) -> Dict[str, JointMetrics]:
"""Analyze all joints from one file."""
times, columns = load_csv(path)
if len(times) < 4:
raise ValueError(f"Not enough points in {path}")
return {joint: analyze_joint(times, values) for joint, values in columns.items()}
def pick_latest_csvs(directory: str, limit: int) -> List[str]:
"""Pick latest trajectory_*.csv files by modified time."""
pattern = os.path.join(directory, "trajectory_*.csv")
candidates = glob.glob(pattern)
candidates.sort(key=os.path.getmtime, reverse=True)
return candidates[:limit]
def main() -> None:
"""CLI entry point."""
parser = argparse.ArgumentParser(
description="Analyze smoothness metrics from trajectory csv files."
)
parser.add_argument(
"--csv",
nargs="*",
default=[],
help="CSV files to analyze. If empty, use latest files in --dir.",
)
parser.add_argument(
"--dir",
default=".",
help="Directory for auto-discovery (default: current directory).",
)
parser.add_argument(
"--latest",
type=int,
default=4,
help="Number of latest csv files when --csv not provided.",
)
args = parser.parse_args()
files = list(args.csv)
if not files:
files = pick_latest_csvs(args.dir, args.latest)
if not files:
raise SystemExit("No trajectory csv files found.")
for path in files:
metrics = analyze_file(path)
print(f"\n=== {path} ===")
for joint_name, item in metrics.items():
print(
f"{joint_name:32s} "
f"max|v|={item.max_abs_vel:8.4f} "
f"max|a|={item.max_abs_acc:8.4f} "
f"max|j|={item.max_abs_jerk:9.4f} "
f"rms(j)={item.rms_jerk:9.4f}"
)
if __name__ == "__main__":
main()

334
src/robot_control/scripts/mock_dual_arm_hardware.py
View File

@@ -1,334 +0,0 @@
#!/usr/bin/env python3
# Copyright (c) 2026 HiveCore. All rights reserved.
#
# @file mock_dual_arm_hardware.py
# @brief Mock 双臂硬件节点：提供左右 FollowJointTrajectory 并发布 /joint_states
"""Mock dual-arm hardware node for no-hardware integration tests.
This node provides:
1) Two FollowJointTrajectory action servers:
- /left_arm_controller/follow_joint_trajectory
- /right_arm_controller/follow_joint_trajectory
2) A continuous /joint_states publisher to satisfy robot_control init logic.
"""
from __future__ import annotations
import threading
import time
from dataclasses import dataclass
from typing import Dict, List, Optional, Tuple
import rclpy
from builtin_interfaces.msg import Duration
from control_msgs.action import FollowJointTrajectory
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.action.server import ServerGoalHandle
from rclpy.node import Node
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
def duration_to_sec(duration: Duration) -> float:
"""Convert ROS Duration to seconds."""
return float(duration.sec) + float(duration.nanosec) * 1e-9
@dataclass
class ArmExecutionState:
"""Runtime state for one arm trajectory playback."""
active: bool
start_time_sec: float
joint_names: List[str]
points: List[JointTrajectoryPoint]
current_positions: List[float]
current_velocities: List[float]
finished: bool
class MockDualArmHardware(Node):
"""Mock dual-arm hardware for robot_control integration tests."""
def __init__(self) -> None:
super().__init__("mock_dual_arm_hardware")
default_joint_names = [
"left_arm_joint_1",
"left_arm_joint_2",
"left_arm_joint_3",
"left_arm_joint_4",
"left_arm_joint_5",
"left_arm_joint_6",
"right_arm_joint_1",
"right_arm_joint_2",
"right_arm_joint_3",
"right_arm_joint_4",
"right_arm_joint_5",
"right_arm_joint_6",
]
self.declare_parameter("joint_names", default_joint_names)
self.declare_parameter("publish_rate_hz", 100.0)
self.declare_parameter("goal_start_delay_sec", 0.02)
self._joint_names: List[str] = (
self.get_parameter("joint_names").get_parameter_value().string_array_value
)
self._publish_rate_hz: float = (
self.get_parameter("publish_rate_hz").get_parameter_value().double_value
)
self._goal_start_delay_sec: float = (
self.get_parameter("goal_start_delay_sec").get_parameter_value().double_value
)
self._publish_rate_hz = max(self._publish_rate_hz, 20.0)
self._goal_start_delay_sec = max(self._goal_start_delay_sec, 0.0)
self._left_joint_names = self._joint_names[:6]
self._right_joint_names = self._joint_names[6:12]
self._lock = threading.Lock()
self._arm_states: Dict[str, ArmExecutionState] = {
"left": ArmExecutionState(
active=False,
start_time_sec=0.0,
joint_names=self._left_joint_names.copy(),
points=[],
current_positions=[0.0] * len(self._left_joint_names),
current_velocities=[0.0] * len(self._left_joint_names),
finished=False,
),
"right": ArmExecutionState(
active=False,
start_time_sec=0.0,
joint_names=self._right_joint_names.copy(),
points=[],
current_positions=[0.0] * len(self._right_joint_names),
current_velocities=[0.0] * len(self._right_joint_names),
finished=False,
),
}
self._joint_pub = self.create_publisher(JointState, "/joint_states", 10)
self._pub_timer = self.create_timer(
1.0 / self._publish_rate_hz, self._on_publish_joint_states
)
self._left_server = ActionServer(
self,
FollowJointTrajectory,
"/left_arm_controller/follow_joint_trajectory",
goal_callback=self._make_goal_callback("left"),
cancel_callback=self._make_cancel_callback("left"),
execute_callback=self._make_execute_callback("left"),
)
self._right_server = ActionServer(
self,
FollowJointTrajectory,
"/right_arm_controller/follow_joint_trajectory",
goal_callback=self._make_goal_callback("right"),
cancel_callback=self._make_cancel_callback("right"),
execute_callback=self._make_execute_callback("right"),
)
self.get_logger().info(
f"Mock dual-arm hardware started. publish_rate={self._publish_rate_hz:.1f} Hz"
)
def _make_goal_callback(self, arm: str):
"""Build per-arm goal callback."""
def _goal_callback(goal_request: FollowJointTrajectory.Goal) -> GoalResponse:
if not goal_request.trajectory.points:
self.get_logger().error(f"[{arm}] Reject goal: empty trajectory")
return GoalResponse.REJECT
if not self._validate_joint_names(arm, goal_request.trajectory.joint_names):
self.get_logger().error(
f"[{arm}] Reject goal: joint names mismatch {goal_request.trajectory.joint_names}"
)
return GoalResponse.REJECT
return GoalResponse.ACCEPT
return _goal_callback
def _make_cancel_callback(self, arm: str):
"""Build per-arm cancel callback."""
def _cancel_callback(_: ServerGoalHandle) -> CancelResponse:
self.get_logger().info(f"[{arm}] Cancel request accepted")
with self._lock:
state = self._arm_states[arm]
state.active = False
state.finished = True
state.current_velocities = [0.0] * len(state.current_velocities)
return CancelResponse.ACCEPT
return _cancel_callback
def _make_execute_callback(self, arm: str):
"""Build per-arm execute callback."""
def _execute_callback(
goal_handle: ServerGoalHandle,
) -> FollowJointTrajectory.Result:
goal = goal_handle.request
trajectory = goal.trajectory
with self._lock:
state = self._arm_states[arm]
state.active = True
state.finished = False
state.joint_names = list(trajectory.joint_names)
state.points = list(trajectory.points)
state.start_time_sec = time.time() + self._goal_start_delay_sec
first_point = trajectory.points[0]
if first_point.positions:
state.current_positions = list(first_point.positions)
if first_point.velocities:
state.current_velocities = list(first_point.velocities)
else:
state.current_velocities = [0.0] * len(state.current_positions)
while rclpy.ok():
if goal_handle.is_cancel_requested:
with self._lock:
state = self._arm_states[arm]
state.active = False
state.finished = True
state.current_velocities = [0.0] * len(state.current_velocities)
goal_handle.canceled()
result = FollowJointTrajectory.Result()
result.error_code = FollowJointTrajectory.Result.SUCCESSFUL
result.error_string = "Goal canceled"
return result
with self._lock:
finished = self._arm_states[arm].finished
if finished:
break
time.sleep(0.005)
goal_handle.succeed()
result = FollowJointTrajectory.Result()
result.error_code = FollowJointTrajectory.Result.SUCCESSFUL
result.error_string = "Mock trajectory completed"
return result
return _execute_callback
def _validate_joint_names(self, arm: str, names: List[str]) -> bool:
"""Validate incoming trajectory joint names with expected arm names."""
expected = self._left_joint_names if arm == "left" else self._right_joint_names
if len(names) != len(expected):
return False
return set(names) == set(expected)
def _sample_trajectory(
self, points: List[JointTrajectoryPoint], elapsed_sec: float
) -> Tuple[List[float], List[float], bool]:
"""Sample trajectory by linear interpolation at elapsed time."""
if not points:
return [], [], True
if len(points) == 1:
pos = list(points[0].positions)
vel = (
list(points[0].velocities)
if points[0].velocities
else [0.0] * len(points[0].positions)
)
return pos, vel, True
times = [duration_to_sec(point.time_from_start) for point in points]
total = times[-1]
if elapsed_sec <= times[0]:
p = points[0]
pos = list(p.positions)
vel = list(p.velocities) if p.velocities else [0.0] * len(pos)
return pos, vel, False
if elapsed_sec >= total:
p = points[-1]
pos = list(p.positions)
vel = [0.0] * len(pos)
return pos, vel, True
index = 0
for i in range(len(times) - 1):
if times[i] <= elapsed_sec <= times[i + 1]:
index = i
break
p0 = points[index]
p1 = points[index + 1]
t0 = times[index]
t1 = times[index + 1]
dt = max(t1 - t0, 1e-6)
alpha = (elapsed_sec - t0) / dt
pos: List[float] = []
vel: List[float] = []
for j in range(len(p0.positions)):
q0 = p0.positions[j]
q1 = p1.positions[j]
pos.append(q0 + alpha * (q1 - q0))
if p0.velocities and p1.velocities:
v0 = p0.velocities[j]
v1 = p1.velocities[j]
vel.append(v0 + alpha * (v1 - v0))
else:
vel.append((q1 - q0) / dt)
return pos, vel, False
def _on_publish_joint_states(self) -> None:
"""Publish /joint_states and progress active trajectories."""
now_sec = time.time()
with self._lock:
for arm in ("left", "right"):
state = self._arm_states[arm]
if not state.active:
continue
elapsed = max(0.0, now_sec - state.start_time_sec)
pos, vel, done = self._sample_trajectory(state.points, elapsed)
if pos:
state.current_positions = pos
if vel:
state.current_velocities = vel
if done:
state.active = False
state.finished = True
state.current_velocities = [0.0] * len(state.current_velocities)
msg = JointState()
msg.header.stamp = self.get_clock().now().to_msg()
msg.name = self._joint_names.copy()
msg.position = (
self._arm_states["left"].current_positions
+ self._arm_states["right"].current_positions
)
msg.velocity = (
self._arm_states["left"].current_velocities
+ self._arm_states["right"].current_velocities
)
msg.effort = [0.0] * len(msg.position)
self._joint_pub.publish(msg)
def main() -> None:
"""Entry point."""
rclpy.init()
node = MockDualArmHardware()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()

68
src/robot_control/scripts/plot_joint_trajectory_multi.py
View File

@@ -1,68 +0,0 @@
import numpy as np
import matplotlib.pyplot as plt
import os
def plot_joint_trajectories(file_path):
# 检查文件是否存在
if not os.path.exists(file_path):
print(f"错误：文件不存在 - {file_path}")
return
# 读取数据
timestamps = []
joint_data = [] # 存储所有关节数据，格式：[ [关节1数据], [关节2数据], ... ]
with open(file_path, 'r') as f:
for line in f:
line = line.strip()
if not line:
continue
# 解析一行数据（时间戳 + 所有关节值）
parts = list(map(float, line.split(',')))
timestamp = parts[0]
joints = parts[1:] # 第0个是时间戳，后面是关节数据
timestamps.append(timestamp)
# 初始化关节数据列表（首次读取时）
if not joint_data:
joint_data = [[] for _ in range(len(joints))]
# 按关节索引存储数据
for i, val in enumerate(joints):
if i < len(joint_data): # 避免索引越界
joint_data[i].append(val)
# 转换为相对时间（以第一个时间戳为起点）
if not timestamps:
print("警告：未读取到有效数据")
return
start_time = timestamps[0]
relative_times = [t - start_time for t in timestamps]
# 绘制所有关节轨迹（最多显示12个关节，避免图过于拥挤）
num_joints = len(joint_data)
num_plots = min(num_joints, 12) # 超过12个关节只显示前12个
plt.figure(figsize=(12, 8))
for i in range(num_plots):
plt.plot(joint_data[i], label=f'关节 {i+1}')
plt.xlabel('时间点索引')
plt.ylabel('关节值')
plt.title('关节轨迹图')
plt.legend()
plt.grid(True)
plt.tight_layout()
plt.show()
if __name__ == "__main__":
import sys
if len(sys.argv) < 2:
print("用法: python plot_joint_trajectory_multi.py <数据文件路径>")
sys.exit(1)
file_path = "/tmp/trajectory_dual_right_20260226_151304.csv"
plot_joint_trajectories(file_path)

203
src/robot_control/scripts/run_dual_arm_mock_tests.py
View File

@@ -15,6 +15,7 @@ Cases:
from __future__ import annotations
import subprocess
import time
from typing import Dict, List, Optional, Tuple
@@ -25,7 +26,23 @@ from interfaces.msg import ArmMotionParams, RobotArmStatus
from interfaces.srv import InverseKinematics
from rclpy.action import ActionClient
from rclpy.node import Node
from rclpy.task import Future
from sensor_msgs.msg import JointState
def _check_duplicate_nodes(node_name: str = "robot_control_node") -> int:
"""Return the number of running instances of node_name via ros2 node list."""
try:
result = subprocess.run(
["ros2", "node", "list"],
capture_output=True,
text=True,
timeout=5,
)
lines = result.stdout.splitlines()
count = sum(1 for line in lines if line.strip() == f"/{node_name}")
return count
except Exception:
return -1
class DualArmMockTestRunner(Node):
@@ -44,12 +61,69 @@ class DualArmMockTestRunner(Node):
self._on_status,
10,
)
self._last_joint_stamp: Optional[float] = None
self._joint_recv_times: List[float] = []
self._joint_sub = self.create_subscription(
JointState,
"/joint_states",
self._on_joint_states,
10,
)
def _on_status(self, msg: RobotArmStatus) -> None:
self._last_status_msg = msg
def wait_ready(self, timeout_sec: float = 20.0) -> bool:
"""Wait for action server and arm status topic."""
def _on_joint_states(self, msg: JointState) -> None:
self._last_joint_stamp = time.monotonic()
self._joint_recv_times.append(time.monotonic())
# Keep only last 20 samples for frequency estimate
if len(self._joint_recv_times) > 20:
self._joint_recv_times.pop(0)
def _check_joint_states_health(
self, min_freq_hz: float = 20.0, timeout_sec: float = 5.0
) -> bool:
"""Spin until /joint_states is received at >=min_freq_hz, or timeout."""
self.get_logger().info(
f"Checking /joint_states health (need >={min_freq_hz:.0f} Hz)..."
)
deadline = time.monotonic() + timeout_sec
while time.monotonic() < deadline:
rclpy.spin_once(self, timeout_sec=0.1)
if len(self._joint_recv_times) >= 5:
dt = self._joint_recv_times[-1] - self._joint_recv_times[0]
n = len(self._joint_recv_times) - 1
if dt > 1e-6:
freq = n / dt
if freq >= min_freq_hz:
self.get_logger().info(
f"/joint_states healthy: {freq:.1f} Hz"
)
return True
self.get_logger().error(
f"/joint_states not healthy within {timeout_sec:.0f}s"
)
return False
def wait_ready(self, timeout_sec: float = 30.0) -> bool:
"""Wait for action server, IK service, and healthy joint states.
Also aborts early if duplicate robot_control_node instances are detected.
"""
# Duplicate node guard
count = _check_duplicate_nodes("robot_control_node")
if count > 1:
self.get_logger().error(
f"Found {count} instances of /robot_control_node. "
"Kill duplicate processes before running tests:\n"
" pkill -f robot_control_node"
)
return False
# joint_states health check first (mock hardware must be running)
if not self._check_joint_states_health(min_freq_hz=20.0, timeout_sec=10.0):
return False
self.get_logger().info("Waiting for DualArm action server...")
if not self._action_client.wait_for_server(timeout_sec=timeout_sec):
self.get_logger().error("DualArm action server timeout")
@@ -118,50 +192,55 @@ class DualArmMockTestRunner(Node):
param.blend_radius = 0
return param
@staticmethod
def _offset_pose(src: Pose, dx: float, dy: float, dz: float) -> Pose:
pose = Pose()
pose.position.x = src.position.x + dx
pose.position.y = src.position.y + dy
pose.position.z = src.position.z + dz
pose.orientation = src.orientation
return pose
def _check_ik(self, arm_id: int, pose: Pose, timeout_sec: float = 3.0) -> bool:
def _check_ik(self, arm_id: int, pose: Pose, timeout_sec: float = 5.0) -> bool:
"""Call InverseKinematics service and return True on success."""
request = InverseKinematics.Request()
request.arm_id = int(arm_id)
request.pose = pose
future = self._ik_client.call_async(request)
rclpy.spin_until_future_complete(self, future, timeout_sec=timeout_sec)
if not future.done():
self.get_logger().warn("IK service call timed out")
return False
response = future.result()
if response is None:
return False
return int(response.result) == 0 and len(response.joint_angles) > 0
def _find_reachable_movel_pose(self, arm_id: int) -> Optional[Pose]:
"""Find one reachable pose candidate for MOVEL test."""
y_value = 0.25 if arm_id == ArmMotionParams.ARM_LEFT else -0.25
x_candidates = [0.40, 0.35, 0.30, 0.25]
z_candidates = [0.35, 0.30, 0.25]
@staticmethod
def _home_ee_pose(arm_id: int, dx: float = 0.0, dy: float = 0.0, dz: float = 0.0) -> Pose:
"""Return a Pose near the home end-effector position (all joints = 0).
for x in x_candidates:
for z in z_candidates:
pose = Pose()
pose.position.x = x
pose.position.y = y_value
pose.position.z = z
pose.orientation.x = 0.0
pose.orientation.y = 0.0
pose.orientation.z = 0.0
pose.orientation.w = 1.0
if self._check_ik(arm_id, pose):
return pose
return None
FK result with all joints = 0 (from dual_arm.urdf kinematics):
left_arm_link_6 → [0, +0.2645, -0.601] in base_link frame
right_arm_link_6 → [0, -0.2645, -0.601] in base_link frame
Both have identity orientation when all joints are at 0.
dx/dy/dz are offsets from the home EE position (in base_link frame).
Only small offsets are guaranteed reachable from the home seed.
"""
y_base = 0.2645 if arm_id == ArmMotionParams.ARM_LEFT else -0.2645
pose = Pose()
pose.position.x = 0.0 + dx
pose.position.y = y_base + dy
pose.position.z = -0.601 + dz
pose.orientation.x = 0.0
pose.orientation.y = 0.0
pose.orientation.z = 0.0
pose.orientation.w = 1.0
return pose
def run(self) -> int:
"""Run all test cases and return process exit code."""
"""Run all test cases and return process exit code.
Test sequence:
1. single_left_movej — left arm MOVEJ to a non-zero target
2. single_right_movej — right arm MOVEJ to a non-zero target
3. dual_movej — both arms MOVEJ simultaneously
4. return_to_home — both arms back to all-zeros (prerequisite for MOVEL)
5. dual_movel_ik_check — verify IK service can solve for home+offset poses
6. dual_movel — both arms MOVEL to home EE +0.08m in X
"""
if not self.wait_ready():
return 2
@@ -170,6 +249,7 @@ class DualArmMockTestRunner(Node):
left_target = [0.40, -0.30, 0.45, -0.25, 0.20, 0.0]
right_target = [-0.40, 0.30, -0.45, 0.25, -0.20, 0.0]
# ── 1. Single-arm MOVEJ ───────────────────────────────────────────────
summary["single_left_movej"] = self.send_goal_and_wait(
[self._make_movej(ArmMotionParams.ARM_LEFT, left_target)]
)
@@ -180,6 +260,7 @@ class DualArmMockTestRunner(Node):
)
time.sleep(0.3)
# ── 2. Dual-arm MOVEJ ─────────────────────────────────────────────────
summary["dual_movej"] = self.send_goal_and_wait(
[
self._make_movej(
@@ -193,24 +274,68 @@ class DualArmMockTestRunner(Node):
)
time.sleep(0.3)
left_pose = self._find_reachable_movel_pose(ArmMotionParams.ARM_LEFT)
right_pose = self._find_reachable_movel_pose(ArmMotionParams.ARM_RIGHT)
if left_pose is None or right_pose is None:
summary["dual_movel"] = (False, "failed to find reachable poses by IK probe")
# ── 3. Return both arms to home — CRITICAL for reliable MOVEL IK ─────
# After the MOVEJ tests the arms are at arbitrary positions. The IK
# solver (KDL) uses the CURRENT joint state as seed. If the seed is
# far from the target the solver often fails. Returning to home first
# (all-zeros) makes the seed coincide with the MOVEL starting pose so
# IK is trivially one step away.
self.get_logger().info("Returning arms to home before MOVEL test...")
summary["return_to_home"] = self.send_goal_and_wait(
[
self._make_movej(ArmMotionParams.ARM_LEFT, [0.0] * 6),
self._make_movej(ArmMotionParams.ARM_RIGHT, [0.0] * 6),
],
velocity_scaling=40,
timeout_sec=40.0,
)
time.sleep(0.5)
# ── 4. Dual-arm MOVEL ─────────────────────────────────────────────────
# Home EE positions (FK with all joints=0, from dual_arm.urdf):
# left_arm_link_6 → [0, +0.2645, -0.601], orientation = identity
# right_arm_link_6 → [0, -0.2645, -0.601], orientation = identity
#
# MOVEL target = home EE + 0.08 m in X (forward).
# Small offset → IK seed (home) is very close to solution → reliable.
movel_dx = 0.08 # meters forward along X
left_movel_pose = self._home_ee_pose(ArmMotionParams.ARM_LEFT, dx=movel_dx)
right_movel_pose = self._home_ee_pose(ArmMotionParams.ARM_RIGHT, dx=movel_dx)
self.get_logger().info(
f"MOVEL targets — left: ({left_movel_pose.position.x:.3f}, "
f"{left_movel_pose.position.y:.3f}, {left_movel_pose.position.z:.3f}) "
f"right: ({right_movel_pose.position.x:.3f}, "
f"{right_movel_pose.position.y:.3f}, {right_movel_pose.position.z:.3f})"
)
# Verify IK is solvable before committing to the goal
self.get_logger().info("Checking IK for MOVEL targets...")
left_ik_ok = self._check_ik(ArmMotionParams.ARM_LEFT, left_movel_pose)
right_ik_ok = self._check_ik(ArmMotionParams.ARM_RIGHT, right_movel_pose)
if not left_ik_ok or not right_ik_ok:
summary["dual_movel"] = (
False,
f"IK pre-check failed (left_ok={left_ik_ok}, right_ok={right_ik_ok}). "
"Check MoveIt initialization and kinematics plugin.",
)
else:
self.get_logger().info("IK pre-check passed — sending MOVEL goal")
summary["dual_movel"] = self.send_goal_and_wait(
[
self._make_movel(ArmMotionParams.ARM_LEFT, left_pose),
self._make_movel(ArmMotionParams.ARM_RIGHT, right_pose),
self._make_movel(ArmMotionParams.ARM_LEFT, left_movel_pose),
self._make_movel(ArmMotionParams.ARM_RIGHT, right_movel_pose),
],
velocity_scaling=25,
timeout_sec=60.0,
)
# ── Summary ───────────────────────────────────────────────────────────
self.get_logger().info("========== Test Summary ==========")
all_ok = True
for name, (ok, message) in summary.items():
self.get_logger().info(f"{name}: ok={ok} message={message}")
status = "PASS" if ok else "FAIL"
self.get_logger().info(f" [{status}] {name}: {message}")
all_ok = all_ok and ok
return 0 if all_ok else 1

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

@@ -31,7 +31,7 @@ ActionManager::ActionManager(
ActionManager::~ActionManager() = default;
void ActionManager::initialize()
void ActionManager::Initialize()
{
ActionContext ctx;
ctx.node = node_;
@@ -44,11 +44,11 @@ void ActionManager::initialize()
move_wheel_ = std::make_unique<MoveWheelAction>(ctx, motor_service_);
dual_arm_ = std::make_unique<DualArmAction>(ctx);
move_home_->initialize();
move_leg_->initialize();
move_waist_->initialize();
move_wheel_->initialize();
dual_arm_->initialize();
move_home_->Initialize();
move_leg_->Initialize();
move_waist_->Initialize();
move_wheel_->Initialize();
dual_arm_->Initialize();
}
} // namespace robot_control

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

@@ -1,4 +1,10 @@
/**
* @file dual_arm_action.cpp
* @brief 双臂协调动作服务端实现
*/
#include "actions/dual_arm_action.hpp"
#include "core/robot_control_manager.hpp"
#include "core/common_enum.hpp"
#include "services/safety_service.hpp"
@@ -6,17 +12,17 @@
#include "utils/trajectory_utils.hpp"
#include "utils/velocity_scale_utils.hpp"
#include <thread>
#include <unordered_map>
#include <chrono>
#include <ctime>
#include "rclcpp/rclcpp.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <ctime>
#include <fstream>
#include <iomanip>
#include <sstream>
#include "rclcpp/rclcpp.hpp"
#include <thread>
#include <unordered_map>
namespace robot_control
{
@@ -25,7 +31,7 @@ DualArmAction::DualArmAction(const ActionContext& ctx)
{
}
void DualArmAction::initialize()
void DualArmAction::Initialize()
{
using namespace std::placeholders;

11
src/robot_control/src/actions/move_home_action.cpp
View File

@@ -1,10 +1,15 @@
#include "actions/move_home_action.hpp"
/**
* @file move_home_action.cpp
* @brief 归零动作服务端实现
*/
#include <thread>
#include "actions/move_home_action.hpp"
#include "core/robot_control_manager.hpp"
#include "core/common_enum.hpp"
#include <thread>
namespace robot_control
{
MoveHomeAction::MoveHomeAction(const ActionContext& ctx)
@@ -12,7 +17,7 @@ MoveHomeAction::MoveHomeAction(const ActionContext& ctx)
{
}
void MoveHomeAction::initialize()
void MoveHomeAction::Initialize()
{
using namespace std::placeholders;
server_ = rclcpp_action::create_server<MoveHome>(

11
src/robot_control/src/actions/move_leg_action.cpp
View File

@@ -1,10 +1,15 @@
#include "actions/move_leg_action.hpp"
/**
* @file move_leg_action.cpp
* @brief 腿部运动动作服务端实现
*/
#include <thread>
#include "actions/move_leg_action.hpp"
#include "core/robot_control_manager.hpp"
#include "core/common_enum.hpp"
#include <thread>
namespace robot_control
{
MoveLegAction::MoveLegAction(const ActionContext& ctx)
@@ -12,7 +17,7 @@ MoveLegAction::MoveLegAction(const ActionContext& ctx)
{
}
void MoveLegAction::initialize()
void MoveLegAction::Initialize()
{
using namespace std::placeholders;
server_ = rclcpp_action::create_server<MoveLeg>(

11
src/robot_control/src/actions/move_waist_action.cpp
View File

@@ -1,10 +1,15 @@
#include "actions/move_waist_action.hpp"
/**
* @file move_waist_action.cpp
* @brief 腰部运动动作服务端实现
*/
#include <thread>
#include "actions/move_waist_action.hpp"
#include "core/robot_control_manager.hpp"
#include "core/common_enum.hpp"
#include <thread>
namespace robot_control
{
MoveWaistAction::MoveWaistAction(const ActionContext& ctx)
@@ -12,7 +17,7 @@ MoveWaistAction::MoveWaistAction(const ActionContext& ctx)
{
}
void MoveWaistAction::initialize()
void MoveWaistAction::Initialize()
{
using namespace std::placeholders;
server_ = rclcpp_action::create_server<MoveWaist>(

19
src/robot_control/src/actions/move_wheel_action.cpp
View File

@@ -1,12 +1,17 @@
#include "actions/move_wheel_action.hpp"
/**
* @file move_wheel_action.cpp
* @brief 轮式运动动作服务端实现
*/
#include <cmath>
#include <thread>
#include "actions/move_wheel_action.hpp"
#include "core/robot_control_manager.hpp"
#include "core/common_enum.hpp"
#include "services/motor_service.hpp"
#include <cmath>
#include <thread>
namespace robot_control
{
MoveWheelAction::MoveWheelAction(
@@ -17,7 +22,7 @@ MoveWheelAction::MoveWheelAction(
{
}
void MoveWheelAction::initialize()
void MoveWheelAction::Initialize()
{
using namespace std::placeholders;
server_ = rclcpp_action::create_server<MoveWheel>(
@@ -95,14 +100,14 @@ void MoveWheelAction::execute_(const std::shared_ptr<GoalHandleMoveWheel> goal_h
bool is_jog_mode = ctx_.robot_control_manager->GetJogWheel();
if (motor_service_) {
motor_service_->configure_wheel_speed(goal->move_distance, ratio, is_jog_mode);
motor_service_->ConfigureWheelSpeed(goal->move_distance, ratio, is_jog_mode);
}
ctx_.robot_control_manager->MoveWheel();
const auto& wheel_commands = ctx_.robot_control_manager->GetWheelCommandPositions();
if (motor_service_) {
motor_service_->publish_wheel_command(wheel_commands);
motor_service_->PublishWheelCommand(wheel_commands);
}
while (executing_.load() && rclcpp::ok()) {
@@ -131,7 +136,7 @@ void MoveWheelAction::execute_(const std::shared_ptr<GoalHandleMoveWheel> goal_h
if ((goal->move_distance > 0.0) && !ctx_.robot_control_manager->GetJogWheel()) {
double default_ratio = 1.0;
if (motor_service_) {
motor_service_->configure_wheel_speed(0.2, default_ratio, false);
motor_service_->ConfigureWheelSpeed(0.2, default_ratio, false);
}
}

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

@@ -1,12 +1,12 @@
/**
* @file arm_control.cpp
* @brief 手臂控制器实现，封装 MoveIt 轨迹规划与执行
*/
#include "controllers/arm_control.hpp"
#include "utils/duration_utils.hpp"
#include <stdexcept>
#include <vector>
#include <memory>
#include <cmath>
#include <algorithm>
#include <limits>
#include <unordered_map>
#include <Eigen/Geometry>
#include "rclcpp/rclcpp.hpp"
#include "moveit/move_group_interface/move_group_interface.h"
@@ -14,6 +14,14 @@
#include "moveit_msgs/msg/move_it_error_codes.hpp"
#include "moveit_msgs/msg/robot_trajectory.hpp"
#include <algorithm>
#include <cmath>
#include <limits>
#include <memory>
#include <stdexcept>
#include <unordered_map>
#include <vector>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
@@ -346,7 +354,10 @@ bool ArmControl::SolveInverseKinematics(
}
try {
moveit::core::RobotStatePtr state = move_group->getCurrentState(2.0);
// Use the cached state as the IK seed to avoid blocking the single-threaded executor.
// getCurrentState(timeout) would deadlock: the IK service callback holds the thread
// while waiting for a fresh /joint_states callback that is queued in the same executor.
moveit::core::RobotStatePtr state = move_group->getCurrentState();
if (!state) {
if (error_msg) *error_msg = "Failed to get current robot state";
return false;
@@ -359,7 +370,7 @@ bool ArmControl::SolveInverseKinematics(
return false;
}
const bool ik_ok = state->setFromIK(jmg, target_pose, 0.05);
const bool ik_ok = state->setFromIK(jmg, target_pose, 0.5);
if (!ik_ok) {
if (error_msg) *error_msg = "setFromIK failed";
return false;
@@ -691,7 +702,7 @@ bool ArmControl::sCurveTimeParameterize_(
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});
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);
@@ -712,14 +723,14 @@ bool ArmControl::sCurveTimeParameterize_(
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);
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;
t_samples.push_back(t);
s_samples.push_back(s);
if (s_curve.isFinished(t) || (total_length - s) <= 1e-6) {
if (s_curve.IsFinished(t) || (total_length - s) <= 1e-6) {
done = true;
break;
}
@@ -1008,7 +1019,7 @@ bool ArmControl::MoveToTargetJoint(
if (PlanJointMotion(arm_id, target_joint, 0.5, 0.5))
{
is_moving_ = true;
movingDurationTime_ = 0.0;
moving_duration_time_ = 0.0;
}
else
{
@@ -1031,7 +1042,7 @@ bool ArmControl::MoveToTargetJoint(
joint_info.current_position = interpolated_joints[i];
}
// 注意：GetInterpolatedPoint 会更新速度加速度，但需要通过其他方式获取并更新到 joints_info_
movingDurationTime_ += dt;
moving_duration_time_ += dt;
return false; // 还在运动
}
else
@@ -1050,7 +1061,7 @@ bool ArmControl::MoveToTargetJoint(
joint_info.command_acceleration = 0.0;
}
is_moving_ = false;
movingDurationTime_ = 0.0;
moving_duration_time_ = 0.0;
ClearTrajectory(current_arm_id_);
return true; // 到达目标
}

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

@@ -1,3 +1,8 @@
/**
* @file control_base.cpp
* @brief 控制器基类实现
*/
#include "controllers/control_base.hpp"
#include "core/motion_parameters.hpp"
#include <stdexcept>
@@ -50,8 +55,8 @@ ControlBase::ControlBase(
is_stopping_ = false;
is_joint_position_initialized_ = false;
stopDurationTime_ = 0.0;
movingDurationTime_ = 0.0;
stop_duration_time_ = 0.0;
moving_duration_time_ = 0.0;
}
ControlBase::~ControlBase()
@@ -92,35 +97,35 @@ bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const
// 从当前关节位置开始规划轨迹（根据规划器类型选择）
std::vector<double> current_pos = getCurrentPositions();
if (planner_type_ == TrajectoryPlannerType::TRAPEZOIDAL) {
trapezoidalTrajectory_->init(current_pos, target_joint);
trapezoidalTrajectory_->Init(current_pos, target_joint);
} else {
s_curveTrajectory_->init(current_pos, target_joint);
s_curveTrajectory_->Init(current_pos, target_joint);
}
movingDurationTime_ = 0.0;
moving_duration_time_ = 0.0;
is_moving_ = true;
}
movingDurationTime_ += dt;
moving_duration_time_ += dt;
// 根据规划器类型更新轨迹
std::vector<double> desired_pos;
bool is_finished;
if (planner_type_ == TrajectoryPlannerType::TRAPEZOIDAL) {
desired_pos = trapezoidalTrajectory_->update(movingDurationTime_);
is_finished = trapezoidalTrajectory_->isFinished(movingDurationTime_);
desired_pos = trapezoidalTrajectory_->Update(moving_duration_time_);
is_finished = trapezoidalTrajectory_->IsFinished(moving_duration_time_);
} else {
desired_pos = s_curveTrajectory_->update(movingDurationTime_);
is_finished = s_curveTrajectory_->isFinished(movingDurationTime_);
desired_pos = s_curveTrajectory_->Update(moving_duration_time_);
is_finished = s_curveTrajectory_->IsFinished(moving_duration_time_);
}
// 获取速度和加速度（根据规划器类型）
std::vector<double> velocities, accelerations;
if (planner_type_ == TrajectoryPlannerType::TRAPEZOIDAL) {
velocities = trapezoidalTrajectory_->getVelocity();
accelerations = trapezoidalTrajectory_->getAcceleration();
velocities = trapezoidalTrajectory_->GetVelocity();
accelerations = trapezoidalTrajectory_->GetAcceleration();
} else {
velocities = s_curveTrajectory_->getVelocity();
accelerations = s_curveTrajectory_->getAcceleration();
velocities = s_curveTrajectory_->GetVelocity();
accelerations = s_curveTrajectory_->GetAcceleration();
}
// 检查是否到达目标位置
@@ -139,7 +144,7 @@ bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const
if (i < accelerations.size()) joint_info.command_acceleration = accelerations[i];
}
is_moving_ = false;
movingDurationTime_ = 0.0;
moving_duration_time_ = 0.0;
return true;
}
@@ -181,25 +186,25 @@ bool ControlBase::Stop(std::vector<double>& joint_positions, double dt)
is_stopping_ = true;
// 根据规划器类型计算急停轨迹
if (planner_type_ == TrajectoryPlannerType::TRAPEZOIDAL) {
trapezoidalTrajectory_->calculateStopTrajectory();
trapezoidalTrajectory_->CalculateStopTrajectory();
} else {
s_curveTrajectory_->calculateStopTrajectory();
s_curveTrajectory_->CalculateStopTrajectory();
}
stopDurationTime_ = 0.0;
stop_duration_time_ = 0.0;
}
if (is_stopping_)
{
stopDurationTime_ += dt;
stop_duration_time_ += dt;
// 根据规划器类型更新急停轨迹
bool is_stop_finished;
std::vector<double> stop_positions;
if (planner_type_ == TrajectoryPlannerType::TRAPEZOIDAL) {
stop_positions = trapezoidalTrajectory_->updateStop(stopDurationTime_);
is_stop_finished = trapezoidalTrajectory_->isStopFinished(stopDurationTime_);
stop_positions = trapezoidalTrajectory_->UpdateStop(stop_duration_time_);
is_stop_finished = trapezoidalTrajectory_->IsStopFinished(stop_duration_time_);
} else {
stop_positions = s_curveTrajectory_->updateStop(stopDurationTime_);
is_stop_finished = s_curveTrajectory_->isStopFinished(stopDurationTime_);
stop_positions = s_curveTrajectory_->UpdateStop(stop_duration_time_);
is_stop_finished = s_curveTrajectory_->IsStopFinished(stop_duration_time_);
}
// 更新 joints_info_map_ 中的位置
@@ -215,7 +220,7 @@ bool ControlBase::Stop(std::vector<double>& joint_positions, double dt)
{
is_moving_ = false;
is_stopping_ = false;
stopDurationTime_ = 0.0;
stop_duration_time_ = 0.0;
return true;
}
return false;
@@ -282,7 +287,7 @@ void ControlBase::SetTrajectoryPlannerType(TrajectoryPlannerType planner_type)
planner_type_ = planner_type;
}
bool ControlBase::checkJointLimits(std::vector<double>& joint_positions)
bool ControlBase::CheckJointLimits(std::vector<double>& joint_positions)
{
for (size_t i = 0; i < joint_positions.size() && i < joint_names_.size(); i++)
{

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

@@ -1,3 +1,8 @@
/**
* @file leg_control.cpp
* @brief 腿部控制器实现
*/
#include "controllers/leg_control.hpp"
#include "core/motion_parameters.hpp"
#include <stdexcept>
@@ -45,7 +50,7 @@ bool LegControl::MoveUp(std::vector<double>& joint_positions, double dt)
return result;
}
double LegControl::calculate_angle_from_links(double side1, double side2, double side_opposite, bool is_degree)
double LegControl::CalculateAngleFromLinks_(double side1, double side2, double side_opposite, bool is_degree)
{
const double EPS = 1e-6; // 浮点精度容错值
if (side1 < EPS || side2 < EPS || side_opposite < EPS) {
@@ -112,12 +117,12 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
double backFinalAngle = std::acos(finalBackLegHeight / lengthParameters_[0]);
double tempFrountLegLength = std::sqrt(finalFrountLegHeight * finalFrountLegHeight + currentFrountLegDistance * currentFrountLegDistance);
double frountAngle1_part1 = calculate_angle_from_links(finalFrountLegHeight, tempFrountLegLength, currentFrountLegDistance, false);
double frountAngle1_part2 = calculate_angle_from_links(tempFrountLegLength, lengthParameters_[1], lengthParameters_[2], false);
double frountAngle1_part1 = CalculateAngleFromLinks_(finalFrountLegHeight, tempFrountLegLength, currentFrountLegDistance, false);
double frountAngle1_part2 = CalculateAngleFromLinks_(tempFrountLegLength, lengthParameters_[1], lengthParameters_[2], false);
double frountFinalAngle1 = frountAngle1_part1 + frountAngle1_part2;
double frountFinalAngle2 = calculate_angle_from_links(lengthParameters_[1], lengthParameters_[2], tempFrountLegLength, false);
double frountFinalAngle2 = CalculateAngleFromLinks_(lengthParameters_[1], lengthParameters_[2], tempFrountLegLength, false);
// 零点为水平方向：把原来的 90/180 度常数替换为 π/2, π（弧度）
@@ -128,7 +133,7 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
tempPosition[4] = (M_PI - frountFinalAngle2);
tempPosition[5] = backFinalAngle;
if (!checkJointLimits(tempPosition))
if (!CheckJointLimits(tempPosition))
{
return false;
}

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

@@ -1,3 +1,8 @@
/**
* @file waist_control.cpp
* @brief 腰部控制器实现
*/
#include "controllers/waist_control.hpp"
#include "core/motion_parameters.hpp"
#include <stdexcept>
@@ -70,7 +75,7 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey
joints_info_map_[joint_names_[1]].home_position +
moveyaw_rad;
if (!checkJointLimits(tempPosition))
if (!CheckJointLimits(tempPosition))
{
return false;
}

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

@@ -1,3 +1,8 @@
/**
* @file wheel_control.cpp
* @brief 轮式驱动控制器实现
*/
#include "controllers/wheel_control.hpp"
#include "core/motion_parameters.hpp"
#include <vector>
@@ -10,8 +15,8 @@ WheelControl::WheelControl(
const std::map<std::string, JointInfo>& joints_info_map,
const std::vector<double>& lengthParameters
): ControlBase(joint_names, joints_info_map, lengthParameters)
, lastMoveDistance_(0.0)
, moveRatio_(0.0)
, last_move_distance_(0.0)
, move_ratio_(0.0)
, target_joint_positions_(joint_names.size(), 0.0)
{
}
@@ -21,7 +26,7 @@ WheelControl::~WheelControl()
// ControlBase owns and deletes trajectory planners
}
int tempAdjustCount = 0;
static int temp_adjust_count = 0;
bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle)
{
@@ -46,7 +51,7 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
double beta = moveWheelAngle * 0.8 / 360.0 * M_PI * 2 * lengthParameters_[1] / lengthParameters_[0] * 180.0 / M_PI;
if(moveWheelDistance > lastMoveDistance_ && (moveWheelDistance - lastMoveDistance_ > 1.5) && beta != 0)
if(moveWheelDistance > last_move_distance_ && (moveWheelDistance - last_move_distance_ > 1.5) && beta != 0)
{
if (beta > 0)
{
@@ -58,9 +63,9 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
}
}
if(moveWheelDistance < lastMoveDistance_ && (moveWheelDistance - lastMoveDistance_ < -1.5) && beta != 0)
if(moveWheelDistance < last_move_distance_ && (moveWheelDistance - last_move_distance_ < -1.5) && beta != 0)
{
tempAdjustCount ++;
temp_adjust_count ++;
if (beta > 0)
{
tempHomePositions[0] = joints_info_map_[joint_names_[0]].home_position - beta * 2.0;
@@ -71,10 +76,10 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
tempHomePositions[0] = joints_info_map_[joint_names_[0]].home_position - beta;
}
if (tempAdjustCount == 1)
if (temp_adjust_count == 1)
{
tempHomePositions[0] -= 40;
tempAdjustCount = 0;
temp_adjust_count = 0;
}
}
@@ -89,16 +94,16 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
if (a2 > 0.0 && a1 > 0.0)
{
moveRatio_ = ((a1 / a2) - 1.0) * 0.9;
move_ratio_ = ((a1 / a2) - 1.0) * 0.9;
}
else
{
moveRatio_ = 0.0;
move_ratio_ = 0.0;
}
moveRatio_ = moveRatio_ + 1.0;
move_ratio_ = move_ratio_ + 1.0;
if (moveRatio_ > 0.99 && moveRatio_ < 1.009)
if (move_ratio_ > 0.99 && move_ratio_ < 1.009)
{
// 使用默认方向 1（正方向）
for (size_t i = 0; i < total_joints_; i++)
@@ -115,7 +120,7 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
}
}
lastMoveDistance_ = moveWheelDistance;
last_move_distance_ = moveWheelDistance;
is_target_set_ = true;
@@ -152,7 +157,7 @@ bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, d
tempDesiredPositions[i] = tempHomePositions[i];
}
moveRatio_ = 1;
move_ratio_ = 1;
for (size_t i = 0; i < total_joints_; i++)
{
@@ -190,7 +195,7 @@ bool WheelControl::MoveWheel(std::vector<double>& joint_positions)
}
double WheelControl::GetWheelRatioInternal()
{
return moveRatio_;
return move_ratio_;
}
}

12
src/robot_control/src/core/controller_factory.cpp
View File

@@ -18,7 +18,7 @@
using namespace robot_control;
std::unique_ptr<ControlBase> ControllerFactory::create_controller(
std::unique_ptr<ControlBase> ControllerFactory::CreateController(
ControllerType type,
const MotionParameters& motion_params)
{
@@ -98,7 +98,7 @@ std::unique_ptr<ControlBase> ControllerFactory::create_controller(
}
}
std::string ControllerFactory::controller_type_to_string(ControllerType type)
std::string ControllerFactory::ControllerTypeToString(ControllerType type)
{
switch (type)
{
@@ -117,7 +117,7 @@ std::string ControllerFactory::controller_type_to_string(ControllerType type)
}
}
ControllerType ControllerFactory::string_to_controller_type(const std::string& type_str)
ControllerType ControllerFactory::StringToControllerType(const std::string& type_str)
{
std::string lower_str = type_str;
std::transform(lower_str.begin(), lower_str.end(), lower_str.begin(), ::tolower);
@@ -134,12 +134,12 @@ ControllerType ControllerFactory::string_to_controller_type(const std::string& t
return ControllerType::UNKNOWN_CONTROLLER;
}
bool ControllerFactory::is_controller_enabled(
bool ControllerFactory::IsControllerEnabled(
ControllerType type,
const std::vector<std::string>& enabled_controllers)
{
// 使用 controller_type_to_string 将枚举转换为字符串
std::string type_str = controller_type_to_string(type);
// 使用 ControllerTypeToString 将枚举转换为字符串
std::string type_str = ControllerTypeToString(type);
// 如果类型无效，返回false
if (type_str == "unknown")

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

@@ -15,13 +15,13 @@ RemoteController::RemoteController(
rclcpp::Node* node,
RobotControlManager& robot_control_manager)
: node_(node)
, robotControlManager_(robot_control_manager)
, isJogMode_(false)
, jogDirection_(0)
, jogIndex_(0)
, isStopping_(false)
, robot_control_manager_(robot_control_manager)
, is_jog_mode_(false)
, jog_direction_(0)
, jog_index_(0)
, is_stopping_(false)
{
lastCommand_.clear();
last_command_.clear();
RCLCPP_INFO(node_->get_logger(), "RemoteController initialized");
}
@@ -41,10 +41,10 @@ void RemoteController::ProcessCommand(const std_msgs::msg::String::SharedPtr msg
const std::string& command = msg->data;
// 去重：忽略重复的命令（避免快速重复触发）
if (IsDuplicateCommand(command)) {
if (IsDuplicateCommand_(command)) {
return;
}
UpdateLastCommand(command);
UpdateLastCommand_(command);
// ==================== 点动模式控制 ====================
@@ -81,21 +81,21 @@ void RemoteController::ProcessCommand(const std_msgs::msg::String::SharedPtr msg
void RemoteController::HandleJogModeToggle(const std::string& command)
{
// 检查机器人是否正在运动
if (robotControlManager_.IsMoving(MovementPart::ALL)) {
if (robot_control_manager_.IsMoving(MovementPart::ALL)) {
RCLCPP_WARN(node_->get_logger(), "Robot is moving, cannot toggle jog mode");
return;
}
if (command == "A,1") {
// 开启点动模式
isJogMode_ = true;
jogDirection_ = 0; // 重置方向
is_jog_mode_ = true;
jog_direction_ = 0; // 重置方向
RCLCPP_INFO(node_->get_logger(), "Jog mode enabled");
}
else if (command == "B,1") {
// 关闭点动模式
isJogMode_ = false;
jogDirection_ = 0; // 重置方向
is_jog_mode_ = false;
jog_direction_ = 0; // 重置方向
RCLCPP_INFO(node_->get_logger(), "Jog mode disabled");
}
}
@@ -103,26 +103,26 @@ void RemoteController::HandleJogModeToggle(const std::string& command)
void RemoteController::HandleJogAxisSwitch(const std::string& command)
{
// 仅在点动模式且当前无方向时允许切换
if (!isJogMode_ || jogDirection_ != 0) {
if (!is_jog_mode_ || jog_direction_ != 0) {
return;
}
const size_t max_joints = robotControlManager_.GetMotionParameters().total_joints_;
const size_t max_joints = robot_control_manager_.GetMotionParameters().total_joints_;
if (command == "RB,1") {
// 切换到下一个关节
if (jogIndex_ < max_joints - 1) {
jogIndex_++;
RCLCPP_INFO(node_->get_logger(), "Switched to jog axis: %zu", jogIndex_ + 1);
if (jog_index_ < max_joints - 1) {
jog_index_++;
RCLCPP_INFO(node_->get_logger(), "Switched to jog axis: %zu", jog_index_ + 1);
} else {
RCLCPP_WARN(node_->get_logger(), "Reached max joint (%zu), cannot switch to next axis", max_joints);
}
}
else if (command == "LB,1") {
// 切换到上一个关节
if (jogIndex_ > 0) {
jogIndex_--;
RCLCPP_INFO(node_->get_logger(), "Switched to jog axis: %zu", jogIndex_ + 1);
if (jog_index_ > 0) {
jog_index_--;
RCLCPP_INFO(node_->get_logger(), "Switched to jog axis: %zu", jog_index_ + 1);
} else {
RCLCPP_WARN(node_->get_logger(), "Reached min joint (0), cannot switch to previous axis");
}
@@ -132,32 +132,32 @@ void RemoteController::HandleJogAxisSwitch(const std::string& command)
void RemoteController::HandleJogDirection(const std::string& command)
{
// 仅在点动模式下有效
if (!isJogMode_) {
if (!is_jog_mode_) {
return;
}
if (command == "方向垂直,-1.0") {
// 负方向点动
jogDirection_ = -1;
RCLCPP_DEBUG(node_->get_logger(), "Jog axis %zu: negative direction", jogIndex_ + 1);
jog_direction_ = -1;
RCLCPP_DEBUG(node_->get_logger(), "Jog axis %zu: negative direction", jog_index_ + 1);
}
else if (command == "方向垂直,1.0") {
// 正方向点动
jogDirection_ = 1;
RCLCPP_DEBUG(node_->get_logger(), "Jog axis %zu: positive direction", jogIndex_ + 1);
jog_direction_ = 1;
RCLCPP_DEBUG(node_->get_logger(), "Jog axis %zu: positive direction", jog_index_ + 1);
}
else if (command == "方向垂直,0.0") {
// 停止点动
jogDirection_ = 0;
RCLCPP_DEBUG(node_->get_logger(), "Jog axis %zu: stopped", jogIndex_ + 1);
jog_direction_ = 0;
RCLCPP_DEBUG(node_->get_logger(), "Jog axis %zu: stopped", jog_index_ + 1);
}
}
void RemoteController::HandleEmergencyStop()
{
// 紧急停止（仅在非点动模式下）
if (!isJogMode_) {
isStopping_ = true;
if (!is_jog_mode_) {
is_stopping_ = true;
RCLCPP_WARN(node_->get_logger(), "Emergency stop requested");
// Stop is handled by the action layer / higher-level logic; we only set the flag here.
}
@@ -173,13 +173,13 @@ void RemoteController::HandleMovementControl(const std::string& command)
RCLCPP_DEBUG(node_->get_logger(), "Unknown or unhandled movement command: '%s'", command.c_str());
}
bool RemoteController::IsDuplicateCommand(const std::string& command)
bool RemoteController::IsDuplicateCommand_(const std::string& command)
{
return command == lastCommand_;
return command == last_command_;
}
void RemoteController::UpdateLastCommand(const std::string& command)
void RemoteController::UpdateLastCommand_(const std::string& command)
{
lastCommand_ = command;
last_command_ = command;
}

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

@@ -23,51 +23,51 @@ inline rclcpp::Logger logger()
RobotControlManager::RobotControlManager()
{
this->init();
this->Init_();
}
void RobotControlManager::init()
void RobotControlManager::Init_()
{
gyroValues_.resize(4, 0.0);
gyroVelocities_.resize(3, 0.0);
gyroAccelerations_.resize(3, 0.0);
gyro_values_.resize(4, 0.0);
gyro_velocities_.resize(3, 0.0);
gyro_accelerations_.resize(3, 0.0);
is_wheel_jog_ = false;
// Initialize the wheel commands
wheelCommands_.assign(motionParams_.wheel_joint_names_.size(), 0.0);
wheel_commands_.assign(motion_params_.wheel_joint_names_.size(), 0.0);
// 初始化控制器启用标志
leg_controller_enabled_ = motionParams_.leg_controller_enabled_;
wheel_controller_enabled_ = motionParams_.wheel_controller_enabled_;
waist_controller_enabled_ = motionParams_.waist_controller_enabled_;
arm_controller_enabled_ = motionParams_.arm_controller_enabled_;
leg_controller_enabled_ = motion_params_.leg_controller_enabled_;
wheel_controller_enabled_ = motion_params_.wheel_controller_enabled_;
waist_controller_enabled_ = motion_params_.waist_controller_enabled_;
arm_controller_enabled_ = motion_params_.arm_controller_enabled_;
// 动态加载控制器
if (leg_controller_enabled_)
{
leg_controller_ = std::unique_ptr<LegControl>(
static_cast<LegControl*>(ControllerFactory::create_controller(
ControllerType::LEG_CONTROLLER, motionParams_).release()));
static_cast<LegControl*>(ControllerFactory::CreateController(
ControllerType::LEG_CONTROLLER, motion_params_).release()));
}
if (wheel_controller_enabled_)
{
wheel_controller_ = std::unique_ptr<WheelControl>(
static_cast<WheelControl*>(ControllerFactory::create_controller(
ControllerType::WHEEL_CONTROLLER, motionParams_).release()));
static_cast<WheelControl*>(ControllerFactory::CreateController(
ControllerType::WHEEL_CONTROLLER, motion_params_).release()));
}
if (waist_controller_enabled_)
{
waist_controller_ = std::unique_ptr<WaistControl>(
static_cast<WaistControl*>(ControllerFactory::create_controller(
ControllerType::WAIST_CONTROLLER, motionParams_).release()));
static_cast<WaistControl*>(ControllerFactory::CreateController(
ControllerType::WAIST_CONTROLLER, motion_params_).release()));
}
if (arm_controller_enabled_)
{
auto arm_ctrl = ControllerFactory::create_controller(ControllerType::ARM_CONTROLLER, motionParams_);
auto arm_ctrl = ControllerFactory::CreateController(ControllerType::ARM_CONTROLLER, motion_params_);
if (arm_ctrl)
{
arm_controller_ = std::unique_ptr<ArmControl>(
@@ -75,10 +75,10 @@ void RobotControlManager::init()
}
}
isWheelHomed_ = false;
is_wheel_homed_ = false;
isJointInitValueSet_ = false;
isGyroInited_ = false;
is_joint_init_value_set_ = false;
is_gyro_inited_ = false;
joint_name_mapping_initialized_ = false;
}
@@ -92,7 +92,7 @@ bool RobotControlManager::ValidateArmJointTarget(
const std::vector<double>& target_joints,
std::string* error_msg) const
{
const size_t ARM_DOF = motionParams_.arm_dof_;
const size_t ARM_DOF = motion_params_.arm_dof_;
if (arm_id != 0 && arm_id != 1) {
if (error_msg) *error_msg = "Invalid arm_id (expected 0=left, 1=right)";
@@ -105,16 +105,16 @@ bool RobotControlManager::ValidateArmJointTarget(
}
return false;
}
if (motionParams_.dual_arm_joint_names_.size() < 2 * ARM_DOF) {
if (motion_params_.dual_arm_joint_names_.size() < 2 * ARM_DOF) {
if (error_msg) *error_msg = "dual_arm_joint_names_ size is invalid";
return false;
}
const size_t start_idx = (arm_id == 0) ? 0 : ARM_DOF;
for (size_t i = 0; i < ARM_DOF; ++i) {
const std::string& joint_name = motionParams_.dual_arm_joint_names_[start_idx + i];
auto it = motionParams_.joints_info_map_.find(joint_name);
if (it == motionParams_.joints_info_map_.end()) {
const std::string& joint_name = motion_params_.dual_arm_joint_names_[start_idx + i];
auto it = motion_params_.joints_info_map_.find(joint_name);
if (it == motion_params_.joints_info_map_.end()) {
if (error_msg) *error_msg = "JointInfo not found for joint: " + joint_name;
return false;
}
@@ -226,16 +226,16 @@ bool RobotControlManager::PlanDualArmJointMotion(
if (!arm_controller_enabled_ || !arm_controller_) return false;
if (!arm_controller_->IsMoveItInitialized()) return false;
const size_t ARM_DOF = motionParams_.arm_dof_;
const size_t ARM_DOF = motion_params_.arm_dof_;
// 构建 12 关节目标：默认保持当前 jointPositions_（如未初始化，则保持 0）
// 构建 12 关节目标：默认保持当前 joint_positions_（如未初始化，则保持 0）
std::vector<double> target(2 * ARM_DOF, 0.0);
if (joint_name_mapping_initialized_) {
for (size_t i = 0; i < 2 * ARM_DOF; ++i) {
const std::string& jn = motionParams_.dual_arm_joint_names_[i];
auto it = motionParams_.joint_name_to_index_.find(jn);
if (it != motionParams_.joint_name_to_index_.end() && it->second < jointPositions_.size()) {
target[i] = jointPositions_[it->second];
const std::string& jn = motion_params_.dual_arm_joint_names_[i];
auto it = motion_params_.joint_name_to_index_.find(jn);
if (it != motion_params_.joint_name_to_index_.end() && it->second < joint_positions_.size()) {
target[i] = joint_positions_[it->second];
}
}
}
@@ -290,8 +290,8 @@ bool RobotControlManager::GetArmCurrentPose(
return false;
}
const size_t arm_dof = motionParams_.arm_dof_;
if (motionParams_.dual_arm_joint_names_.size() < 2 * arm_dof) {
const size_t arm_dof = motion_params_.arm_dof_;
if (motion_params_.dual_arm_joint_names_.size() < 2 * arm_dof) {
if (error_msg) *error_msg = "dual_arm_joint_names_ size is invalid";
return false;
}
@@ -300,7 +300,7 @@ bool RobotControlManager::GetArmCurrentPose(
arm_joint_names.reserve(arm_dof);
const size_t start_idx = (arm_id == 0) ? 0 : arm_dof;
for (size_t i = 0; i < arm_dof; ++i) {
arm_joint_names.push_back(motionParams_.dual_arm_joint_names_[start_idx + i]);
arm_joint_names.push_back(motion_params_.dual_arm_joint_names_[start_idx + i]);
}
std::vector<double> joint_values;
@@ -327,7 +327,7 @@ bool RobotControlManager::ExportArmPlannedTrajectory(uint8_t arm_id, trajectory_
const std::vector<double>& RobotControlManager::GetJointPositions() const
{
return jointPositions_;
return joint_positions_;
}
bool RobotControlManager::GetJointPositionsByNames(
@@ -342,7 +342,7 @@ bool RobotControlManager::GetJointPositionsByNames(
out_positions->clear();
out_positions->reserve(joint_names.size());
const auto& mp = GetMotionParameters();
const auto& jp = jointPositions_;
const auto& jp = joint_positions_;
for (const auto& jn : joint_names) {
auto it = mp.joint_name_to_index_.find(jn);
if (it == mp.joint_name_to_index_.end()) {
@@ -370,7 +370,7 @@ bool RobotControlManager::GetJointVelocitiesByNames(
out_velocities->clear();
out_velocities->reserve(joint_names.size());
const auto& mp = GetMotionParameters();
const auto& jv = jointVelocities_;
const auto& jv = joint_velocities_;
for (const auto& jn : joint_names) {
auto it = mp.joint_name_to_index_.find(jn);
if (it == mp.joint_name_to_index_.end()) {
@@ -412,12 +412,12 @@ trajectory_msgs::msg::JointTrajectory RobotControlManager::MakeStopTrajectory(
const std::vector<double>& RobotControlManager::GetWheelPositions() const
{
return wheelPositions_;
return wheel_positions_;
}
const std::vector<double>& RobotControlManager::GetWheelCommandPositions() const
{
return wheelCommands_;
return wheel_commands_;
}
double RobotControlManager::GetWheelRatio()
@@ -427,16 +427,16 @@ double RobotControlManager::GetWheelRatio()
double RobotControlManager::GetImuYawDifference() const
{
if (gyroValues_.size() <= 2 || gyroInitValues_.size() <= 2) {
if (gyro_values_.size() <= 2 || gyro_init_values_.size() <= 2) {
return 0.0;
}
return gyroValues_[2] - gyroInitValues_[2];
return gyro_values_[2] - gyro_init_values_[2];
}
void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg)
{
imuMsg_ = *msg;
ImuMsg tempMsg = imuMsg_;
imu_msg_ = *msg;
ImuMsg tempMsg = imu_msg_;
if (tempMsg.orientation.w == 0 && tempMsg.orientation.x == 0 && tempMsg.orientation.y == 0 && tempMsg.orientation.z == 0)
{
@@ -448,34 +448,34 @@ void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg)
return;
}
QuaternionToRPYDeg(
QuaternionToRpyDeg_(
tempMsg.orientation.w,
tempMsg.orientation.x,
tempMsg.orientation.y,
tempMsg.orientation.z,
gyroValues_[0],
gyroValues_[1],
gyroValues_[2]
gyro_values_[0],
gyro_values_[1],
gyro_values_[2]
);
if (!isGyroInited_)
if (!is_gyro_inited_)
{
gyroInitValues_ = gyroValues_;
isGyroInited_ = true;
gyro_init_values_ = gyro_values_;
is_gyro_inited_ = true;
}
gyroVelocities_[0] = tempMsg.angular_velocity.x;
gyroVelocities_[1] = tempMsg.angular_velocity.y;
gyroVelocities_[2] = tempMsg.angular_velocity.z;
gyro_velocities_[0] = tempMsg.angular_velocity.x;
gyro_velocities_[1] = tempMsg.angular_velocity.y;
gyro_velocities_[2] = tempMsg.angular_velocity.z;
gyroAccelerations_[0] = tempMsg.linear_acceleration.x;
gyroAccelerations_[1] = tempMsg.linear_acceleration.y;
gyroAccelerations_[2] = tempMsg.linear_acceleration.z;
gyro_accelerations_[0] = tempMsg.linear_acceleration.x;
gyro_accelerations_[1] = tempMsg.linear_acceleration.y;
gyro_accelerations_[2] = tempMsg.linear_acceleration.z;
}
void RobotControlManager::QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
void RobotControlManager::QuaternionToRpyRad_(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
{
double norm = sqrt(qw*qw + qx*qx + qy*qy + qz*qz);
if (norm < 1e-12) { // 避免除以零
@@ -502,39 +502,39 @@ void RobotControlManager::QuaternionToRPYRad(double qw, double qx, double qy, do
yaw = atan2(2 * (qw*qz + qx*qy), 1 - 2 * (qy*qy + qz*qz));
}
void RobotControlManager::QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
void RobotControlManager::QuaternionToRpyDeg_(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
{
QuaternionToRPYRad(qw, qx, qy, qz, roll, pitch, yaw);
QuaternionToRpyRad_(qw, qx, qy, qz, roll, pitch, yaw);
}
void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg)
{
motorPos_ = *msg;
wheelPositions_.resize(motionParams_.wheel_joint_names_.size());
wheelVelocities_.resize(motionParams_.wheel_joint_names_.size());
motor_pos_ = *msg;
wheel_positions_.resize(motion_params_.wheel_joint_names_.size());
wheel_velocities_.resize(motion_params_.wheel_joint_names_.size());
if (motorPos_.motor_angle.size() != motionParams_.wheel_joint_names_.size())
if (motor_pos_.motor_angle.size() != motion_params_.wheel_joint_names_.size())
{
RCLCPP_WARN(logger(), "Wheel states size mismatch (got %zu, expected %zu)",
motorPos_.motor_angle.size(), motionParams_.wheel_joint_names_.size());
motor_pos_.motor_angle.size(), motion_params_.wheel_joint_names_.size());
return;
}
for (size_t i = 0; i < motorPos_.motor_angle.size(); i++)
for (size_t i = 0; i < motor_pos_.motor_angle.size(); i++)
{
wheelPositions_[i] = motorPos_.motor_angle[i];
wheelVelocities_[i] = 0.0;
wheel_positions_[i] = motor_pos_.motor_angle[i];
wheel_velocities_[i] = 0.0;
}
if (wheel_controller_)
{
if (!isWheelHomed_)
if (!is_wheel_homed_)
{
wheel_controller_->SetHomePositions(wheelPositions_);
isWheelHomed_ = true;
wheel_controller_->SetHomePositions(wheel_positions_);
is_wheel_homed_ = true;
}
wheel_controller_->UpdateJointStates(wheelPositions_, wheelVelocities_, std::vector<double>());
wheel_controller_->UpdateJointStates(wheel_positions_, wheel_velocities_, std::vector<double>());
}
}
@@ -553,48 +553,48 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
return;
}
jointStates_ = *msg;
joint_states_ = *msg;
// 首次收到 JointState 消息时，初始化关节名称映射
if (!joint_name_mapping_initialized_ && !jointStates_.name.empty())
if (!joint_name_mapping_initialized_ && !joint_states_.name.empty())
{
motionParams_.InitializeJointNameMapping(jointStates_.name);
motion_params_.InitializeJointNameMapping(joint_states_.name);
joint_name_mapping_initialized_ = true;
RCLCPP_INFO(logger(), "Joint name mapping initialized with %zu joints", motionParams_.total_joints_);
jointPositions_.resize(motionParams_.total_joints_);
jointVelocities_.resize(motionParams_.total_joints_);
jointEfforts_.resize(motionParams_.total_joints_);
RCLCPP_INFO(logger(), "Joint name mapping initialized with %zu joints", motion_params_.total_joints_);
joint_positions_.resize(motion_params_.total_joints_);
joint_velocities_.resize(motion_params_.total_joints_);
joint_efforts_.resize(motion_params_.total_joints_);
}
for (size_t i = 0; i < jointStates_.name.size(); i++)
for (size_t i = 0; i < joint_states_.name.size(); i++)
{
if (joint_name_mapping_initialized_) {
auto it = motionParams_.joint_name_to_index_.find(jointStates_.name[i]);
if (it == motionParams_.joint_name_to_index_.end()) {
auto it = motion_params_.joint_name_to_index_.find(joint_states_.name[i]);
if (it == motion_params_.joint_name_to_index_.end()) {
continue;
}
const size_t internal_index = it->second;
if (internal_index >= jointPositions_.size() ||
internal_index >= jointVelocities_.size() ||
internal_index >= jointEfforts_.size()) {
if (internal_index >= joint_positions_.size() ||
internal_index >= joint_velocities_.size() ||
internal_index >= joint_efforts_.size()) {
continue;
}
const double pos = jointStates_.position[i];
const double pos = joint_states_.position[i];
// 过滤 NaN/Inf，避免传播脏数据
jointPositions_[internal_index] = std::isfinite(pos) ? pos : 0.0;
joint_positions_[internal_index] = std::isfinite(pos) ? pos : 0.0;
// velocity/effort 允许为空或更短，必须做边界保护
if (i < jointStates_.velocity.size() && std::isfinite(jointStates_.velocity[i])) {
jointVelocities_[internal_index] = jointStates_.velocity[i];
if (i < joint_states_.velocity.size() && std::isfinite(joint_states_.velocity[i])) {
joint_velocities_[internal_index] = joint_states_.velocity[i];
} else {
jointVelocities_[internal_index] = 0.0;
joint_velocities_[internal_index] = 0.0;
}
if (i < jointStates_.effort.size() && std::isfinite(jointStates_.effort[i])) {
jointEfforts_[internal_index] = jointStates_.effort[i];
if (i < joint_states_.effort.size() && std::isfinite(joint_states_.effort[i])) {
joint_efforts_[internal_index] = joint_states_.effort[i];
} else {
jointEfforts_[internal_index] = 0.0;
joint_efforts_[internal_index] = 0.0;
}
}
}
@@ -602,31 +602,31 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
// 初始化完成判定：关节映射完成 + 全部轴 position 已有效且不为 -99(非OP哨兵值)
bool all_axes_ready = false;
if (joint_name_mapping_initialized_ &&
motionParams_.total_joints_ > 0 &&
jointPositions_.size() >= motionParams_.total_joints_) {
motion_params_.total_joints_ > 0 &&
joint_positions_.size() >= motion_params_.total_joints_) {
all_axes_ready = true;
constexpr double kNotOpPosition = -99.0;
constexpr double kEps = 1e-6;
for (size_t i = 0; i < motionParams_.total_joints_; ++i) {
const double pos = jointPositions_[i];
for (size_t i = 0; i < motion_params_.total_joints_; ++i) {
const double pos = joint_positions_[i];
if (!std::isfinite(pos) || std::abs(pos - kNotOpPosition) < kEps) {
all_axes_ready = false;
break;
}
}
}
if (all_axes_ready && !isJointInitValueSet_) {
if (all_axes_ready && !is_joint_init_value_set_) {
RCLCPP_INFO(logger(), "All joints are initialized and entered OP state");
}
isJointInitValueSet_ = all_axes_ready;
is_joint_init_value_set_ = all_axes_ready;
// 使用统一函数更新各子控制器关节状态
if (waist_controller_) {
std::vector<double> positions;
std::vector<double> velocities;
std::vector<double> efforts;
if (BuildControllerJointStatesByNames(
motionParams_.waist_joint_names_,
if (BuildControllerJointStatesByNames_(
motion_params_.waist_joint_names_,
&positions,
&velocities,
&efforts)) {
@@ -637,8 +637,8 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
std::vector<double> positions;
std::vector<double> velocities;
std::vector<double> efforts;
if (BuildControllerJointStatesByNames(
motionParams_.leg_joint_names_,
if (BuildControllerJointStatesByNames_(
motion_params_.leg_joint_names_,
&positions,
&velocities,
&efforts)) {
@@ -649,8 +649,8 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
std::vector<double> positions;
std::vector<double> velocities;
std::vector<double> efforts;
if (BuildControllerJointStatesByNames(
motionParams_.dual_arm_joint_names_,
if (BuildControllerJointStatesByNames_(
motion_params_.dual_arm_joint_names_,
&positions,
&velocities,
&efforts)) {
@@ -661,7 +661,7 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
const MotionParameters& RobotControlManager::GetMotionParameters() const
{
return motionParams_;
return motion_params_;
}
bool RobotControlManager::IsMoving(MovementPart part)
@@ -688,7 +688,7 @@ bool RobotControlManager::IsMoving(MovementPart part)
bool RobotControlManager::RobotInitFinished()
{
return isJointInitValueSet_;
return is_joint_init_value_set_;
}
bool RobotControlManager::MoveWheel()
@@ -698,29 +698,29 @@ bool RobotControlManager::MoveWheel()
return false;
}
tempWheelCmd_.resize(motionParams_.wheel_joint_names_.size());
temp_wheel_cmd_.resize(motion_params_.wheel_joint_names_.size());
bool result = wheel_controller_->MoveWheel(tempWheelCmd_);
bool result = wheel_controller_->MoveWheel(temp_wheel_cmd_);
// wheelCommands_ is wheel-joint-order array
wheelCommands_ = tempWheelCmd_;
// wheel_commands_ is wheel-joint-order array
wheel_commands_ = temp_wheel_cmd_;
return result;
}
std::vector<size_t> RobotControlManager::GetJointIndicesFromNames(const std::vector<std::string>& joint_names) const
std::vector<size_t> RobotControlManager::GetJointIndicesFromNames_(const std::vector<std::string>& joint_names) const
{
std::vector<size_t> indices;
for (const auto& name : joint_names) {
auto it = motionParams_.joint_name_to_index_.find(name);
if (it != motionParams_.joint_name_to_index_.end()) {
auto it = motion_params_.joint_name_to_index_.find(name);
if (it != motion_params_.joint_name_to_index_.end()) {
indices.push_back(it->second);
}
}
return indices;
}
bool RobotControlManager::BuildControllerJointStatesByNames(
bool RobotControlManager::BuildControllerJointStatesByNames_(
const std::vector<std::string>& joint_names,
std::vector<double>* out_positions,
std::vector<double>* out_velocities,
@@ -737,18 +737,18 @@ bool RobotControlManager::BuildControllerJointStatesByNames(
return false;
}
const std::vector<size_t> indices = GetJointIndicesFromNames(joint_names);
const std::vector<size_t> indices = GetJointIndicesFromNames_(joint_names);
out_positions->reserve(indices.size());
out_velocities->reserve(indices.size());
out_efforts->reserve(indices.size());
for (const size_t idx : indices) {
if (idx >= jointPositions_.size()) {
if (idx >= joint_positions_.size()) {
continue;
}
out_positions->push_back(jointPositions_[idx]);
out_velocities->push_back((idx < jointVelocities_.size()) ? jointVelocities_[idx] : 0.0);
out_efforts->push_back((idx < jointEfforts_.size()) ? jointEfforts_[idx] : 0.0);
out_positions->push_back(joint_positions_[idx]);
out_velocities->push_back((idx < joint_velocities_.size()) ? joint_velocities_[idx] : 0.0);
out_efforts->push_back((idx < joint_efforts_.size()) ? joint_efforts_[idx] : 0.0);
}
return !out_positions->empty();

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

@@ -1,13 +1,21 @@
#include <thread>
#include <algorithm>
#include <unistd.h>
/**
* @file robot_control_node.cpp
* @brief 机器人控制主节点实现
*/
#include "core/robot_control_node.hpp"
#include "actions/action_manager.hpp"
#include "core/common_enum.hpp"
#include "core/motion_parameters.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "core/robot_control_node.hpp"
#include "actions/action_manager.hpp"
#include "core/motion_parameters.hpp"
#include "core/common_enum.hpp"
#include <algorithm>
#include <chrono>
#include <thread>
#include <unistd.h>
using namespace robot_control;
@@ -24,30 +32,30 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
robot_arm_status_pub_ = this->create_publisher<interfaces::msg::RobotArmStatus>(
"/robot_control/arm_status", 10);
switch_client_ = create_client<controller_manager_msgs::srv::SwitchController>("/controller_manager/switch_controller");
imuMsgSub_ = this->create_subscription<ImuMsg>("/openzen/imu_msg", 10,std::bind(&RobotControlNode::ImuMsgCallback, this, std::placeholders::_1));
jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1));
wheelStatesSub_ = this->create_subscription<MotorPos>("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, this, std::placeholders::_1));
joyCommandSub_ = this->create_subscription<std_msgs::msg::String>("/gamepad_msg", 10,std::bind(&RobotControlNode::JoyCommandCallback, this, std::placeholders::_1));
imu_msg_sub_ = this->create_subscription<ImuMsg>("/openzen/imu_msg", 10,std::bind(&RobotControlNode::ImuMsgCallback, this, std::placeholders::_1));
joint_states_sub_ = this->create_subscription<sensor_msgs::msg::JointState>("/joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1));
wheel_states_sub_ = this->create_subscription<MotorPos>("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, this, std::placeholders::_1));
joy_command_sub_ = this->create_subscription<std_msgs::msg::String>("/gamepad_msg", 10,std::bind(&RobotControlNode::JoyCommandCallback, this, std::placeholders::_1));
arm_hardware_service_ = std::make_unique<ArmHardwareService>(
this,
&robotControlManager_,
&robot_control_manager_,
left_gpio_pub_,
right_gpio_pub_,
&joint_enabled_cache_,
&joint_error_code_cache_);
kinematics_service_ = std::make_unique<KinematicsService>(
this,
&robotControlManager_);
&robot_control_manager_);
safety_service_ = std::make_shared<SafetyService>(this);
arm_status_service_ = std::make_unique<ArmStatusService>(
this,
&robotControlManager_,
&robot_control_manager_,
safety_service_.get(),
robot_arm_status_pub_,
&joint_enabled_cache_,
&joint_error_code_cache_);
dynamicJointStatesSub_ = this->create_subscription<control_msgs::msg::DynamicJointState>(
dynamic_joint_states_sub_ = this->create_subscription<control_msgs::msg::DynamicJointState>(
"/dynamic_joint_states",
10,
std::bind(
@@ -78,18 +86,18 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
std::placeholders::_2));
// 初始化 RemoteController（用于 gamepad/jog 相关逻辑）
remoteController_ = std::make_unique<RemoteController>(
remote_controller_ = std::make_unique<RemoteController>(
this,
robotControlManager_);
robot_control_manager_);
// 初始化 ActionManager（内部会创建所需的 MotorService）
action_manager_ = std::make_unique<ActionManager>(
this,
robotControlManager_,
robot_control_manager_,
safety_service_);
action_manager_->initialize();
action_manager_->Initialize();
const bool ret1 = activateController("left_arm_controller");
const bool ret1 = ActivateController_("left_arm_controller");
if (ret1) {
RCLCPP_INFO(this->get_logger(), "Activated left_arm_controller: trajectory_controller");
@@ -97,7 +105,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
RCLCPP_ERROR(this->get_logger(), "Failed to activate left_arm_controller: trajectory_controller");
}
const bool ret2 = activateController("right_arm_controller");
const bool ret2 = ActivateController_("right_arm_controller");
if (ret2) {
RCLCPP_INFO(this->get_logger(), "Activated right_arm_controller: trajectory_controller");
@@ -107,13 +115,13 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
// 初始化MoveIt（需要在ROS节点创建后调用）
if (robotControlManager_.InitializeArmMoveIt(this)) {
if (robot_control_manager_.InitializeArmMoveIt(this)) {
RCLCPP_INFO(this->get_logger(), "MoveIt initialized successfully");
} else {
RCLCPP_WARN(this->get_logger(), "MoveIt initialization failed or arm controller not enabled");
}
const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
const MotionParameters& motion_params = robot_control_manager_.GetMotionParameters();
const size_t arm_joint_count = motion_params.arm_dof_ * 2;
joint_enabled_cache_.assign(arm_joint_count, false);
joint_error_code_cache_.assign(arm_joint_count, 0);
@@ -133,8 +141,8 @@ RobotControlNode::~RobotControlNode()
void RobotControlNode::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg)
{
// 委托给 RemoteController 处理
if (remoteController_) {
remoteController_->ProcessCommand(msg);
if (remote_controller_) {
remote_controller_->ProcessCommand(msg);
}
}
@@ -145,13 +153,13 @@ void RobotControlNode::JointStatesCallback(const sensor_msgs::msg::JointState::S
return;
}
robotControlManager_.UpdateJointStates(msg);
robot_control_manager_.UpdateJointStates(msg);
// 去掉周期发布链路：当关节初始化完成后，单次执行电机 fault reset + enable
if (!is_robot_enabled_ && robotControlManager_.RobotInitFinished())
if (!is_robot_enabled_ && robot_control_manager_.RobotInitFinished())
{
motor_reset_fault_all();
motor_enable(0, 1);
MotorResetFaultAll_();
MotorEnable_(0, 1);
is_robot_enabled_ = true;
}
}
@@ -163,7 +171,7 @@ void RobotControlNode::ImuMsgCallback(const ImuMsg::SharedPtr cmd_msg)
return;
}
robotControlManager_.UpdateImuMsg(cmd_msg);
robot_control_manager_.UpdateImuMsg(cmd_msg);
}
void RobotControlNode::WheelStatesCallback(const MotorPos::SharedPtr cmd_msg)
@@ -173,10 +181,10 @@ void RobotControlNode::WheelStatesCallback(const MotorPos::SharedPtr cmd_msg)
return;
}
robotControlManager_.UpdateWheelStates(cmd_msg);
robot_control_manager_.UpdateWheelStates(cmd_msg);
}
bool RobotControlNode::activateController(const std::string& controller_name)
bool RobotControlNode::ActivateController_(const std::string& controller_name)
{
// 通过 controller_manager 的 switch_controller 服务激活指定控制器
if (!switch_client_)
@@ -235,12 +243,12 @@ bool RobotControlNode::activateController(const std::string& controller_name)
return true;
}
void RobotControlNode::motor_fault(int id, double fault)
void RobotControlNode::MotorFault_(int id, double fault)
{
const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
const MotionParameters& motion_params = robot_control_manager_.GetMotionParameters();
std::vector<std::string> left;
std::vector<std::string> right;
if (!split_arm_joint_names_(motion_params, left, right)) {
if (!SplitArmJointNames_(motion_params, left, right)) {
return;
}
@@ -248,16 +256,16 @@ void RobotControlNode::motor_fault(int id, double fault)
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));
PublishGpioCommand_(left_gpio_pub_, left, "fault", id, fault, 0);
PublishGpioCommand_(right_gpio_pub_, right, "fault", id, fault, static_cast<int>(arm_dof));
}
void RobotControlNode::motor_enable(int id, double enable)
void RobotControlNode::MotorEnable_(int id, double enable)
{
const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
const MotionParameters& motion_params = robot_control_manager_.GetMotionParameters();
std::vector<std::string> left;
std::vector<std::string> right;
if (!split_arm_joint_names_(motion_params, left, right)) {
if (!SplitArmJointNames_(motion_params, left, right)) {
return;
}
@@ -265,11 +273,11 @@ void RobotControlNode::motor_enable(int id, double enable)
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));
PublishGpioCommand_(left_gpio_pub_, left, "enable", id, enable, 0);
PublishGpioCommand_(right_gpio_pub_, right, "enable", id, enable, static_cast<int>(arm_dof));
}
bool RobotControlNode::split_arm_joint_names_(
bool RobotControlNode::SplitArmJointNames_(
const MotionParameters& motion_params,
std::vector<std::string>& left,
std::vector<std::string>& right) const
@@ -294,7 +302,7 @@ bool RobotControlNode::split_arm_joint_names_(
return true;
}
void RobotControlNode::publish_gpio_command_(
void RobotControlNode::PublishGpioCommand_(
const rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr& pub,
const std::vector<std::string>& joints,
const std::string& interface_name,
@@ -320,10 +328,10 @@ void RobotControlNode::publish_gpio_command_(
pub->publish(msg);
}
void RobotControlNode::motor_reset_fault_all()
void RobotControlNode::MotorResetFaultAll_()
{
motor_fault(0, 1);
MotorFault_(0, 1);
usleep(500);
motor_fault(0, 0);
MotorFault_(0, 0);
}

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

@@ -1,7 +1,12 @@
#include <rclcpp/rclcpp.hpp>
/**
* @file main.cpp
* @brief 机器人控制节点入口
*/
#include "core/robot_control_node.hpp"
#include <rclcpp/rclcpp.hpp>
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);

5
src/robot_control/src/services/arm_hardware_service.cpp
View File

@@ -1,3 +1,8 @@
/**
* @file arm_hardware_service.cpp
* @brief 手臂硬件服务实现，管理使能与错误清除
*/
#include "services/arm_hardware_service.hpp"
#include <unistd.h>

19
src/robot_control/src/services/arm_status_service.cpp
View File

@@ -1,3 +1,8 @@
/**
* @file arm_status_service.cpp
* @brief 手臂状态服务实现
*/
#include "services/arm_status_service.hpp"
#include <algorithm>
@@ -19,6 +24,14 @@ ArmStatusService::ArmStatusService(
joint_enabled_cache_(joint_enabled_cache),
joint_error_code_cache_(joint_error_code_cache)
{
if (node_) {
if (!node_->has_parameter("arm_status.publish_arm_pose")) {
publish_arm_pose_ = node_->declare_parameter<bool>(
"arm_status.publish_arm_pose", publish_arm_pose_);
} else {
(void)node_->get_parameter("arm_status.publish_arm_pose", publish_arm_pose_);
}
}
}
void ArmStatusService::OnDynamicJointStates(const control_msgs::msg::DynamicJointState::SharedPtr msg)
@@ -106,8 +119,10 @@ void ArmStatusService::PublishRobotArmStatus()
msg.estop_generation = 0;
}
(void)manager_->GetArmCurrentPose(0, &msg.left_arm_pose, nullptr);
(void)manager_->GetArmCurrentPose(1, &msg.right_arm_pose, nullptr);
if (publish_arm_pose_) {
(void)manager_->GetArmCurrentPose(0, &msg.left_arm_pose, nullptr);
(void)manager_->GetArmCurrentPose(1, &msg.right_arm_pose, nullptr);
}
status_pub_->publish(msg);
}

5
src/robot_control/src/services/kinematics_service.cpp
View File

@@ -1,3 +1,8 @@
/**
* @file kinematics_service.cpp
* @brief 运动学服务实现，处理 IK 请求
*/
#include "services/kinematics_service.hpp"
namespace robot_control

9
src/robot_control/src/services/motor_service.cpp
View File

@@ -1,3 +1,8 @@
/**
* @file motor_service.cpp
* @brief 电机服务实现，管理轮子速度指令
*/
#include "services/motor_service.hpp"
#include <cmath>
@@ -16,7 +21,7 @@ MotorService::MotorService(rclcpp::Node* node)
motor_param_client_ = node_->create_client<MotorParam>("/motor_param");
}
void MotorService::configure_wheel_speed(double move_distance, double ratio, bool is_jog_mode)
void MotorService::ConfigureWheelSpeed(double move_distance, double ratio, bool is_jog_mode)
{
// 与原 MoveWheelAction 中逻辑保持一致：
// 仅在向前运动且非点动模式时，调整电机速度到 ratio * 51
@@ -32,7 +37,7 @@ void MotorService::configure_wheel_speed(double move_distance, double ratio, boo
}
}
void MotorService::publish_wheel_command(const std::vector<double>& wheel_commands)
void MotorService::PublishWheelCommand(const std::vector<double>& wheel_commands)
{
if (!motor_cmd_pub_) {
return;

5
src/robot_control/src/services/safety_service.cpp
View File

@@ -1,3 +1,8 @@
/**
* @file safety_service.cpp
* @brief 安全服务实现，管理急停状态
*/
#include "services/safety_service.hpp"
#include <functional>

38
src/robot_control/src/utils/s_curve_trajectory.cpp
View File

@@ -1,8 +1,12 @@
/**
* @file s_curve_trajectory.cpp
* @brief S 曲线轨迹生成器实现
*/
#include "utils/s_curve_trajectory.hpp"
#include <cmath>
#include <stdexcept>
#include <algorithm>
#include <iostream>
namespace robot_control
{
@@ -58,7 +62,7 @@ S_CurveTrajectory::S_CurveTrajectory(double max_velocity, double max_acceleratio
}
// 多轴初始化
void S_CurveTrajectory::init(const std::vector<double>& start_pos,
void S_CurveTrajectory::Init(const std::vector<double>& start_pos,
const std::vector<double>& target_pos)
{
if (start_pos.size() != target_pos.size())
@@ -94,7 +98,7 @@ void S_CurveTrajectory::init(const std::vector<double>& start_pos,
}
// 单轴初始化
void S_CurveTrajectory::init(double start_pos, double target_pos)
void S_CurveTrajectory::Init(double start_pos, double target_pos)
{
is_single_joint_ = true;
is_stopping_ = false;
@@ -225,12 +229,12 @@ void S_CurveTrajectory::calculateTrajectoryParams()
}
// 多轴位置更新
std::vector<double> S_CurveTrajectory::update(double time)
std::vector<double> S_CurveTrajectory::Update(double time)
{
if (is_single_joint_)
throw std::runtime_error("Call updateSingle for single joint mode");
throw std::runtime_error("Call UpdateSingle for single joint mode");
if (is_stopping_)
throw std::runtime_error("In stop mode, use updateStop instead");
throw std::runtime_error("In stop mode, use UpdateStop instead");
size_t dof = start_pos_.size();
double t = std::clamp(time, 0.0, total_time_);
@@ -360,19 +364,19 @@ std::vector<double> S_CurveTrajectory::update(double time)
}
// 单轴位置更新
double S_CurveTrajectory::updateSingle(double time)
double S_CurveTrajectory::UpdateSingle(double time)
{
if (!is_single_joint_)
throw std::runtime_error("Call update for multi-joint mode");
throw std::runtime_error("Call Update for multi-joint mode");
if (is_stopping_)
throw std::runtime_error("In stop mode, use updateStop instead");
throw std::runtime_error("In stop mode, use UpdateStop instead");
std::vector<double> result = update(time);
std::vector<double> result = Update(time);
return result[0];
}
// 设置各轴最大速度
void S_CurveTrajectory::setMaxVelocities(const std::vector<double>& max_velocities)
void S_CurveTrajectory::SetMaxVelocities(const std::vector<double>& max_velocities)
{
if (max_velocities.size() != max_velocity_.size())
throw std::invalid_argument("Velocity size mismatch");
@@ -381,14 +385,14 @@ void S_CurveTrajectory::setMaxVelocities(const std::vector<double>& max_velociti
max_velocity_ = max_velocities;
}
void S_CurveTrajectory::setMaxVelocity(double max_velocity)
void S_CurveTrajectory::SetMaxVelocity(double max_velocity)
{
if (max_velocity <= 0)
throw std::invalid_argument("Max velocity must be positive");
max_velocity_[0] = max_velocity;
}
void S_CurveTrajectory::setMaxAccelerations(const std::vector<double>& max_accelerations)
void S_CurveTrajectory::SetMaxAccelerations(const std::vector<double>& max_accelerations)
{
if (max_accelerations.size() != max_acceleration_.size())
throw std::invalid_argument("Acceleration size mismatch");
@@ -397,7 +401,7 @@ void S_CurveTrajectory::setMaxAccelerations(const std::vector<double>& max_accel
max_acceleration_ = max_accelerations;
}
void S_CurveTrajectory::setMaxAcceleration(double max_acceleration)
void S_CurveTrajectory::SetMaxAcceleration(double max_acceleration)
{
if (max_acceleration <= 0)
throw std::invalid_argument("Max acceleration must be positive");
@@ -405,7 +409,7 @@ void S_CurveTrajectory::setMaxAcceleration(double max_acceleration)
}
// 计算急停轨迹参数（简化版本，使用梯形急停）
void S_CurveTrajectory::calculateStopTrajectory()
void S_CurveTrajectory::CalculateStopTrajectory()
{
size_t dof = current_pos_.size();
is_stopping_ = true;
@@ -446,10 +450,10 @@ void S_CurveTrajectory::calculateStopTrajectory()
}
// 更新急停轨迹（简化版本）
std::vector<double> S_CurveTrajectory::updateStop(double dt)
std::vector<double> S_CurveTrajectory::UpdateStop(double dt)
{
if (!is_stopping_)
throw std::runtime_error("Not in stop mode, call calculateStopTrajectory first");
throw std::runtime_error("Not in stop mode, call CalculateStopTrajectory first");
size_t dof = stop_start_pos_.size();
double t = std::clamp(dt, 0.0, stop_total_time_); // 统一急停时间

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

@@ -1,8 +1,12 @@
/**
* @file trapezoidal_trajectory.cpp
* @brief 梯形速度轨迹生成器实现
*/
#include "utils/trapezoidal_trajectory.hpp"
#include <cmath>
#include <stdexcept>
#include <algorithm>
#include <iostream>
namespace robot_control
{
@@ -40,7 +44,7 @@ TrapezoidalTrajectory::TrapezoidalTrajectory(double max_velocity, double max_acc
}
// 多轴初始化
void TrapezoidalTrajectory::init(const std::vector<double>& start_pos,
void TrapezoidalTrajectory::Init(const std::vector<double>& start_pos,
const std::vector<double>& target_pos)
{
if (start_pos.size() != target_pos.size())
@@ -71,7 +75,7 @@ void TrapezoidalTrajectory::init(const std::vector<double>& start_pos,
}
// 单轴初始化
void TrapezoidalTrajectory::init(double start_pos, double target_pos)
void TrapezoidalTrajectory::Init(double start_pos, double target_pos)
{
is_single_joint_ = true;
is_stopping_ = false;
@@ -181,12 +185,12 @@ void TrapezoidalTrajectory::calculateTrajectoryParams()
}
// 多轴位置更新
std::vector<double> TrapezoidalTrajectory::update(double time)
std::vector<double> TrapezoidalTrajectory::Update(double time)
{
if (is_single_joint_)
throw std::runtime_error("Call updateSingle for single joint mode");
throw std::runtime_error("Call UpdateSingle for single joint mode");
if (is_stopping_)
throw std::runtime_error("In stop mode, use updateStop instead");
throw std::runtime_error("In stop mode, use UpdateStop instead");
size_t dof = start_pos_.size();
double t = std::clamp(time, 0.0, total_time_); // 统一时间
@@ -247,12 +251,12 @@ std::vector<double> TrapezoidalTrajectory::update(double time)
}
// 单轴位置更新
double TrapezoidalTrajectory::updateSingle(double time)
double TrapezoidalTrajectory::UpdateSingle(double time)
{
if (!is_single_joint_)
throw std::runtime_error("Call update for multi-joint mode");
throw std::runtime_error("Call Update for multi-joint mode");
if (is_stopping_)
throw std::runtime_error("In stop mode, use updateStop instead");
throw std::runtime_error("In stop mode, use UpdateStop instead");
double t = std::clamp(time, 0.0, total_time_);
double dist = total_distance_[0];
@@ -305,7 +309,7 @@ double TrapezoidalTrajectory::updateSingle(double time)
}
// 设置各轴最大速度
void TrapezoidalTrajectory::setMaxVelocities(const std::vector<double>& max_velocities)
void TrapezoidalTrajectory::SetMaxVelocities(const std::vector<double>& max_velocities)
{
if (max_velocities.size() != max_velocity_.size())
throw std::invalid_argument("Velocity size mismatch");
@@ -315,7 +319,7 @@ void TrapezoidalTrajectory::setMaxVelocities(const std::vector<double>& max_velo
}
// 设置单轴最大速度
void TrapezoidalTrajectory::setMaxVelocity(double max_velocity)
void TrapezoidalTrajectory::SetMaxVelocity(double max_velocity)
{
if (max_velocity <= 0)
throw std::invalid_argument("Max velocity must be positive");
@@ -323,7 +327,7 @@ void TrapezoidalTrajectory::setMaxVelocity(double max_velocity)
}
// 设置各轴最大加速度
void TrapezoidalTrajectory::setMaxAccelerations(const std::vector<double>& max_accelerations)
void TrapezoidalTrajectory::SetMaxAccelerations(const std::vector<double>& max_accelerations)
{
if (max_accelerations.size() != max_acceleration_.size())
throw std::invalid_argument("Acceleration size mismatch");
@@ -333,7 +337,7 @@ void TrapezoidalTrajectory::setMaxAccelerations(const std::vector<double>& max_a
}
// 设置单轴最大加速度
void TrapezoidalTrajectory::setMaxAcceleration(double max_acceleration)
void TrapezoidalTrajectory::SetMaxAcceleration(double max_acceleration)
{
if (max_acceleration <= 0)
throw std::invalid_argument("Max acceleration must be positive");
@@ -341,7 +345,7 @@ void TrapezoidalTrajectory::setMaxAcceleration(double max_acceleration)
}
// 计算急停轨迹参数
void TrapezoidalTrajectory::calculateStopTrajectory()
void TrapezoidalTrajectory::CalculateStopTrajectory()
{
size_t dof = current_pos_.size();
is_stopping_ = true;
@@ -386,10 +390,10 @@ void TrapezoidalTrajectory::calculateStopTrajectory()
}
// 更新急停轨迹
std::vector<double> TrapezoidalTrajectory::updateStop(double dt)
std::vector<double> TrapezoidalTrajectory::UpdateStop(double dt)
{
if (!is_stopping_)
throw std::runtime_error("Not in stop mode, call calculateStopTrajectory first");
throw std::runtime_error("Not in stop mode, call CalculateStopTrajectory first");
size_t dof = stop_start_pos_.size();
double t = std::clamp(dt, 0.0, stop_total_time_); // 统一急停时间