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

optimize moveit and add new remote pacakge

Browse Source
This commit is contained in:
NuoDaJia
2026-01-29 18:25:35 +08:00
parent 237f9482c0
commit 20ffabcd81
271 changed files with 2937 additions and 5382 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
arm_keyboard_input/CMakeLists.txt
View File

@@ -1,41 +0,0 @@
cmake_minimum_required(VERSION 3.8)
project(arm_keyboard_input)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
add_compile_options(-g)
include_directories(
include
${CMAKE_CURRENT_SOURCE_DIR}/../include
${EIGEN3_INCLUDE_DIRS} # 引入Eigen3的头文件路径
)
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(interfaces REQUIRED)
find_package(gjk_interface REQUIRED)
find_package(rclcpp_action REQUIRED)
# 设置源文件
set(SOURCES
src/arm_keyboard_node.cpp
)
add_executable(arm_keyboard_input ${SOURCES})
ament_target_dependencies(arm_keyboard_input
rclcpp
std_msgs
rclcpp_action
gjk_interface
interfaces
)
install(TARGETS
arm_keyboard_input
DESTINATION lib/${PROJECT_NAME})
ament_package()

53
arm_keyboard_input/include/arm_keyboard_node.hpp
View File

@@ -1,53 +0,0 @@
#ifndef ARM_KEYBOARD_NODE_HPP
#define ARM_KEYBOARD_NODE_HPP
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "interfaces/action/arm.hpp"
#include "arm_action_define.h"
#include "rm_define.h"
using ArmAction = interfaces::action::Arm;
using GoalClientArm = rclcpp_action::Client<ArmAction>;
using GoalHandleArm = rclcpp_action::ClientGoalHandle<ArmAction>;
class ArmKeyboardNode: public rclcpp::Node
{
public:
ArmKeyboardNode();
~ArmKeyboardNode();
static rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr leftArmAimPub_;
static rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr rightArmAimPub_;
std::atomic<bool> exit_flag;
GoalClientArm::SharedPtr client_ptr_;
void SendAngleSteppingGoal(const char *command);
void SendAngleMovingGoal(const float *values, int armId);
void SendDirectSteppingGoal(const char *command);
void SendDirectMovingGoal(const float *values, int armId);
void SendDirectSteppingGoal2(const float *values, int armId);
void StopGoal();
private:
std::shared_mutex rw_mutex_;
void goal_response_callback(GoalHandleArm::SharedPtr goal_handle);
void feedback_callback(
GoalHandleArm::SharedPtr goal_handle,
const std::shared_ptr<const ArmAction::Feedback> feedback);
void result_callback(const GoalHandleArm::WrappedResult & result);
rclcpp_action::Client<ArmAction>::SendGoalOptions goal_options_;
double leftArmAngleAim_[USED_ARM_DOF];
double rightArmAngleAim_[USED_ARM_DOF];
int64_t leftCommandId_;
int64_t rightCommandId_;
int leftDirectCommand_;
int rightDirectCommand_;
GoalHandleArm::SharedPtr leftArmGoalHandle_;
GoalHandleArm::SharedPtr rightArmGoalHandle_;
};
#endif // ARM_KEYBOARD_NODE_HPP

20
arm_keyboard_input/package.xml
View File

@@ -1,20 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>arm_keyboard_input</name>
<version>0.0.0</version>
<description>arm_keyboard_input</description>
<maintainer email="zj@todo.todo">zj</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>interfaces</depend>
<depend>rclcpp</depend>
<depend>gjk_interface</depend>
<exec_depend>rclcpp_action</exec_depend>
<depend>std_msgs</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

870
arm_keyboard_input/src/arm_keyboard_node.cpp
View File

@@ -1,870 +0,0 @@
#include <thread>
#include <functional>
#include <future>
#include <sstream>
#include <iostream>
#include <chrono>
#include <mutex>
#include <string>
#include <fstream>
#include <termios.h>
#include <fcntl.h>
#include <unistd.h>
#include <dirent.h>
#include <linux/input.h>
#include "arm_keyboard_node.hpp"
#include "gjk_interface.hpp"
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr ArmKeyboardNode::leftArmAimPub_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr ArmKeyboardNode::rightArmAimPub_;
std::thread inputThread_;
std::mutex armAimMutex_;
int64_t commandId = 0;
using namespace std::placeholders;
using namespace Eigen;
const std::unordered_map<int, int> keyToCommand = {
{KEY_A, 0}, {KEY_S, 1}, {KEY_D, 2}, {KEY_F, 3}, {KEY_G, 4}, {KEY_H, 5},
{KEY_Z, 6}, {KEY_X, 7}, {KEY_C, 8}, {KEY_V, 9}, {KEY_B, 10}, {KEY_N, 11},
{KEY_W, 12}, {KEY_E, 13}, {KEY_R, 14}
};
enum {
KEEP_ACTION = 0,
STOP_ACTION = 1,
NEW_ACTION = 2
};
void SetNonCanonicalMode(bool enable) {
static struct termios oldt, newt;
if (enable) {
// 保存当前终端设置
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
// 禁用缓冲和回显
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
} else {
// 恢复终端设置
oldt.c_lflag |= (ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
}
}
std::string to_lower(const std::string& str) {
std::string res;
for (char c : str) {
res += tolower(c);
}
return res;
}
// 检查设备名称是否包含键盘关键词
bool is_keyboard_device(const std::string& event_path) {
// 设备名称文件路径：/sys/class/input/eventX/device/name
std::string name_path = "/sys/class/input/" + event_path + "/device/name";
// 读取设备名称
std::ifstream name_file(name_path);
if (!name_file.is_open()) {
return false;
}
std::string name;
std::getline(name_file, name);
name_file.close();
// 检查名称是否包含键盘相关关键词
std::string lower_name = to_lower(name);
const std::vector<std::string> keywords = {"at translated set 2 keyboard"};
for (const auto& kw : keywords) {
if (lower_name.find(kw) != std::string::npos) {
return true;
}
}
return false;
}
// 查找所有输入设备并筛选出键盘
std::string find_keyboard_devices() {
const std::string input_dir = "/sys/class/input/"; // 输入设备的sysfs目录
// 打开sysfs输入设备目录
DIR* dir = opendir(input_dir.c_str());
if (!dir) {
std::cerr << "无法打开目录: " << input_dir << std::endl;
return "";
}
// 遍历所有event设备（event0, event1, ...）
dirent* entry;
while ((entry = readdir(dir)) != nullptr) {
std::string entry_name = entry->d_name;
// 只处理event开头的设备（输入设备通常以event命名）
if (entry_name.substr(0, 5) != "event") {
continue;
}
// 检查该设备是否为键盘
if (is_keyboard_device(entry_name)) {
// 转换为/dev/input/eventX格式的设备路径
std::string device_path = "/dev/input/" + entry_name;
// 打印设备信息（可选）
std::ifstream name_file("/sys/class/input/" + entry_name + "/device/name");
std::string name;
std::getline(name_file, name);
std::cout << "找到键盘设备: " << device_path << " (" << name << ")" << std::endl;
return device_path;
}
}
closedir(dir);
return "";
}
void KeyboardInputAngleStepping(std::shared_ptr<ArmKeyboardNode> armNode) {
// 设置终端为无缓冲模式
std::cout << "q:退出,[a,s,d,f,g,h]:左臂控制,[z,x,c,v,b,n]:右臂控制" << std::endl;
SetNonCanonicalMode(true);
std::string device = find_keyboard_devices();
std::cout << "找到键盘设备: " << device << std::endl;
const char* device_path = device.c_str(); // 替换为你的键盘设备
if (device_path == nullptr || strlen(device_path) == 0) {
std::cerr << "未找到键盘设备，请确保键盘已连接。" << std::endl;
SetNonCanonicalMode(false);
return;
}
int fd = open(device_path, O_RDONLY);
if (fd < 0) {
std::cerr << "无法打开设备 " << device_path << "，请检查权限或路径是否正确" << std::endl;
SetNonCanonicalMode(false);
return;
}
struct input_event ev;
char command[16] = {0};
rclcpp::Time startTime = armNode->get_clock()->now();
while (rclcpp::ok()) {
int cancel_flag = 0;
ssize_t n = read(fd, &ev, sizeof(ev));
if (n < (ssize_t)sizeof(ev)) {
continue; // 事件不完整，跳过
}
// RCLCPP_INFO(armNode->get_logger(), "type: %d, code: %d, value: %d\n", ev.type, ev.code, ev.value);
// 仅处理键盘事件
if (ev.type == EV_KEY) {
// ev.code：键码（如KEY_A=30），ev.value：0=释放，1=按下，2=重复（可忽略）
if (ev.value == 1) { // 按键按下
if (keyToCommand.find(ev.code) == keyToCommand.end()) {
if (ev.code == KEY_Q) { // 按下'q'退出
std::cout << "退出遥控模式。" << std::endl;
break;
}
continue; // 非定义按键，忽略
}
command[keyToCommand.at(ev.code)] = 1;
} else if (ev.value == 0) { // 按键释放
if (keyToCommand.find(ev.code) == keyToCommand.end()) {
continue; // 非定义按键，忽略
}
command[keyToCommand.at(ev.code)] = 0;
cancel_flag = 1;
}
}
rclcpp::Time nowTime = armNode->get_clock()->now();
int64_t duration = (nowTime - startTime).nanoseconds();
if (duration >= 100000000 || cancel_flag) { // 每100ms发送一次命令
armNode->SendAngleSteppingGoal(command);
startTime = nowTime;
}
}
// 清理资源
close(fd);
SetNonCanonicalMode(false);
}
void KeyboardInputAngleMoving(std::shared_ptr<ArmKeyboardNode> armNode) {
std::string line;
while (rclcpp::ok()) {
std::cout << "请输入: 整数(0(退出)/1(左臂)/2(右臂)),6个float,用','分割,以';'结尾:" << std::endl;
std::getline(std::cin, line, ';');
std::istringstream ss(line);
std::string token;
int mode;
float values[USED_ARM_DOF];
bool valid = true;
// 解析整数
if (!std::getline(ss, token, ',')) {
valid = false;
}
else {
try {
mode = std::stoi(token);
if (mode != 1 && mode != 2) valid = false;
} catch (...) {
valid = false;
}
}
// 解析6个float
for (int i = 0; i < USED_ARM_DOF && valid; ++i) {
if (!std::getline(ss, token, ',')) { valid = false; break; }
try {
values[i] = std::stof(token);
} catch (...) { valid = false; }
}
if (mode == 0) {
std::cout << "退出输入角度模式。" << std::endl;
return;
}
if (valid) {
if (mode == 1)
armNode->SendAngleMovingGoal(values, LEFT_ARM);
else if (mode == 2)
armNode->SendAngleMovingGoal(values, RIGHT_ARM);
} else {
std::cout << "输入格式错误,请重试。" << std::endl;
}
std::this_thread::sleep_for(std::chrono::seconds(1));
}
}
void KeyboardInputPoseMoving(std::shared_ptr<ArmKeyboardNode> armNode) {
std::string line;
while (rclcpp::ok()) {
std::cout << "请输入: 整数(0(退出)/1(左臂)/2(右臂)),7个float,用','分割,以';'结尾:" << std::endl;
std::getline(std::cin, line, ';');
std::istringstream ss(line);
std::string token;
int mode;
float values[POSE_DIMENSION];
bool valid = true;
// 解析整数
if (!std::getline(ss, token, ',')) {
valid = false;
}
else {
try {
mode = std::stoi(token);
if (mode != 1 && mode != 2) valid = false;
} catch (...) {
valid = false;
}
}
// 解析6个float
for (int i = 0; i < POSE_DIMENSION && valid; ++i) {
if (!std::getline(ss, token, ',')) { valid = false; break; }
try {
values[i] = std::stof(token);
} catch (...) { valid = false; }
}
if (mode == 0) {
std::cout << "退出输入pose模式。" << std::endl;
return;
}
if (valid) {
if (mode == 1)
armNode->SendDirectMovingGoal(values, LEFT_ARM);
else if (mode == 2)
armNode->SendDirectMovingGoal(values, RIGHT_ARM);
} else {
std::cout << "输入格式错误,请重试。" << std::endl;
}
std::this_thread::sleep_for(std::chrono::seconds(1));
}
}
void KeyboardInputPoseStepping2(std::shared_ptr<ArmKeyboardNode> armNode) {
std::string line;
auto message = std_msgs::msg::Float64MultiArray();
message.data.resize(ARM_DOF);
while (rclcpp::ok()) {
std::cout << "请输入: 整数(0(退出)/1(左臂)/2(右臂)/3(停止)),7个float,用','分割,以';'结尾:" << std::endl;
std::getline(std::cin, line, ';');
std::istringstream ss(line);
std::string token;
int mode;
float values[POSE_DIMENSION];
bool valid = true;
// 解析整数
if (!std::getline(ss, token, ',')) {
valid = false;
}
else {
try {
mode = std::stoi(token);
if (mode != 1 && mode != 2) valid = false;
} catch (...) {
valid = false;
}
}
// 解析6个float
for (int i = 0; i < POSE_DIMENSION && valid; ++i) {
if (!std::getline(ss, token, ',')) { valid = false; break; }
try {
values[i] = std::stof(token);
} catch (...) { valid = false; }
}
if (mode == 0) {
std::cout << "退出输入角度模式。" << std::endl;
return;
}
if (mode == 3) {
armNode->StopGoal();
continue;
}
if (valid) {
if (mode == 1)
armNode->SendDirectSteppingGoal2(values, LEFT_ARM);
else if (mode == 2)
armNode->SendDirectSteppingGoal2(values, RIGHT_ARM);
} else {
std::cout << "输入格式错误,请重试。" << std::endl;
}
std::this_thread::sleep_for(std::chrono::seconds(1));
}
}
void ArmKeyboardNode::StopGoal()
{
std::cout << "取消当前目标。" << std::endl;
std::lock_guard<std::mutex> lock(armAimMutex_);
if (leftArmGoalHandle_ != nullptr && leftCommandId_ != 0) {
client_ptr_->async_cancel_goal(leftArmGoalHandle_);
leftArmGoalHandle_.reset();
std::cout << "左臂目标已取消。" << std::endl;
}
if (rightArmGoalHandle_ != nullptr && rightCommandId_ != 0) {
client_ptr_->async_cancel_goal(rightArmGoalHandle_);
rightArmGoalHandle_.reset();
std::cout << "右臂目标已取消。" << std::endl;
}
}
void ArmKeyboardNode::SendDirectSteppingGoal2(const float *values, int armId)
{
std::lock_guard<std::mutex> lock(armAimMutex_);
int64_t timeStamp = get_clock()->now().nanoseconds();
auto goal_msg = ArmAction::Goal();
commandId++;
goal_msg.body_id = armId;
goal_msg.data_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制
goal_msg.data_length = POSE_DIMENSION;
goal_msg.frame_time_stamp = timeStamp;
goal_msg.command_id = commandId;
for (int i = 0; i < POSE_DIMENSION; ++i) {
goal_msg.data_array.push_back(values[i]);
}
auto future = client_ptr_->async_send_goal(goal_msg, goal_options_);
if (armId == LEFT_ARM) {
leftCommandId_ = commandId;
leftArmGoalHandle_ = future.get();
} else {
rightCommandId_ = commandId;
rightArmGoalHandle_ = future.get();
}
}
void ArmKeyboardNode::SendDirectMovingGoal(const float *values, int armId)
{
std::lock_guard<std::mutex> lock(armAimMutex_);
int64_t timeStamp = get_clock()->now().nanoseconds();
auto goal_msg = ArmAction::Goal();
commandId++;
goal_msg.body_id = armId;
goal_msg.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE; // 位置控制
goal_msg.data_length = POSE_DIMENSION;
goal_msg.frame_time_stamp = timeStamp;
goal_msg.command_id = commandId;
for (int i = 0; i < POSE_DIMENSION; ++i) {
goal_msg.data_array.push_back(values[i]);
}
auto future = client_ptr_->async_send_goal(goal_msg, goal_options_);
if (armId == LEFT_ARM) {
leftCommandId_ = commandId;
leftArmGoalHandle_ = future.get();
} else {
rightCommandId_ = commandId;
rightArmGoalHandle_ = future.get();
}
}
void ArmKeyboardNode::SendAngleMovingGoal(const float *values, int armId)
{
std::lock_guard<std::mutex> lock(armAimMutex_);
int64_t timeStamp = get_clock()->now().nanoseconds();
auto goal_msg = ArmAction::Goal();
commandId++;
RCLCPP_INFO(this->get_logger(), "SendAngleMovingGoal id=%ld", commandId);
goal_msg.body_id = armId;
goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE; // 位置控制
goal_msg.data_length = USED_ARM_DOF;
goal_msg.frame_time_stamp = timeStamp;
goal_msg.command_id = commandId;
for (int i = 0; i < USED_ARM_DOF; ++i) {
goal_msg.data_array.push_back(values[i]);
}
auto future = client_ptr_->async_send_goal(goal_msg, goal_options_);
if (armId == LEFT_ARM) {
leftCommandId_ = commandId;
leftArmGoalHandle_ = future.get();
} else {
rightCommandId_ = commandId;
rightArmGoalHandle_ = future.get();
}
}
void KeyboardInputDirectStepping(std::shared_ptr<ArmKeyboardNode> armNode) {
// 设置终端为无缓冲模式
std::cout << "q:退出,w:左臂控制,e:右臂控制,r:负方向增量,a:x方向移动,s:y方向移动,"
<< "d:z方向移动,z:rx方向旋转,x:ry方向旋转,c:rz方向旋转,v:rw方向旋转" << std::endl;
SetNonCanonicalMode(true);
std::string device = find_keyboard_devices();
std::cout << "找到键盘设备: " << device << std::endl;
const char* device_path = device.c_str(); // 替换为你的键盘设备
if (device_path == nullptr || strlen(device_path) == 0) {
std::cerr << "未找到键盘设备，请确保键盘已连接。" << std::endl;
SetNonCanonicalMode(false);
return;
}
int fd = open(device_path, O_RDONLY);
if (fd < 0) {
std::cerr << "无法打开设备 " << device_path << "，请检查权限或路径是否正确" << std::endl;
SetNonCanonicalMode(false);
return;
}
struct input_event ev;
char command[16] = {0};
rclcpp::Time startTime = armNode->get_clock()->now();
while (rclcpp::ok()) {
int cancel_flag = 0;
ssize_t n = read(fd, &ev, sizeof(ev));
if (n < (ssize_t)sizeof(ev)) {
continue; // 事件不完整，跳过
}
// RCLCPP_INFO(armNode->get_logger(), "type: %d, code: %d, value: %d\n", ev.type, ev.code, ev.value);
// 仅处理键盘事件
if (ev.type == EV_KEY) {
// ev.code：键码（如KEY_A=30），ev.value：0=释放，1=按下，2=重复（可忽略）
if (ev.value == 1) { // 按键按下
if (keyToCommand.find(ev.code) == keyToCommand.end()) {
if (ev.code == KEY_Q) { // 按下'q'退出
std::cout << "退出遥控模式。" << std::endl;
break;
}
continue; // 非定义按键，忽略
}
command[keyToCommand.at(ev.code)] = 1;
} else if (ev.value == 0) { // 按键释放
if (keyToCommand.find(ev.code) == keyToCommand.end()) {
continue; // 非定义按键，忽略
}
command[keyToCommand.at(ev.code)] = 0;
cancel_flag = 1;
}
}
rclcpp::Time nowTime = armNode->get_clock()->now();
int64_t duration = (nowTime - startTime).nanoseconds();
if (duration >= 100000000 || cancel_flag) { // 每100ms发送一次命令
armNode->SendDirectSteppingGoal(command);
startTime = nowTime;
}
}
// 清理资源
close(fd);
SetNonCanonicalMode(false);
}
static void ListenInputThread(std::shared_ptr<ArmKeyboardNode> armNode) {
std::string line;
auto message = std_msgs::msg::Float64MultiArray();
message.data.resize(ARM_DOF);
while (rclcpp::ok()) {
std::cout << "请输入选择控制模式,0:退出,1:移动至目标角度,2:移动至目标位置,3:持续旋转角度,4:持续往某个方向移动,以';'结尾:" << std::endl;
std::getline(std::cin, line, ';');
int mode = 0;
try {
mode = std::stoi(line);
} catch (...) {
std::cout << "输入格式错误,请重试。" << std::endl;
continue;
}
switch (mode) {
case 0:
std::cout << "退出输入监听线程。" << std::endl;
armNode->exit_flag = true;
return;
case 1:
KeyboardInputAngleMoving(armNode);
break;
case 2:
KeyboardInputPoseMoving(armNode);
break;
case 3:
KeyboardInputAngleStepping(armNode);
std::getline(std::cin, line, 'q');
break;
case 4:
KeyboardInputPoseStepping2(armNode);
break;
case 5:
KeyboardInputPoseStepping2(armNode);
break;
default:
if (mode != 0) {
std::cout << "无效选择，请重试。" << std::endl;
}
break;
}
}
}
ArmKeyboardNode::ArmKeyboardNode() : Node("arm_keyboard_node")
{
for (int i = 0; i < USED_ARM_DOF; ++i) {
leftArmAngleAim_[i] = 0.0;
rightArmAngleAim_[i] = 0.0;
}
leftCommandId_ = 0;
rightCommandId_ = 0;
leftArmAimPub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("left_arm_command", 10);
rightArmAimPub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("right_arm_command", 10);
exit_flag = false;
this->client_ptr_ = rclcpp_action::create_client<ArmAction>(
this,
"ArmAction");
goal_options_.goal_response_callback =
std::bind(&ArmKeyboardNode::goal_response_callback, this, _1);
goal_options_.feedback_callback =
std::bind(&ArmKeyboardNode::feedback_callback, this, _1, _2);
goal_options_.result_callback =
std::bind(&ArmKeyboardNode::result_callback, this, _1);
}
ArmKeyboardNode::~ArmKeyboardNode()
{
if (inputThread_.joinable()) {
inputThread_.join(); // 或 join()，视你的需求
}
rightArmAimPub_.reset();
leftArmAimPub_.reset();
}
void ArmKeyboardNode::goal_response_callback(GoalHandleArm::SharedPtr goal_handle)
{
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
} else {
RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
}
}
void ArmKeyboardNode::feedback_callback(
GoalHandleArm::SharedPtr goal_handle,
const std::shared_ptr<const ArmAction::Feedback> feedback)
{
if (!goal_handle || !feedback) {
RCLCPP_INFO(this->get_logger(), "feedback received err.");
}
}
void ArmKeyboardNode::result_callback(const GoalHandleArm::WrappedResult & result)
{
std::unique_lock lock(rw_mutex_);
int64_t command_id = result.result->command_id;
if (command_id == leftCommandId_) {
leftCommandId_ = 0;
leftArmGoalHandle_ = nullptr;
} else if (command_id == rightCommandId_) {
rightCommandId_ = 0;
rightArmGoalHandle_ = nullptr;
} else {
RCLCPP_ERROR(this->get_logger(), "Unknown command id in result: %ld", command_id);
return;
}
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
RCLCPP_INFO(this->get_logger(), "Goal was succeeded, command id: %ld", command_id);
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted, command id: %ld", command_id);
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "Goal was canceled, command id: %ld", command_id);
return;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code, command id: %ld", command_id);
return;
}
}
int directCommandMap[POSE_DIMENSION] = {0, 1, 2, 6, 7, 8, 9};
void ArmKeyboardNode::SendDirectSteppingGoal(const char *command)
{
std::unique_lock lock(rw_mutex_);
std::string commandStr = "command: ";
for (int i = 0; i < 16; ++i) {
if (command[i]) {
commandStr += std::to_string(i) + " ";
}
}
RCLCPP_INFO(this->get_logger(), "%s", commandStr.c_str());
if (client_ptr_ == nullptr) {
RCLCPP_ERROR(this->get_logger(), "Action client not initialized");
return;
}
if (!client_ptr_->action_server_is_ready()) {
RCLCPP_ERROR(this->get_logger(), "Action server not ready");
return;
}
auto goal_msg = ArmAction::Goal();
int commandValueBitMap = 0;
double speedadd = 0.01;
if (command[14]) {
speedadd = -0.01;
}
goal_msg.data_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制
goal_msg.data_length = POSE_DIMENSION;
goal_msg.command_id = commandId;
for (int i = 0; i < POSE_DIMENSION; ++i) {
if (command[directCommandMap[i]]) {
goal_msg.data_array.push_back(speedadd);
commandValueBitMap |= (1 << i);
} else {
goal_msg.data_array.push_back(0.0);
}
}
if (command[12] && leftDirectCommand_!= commandValueBitMap) {
commandId++;
goal_msg.body_id = LEFT_ARM;
leftDirectCommand_ = commandValueBitMap;
if (leftArmGoalHandle_ != nullptr && leftCommandId_ != 0) {
RCLCPP_INFO(this->get_logger(), "cancel left arm goal. command id: %ld", leftCommandId_);
client_ptr_->async_cancel_goal(leftArmGoalHandle_);
leftArmGoalHandle_.reset();
} else if (rightArmGoalHandle_ != nullptr && rightCommandId_ != 0) {
RCLCPP_INFO(this->get_logger(), "cancel right arm goal. command id: %ld", rightCommandId_);
client_ptr_->async_cancel_goal(rightArmGoalHandle_);
rightArmGoalHandle_.reset();
}
if (commandValueBitMap != 0) {
auto future = client_ptr_->async_send_goal(goal_msg, goal_options_);
leftCommandId_ = commandId;
leftArmGoalHandle_ = future.get();
RCLCPP_INFO(this->get_logger(), "send left arm goal, command id: %ld", commandId);
}
} else if (command[13] && rightDirectCommand_!= commandValueBitMap) {
commandId++;
goal_msg.body_id = RIGHT_ARM;
rightDirectCommand_ = commandValueBitMap;
if (rightArmGoalHandle_ != nullptr && rightCommandId_ != 0) {
RCLCPP_INFO(this->get_logger(), "cancel right arm goal. command id: %ld", rightCommandId_);
client_ptr_->async_cancel_goal(rightArmGoalHandle_);
rightArmGoalHandle_.reset();
} else if (leftArmGoalHandle_ != nullptr && leftCommandId_ != 0) {
RCLCPP_INFO(this->get_logger(), "cancel left arm goal. command id: %ld", leftCommandId_);
client_ptr_->async_cancel_goal(leftArmGoalHandle_);
leftArmGoalHandle_.reset();
}
if (commandValueBitMap != 0) {
auto future = client_ptr_->async_send_goal(goal_msg, goal_options_);
rightCommandId_ = commandId;
rightArmGoalHandle_ = future.get();
RCLCPP_INFO(this->get_logger(), "send right arm goal, command id: %ld", commandId);
}
} else {
RCLCPP_ERROR(this->get_logger(), "No arm selected");
return;
}
}
int actionChange[8] = {KEEP_ACTION, NEW_ACTION, KEEP_ACTION, NEW_ACTION,
STOP_ACTION, NEW_ACTION, NEW_ACTION, NEW_ACTION};
void ArmKeyboardNode::SendAngleSteppingGoal(const char *command) {
std::unique_lock lock(rw_mutex_);
std::string commandStr = "command: ";
for (int i = 0; i < 16; ++i) {
if (command[i]) {
commandStr += std::to_string(i) + " ";
}
}
RCLCPP_INFO(this->get_logger(), "%s", commandStr.c_str());
if (client_ptr_ == nullptr) {
RCLCPP_ERROR(this->get_logger(), "Action client not initialized");
return;
}
if (!client_ptr_->action_server_is_ready()) {
RCLCPP_ERROR(this->get_logger(), "Action server not ready");
return;
}
float speedadd = 0.02;
if (command[12]) {
speedadd = -0.02;
}
int rightArmMove = 0;
int leftArmMove = 0;
int rightArmStop = 0;
int leftArmStop = 0;
int rightArmNew = 0;
int leftArmNew = 0;
int rightArmKeep = 0;
int leftArmKeep = 0;
for (int i = 0; i < USED_ARM_DOF; ++i) {
if (command[i]) {
if (fabs(leftArmAngleAim_[i] + speedadd) < 0.1) {
leftArmAngleAim_[i] += speedadd;
leftArmNew = 1;
} else {
leftArmKeep = 1;
}
} else {
if (leftArmAngleAim_[i] != 0.0f) {
leftArmAngleAim_[i] = 0;
leftArmStop = 1;
}
}
if (command[i + USED_ARM_DOF]) {
if (fabs(rightArmAngleAim_[i] + speedadd) < 0.1) {
rightArmAngleAim_[i] += speedadd;
rightArmNew = 1;
} else {
rightArmKeep = 1;
}
} else {
if (rightArmAngleAim_[i] != 0.0f) {
rightArmAngleAim_[i] = 0;
rightArmStop = 1;
}
}
}
leftArmMove = actionChange[leftArmNew | (leftArmStop << 2) | (leftArmKeep << 1)];
rightArmMove = actionChange[rightArmNew | (rightArmStop << 2) | (rightArmKeep << 1)];
if (leftArmMove != KEEP_ACTION) {
if (leftArmGoalHandle_ != nullptr && leftCommandId_ != 0) {
RCLCPP_INFO(this->get_logger(), "cancel left arm goal. command id: %ld", leftCommandId_);
client_ptr_->async_cancel_goal(leftArmGoalHandle_);
leftArmGoalHandle_.reset();
}
if ((leftArmMove & NEW_ACTION) != 0) {
auto goal_msg = ArmAction::Goal();
commandId++;
leftCommandId_ = commandId;
goal_msg.body_id = LEFT_ARM;
goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_STEP_ON; // 位置控制
goal_msg.data_length = USED_ARM_DOF;
goal_msg.command_id = commandId;
for (int i = 0; i < USED_ARM_DOF; ++i) {
goal_msg.data_array.push_back(leftArmAngleAim_[i]);
}
auto future = client_ptr_->async_send_goal(goal_msg, goal_options_);
leftArmGoalHandle_ = future.get();
RCLCPP_INFO(this->get_logger(), "send left arm goal, command id: %ld", commandId);
}
}
if (rightArmMove != KEEP_ACTION) {
if (rightArmGoalHandle_ != nullptr && rightCommandId_ != 0) {
RCLCPP_INFO(this->get_logger(), "cancel right arm goal. command id: %ld", rightCommandId_);
client_ptr_->async_cancel_goal(rightArmGoalHandle_);
rightArmGoalHandle_.reset();
}
if ((rightArmMove & NEW_ACTION) != 0) {
auto goal_msg = ArmAction::Goal();
commandId++;
rightCommandId_ = commandId;
goal_msg.body_id = RIGHT_ARM;
goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_STEP_ON; // 位置控制
goal_msg.data_length = USED_ARM_DOF;
goal_msg.command_id = commandId;
for (int i = 0; i < USED_ARM_DOF; ++i) {
goal_msg.data_array.push_back(rightArmAngleAim_[i]);
}
auto future = client_ptr_->async_send_goal(goal_msg, goal_options_);
rightArmGoalHandle_ = future.get();
RCLCPP_INFO(this->get_logger(), "send right arm goal, command id: %ld", commandId);
}
}
}
int main(int argc, char *argv[])
{
/*
obb1: center(-0.1883, 0.0863, 0.6624), axis1(-0.7700, 0.1477, -0.6207), axis2(-0.6119, 0.1045, 0.7840), axis3(0.1806, 0.9835, 0.0099), half_lengths(0.0316, 0.0330, 0.0145)
obb2: center(0.0095, 0.0001, 0.4868), axis1(-1.0000, -0.0071, -0.0000), axis2(0.0071, -1.0000, -0.0000), axis3(-0.0000, -0.0000, 1.0000), half_lengths(0.1505, 0.1795, 0.3083)
*/
OBB obbA;
OBB obbB;
obbA.center = Vector3d(-0.1883, 0.0863, 0.6624);
obbA.axis[0] = Vector3d(-0.7700, 0.1477, -0.6207);
obbA.axis[1] = Vector3d(-0.6119, 0.1045, 0.7840);
obbA.axis[2] = Vector3d(0.1806, 0.9835, 0.0099);
obbA.halfExtent[0] = 0.0316;
obbA.halfExtent[1] = 0.0330;
obbA.halfExtent[2] = 0.0145;
obbB.center = Vector3d(0.0095, 0.0001, 0.4868);
obbB.axis[0] = Vector3d(-1.0000, -0.0071, -0.0000);
obbB.axis[1] = Vector3d(0.0071, -1.0000, -0.0000);
obbB.axis[2] = Vector3d(-0.0000, -0.0000, 1.0000);
obbB.halfExtent[0] = 0.1505;
obbB.halfExtent[1] = 0.1795;
obbB.halfExtent[2] = 0.3083;
AxisStats stats;
bool inCollisionOBB = gjk_interface::checkOBBCollisionNew(obbA, obbB, &stats);
if (inCollisionOBB) {
printf("dect obb A and B collision\n");
} else {
printf("dect obb A and B safe\n");
}
inCollisionOBB = gjk_interface::checkOBBCollision(obbA, obbB);
if (inCollisionOBB) {
printf("dect obb A and B collision\n");
} else {
printf("dect obb A and B safe\n");
}
rclcpp::init(argc, argv);
rclcpp::Node::SharedPtr node = std::make_shared<ArmKeyboardNode>();
std::shared_ptr<ArmKeyboardNode> armNode = std::dynamic_pointer_cast<ArmKeyboardNode>(node);
// 启动输入监听线程
inputThread_ = std::thread(ListenInputThread, armNode);
while (rclcpp::ok() && !armNode->exit_flag) {
rclcpp::spin_some(node); // 处理所有待处理的事件
std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 短暂休眠，降低CPU占用
}
rclcpp::shutdown();
return 0;
}

16
common_msg/CMakeLists.txt
View File

@@ -1,16 +0,0 @@
cmake_minimum_required(VERSION 3.8)
project(common_msg)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/ArmCommnd.msg"
)
ament_package()

202
common_msg/LICENSE
View File

@@ -1,202 +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 [yyyy] [name of copyright owner]
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.

5
common_msg/msg/ArmCommnd.msg
View File

@@ -1,5 +0,0 @@
int8 body_id
int8 data_type
int16 data_length
int32 command_id
float64[] data_array

17
common_msg/package.xml
View File

@@ -1,17 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>common_msg</name>
<version>0.0.0</version>
<description>common_msg</description>
<maintainer email="zj@todo.todo">zj</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

48
create_robot_describer/CMakeLists.txt
View File

@@ -1,48 +0,0 @@
cmake_minimum_required(VERSION 3.8)
project(create_robot_describer)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
add_compile_options(-g)
#set(CMAKE_BUILD_TYPE Release)
set(CGAL_DO_NOT_WARN_ABOUT_CMAKE_BUILD_TYPE TRUE)
set(CGAL_DATA_DIR ".")
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(gjk_interface REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(CGAL REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
include_directories(
include
${CMAKE_CURRENT_SOURCE_DIR}/../include
${EIGEN3_INCLUDE_DIRS} # 引入Eigen3的头文件路径
)
# 设置源文件
set(SOURCES
src/create_robot_describe.cpp
)
# 添加可执行文件
add_executable(create_robot_describer ${SOURCES})
ament_target_dependencies(create_robot_describer
gjk_interface
moveit_ros_planning_interface
)
target_link_libraries(create_robot_describer CGAL::CGAL)
install(TARGETS
create_robot_describer
DESTINATION lib/${PROJECT_NAME})
ament_package()

202
create_robot_describer/LICENSE
View File

@@ -1,202 +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 [yyyy] [name of copyright owner]
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.

65
create_robot_describer/include/create_robot_describe.hpp
View File

@@ -1,65 +0,0 @@
#ifndef CREATE_ROBOT_DESCRIBE_HPP
#define CREATE_ROBOT_DESCRIBE_HPP
#include <string>
#include <vector>
#include <memory>
#include <map>
#include <moveit/collision_detection_bullet/collision_detector_allocator_bullet.h>
#include "gjk_interface.hpp"
struct JointsModel {
std::string name; // 关节名称
int type; // 关节类型
int parent_link; // 父连杆
int child_link; // 子连杆
Eigen::Vector3d axis; // 关节轴
Eigen::Vector3d nextLinkOffset; // 下一个连杆关于关节原点偏移
Eigen::Quaterniond rotation; // 关节旋转
double limit_lower;
double limit_upper;
double limit_v;
double limit_e;
};
struct CollisionModel {
std::string link_name; // 所属链接名称
int geometry_type; // 几何类型
std::string mesh_filename; // mesh文件路径（如STL）
std::vector<OBB> obbModelList; // 网格边界盒 [min_x, max_x, min_y, max_y, min_z, max_z]
std::vector<int> jointIndexList; // 网格边界盒所属链接索引
std::vector<int> obbObbIndexList; // 网格边界盒所属链接索引
int joint_count;
int link_index;
int parent_link_index;
Eigen::Quaterniond rotation; // 链接旋转
Eigen::Vector3d offset; // 链接偏移
};
class RobotDescription
{
public:
RobotDescription(const std::string &urdf, const std::string &srdf);
~RobotDescription();
// 其他成员函数和变量
std::vector<CollisionModel> collision_structures_;
std::vector<JointsModel> jointMap_;
std::map<std::string, int> linkCollisionMap_;
int rootLinkIndex_;
int maxObbCnt;
bool *g_linkCollisionDetectMatrix;
private:
int InitJointModel();
int InitJoint(const urdf::Model &urdf_model);
int GetMeshLinkdimensions(const std::string& filename, CollisionModel& col_struct);
int InitCollisionStructureList(const urdf::Model &urdf_model);
void UpdatePolyhedrons();
int InitLinkCollisionDetectMatrix(const std::string& srdf_str);
};
#endif // CREATE_ROBOT_DESCRIBE_HPP

17
create_robot_describer/package.xml
View File

@@ -1,17 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>create_robot_describer</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="zhengjie@hivecore.cn">zj</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>gjk_interface</depend>
<exec_depend>moveit_ros_planning_interface</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

688
create_robot_describer/src/create_robot_describe.cpp
View File

@@ -1,688 +0,0 @@
#include <fstream>
#include <string>
#include <stdexcept> // 用于抛出异常
#include <filesystem>
#include <iostream>
#include <moveit/robot_model/robot_model.h>
#include <CGAL/optimal_bounding_box.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <create_robot_describe.hpp>
#define UNUSED(x) (void)(x)
typedef CGAL::Exact_predicates_inexact_constructions_kernel CgalK;
typedef CgalK::Point_3 CgalPoint;
namespace fs = std::filesystem;
using namespace tinyxml2;
using namespace Eigen;
int RobotDescription::InitJointModel()
{
int linkIndex = rootLinkIndex_;
CollisionModel *colStruct = &collision_structures_[linkIndex];
std::vector<int> jointList;
int index = 0;
int maxJointCount = 0;
Quaterniond rotationAll = Quaterniond::Identity();
while (colStruct->joint_count > 0 || index < maxJointCount) {
for (int i = 0; i < colStruct->joint_count; i++) {
jointList.push_back(colStruct->jointIndexList[i]);
maxJointCount++;
int jointIndex = colStruct->jointIndexList[i];
JointsModel *jointInfo = &jointMap_[jointIndex];
printf("链接 %s 包含关节 %s, 原offset: (%.4f, %.4f, %.4f)\n", colStruct->link_name.c_str(), jointInfo->name.c_str(),
jointInfo->nextLinkOffset.x(),
jointInfo->nextLinkOffset.y(),
jointInfo->nextLinkOffset.z());
jointInfo->rotation = rotationAll * jointInfo->rotation;
jointInfo->axis = jointInfo->rotation * jointInfo->axis;
jointInfo->nextLinkOffset = rotationAll * jointInfo->nextLinkOffset;
printf("更新后关节 %s 旋转四元数: (%.4f, %.4f, %.4f, %.4f), 轴向: (%.4f, %.4f, %.4f), 偏移: (%.4f, %.4f, %.4f)\n",
jointInfo->name.c_str(),
jointInfo->rotation.w(),
jointInfo->rotation.x(),
jointInfo->rotation.y(),
jointInfo->rotation.z(),
jointInfo->axis.x(),
jointInfo->axis.y(),
jointInfo->axis.z(),
jointInfo->nextLinkOffset.x(),
jointInfo->nextLinkOffset.y(),
jointInfo->nextLinkOffset.z());
}
int jointIndex = jointList[index];
JointsModel *jointInfo = &jointMap_[jointIndex];
rotationAll = jointInfo->rotation;
index++;
linkIndex = jointInfo->child_link;
colStruct = &collision_structures_[linkIndex];
for (size_t i = 0; i < colStruct->obbModelList.size(); i++) {
colStruct->obbModelList[i] = gjk_interface::updateOBBFromPoints(colStruct->obbModelList[i],
Vector3d::Zero(),
rotationAll);
}
}
return 0;
}
int RobotDescription::InitJoint(const urdf::Model &urdf_model)
{
int joint_index = 0;
for (const auto& joint_pair : urdf_model.joints_) {
urdf::JointConstSharedPtr joint = joint_pair.second;
JointsModel joint_info;
joint_info.name = joint->name;
joint_info.type = joint->type;
joint_info.parent_link = -1;
joint_info.child_link = -1;
if (joint->parent_link_name != "") {
if (linkCollisionMap_.find(joint->parent_link_name) != linkCollisionMap_.end()) {
joint_info.parent_link = linkCollisionMap_[joint->parent_link_name];
} else {
printf("Joint %s 的父链接 %s 未找到！", joint->name.c_str(), joint->parent_link_name.c_str());
return -1;
}
}
if (joint->child_link_name != "") {
if (linkCollisionMap_.find(joint->child_link_name) != linkCollisionMap_.end()) {
joint_info.child_link = linkCollisionMap_[joint->child_link_name];
} else {
printf("Joint %s 的子链接 %s 未找到！", joint->name.c_str(), joint->child_link_name.c_str());
return -1;
}
}
joint_info.nextLinkOffset = Vector3d(joint->parent_to_joint_origin_transform.position.x,
joint->parent_to_joint_origin_transform.position.y,
joint->parent_to_joint_origin_transform.position.z);
joint_info.rotation = Quaterniond(
joint->parent_to_joint_origin_transform.rotation.w,
joint->parent_to_joint_origin_transform.rotation.x,
joint->parent_to_joint_origin_transform.rotation.y,
joint->parent_to_joint_origin_transform.rotation.z);
if (joint->type != urdf::Joint::FIXED) {
joint_info.axis = Vector3d(joint->axis.x, joint->axis.y, joint->axis.z);
if (joint->limits != NULL) {
joint_info.limit_lower = joint->limits->lower;
joint_info.limit_upper = joint->limits->upper;
joint_info.limit_v = joint->limits->velocity;
joint_info.limit_e = joint->limits->effort;
}
} else {
joint_info.axis = Vector3d::Zero();
joint_info.limit_lower = 0.0;
joint_info.limit_upper = 0.0;
joint_info.limit_v = 0.0;
joint_info.limit_e = 0.0;
}
jointMap_.push_back(joint_info);
CollisionModel *parentColStruct = &collision_structures_[joint_info.parent_link];
parentColStruct->jointIndexList.push_back(joint_index);
parentColStruct->joint_count++;
CollisionModel *childColStruct = &collision_structures_[joint_info.child_link];
childColStruct->parent_link_index = joint_info.parent_link;
joint_index++;
}
InitJointModel();
return 0;
}
static OBB GetMeshesBounds(const std::string& filename)
{
OBB obb;
shapes::Mesh* mesh = shapes::createMeshFromResource(filename);
if (!mesh) {
printf( "无法加载网格文件: %s", filename.c_str());
return obb;
}
// 计算边界盒
double min_x = mesh->vertices[0], max_x = mesh->vertices[0];
double min_y = mesh->vertices[1], max_y = mesh->vertices[1];
double min_z = mesh->vertices[2], max_z = mesh->vertices[2];
for (unsigned int i = 1; i < mesh->vertex_count; ++i) {
if (min_x > mesh->vertices[3*i]) {
min_x = mesh->vertices[3*i];
}
if (min_y > mesh->vertices[3*i + 1]) {
min_y = mesh->vertices[3*i + 1];
}
if (min_z > mesh->vertices[3*i + 2]) {
min_z = mesh->vertices[3*i + 2];
}
if (max_x < mesh->vertices[3*i]) {
max_x = mesh->vertices[3*i];
}
if (max_y < mesh->vertices[3*i + 1]) {
max_y = mesh->vertices[3*i + 1];
}
if (max_z < mesh->vertices[3*i + 2]) {
max_z = mesh->vertices[3*i + 2];
}
}
std::vector<Vector3d> points = {
Vector3d(min_x, min_y, min_z),
Vector3d(max_x, min_y, min_z),
Vector3d(min_x, max_y, min_z),
Vector3d(max_x, max_y, min_z),
Vector3d(min_x, min_y, max_z),
Vector3d(max_x, min_y, max_z),
Vector3d(min_x, max_y, max_z),
Vector3d(max_x, max_y, max_z)
};
obb = gjk_interface::createOBBFromPoints(points);
// 释放网格资源
delete mesh;
return obb;
}
int RobotDescription::GetMeshLinkdimensions(const std::string& filename, CollisionModel& col_struct)
{
shapes::Mesh* mesh = shapes::createMeshFromResource(filename);
if (!mesh) {
printf("无法加载网格文件: %s\n", filename.c_str());
return false;
}
std::vector<Eigen::Vector3d> eigen_points;
std::vector<CgalPoint> points;
for (unsigned int i = 0; i < mesh->vertex_count; ++i) {
points.emplace_back(mesh->vertices[3*i], mesh->vertices[3*i + 1], mesh->vertices[3*i + 2]);
}
std::array<CgalPoint, 8> obb_points;
CGAL::oriented_bounding_box(points, obb_points, CGAL::parameters::use_convex_hull(true));
printf("计算%s得到的 OBB 顶点:\n", col_struct.link_name.c_str());
for (const auto& pt : obb_points) {
eigen_points.push_back(Eigen::Vector3d(pt.x(), pt.y(), pt.z()));
printf("(%.4f, %.4f, %.4f)\n", pt.x(), pt.y(), pt.z());
}
/*UNUSED(filename);
static std::map<std::string, std::array<Vector3d, 8>> pointMap = {
{"LeftLink1",
{Vector3d(0.0411, 0.0456, -0.1191), Vector3d(-0.0409, 0.0458, -0.1191), Vector3d(-0.0411, -0.0444, -0.1013), Vector3d(0.0409, -0.0446, -0.1013), Vector3d(0.0410, -0.0137, 0.0545), Vector3d(0.0412, 0.0765, 0.0366), Vector3d(-0.0408, 0.0767, 0.0366), Vector3d(-0.0410, -0.0135, 0.0545)}},
{"LeftLink2",
{Vector3d(0.2879, 0.0332, -0.0733), Vector3d(-0.0475, 0.0417, -0.0150), Vector3d(-0.0496, -0.0393, -0.0149), Vector3d(0.2858, -0.0478, -0.0732), Vector3d(0.3004, -0.0481, 0.0110), Vector3d(0.3025, 0.0329, 0.0109), Vector3d(-0.0329, 0.0414, 0.0691), Vector3d(-0.0349, -0.0396, 0.0692)}},
{"LeftLink3",
{Vector3d(0.0464, 0.0342, 0.0241), Vector3d(-0.0012, 0.0428, -0.0180), Vector3d(-0.0025, -0.1243, -0.0505), Vector3d(0.0451, -0.1329, -0.0084), Vector3d(0.0019, -0.1416, 0.0386), Vector3d(0.0032, 0.0254, 0.0710), Vector3d(-0.0443, 0.0340, 0.0289), Vector3d(-0.0456, -0.1331, -0.0036)}},
{"LeftLink4",
{Vector3d(0.0286, 0.0314, -0.0811), Vector3d(-0.0294, 0.0307, -0.0811), Vector3d(-0.0285, -0.0395, -0.0933), Vector3d(0.0295, -0.0388, -0.0933), Vector3d(0.0298, -0.0592, 0.0251), Vector3d(0.0289, 0.0110, 0.0372), Vector3d(-0.0291, 0.0103, 0.0372), Vector3d(-0.0282, -0.0600, 0.0251)}},
{"LeftLink5",
{Vector3d(0.0290, 0.0328, -0.0184), Vector3d(-0.0282, 0.0330, -0.0190), Vector3d(-0.0284, -0.1144, -0.0341), Vector3d(0.0288, -0.1145, -0.0335), Vector3d(0.0282, -0.1208, 0.0281), Vector3d(0.0284, 0.0265, 0.0432), Vector3d(-0.0288, 0.0266, 0.0427), Vector3d(-0.0290, -0.1207, 0.0275)}},
{"LeftLink6",
{Vector3d(0.0492, 0.0104, -0.0290), Vector3d(-0.0023, 0.0469, -0.0290), Vector3d(-0.0404, -0.0069, -0.0290), Vector3d(0.0111, -0.0434, -0.0290), Vector3d(0.0111, -0.0434, 0.0000), Vector3d(0.0492, 0.0104, 0.0000), Vector3d(-0.0023, 0.0469, 0.0000), Vector3d(-0.0404, -0.0069, 0.0000)}},
{"RightLink1",
{Vector3d(0.0411, 0.0456, -0.1191), Vector3d(-0.0409, 0.0458, -0.1191), Vector3d(-0.0411, -0.0444, -0.1013), Vector3d(0.0409, -0.0446, -0.1013), Vector3d(0.0410, -0.0137, 0.0545), Vector3d(0.0412, 0.0765, 0.0366), Vector3d(-0.0408, 0.0767, 0.0366), Vector3d(-0.0410, -0.0135, 0.0545)}},
{"RightLink2",
{Vector3d(0.3025, 0.0329, 0.0109), Vector3d(0.2879, 0.0332, -0.0733), Vector3d(0.2858, -0.0478, -0.0732), Vector3d(0.3004, -0.0481, 0.0110), Vector3d(-0.0349, -0.0396, 0.0692), Vector3d(-0.0329, 0.0414, 0.0691), Vector3d(-0.0475, 0.0417, -0.0150), Vector3d(-0.0496, -0.0393, -0.0149)}},
{"RightLink3",
{Vector3d(0.0464, 0.0342, 0.0241), Vector3d(-0.0012, 0.0428, -0.0180), Vector3d(-0.0025, -0.1243, -0.0505), Vector3d(0.0451, -0.1329, -0.0084), Vector3d(0.0019, -0.1416, 0.0386), Vector3d(0.0032, 0.0254, 0.0710), Vector3d(-0.0443, 0.0340, 0.0289), Vector3d(-0.0456, -0.1331, -0.0036)}},
{"RightLink4",
{Vector3d(0.0295, -0.0388, -0.0933), Vector3d(-0.0285, -0.0395, -0.0933), Vector3d(-0.0282, -0.0600, 0.0251), Vector3d(0.0298, -0.0592, 0.0251), Vector3d(0.0289, 0.0110, 0.0372), Vector3d(0.0286, 0.0314, -0.0811), Vector3d(-0.0294, 0.0307, -0.0811), Vector3d(-0.0291, 0.0103, 0.0372)}},
{"RightLink5",
{Vector3d(0.0288, -0.1145, -0.0335), Vector3d(-0.0284, -0.1144, -0.0341), Vector3d(-0.0290, -0.1207, 0.0275), Vector3d(0.0282, -0.1208, 0.0281), Vector3d(0.0284, 0.0265, 0.0432), Vector3d(0.0290, 0.0328, -0.0184), Vector3d(-0.0282, 0.0330, -0.0190), Vector3d(-0.0288, 0.0266, 0.0427)}},
{"RightLink6",
{Vector3d(0.0492, 0.0104, -0.0290), Vector3d(-0.0023, 0.0469, -0.0290), Vector3d(-0.0404, -0.0069, -0.0290), Vector3d(0.0111, -0.0434, -0.0290), Vector3d(0.0111, -0.0434, 0.0000), Vector3d(0.0492, 0.0104, 0.0000), Vector3d(-0.0023, 0.0469, 0.0000), Vector3d(-0.0404, -0.0069, 0.0000)}},
{"base_link",
{Vector3d(0.2085, 0.1895, -0.0856), Vector3d(-0.1905, 0.1895, -0.0880), Vector3d(-0.1905, -0.1895, -0.0880), Vector3d(0.2085, -0.1895, -0.0856), Vector3d(0.2071, -0.1895, 0.1679), Vector3d(0.2071, 0.1895, 0.1679), Vector3d(-0.1920, 0.1895, 0.1656), Vector3d(-0.1920, -0.1895, 0.1656)}},
{"base_link_leftarm",
{Vector3d(0.0661, -0.0353, 0.1385), Vector3d(0.0661, -0.0353, -0.0000), Vector3d(-0.0339, -0.0699, -0.0000), Vector3d(-0.0339, -0.0699, 0.1385), Vector3d(-0.0698, 0.0339, 0.1385), Vector3d(0.0302, 0.0685, 0.1385), Vector3d(0.0302, 0.0685, -0.0000), Vector3d(-0.0698, 0.0339, -0.0000)}},
{"base_link_rightarm",
{Vector3d(0.0661, -0.0353, -0.0000), Vector3d(0.0302, 0.0685, -0.0000), Vector3d(-0.0698, 0.0339, -0.0000), Vector3d(-0.0339, -0.0699, -0.0000), Vector3d(-0.0339, -0.0699, 0.1385), Vector3d(0.0661, -0.0353, 0.1385), Vector3d(0.0302, 0.0685, 0.1385), Vector3d(-0.0698, 0.0339, 0.1385)}},
{"body_2_link",
{Vector3d(0.1595, 0.0756, -0.0750), Vector3d(-0.1415, 0.0735, -0.0750), Vector3d(-0.1390, -0.2855, -0.0750), Vector3d(0.1620, -0.2834, -0.0750), Vector3d(0.1620, -0.2834, 0.5416), Vector3d(0.1595, 0.0756, 0.5416), Vector3d(-0.1415, 0.0735, 0.5416), Vector3d(-0.1390, -0.2855, 0.5416)}},
{"body_link",
{Vector3d(0.0593, 0.1034, -0.0010), Vector3d(-0.0842, 0.1024, -0.0010), Vector3d(-0.0828, -0.1036, -0.0010), Vector3d(0.0607, -0.1026, -0.0010), Vector3d(0.0607, -0.1026, 0.1600), Vector3d(0.0593, 0.1034, 0.1600), Vector3d(-0.0842, 0.1024, 0.1600), Vector3d(-0.0828, -0.1036, 0.1600)}},
{"head_link",
{Vector3d(0.0672, 0.1175, -0.0000), Vector3d(-0.0658, 0.1165, 0.0000), Vector3d(-0.0642, -0.1175, 0.0000), Vector3d(0.0689, -0.1165, -0.0000), Vector3d(0.0689, -0.1165, 0.2031), Vector3d(0.0672, 0.1175, 0.2031), Vector3d(-0.0658, 0.1165, 0.2031), Vector3d(-0.0642, -0.1175, 0.2031)}},
{"left_behind_hip_link",
{Vector3d(0.0626, 0.0721, 0.0288), Vector3d(-0.1058, 0.0721, -0.4340), Vector3d(-0.1058, 0.0000, -0.4340), Vector3d(0.0626, 0.0000, 0.0288), Vector3d(-0.0295, 0.0000, 0.0623), Vector3d(-0.0295, 0.0721, 0.0623), Vector3d(-0.1979, 0.0721, -0.4005), Vector3d(-0.1979, 0.0000, -0.4005)}},
{"left_behind_leg_link",
{Vector3d(0.0648, -0.0402, -0.0206), Vector3d(0.0648, 0.0235, -0.0206), Vector3d(-0.0927, 0.0235, -0.4532), Vector3d(-0.0927, -0.0402, -0.4532), Vector3d(-0.2204, -0.0402, -0.4068), Vector3d(-0.0629, -0.0402, 0.0259), Vector3d(-0.0629, 0.0235, 0.0259), Vector3d(-0.2204, 0.0235, -0.4068)}},
{"left_behind_wheel_link",
{Vector3d(0.0852, 0.0370, -0.0945), Vector3d(-0.0944, 0.0370, -0.0853), Vector3d(-0.0944, -0.0125, -0.0853), Vector3d(0.0852, -0.0125, -0.0945), Vector3d(0.0944, -0.0125, 0.0853), Vector3d(0.0944, 0.0370, 0.0853), Vector3d(-0.0852, 0.0370, 0.0945), Vector3d(-0.0852, -0.0125, 0.0945)}},
{"left_front_hip_link",
{Vector3d(0.1987, 0.1315, -0.3903), Vector3d(0.1020, 0.1315, -0.4259), Vector3d(0.1020, 0.0190, -0.4259), Vector3d(0.1987, 0.0190, -0.3903), Vector3d(0.0305, 0.0190, 0.0661), Vector3d(0.0305, 0.1315, 0.0661), Vector3d(-0.0661, 0.1315, 0.0305), Vector3d(-0.0661, 0.0190, 0.0305)}},
{"left_front_knee_link",
{Vector3d(0.2234, 0.0230, -0.3181), Vector3d(0.1136, 0.0230, -0.3665), Vector3d(0.1136, -0.0620, -0.3665), Vector3d(0.2234, -0.0620, -0.3181), Vector3d(0.0518, -0.0620, 0.0708), Vector3d(0.0518, 0.0230, 0.0708), Vector3d(-0.0580, 0.0230, 0.0224), Vector3d(-0.0580, -0.0620, 0.0224)}},
{"left_front_roll_link",
{Vector3d(0.0960, 0.0565, 0.0000), Vector3d(-0.0060, 0.0565, -0.1020), Vector3d(-0.0060, -0.0285, -0.1020), Vector3d(0.0960, -0.0285, 0.0000), Vector3d(-0.0060, -0.0285, 0.1020), Vector3d(-0.0060, 0.0565, 0.1020), Vector3d(-0.1080, 0.0565, -0.0000), Vector3d(-0.1080, -0.0285, -0.0000)}},
{"left_front_wheel_link",
{Vector3d(0.1223, 0.0000, -0.0336), Vector3d(-0.0336, 0.0000, -0.1223), Vector3d(-0.0336, -0.0570, -0.1223), Vector3d(0.1223, -0.0570, -0.0336), Vector3d(0.0336, -0.0570, 0.1223), Vector3d(0.0336, 0.0000, 0.1223), Vector3d(-0.1223, 0.0000, 0.0336), Vector3d(-0.1223, -0.0570, 0.0336)}},
{"right_behind_hip_link",
{Vector3d(-0.1058, -0.0000, -0.4340), Vector3d(-0.1979, -0.0000, -0.4005), Vector3d(-0.1979, -0.0721, -0.4005), Vector3d(-0.1058, -0.0721, -0.4340), Vector3d(0.0626, -0.0721, 0.0288), Vector3d(0.0626, -0.0000, 0.0288), Vector3d(-0.0295, -0.0000, 0.0623), Vector3d(-0.0295, -0.0721, 0.0623)}},
{"right_behind_leg_link",
{Vector3d(0.0648, 0.0402, -0.0206), Vector3d(-0.0927, 0.0402, -0.4532), Vector3d(-0.0927, -0.0235, -0.4532), Vector3d(0.0648, -0.0235, -0.0206), Vector3d(-0.0629, -0.0235, 0.0259), Vector3d(-0.0629, 0.0402, 0.0259), Vector3d(-0.2204, 0.0402, -0.4068), Vector3d(-0.2204, -0.0235, -0.4068)}},
{"right_behind_wheel_link",
{Vector3d(0.1259, 0.0125, -0.0183), Vector3d(-0.0182, 0.0125, -0.1259), Vector3d(-0.0182, -0.0370, -0.1259), Vector3d(0.1259, -0.0370, -0.0183), Vector3d(0.0182, -0.0370, 0.1259), Vector3d(0.0182, 0.0125, 0.1259), Vector3d(-0.1259, 0.0125, 0.0183), Vector3d(-0.1259, -0.0370, 0.0183)}},
{"right_front_hip_link",
{Vector3d(0.1956, -0.0190, -0.3918), Vector3d(0.0987, -0.0190, -0.4267), Vector3d(0.0987, -0.1320, -0.4267), Vector3d(0.1956, -0.1320, -0.3918), Vector3d(0.0310, -0.1320, 0.0659), Vector3d(0.0310, -0.0190, 0.0659), Vector3d(-0.0659, -0.0190, 0.0310), Vector3d(-0.0659, -0.1320, 0.0310)}},
{"right_front_knee_link",
{Vector3d(0.2234, 0.0620, -0.3181), Vector3d(0.1136, 0.0620, -0.3665), Vector3d(0.1136, -0.0230, -0.3665), Vector3d(0.2234, -0.0230, -0.3181), Vector3d(0.0518, -0.0230, 0.0708), Vector3d(0.0518, 0.0620, 0.0708), Vector3d(-0.0580, 0.0620, 0.0224), Vector3d(-0.0580, -0.0230, 0.0224)}},
{"right_front_roll_link",
{Vector3d(0.0960, 0.0285, 0.0000), Vector3d(-0.0060, 0.0285, -0.1020), Vector3d(-0.0060, -0.0565, -0.1020), Vector3d(0.0960, -0.0565, 0.0000), Vector3d(-0.0060, -0.0565, 0.1020), Vector3d(-0.0060, 0.0285, 0.1020), Vector3d(-0.1080, 0.0285, -0.0000), Vector3d(-0.1080, -0.0565, -0.0000)}},
{"right_front_wheel_link",
{Vector3d(0.1262, 0.0528, -0.0125), Vector3d(-0.0125, 0.0528, -0.1262), Vector3d(-0.0125, -0.0042, -0.1262), Vector3d(0.1262, -0.0042, -0.0125), Vector3d(0.0125, -0.0042, 0.1262), Vector3d(0.0125, 0.0528, 0.1262), Vector3d(-0.1262, 0.0528, 0.0125), Vector3d(-0.1262, -0.0042, 0.0125)}},
};
std::array<Vector3d, 8> eigen_points_arr = pointMap.at(col_struct.link_name);
std::vector<Eigen::Vector3d> eigen_points(eigen_points_arr.begin(), eigen_points_arr.end());*/
OBB boundsObb = GetMeshesBounds(filename);
OBB obb = gjk_interface::createOBBFromPoints(eigen_points);
if (boundsObb.halfExtent[0] > 0) {
double v1 = boundsObb.halfExtent[0] * boundsObb.halfExtent[1] * boundsObb.halfExtent[2];
double v2 = obb.halfExtent[0] * obb.halfExtent[1] * obb.halfExtent[2];
printf("网格文件: %s 计算得到的 边界 体积: %.6f, 计算 OBB 体积: %.6f\n", filename.c_str(), v1 * 8, v2 * 8);
}
col_struct.obbModelList.push_back(obb);
return 0;
}
int RobotDescription::InitCollisionStructureList(const urdf::Model &urdf_model)
{
int result = 0;
int linkIndex = 0;
maxObbCnt = 0;
for (const auto& link_pair : urdf_model.links_) {
// link_pair是一个键值对: 键为链接名称，值为链接指针
urdf::LinkConstSharedPtr link = link_pair.second;
for (const auto& collision : link->collision_array) {
CollisionModel colStruct;
colStruct.link_name = link->name;
colStruct.geometry_type = collision->geometry->type;
linkCollisionMap_[link->name] = linkIndex;
int obbCnt = 1;
if (collision->geometry->type == urdf::Geometry::BOX) {
urdf::Box* box = dynamic_cast<urdf::Box*>(collision->geometry.get());
std::vector<Eigen::Vector3d> obb_points = {
Eigen::Vector3d(-box->dim.x / 2, -box->dim.y / 2, -box->dim.z / 2),
Eigen::Vector3d(box->dim.x / 2, -box->dim.y / 2, -box->dim.z / 2),
Eigen::Vector3d(-box->dim.x / 2, box->dim.y / 2, -box->dim.z / 2),
Eigen::Vector3d(box->dim.x / 2, box->dim.y / 2, -box->dim.z / 2),
Eigen::Vector3d(-box->dim.x / 2, -box->dim.y / 2, box->dim.z / 2),
Eigen::Vector3d(box->dim.x / 2, -box->dim.y / 2, box->dim.z / 2),
Eigen::Vector3d(-box->dim.x / 2, box->dim.y / 2, box->dim.z / 2),
Eigen::Vector3d(box->dim.x / 2, box->dim.y / 2, box->dim.z / 2)
};
OBB obb = gjk_interface::createOBBFromPoints(obb_points);
colStruct.obbModelList.push_back(obb);
} else if (collision->geometry->type == urdf::Geometry::MESH) {
urdf::Mesh* mesh = dynamic_cast<urdf::Mesh*>(collision->geometry.get());
colStruct.mesh_filename = mesh->filename;
result = GetMeshLinkdimensions(mesh->filename, colStruct);
if (result != 0) {
printf("Failed to get mesh dimensions for %s\n", mesh->filename.c_str());
return result;
}
}
for (int i = 0; i < obbCnt; i++) {
colStruct.obbObbIndexList.push_back(maxObbCnt);
maxObbCnt++;
}
colStruct.link_index = linkIndex;
colStruct.joint_count = 0;
if (link->name == "base_link") {
rootLinkIndex_ = linkIndex;
colStruct.parent_link_index = -1;
colStruct.rotation = Quaterniond::Identity();
colStruct.offset = Vector3d::Zero();
printf("设置根链接: %s, 索引: %d 四元数: (%.4f, %.4f, %.4f, %.4f)\n", colStruct.link_name.c_str(), linkIndex,
colStruct.rotation.x(), colStruct.rotation.y(), colStruct.rotation.z(), colStruct.rotation.w());
}
collision_structures_.push_back(colStruct);
linkIndex++;
}
}
return result;
}
void RobotDescription::UpdatePolyhedrons()
{
int linkIndex = rootLinkIndex_;
CollisionModel *colStruct = &collision_structures_[linkIndex];
std::vector<int> jointList;
int index = 0;
int maxJointCount = 0;
while (colStruct->joint_count > 0 || index < maxJointCount) {
for (int i = 0; i < colStruct->joint_count; i++) {
jointList.push_back(colStruct->jointIndexList[i]);
maxJointCount++;
}
int jointIndex = jointList[index];
JointsModel *jointInfo = &jointMap_[jointIndex];
Quaterniond rotation = collision_structures_[jointInfo->parent_link].rotation;
Vector3d offset = collision_structures_[jointInfo->parent_link].offset;
index++;
linkIndex = jointInfo->child_link;
colStruct = &collision_structures_[linkIndex];
offset = offset + rotation * jointInfo->nextLinkOffset;
colStruct->rotation = rotation;
colStruct->offset = offset;
for (size_t i = 0; i < colStruct->obbModelList.size(); i++) {
OBB obb = gjk_interface::updateOBBFromPoints(colStruct->obbModelList[i],
offset,
rotation);
printf("更新链接 %s 的 OBB:\n", colStruct->link_name.c_str());
printf("中心点: (%.4f, %.4f, %.4f)\n", obb.center(0), obb.center(1), obb.center(2));
/*printf("轴向:\n");
for (int j = 0; j < 3; j++) {
printf(" (%.4f, %.4f, %.4f)\n", obb.axis[j].x(), obb.axis[j].y(), obb.axis[j].z());
}
printf("半尺寸: (%.4f, %.4f, %.4f)\n", obb.halfExtent[0], obb.halfExtent[1], obb.halfExtent[2]);*/
}
}
}
int RobotDescription::InitLinkCollisionDetectMatrix(const std::string& srdf_str)
{
g_linkCollisionDetectMatrix = new bool[maxObbCnt * maxObbCnt];
for (int i = 0; i < maxObbCnt; ++i) {
for (int j = i + 1; j < maxObbCnt; ++j) {
g_linkCollisionDetectMatrix[i * maxObbCnt + j] = true;
}
}
tinyxml2::XMLDocument doc;
tinyxml2::XMLError error = doc.Parse(srdf_str.c_str());
if (error != tinyxml2::XML_SUCCESS) {
printf("Failed to parse SRDF: %s, length: %ld\n", doc.ErrorStr(), srdf_str.length());
return -1;
}
// 获取根节点<robot>
tinyxml2::XMLElement* robot_root = doc.FirstChildElement("robot");
if (!robot_root) {
printf("SRDF has no <robot> root node\n");
return -1;
}
int disable_count = 0;
for (tinyxml2::XMLElement* dc_elem = robot_root->FirstChildElement("disable_collisions");
dc_elem;
dc_elem = dc_elem->NextSiblingElement("disable_collisions")) {
std::string link1 = dc_elem->Attribute("link1");
std::string link2 = dc_elem->Attribute("link2");
int link1Index = linkCollisionMap_.find(link1) != linkCollisionMap_.end() ? linkCollisionMap_[link1] : -1;
int link2Index = linkCollisionMap_.find(link2) != linkCollisionMap_.end() ? linkCollisionMap_[link2] : -1;
if (link1Index != -1 && link2Index != -1) {
CollisionModel* colStruct1 = &collision_structures_[link1Index];
CollisionModel* colStruct2 = &collision_structures_[link2Index];
for (int obb1Idx : colStruct1->obbObbIndexList) {
for (int obb2Idx : colStruct2->obbObbIndexList) {
g_linkCollisionDetectMatrix[obb1Idx * maxObbCnt + obb2Idx] = false;
g_linkCollisionDetectMatrix[obb2Idx * maxObbCnt + obb1Idx] = false;
}
}
}
disable_count++;
}
printf("Initialized link collision detect matrix, disabled %d collision pairs. total %d\n", disable_count * 2, maxObbCnt * maxObbCnt);
/*for (int i = 0; i < maxObbCnt; ++i) {
for (int j = 0; j < maxObbCnt; ++j) {
if (g_linkCollisionDetectMatrix[i * maxObbCnt + j]) {
printf("true ");
} else {
printf("false ");
}
}
printf("\n");
}*/
return 0;
}
RobotDescription::RobotDescription(const std::string &urdf, const std::string &srdf)
{
urdf::Model urdf_model;
if (!urdf_model.initString(urdf)) {
printf("无法解析URDF模型\n");
return;
}
InitCollisionStructureList(urdf_model);
InitJoint(urdf_model);
UpdatePolyhedrons();
InitLinkCollisionDetectMatrix(srdf);
}
RobotDescription::~RobotDescription()
{
// 清理资源（如果有的话）
delete g_linkCollisionDetectMatrix;
}
/**
* @brief 以 std::string 形式读取文件全部内容
* @param file_path 文件绝对路径或相对路径
* @return 文件内容的 std::string
* @throws 若文件不存在、无法打开或读取失败，抛出 std::runtime_error
*/
std::string readFileToString(const std::string& file_path) {
// 1. 打开文件（二进制模式避免换行符转换，确保内容完整）
std::ifstream file(file_path, std::ios::in | std::ios::binary);
if (!file.is_open()) {
throw std::runtime_error("无法打开文件：" + file_path + "（可能路径错误或权限不足）\n");
}
// 2. 读取文件内容到 string
std::string content;
try {
// 定位到文件末尾，获取文件大小
file.seekg(0, std::ios::end);
// 预留内存（避免多次扩容，提升效率）
content.reserve(static_cast<size_t>(file.tellg()));
// 回到文件开头
file.seekg(0, std::ios::beg);
// 用迭代器读取全部内容（高效且简洁）
content.assign(std::istreambuf_iterator<char>(file), std::istreambuf_iterator<char>());
} catch (const std::exception& e) {
file.close(); // 异常时确保文件关闭
throw std::runtime_error("读取文件失败：" + file_path + "（错误信息：" + e.what() + "）\n");
}
// 3. 关闭文件（ifstream 析构时会自动关闭，但显式关闭更严谨）
file.close();
// 4. 可选：检查内容是否为空（根据需求决定是否抛出异常）
if (content.empty()) {
std::cerr << "[警告] 文件内容为空：" << file_path << std::endl;
// 若需要严格检查，可抛出异常：
// throw std::runtime_error("文件内容为空：" + file_path);
}
return content;
}
void searchRecursive(const fs::path& dir, const std::string& target, std::string& result, std::string& result_dir) {
for (const auto& entry : fs::directory_iterator(dir)) {
if (entry.is_directory()) {
searchRecursive(entry.path(), target, result, result_dir); // 递归子目录
if (!result.empty()) return; // 找到后提前返回
} else if (entry.is_regular_file() && entry.path().filename() == target) {
result = entry.path().string();
result_dir = entry.path().parent_path().string();
return;
}
}
}
std::string findFileInCurrentDir(const std::string& target_filename, std::string& result_dir) {
// 获取当前工作目录
std::string current_dir;
try {
current_dir = fs::current_path().string();
} catch (const fs::filesystem_error& e) {
throw std::runtime_error("无法获取当前目录：" + std::string(e.what()));
}
// 遍历当前目录下的所有文件（不递归子目录）
try {
std::string found_path;
searchRecursive(current_dir, target_filename, found_path, result_dir);
if (!found_path.empty()) {
return found_path; // 找到文件，返回路径
}
} catch (const fs::filesystem_error& e) {
throw std::runtime_error("遍历目录失败：" + std::string(e.what()));
}
// 未找到文件
return "";
}
std::string GetWriteFileName(const std::string& dir, const std::string& filename) {
try {
// 1. 检查并创建目录
if (!fs::exists(dir)) {
if (!fs::create_directories(dir)) {
throw std::runtime_error("无法创建目录：" + dir);
}
std::cout << "已创建目录：" << dir << std::endl;
}
// 2. 拼接完整文件路径
fs::path file_path = fs::path(dir) / filename;
std::string full_path = file_path.string();
return full_path;
} catch (const fs::filesystem_error& e) {
throw std::runtime_error("文件系统错误：" + std::string(e.what()));
} catch (const std::exception& e) {
throw std::runtime_error("操作失败：" + std::string(e.what()));
}
return "";
}
void WriteXmlFile(const std::string& file_path, RobotDescription& robot_description) {
XMLDocument doc;
// 添加XML声明（可选，但推荐）
XMLDeclaration* decl = doc.NewDeclaration();
doc.InsertFirstChild(decl);
// 创建根节点 <robot>
XMLElement* root = doc.NewElement("robot");
root->SetAttribute("name", "FHrobot");
root->SetAttribute("rootLinkIndex_", robot_description.rootLinkIndex_);
root->SetAttribute("maxObbCnt", robot_description.maxObbCnt);
root->SetAttribute("collision_structure_count", static_cast<int>(robot_description.collision_structures_.size()));
root->SetAttribute("joint_count", static_cast<int>(robot_description.jointMap_.size()));
doc.InsertEndChild(root); // 将根节点添加到文档
for (const auto& col_struct : robot_description.collision_structures_) {
XMLElement* collisionElem = doc.NewElement("collision_model");
collisionElem->SetAttribute("link_name", col_struct.link_name.c_str());
collisionElem->SetAttribute("link_index", col_struct.link_index);
collisionElem->SetAttribute("geometry_type", col_struct.geometry_type);
collisionElem->SetAttribute("mesh_filename", col_struct.mesh_filename.c_str());
collisionElem->SetAttribute("parent_link_index", col_struct.parent_link_index);
collisionElem->SetAttribute("offset_x", col_struct.offset.x());
collisionElem->SetAttribute("offset_y", col_struct.offset.y());
collisionElem->SetAttribute("offset_z", col_struct.offset.z());
collisionElem->SetAttribute("rotation_x", col_struct.rotation.x());
collisionElem->SetAttribute("rotation_y", col_struct.rotation.y());
collisionElem->SetAttribute("rotation_z", col_struct.rotation.z());
collisionElem->SetAttribute("rotation_w", col_struct.rotation.w());
collisionElem->SetAttribute("obb_count", static_cast<int>(col_struct.obbModelList.size()));
for (size_t i = 0; i < col_struct.obbModelList.size(); ++i) {
const OBB& obb = col_struct.obbModelList[i];
XMLElement* obbElem = doc.NewElement("obb");
obbElem->SetAttribute("index", col_struct.obbObbIndexList[i]);
obbElem->SetAttribute("center_x", obb.center.x());
obbElem->SetAttribute("center_y", obb.center.y());
obbElem->SetAttribute("center_z", obb.center.z());
obbElem->SetAttribute("half_extent_x", obb.halfExtent[0]);
obbElem->SetAttribute("half_extent_y", obb.halfExtent[1]);
obbElem->SetAttribute("half_extent_z", obb.halfExtent[2]);
obbElem->SetAttribute("axis_0_x", obb.axis[0].x());
obbElem->SetAttribute("axis_0_y", obb.axis[0].y());
obbElem->SetAttribute("axis_0_z", obb.axis[0].z());
obbElem->SetAttribute("axis_1_x", obb.axis[1].x());
obbElem->SetAttribute("axis_1_y", obb.axis[1].y());
obbElem->SetAttribute("axis_1_z", obb.axis[1].z());
obbElem->SetAttribute("axis_2_x", obb.axis[2].x());
obbElem->SetAttribute("axis_2_y", obb.axis[2].y());
obbElem->SetAttribute("axis_2_z", obb.axis[2].z());
obbElem->SetAttribute("max_radius", obb.maxRadius);
obbElem->SetAttribute("self_min_project_x", obb.selfMinProject[0]);
obbElem->SetAttribute("self_min_project_y", obb.selfMinProject[1]);
obbElem->SetAttribute("self_min_project_z", obb.selfMinProject[2]);
obbElem->SetAttribute("self_max_project_x", obb.selfMaxProject[0]);
obbElem->SetAttribute("self_max_project_y", obb.selfMaxProject[1]);
obbElem->SetAttribute("self_max_project_z", obb.selfMaxProject[2]);
// 可根据需要添加轴向信息
collisionElem->InsertEndChild(obbElem);
}
collisionElem->SetAttribute("joint_index_count", static_cast<int>(col_struct.jointIndexList.size()));
for (size_t i = 0; i < col_struct.jointIndexList.size(); ++i) {
XMLElement* jointIndexElem = doc.NewElement("joint_index");
jointIndexElem->SetAttribute("index", col_struct.jointIndexList[i]);
collisionElem->InsertEndChild(jointIndexElem);
}
root->InsertEndChild(collisionElem);
}
for (size_t i = 0; i < robot_description.jointMap_.size(); ++i) {
const JointsModel& joint = robot_description.jointMap_[i];
XMLElement* jointElem = doc.NewElement("joint_model");
jointElem->SetAttribute("joint_name", joint.name.c_str());
jointElem->SetAttribute("parent_link", joint.parent_link);
jointElem->SetAttribute("child_link", joint.child_link);
jointElem->SetAttribute("nextLinkOffset_x", joint.nextLinkOffset.x());
jointElem->SetAttribute("nextLinkOffset_y", joint.nextLinkOffset.y());
jointElem->SetAttribute("nextLinkOffset_z", joint.nextLinkOffset.z());
jointElem->SetAttribute("joint_index", i);
jointElem->SetAttribute("type", joint.type);
jointElem->SetAttribute("axis_x", joint.axis.x());
jointElem->SetAttribute("axis_y", joint.axis.y());
jointElem->SetAttribute("axis_z", joint.axis.z());
jointElem->SetAttribute("limit_lower", joint.limit_lower);
jointElem->SetAttribute("limit_upper", joint.limit_upper);
jointElem->SetAttribute("limit_effort", joint.limit_e);
jointElem->SetAttribute("limit_velocity", joint.limit_v);
jointElem->SetAttribute("rotation_x", joint.rotation.x());
jointElem->SetAttribute("rotation_y", joint.rotation.y());
jointElem->SetAttribute("rotation_z", joint.rotation.z());
jointElem->SetAttribute("rotation_w", joint.rotation.w());
root->InsertEndChild(jointElem);
}
// 4. 将XML内容写入文件
XMLError error = doc.SaveFile(file_path.c_str());
if (error != XML_SUCCESS) {
throw std::runtime_error("写入XML文件失败:" + file_path + "（错误码：" + std::to_string(error) + "）");
}
printf("成功写入XML文件: %s\n", file_path.c_str());
}
int main(int argc, char *argv[])
{
UNUSED(argc);
UNUSED(argv);
// 示例URDF和SRDF字符串
std::string urdf_dir = "";
std::string srdf_dir = "";
std::string urdf_string = readFileToString(findFileInCurrentDir("FHrobot.urdf", urdf_dir));
std::string srdf_string = readFileToString(findFileInCurrentDir("FHrobot.srdf", srdf_dir));
printf("URDF路径: %s\n", urdf_dir.c_str());
printf("SRDF路径: %s\n", srdf_dir.c_str());
// 创建RobotDescription对象
RobotDescription robot_description(urdf_string, srdf_string);
std::string output_file = GetWriteFileName(urdf_dir, "FHrobot.xml");
WriteXmlFile(output_file, robot_description);
return 0;
}

75
dual_arm_moveit_action_server/config/joint_limits.yaml
View File

@@ -1,75 +0,0 @@
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
# For beginners, we downscale velocity and acceleration limits.
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
default_velocity_scaling_factor: 0.1
default_acceleration_scaling_factor: 0.1
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
body_2_joint:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
left_joint1:
has_velocity_limits: true
max_velocity: 3.1400000000000001
has_acceleration_limits: false
max_acceleration: 0
left_joint2:
has_velocity_limits: true
max_velocity: 3.1400000000000001
has_acceleration_limits: false
max_acceleration: 0
left_joint3:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
left_joint4:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
left_joint5:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
left_joint6:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
right_joint1:
has_velocity_limits: true
max_velocity: 3.1400000000000001
has_acceleration_limits: false
max_acceleration: 0
right_joint2:
has_velocity_limits: true
max_velocity: 3.1400000000000001
has_acceleration_limits: false
max_acceleration: 0
right_joint3:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
right_joint4:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
right_joint5:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
right_joint6:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0

33
dual_arm_moveit_action_server/config/moveit_controllers.yaml
View File

@@ -1,33 +0,0 @@
# MoveIt uses this configuration for controller management
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
controller_names:
- leftarm_controller
- rightarm_controller
leftarm_controller:
type: FollowJointTrajectory
joints:
- body_2_joint
- left_joint1
- left_joint2
- left_joint3
- left_joint4
- left_joint5
- left_joint6
action_ns: follow_joint_trajectory
default: true
rightarm_controller:
type: FollowJointTrajectory
joints:
- body_2_joint
- right_joint1
- right_joint2
- right_joint3
- right_joint4
- right_joint5
- right_joint6
action_ns: follow_joint_trajectory
default: true

46
dual_arm_moveit_action_server/config/ros2_controllers.yaml
View File

@@ -1,46 +0,0 @@
# This config file is used by ros2_control
controller_manager:
ros__parameters:
update_rate: 100 # Hz
leftarm_controller:
type: joint_trajectory_controller/JointTrajectoryController
rightarm_controller:
type: joint_trajectory_controller/JointTrajectoryController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
leftarm_controller:
ros__parameters:
joints:
- body_2_joint
- left_joint1
- left_joint2
- left_joint3
- left_joint4
- left_joint5
- left_joint6
command_interfaces:
- position
state_interfaces:
- position
- velocity
rightarm_controller:
ros__parameters:
joints:
- body_2_joint
- right_joint1
- right_joint2
- right_joint3
- right_joint4
- right_joint5
- right_joint6
command_interfaces:
- position
state_interfaces:
- position
- velocity

40
dual_arm_moveit_action_server/launch/dual_arm_moveit_action_server.launch.py
View File

@@ -1,40 +0,0 @@
from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_move_group_launch, generate_moveit_rviz_launch
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
moveit_config = MoveItConfigsBuilder(
"FHrobot", package_name="dual_arm_moveit_action_server"
).to_moveit_configs()
move_group_launch = generate_move_group_launch(moveit_config)
rviz_launch = generate_moveit_rviz_launch(moveit_config)
action_server_node = Node(
package="dual_arm_moveit_action_server",
executable="dual_arm_moveit_action_server",
output="screen",
parameters=[
moveit_config.to_dict(),
{
"left_group_name": "LeftArm",
"right_group_name": "RightArm",
"planning_time": 5.0,
"acceleration_scaling": 0.5,
"cartesian_step": 0.01,
"cartesian_jump_threshold": 0.0,
"cartesian_min_fraction": 0.9,
},
],
)
return LaunchDescription(
[
move_group_launch,
rviz_launch,
action_server_node,
]
)

202
gjk_interface/LICENSE
View File

@@ -1,202 +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 [yyyy] [name of copyright owner]
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.

4
dual_arm_action_server/CMakeLists.txt → src/dual_arm_action_server/CMakeLists.txt
View File

@@ -14,7 +14,7 @@ find_package(tf2 REQUIRED)
include_directories(
include
${CMAKE_CURRENT_SOURCE_DIR}/../include
${CMAKE_CURRENT_SOURCE_DIR}/../../include
)
add_executable(dual_arm_action_server_node
@@ -30,7 +30,7 @@ ament_target_dependencies(dual_arm_action_server_node
tf2
)
find_library(RMAN_API_LIB NAMES api_cpp PATHS "${CMAKE_CURRENT_SOURCE_DIR}/../lib")
find_library(RMAN_API_LIB NAMES api_cpp PATHS "${CMAKE_CURRENT_SOURCE_DIR}/../../lib")
target_link_libraries(dual_arm_action_server_node ${RMAN_API_LIB})
install(TARGETS

0
dual_arm_action_server/config/dual_arm_action_server.yaml → src/dual_arm_action_server/config/dual_arm_action_server.yaml
View File

0
dual_arm_action_server/include/dual_arm_action_server/dual_arm_action_server.hpp → src/dual_arm_action_server/include/dual_arm_action_server/dual_arm_action_server.hpp
View File

0
dual_arm_action_server/include/dual_arm_action_server/real_robot_driver.hpp → src/dual_arm_action_server/include/dual_arm_action_server/real_robot_driver.hpp
View File

0
dual_arm_action_server/launch/dual_arm_action_server.launch.py → src/dual_arm_action_server/launch/dual_arm_action_server.launch.py
View File

0
dual_arm_action_server/package.xml → src/dual_arm_action_server/package.xml
View File

0
dual_arm_action_server/src/dual_arm_action_server_node.cpp → src/dual_arm_action_server/src/dual_arm_action_server_node.cpp
View File

0
dual_arm_action_server/src/real_robot_driver.cpp → src/dual_arm_action_server/src/real_robot_driver.cpp
View File

8
src/dual_arm_moveit_action_server/.setup_assistant Normal file
View File

@@ -0,0 +1,8 @@
moveit_setup_assistant_config:
URDF:
package: dual_arm_moveit_action_server
relative_path: config/FHrobot.urdf.xacro
xacro_args: ""
SRDF:
relative_path: config/FHrobot.srdf

4
dual_arm_moveit_action_server/CMakeLists.txt → src/dual_arm_moveit_action_server/CMakeLists.txt
View File

@@ -46,5 +46,9 @@ install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}
)
install(FILES .setup_assistant
DESTINATION share/${PROJECT_NAME}
)
ament_package()

9
dual_arm_moveit_action_server/config/FHrobot.ros2_control.xacro → src/dual_arm_moveit_action_server/config/FHrobot.ros2_control.xacro
View File

@@ -1,20 +1,13 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="FHrobot_ros2_control" params="name initial_positions_file">
<xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>
<xacro:property name="initial_positions" value="${xacro.load_yaml(initial_positions_file)['initial_positions']}"/>
<ros2_control name="${name}" type="system">
<hardware>
<!-- By default, set up controllers for simulation. This won't work on real hardware -->
<plugin>mock_components/GenericSystem</plugin>
</hardware>
<joint name="body_2_joint">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['body_2_joint']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="left_joint1">
<command_interface name="position"/>
<state_interface name="position">

57
rm_arm_control/urdf/FHrobot.srdf → src/dual_arm_moveit_action_server/config/FHrobot.srdf
View File

@@ -10,44 +10,13 @@
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
<group name="LeftArm">
<link name="body_2_link"/>
<link name="base_link_leftarm"/>
<link name="LeftLink1"/>
<link name="LeftLink2"/>
<link name="LeftLink3"/>
<link name="LeftLink4"/>
<link name="LeftLink5"/>
<link name="LeftLink6"/>
<joint name="left_arm_base_joint"/>
<joint name="left_joint1"/>
<joint name="left_joint2"/>
<joint name="left_joint3"/>
<joint name="left_joint4"/>
<joint name="left_joint5"/>
<joint name="left_joint6"/>
<chain base_link="body_2_link" tip_link="LeftLink6"/>
<chain base_link="base_link_leftarm" tip_link="LeftLink6"/>
</group>
<group name="RightArm">
<link name="body_2_link"/>
<link name="base_link_rightarm"/>
<link name="RightLink1"/>
<link name="RightLink2"/>
<link name="RightLink3"/>
<link name="RightLink4"/>
<link name="RightLink5"/>
<link name="RightLink6"/>
<joint name="right_arm_base_joint"/>
<joint name="right_joint1"/>
<joint name="right_joint2"/>
<joint name="right_joint3"/>
<joint name="right_joint4"/>
<joint name="right_joint5"/>
<joint name="right_joint6"/>
<chain base_link="body_2_link" tip_link="RightLink6"/>
<chain base_link="base_link_rightarm" tip_link="RightLink6"/>
</group>
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
<group_state name="LeftZero" group="LeftArm">
<joint name="body_2_joint" value="0"/>
<joint name="left_joint1" value="0"/>
<joint name="left_joint2" value="0"/>
<joint name="left_joint3" value="0"/>
@@ -56,7 +25,6 @@
<joint name="left_joint6" value="0"/>
</group_state>
<group_state name="RightZero" group="RightArm">
<joint name="body_2_joint" value="0"/>
<joint name="right_joint1" value="0"/>
<joint name="right_joint2" value="0"/>
<joint name="right_joint3" value="0"/>
@@ -77,7 +45,6 @@
<disable_collisions link1="LeftLink1" link2="base_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="base_link_leftarm" reason="Adjacent"/>
<disable_collisions link1="LeftLink1" link2="base_link_rightarm" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="body_2_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="body_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="head_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="left_behind_hip_link" reason="Never"/>
@@ -184,7 +151,6 @@
<disable_collisions link1="RightLink1" link2="base_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="base_link_leftarm" reason="Never"/>
<disable_collisions link1="RightLink1" link2="base_link_rightarm" reason="Adjacent"/>
<disable_collisions link1="RightLink1" link2="body_2_link" reason="Default"/>
<disable_collisions link1="RightLink1" link2="body_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="head_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="left_behind_hip_link" reason="Never"/>
@@ -282,7 +248,6 @@
<disable_collisions link1="RightLink6" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="base_link" link2="base_link_leftarm" reason="Never"/>
<disable_collisions link1="base_link" link2="base_link_rightarm" reason="Never"/>
<disable_collisions link1="base_link" link2="body_2_link" reason="Never"/>
<disable_collisions link1="base_link" link2="body_link" reason="Adjacent"/>
<disable_collisions link1="base_link" link2="head_link" reason="Never"/>
<disable_collisions link1="base_link" link2="left_behind_hip_link" reason="Adjacent"/>
@@ -299,7 +264,6 @@
<disable_collisions link1="base_link" link2="right_front_roll_link" reason="Adjacent"/>
<disable_collisions link1="base_link" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="base_link_rightarm" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="body_2_link" reason="Adjacent"/>
<disable_collisions link1="base_link_leftarm" link2="body_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="head_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="left_behind_hip_link" reason="Never"/>
@@ -316,7 +280,6 @@
<disable_collisions link1="base_link_leftarm" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="body_2_link" reason="Adjacent"/>
<disable_collisions link1="base_link_rightarm" link2="body_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="head_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="left_behind_hip_link" reason="Never"/>
@@ -333,22 +296,6 @@
<disable_collisions link1="base_link_rightarm" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="body_link" reason="Adjacent"/>
<disable_collisions link1="body_2_link" link2="head_link" reason="Adjacent"/>
<disable_collisions link1="body_2_link" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="body_link" link2="head_link" reason="Never"/>
<disable_collisions link1="body_link" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="body_link" link2="left_behind_leg_link" reason="Never"/>

0
dual_arm_moveit_action_server/config/FHrobot.urdf.xacro → src/dual_arm_moveit_action_server/config/FHrobot.urdf.xacro
View File

1
dual_arm_moveit_action_server/config/initial_positions.yaml → src/dual_arm_moveit_action_server/config/initial_positions.yaml
View File

@@ -1,7 +1,6 @@
# Default initial positions for FHrobot's ros2_control fake system
initial_positions:
body_2_joint: 0
left_joint1: 0
left_joint2: 0
left_joint3: 0

53
srdf/config/joint_limits.yaml → src/dual_arm_moveit_action_server/config/joint_limits.yaml
View File

@@ -8,68 +8,63 @@ default_acceleration_scaling_factor: 0.1
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
body_2_joint:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
left_joint1:
has_velocity_limits: true
max_velocity: 3.1400000000000001
has_acceleration_limits: false
max_acceleration: 0
has_acceleration_limits: true
max_acceleration: 0.5
left_joint2:
has_velocity_limits: true
max_velocity: 3.1400000000000001
has_acceleration_limits: false
max_acceleration: 0
has_acceleration_limits: true
max_acceleration: 0.5
left_joint3:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
has_acceleration_limits: true
max_acceleration: 0.5
left_joint4:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
has_acceleration_limits: true
max_acceleration: 0.5
left_joint5:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
has_acceleration_limits: true
max_acceleration: 0.5
left_joint6:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
has_acceleration_limits: true
max_acceleration: 0.5
right_joint1:
has_velocity_limits: true
max_velocity: 3.1400000000000001
has_acceleration_limits: false
max_acceleration: 0
has_acceleration_limits: true
max_acceleration: 0.5
right_joint2:
has_velocity_limits: true
max_velocity: 3.1400000000000001
has_acceleration_limits: false
max_acceleration: 0
has_acceleration_limits: true
max_acceleration: 0.5
right_joint3:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
has_acceleration_limits: true
max_acceleration: 0.5
right_joint4:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
has_acceleration_limits: true
max_acceleration: 0.5
right_joint5:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
has_acceleration_limits: true
max_acceleration: 0.5
right_joint6:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
has_acceleration_limits: true
max_acceleration: 0.5

0
dual_arm_moveit_action_server/config/kinematics.yaml → src/dual_arm_moveit_action_server/config/kinematics.yaml
View File

0
dual_arm_moveit_action_server/config/moveit.rviz → src/dual_arm_moveit_action_server/config/moveit.rviz
View File

2
srdf/config/moveit_controllers.yaml → src/dual_arm_moveit_action_server/config/moveit_controllers.yaml
View File

@@ -10,7 +10,6 @@ moveit_simple_controller_manager:
leftarm_controller:
type: FollowJointTrajectory
joints:
- body_2_joint
- left_joint1
- left_joint2
- left_joint3
@@ -22,7 +21,6 @@ moveit_simple_controller_manager:
rightarm_controller:
type: FollowJointTrajectory
joints:
- body_2_joint
- right_joint1
- right_joint2
- right_joint3

0
dual_arm_moveit_action_server/config/pilz_cartesian_limits.yaml → src/dual_arm_moveit_action_server/config/pilz_cartesian_limits.yaml
View File

3
srdf/config/ros2_controllers.yaml → src/dual_arm_moveit_action_server/config/ros2_controllers.yaml
View File

@@ -6,7 +6,6 @@ controller_manager:
leftarm_controller:
type: joint_trajectory_controller/JointTrajectoryController
rightarm_controller:
type: joint_trajectory_controller/JointTrajectoryController
@@ -17,7 +16,6 @@ controller_manager:
leftarm_controller:
ros__parameters:
joints:
- body_2_joint
- left_joint1
- left_joint2
- left_joint3
@@ -32,7 +30,6 @@ leftarm_controller:
rightarm_controller:
ros__parameters:
joints:
- body_2_joint
- right_joint1
- right_joint2
- right_joint3

7
dual_arm_moveit_action_server/include/dual_arm_moveit_action_server/dual_arm_moveit_action_server.hpp → src/dual_arm_moveit_action_server/include/dual_arm_moveit_action_server/dual_arm_moveit_action_server.hpp
View File

@@ -2,6 +2,7 @@
#include <atomic>
#include <memory>
#include <mutex>
#include <string>
#include <vector>
@@ -64,13 +65,19 @@ private:
void fill_feedback_joints(DualArm::Feedback & feedback);
void publish_feedback_once(const std::shared_ptr<GoalHandleDualArm> & goal_handle);
void update_feedback(uint8_t status, double progress);
rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<DualArm>::SharedPtr action_server_;
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> left_move_group_;
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> right_move_group_;
rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>::SharedPtr display_pub_;
std::shared_ptr<DualArm::Feedback> feedback_state_;
std::mutex feedback_mutex_;
std::atomic_bool planning_busy_{false};
std::atomic_bool cancel_requested_{false};
std::string left_group_name_;
std::string right_group_name_;
double planning_time_{5.0};

106
src/dual_arm_moveit_action_server/launch/dual_arm_moveit_action_server.launch.py Normal file
View File

@@ -0,0 +1,106 @@
from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_move_group_launch
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
moveit_config = MoveItConfigsBuilder(
"FHrobot", package_name="dual_arm_moveit_action_server"
).to_moveit_configs()
move_group_launch = generate_move_group_launch(moveit_config)
rviz_config = LaunchConfiguration("rviz_config")
rviz_config_arg = DeclareLaunchArgument(
"rviz_config",
default_value=str(moveit_config.package_path / "config/moveit.rviz"),
)
controllers_yaml = str(moveit_config.package_path / "config/ros2_controllers.yaml")
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
output="screen",
parameters=[
moveit_config.robot_description,
controllers_yaml,
],
)
rsp_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="screen",
parameters=[moveit_config.robot_description],
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
output="screen",
)
leftarm_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["leftarm_controller", "--controller-manager", "/controller_manager"],
output="screen",
)
rightarm_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["rightarm_controller", "--controller-manager", "/controller_manager"],
output="screen",
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
output="log",
arguments=["-d", rviz_config],
parameters=[
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.planning_pipelines,
moveit_config.robot_description_kinematics,
moveit_config.joint_limits,
],
)
action_server_node = Node(
package="dual_arm_moveit_action_server",
executable="dual_arm_moveit_action_server",
output="screen",
parameters=[
moveit_config.to_dict(),
{
"left_group_name": "LeftArm",
"right_group_name": "RightArm",
"planning_time": 5.0,
"acceleration_scaling": 0.5,
"cartesian_step": 0.005,
"cartesian_jump_threshold": 0.1,
"cartesian_min_fraction": 0.5,
},
],
)
return LaunchDescription(
[
rsp_node,
ros2_control_node,
joint_state_broadcaster_spawner,
leftarm_controller_spawner,
rightarm_controller_spawner,
move_group_launch,
rviz_config_arg,
rviz_node,
action_server_node,
]
)

0
dual_arm_moveit_action_server/meshes/arm_base_link.STL → src/dual_arm_moveit_action_server/meshes/arm_base_link.STL
View File

0
dual_arm_moveit_action_server/meshes/base_link.STL → src/dual_arm_moveit_action_server/meshes/base_link.STL
View File

0
dual_arm_moveit_action_server/meshes/body_2_link.STL → src/dual_arm_moveit_action_server/meshes/body_2_link.STL
View File

0
dual_arm_moveit_action_server/meshes/body_link.STL → src/dual_arm_moveit_action_server/meshes/body_link.STL
View File

0
dual_arm_moveit_action_server/meshes/head_link.STL → src/dual_arm_moveit_action_server/meshes/head_link.STL
View File

0
dual_arm_moveit_action_server/meshes/left_arm_1_link.STL → src/dual_arm_moveit_action_server/meshes/left_arm_1_link.STL
View File

0
dual_arm_moveit_action_server/meshes/left_arm_2_link.STL → src/dual_arm_moveit_action_server/meshes/left_arm_2_link.STL
View File

0
dual_arm_moveit_action_server/meshes/left_arm_3_link.STL → src/dual_arm_moveit_action_server/meshes/left_arm_3_link.STL
View File

0
dual_arm_moveit_action_server/meshes/left_arm_4_link.STL → src/dual_arm_moveit_action_server/meshes/left_arm_4_link.STL
View File

0
dual_arm_moveit_action_server/meshes/left_behind_hip_link.STL → src/dual_arm_moveit_action_server/meshes/left_behind_hip_link.STL
View File

0
dual_arm_moveit_action_server/meshes/left_behind_leg_link.STL → src/dual_arm_moveit_action_server/meshes/left_behind_leg_link.STL
View File

0
dual_arm_moveit_action_server/meshes/left_behind_wheel_link.STL → src/dual_arm_moveit_action_server/meshes/left_behind_wheel_link.STL
View File

0
dual_arm_moveit_action_server/meshes/left_front_hip_link.STL → src/dual_arm_moveit_action_server/meshes/left_front_hip_link.STL
View File

0
dual_arm_moveit_action_server/meshes/left_front_knee_link.STL → src/dual_arm_moveit_action_server/meshes/left_front_knee_link.STL
View File

0
dual_arm_moveit_action_server/meshes/left_front_roll_link.STL → src/dual_arm_moveit_action_server/meshes/left_front_roll_link.STL
View File

0
dual_arm_moveit_action_server/meshes/left_front_wheel_link.STL → src/dual_arm_moveit_action_server/meshes/left_front_wheel_link.STL
View File

0
dual_arm_moveit_action_server/meshes/left_shoulder_link.STL → src/dual_arm_moveit_action_server/meshes/left_shoulder_link.STL
View File

0
dual_arm_moveit_action_server/meshes/left_wrist_link.STL → src/dual_arm_moveit_action_server/meshes/left_wrist_link.STL
View File

0
dual_arm_moveit_action_server/meshes/link1.STL → src/dual_arm_moveit_action_server/meshes/link1.STL
View File

0
dual_arm_moveit_action_server/meshes/link2.STL → src/dual_arm_moveit_action_server/meshes/link2.STL
View File

0
dual_arm_moveit_action_server/meshes/link3.STL → src/dual_arm_moveit_action_server/meshes/link3.STL
View File

0
dual_arm_moveit_action_server/meshes/link4.STL → src/dual_arm_moveit_action_server/meshes/link4.STL
View File

0
dual_arm_moveit_action_server/meshes/link5.STL → src/dual_arm_moveit_action_server/meshes/link5.STL
View File

0
dual_arm_moveit_action_server/meshes/link6.STL → src/dual_arm_moveit_action_server/meshes/link6.STL
View File

0
dual_arm_moveit_action_server/meshes/right_arm_1_link.STL → src/dual_arm_moveit_action_server/meshes/right_arm_1_link.STL
View File

0
dual_arm_moveit_action_server/meshes/right_arm_2_link.STL → src/dual_arm_moveit_action_server/meshes/right_arm_2_link.STL
View File

0
dual_arm_moveit_action_server/meshes/right_arm_3_link.STL → src/dual_arm_moveit_action_server/meshes/right_arm_3_link.STL
View File

0
dual_arm_moveit_action_server/meshes/right_arm_4_link.STL → src/dual_arm_moveit_action_server/meshes/right_arm_4_link.STL
View File

0
dual_arm_moveit_action_server/meshes/right_behind_hip_link.STL → src/dual_arm_moveit_action_server/meshes/right_behind_hip_link.STL
View File

0
dual_arm_moveit_action_server/meshes/right_behind_leg_link.STL → src/dual_arm_moveit_action_server/meshes/right_behind_leg_link.STL
View File

0
dual_arm_moveit_action_server/meshes/right_behind_wheel_link.STL → src/dual_arm_moveit_action_server/meshes/right_behind_wheel_link.STL
View File

0
dual_arm_moveit_action_server/meshes/right_front_hip_link.STL → src/dual_arm_moveit_action_server/meshes/right_front_hip_link.STL
View File

0
dual_arm_moveit_action_server/meshes/right_front_knee_link.STL → src/dual_arm_moveit_action_server/meshes/right_front_knee_link.STL
View File

0
dual_arm_moveit_action_server/meshes/right_front_roll_link.STL → src/dual_arm_moveit_action_server/meshes/right_front_roll_link.STL
View File

0
dual_arm_moveit_action_server/meshes/right_front_wheel_link.STL → src/dual_arm_moveit_action_server/meshes/right_front_wheel_link.STL
View File

0
dual_arm_moveit_action_server/meshes/right_shoulder_link.STL → src/dual_arm_moveit_action_server/meshes/right_shoulder_link.STL
View File

0
dual_arm_moveit_action_server/meshes/right_wrist_link.STL → src/dual_arm_moveit_action_server/meshes/right_wrist_link.STL
View File

0
dual_arm_moveit_action_server/package.xml → src/dual_arm_moveit_action_server/package.xml
View File

134
src/dual_arm_moveit_action_server/scripts/dualarm_action_test.py Normal file
View File

@@ -0,0 +1,134 @@
#!/usr/bin/env python3
import argparse
import random
import sys
import time
from typing import List
import rclpy
from rclpy.action import ActionClient
from geometry_msgs.msg import Pose
from interfaces.action import DualArm
from interfaces.msg import ArmMotionParams
def make_movej(arm_id: int, joints: List[float]) -> ArmMotionParams:
msg = ArmMotionParams()
msg.arm_id = arm_id
msg.motion_type = ArmMotionParams.MOVEJ
msg.target_joints = joints
msg.blend_radius = 0.0
return msg
def _format_pose(pose: Pose) -> str:
return (
f"pos=({pose.position.x:.3f}, {pose.position.y:.3f}, {pose.position.z:.3f}) "
f"quat=({pose.orientation.x:.3f}, {pose.orientation.y:.3f}, "
f"{pose.orientation.z:.3f}, {pose.orientation.w:.3f})"
)
def send_goal(action_client: ActionClient, goal_msg: DualArm.Goal, label: str) -> bool:
print(f"Sending goal: {label}")
for params in goal_msg.arm_motion_params:
if params.motion_type == ArmMotionParams.MOVEJ:
print(f"[{label}] MOVEJ arm={params.arm_id} target_joints={list(params.target_joints)}")
else:
print(f"[{label}] MOVEL arm={params.arm_id} target_pose={_format_pose(params.target_pose)}")
if not action_client.wait_for_server(timeout_sec=5.0):
print("Action server 'DualArm' not available", file=sys.stderr)
return False
feedback_log = {"last": 0.0}
def feedback_cb(feedback_msg):
progress = feedback_msg.feedback.progress
if progress != feedback_log["last"]:
feedback_log["last"] = progress
print(f"[{label}] progress={progress:.2f} status={feedback_msg.feedback.status}")
send_future = action_client.send_goal_async(goal_msg, feedback_callback=feedback_cb)
rclpy.spin_until_future_complete(action_client._node, send_future)
goal_handle = send_future.result()
if not goal_handle.accepted:
print(f"[{label}] goal rejected", file=sys.stderr)
return False
result_future = goal_handle.get_result_async()
while not result_future.done():
rclpy.spin_once(action_client._node, timeout_sec=0.1)
result = result_future.result().result
print(f"[{label}] success={result.success} progress={result.final_progress:.2f} msg={result.message}")
return result.success
def main() -> int:
parser = argparse.ArgumentParser(description="DualArm action test client")
parser.add_argument("--velocity-scaling", type=float, default=0.2)
parser.add_argument("--dual", action="store_true", help="Send left+right in a single goal")
parser.add_argument(
"--mode",
choices=["mixed", "dual", "single"],
default="dual",
help="Test mode: mixed=dual+single, dual=both arms, single=left/right only",
)
parser.add_argument("--cases", type=int, default=3)
parser.add_argument("--joints", type=int, default=6, help="Joint count per arm for MOVEJ")
parser.add_argument("--joint-limit", type=float, default=0.5)
parser.add_argument("--between-wait", type=float, default=1.0)
parser.add_argument("--loops", type=int, default=3)
args = parser.parse_args()
rclpy.init()
node = rclpy.create_node("dualarm_action_test")
action_client = ActionClient(node, DualArm, "DualArm")
def random_joints() -> List[float]:
return [random.uniform(-args.joint_limit, args.joint_limit) for _ in range(args.joints)]
def generate_cases(label: str) -> List[tuple[str, DualArm.Goal]]:
cases: List[tuple[str, DualArm.Goal]] = []
for case_idx in range(1, max(1, args.cases) + 1):
goal = DualArm.Goal()
goal.velocity_scaling = args.velocity_scaling
if label == "dual":
goal.arm_motion_params = [
make_movej(ArmMotionParams.ARM_LEFT, random_joints()),
make_movej(ArmMotionParams.ARM_RIGHT, random_joints()),
]
elif label == "left":
goal.arm_motion_params = [
make_movej(ArmMotionParams.ARM_LEFT, random_joints()),
]
else:
goal.arm_motion_params = [
make_movej(ArmMotionParams.ARM_RIGHT, random_joints()),
]
cases.append((f"{label}_case{case_idx}", goal))
return cases
success = True
for loop_idx in range(1, max(1, args.loops) + 1):
loop_cases: List[tuple[str, DualArm.Goal]] = []
mode = "dual" if args.dual else args.mode
if mode in ("mixed", "dual"):
loop_cases.extend(generate_cases("dual"))
if mode in ("mixed", "single"):
loop_cases.extend(generate_cases("left"))
loop_cases.extend(generate_cases("right"))
for label, goal in loop_cases:
ok = send_goal(action_client, goal, f"{label}_loop{loop_idx}")
success = success and ok
time.sleep(max(0.0, args.between_wait))
node.destroy_node()
rclpy.shutdown()
return 0 if success else 1
if __name__ == "__main__":
raise SystemExit(main())

261
dual_arm_moveit_action_server/src/dual_arm_moveit_action_server.cpp → src/dual_arm_moveit_action_server/src/dual_arm_moveit_action_server.cpp
View File

@@ -1,7 +1,57 @@
#include "dual_arm_moveit_action_server/dual_arm_moveit_action_server.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <thread>
namespace
{
struct BusyReset
{
std::atomic_bool & flag;
explicit BusyReset(std::atomic_bool & flag_in) : flag(flag_in) {}
~BusyReset() { flag.store(false); }
};
struct FeedbackReset
{
std::shared_ptr<dual_arm_moveit_action_server::DualArm::Feedback> & feedback;
std::mutex & mutex;
FeedbackReset(
std::shared_ptr<dual_arm_moveit_action_server::DualArm::Feedback> & feedback_in,
std::mutex & mutex_in)
: feedback(feedback_in), mutex(mutex_in)
{
}
~FeedbackReset()
{
std::lock_guard<std::mutex> lock(mutex);
feedback.reset();
}
};
struct ExecTask
{
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group;
moveit_msgs::msg::RobotTrajectory trajectory;
std::vector<std::string> joint_names;
std::vector<double> target_positions;
std::chrono::steady_clock::time_point deadline;
bool done{false};
};
double compute_trajectory_duration(const moveit_msgs::msg::RobotTrajectory & trajectory)
{
if (trajectory.joint_trajectory.points.empty()) {
return 0.0;
}
const auto & last_point = trajectory.joint_trajectory.points.back();
return static_cast<double>(last_point.time_from_start.sec) +
static_cast<double>(last_point.time_from_start.nanosec) * 1e-9;
}
} // namespace
namespace dual_arm_moveit_action_server
{
@@ -16,9 +66,9 @@ void DualArmMoveitActionServer::initialize()
right_group_name_ = node_->declare_parameter<std::string>("right_group_name", "RightArm");
planning_time_ = node_->declare_parameter<double>("planning_time", 5.0);
acceleration_scaling_ = node_->declare_parameter<double>("acceleration_scaling", 0.5);
cartesian_step_ = node_->declare_parameter<double>("cartesian_step", 0.01);
cartesian_jump_threshold_ = node_->declare_parameter<double>("cartesian_jump_threshold", 0.0);
cartesian_min_fraction_ = node_->declare_parameter<double>("cartesian_min_fraction", 0.9);
cartesian_step_ = node_->declare_parameter<double>("cartesian_step", 0.005);
cartesian_jump_threshold_ = node_->declare_parameter<double>("cartesian_jump_threshold", 0.1);
cartesian_min_fraction_ = node_->declare_parameter<double>("cartesian_min_fraction", 0.5);
try {
left_move_group_ =
@@ -78,6 +128,13 @@ rclcpp_action::CancelResponse DualArmMoveitActionServer::handle_cancel(
const std::shared_ptr<GoalHandleDualArm> /*goal_handle*/)
{
RCLCPP_INFO(node_->get_logger(), "DualArm cancel requested");
cancel_requested_.store(true);
if (left_move_group_) {
left_move_group_->stop();
}
if (right_move_group_) {
right_move_group_->stop();
}
return rclcpp_action::CancelResponse::ACCEPT;
}
@@ -88,31 +145,40 @@ void DualArmMoveitActionServer::handle_accepted(const std::shared_ptr<GoalHandle
void DualArmMoveitActionServer::execute(const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
struct BusyReset
{
std::atomic_bool & flag;
~BusyReset() { flag.store(false); }
} busy_reset{planning_busy_};
cancel_requested_.store(false);
auto feedback = std::make_shared<DualArm::Feedback>();
BusyReset busy_reset(planning_busy_);
FeedbackReset feedback_reset(feedback_state_, feedback_mutex_);
auto result = std::make_shared<DualArm::Result>();
auto feedback = std::make_shared<DualArm::Feedback>();
result->success = false;
result->final_progress = 0.0;
result->message = "DualArm planning failed";
const auto goal = goal_handle->get_goal();
const auto params_list = normalize_goal_params(*goal);
const size_t total = params_list.size();
const auto should_cancel = [&]() {
return cancel_requested_.load() || goal_handle->is_canceling();
};
const auto stop_execution = [&]() {
if (left_move_group_) {
left_move_group_->stop();
}
if (right_move_group_) {
right_move_group_->stop();
}
};
feedback->status = DualArm::Goal::STATUS_PLANNING;
feedback->progress = 0.0;
fill_feedback_joints(*feedback);
goal_handle->publish_feedback(feedback);
update_feedback(DualArm::Goal::STATUS_PLANNING, 0.0);
publish_feedback_once(goal_handle);
std::vector<moveit_msgs::msg::RobotTrajectory> trajectories;
trajectories.reserve(params_list.size());
std::vector<ExecTask> exec_tasks;
std::vector<std::string> exec_errors(total);
exec_tasks.reserve(total);
for (size_t i = 0; i < params_list.size(); ++i) {
if (goal_handle->is_canceling()) {
for (size_t i = 0; i < total; ++i) {
if (should_cancel()) {
result->message = "DualArm canceled";
result->success = false;
result->final_progress = 0.0;
@@ -134,21 +200,116 @@ void DualArmMoveitActionServer::execute(const std::shared_ptr<GoalHandleDualArm>
}
publish_display_trajectory(trajectory, *move_group);
trajectories.push_back(trajectory);
feedback->status = (i + 1 == params_list.size()) ?
DualArm::Goal::STATUS_DONE :
DualArm::Goal::STATUS_EXECUTING;
feedback->progress =
static_cast<double>(i + 1) / static_cast<double>(params_list.size());
fill_feedback_joints(*feedback);
goal_handle->publish_feedback(feedback);
ExecTask task;
task.move_group = move_group;
task.trajectory = trajectory;
task.joint_names = trajectory.joint_trajectory.joint_names;
if (!trajectory.joint_trajectory.points.empty()) {
task.target_positions = trajectory.joint_trajectory.points.back().positions;
}
const double duration = compute_trajectory_duration(trajectory);
const double timeout_sec = std::max(2.0, duration + 2.0);
task.deadline = std::chrono::steady_clock::now() +
std::chrono::milliseconds(static_cast<int>(timeout_sec * 1000.0));
const auto exec_code = move_group->asyncExecute(trajectory);
if (exec_code != moveit::core::MoveItErrorCode::SUCCESS) {
result->message = "MoveIt asyncExecute failed";
goal_handle->abort(result);
return;
}
exec_tasks.push_back(std::move(task));
}
result->success = true;
result->final_progress = 1.0;
result->message = "DualArm planned successfully and published to RViz";
goal_handle->succeed(result);
while (rclcpp::ok()) {
if (should_cancel()) {
stop_execution();
result->message = "DualArm canceled";
result->success = false;
result->final_progress = 0.0;
goal_handle->canceled(result);
return;
}
size_t done_count = 0;
for (size_t i = 0; i < exec_tasks.size(); ++i) {
auto & task = exec_tasks[i];
if (task.done) {
++done_count;
continue;
}
auto current_state = task.move_group->getCurrentState();
if (current_state && !task.joint_names.empty() &&
task.joint_names.size() == task.target_positions.size())
{
double max_error = 0.0;
for (size_t j = 0; j < task.joint_names.size(); ++j) {
const double current = current_state->getVariablePosition(task.joint_names[j]);
const double error = std::abs(current - task.target_positions[j]);
if (error > max_error) {
max_error = error;
}
}
if (max_error <= 0.01) {
task.done = true;
++done_count;
continue;
}
}
if (std::chrono::steady_clock::now() >= task.deadline) {
exec_errors[i] = "MoveIt execution timeout";
stop_execution();
result->message = exec_errors[i];
result->success = false;
result->final_progress = 0.0;
goal_handle->abort(result);
return;
}
}
feedback->progress = total > 0 ? static_cast<double>(done_count) / total : 0.0;
feedback->status = DualArm::Goal::STATUS_EXECUTING;
fill_feedback_joints(*feedback);
goal_handle->publish_feedback(feedback);
if (done_count == total) {
break;
}
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
for (const auto & task : exec_tasks) {
if (task.move_group) {
publish_display_trajectory(task.trajectory, *task.move_group);
}
}
bool all_success = true;
for (const auto & error : exec_errors) {
if (!error.empty()) {
all_success = false;
break;
}
}
result->success = all_success;
result->final_progress = all_success ? 1.0 : 0.0;
if (all_success) {
result->message = "DualArm planned and executed successfully";
goal_handle->succeed(result);
} else {
for (const auto & error : exec_errors) {
if (!error.empty()) {
result->message = error;
break;
}
}
goal_handle->abort(result);
}
}
bool DualArmMoveitActionServer::validate_goal(const DualArm::Goal & goal, std::string * error_msg) const
@@ -295,12 +456,22 @@ bool DualArmMoveitActionServer::plan_for_arm(
}
if (params.motion_type == interfaces::msg::ArmMotionParams::MOVEL) {
if (params.arm_id == interfaces::msg::ArmMotionParams::ARM_LEFT) {
move_group.setPoseReferenceFrame("base_link_leftarm");
} else if (params.arm_id == interfaces::msg::ArmMotionParams::ARM_RIGHT) {
move_group.setPoseReferenceFrame("base_link_rightarm");
}
RCLCPP_INFO(
node_->get_logger(),
"MOVEL frames: pose_reference_frame=%s planning_frame=%s",
move_group.getPoseReferenceFrame().c_str(),
move_group.getPlanningFrame().c_str());
std::vector<geometry_msgs::msg::Pose> waypoints;
waypoints.push_back(params.target_pose);
moveit_msgs::msg::RobotTrajectory trajectory;
const double fraction = move_group.computeCartesianPath(
waypoints, cartesian_step_, cartesian_jump_threshold_, trajectory, true);
waypoints, cartesian_step_, cartesian_jump_threshold_, trajectory, false);
move_group.clearPoseTargets();
@@ -406,6 +577,36 @@ void DualArmMoveitActionServer::fill_feedback_joints(DualArm::Feedback & feedbac
}
}
void DualArmMoveitActionServer::publish_feedback_once(
const std::shared_ptr<GoalHandleDualArm> & goal_handle)
{
if (!goal_handle) {
return;
}
std::shared_ptr<DualArm::Feedback> feedback_copy;
{
std::lock_guard<std::mutex> lock(feedback_mutex_);
if (!feedback_state_) {
return;
}
feedback_copy = std::make_shared<DualArm::Feedback>(*feedback_state_);
}
fill_feedback_joints(*feedback_copy);
goal_handle->publish_feedback(feedback_copy);
}
void DualArmMoveitActionServer::update_feedback(uint8_t status, double progress)
{
std::lock_guard<std::mutex> lock(feedback_mutex_);
if (!feedback_state_) {
feedback_state_ = std::make_shared<DualArm::Feedback>();
}
feedback_state_->status = status;
feedback_state_->progress = progress;
}
} // namespace dual_arm_moveit_action_server
int main(int argc, char ** argv)

3461
dual_arm_moveit_action_server/urdf/FHrobot.urdf → src/dual_arm_moveit_action_server/urdf/FHrobot.urdf
View File

File diff suppressed because it is too large Load Diff

49
src/dual_arm_remote_teleop/CMakeLists.txt Normal file
View File

@@ -0,0 +1,49 @@
cmake_minimum_required(VERSION 3.8)
project(dual_arm_remote_teleop)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
include_directories(
include
${CMAKE_CURRENT_SOURCE_DIR}/../../include
)
add_executable(dual_arm_remote_teleop_node
src/remote_teleop_node.cpp
src/real_robot_driver.cpp
)
ament_target_dependencies(dual_arm_remote_teleop_node
rclcpp
interfaces
geometry_msgs
tf2
)
find_library(RMAN_API_LIB NAMES api_cpp PATHS "${CMAKE_CURRENT_SOURCE_DIR}/../../lib")
target_link_libraries(dual_arm_remote_teleop_node ${RMAN_API_LIB})
install(TARGETS
dual_arm_remote_teleop_node
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY include/
DESTINATION include/${PROJECT_NAME}
)
install(
DIRECTORY launch config
DESTINATION share/${PROJECT_NAME}
OPTIONAL
)
ament_package()

20
src/dual_arm_remote_teleop/config/dual_arm_remote_teleop.yaml Normal file
View File

@@ -0,0 +1,20 @@
dual_arm_remote_teleop:
ros__parameters:
left_arm_ip: "192.168.2.2"
right_arm_ip: "192.168.2.18"
arm_port: 8080
left_push_ip: "192.168.2.3"
right_push_ip: "192.168.2.19"
push_port: 8089
push_cycle: 20
meta_topic: "/meta_key"
command_rate_hz: 250.0
interpolation_delay_ms: 30
smooth_alpha: 0.3
stale_timeout_ms: 500
require_trigger: false
trigger_threshold: 1
canfd_follow: true
canfd_trajectory_mode: 2
canfd_smooth_ratio: 200

68
src/dual_arm_remote_teleop/include/dual_arm_remote_teleop/real_robot_driver.hpp Normal file
View File

@@ -0,0 +1,68 @@
#pragma once
#include "rm_define.h"
#include "rm_service.h"
#include <rclcpp/rclcpp.hpp>
#include <atomic>
#include <mutex>
#include <string>
namespace dual_arm_remote_teleop
{
class RealRobotDriver
{
public:
RealRobotDriver();
~RealRobotDriver();
bool initialize(
const rclcpp::Logger & logger,
const std::string & left_ip,
const std::string & right_ip,
int arm_port,
const std::string & left_push_ip,
const std::string & right_push_ip,
int push_port,
int push_cycle);
void shutdown();
bool is_connected() const;
bool send_pose_canfd(
uint8_t arm_id,
const rm_pose_t & pose,
bool follow,
int trajectory_mode,
int smooth_ratio,
std::string * error_msg);
void stop_arm(uint8_t arm_id);
void stop_all();
private:
static void realtime_state_callback(rm_realtime_arm_joint_state_t data);
static void api_log_callback(const char * message, va_list args);
bool init_handle(
rm_robot_handle ** handle,
const std::string & ip,
int port,
const std::string & push_ip,
int push_port,
int push_cycle);
rm_robot_handle * handle_for(uint8_t arm_id) const;
static int clamp_smooth_ratio(int smooth_ratio);
static RealRobotDriver * instance_;
rclcpp::Logger logger_;
std::unique_ptr<RM_Service> rm_service_;
rm_robot_handle * left_handle_;
rm_robot_handle * right_handle_;
std::atomic<bool> connected_;
std::string left_ip_;
std::string right_ip_;
std::mutex api_mutex_;
};
} // namespace dual_arm_remote_teleop

23
src/dual_arm_remote_teleop/launch/dual_arm_remote_teleop.launch.py Normal file
View File

@@ -0,0 +1,23 @@
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
config_path = os.path.join(
get_package_share_directory("dual_arm_remote_teleop"),
"config",
"dual_arm_remote_teleop.yaml",
)
return LaunchDescription([
Node(
package="dual_arm_remote_teleop",
executable="dual_arm_remote_teleop_node",
name="dual_arm_remote_teleop",
output="screen",
parameters=[config_path],
)
])

23
src/dual_arm_remote_teleop/package.xml Normal file
View File

@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<package format="3">
<name>dual_arm_remote_teleop</name>
<version>0.1.0</version>
<description>Remote teleoperation node for real dual-arm robot using RM API</description>
<maintainer email="dev@hivecore.ai">hivecore</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>interfaces</depend>
<depend>geometry_msgs</depend>
<depend>tf2</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

217
src/dual_arm_remote_teleop/src/real_robot_driver.cpp Normal file
View File

@@ -0,0 +1,217 @@
#include "dual_arm_remote_teleop/real_robot_driver.hpp"
#include <algorithm>
#include <cstdarg>
#include <cstdio>
#include <cstring>
namespace dual_arm_remote_teleop
{
RealRobotDriver * RealRobotDriver::instance_ = nullptr;
RealRobotDriver::RealRobotDriver()
: logger_(rclcpp::get_logger("dual_arm_remote_teleop")),
left_handle_(nullptr),
right_handle_(nullptr),
connected_(false)
{
}
RealRobotDriver::~RealRobotDriver()
{
shutdown();
}
bool RealRobotDriver::initialize(
const rclcpp::Logger & logger,
const std::string & left_ip,
const std::string & right_ip,
int arm_port,
const std::string & left_push_ip,
const std::string & right_push_ip,
int push_port,
int push_cycle)
{
logger_ = logger;
left_ip_ = left_ip;
right_ip_ = right_ip;
rm_service_ = std::make_unique<RM_Service>();
if (!rm_service_) {
RCLCPP_ERROR(logger_, "Failed to allocate RM_Service");
return false;
}
if (rm_service_->rm_init(RM_TRIPLE_MODE_E) != 0) {
RCLCPP_ERROR(logger_, "rm_init failed");
rm_service_.reset();
return false;
}
instance_ = this;
rm_service_->rm_set_log_call_back(api_log_callback, 3);
rm_service_->rm_realtime_arm_state_call_back(realtime_state_callback);
if (!init_handle(&left_handle_, left_ip, arm_port, left_push_ip, push_port, push_cycle)) {
shutdown();
return false;
}
if (!init_handle(&right_handle_, right_ip, arm_port, right_push_ip, push_port, push_cycle)) {
shutdown();
return false;
}
connected_.store(true);
return true;
}
void RealRobotDriver::shutdown()
{
connected_.store(false);
if (rm_service_) {
rm_service_->rm_destroy();
rm_service_.reset();
}
left_handle_ = nullptr;
right_handle_ = nullptr;
instance_ = nullptr;
}
bool RealRobotDriver::is_connected() const
{
return connected_.load();
}
bool RealRobotDriver::send_pose_canfd(
uint8_t arm_id,
const rm_pose_t & pose,
bool follow,
int trajectory_mode,
int smooth_ratio,
std::string * error_msg)
{
std::lock_guard<std::mutex> lock(api_mutex_);
rm_robot_handle * handle = handle_for(arm_id);
if (!handle) {
if (error_msg) {
*error_msg = "Invalid arm_id or missing robot handle";
}
return false;
}
const int ratio = clamp_smooth_ratio(smooth_ratio);
const int result = rm_service_->rm_movep_canfd(
handle,
pose,
follow,
trajectory_mode,
ratio);
if (result != 0) {
if (error_msg) {
*error_msg = "rm_movep_canfd failed with code " + std::to_string(result);
}
return false;
}
return true;
}
void RealRobotDriver::stop_arm(uint8_t arm_id)
{
std::lock_guard<std::mutex> lock(api_mutex_);
rm_robot_handle * handle = handle_for(arm_id);
if (handle && rm_service_) {
rm_service_->rm_set_arm_stop(handle);
}
}
void RealRobotDriver::stop_all()
{
stop_arm(0);
stop_arm(1);
}
void RealRobotDriver::realtime_state_callback(rm_realtime_arm_joint_state_t data)
{
(void)data;
}
void RealRobotDriver::api_log_callback(const char * message, va_list args)
{
if (!instance_ || !message) {
return;
}
char buffer[1024];
vsnprintf(buffer, sizeof(buffer), message, args);
RCLCPP_INFO(instance_->logger_, "%s", buffer);
}
bool RealRobotDriver::init_handle(
rm_robot_handle ** handle,
const std::string & ip,
int port,
const std::string & push_ip,
int push_port,
int push_cycle)
{
if (!rm_service_) {
return false;
}
*handle = rm_service_->rm_create_robot_arm(ip.c_str(), port);
if (*handle == nullptr || (*handle)->id <= 0) {
RCLCPP_ERROR(logger_, "Failed to create handle for arm %s", ip.c_str());
return false;
}
rm_realtime_push_config_t config{};
config.cycle = push_cycle;
config.enable = true;
config.port = push_port;
config.force_coordinate = 0;
std::snprintf(config.ip, sizeof(config.ip), "%s", push_ip.c_str());
const int push_result = rm_service_->rm_set_realtime_push(*handle, config);
if (push_result != 0) {
RCLCPP_ERROR(
logger_,
"Failed to set realtime push for arm %s (code %d)",
ip.c_str(),
push_result);
return false;
}
rm_robot_info_t robot_info{};
const int info_result = rm_service_->rm_get_robot_info(*handle, &robot_info);
if (info_result != 0 || robot_info.arm_dof <= 0) {
RCLCPP_ERROR(
logger_,
"Invalid robot info for arm %s (code %d, dof %d)",
ip.c_str(),
info_result,
robot_info.arm_dof);
return false;
}
return true;
}
rm_robot_handle * RealRobotDriver::handle_for(uint8_t arm_id) const
{
if (arm_id == 0) {
return left_handle_;
}
if (arm_id == 1) {
return right_handle_;
}
return nullptr;
}
int RealRobotDriver::clamp_smooth_ratio(int smooth_ratio)
{
return std::clamp(smooth_ratio, 0, 1000);
}
} // namespace dual_arm_remote_teleop

358
src/dual_arm_remote_teleop/src/remote_teleop_node.cpp Normal file
View File

@@ -0,0 +1,358 @@
#include "dual_arm_remote_teleop/real_robot_driver.hpp"
#include "interfaces/msg/meta_key.hpp"
#include <geometry_msgs/msg/pose.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Matrix3x3.hpp>
#include <tf2/LinearMath/Quaternion.hpp>
#include <algorithm>
#include <chrono>
#include <deque>
#include <string>
namespace dual_arm_remote_teleop
{
namespace
{
constexpr uint8_t kLeftArmId = 0;
constexpr uint8_t kRightArmId = 1;
} // namespace
class RemoteTeleopNode : public rclcpp::Node
{
public:
RemoteTeleopNode()
: rclcpp::Node("dual_arm_remote_teleop")
{
declare_parameter<std::string>("left_arm_ip");
declare_parameter<std::string>("right_arm_ip");
declare_parameter<int>("arm_port");
declare_parameter<std::string>("left_push_ip");
declare_parameter<std::string>("right_push_ip");
declare_parameter<int>("push_port");
declare_parameter<int>("push_cycle");
declare_parameter<std::string>("meta_topic", "/meta_key");
declare_parameter<double>("command_rate_hz", 250.0);
declare_parameter<int>("interpolation_delay_ms", 30);
declare_parameter<double>("smooth_alpha", 0.3);
declare_parameter<int>("stale_timeout_ms", 500);
declare_parameter<bool>("require_trigger", false);
declare_parameter<int>("trigger_threshold", 1);
declare_parameter<bool>("canfd_follow", true);
declare_parameter<int>("canfd_trajectory_mode", 2);
declare_parameter<int>("canfd_smooth_ratio", 200);
load_parameters();
if (!driver_.initialize(
get_logger(),
left_arm_ip_,
right_arm_ip_,
arm_port_,
left_push_ip_,
right_push_ip_,
push_port_,
push_cycle_))
{
RCLCPP_ERROR(get_logger(), "Failed to initialize RM driver");
}
meta_sub_ = create_subscription<interfaces::msg::MetaKey>(
meta_topic_,
10,
std::bind(&RemoteTeleopNode::on_meta_key, this, std::placeholders::_1));
const auto period = std::chrono::duration<double>(1.0 / command_rate_hz_);
timer_ = create_wall_timer(
std::chrono::duration_cast<std::chrono::nanoseconds>(period),
std::bind(&RemoteTeleopNode::on_timer, this));
}
private:
struct TeleopState {
struct Sample {
geometry_msgs::msg::Pose pose;
rclcpp::Time stamp;
};
std::deque<Sample> samples;
geometry_msgs::msg::Pose pose;
rclcpp::Time stamp;
bool active{false};
bool has_pose{false};
geometry_msgs::msg::Pose smoothed_pose;
bool has_smoothed{false};
};
void load_parameters()
{
if (!get_parameter("left_arm_ip", left_arm_ip_) ||
!get_parameter("right_arm_ip", right_arm_ip_) ||
!get_parameter("arm_port", arm_port_) ||
!get_parameter("left_push_ip", left_push_ip_) ||
!get_parameter("right_push_ip", right_push_ip_) ||
!get_parameter("push_port", push_port_) ||
!get_parameter("push_cycle", push_cycle_))
{
RCLCPP_ERROR(get_logger(),
"Missing/invalid parameters. Please load config/dual_arm_remote_teleop.yaml");
}
get_parameter("meta_topic", meta_topic_);
get_parameter("command_rate_hz", command_rate_hz_);
get_parameter("interpolation_delay_ms", interpolation_delay_ms_);
get_parameter("smooth_alpha", smooth_alpha_);
get_parameter("stale_timeout_ms", stale_timeout_ms_);
get_parameter("require_trigger", require_trigger_);
get_parameter("trigger_threshold", trigger_threshold_);
get_parameter("canfd_follow", canfd_follow_);
get_parameter("canfd_trajectory_mode", canfd_trajectory_mode_);
get_parameter("canfd_smooth_ratio", canfd_smooth_ratio_);
if (command_rate_hz_ <= 0.0) {
command_rate_hz_ = 10.0;
}
if (smooth_alpha_ <= 0.0) {
smooth_alpha_ = 0.1;
} else if (smooth_alpha_ > 1.0) {
smooth_alpha_ = 1.0;
}
}
void on_meta_key(const interfaces::msg::MetaKey::SharedPtr msg)
{
const bool active = !require_trigger_ || msg->trigger >= trigger_threshold_;
if (msg->type == "left") {
update_state(left_state_, msg->pose, active);
return;
}
if (msg->type == "right") {
update_state(right_state_, msg->pose, active);
return;
}
}
void update_state(TeleopState & state, const geometry_msgs::msg::Pose & pose, bool active)
{
state.active = active;
state.pose = pose;
state.stamp = now();
state.has_pose = true;
state.samples.push_back({pose, state.stamp});
while (state.samples.size() > 4) {
state.samples.pop_front();
}
}
void on_timer()
{
if (!driver_.is_connected()) {
return;
}
const auto now_time = now();
const auto timeout = rclcpp::Duration::from_nanoseconds(
static_cast<int64_t>(stale_timeout_ms_) * 1000000LL);
const auto target_time = now_time - rclcpp::Duration::from_nanoseconds(
static_cast<int64_t>(interpolation_delay_ms_) * 1000000LL);
send_if_ready(kLeftArmId, left_state_, now_time, timeout, target_time);
send_if_ready(kRightArmId, right_state_, now_time, timeout, target_time);
}
void send_if_ready(
uint8_t arm_id,
TeleopState & state,
const rclcpp::Time & now_time,
const rclcpp::Duration & timeout,
const rclcpp::Time & target_time)
{
if (!state.has_pose || !state.active) {
return;
}
if ((now_time - state.stamp) > timeout) {
return;
}
geometry_msgs::msg::Pose target_pose;
if (!interpolate_pose(state, target_time, &target_pose)) {
return;
}
geometry_msgs::msg::Pose smoothed = apply_smoothing(state, target_pose);
state.smoothed_pose = smoothed;
state.has_smoothed = true;
rm_pose_t target = to_rm_pose(smoothed);
std::string error_msg;
if (!driver_.send_pose_canfd(
arm_id,
target,
canfd_follow_,
canfd_trajectory_mode_,
canfd_smooth_ratio_,
&error_msg))
{
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "Teleop send failed: %s",
error_msg.c_str());
}
}
bool interpolate_pose(
const TeleopState & state,
const rclcpp::Time & target_time,
geometry_msgs::msg::Pose * out) const
{
if (state.samples.empty()) {
return false;
}
const auto & first = state.samples.front();
const auto & last = state.samples.back();
if (target_time <= first.stamp || state.samples.size() == 1) {
*out = first.pose;
return true;
}
if (target_time >= last.stamp) {
*out = last.pose;
return true;
}
for (size_t i = 1; i < state.samples.size(); ++i) {
const auto & prev = state.samples[i - 1];
const auto & next = state.samples[i];
if (target_time <= next.stamp) {
const double dt = (next.stamp - prev.stamp).seconds();
double t = 0.0;
if (dt > 1e-6) {
t = (target_time - prev.stamp).seconds() / dt;
}
t = std::clamp(t, 0.0, 1.0);
out->position.x = prev.pose.position.x + (next.pose.position.x - prev.pose.position.x) * t;
out->position.y = prev.pose.position.y + (next.pose.position.y - prev.pose.position.y) * t;
out->position.z = prev.pose.position.z + (next.pose.position.z - prev.pose.position.z) * t;
tf2::Quaternion q_prev(
prev.pose.orientation.x,
prev.pose.orientation.y,
prev.pose.orientation.z,
prev.pose.orientation.w);
tf2::Quaternion q_next(
next.pose.orientation.x,
next.pose.orientation.y,
next.pose.orientation.z,
next.pose.orientation.w);
tf2::Quaternion q_interp = q_prev.slerp(q_next, t);
q_interp.normalize();
out->orientation.x = q_interp.x();
out->orientation.y = q_interp.y();
out->orientation.z = q_interp.z();
out->orientation.w = q_interp.w();
return true;
}
}
*out = last.pose;
return true;
}
geometry_msgs::msg::Pose apply_smoothing(
const TeleopState & state,
const geometry_msgs::msg::Pose & input) const
{
if (!state.has_smoothed) {
return input;
}
geometry_msgs::msg::Pose output;
const double a = smooth_alpha_;
output.position.x = state.smoothed_pose.position.x +
(input.position.x - state.smoothed_pose.position.x) * a;
output.position.y = state.smoothed_pose.position.y +
(input.position.y - state.smoothed_pose.position.y) * a;
output.position.z = state.smoothed_pose.position.z +
(input.position.z - state.smoothed_pose.position.z) * a;
tf2::Quaternion q_prev(
state.smoothed_pose.orientation.x,
state.smoothed_pose.orientation.y,
state.smoothed_pose.orientation.z,
state.smoothed_pose.orientation.w);
tf2::Quaternion q_input(
input.orientation.x,
input.orientation.y,
input.orientation.z,
input.orientation.w);
tf2::Quaternion q_out = q_prev.slerp(q_input, a);
q_out.normalize();
output.orientation.x = q_out.x();
output.orientation.y = q_out.y();
output.orientation.z = q_out.z();
output.orientation.w = q_out.w();
return output;
}
rm_pose_t to_rm_pose(const geometry_msgs::msg::Pose & pose) const
{
rm_pose_t target{};
target.position.x = static_cast<float>(pose.position.x);
target.position.y = static_cast<float>(pose.position.y);
target.position.z = static_cast<float>(pose.position.z);
target.quaternion.w = static_cast<float>(pose.orientation.w);
target.quaternion.x = static_cast<float>(pose.orientation.x);
target.quaternion.y = static_cast<float>(pose.orientation.y);
target.quaternion.z = static_cast<float>(pose.orientation.z);
tf2::Quaternion q(
target.quaternion.x,
target.quaternion.y,
target.quaternion.z,
target.quaternion.w);
tf2::Matrix3x3 m(q);
double roll = 0.0;
double pitch = 0.0;
double yaw = 0.0;
m.getRPY(roll, pitch, yaw);
target.euler.rx = static_cast<float>(roll);
target.euler.ry = static_cast<float>(pitch);
target.euler.rz = static_cast<float>(yaw);
return target;
}
RealRobotDriver driver_;
rclcpp::Subscription<interfaces::msg::MetaKey>::SharedPtr meta_sub_;
rclcpp::TimerBase::SharedPtr timer_;
TeleopState left_state_;
TeleopState right_state_;
std::string left_arm_ip_;
std::string right_arm_ip_;
int arm_port_{0};
std::string left_push_ip_;
std::string right_push_ip_;
int push_port_{0};
int push_cycle_{0};
std::string meta_topic_;
double command_rate_hz_{10.0};
int interpolation_delay_ms_{30};
double smooth_alpha_{0.3};
int stale_timeout_ms_{500};
bool require_trigger_{false};
int trigger_threshold_{1};
bool canfd_follow_{true};
int canfd_trajectory_mode_{2};
int canfd_smooth_ratio_{200};
};
} // namespace dual_arm_remote_teleop
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<dual_arm_remote_teleop::RemoteTeleopNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

0
gjk_interface/CMakeLists.txt → src/gjk_interface/CMakeLists.txt
View File

0
arm_keyboard_input/LICENSE → src/gjk_interface/LICENSE
View File

0
gjk_interface/include/gjk_interface.hpp → src/gjk_interface/include/gjk_interface.hpp
View File

0
gjk_interface/package.xml → src/gjk_interface/package.xml
View File

0
gjk_interface/src/gjk_interface.cpp → src/gjk_interface/src/gjk_interface.cpp
View File

2
rm_arm_control/CMakeLists.txt → src/rm_arm_control/CMakeLists.txt
View File

@@ -57,7 +57,7 @@ pkg_check_modules(TINYXML2 REQUIRED tinyxml2)
include_directories(
include
${CMAKE_CURRENT_SOURCE_DIR}/../include
${CMAKE_CURRENT_SOURCE_DIR}/../../include
${EIGEN3_INCLUDE_DIRS} # 引入Eigen3的头文件路径
${TINYXML2_INCLUDE_DIR}
)

0
rm_arm_control/LICENSE → src/rm_arm_control/LICENSE
View File

0
rm_arm_control/Robotic_Arm/lib/api_cpp.dll → src/rm_arm_control/Robotic_Arm/lib/api_cpp.dll
View File

0
rm_arm_control/Robotic_Arm/lib/api_cpp.lib → src/rm_arm_control/Robotic_Arm/lib/api_cpp.lib
View File

0
rm_arm_control/Robotic_Arm/lib/libapi_cpp.so → src/rm_arm_control/Robotic_Arm/lib/libapi_cpp.so
View File

0
rm_arm_control/build/CMakeCache.txt → src/rm_arm_control/build/CMakeCache.txt
View File

0
rm_arm_control/build/CMakeFiles/3.22.1/CMakeCCompiler.cmake → src/rm_arm_control/build/CMakeFiles/3.22.1/CMakeCCompiler.cmake
View File

Some files were not shown because too many files have changed in this diff Show More