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

Compare commits

merge into: HiveCoreRD:Dev_Ray
Branches Tags
HiveCoreRD:master HiveCoreRD:zhengjie_arm_branch HiveCoreRD:lyx_dev HiveCoreRD:Dev_Ray HiveCoreRD:develop
...
pull from: HiveCoreRD:zhengjie_arm_branch
Branches Tags
HiveCoreRD:zhengjie_arm_branch HiveCoreRD:lyx_dev HiveCoreRD:Dev_Ray HiveCoreRD:develop HiveCoreRD:master

9 Commits
Dev_Ray ... zhengjie_a

Author SHA1 Message Date
zj
695c86384f 运动学规划解耦合 2025-10-16 09:59:42 +08:00
zj
2f2d8e1490 关闭碰撞检测 2025-10-14 19:42:21 +08:00
zj
97523755b4 指定方向移动到极限位置不报错 2025-10-14 19:10:52 +08:00
zj
6be64b3229 修正0位移 2025-10-14 19:03:44 +08:00
zj
9302408945 解耦合初始化 2025-10-14 18:25:45 +08:00
zj
9e71b2c3b0 虚拟机版本 2025-10-14 17:43:11 +08:00
zj
166222c4ab 依赖倒置 2025-10-14 10:22:20 +08:00
zj
8fc6208dbe 调试版本 2025-10-11 15:33:17 +08:00
zj
aae6e279ef 轨迹规划更新 2025-09-30 09:41:29 +08:00
51 changed files with 3738 additions and 2388 deletions
Expand all files Collapse all files

3
.gitmodules vendored Normal file
View File

@@ -0,0 +1,3 @@
[submodule "HiveCoreR1/src/hivecore_robot_interfaces"]
path = HiveCoreR1/src/hivecore_robot_interfaces
url = http://192.168.10.50:3000/HiveCoreRD/hivecore_robot_interfaces.git

2
HiveCoreR0/src/robot_vision/realsense2_camera/tools/frame_latency/frame_latency.cpp
View File

@@ -68,7 +68,7 @@ void FrameLatencyNode::createTFListener(std::string topicName, const rmw_qos_pro
rclcpp::QoS( rclcpp::QoSInitialization::from_rmw( qos_profile ),
qos_profile ),
[&, this]( const std::shared_ptr<tf2_msgs::msg::TFMessage> msg ) {
rclcpp::Time curr_time = this->get_clock()->now();
rclcpp::Time curr_time = this->()->now();
auto latency = ( curr_time - msg->transforms.back().header.stamp ).seconds();
ROS_INFO_STREAM( "Got msg with "<< msg->transforms.back().header.frame_id <<" frame id at address 0x"
<< std::hex << reinterpret_cast< std::uintptr_t >( msg.get() )

5
HiveCoreR1/.vscode/settings.json vendored
View File

@@ -82,6 +82,9 @@
"typeindex": "cpp",
"typeinfo": "cpp",
"valarray": "cpp",
"variant": "cpp"
"variant": "cpp",
"charconv": "cpp",
"slist": "cpp",
"ranges": "cpp"
}
}

41
HiveCoreR1/include/arm_define.h
View File

@@ -2,8 +2,23 @@
#define ARM_DEFINE_H
#define USED_ARM_DOF (6)
#define GOAL_DATA_LENGTH (7)
#define MAX_ARM_GOAL_COUNT 16
#define MAX_TRAJECTORY_HISTORY 64
#define ARM_FOLLOWING_PERIOD (5e6) //ns == 5ms
#define ARM_FOLLOWING_CHECKING_STEP (4)
#define ARM_FOLLOWING_CHECKING (ARM_FOLLOWING_PERIOD * ARM_FOLLOWING_CHECKING_STEP) //ns == 20ms
#define GET_FUNC_LINE_STR() \
(std::string("[") + __func__ + ":" + std::to_string(__LINE__) + "]")
#define LEFT_ARM_IP_ADDRESS "192.168.2.2"
#define RIGHT_ARM_IP_ADDRESS "192.168.2.18"
#define ARM_IP_PORT (8080)
#define TRAJECTORY_COMPLETE 1
#define TRAJECTORY_ONGOING 0
#define TRAJECTORY_ERROR -1
typedef enum {
ARM_COMMAND_TYPE_ANGLE_STEP_ON = 0,
@@ -17,10 +32,11 @@ typedef enum {
POSE_POSITION_X = 0,
POSE_POSITION_Y,
POSE_POSITION_Z,
POSE_EULER_ROLL,
POSE_EULER_PITCH,
POSE_EULER_YAW,
POSE_DIMENSION, // should be 6
POSE_QUATERNION_X,
POSE_QUATERNION_Y,
POSE_QUATERNION_Z,
POSE_QUATERNION_W,
POSE_DIMENSION, // should be 7
} PoseDimensionE;
typedef enum {
@@ -29,4 +45,21 @@ typedef enum {
ARM_ID_ERR
} ArmIdE;
typedef enum {
GOAL_TYPE_MOVING = 0,
GOAL_TYPE_STEPPING,
GOAL_TYPE_POSE_STEPPING,
GOAL_TYPE_ERROR
} GoalTypeE;
typedef enum {
OK = 0,
UNKNOWN_ERR = -1,
ARM_NOW_FORCE_MOVING = -2,
ARM_COLLISION = -3,
ARM_AIM_CANNOT_REACH = -4,
ARM_NOW_NO_GOAL = -5,
ARM_GOAL_CANCELLED = -6,
} ErrCodeE;
#endif // ARM_DEFINE_H

18
HiveCoreR1/src/arm_description/CMakeLists.txt
View File

@@ -1,18 +0,0 @@
cmake_minimum_required(VERSION 3.10)
project(arm_description)
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(urdf REQUIRED)
find_package(xacro REQUIRED)
install(
DIRECTORY launch urdf meshes # 安装 launch、urdf、meshes 目录
DESTINATION share/${PROJECT_NAME}
)
ament_package()

92
HiveCoreR1/src/arm_description/launch/display_robot.launch.py
View File

@@ -1,92 +0,0 @@
# 导入 ROS 2 launch 相关模块
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch.actions import OpaqueFunction
import os
'''
def get_new_body_part(context, part_name):
# 这里可以根据需要返回不同的部件配置
package_share = FindPackageShare(package='arm_description').perform(context)
# 构建URDF文件的实际路径
urdf_path = PathJoinSubstitution(
[package_share, 'urdf', part_name + '.urdf'] # 替换为你的URDF路径
).perform(context)
# 读取URDF文件内容
with open(urdf_path, 'r') as f:
urdf_content = f.read()
# 创建机器人状态发布器节点
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name=part_name + '_state_publisher',
parameters=[{part_name + '_description': urdf_content}],
remappings=[('/joint_states', '/' + part_name + '_joint_states')]
)
return robot_state_publisher
'''
def load_urdf(context, *args, **kwargs):
nodeList = []
# 获取包的共享目录路径（解析为实际字符串）
package_share = FindPackageShare(package='arm_description').perform(context)
# 构建URDF文件的实际路径
urdf_path = PathJoinSubstitution(
[package_share, 'urdf', 'rm_65_b_description.urdf'] # 替换为你的URDF路径
).perform(context)
# 读取URDF文件内容
with open(urdf_path, 'r') as f:
urdf_content = f.read()
# 创建机器人状态发布器节点
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': urdf_content}]
)
nodeList.append(robot_state_publisher)
'''
# nodeList.append(get_new_body_part(context, 'rm_65_b'))
nodeList.append(get_new_body_part(context, 'robot'))
nodeList.append(get_new_body_part(context, 'leftarm'))
nodeList.append(get_new_body_part(context, 'rightarm'))
'''
'''joint_state_gui = Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
)
nodeList.append(joint_state_gui)'''
# 创建RViz节点（可选）
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=[
'-d', os.path.join(
FindPackageShare('arm_description').find('arm_description'),
'launch', 'robot_view.rviz' # 保存的RViz配置文件路径
)
],
parameters=[{
# 这里可以设置一些RViz参数
}]
)
nodeList.append(rviz_node)
return nodeList
def generate_launch_description():
return LaunchDescription([
OpaqueFunction(function=load_urdf)
])

20
HiveCoreR1/src/arm_description/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_description</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="zj@todo.todo">zj</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>urdf</depend>
<depend>xacro</depend>
<depend>robot_state_publisher</depend>
<depend>rviz2</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

827
HiveCoreR1/src/arm_description/urdf/rm_65_b_description copy.urdf
View File

@@ -1,827 +0,0 @@
<?xml version="1.0" encoding="utf-8"?>
<robot
name="arm_description">
<link name="world" />
<link
name="base_link">
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<box size="0.251 0.1 1.0"/>
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<box size="0.251 0.1 1.0"/>
</geometry>
</collision>
</link>
<joint name="world_to_base" type="fixed">
<parent link="world"/>
<child link="base_link"/>
</joint>
<link
name="base_link_leftarm">
<inertial>
<origin
xyz="-0.000433277303987328 -3.54664423471128E-05 0.0599427668933796"
rpy="0 0 0" />
<mass
value="0.841070778135659" />
<inertia
ixx="0.0017261110801622"
ixy="2.52746264980217E-06"
ixz="-3.67690303614961E-05"
iyy="0.00170987405835604"
iyz="1.67996364994424E-06"
izz="0.000904023422915791" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/base_link.STL" />
</geometry>
</collision>
</link>
<joint name="left_arm_joint" type="fixed">
<origin
xyz="0.1255 0 0.42"
rpy="0 1.5708 -1.5708" />
<parent
link="base_link" />
<child
link="base_link_leftarm" />
</joint>
<link
name="LeftLink1">
<inertial>
<origin
xyz="1.2226305431569E-08 0.0211079974844683 -0.0251854220842269"
rpy="0 0 0" />
<mass
value="0.593563443690403" />
<inertia
ixx="0.00126614120341847"
ixy="-1.294980943835E-08"
ixz="-9.80120923066996E-09"
iyy="0.00118168178300364"
iyz="-0.00021121727444415"
izz="0.00056135241627747" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link1.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link1.STL" />
</geometry>
</collision>
</link>
<joint
name="left_joint1"
type="revolute">
<origin
xyz="0 0 0.2405"
rpy="0 0 0" />
<parent
link="base_link_leftarm" />
<child
link="LeftLink1" />
<axis
xyz="0 0 1" />
<limit
lower="-3.1"
upper="3.1"
effort="60"
velocity="3.14" />
</joint>
<link
name="LeftLink2">
<inertial>
<origin
xyz="0.152256463426163 4.75383656106654E-07 -0.00620260383607792"
rpy="0 0 0" />
<mass
value="0.864175046869043" />
<inertia
ixx="0.00089150298478414"
ixy="-2.23268489334765E-08"
ixz="0.00156246461035015"
iyy="0.00733754343083901"
iyz="6.28110889329165E-09"
izz="0.00697869103915473" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link2.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link2.STL" />
</geometry>
</collision>
</link>
<joint
name="left_joint2"
type="revolute">
<origin
xyz="0 0 0"
rpy="1.5708 -1.5708 0" />
<parent
link="LeftLink1" />
<child
link="LeftLink2" />
<axis
xyz="0 0 1" />
<limit
lower="-2.268"
upper="2.268"
effort="60"
velocity="3.14" />
</joint>
<link
name="LeftLink3">
<inertial>
<origin
xyz="5.05312670989961E-06 -0.0595925663694732 0.010569069212336"
rpy="0 0 0" />
<mass
value="0.289633681624654" />
<inertia
ixx="0.00063737100450158"
ixy="-7.05261293649751E-08"
ixz="-3.86643272239426E-08"
iyy="0.00015648388095025"
iyz="-0.00014461035994916"
izz="0.000614178164773085" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link3.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link3.STL" />
</geometry>
</collision>
</link>
<joint
name="left_joint3"
type="revolute">
<origin
xyz="0.256 0 0"
rpy="0 0 1.5708" />
<parent
link="LeftLink2" />
<child
link="LeftLink3" />
<axis
xyz="0 0 1" />
<limit
lower="-2.355"
upper="2.355"
effort="30"
velocity="3.92" />
</joint>
<link
name="LeftLink4">
<inertial>
<origin
xyz="1.15516617405898E-06 -0.0180424468598241 -0.0215394748352687"
rpy="0 0 0" />
<mass
value="0.239419768320061" />
<inertia
ixx="0.000285938919722783"
ixy="3.07101359163101E-09"
ixz="-2.21994118981953E-09"
iyy="0.000262727540304212"
iyz="4.4236583260078E-05"
izz="0.000119888082791859" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link4.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link4.STL" />
</geometry>
</collision>
</link>
<joint
name="left_joint4"
type="revolute">
<origin
xyz="0 -0.21 0"
rpy="1.5708 0 0" />
<parent
link="LeftLink3" />
<child
link="LeftLink4" />
<axis
xyz="0 0 1" />
<limit
lower="-3.1"
upper="3.1"
effort="10"
velocity="3.92" />
</joint>
<link
name="LeftLink5">
<inertial>
<origin
xyz="3.19794786262152E-06 -0.0593808368101458 0.00736804250989326"
rpy="0 0 0" />
<mass
value="0.218799761431678" />
<inertia
ixx="0.000350540363914072"
ixy="-3.41781619975602E-08"
ixz="-1.77056457224373E-08"
iyy="0.000104927867487581"
iyz="-7.82431228461971E-05"
izz="0.000334482418423629" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link5.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link5.STL" />
</geometry>
</collision>
</link>
<joint
name="left_joint5"
type="revolute">
<origin
xyz="0 0 0"
rpy="-1.5708 0 0" />
<parent
link="LeftLink4" />
<child
link="LeftLink5" />
<axis
xyz="0 0 1" />
<limit
lower="-2.233"
upper="2.233"
effort="10"
velocity="3.92" />
</joint>
<link
name="LeftLink6">
<inertial>
<origin
xyz="0.000714234511756999 -0.000396718732824521 -0.0126723660946126"
rpy="0 0 0" />
<mass
value="0.0649018034311231" />
<inertia
ixx="2.02766547502765E-05"
ixy="-1.32505200276849E-06"
ixz="-2.5845091522508E-08"
iyy="1.87986725225022E-05"
iyz="3.39471452125439E-09"
izz="3.17885459163081E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link6.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link6.STL" />
</geometry>
</collision>
</link>
<joint
name="left_joint6"
type="revolute">
<origin
xyz="0 -0.144 0"
rpy="1.5708 0 0" />
<parent
link="LeftLink5" />
<child
link="LeftLink6" />
<axis
xyz="0 0 1" />
<limit
lower="-6.28"
upper="6.28"
effort="10"
velocity="3.92" />
</joint>
<link
name="base_link_rightarm">
<inertial>
<origin
xyz="-0.000433277303987328 -3.54664423471128E-05 0.0599427668933796"
rpy="0 0 0" />
<mass
value="0.841070778135659" />
<inertia
ixx="0.0017261110801622"
ixy="2.52746264980217E-06"
ixz="-3.67690303614961E-05"
iyy="0.00170987405835604"
iyz="1.67996364994424E-06"
izz="0.000904023422915791" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/base_link.STL" />
</geometry>
</collision>
</link>
<joint name="right_arm_joint" type="fixed">
<origin
xyz="-0.1255 0 0.42"
rpy="0 -1.5708 -1.5708" />
<parent
link="base_link" />
<child
link="base_link_rightarm" />
</joint>
<link
name="RightLink1">
<inertial>
<origin
xyz="1.2226305431569E-08 0.0211079974844683 -0.0251854220842269"
rpy="0 0 0" />
<mass
value="0.593563443690403" />
<inertia
ixx="0.00126614120341847"
ixy="-1.294980943835E-08"
ixz="-9.80120923066996E-09"
iyy="0.00118168178300364"
iyz="-0.00021121727444415"
izz="0.00056135241627747" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link1.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link1.STL" />
</geometry>
</collision>
</link>
<joint
name="right_joint1"
type="revolute">
<origin
xyz="0 0 0.2405"
rpy="0 0 0" />
<parent
link="base_link_rightarm" />
<child
link="RightLink1" />
<axis
xyz="0 0 1" />
<limit
lower="-3.1"
upper="3.1"
effort="60"
velocity="3.14" />
</joint>
<link
name="RightLink2">
<inertial>
<origin
xyz="0.152256463426163 4.75383656106654E-07 -0.00620260383607792"
rpy="0 0 0" />
<mass
value="0.864175046869043" />
<inertia
ixx="0.00089150298478414"
ixy="-2.23268489334765E-08"
ixz="0.00156246461035015"
iyy="0.00733754343083901"
iyz="6.28110889329165E-09"
izz="0.00697869103915473" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link2.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link2.STL" />
</geometry>
</collision>
</link>
<joint
name="right_joint2"
type="revolute">
<origin
xyz="0 0 0"
rpy="1.5708 -1.5708 0" />
<parent
link="RightLink1" />
<child
link="RightLink2" />
<axis
xyz="0 0 1" />
<limit
lower="-2.268"
upper="2.268"
effort="60"
velocity="3.14" />
</joint>
<link
name="RightLink3">
<inertial>
<origin
xyz="5.05312670989961E-06 -0.0595925663694732 0.010569069212336"
rpy="0 0 0" />
<mass
value="0.289633681624654" />
<inertia
ixx="0.00063737100450158"
ixy="-7.05261293649751E-08"
ixz="-3.86643272239426E-08"
iyy="0.00015648388095025"
iyz="-0.00014461035994916"
izz="0.000614178164773085" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link3.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link3.STL" />
</geometry>
</collision>
</link>
<joint
name="right_joint3"
type="revolute">
<origin
xyz="0.256 0 0"
rpy="0 0 1.5708" />
<parent
link="RightLink2" />
<child
link="RightLink3" />
<axis
xyz="0 0 1" />
<limit
lower="-2.355"
upper="2.355"
effort="30"
velocity="3.92" />
</joint>
<link
name="RightLink4">
<inertial>
<origin
xyz="1.15516617405898E-06 -0.0180424468598241 -0.0215394748352687"
rpy="0 0 0" />
<mass
value="0.239419768320061" />
<inertia
ixx="0.000285938919722783"
ixy="3.07101359163101E-09"
ixz="-2.21994118981953E-09"
iyy="0.000262727540304212"
iyz="4.4236583260078E-05"
izz="0.000119888082791859" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link4.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link4.STL" />
</geometry>
</collision>
</link>
<joint
name="right_joint4"
type="revolute">
<origin
xyz="0 -0.21 0"
rpy="1.5708 0 0" />
<parent
link="RightLink3" />
<child
link="RightLink4" />
<axis
xyz="0 0 1" />
<limit
lower="-3.1"
upper="3.1"
effort="10"
velocity="3.92" />
</joint>
<link
name="RightLink5">
<inertial>
<origin
xyz="3.19794786262152E-06 -0.0593808368101458 0.00736804250989326"
rpy="0 0 0" />
<mass
value="0.218799761431678" />
<inertia
ixx="0.000350540363914072"
ixy="-3.41781619975602E-08"
ixz="-1.77056457224373E-08"
iyy="0.000104927867487581"
iyz="-7.82431228461971E-05"
izz="0.000334482418423629" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link5.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link5.STL" />
</geometry>
</collision>
</link>
<joint
name="right_joint5"
type="revolute">
<origin
xyz="0 0 0"
rpy="-1.5708 0 0" />
<parent
link="RightLink4" />
<child
link="RightLink5" />
<axis
xyz="0 0 1" />
<limit
lower="-2.233"
upper="2.233"
effort="10"
velocity="3.92" />
</joint>
<link
name="RightLink6">
<inertial>
<origin
xyz="0.000714234511756999 -0.000396718732824521 -0.0126723660946126"
rpy="0 0 0" />
<mass
value="0.0649018034311231" />
<inertia
ixx="2.02766547502765E-05"
ixy="-1.32505200276849E-06"
ixz="-2.5845091522508E-08"
iyy="1.87986725225022E-05"
iyz="3.39471452125439E-09"
izz="3.17885459163081E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link6.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link6.STL" />
</geometry>
</collision>
</link>
<joint
name="right_joint6"
type="revolute">
<origin
xyz="0 -0.144 0"
rpy="1.5708 0 0" />
<parent
link="RightLink5" />
<child
link="RightLink6" />
<axis
xyz="0 0 1" />
<limit
lower="-6.28"
upper="6.28"
effort="10"
velocity="3.92" />
</joint>
</robot>

15
HiveCoreR1/src/arm_keyboard_input/CMakeLists.txt
View File

@@ -16,17 +16,26 @@ include_directories(
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(keyBoardInputer ${SOURCES})
ament_target_dependencies(keyBoardInputer rclcpp std_msgs)
add_executable(arm_keyboard_input ${SOURCES})
ament_target_dependencies(arm_keyboard_input
rclcpp
std_msgs
rclcpp_action
gjk_interface
interfaces
)
install(TARGETS
keyBoardInputer
arm_keyboard_input
DESTINATION lib/${PROJECT_NAME})
ament_package()

32
HiveCoreR1/src/arm_keyboard_input/include/arm_keyboard_node.hpp
View File

@@ -3,9 +3,15 @@
#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_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:
@@ -14,6 +20,32 @@ public:
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 SendDirectSteppingGoal(const char *command);
void SendDirectMovingGoal(const float *values, int armId);
void SendDirectSteppingGoal2(const float *values, int armId);
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

3
HiveCoreR1/src/arm_keyboard_input/package.xml
View File

@@ -8,7 +8,10 @@
<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>

731
HiveCoreR1/src/arm_keyboard_input/src/arm_keyboard_node.cpp
View File

@@ -1,18 +1,196 @@
#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;
static void ListenInputThread(std::shared_ptr<ArmKeyboardNode> armNode) {
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 = {"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;
auto message = std_msgs::msg::Float64MultiArray();
message.data.resize(ARM_DOF);
@@ -26,12 +204,16 @@ static void ListenInputThread(std::shared_ptr<ArmKeyboardNode> armNode) {
bool valid = true;
// 解析整数
if (!std::getline(ss, token, ',')) { valid = false; }
if (!std::getline(ss, token, ',')) {
valid = false;
}
else {
try {
mode = std::stoi(token);
if (mode != 0 && mode != 1 && mode != 2) valid = false;
} catch (...) { valid = false; }
if (mode != 1 && mode != 2) valid = false;
} catch (...) {
valid = false;
}
}
// 解析6个float
@@ -43,9 +225,8 @@ static void ListenInputThread(std::shared_ptr<ArmKeyboardNode> armNode) {
}
if (mode == 0) {
std::cout << "退出输入监听线程。" << std::endl;
armNode->exit_flag = true;
break;
std::cout << "退出输入角度模式。" << std::endl;
return;
}
if (valid) {
@@ -65,11 +246,277 @@ static void ListenInputThread(std::shared_ptr<ArmKeyboardNode> armNode) {
}
}
void KeyboardInputPoseMoving(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(右臂)),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 (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(右臂)),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 (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::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_);
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_);
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:
KeyboardInputDirectStepping(armNode);
std::getline(std::cin, line, 'q');
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()
@@ -81,8 +528,278 @@ ArmKeyboardNode::~ArmKeyboardNode()
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_ = nullptr;
} 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_ = nullptr;
}
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_ = nullptr;
} 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_ = nullptr;
}
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_ = nullptr;
}
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_ = nullptr;
}
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[])
{
/*std::vector<Eigen::Vector3d> polyA = {Vector3d(2,0,0), Vector3d(4,0,0), Vector3d(2,2,0), Vector3d(4,2,0),
Vector3d(2,0,2), Vector3d(4,0,2), Vector3d(2,2,2), Vector3d(4,2,2)};
std::vector<Eigen::Vector3d> polyB = {Vector3d(3,1,1), Vector3d(5,1,1), Vector3d(3,3,1), Vector3d(5,3,1),
Vector3d(3,1,3), Vector3d(5,1,3), Vector3d(3,3,3), Vector3d(5,3,3)};
OBB obbA = gjk_interface::createOBBFromVertices(polyA, 2, 2, 2);
OBB obbB = gjk_interface::createOBBFromVertices(polyB, 2, 2, 2);
bool inCollisionOBB = gjk_interface::checkOBBCollision(obbA, obbB);
if (inCollisionOBB) {
printf("dect poly A and B collision\n");
} else {
printf("dect poly A and B safe\n");
}
polyA = {Vector3d(2,0,0), Vector3d(4,0,0), Vector3d(2,2,0), Vector3d(4,2,0),
Vector3d(2,0,2), Vector3d(4,0,2), Vector3d(2,2,2), Vector3d(4,2,2)};
polyB = {Vector3d(3.99,0,0), Vector3d(5.99,0,0), Vector3d(3.99,2,0), Vector3d(5.99,2,0),
Vector3d(3.99,0,2), Vector3d(5.99,0,2), Vector3d(3.99,2,2), Vector3d(5.99,2,2)};
obbA = gjk_interface::createOBBFromVertices(polyA, 2, 2, 2);
obbB = gjk_interface::createOBBFromVertices(polyB, 2, 2, 2);
inCollisionOBB = gjk_interface::checkOBBCollision(obbA, obbB);
if (inCollisionOBB) {
printf("dect poly A and B collision\n");
} else {
printf("dect poly 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);

20
HiveCoreR1/src/common_action_interface/CMakeLists.txt
View File

@@ -1,20 +0,0 @@
cmake_minimum_required(VERSION 3.8)
project(common_action_interface)
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)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/Arm.action"
)
ament_package()

14
HiveCoreR1/src/common_action_interface/action/Arm.action
View File

@@ -1,14 +0,0 @@
int8 body_id
int8 data_type
int16 data_length
int64 command_id
float64[] data_array
---
int64 start_time
int64 end_time
---
int64 command_id
int16 int_lenth
int16 float_length
int32[] int_data_array
float64[] float_data_array

21
HiveCoreR1/src/common_action_interface/package.xml
View File

@@ -1,21 +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_action_interface</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="zj@todo.todo">zj</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>action_msgs</depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

29
HiveCoreR1/src/gjk_interface/src/gjk_interface.cpp
View File

@@ -175,9 +175,9 @@ namespace gjk_interface {
obb.center = (vertices[1] + vertices[2] + vertices[4] - vertices[0]) * (1.0 / 2.0);
// 正交化（格拉姆-施密特过程），确保3条轴正交
obb.axis[0] = vertices[1] - vertices[0];
obb.axis[1] = vertices[2] - vertices[0];
obb.axis[2] = vertices[4] - vertices[0];
obb.axis[0] = (vertices[1] - vertices[0]).normalized();
obb.axis[1] = (vertices[2] - vertices[0]).normalized();
obb.axis[2] = (vertices[4] - vertices[0]).normalized();
obb.halfExtent[0] = length * (1.0 / 2.0);
obb.halfExtent[1] = width * (1.0 / 2.0);
@@ -214,23 +214,28 @@ namespace gjk_interface {
for (int j = 0; j < 3; ++j) {
Vector3d crossAxis = obb1.axis[i].cross(obb2.axis[j]);
if (crossAxis.dot(crossAxis) > 1e-6) { // 排除零向量（轴平行时叉积为零）
separationAxes.push_back(crossAxis);
auto [aMin, aMax] = projectOBB(obb1, crossAxis.normalized());
auto [bMin, bMax] = projectOBB(obb2, crossAxis.normalized());
if (!areProjectionsOverlapping(aMin, aMax, bMin, bMax)) {
// 找到分离轴，判定为不碰撞
return false;
}
}
}
}
// 2.1 第一个OBB的3条轴
for (int i = 0; i < 3; ++i) {
separationAxes.push_back(obb1.axis[i]);
auto [aMin, aMax] = projectOBB(obb1, obb1.axis[i]);
auto [bMin, bMax] = projectOBB(obb2, obb1.axis[i]);
if (!areProjectionsOverlapping(aMin, aMax, bMin, bMax)) {
// 找到分离轴，判定为不碰撞
return false;
}
}
// 2.2 第二个OBB的3条轴
for (int i = 0; i < 3; ++i) {
separationAxes.push_back(obb2.axis[i]);
}
// 3. 检查所有分离轴的投影重叠情况
for (const auto& axis : separationAxes) {
auto [aMin, aMax] = projectOBB(obb1, axis);
auto [bMin, bMax] = projectOBB(obb2, axis);
auto [aMin, aMax] = projectOBB(obb1, obb2.axis[i]);
auto [bMin, bMax] = projectOBB(obb2, obb2.axis[i]);
if (!areProjectionsOverlapping(aMin, aMax, bMin, bMax)) {
// 找到分离轴，判定为不碰撞
return false;

1
HiveCoreR1/src/hivecore_robot_interfaces Submodule

Submodule HiveCoreR1/src/hivecore_robot_interfaces added at e0b3ba12b9

16
HiveCoreR1/src/rm_arm_control/CMakeLists.txt
View File

@@ -21,7 +21,7 @@ find_package(trajectory_msgs REQUIRED)
find_package(control_msgs REQUIRED)
find_package(common_msg REQUIRED)
find_package(gjk_interface REQUIRED)
find_package(common_action_interface REQUIRED)
find_package(interfaces REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
@@ -35,7 +35,12 @@ include_directories(
set(SOURCES
src/rm_arm_handler.cpp
src/rm_arm_node.cpp
src/rm_arm_simulator.cpp
simulator/rm_arm_simulator.cpp
simulator/trapezoidal_trajectory_kinematics.cpp
src/goal_manager.cpp
driver/arm_driver.cpp
driver/rm_arm_driver.cpp
src/main.cpp
)
# 添加可执行文件
@@ -50,7 +55,7 @@ ament_target_dependencies(rm_arm_control
control_msgs
common_msg
gjk_interface
common_action_interface
interfaces
moveit_core
moveit_ros_planning_interface
)
@@ -82,8 +87,9 @@ install(DIRECTORY
FILES_MATCHING PATTERN "*.so*"
)
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}/
install(
DIRECTORY launch urdf meshes # 安装 launch、urdf、meshes 目录
DESTINATION share/${PROJECT_NAME}
)
ament_package()

79
HiveCoreR1/src/rm_arm_control/driver/arm_driver.cpp Normal file
View File

@@ -0,0 +1,79 @@
#include "arm_driver.hpp"
#include <cstring>
ArmStatusCallbackData ArmDriver::leftCallbackData_;
ArmStatusCallbackData ArmDriver::rightCallbackData_;
std::shared_mutex ArmDriver::default_update_status_mutex_;
ArmDriver::ArmStatusCallbackFun ArmDriver::armStatusCallback_ = nullptr;
ArmDriver::ArmDriver()
{
armStatusCallback_ = nullptr;
armMoveFun_ = nullptr;
initFlag_ = ARM_MOVE_STATUS_UNINIT;
}
ArmDriver::~ArmDriver()
{
}
void ArmDriver::PublicThreadUpdate()
{
std::unique_lock lock(default_update_status_mutex_);
armStatusCallback_(&leftCallbackData_);
armStatusCallback_(&rightCallbackData_);
}
int ArmDriver::DefaultArmMove(const float points[USED_ARM_DOF], int armId)
{
ArmStatusCallbackData *callbackData;
if (armId == LEFT_ARM) {
callbackData = &leftCallbackData_;
} else if (armId == RIGHT_ARM) {
callbackData = &rightCallbackData_;
} else {
return UNKNOWN_ERR;
}
std::shared_lock lock(default_update_status_mutex_);
callbackData->errCode = 0;
for (size_t i = 0; i < USED_ARM_DOF; i++) {
callbackData->jointAngle[i] = points[i];
}
armStatusCallback_(callbackData);
return 0;
}
int ArmDriver::Init()
{
if (armStatusCallback_ == nullptr) {
return UNKNOWN_ERR;
}
if (armMoveFun_ == nullptr) {
armMoveFun_ = DefaultArmMove;
memset(&leftCallbackData_, 0, sizeof(ArmStatusCallbackData));
memset(&rightCallbackData_, 0, sizeof(ArmStatusCallbackData));
rightCallbackData_.armId = RIGHT_ARM;
leftCallbackData_.armId = LEFT_ARM;
for (size_t i = 0; i < USED_ARM_DOF; i++) {
leftCallbackData_.jointEnFlag[i] = true;
rightCallbackData_.jointEnFlag[i] = true;
leftCallbackData_.jointTemp[i] = 0.0f;
rightCallbackData_.jointTemp[i] = 0.0f;
leftCallbackData_.jointVoltage[i] = 0.0f;
rightCallbackData_.jointVoltage[i] = 0.0f;
leftCallbackData_.jointCurrent[i] = 0.0f;
rightCallbackData_.jointCurrent[i] = 0.0f;
leftCallbackData_.jointSpeed[i] = 0.0f;
rightCallbackData_.jointSpeed[i] = 0.0f;
leftCallbackData_.jointTorque[i] = 0.0f;
rightCallbackData_.jointTorque[i] = 0.0f;
leftCallbackData_.jointErrCode[i] = 0;
rightCallbackData_.jointErrCode[i] = 0;
leftCallbackData_.jointAngle[i] = 0.0f;
rightCallbackData_.jointAngle[i] = 0.0f;
}
}
initFlag_ = ARM_MOVE_STATUS_READY;
return OK;
}

167
HiveCoreR1/src/rm_arm_control/driver/rm_arm_driver.cpp Normal file
View File

@@ -0,0 +1,167 @@
#include "arm_driver.hpp"
#include "rm_arm_driver.hpp"
#include <cstring>
RmArmDriver::RmArmDriver()
{
rmService_ = nullptr;
initFlag_ = RM_DRIVER_UNINIT;
}
RmArmDriver::~RmArmDriver()
{
if (rmService_ != nullptr) {
rmService_->rm_destroy();
delete rmService_;
}
}
RmArmDriver::RmArmStatusCallbackFun RmArmDriver::armStatusCallback_ = nullptr;
RmArmDriver::ApiLogFunc RmArmDriver::customApiLog_ = nullptr;
rm_robot_handle *RmArmDriver::leftRobotHandle_;
rm_robot_handle *RmArmDriver::rightRobotHandle_;
RM_Service *RmArmDriver::rmService_ = nullptr;
int RmArmDriver::ArmMove(const float points[USED_ARM_DOF], int armId)
{
float joints[ARM_DOF] = {0};
for (size_t i = 0; i < USED_ARM_DOF; i++) {
joints[i] = points[i];
}
if (armId == LEFT_ARM) {
int result = rmService_->rm_movej_canfd(leftRobotHandle_, joints, true, 0);
if (result != 0) {
api_log("Error moving left arm to joint space!");
return UNKNOWN_ERR;
}
} else if (armId == RIGHT_ARM) {
int result = rmService_->rm_movej_canfd(rightRobotHandle_, joints, true, 0);
if (result != 0) {
api_log("Error moving right arm to joint space!");
return UNKNOWN_ERR;
}
} else {
api_log("Invalid armId: %d", armId);
return UNKNOWN_ERR;
}
return OK;
}
void RmArmDriver::CallbackRmRealtimeArmJointState(rm_realtime_arm_joint_state_t data)
{
ArmStatusCallbackData callbackData;
callbackData.errCode = 0;
if (armStatusCallback_ == nullptr) {
return;
}
if (strcmp(data.arm_ip, LEFT_ARM_IP_ADDRESS) == 0) {
callbackData.armId = LEFT_ARM;
}
else if (strcmp(data.arm_ip, RIGHT_ARM_IP_ADDRESS) == 0) {
callbackData.armId = RIGHT_ARM;
} else {
api_log("Unknown arm ip: %s", data.arm_ip);
return;
}
callbackData.errCode = data.errCode;
for (size_t i = 0; i < USED_ARM_DOF; i++) {
callbackData.jointCurrent[i] = data.joint_status.joint_current[i];
callbackData.jointEnFlag[i] = data.joint_status.joint_en_flag[i];
callbackData.jointAngle[i] = data.joint_status.joint_position[i];
callbackData.jointSpeed[i] = data.joint_status.joint_speed[i];
callbackData.jointTemp[i] = data.joint_status.joint_temperature[i];
callbackData.jointVoltage[i] = data.joint_status.joint_voltage[i];
callbackData.jointTorque[i] = data.force_sensor.force[i];
callbackData.jointErrCode[i] = data.joint_status.joint_err_code[i];
}
armStatusCallback_(&callbackData);
}
void RmArmDriver::api_log(const char* format, ...)
{
va_list args;
va_start(args, format);// 初始化可变参数列表
customApiLog_(format, args);// 调用函数指针，传递 va_list
va_end(args);// 清理可变参数列表
}
int RmArmDriver::InitHandle(rm_robot_handle **robotHandle_, const char *robot_ip_address, int robot_port, rm_realtime_push_config_t *config)
{
*robotHandle_ = rmService_->rm_create_robot_arm(robot_ip_address, robot_port);
if (*robotHandle_ == NULL) {
api_log("Failed to create arm(%s) handle.", robot_ip_address);
return UNKNOWN_ERR;
}
if ((*robotHandle_)->id <= 0) {
api_log("Failed to create arm(%s) handle. %d", robot_ip_address, (*robotHandle_)->id);
return UNKNOWN_ERR;
}
int result = rmService_-> rm_set_realtime_push(*robotHandle_, *config);
if (result != 0) {
api_log("Failed to set realtime push for arm(%s).", robot_ip_address);
return UNKNOWN_ERR;
}
result = rm_set_arm_max_line_speed(*robotHandle_, 0.01f);
if (result != 0) {
api_log("Failed to set max line speed for arm(%s).", robot_ip_address);
return UNKNOWN_ERR;
}
rm_robot_info_t robot_info;
int info_result = rmService_->rm_get_robot_info(*robotHandle_, &robot_info);
if (info_result != 0) {
api_log("Failed to get robot info(%s)", robot_ip_address);
return UNKNOWN_ERR;
}
int dof = robot_info.arm_dof;
if (dof != USED_ARM_DOF) {
api_log("Invalid degree of freedom(%s)", robot_ip_address);
return UNKNOWN_ERR;
}
return 0;
}
int RmArmDriver::Init()
{
if (initFlag_ == RM_DRIVER_INIT_OK) {
return OK;
}
if (customApiLog_ == nullptr || armStatusCallback_ == nullptr) {
return UNKNOWN_ERR;
}
rmService_ = new RM_Service();
if (rmService_ == nullptr) {
return UNKNOWN_ERR;
}
int result = rmService_->rm_init(RM_TRIPLE_MODE_E);
if (result != 0) {
rmService_->rm_destroy();
delete rmService_;
return UNKNOWN_ERR;
}
rmService_->rm_set_log_call_back(customApiLog_, 3);
rmService_->rm_realtime_arm_state_call_back(CallbackRmRealtimeArmJointState);
int robot_port = ARM_IP_PORT;
rm_realtime_push_config_t config = {20, true, 8089, 0, "192.168.2.3"};
result = InitHandle(&leftRobotHandle_, LEFT_ARM_IP_ADDRESS, robot_port, &config);
if (result != 0) {
rmService_->rm_destroy();
delete rmService_;
return UNKNOWN_ERR;
}
config = {20, true, 8089, 0, "192.168.2.19"};
result = InitHandle(&rightRobotHandle_, RIGHT_ARM_IP_ADDRESS, robot_port, &config);
if (result != 0) {
rmService_->rm_destroy();
delete rmService_;
return UNKNOWN_ERR;
}
initFlag_ = RM_DRIVER_INIT_OK;
return OK;
}

75
HiveCoreR1/src/rm_arm_control/include/arm_driver.hpp Normal file
View File

@@ -0,0 +1,75 @@
#ifndef ARM_DRIVER_HPP
#define ARM_DRIVER_HPP
#include <memory>
#include <string>
#include <vector>
#include "arm_define.h"
#include <shared_mutex>
#include <mutex>
struct ArmStatusCallbackData {
int errCode; ///< 数据解析错误码，-3为数据解析错误，代表推送的数据不完整或格式不正确
int armId; ///< 机械臂ID，0为左臂，1为右臂
float jointCurrent[USED_ARM_DOF]; ///< 关节电流，单位mA，精度：0.001mA
bool jointEnFlag[USED_ARM_DOF]; ///< 当前关节使能状态 ，1为上使能，0为掉使能
float jointAngle[USED_ARM_DOF]; ///< 关节角度，单位度，精度：0.001度
float jointSpeed[USED_ARM_DOF]; ///< 关节速度，单位度/秒，精度：0.001度/秒
float jointTemp[USED_ARM_DOF]; ///< 关节温度，单位摄氏度，精度：0.1摄氏度
float jointVoltage[USED_ARM_DOF]; ///< 关节电压，单位V，精度：0.01V
float jointTorque[USED_ARM_DOF]; ///< 关节力矩，单位Nm，精度：0.001Nm
int jointErrCode[USED_ARM_DOF]; ///< 当前关节错误码
};
enum ArmDriverStatus {
ARM_MOVE_STATUS_UNINIT = 0,
ARM_MOVE_STATUS_READY = 1,
ARM_MOVE_STATUS_ERROR = 2
};
class ArmDriver
{
public:
ArmDriver();
~ArmDriver();
int Init();
using ArmStatusCallbackFun = void(*)(ArmStatusCallbackData *data);
void RegArmStatusCallback(ArmStatusCallbackFun callback) {
armStatusCallback_ = callback;
}
static void CallArmStatusCallback(ArmStatusCallbackData *data) {
if (armStatusCallback_ != nullptr) {
armStatusCallback_(data);
}
}
using ArmMoveFun = int(*)(const float points[USED_ARM_DOF], int armId);
void RegArmMoveCallback(ArmMoveFun callback) {
armMoveFun_ = callback;
}
int ArmMove(const float points[USED_ARM_DOF], int armId) {
if (armMoveFun_ != nullptr) {
return armMoveFun_(points, armId);
}
return UNKNOWN_ERR;
}
void PublicThreadUpdate();
private:
int initFlag_;
static ArmStatusCallbackFun armStatusCallback_;
ArmMoveFun armMoveFun_;
static std::shared_mutex default_update_status_mutex_;
static ArmStatusCallbackData leftCallbackData_;
static ArmStatusCallbackData rightCallbackData_;
static int DefaultArmMove(const float points[USED_ARM_DOF], int armId);
};
#endif // ARM_DRIVER_HPP

39
HiveCoreR1/src/rm_arm_control/include/goal_manager.hpp Normal file
View File

@@ -0,0 +1,39 @@
#ifndef GOAL_MANAGER_HPP
#define GOAL_MANAGER_HPP
#include <cstdlib>
#include <unordered_map>
#include "arm_define.h"
class GoalManager
{
public:
GoalManager();
~GoalManager();
int AddGoal(const float points[GOAL_DATA_LENGTH], const int64_t &index, int type);
int ConsumeGoal();
int RemoveGoal(int64_t index);
int SetNowGoalResult(int result);
int GetNowGoalResult() const;
int GetGoalType() const;
int64_t GetGoalNowIndex();
int64_t GetCompeletedGoalNowIndex();
void ClearGoals();
const float* GetGoal() const;
void GoalRunOff();
int GetRunOffFlag() const;
private:
float nowJointsGoalList_[MAX_ARM_GOAL_COUNT][GOAL_DATA_LENGTH];
int goalResultList_[MAX_ARM_GOAL_COUNT];
int goalTypeList_[MAX_ARM_GOAL_COUNT];
int64_t goalIndexList_[MAX_ARM_GOAL_COUNT];
int startGoalIndex_;
int endGoalIndex_;
int goalFullFlag_;
int currentGoalRunOffFlag_;
std::unordered_map<int64_t, int> goalIndex{MAX_ARM_GOAL_COUNT};
};
#endif

45
HiveCoreR1/src/rm_arm_control/include/rm_arm_driver.hpp Normal file
View File

@@ -0,0 +1,45 @@
#ifndef RM_ARM_DRIVER_HPP
#define RM_ARM_DRIVER_HPP
#include "arm_driver.hpp"
#include "rm_service.h"
#include "rm_define.h"
enum {
RM_DRIVER_UNINIT = 0,
RM_DRIVER_INIT_OK,
RM_DRIVER_INIT_ERR
};
class RmArmDriver
{
public:
RmArmDriver();
~RmArmDriver();
int Init();
static int ArmMove(const float points[USED_ARM_DOF], int armId);
using ApiLogFunc = void(*)(const char* message, va_list args);
void SetCustomApiLog(ApiLogFunc logFunc) {customApiLog_ = logFunc;}
using RmArmStatusCallbackFun = void(*)(ArmStatusCallbackData *data);
void RegArmStatusCallback(RmArmStatusCallbackFun callback) {
armStatusCallback_ = callback;
}
static RM_Service *rmService_;
private:
int initFlag_;
static ApiLogFunc customApiLog_;
static RmArmStatusCallbackFun armStatusCallback_;
static rm_robot_handle *leftRobotHandle_;
static rm_robot_handle *rightRobotHandle_;
static void CallbackRmRealtimeArmJointState(rm_realtime_arm_joint_state_t data);
int InitHandle(rm_robot_handle **robotHandle_, const char *robot_ip_address, int robot_port, rm_realtime_push_config_t *config);
static void api_log(const char* format, ...);
};
#endif // RM_ARM_DRIVER_HPP

59
HiveCoreR1/src/rm_arm_control/include/rm_arm_handler.hpp
View File

@@ -4,60 +4,61 @@
#include <cstdlib>
#include <stdio.h>
#include <unistd.h>
#include <functional>
#include <sys/stat.h>
#include <sys/time.h>
#include <semaphore.h>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "goal_manager.hpp"
#include "arm_define.h"
#include "rm_service.h"
#include "arm_driver.hpp"
#include "rm_arm_simulator.hpp"
typedef enum {
ARM_STATUS_INIT = 0,
ARM_STATUS_READY,
ARM_STATUS_MOVING,
ARM_STATUS_STEPPING,
ARM_STATUS_WAITING,
ARM_STATUS_ERROR
} ARM_STATUS_E;
class RmArmHandler
{
private:
RM_Service *roboticService_;
rm_robot_handle *robotHandle_;
rm_robot_info_t robot_info_;
std::shared_mutex rw_mutex_;
sem_t *sem_;
float nowJointsGoalList_[MAX_ARM_GOAL_COUNT][ARM_DOF];
int64_t goalIndexList_[MAX_ARM_GOAL_COUNT];
int startGoalIndex_;
int endGoalIndex_;
int goalFullFlag_;
float nowSpeed_[ARM_DOF];
std::unordered_map<int64_t, int> goalIndex{16};
GoalManager *goalManager_;
ArmDriver *driver_;
public:
RmArmHandler(RM_Service *roboticService,
const char *robot_ip_address, int robot_port, rm_realtime_push_config_t *config);
RmArmHandler(ArmDriver *driver, int armType, sem_t *sem, KinematicsManager *trapezoidalTrajectory);
~RmArmHandler();
void ArmDealCallBackInfo(rm_realtime_arm_joint_state_t *data);
void ArmCommandCallbackStep(float points[ARM_DOF]);
void ArmCommandCallbackDirect(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
KinematicsManager *trapezoidalTrajectory_;
int armType_; // 0:left 1:right
rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr armCommandSub_;
float nowJoints_[ARM_DOF];
float nowAimPose_[POSE_DIMENSION];
float nextJoints_[ARM_DOF];
int poseNeedUpdate_;
void ArmDealCallBackInfo(ArmStatusCallbackData *data);
int AddNextPoint(float points[ARM_DOF], const int64_t &index);
int ConsumeNextPoint();
float nowJoints_[USED_ARM_DOF];
float nextJoints_[USED_ARM_DOF];
int AddNextPoint(float points[GOAL_DATA_LENGTH], const int64_t &index, int type);
int DeletePoint(int64_t index);
void ArmRun();
void ArmSimu();
int64_t GetCurrentRunOffGoalIndex();
int64_t GetCurrentGoalIndex();
void ArmRun(int64_t timeStamp);
void ArmStop(int result = 0);
void ArmStopStart(int result);
void ArmMove();
int GoalReached();
void UpdateNowPoseFromJoints();
int AddNextPointFromPose(const float pose[POSE_DIMENSION], const int64_t &index);
int AddNextPointFromPose(const float pose[POSE_DIMENSION], const int64_t &index, int type);
int AddNextPointFromPoseMoving(const float pose[POSE_DIMENSION], const int64_t &index);
int GetNextPointFromPoseStepping(const float pose[POSE_DIMENSION], float *joints);
int StartNewTrajectory();
ARM_STATUS_E armStatus;
};

32
HiveCoreR1/src/rm_arm_control/include/rm_arm_node.hpp
View File

@@ -1,47 +1,53 @@
#ifndef RM_ARM_NODE_HPP
#define RM_ARM_NODE_HPP
#include "rm_service.h"
#include <semaphore.h>
#include "rclcpp_action/rclcpp_action.hpp"
#include "common_action_interface/action/arm.hpp"
#include "interfaces/action/arm.hpp"
#include "common_msg/msg/arm_commnd.hpp"
#include <std_msgs/msg/string.hpp>
#include "arm_driver.hpp"
#include "rm_arm_handler.hpp"
using namespace std::chrono_literals;
using std::placeholders::_1;
using ArmAction = common_action_interface::action::Arm;
using ArmAction = interfaces::action::Arm;
using GoalHandleArm = rclcpp_action::ServerGoalHandle<ArmAction>;
class RmArmNode: public rclcpp::Node
{
public:
RmArmNode();
RmArmNode(ArmDriver *armDriver, Kinematics *kinematics);
~RmArmNode();
static RmArmHandler *leftArmHandler_;
static RmArmHandler *rightArmHandler_;
static void CallbackRealtimeArmJointState(ArmStatusCallbackData *data);
private:
RM_Service *rmService_;
sem_t sem_;
ArmDriver *armDriver_;
rclcpp_action::Server<ArmAction>::SharedPtr action_server_;
static sensor_msgs::msg::JointState armStates;
rclcpp_action::GoalResponse ArmHandleGoal(
rclcpp_action::GoalResponse ArmActionGoal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const ArmAction::Goal> goal);
rclcpp_action::CancelResponse ArmHandleCancel(
rclcpp_action::CancelResponse ArmActionCancel(
const std::shared_ptr<GoalHandleArm> goal_handle);
void ArmHandleAccepted(const std::shared_ptr<GoalHandleArm> goal_handle);
void ArmHandleExecute();
void ArmActionAccepted(const std::shared_ptr<GoalHandleArm> goal_handle);
void ArmActionFeedback(int collision);
void ArmActionExecute();
void ArmActionFinish(int64_t index, int result);
void ArmActionCheck();
void StartNewGoal(RmArmHandler *handle);
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robotDescriptionSub_;
static rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr armStatesPub_;
std::unordered_map<int64_t, std::shared_ptr<GoalHandleArm>> goal_handles_;
static void CallbackRmRealtimeArmJointState(rm_realtime_arm_joint_state_t data);
void CallbackGetRobotDescription(const std_msgs::msg::String::SharedPtr msg);
static void custom_api_log(const char* message, va_list args);
};
#endif // RM_ARM_NODE_HPP

55
HiveCoreR1/src/rm_arm_control/include/rm_arm_simulator.hpp
View File

@@ -4,9 +4,21 @@
#include <unordered_map>
#include <map>
#include <urdf_model/pose.h>
#include "rm_arm_node.hpp"
#include <std_msgs/msg/string.hpp>
#include "arm_define.h"
#include "trajectory.hpp"
#include "gjk_interface.hpp"
#define ARM_COLLISION_CHECK_SIMPLE_CNT 2
typedef enum {
FRAME_CHANGE_TYPE_ARM_BASE_TO_ORI,
FRAME_CHANGE_TYPE_ORI_TO_ARM_BASE,
FRAME_CHANGE_TYPE_ARM_END_TO_ORI,
FRAME_CHANGE_TYPE_ORI_TO_ARM_END,
FRAME_CHANGE_TYPE_ERR
} FrameChangeTypeE;
typedef std::vector<Eigen::Vector3d> Polyhedron;
struct CollisionStructure {
std::string link_name; // 所属链接名称
@@ -24,8 +36,10 @@ struct JointInfo {
int type; // 关节类型
int parent_link; // 父连杆
int child_link; // 子连杆
urdf::Pose origin;
urdf::Rotation rotationAll;
Eigen::Quaterniond rotationAll;
Eigen::Vector3d axis; // 关节轴
Eigen::Vector3d offset; // 关节偏移
Eigen::Quaterniond rotation; // 关节旋转
double limit_lower;
double limit_upper;
double limit_v;
@@ -39,22 +53,41 @@ enum {
RIGHT_ARM_COLLISION,
};
using namespace ArmKinematicsNs;
class ArmSimulator
{
public:
ArmSimulator(RmArmNode *armNode, std_msgs::msg::String::SharedPtr urdf_string_ptr);
ArmSimulator(std_msgs::msg::String::SharedPtr urdf_string_ptr, float *left_now_joints, float *right_now_joints,
KinematicsManager *leftTrajectory, KinematicsManager *rightTrajectory);
ArmSimulator(std::string urdf_string, float *left_now_joints, float *right_now_joints,
KinematicsManager *leftTrajectory, KinematicsManager *rightTrajectory);
~ArmSimulator();
int CheckCollision();
int CheckJointsAngleLimit(float *joints);
int UpdateCollisionForNewGoal(int arm);
int AddPolyhedron(int id, const Polyhedron& poly);
int RemovePolyhedron(int id);
RmArmNode *armNode_;
std::vector<CollisionStructure> collision_structures;
std::map<int, JointInfo> jointMap;
std::vector<Polyhedron> armPolyhedrons_;
std::vector<Polyhedron> armPolyhedronsNext_;
std::vector<OBB> addedOBBs_;
std::vector<OBB> addedOBBsNext_;
int ChangeArmBaseFrameToArmEndPose(float *pose, int arm, int64_t frameTime);
int ChangeOriFrameToArmBasePose(float *pose, int arm);
int ChangeArmEndPoseToArmBaseFrame(float *pose, int arm, int64_t frameTime);
int ChangeArmBasePoseToOriFrame(float *pose, int arm);
int GetFrameChange(float *pose, int arm, int64_t frameTime, JointInfo *jointInfo);
KinematicsManager *leftTrajectory_;
KinematicsManager *rightTrajectory_;
std::vector<CollisionStructure> collision_structures_;
std::map<int, JointInfo> jointMap_;
std::vector<Polyhedron> armPolyhedrons_[ARM_COLLISION_CHECK_SIMPLE_CNT];
std::vector<OBB> addedOBBs_[ARM_COLLISION_CHECK_SIMPLE_CNT];
int simpleUpdateIndex_;
float leftArmJoints_[ARM_COLLISION_CHECK_SIMPLE_CNT][USED_ARM_DOF];
float rightArmJoints_[ARM_COLLISION_CHECK_SIMPLE_CNT][USED_ARM_DOF];
private:
std::unordered_map<int, Polyhedron> addedPolyhedrons_;
};

121
HiveCoreR1/src/rm_arm_control/include/trajectory.hpp Normal file
View File

@@ -0,0 +1,121 @@
#ifndef TRAJECTORY_HPP
#define TRAJECTORY_HPP
#include <vector>
#include "arm_define.h"
#include "trapezoidal_trajectory_kinematics.hpp"
namespace ArmKinematicsNs {
template <typename T, typename K>
class TrajectoryManager
{
public:
TrajectoryManager(K *armKinematics) : armKinematics_(armKinematics) {
nowTrajectoryIndex_ = -1;
trajectoryFullFlag_ = 0;
}
~TrajectoryManager() {}
int computeTrajectory(const float *start, const float *end, int type) {
T* newTrajectory = addTrajectory();
if (type == GOAL_TYPE_MOVING) {
armKinematics_->computeMovingTrajectory(newTrajectory, start, end);
} else if (type == GOAL_TYPE_POSE_STEPPING) {
armKinematics_->computePoseSteppingTrajectory(newTrajectory, start, end);
} else {
armKinematics_->computeSteppingTrajectory(newTrajectory, start, end);
}
return 0;
}
int updateTrajectory(int64_t startTime, float *aimAngles) {
if (nowTrajectoryIndex_ < 0) {
return OK;
}
if (trajectoryStartTime_[nowTrajectoryIndex_] == 0) {
trajectoryStartTime_[nowTrajectoryIndex_] = startTime;
}
return armKinematics_->updateTrajectory(&trajectoryHistory_[nowTrajectoryIndex_], aimAngles);
}
int getAngleFromTime(float *aimAngles, int64_t timeStamp) {
if (nowTrajectoryIndex_ < 0) {
return OK;
}
int timeStep = 0;
int aimIndex = -1;
int startIndex = nowTrajectoryIndex_ + 1;
int endIndex = trajectoryFullFlag_ == 1 ? startIndex : 0;
do {
startIndex = (startIndex + MAX_TRAJECTORY_HISTORY - 1) % MAX_TRAJECTORY_HISTORY;
if (trajectoryStartTime_[startIndex] < timeStamp) {
aimIndex = startIndex;
timeStep = static_cast<int>((timeStamp - trajectoryStartTime_[startIndex]) / ARM_FOLLOWING_PERIOD);
break;
}
} while (startIndex != endIndex);
if (aimIndex == -1) {
return UNKNOWN_ERR;
}
armKinematics_->getAngleForCheckingInner(&trajectoryHistory_[aimIndex], timeStep, aimAngles);
return 0;
}
void getAngleForChecking(float *aimAngles) {
if (nowTrajectoryIndex_ < 0) {
return;
}
T* nowTrajectory = &trajectoryHistory_[nowTrajectoryIndex_];
armKinematics_->getAngleForCheckingOffset(nowTrajectory, ARM_FOLLOWING_CHECKING_STEP * 2, aimAngles);
}
void getAngleForStarting(float *aimAngles) {
if (nowTrajectoryIndex_ < 0) {
return;
}
T* nowTrajectory = &trajectoryHistory_[nowTrajectoryIndex_];
armKinematics_->getAngleForCheckingOffset(nowTrajectory, ARM_FOLLOWING_CHECKING_STEP, aimAngles);
}
void stopTrajectory() {
armKinematics_->stopTrajectory(&trajectoryHistory_[nowTrajectoryIndex_]);
}
int continuePoseSteppingTrajectory(float *nowAngles, const float *direct) {
return armKinematics_->continuePoseSteppingTrajectory(&trajectoryHistory_[nowTrajectoryIndex_], nowAngles, direct);
}
T* getNowTrajectory() {
if (nowTrajectoryIndex_ < 0) {
return nullptr;
}
return &trajectoryHistory_[nowTrajectoryIndex_];
}
K *armKinematics_;
private:
T trajectoryHistory_[MAX_TRAJECTORY_HISTORY];
int64_t trajectoryStartTime_[MAX_TRAJECTORY_HISTORY];
int nowTrajectoryIndex_;
int trajectoryFullFlag_;
T* addTrajectory() {
nowTrajectoryIndex_++;
if (nowTrajectoryIndex_ >= MAX_TRAJECTORY_HISTORY) {
nowTrajectoryIndex_ = 0;
trajectoryFullFlag_ = 1;
}
trajectoryStartTime_[nowTrajectoryIndex_] = 0;
return &trajectoryHistory_[nowTrajectoryIndex_];
}
};
using KinematicsManager = TrajectoryManager<TrapezoidalTrajectory, TrapezoidalTrajectoryKinematics>;
using Trajectory = TrapezoidalTrajectory;
using Kinematics = TrapezoidalTrajectoryKinematics;
} // namespace ArmKinematicsNs
#endif // TRAJECTORY_HPP

51
HiveCoreR1/src/rm_arm_control/include/trapezoidal_trajectory_kinematics.hpp Normal file
View File

@@ -0,0 +1,51 @@
#ifndef TRAPEZOIDAL_TRAJECTORY_KINEMATICS_HPP
#define TRAPEZOIDAL_TRAJECTORY_KINEMATICS_HPP
#include "arm_define.h"
#include "rm_service.h"
#define STEPPING_UNIFORM_TIME_STAMP -1
struct TrapezoidalTrajectory {
float startAngles[USED_ARM_DOF];
float endAngles[USED_ARM_DOF];
float aim_velocity[USED_ARM_DOF];
float now_acceleration[USED_ARM_DOF];
int64_t startTime;
int accelerate_time_step;
int uniform_time_step;
int goalType;
int current_time_step;
int64_t stopTime;
};
class TrapezoidalTrajectoryKinematics
{
public:
TrapezoidalTrajectoryKinematics(RM_Service *rmService);
TrapezoidalTrajectoryKinematics();
~TrapezoidalTrajectoryKinematics();
void getAngleForCheckingInner(TrapezoidalTrajectory *trajectory, int timeStep, float *aimAngles);
void getAngleForCheckingOffset(TrapezoidalTrajectory *trajectory, int timeStepOffset, float *aimAngles);
void computeMovingTrajectory(TrapezoidalTrajectory *trajectory, const float *start, const float *end);
void computeSteppingTrajectory(TrapezoidalTrajectory *trajectory, const float *start, const float *speed);
void computePoseSteppingTrajectory(TrapezoidalTrajectory *trajectory, const float *start, const float *end);
int updateTrajectory(TrapezoidalTrajectory *trajectory, float *aimAngles);
void stopTrajectory(TrapezoidalTrajectory *trajectory);
int continuePoseSteppingTrajectory(TrapezoidalTrajectory *trajectory, float *nowAngles, const float *direct);
int ForwardKinematics(float *jointAngles, float *endEffectorPose);
int InverseKinematics(const float *endEffectorPose, float *jointAngles);
private:
RM_Service *rmService_;
float max_velocity_;
float max_acceleration_;
int max_accelerate_time_;
float max_accelerate_angle_;
float min_angle_for_pose_stepping_max_speed_;
};
#endif // TRAPEZOIDAL_TRAJECTORY_KINEMATICS_HPP

83
HiveCoreR1/src/rm_arm_control/launch/rm_arm_control.launch.py
View File

@@ -1,23 +1,72 @@
# 导入 ROS 2 launch 相关模块
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import TimerAction
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch.actions import OpaqueFunction
import os
def load_urdf(context, *args, **kwargs):
nodeList = []
# 获取包的共享目录路径（解析为实际字符串）
package_share = FindPackageShare(package='rm_arm_control').perform(context)
# 构建URDF文件的实际路径
urdf_path = PathJoinSubstitution(
[package_share, 'urdf', 'rm_65_b_description.urdf'] # 替换为你的URDF路径
).perform(context)
# 读取URDF文件内容
with open(urdf_path, 'r') as f:
urdf_content = f.read()
robot_controller = Node(
package='rm_arm_control',
executable='rm_arm_control',
name='rm_arm_control',
output='screen',
parameters=[{
'robot_description': urdf_content,
'use_sim_time': False
}]
)
nodeList.append(robot_controller)
# 创建机器人状态发布器节点
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': urdf_content}]
)
nodeList.append(robot_state_publisher)
'''joint_state_gui = Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
)
nodeList.append(joint_state_gui)'''
# 创建RViz节点（可选）
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=[
'-d', os.path.join(
FindPackageShare('rm_arm_control').find('rm_arm_control'),
'launch', 'robot_view.rviz' # 保存的RViz配置文件路径
)
],
parameters=[{
# 这里可以设置一些RViz参数
}]
)
nodeList.append(rviz_node)
return nodeList
def generate_launch_description():
# 创建节点启动描述
rm_arm_control_node = Node(
package='rm_arm_control', # 功能包名称
executable='rm_arm_control', # 可执行文件名称
name='rm_arm_control', # 节点名称（可自定义）
output='screen', # 输出到屏幕
parameters=[{"use_sim_time": False}] # 启用模拟时间
)
start_rm_arm_control_node = TimerAction(
period=1.0,
actions=[rm_arm_control_node],
)
# 组装launch描述
return LaunchDescription([
start_rm_arm_control_node
OpaqueFunction(function=load_urdf)
])

0
HiveCoreR1/src/arm_description/launch/robot_view.rviz → HiveCoreR1/src/rm_arm_control/launch/robot_view.rviz
View File

0
HiveCoreR1/src/arm_description/meshes/base_link.STL → HiveCoreR1/src/rm_arm_control/meshes/base_link.STL
View File

0
HiveCoreR1/src/arm_description/meshes/link1.STL → HiveCoreR1/src/rm_arm_control/meshes/link1.STL
View File

0
HiveCoreR1/src/arm_description/meshes/link2.STL → HiveCoreR1/src/rm_arm_control/meshes/link2.STL
View File

0
HiveCoreR1/src/arm_description/meshes/link3.STL → HiveCoreR1/src/rm_arm_control/meshes/link3.STL
View File

0
HiveCoreR1/src/arm_description/meshes/link4.STL → HiveCoreR1/src/rm_arm_control/meshes/link4.STL
View File

0
HiveCoreR1/src/arm_description/meshes/link5.STL → HiveCoreR1/src/rm_arm_control/meshes/link5.STL
View File

0
HiveCoreR1/src/arm_description/meshes/link6.STL → HiveCoreR1/src/rm_arm_control/meshes/link6.STL
View File

2
HiveCoreR1/src/rm_arm_control/package.xml
View File

@@ -8,7 +8,7 @@
<license>Apache-2.0</license>
<build_depend>ament_cmake</build_depend>
<depend>common_action_interface</depend>
<depend>interfaces</depend>
<depend>gjk_interface</depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>rclcpp_action</exec_depend>

1065
HiveCoreR1/src/rm_arm_control/simulator/rm_arm_simulator.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

319
HiveCoreR1/src/rm_arm_control/simulator/trapezoidal_trajectory_kinematics.cpp Normal file
View File

@@ -0,0 +1,319 @@
#include <stdexcept>
#include "arm_define.h"
#include "trapezoidal_trajectory_kinematics.hpp"
#include <cstring>
#include <cmath>
TrapezoidalTrajectoryKinematics::TrapezoidalTrajectoryKinematics(RM_Service *rmService)
: max_velocity_(1.0), max_acceleration_(0.25), rmService_(rmService)
{
max_accelerate_time_ = 4;
max_accelerate_angle_ = max_velocity_ * max_accelerate_time_;
min_angle_for_pose_stepping_max_speed_ = ARM_FOLLOWING_CHECKING_STEP * max_velocity_ * 2;
rm_robot_arm_model_e Mode = RM_MODEL_RM_65_E;
rm_force_type_e Type = RM_MODEL_RM_B_E;
rm_algo_init_sys_data(Mode, Type);
}
TrapezoidalTrajectoryKinematics::TrapezoidalTrajectoryKinematics()
: max_velocity_(1.0), max_acceleration_(0.25), rmService_(nullptr)
{
max_accelerate_time_ = 4;
max_accelerate_angle_ = max_velocity_ * max_accelerate_time_;
min_angle_for_pose_stepping_max_speed_ = ARM_FOLLOWING_CHECKING_STEP * max_velocity_ * 2;
rmService_ = new RM_Service();
int result = rmService_->rm_init(RM_TRIPLE_MODE_E);
if (result != 0) {
throw std::runtime_error("Failed to init rm.");
return;
}
rm_robot_arm_model_e Mode = RM_MODEL_RM_65_E;
rm_force_type_e Type = RM_MODEL_RM_B_E;
rm_algo_init_sys_data(Mode, Type);
}
TrapezoidalTrajectoryKinematics::~TrapezoidalTrajectoryKinematics() {
delete rmService_;
}
void TrapezoidalTrajectoryKinematics::getAngleForCheckingInner(TrapezoidalTrajectory *traj, int timeStep, float *aimAngles)
{
if (timeStep >= traj->accelerate_time_step * 2 + traj->uniform_time_step
&& traj->uniform_time_step != STEPPING_UNIFORM_TIME_STAMP) { // 分支预判优化
for (int i = 0; i < USED_ARM_DOF; ++i) {
aimAngles[i] = traj->endAngles[i];
}
return;
}
for (int i = 0; i < USED_ARM_DOF; ++i) {
if (timeStep < traj->accelerate_time_step) {
aimAngles[i] = traj->startAngles[i] + 0.5 * traj->now_acceleration[i] * (timeStep + 1) * timeStep;
} else if (traj->uniform_time_step == STEPPING_UNIFORM_TIME_STAMP){
aimAngles[i] = traj->startAngles[i] + 0.5 * traj->now_acceleration[i] * (traj->accelerate_time_step + 1) * (traj->accelerate_time_step);
aimAngles[i] += traj->aim_velocity[i] * (timeStep - traj->accelerate_time_step);
}else if (timeStep < traj->accelerate_time_step + traj->uniform_time_step) {
aimAngles[i] = traj->endAngles[i] - 0.5 * traj->now_acceleration[i] * (traj->accelerate_time_step + 1) * (traj->accelerate_time_step);
aimAngles[i] -= traj->aim_velocity[i] * (traj->uniform_time_step - (timeStep - traj->accelerate_time_step));
} else if (timeStep <= 2 * traj->accelerate_time_step + traj->uniform_time_step) {
int decelerate_time_step = timeStep - traj->accelerate_time_step - traj->uniform_time_step;
aimAngles[i] = traj->endAngles[i] - 0.5 * traj->now_acceleration[i] * (decelerate_time_step + 1) * decelerate_time_step;
}
}
}
void TrapezoidalTrajectoryKinematics::getAngleForCheckingOffset(TrapezoidalTrajectory *traj, int timeStepOffset, float *aimAngles)
{
int timeStep = traj->current_time_step + timeStepOffset;
getAngleForCheckingInner(traj, timeStep, aimAngles);
}
int TrapezoidalTrajectoryKinematics::updateTrajectory(TrapezoidalTrajectory *trajectory, float *aimAngles)
{
trajectory->current_time_step++;
if ((trajectory->goalType == GOAL_TYPE_POSE_STEPPING && trajectory->current_time_step >= trajectory->accelerate_time_step + trajectory->uniform_time_step) ||
(trajectory->current_time_step >= trajectory->accelerate_time_step * 2 + trajectory->uniform_time_step
&& trajectory->uniform_time_step != STEPPING_UNIFORM_TIME_STAMP)) {
for (int i = 0; i < USED_ARM_DOF; ++i) {
aimAngles[i] = trajectory->endAngles[i];
}
return TRAJECTORY_COMPLETE;
}
getAngleForCheckingInner(trajectory, trajectory->current_time_step, aimAngles);
return TRAJECTORY_ONGOING;
}
void TrapezoidalTrajectoryKinematics::computeMovingTrajectory(TrapezoidalTrajectory *newTrajectory, const float *start, const float *end)
{
float max_angle = 0.0;
float angle_diff[USED_ARM_DOF];
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->startAngles[i] = start[i];
newTrajectory->endAngles[i] = end[i];
angle_diff[i] = end[i] - start[i];
float abs_diff = fabs(angle_diff[i]);
if (abs_diff > max_angle) {
max_angle = abs_diff;
}
}
if (max_angle < 0.01) {
newTrajectory->accelerate_time_step = 0;
newTrajectory->uniform_time_step = 0;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->aim_velocity[i] = 0;
newTrajectory->now_acceleration[i] = 0;
newTrajectory->endAngles[i] = start[i];
}
return;
}
float aim_velocity = max_velocity_;
if (max_angle < max_accelerate_angle_) {
newTrajectory->accelerate_time_step = static_cast<int>(sqrt(max_angle / max_acceleration_)) + 1;
aim_velocity = max_angle / newTrajectory->accelerate_time_step;
newTrajectory->uniform_time_step = 0;
} else {
newTrajectory->accelerate_time_step = max_accelerate_time_;
newTrajectory->uniform_time_step = static_cast<int>((max_angle - max_accelerate_angle_) / max_velocity_);
}
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->aim_velocity[i] = aim_velocity * angle_diff[i] / max_angle;
newTrajectory->now_acceleration[i] = newTrajectory->aim_velocity[i] / newTrajectory->accelerate_time_step;
}
}
void TrapezoidalTrajectoryKinematics::computeSteppingTrajectory(TrapezoidalTrajectory *newTrajectory, const float *start, const float *speed)
{
float maxSpeed = 0.0;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->startAngles[i] = start[i];
if (fabs(speed[i]) > max_velocity_) {
newTrajectory->aim_velocity[i] = (speed[i] > 0 ? 1 : -1) * max_velocity_;
} else {
newTrajectory->aim_velocity[i] = speed[i];
}
if (fabs(newTrajectory->aim_velocity[i]) > maxSpeed) {
maxSpeed = fabs(newTrajectory->aim_velocity[i]);
}
}
newTrajectory->uniform_time_step = -1;
newTrajectory->accelerate_time_step = static_cast<int>(maxSpeed / max_acceleration_) + 1;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->now_acceleration[i] = newTrajectory->aim_velocity[i] / newTrajectory->accelerate_time_step;
newTrajectory->endAngles[i] = 0;
}
}
void TrapezoidalTrajectoryKinematics::computePoseSteppingTrajectory(TrapezoidalTrajectory *newTrajectory, const float *start, const float *end)
{
float max_angle = 0.0;
float angle_diff[USED_ARM_DOF];
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->startAngles[i] = start[i];
newTrajectory->endAngles[i] = end[i];
angle_diff[i] = end[i] - start[i];
float abs_diff = fabs(angle_diff[i]);
if (abs_diff > max_angle) {
max_angle = abs_diff;
}
}
if (max_angle < 0.01) {
newTrajectory->accelerate_time_step = 0;
newTrajectory->uniform_time_step = 0;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->aim_velocity[i] = 0;
newTrajectory->now_acceleration[i] = 0;
newTrajectory->endAngles[i] = start[i];
}
return;
}
float aim_velocity = max_velocity_;
newTrajectory->accelerate_time_step = max_accelerate_time_;
if (max_angle < min_angle_for_pose_stepping_max_speed_) {
aim_velocity = max_angle / (ARM_FOLLOWING_CHECKING_STEP * 2);
newTrajectory->uniform_time_step = ARM_FOLLOWING_CHECKING_STEP * 2 - max_accelerate_time_;
} else {
newTrajectory->uniform_time_step = static_cast<int>((max_angle - (max_accelerate_angle_ - 1) * 0.5) / max_velocity_);
}
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->aim_velocity[i] = aim_velocity * angle_diff[i] / max_angle;
newTrajectory->now_acceleration[i] = newTrajectory->aim_velocity[i] / max_accelerate_time_;
}
}
void TrapezoidalTrajectoryKinematics::stopTrajectory(TrapezoidalTrajectory *traj)
{
int formerPoseSteppingUniformTimeStep = traj->uniform_time_step;
if (traj->current_time_step < traj->accelerate_time_step) {
traj->accelerate_time_step = traj->current_time_step;
traj->uniform_time_step = 0;
} else if (traj->uniform_time_step == STEPPING_UNIFORM_TIME_STAMP ||
traj->current_time_step < traj->accelerate_time_step + traj->uniform_time_step) {
traj->uniform_time_step = traj->current_time_step - traj->accelerate_time_step;
}
traj->stopTime = traj->startTime + ARM_FOLLOWING_PERIOD * (traj->accelerate_time_step * 2 + traj->uniform_time_step);
for (int i = 0; i < USED_ARM_DOF; ++i) {
if (traj->goalType == GOAL_TYPE_POSE_STEPPING) {
traj->now_acceleration[i] = traj->aim_velocity[i] / traj->accelerate_time_step;
traj->endAngles[i] = traj->endAngles[i]
- traj->aim_velocity[i] * (formerPoseSteppingUniformTimeStep - traj->uniform_time_step);
traj->endAngles[i] += 0.5 * traj->now_acceleration[i] * traj->accelerate_time_step * traj->accelerate_time_step;
} else if (traj->current_time_step <= traj->accelerate_time_step + traj->uniform_time_step) {
traj->endAngles[i] = traj->startAngles[i] + traj->aim_velocity[i] * (traj->uniform_time_step + traj->accelerate_time_step);
}
}
if (traj->goalType == GOAL_TYPE_POSE_STEPPING) {
traj->goalType = GOAL_TYPE_MOVING;
}
}
int TrapezoidalTrajectoryKinematics::continuePoseSteppingTrajectory(TrapezoidalTrajectory *newTrajectory, float *nowAngles, const float *direct)
{
float poseTemp[POSE_DIMENSION] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
int result = ForwardKinematics(nowAngles, poseTemp);
if (result != 0) {
return UNKNOWN_ERR;
}
for (int i = 0; i < POSE_DIMENSION; i++) {
poseTemp[i] = direct[i] + poseTemp[i];
if (fabs(poseTemp[i]) < 0.0001) {
poseTemp[i] = 0.0f;
}
}
float newJoints[ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
for (int i = 0; i < USED_ARM_DOF; i++) {
newJoints[i] = nowAngles[i];
}
rm_inverse_kinematics_params_t inverse_params;
memcpy(inverse_params.q_in, newJoints, sizeof(newJoints));
inverse_params.q_pose.position.x = poseTemp[POSE_POSITION_X];
inverse_params.q_pose.position.y = poseTemp[POSE_POSITION_Y];
inverse_params.q_pose.position.z = poseTemp[POSE_POSITION_Z];
inverse_params.q_pose.quaternion.x = poseTemp[POSE_QUATERNION_X];
inverse_params.q_pose.quaternion.y = poseTemp[POSE_QUATERNION_Y];
inverse_params.q_pose.quaternion.z = poseTemp[POSE_QUATERNION_Z];
inverse_params.q_pose.quaternion.w = poseTemp[POSE_QUATERNION_W];
inverse_params.flag = 0;
result = rmService_->rm_algo_inverse_kinematics(NULL, inverse_params, newJoints);
if (result != 0){
return ARM_AIM_CANNOT_REACH; // Inverse kinematics failed
}
float angle_diff[USED_ARM_DOF];
float max_angle = 0.0;
float max_velocity = 0.0;
int fastestIndex = 0;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->endAngles[i] = newJoints[i];
angle_diff[i] = newTrajectory->endAngles[i] - nowAngles[i];
float abs_diff = fabs(angle_diff[i]);
if (abs_diff > max_angle) {
max_angle = abs_diff;
fastestIndex = i;
}
}
max_velocity = fabs(newTrajectory->aim_velocity[fastestIndex]);
if (max_velocity * (ARM_FOLLOWING_CHECKING_STEP * 2 + (max_accelerate_time_ - 1) * 0.5) >= max_angle) {
max_velocity = max_angle / (ARM_FOLLOWING_CHECKING_STEP * 2 + (max_accelerate_time_ - 1) * 0.5);
}
for (int i = 0; i < USED_ARM_DOF; ++i) {
if (max_angle < 0.0001) {
newTrajectory->aim_velocity[i] = 0;
} else {
newTrajectory->aim_velocity[i] = max_velocity * angle_diff[i] / max_angle;
}
}
newTrajectory->uniform_time_step += max_angle / max_velocity;
return 0;
}
int TrapezoidalTrajectoryKinematics::ForwardKinematics(float *jointAngles, float *endEffectorPose)
{
if (!rmService_)
{
return UNKNOWN_ERR; // Error: RM_Service not initialized
}
// Call the RM_Service's ForwardKinematics method
rm_pose_t pose = rmService_->rm_algo_forward_kinematics(NULL, jointAngles);
endEffectorPose[POSE_POSITION_X] = pose.position.x;
endEffectorPose[POSE_POSITION_Y] = pose.position.y;
endEffectorPose[POSE_POSITION_Z] = pose.position.z;
endEffectorPose[POSE_QUATERNION_X] = pose.quaternion.x;
endEffectorPose[POSE_QUATERNION_Y] = pose.quaternion.y;
endEffectorPose[POSE_QUATERNION_Z] = pose.quaternion.z;
endEffectorPose[POSE_QUATERNION_W] = pose.quaternion.w;
return 0;
}
int TrapezoidalTrajectoryKinematics::InverseKinematics(const float *endEffectorPose, float *jointAngles)
{
if (!rmService_)
{
return UNKNOWN_ERR; // Error: RM_Service not initialized
}
float nowJoints_[ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
for (int i = 0; i < USED_ARM_DOF; i++) {
nowJoints_[i] = jointAngles[i];
}
rm_inverse_kinematics_params_t inverse_params;
memcpy(inverse_params.q_in, nowJoints_, sizeof(nowJoints_));
inverse_params.q_pose.position.x = endEffectorPose[POSE_POSITION_X];
inverse_params.q_pose.position.y = endEffectorPose[POSE_POSITION_Y];
inverse_params.q_pose.position.z = endEffectorPose[POSE_POSITION_Z];
inverse_params.q_pose.quaternion.x = endEffectorPose[POSE_QUATERNION_X];
inverse_params.q_pose.quaternion.y = endEffectorPose[POSE_QUATERNION_Y];
inverse_params.q_pose.quaternion.z = endEffectorPose[POSE_QUATERNION_Z];
inverse_params.q_pose.quaternion.w = endEffectorPose[POSE_QUATERNION_W];
inverse_params.flag = 0;
int result = rmService_->rm_algo_inverse_kinematics(NULL, inverse_params, nowJoints_);
if (result != 0){
return ARM_AIM_CANNOT_REACH; // Inverse kinematics failed
}
memcpy(jointAngles, nowJoints_, sizeof(float) * USED_ARM_DOF);
return 0;
}

128
HiveCoreR1/src/rm_arm_control/src/goal_manager.cpp Normal file
View File

@@ -0,0 +1,128 @@
#include <cstring>
#include "goal_manager.hpp"
GoalManager::GoalManager()
: startGoalIndex_(0), endGoalIndex_(0), goalFullFlag_(0), currentGoalRunOffFlag_(0)
{
memset(nowJointsGoalList_, 0, sizeof(nowJointsGoalList_));
memset(goalIndexList_, 0, sizeof(goalIndexList_));
}
GoalManager::~GoalManager() {}
int GoalManager::AddGoal(const float points[GOAL_DATA_LENGTH], const int64_t &index, int type)
{
if (goalFullFlag_ == 1) {
return -1;
}
memcpy(nowJointsGoalList_[endGoalIndex_], points, sizeof(float) * GOAL_DATA_LENGTH);
goalIndex[index] = endGoalIndex_;
goalIndexList_[endGoalIndex_] = index;
goalResultList_[endGoalIndex_] = 0;
goalTypeList_[endGoalIndex_] = type;
endGoalIndex_++;
if (endGoalIndex_ >= MAX_ARM_GOAL_COUNT) {
endGoalIndex_ = 0;
}
if (endGoalIndex_ == startGoalIndex_) {
goalFullFlag_ = 1;
}
return 0;
}
int GoalManager::ConsumeGoal()
{
if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
return -1;
}
goalIndex.erase(goalIndexList_[startGoalIndex_]);
startGoalIndex_++;
if (startGoalIndex_ >= MAX_ARM_GOAL_COUNT) {
startGoalIndex_ = 0;
}
currentGoalRunOffFlag_ = 0;
goalFullFlag_ = 0;
return 0;
}
const float* GoalManager::GetGoal() const
{
if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
return nullptr;
}
return nowJointsGoalList_[startGoalIndex_];
}
int GoalManager::RemoveGoal(int64_t index)
{
auto it = goalIndex.find(index);
if (it == goalIndex.end()) {
return -1;
}
int delIndex = it->second;
goalResultList_[delIndex] = ARM_GOAL_CANCELLED;
return 0;
}
void GoalManager::ClearGoals()
{
startGoalIndex_ = 0;
endGoalIndex_ = 0;
goalFullFlag_ = 0;
goalIndex.clear();
}
int GoalManager::SetNowGoalResult(int result)
{
if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
return -1;
}
goalResultList_[startGoalIndex_] = result;
if (goalTypeList_[startGoalIndex_] == GOAL_TYPE_POSE_STEPPING && result != 0) {
goalTypeList_[startGoalIndex_] = GOAL_TYPE_MOVING;
goalResultList_[startGoalIndex_] = 0;
}
return 0;
}
int GoalManager::GetNowGoalResult() const
{
if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
return -1;
}
return goalResultList_[startGoalIndex_];
}
int64_t GoalManager::GetGoalNowIndex()
{
if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
return -1;
}
return goalIndexList_[startGoalIndex_];
}
int64_t GoalManager::GetCompeletedGoalNowIndex()
{
if ((startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) || currentGoalRunOffFlag_ == 0) {
return -1;
}
return goalIndexList_[startGoalIndex_];
}
int GoalManager::GetGoalType() const
{
if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
return -1;
}
return goalTypeList_[startGoalIndex_];
}
void GoalManager::GoalRunOff()
{
currentGoalRunOffFlag_ = 1;
}
int GoalManager::GetRunOffFlag() const
{
return currentGoalRunOffFlag_;
}

87
HiveCoreR1/src/rm_arm_control/src/main.cpp Normal file
View File

@@ -0,0 +1,87 @@
#include "arm_driver.hpp"
#include "rm_arm_driver.hpp"
#include "rm_arm_node.hpp"
#include "trajectory.hpp"
#include <thread>
std::thread publicThread_;
void PublicThread(ArmDriver *armDriver)
{
while (rclcpp::ok()) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
armDriver->PublicThreadUpdate();
}
}
void custom_api_log(const char* message, va_list args)
{
if (!message) {
fprintf(stderr, "Error: message is a null pointer");
return;
}
char buffer[1024];
vsnprintf(buffer, sizeof(buffer), message, args);
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), " %s", buffer);
}
int InitSimulatorDriver(ArmDriver **armDriver, Kinematics **armKinematics)
{
*armDriver = new ArmDriver();
(*armDriver)->RegArmStatusCallback(RmArmNode::CallbackRealtimeArmJointState);
int initResult = (*armDriver)->Init();
if (initResult != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to initialize arm driver. %d", initResult);
return UNKNOWN_ERR;
}
*armKinematics = new Kinematics();
std::thread(PublicThread, *armDriver).detach();
return 0;
}
int InitRmArmDriver(ArmDriver **armDriver, Kinematics **armKinematics)
{
auto rmArmDriver = new RmArmDriver();
rmArmDriver->SetCustomApiLog(custom_api_log);
rmArmDriver->RegArmStatusCallback(ArmDriver::CallArmStatusCallback);
int initResult = rmArmDriver->Init();
if (initResult != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to initialize rm arm driver. %d", initResult);
return UNKNOWN_ERR;
}
*armDriver = new ArmDriver();
(*armDriver)->RegArmStatusCallback(RmArmNode::CallbackRealtimeArmJointState);
(*armDriver)->RegArmMoveCallback(RmArmDriver::ArmMove);
initResult = (*armDriver)->Init();
if (initResult != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to initialize arm driver. %d", initResult);
return UNKNOWN_ERR;
}
*armKinematics = new Kinematics(rmArmDriver->rmService_);
return 0;
}
int main(int argc, char *argv[]) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Starting rm_arm_control node...");
rclcpp::init(argc, argv);
ArmDriver *armDriver = NULL;
Kinematics *armKinematics = NULL;
int initResult = InitSimulatorDriver(&armDriver, &armKinematics);
if (initResult != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to initialize rm arm driver. %d", initResult);
return UNKNOWN_ERR;
}
auto rm_arm_control = std::make_shared<RmArmNode>(armDriver, armKinematics);
rclcpp::spin(rm_arm_control);
rclcpp::shutdown();
return 0;
}

563
HiveCoreR1/src/rm_arm_control/src/rm_arm_handler.cpp
View File

@@ -2,367 +2,290 @@
const float maxJointSpeedList[USED_ARM_DOF] = {4.0, 4.0, 4.0, 4.0, 4.0, 4.0};
RmArmHandler::RmArmHandler(RM_Service* roboticService,
const char *robot_ip_address, int robot_port, rm_realtime_push_config_t *config)
RmArmHandler::RmArmHandler(ArmDriver *driver, int armType, sem_t *sem, KinematicsManager *trapezoidalTrajectory):
armType_(armType), driver_(driver), trapezoidalTrajectory_(trapezoidalTrajectory)
{
roboticService_ = roboticService;
goalManager_ = new GoalManager();
armStatus = ARM_STATUS_ERROR;
startGoalIndex_ = 0;
endGoalIndex_ = 0;
goalFullFlag_ = 0;
poseNeedUpdate_ = 1;
sem_ = sem;
memset(nowJoints_, 0, sizeof(nowJoints_));
memset(nextJoints_, 0, sizeof(nextJoints_));
robotHandle_ = roboticService_->rm_create_robot_arm(robot_ip_address, robot_port);
if (robotHandle_ == NULL) {
printf("Failed to create arm(%s) handle.\n", robot_ip_address);
return;
}
if (robotHandle_->id <= 0) {
printf("Failed to create arm(%s) handle. %d\n", robot_ip_address, robotHandle_->id);
return;
}
int result = roboticService_-> rm_set_realtime_push(robotHandle_, *config);
if (result != 0) {
printf("Failed to set realtime push for arm(%s).\n", robot_ip_address);
return;
}
result = rm_set_arm_max_line_speed(robotHandle_, 0.01f);
if (result != 0) {
printf("Failed to set max line speed for arm(%s).\n", robot_ip_address);
return;
}
int info_result = roboticService_->rm_get_robot_info(robotHandle_, &robot_info_);
if (info_result != 0) {
printf("Failed to get robot info(%s)\n", robot_ip_address);
return;
}
int dof = robot_info_.arm_dof;
if (dof != USED_ARM_DOF) {
printf("Invalid degree of freedom(%s)\n", robot_ip_address);
return;
}
armStatus = ARM_STATUS_READY;
}
RmArmHandler::~RmArmHandler()
{
if(robotHandle_ != NULL) {
roboticService_->rm_delete_robot_arm(robotHandle_);
robotHandle_ = NULL;
if (goalManager_ != NULL) {
delete goalManager_;
goalManager_ = NULL;
}
if (trapezoidalTrajectory_ != NULL) {
delete trapezoidalTrajectory_;
trapezoidalTrajectory_ = NULL;
}
}
void RmArmHandler::ArmCommandCallbackStep(float points[ARM_DOF])
void PrintData(ArmStatusCallbackData *data)
{
int result = roboticService_->rm_movej_canfd(robotHandle_, points, false, 0);
if (result != 0) {
printf("Error moving to joint space! \n");
}
}
void RmArmHandler::ArmCommandCallbackDirect(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
{
if (armStatus == ARM_STATUS_INIT || armStatus >= ARM_STATUS_ERROR) {
printf("Arm is not ready. Current status: %d\n", armStatus);
return;
}
for (float val : msg->data) {
printf("%.6f ", val);
}
printf("\n");
if (msg->data.size() != ARM_DOF) {
// 处理数据大小不匹配的情况，例如打印错误信息
printf("Error: Data size (%zu) does not match ARM_DOF (%d)\n",
msg->data.size(), ARM_DOF);
// 可以根据需要添加错误处理逻辑，如返回或使用默认值
return;
}
float q_out[ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
std::copy(msg->data.begin(), msg->data.end(), q_out);
ArmCommandCallbackStep(q_out);
return;
float nowJointsTemp[ARM_DOF]; // 注意：const数组不能直接赋值，需先复制到非const数组
std::copy(msg->data.begin(), msg->data.end(), nowJointsTemp);
const float* positions = nowJointsTemp;
rm_inverse_kinematics_params_t inverse_params;
memcpy(inverse_params.q_in, nowJoints_, sizeof(nowJoints_));
rm_pose_t pose = roboticService_->rm_algo_forward_kinematics(robotHandle_, positions);
memcpy(&(inverse_params.q_pose), &pose, sizeof(pose));
inverse_params.flag = 1;
int result = rm_algo_inverse_kinematics(robotHandle_, inverse_params, q_out);
if (result != 0)
{
printf("Failed to calculate inverse kinematics. %d\n", result);
return;
}
ArmCommandCallbackStep(q_out);
return;
}
void PrintData(rm_realtime_arm_joint_state_t *data)
{
printf("==================MYCALLBACK==================\n");
printf("errCode: %d\n", data->errCode);
printf("arm_ip: %s\n", data->arm_ip);
printf("current:");
for (size_t i = 0; i < ARM_DOF; i++){
printf("%.3f ", data->joint_status.joint_current[i]);
}
printf("\n");
printf("enable:");
for (size_t i = 0; i < ARM_DOF; i++){
printf("%d ", data->joint_status.joint_en_flag[i]);
}
printf("\n");
printf("error code:");
for (size_t i = 0; i < ARM_DOF; i++){
printf("%d ", data->joint_status.joint_err_code[i]);
}
printf("\n");
printf("position:");
for (size_t i = 0; i < ARM_DOF; i++){
printf("%.3f ", data->joint_status.joint_position[i]);
}
printf("\n");
printf("speed:");
for (size_t i = 0; i < ARM_DOF; i++){
printf("%.3f ", data->joint_status.joint_speed[i]);
}
printf("\n");
printf("temperature:");
for (size_t i = 0; i < ARM_DOF; i++){
printf("%.3f ", data->joint_status.joint_temperature[i]);
}
printf("\n");
printf("voltage:");
for (size_t i = 0; i < ARM_DOF; i++){
printf("%.3f ", data->joint_status.joint_voltage[i]);
}
printf("\n");
}
void RmArmHandler::ArmDealCallBackInfo(rm_realtime_arm_joint_state_t *data)
{
ARM_STATUS_E temp = armStatus == ARM_STATUS_READY ? ARM_STATUS_MOVING : armStatus;
armStatus = ARM_STATUS_READY;
sensor_msgs::msg::JointState armStates;
armStates.position = {data->joint_status.joint_position[0], data->joint_status.joint_position[1], data->joint_status.joint_position[2], data->joint_status.joint_position[3], data->joint_status.joint_position[4], data->joint_status.joint_position[5]};
for (size_t i = 0; i < ARM_DOF; i++)
{
if (fabs(nowJoints_[i] - data->joint_status.joint_position[i]) > 0.02) {
armStatus = temp;
}
nowJoints_[i] = data->joint_status.joint_position[i];
}
if (armStatus != ARM_STATUS_READY){
PrintData(data);
if (armStatus == ARM_STATUS_MOVING) {
poseNeedUpdate_ = 1;
}
}
}
void simArmMove(RmArmHandler *armHandler, float joints[ARM_DOF])
{
rm_realtime_arm_joint_state_t data;
for (size_t i = 0; i < ARM_DOF; i++) {
data.joint_status.joint_position[i] = joints[i];
}
armHandler->ArmDealCallBackInfo(&data);
}
int RmArmHandler::AddNextPoint(float points[ARM_DOF], const int64_t &index)
{
if (goalFullFlag_ == 1) {
printf("Goal buffer full.\n");
return -1;
}
memcpy(nowJointsGoalList_[endGoalIndex_], points, sizeof(float) * ARM_DOF);
goalIndex[index] = endGoalIndex_;
goalIndexList_[endGoalIndex_] = index;
endGoalIndex_++;
if (endGoalIndex_ >= MAX_ARM_GOAL_COUNT) {
endGoalIndex_ = 0;
}
if (endGoalIndex_ == startGoalIndex_) {
goalFullFlag_ = 1;
}
poseNeedUpdate_ = 1;
return 0;
}
int RmArmHandler::ConsumeNextPoint()
{
if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
printf("No goal point to consume.\n");
return -1;
}
goalIndex.erase(goalIndexList_[startGoalIndex_]);
startGoalIndex_++;
if (startGoalIndex_ >= MAX_ARM_GOAL_COUNT) {
startGoalIndex_ = 0;
}
goalFullFlag_ = 0;
return 0;
}
void RmArmHandler::UpdateNowPoseFromJoints()
{
rm_pose_t pose;
if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
pose = roboticService_->rm_algo_forward_kinematics(robotHandle_, nowJoints_);
} else {
pose = roboticService_->rm_algo_forward_kinematics(robotHandle_, nowJointsGoalList_[endGoalIndex_ - 1]);
}
nowAimPose_[POSE_POSITION_X] = pose.position.x;
nowAimPose_[POSE_POSITION_Y] = pose.position.y;
nowAimPose_[POSE_POSITION_Z] = pose.position.z;
nowAimPose_[POSE_EULER_ROLL] = pose.euler.rx;
nowAimPose_[POSE_EULER_PITCH] = pose.euler.ry;
nowAimPose_[POSE_EULER_YAW] = pose.euler.rz;
poseNeedUpdate_ = 0;
}
int RmArmHandler::AddNextPointFromPose(const float pose[POSE_DIMENSION], const int64_t &index)
{
float q_out[ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
rm_inverse_kinematics_params_t inverse_params;
if (startGoalIndex_ == endGoalIndex_) {
if (goalFullFlag_ == 1) {
printf("Goal buffer full.\n");
return -1;
}
memcpy(inverse_params.q_in, nowJoints_, sizeof(nowJoints_));
} else {
memcpy(inverse_params.q_in, nowJointsGoalList_[endGoalIndex_ - 1], sizeof(nowJoints_));
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "==================MYCALLBACK==================");
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "errCode: %d", data->errCode);
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "current:");
for (size_t i = 0; i < USED_ARM_DOF; i++){
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointCurrent[i]);
}
rm_pose_t target_pose;
target_pose.position.x = pose[POSE_POSITION_X];
target_pose.position.y = pose[POSE_POSITION_Y];
target_pose.position.z = pose[POSE_POSITION_Z];
target_pose.euler.rx = pose[POSE_EULER_ROLL];
target_pose.euler.ry = pose[POSE_EULER_PITCH];
target_pose.euler.rz = pose[POSE_EULER_YAW];
memcpy(&(inverse_params.q_pose), &target_pose, sizeof(target_pose));
inverse_params.flag = 1;
int result = rm_algo_inverse_kinematics(robotHandle_, inverse_params, q_out);
if (result == 0) {
result = AddNextPoint(q_out, index);
if (result == 0) {
nowAimPose_[POSE_POSITION_X] = pose[POSE_POSITION_X];
nowAimPose_[POSE_POSITION_Y] = pose[POSE_POSITION_Y];
nowAimPose_[POSE_POSITION_Z] = pose[POSE_POSITION_Z];
nowAimPose_[POSE_EULER_ROLL] = pose[POSE_EULER_ROLL];
nowAimPose_[POSE_EULER_PITCH] = pose[POSE_EULER_PITCH];
nowAimPose_[POSE_EULER_YAW] = pose[POSE_EULER_YAW];
poseNeedUpdate_ = 0;
}
return result;
} else {
printf("Inverse kinematics failed.\n");
return -1;
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "enable:");
for (size_t i = 0; i < USED_ARM_DOF; i++){
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d ", data->jointEnFlag[i]);
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "error code:");
for (size_t i = 0; i < USED_ARM_DOF; i++){
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d ", data->jointErrCode[i]);
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "position:");
for (size_t i = 0; i < USED_ARM_DOF; i++){
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointAngle[i]);
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "speed:");
for (size_t i = 0; i < USED_ARM_DOF; i++){
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointSpeed[i]);
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "temperature:");
for (size_t i = 0; i < USED_ARM_DOF; i++){
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointTemp[i]);
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "voltage:");
for (size_t i = 0; i < USED_ARM_DOF; i++){
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointVoltage[i]);
}
}
void RmArmHandler::ArmDealCallBackInfo(ArmStatusCallbackData *data)
{
ARM_STATUS_E temp = ARM_STATUS_READY;
for (size_t i = 0; i < USED_ARM_DOF; i++)
{
if (fabs(nowJoints_[i] - data->jointAngle[i]) > 0.02) {
temp = ARM_STATUS_WAITING;
}
nowJoints_[i] = data->jointAngle[i];
}
if (armStatus == ARM_STATUS_WAITING && temp == ARM_STATUS_READY) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ARM_STATUS_READY.");
armStatus = ARM_STATUS_READY;
for (size_t i = 0; i < USED_ARM_DOF; i++) {
nextJoints_[i] = nowJoints_[i];
}
sem_post(sem_);
} else if (armStatus == ARM_STATUS_READY && temp == ARM_STATUS_WAITING) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ARM_STATUS_WAITING.");
armStatus = ARM_STATUS_WAITING;
}
/*if (temp != ARM_STATUS_READY){
PrintData(data);
}*/
}
int RmArmHandler::AddNextPoint(float points[GOAL_DATA_LENGTH], const int64_t &index, int type)
{
std::unique_lock lock(rw_mutex_);
return goalManager_->AddGoal(points, index, type);
}
int RmArmHandler::AddNextPointFromPoseMoving(const float pose[POSE_DIMENSION], const int64_t &index)
{
float q_out[GOAL_DATA_LENGTH] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
for (int i = 0; i < USED_ARM_DOF; i++) {
q_out[i] = nextJoints_[i];
}
int result = trapezoidalTrajectory_->armKinematics_->InverseKinematics(pose, q_out);
if (result == 0) {
std::unique_lock lock(rw_mutex_);
return goalManager_->AddGoal(q_out, index, GOAL_TYPE_MOVING);
}
return result;
}
int RmArmHandler::GetNextPointFromPoseStepping(const float pose[POSE_DIMENSION], float *joints)
{
float poseTemp[POSE_DIMENSION] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
int result = trapezoidalTrajectory_->armKinematics_->ForwardKinematics(nextJoints_, poseTemp);
if (result != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ForwardKinematics error.");
return UNKNOWN_ERR;
}
for (int i = 0; i < POSE_DIMENSION; i++) {
poseTemp[i] = pose[i] + poseTemp[i];
if (fabs(poseTemp[i]) < 0.0001) {
poseTemp[i] = 0.0f;
}
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Current pose:%.6f %.6f %.6f %.6f %.6f %.6f %.6f",
poseTemp[0], poseTemp[1], poseTemp[2], poseTemp[3], poseTemp[4], poseTemp[5], poseTemp[6]);
float q_out[GOAL_DATA_LENGTH] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
for (int i = 0; i < USED_ARM_DOF; i++) {
q_out[i] = nextJoints_[i];
}
result = trapezoidalTrajectory_->armKinematics_->InverseKinematics(poseTemp, q_out);
if (result != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "InverseKinematics error.");
return ARM_AIM_CANNOT_REACH;
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Next joints:%.6f %.6f %.6f %.6f %.6f %.6f",
q_out[0], q_out[1], q_out[2], q_out[3], q_out[4], q_out[5]);
for (int i = 0; i < USED_ARM_DOF; i++) {
joints[i] = q_out[i];
}
return 0;
}
int RmArmHandler::AddNextPointFromPose(const float pose[POSE_DIMENSION], const int64_t &index, int type)
{
if (type == GOAL_TYPE_MOVING) {
return AddNextPointFromPoseMoving(pose, index);
} else if (type == GOAL_TYPE_STEPPING) {
std::unique_lock lock(rw_mutex_);
return goalManager_->AddGoal(pose, index, GOAL_TYPE_POSE_STEPPING);
}
return -1;
}
int RmArmHandler::DeletePoint(int64_t index)
{
auto it = goalIndex.find(index);
if (it == goalIndex.end()) {
printf("No such goal index to delete.\n");
return -1;
int nowIndex;
{
std::shared_lock lock(rw_mutex_);
nowIndex = goalManager_->GetGoalNowIndex();
}
int delIndex = it->second;
if (delIndex == startGoalIndex_) {
ConsumeNextPoint();
if (index == nowIndex) {
ArmStop(0);
return 0;
}
int nextIndex = delIndex + 1;
if (nextIndex >= MAX_ARM_GOAL_COUNT) {
nextIndex = 0;
}
while (nextIndex != endGoalIndex_ || (nextIndex == endGoalIndex_ && goalFullFlag_ == 1)) {
memcpy(nowJointsGoalList_[delIndex], nowJointsGoalList_[nextIndex], sizeof(float) * ARM_DOF);
goalIndexList_[delIndex] = goalIndexList_[nextIndex];
goalIndex[goalIndexList_[nextIndex]] = delIndex;
delIndex = nextIndex;
nextIndex++;
if (nextIndex >= MAX_ARM_GOAL_COUNT) {
nextIndex = 0;
}
}
endGoalIndex_ = delIndex;
if (endGoalIndex_ < 0) {
endGoalIndex_ = MAX_ARM_GOAL_COUNT - 1;
}
goalFullFlag_ = 0;
goalIndex.erase(it);
return 0;
std::unique_lock lock(rw_mutex_);
return goalManager_->RemoveGoal(index);
}
void RmArmHandler::ArmSimu()
void RmArmHandler::ArmRun(int64_t timeStamp)
{
if (armStatus == ARM_STATUS_INIT || armStatus >= ARM_STATUS_ERROR) {
armStatus = ARM_STATUS_READY;
if (armStatus != ARM_STATUS_MOVING) {
return;
}
if (startGoalIndex_ == endGoalIndex_ && goalFullFlag_ == 0) {
armStatus = ARM_STATUS_READY;
int result, type;
{
std::shared_lock lock(rw_mutex_);
type = goalManager_->GetGoalType();
}
result = trapezoidalTrajectory_->updateTrajectory(timeStamp, nextJoints_);
if (driver_->ArmMove(nextJoints_, armType_) != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Error moving to joint space! ");
return;
}
for (int i = 0; i < USED_ARM_DOF; i++) {
float diff = nowJointsGoalList_[startGoalIndex_][i] - nowJoints_[i];
if (fabs(diff) < fabs(nowSpeed_[i])) {
nowSpeed_[i] = diff;
if (result == TRAJECTORY_COMPLETE) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Trajectory complete.");
if (type == GOAL_TYPE_POSE_STEPPING) {
{
std::shared_lock lock(rw_mutex_);
const float* pose = goalManager_->GetGoal();
result = trapezoidalTrajectory_->continuePoseSteppingTrajectory(nextJoints_, pose);
}
if (result != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "GetNextPointFromPoseStepping failed.");
ArmStop(ARM_AIM_CANNOT_REACH);
return;
}
} else {
if (diff > 0) {
nowSpeed_[i] += 2.0;
if (nowSpeed_[i] > maxJointSpeedList[i]) {
nowSpeed_[i] = maxJointSpeedList[i];
}
} else {
nowSpeed_[i] -= 2.0;
if (nowSpeed_[i] < -maxJointSpeedList[i]) {
nowSpeed_[i] = -maxJointSpeedList[i];
}
int runOffFlag;
{
std::shared_lock lock(rw_mutex_);
runOffFlag = goalManager_->GetRunOffFlag();
}
if (fabs(diff) < fabs(nowSpeed_[i])) {
nowSpeed_[i] = diff;
if (runOffFlag == 0) {
std::unique_lock lock(rw_mutex_);
goalManager_->GoalRunOff();
armStatus = ARM_STATUS_WAITING;
}
}
}
for (int i = 0; i < USED_ARM_DOF; i++) {
nextJoints_[i] = nowJoints_[i] + nowSpeed_[i];
}
}
void RmArmHandler::ArmRun()
int RmArmHandler::StartNewTrajectory()
{
if (armStatus == ARM_STATUS_READY) {
return;
std::shared_lock lock(rw_mutex_);
const float* goal = goalManager_->GetGoal();
if (goal == NULL) {
return ARM_NOW_NO_GOAL;
}
/*int result = roboticService_->rm_movej_canfd(robotHandle_, nextJoints_, false, 0);
if (result != 0) {
printf("Error moving to joint space! \n");
return;
}*/
simArmMove(this, nextJoints_);
int allClose = 1;
if (goalManager_->GetNowGoalResult() != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to start new trajectory because canceled. GoalRunOff1.");
goalManager_->GoalRunOff();
return ARM_GOAL_CANCELLED;
}
float start[USED_ARM_DOF];
for (int i = 0; i < USED_ARM_DOF; i++) {
if (fabs(nextJoints_[i] - nowJointsGoalList_[startGoalIndex_][i]) > 0.02) {
allClose = 0;
break;
start[i] = nextJoints_[i];
}
int goalType = goalManager_->GetGoalType();
if (goalType == GOAL_TYPE_POSE_STEPPING) {
float joints[USED_ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
if (GetNextPointFromPoseStepping(goal, joints) != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to start new trajectory. GoalRunOff2.");
goalManager_->GoalRunOff();
return ARM_AIM_CANNOT_REACH;
} else {
trapezoidalTrajectory_->computeTrajectory(start, joints, goalType);
}
} else {
trapezoidalTrajectory_->computeTrajectory(start, goal, goalType);
}
if (allClose == 1) {
ConsumeNextPoint();
}
return OK;
}
void RmArmHandler::ArmMove()
{
armStatus = ARM_STATUS_MOVING;
}
void RmArmHandler::ArmStop(int result)
{
std::unique_lock lock(rw_mutex_);
trapezoidalTrajectory_->stopTrajectory();
goalManager_->SetNowGoalResult(result);
}
void RmArmHandler::ArmStopStart(int result)
{
std::unique_lock lock(rw_mutex_);
trapezoidalTrajectory_->stopTrajectory();
goalManager_->SetNowGoalResult(result);
goalManager_->GoalRunOff();
}
int RmArmHandler::GoalReached()
{
std::unique_lock lock(rw_mutex_);
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "GoalReached, locked.");
int result = goalManager_->GetNowGoalResult();
goalManager_->ConsumeGoal();
return result;
}
int64_t RmArmHandler::GetCurrentRunOffGoalIndex()
{
std::shared_lock lock(rw_mutex_);
return goalManager_->GetCompeletedGoalNowIndex();
}
int64_t RmArmHandler::GetCurrentGoalIndex()
{
std::shared_lock lock(rw_mutex_);
return goalManager_->GetGoalNowIndex();
}

450
HiveCoreR1/src/rm_arm_control/src/rm_arm_node.cpp
View File

@@ -2,103 +2,74 @@
#include "rm_arm_simulator.hpp"
#include "rm_arm_node.hpp"
static ArmSimulator *armSimulaor_;
static ArmSimulator *armSimulator_;
using ArmGoalSharedPtr =
std::shared_ptr<const common_action_interface::action::Arm::Goal>;
std::shared_ptr<const interfaces::action::Arm::Goal>;
namespace ArmCommandHash
{
using ArmCommand = int(*)(RmArmHandler *handle, const ArmGoalSharedPtr goal);
int AngleStepOn(RmArmHandler *handle, const ArmGoalSharedPtr goal)
static int AngleStepOn(RmArmHandler *handle, const ArmGoalSharedPtr goal)
{
if (goal->data_length != USED_ARM_DOF) {
printf("AngleStepOn data length error.");
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleStepOn data length error.");
return -1;
}
if (handle->armStatus == ARM_STATUS_MOVING) {
printf("Arm is moving.\n");
return -1;
}
float points[ARM_DOF] = {0};
float points[GOAL_DATA_LENGTH] = {0};
std::string speeds = "aim speed ";
for (int i = 0; i < USED_ARM_DOF; i++) {
points[i] = goal->data_array[i] + handle->nowJoints_[i];
}
int result = handle->AddNextPoint(points, goal->command_id);
if (result == 0) {
handle->armStatus = ARM_STATUS_STEPPING;
points[i] = goal->data_array[i];
speeds += std::to_string(points[i]) + " ";
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", speeds.c_str());
int result = handle->AddNextPoint(points, goal->command_id, GOAL_TYPE_STEPPING);
return result;
}
int AngleDirectMove(RmArmHandler *handle, std::shared_ptr<const ArmAction::Goal> goal)
static int AngleDirectMove(RmArmHandler *handle, std::shared_ptr<const ArmAction::Goal> goal)
{
if (goal->data_length != USED_ARM_DOF) {
printf("AngleDirectMove data length error.");
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove data length error.");
return -1;
}
if (handle->armStatus == ARM_STATUS_STEPPING) {
printf("Arm is stepping.\n");
return -1;
}
float points[ARM_DOF] = {0};
float points[GOAL_DATA_LENGTH] = {0};
for (int i = 0; i < USED_ARM_DOF; i++) {
points[i] = goal->data_array[i];
}
int result = handle->AddNextPoint(points, goal->command_id);
if (result == 0) {
handle->armStatus = ARM_STATUS_MOVING;
if (armSimulator_->CheckJointsAngleLimit(points) != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove joints limit error.");
return ARM_AIM_CANNOT_REACH;
}
int result = handle->AddNextPoint(points, goal->command_id, GOAL_TYPE_MOVING);
return result;
}
int PoseStepOn(RmArmHandler *handle, std::shared_ptr<const ArmAction::Goal> goal)
static int PoseStepOn(RmArmHandler *handle, std::shared_ptr<const ArmAction::Goal> goal)
{
if (goal->data_length != POSE_DIMENSION) {
printf("PoseStepOn data length error.");
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "PoseStepOn data length error.");
return -1;
}
if (handle->armStatus == ARM_STATUS_MOVING) {
printf("Arm is moving.\n");
return -1;
}
if (handle->poseNeedUpdate_) {
handle->UpdateNowPoseFromJoints();
}
float points[POSE_DIMENSION] = {0};
for (int i = 0; i < POSE_DIMENSION; i++) {
points[i] = goal->data_array[i] + handle->nowAimPose_[i];
}
int result = handle->AddNextPointFromPose(points, goal->command_id);
if (result == 0) {
handle->armStatus = ARM_STATUS_STEPPING;
}
return result;
}
int PoseDirectMove(RmArmHandler *handle, std::shared_ptr<const ArmAction::Goal> goal)
{
if (goal->data_length != POSE_DIMENSION) {
printf("PoseDirectMove data length error.");
return -1;
}
if (handle->armStatus == ARM_STATUS_STEPPING) {
printf("Arm is stepping.\n");
return -1;
}
if (handle->poseNeedUpdate_) {
handle->UpdateNowPoseFromJoints();
}
float points[POSE_DIMENSION] = {0};
float points[GOAL_DATA_LENGTH] = {0};
for (int i = 0; i < POSE_DIMENSION; i++) {
points[i] = goal->data_array[i];
}
int result = handle->AddNextPointFromPose(points, goal->command_id);
if (result == 0) {
handle->armStatus = ARM_STATUS_MOVING;
return handle->AddNextPointFromPose(points, goal->command_id, GOAL_TYPE_STEPPING);
}
static int PoseDirectMove(RmArmHandler *handle, std::shared_ptr<const ArmAction::Goal> goal)
{
if (goal->data_length != POSE_DIMENSION) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "PoseDirectMove data length error.");
return -1;
}
return result;
float points[GOAL_DATA_LENGTH] = {0};
for (int i = 0; i < POSE_DIMENSION; i++) {
points[i] = goal->data_array[i];
}
return handle->AddNextPointFromPose(points, goal->command_id, GOAL_TYPE_MOVING);
}
const std::unordered_map<int, ArmCommand> armCommandMap = {
@@ -127,59 +98,59 @@ static RmArmHandler *GetArmHandler(RmArmNode *node, int body_id)
}
// 机器人控制器构造函数
RmArmNode::RmArmNode() : Node("rm_arm_control")
RmArmNode::RmArmNode(ArmDriver *armDriver, Kinematics *kinematics) : Node("rm_arm_control"), armDriver_(armDriver)
{
localNodePtr = this;
rmService_ = new RM_Service();
sem_init(&sem_, 0, 0);
int result = rmService_->rm_init(RM_TRIPLE_MODE_E);
if (result != 0) {
printf("Initialization failed with error code: %d.\n", result);
throw std::runtime_error("Failed to init rm.");
return;
}
KinematicsManager *leftTrajectory = new KinematicsManager(kinematics);
KinematicsManager *rightTrajectory = new KinematicsManager(kinematics);
rmService_->rm_set_log_call_back(custom_api_log, 3);
rmService_->rm_realtime_arm_state_call_back(CallbackRmRealtimeArmJointState);
robotDescriptionSub_ = this->create_subscription<std_msgs::msg::String>
("robot_description", 10, std::bind(&RmArmNode::CallbackGetRobotDescription, this, std::placeholders::_1));
const char *robot_ip_address = "192.168.1.16";
int robot_port = 8080;
rm_realtime_push_config_t config = {100, true, 8089, 0, "192.168.1.100"};
leftArmHandler_ = new RmArmHandler(rmService_, robot_ip_address, robot_port, &config);
leftArmHandler_ = new RmArmHandler(armDriver_, LEFT_ARM, &sem_, leftTrajectory);
if (leftArmHandler_ != NULL && leftArmHandler_->armStatus == ARM_STATUS_READY) {
leftArmHandler_->armCommandSub_ = this->create_subscription<std_msgs::msg::Float64MultiArray>
("left_arm_command", 10, std::bind(&RmArmHandler::ArmCommandCallbackDirect, leftArmHandler_, std::placeholders::_1));
printf("Left arm handler initialization success.\n");
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Left arm handler initialization success.");
} else {
printf("Left arm handler initialization failed.\n");
leftArmHandler_->armCommandSub_ = NULL;
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Left arm handler initialization failed.");
}
robot_ip_address = "192.168.1.18";
robot_port = 8080;
rightArmHandler_ = new RmArmHandler(rmService_, robot_ip_address, robot_port, &config);
rightArmHandler_ = new RmArmHandler(armDriver_, RIGHT_ARM, &sem_, rightTrajectory);
if (rightArmHandler_ != NULL && rightArmHandler_->armStatus == ARM_STATUS_READY) {
rightArmHandler_->armCommandSub_ = this->create_subscription<std_msgs::msg::Float64MultiArray>
("right_arm_command", 10, std::bind(&RmArmHandler::ArmCommandCallbackDirect, rightArmHandler_, std::placeholders::_1));
printf("Right arm handler initialization success.\n");
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Right arm handler initialization success.");
} else {
printf("Right arm handler initialization failed.\n");
rightArmHandler_->armCommandSub_ = NULL;
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Right arm handler initialization failed.");
}
armStatesPub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
std::thread{std::bind(&RmArmNode::ArmHandleExecute, this)}.detach();
this->action_server_ = rclcpp_action::create_server<ArmAction>(
this,
"arm_controller",
std::bind(&RmArmNode::ArmHandleGoal, this, std::placeholders::_1, std::placeholders::_2),
std::bind(&RmArmNode::ArmHandleCancel, this, std::placeholders::_1),
std::bind(&RmArmNode::ArmHandleAccepted, this, std::placeholders::_1));
"ArmAction",
std::bind(&RmArmNode::ArmActionGoal, this, std::placeholders::_1, std::placeholders::_2),
std::bind(&RmArmNode::ArmActionCancel, this, std::placeholders::_1),
std::bind(&RmArmNode::ArmActionAccepted, this, std::placeholders::_1));
printf("init compelete.\n");
this->declare_parameter<std::string>("robot_description", "");
std::string urdf_string = this->get_parameter("robot_description").as_string();
/*leftArmHandler_->nowJoints_[0] = 180.0;
leftArmHandler_->nowJoints_[1] = 180.0;*/
if (urdf_string != "") {
armStates.name = {
"left_joint1", "left_joint2", "left_joint3", "left_joint4", "left_joint5", "left_joint6",
"right_joint1", "right_joint2", "right_joint3", "right_joint4", "right_joint5", "right_joint6"};
armStates.header.frame_id = "base_link";
armSimulator_ = new ArmSimulator(urdf_string, leftArmHandler_->nowJoints_, rightArmHandler_->nowJoints_,
leftArmHandler_->trapezoidalTrajectory_, rightArmHandler_->trapezoidalTrajectory_);
robotDescriptionSub_ = NULL;
} else {
armSimulator_ = NULL;
robotDescriptionSub_ = this->create_subscription<std_msgs::msg::String>
("robot_description", 10, std::bind(&RmArmNode::CallbackGetRobotDescription, this, std::placeholders::_1));
}
std::thread{std::bind(&RmArmNode::ArmActionCheck, this)}.detach();
std::thread{std::bind(&RmArmNode::ArmActionExecute, this)}.detach();
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Robot arm control node initialized.");
}
RmArmNode::~RmArmNode()
@@ -187,109 +158,282 @@ RmArmNode::~RmArmNode()
armStatesPub_.reset();
delete leftArmHandler_;
delete rightArmHandler_;
delete armSimulaor_;
delete rmService_;
delete armSimulator_;
}
void RmArmNode::CallbackGetRobotDescription(const std_msgs::msg::String::SharedPtr msg)
{
printf("Received robot_description message.\n");
if (armSimulaor_ == NULL) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received robot_description message.");
if (armSimulator_ == NULL) {
armStates.name = {
"left_joint1", "left_joint2", "left_joint3", "left_joint4", "left_joint5", "left_joint6",
"right_joint1", "right_joint2", "right_joint3", "right_joint4", "right_joint5", "right_joint6"};
armStates.header.frame_id = "base_link";
armSimulaor_ = new ArmSimulator(this, msg);
armSimulator_ = new ArmSimulator(msg, leftArmHandler_->nowJoints_, rightArmHandler_->nowJoints_,
leftArmHandler_->trapezoidalTrajectory_, rightArmHandler_->trapezoidalTrajectory_);
}
}
rclcpp_action::GoalResponse RmArmNode::ArmHandleGoal(
rclcpp_action::GoalResponse RmArmNode::ArmActionGoal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const ArmAction::Goal> goal)
{
printf("Received goal request\n");
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received goal request.");
// Accept the goal
if (ArmCommandHash::armCommandMap.find(goal->command_id) == ArmCommandHash::armCommandMap.end()) {
printf("Received goal with invalid command id: %ld\n", goal->command_id);
if (ArmCommandHash::armCommandMap.find(goal->data_type) == ArmCommandHash::armCommandMap.end()) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received goal with invalid command id: %d", goal->data_type);
return rclcpp_action::GoalResponse::REJECT;
}
RmArmHandler *handle = GetArmHandler(this, goal->body_id);
if (handle == NULL) {
printf("arm validation error.");
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm validation error.");
return rclcpp_action::GoalResponse::REJECT;
}
if (handle->armStatus == ARM_STATUS_ERROR || handle->armStatus == ARM_STATUS_INIT) {
printf("Arm in error status.");
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Arm in error status.");
return rclcpp_action::GoalResponse::REJECT;
}
int result = ArmCommandHash::armCommandMap.at(goal->command_id)(handle, goal);
int result = ArmCommandHash::armCommandMap.at(goal->data_type)(handle, goal);
if (result != 0) {
printf("Failed to process the goal.\n");
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to process the goal.");
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse RmArmNode::ArmHandleCancel(
rclcpp_action::CancelResponse RmArmNode::ArmActionCancel(
const std::shared_ptr<GoalHandleArm> goal_handle)
{
printf("Received request to cancel goal\n");
ArmGoalSharedPtr goal = goal_handle->get_goal();
int64_t index = goal->command_id;
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received request to cancel goal. Index: %ld", index);
int result = 0;
RmArmHandler *handle = GetArmHandler(this, goal->body_id);
if (handle == NULL) {
printf("arm validation error.");
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm validation error.");
return rclcpp_action::CancelResponse::REJECT;
}
int result = handle->DeletePoint(goal->command_id);
result = handle->DeletePoint(goal->command_id);
if (result != 0) {
printf("Failed to cancel the goal.\n");
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to cancel the goal.");
return rclcpp_action::CancelResponse::REJECT;
}
// Accept the cancel request
return rclcpp_action::CancelResponse::ACCEPT;
}
void RmArmNode::ArmHandleExecute()
void RmArmNode::ArmActionFeedback(int coll)
{
printf("Executing goal\n");
for (auto it = goal_handles_.begin(); it != goal_handles_.end(); ++it) {
auto goal_handle = it->second;
ArmGoalSharedPtr goal = goal_handle->get_goal();
RmArmHandler *handle = GetArmHandler(this, goal->body_id);
if (handle == NULL) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm validation error.");
continue;
}
if (handle->armStatus == ARM_STATUS_ERROR || handle->armStatus == ARM_STATUS_INIT) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Arm in error status.");
continue;
}
auto feedback = std::make_shared<ArmAction::Feedback>();
feedback->command_id = goal->command_id;
feedback->int_lenth = 1;
feedback->float_length = USED_ARM_DOF;
feedback->int_data_array = {coll};
feedback->float_data_array = {handle->nowJoints_[0], handle->nowJoints_[1], handle->nowJoints_[2],
handle->nowJoints_[3], handle->nowJoints_[4], handle->nowJoints_[5]};
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Publishing feedback for command id: %ld, joints: %f, %f, %f, %f, %f, %f",
feedback->command_id, feedback->float_data_array[0], feedback->float_data_array[1],
feedback->float_data_array[2], feedback->float_data_array[3],
feedback->float_data_array[4], feedback->float_data_array[5]);
goal_handle->publish_feedback(feedback);
}
}
void RmArmNode::ArmActionFinish(int64_t index, int result)
{
rclcpp::Time endColTime = this->get_clock()->now();
auto goal_handle_pair = goal_handles_.find(index);
if (goal_handle_pair != goal_handles_.end()) {
auto goal_handle = goal_handle_pair->second;
auto goalResult = std::make_shared<ArmAction::Result>();
goalResult->end_time = endColTime.nanoseconds();
goalResult->result = result;
goalResult->command_id = index;
if (goal_handle->is_canceling()) {
goal_handle->canceled(goalResult);
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Goal %ld canceled.", index);
} else if (result == OK){
goal_handle->succeed(goalResult);
} else {
goal_handle->abort(goalResult);
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Goal %ld aborted.", index);
}
goal_handles_.erase(index);
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Goal %ld finished. now aim cnt %ld.", index, goal_handles_.size());
} else {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Goal handle not found for index %ld.", index);
}
}
void RmArmNode::StartNewGoal(RmArmHandler *handle)
{
if (goal_handles_.size() == 0) {
return;
}
int ret;
int64_t index = handle->GetCurrentRunOffGoalIndex();
if (index != -1) {
ret = handle->GoalReached();
ArmActionFinish(index, ret);
}
index = handle->GetCurrentGoalIndex();
if (goal_handles_.find(index) == goal_handles_.end()) {
return;
}
do {
ret = handle->StartNewTrajectory();
if (ret == 0) {
ret = armSimulator_->UpdateCollisionForNewGoal(handle->armType_);
if (ret == SAFE) {
handle->ArmMove();
} else {
handle->ArmStopStart(ARM_COLLISION);
ret = ARM_COLLISION;
}
}
if (ret != ARM_NOW_NO_GOAL && ret != 0) {
index = handle->GetCurrentRunOffGoalIndex();
handle->GoalReached();
ArmActionFinish(index, ret);
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to start new goal with error code: %d.", ret);
}
} while (ret != 0 && ret != ARM_NOW_NO_GOAL);
}
int g_checkCnt = 0;
int64_t g_totalColTime = 0;
int64_t g_maxColTime = 0;
void RmArmNode::ArmActionCheck()
{
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Checking\n");
rclcpp::Time startTime = get_clock()->now();
int64_t nextCheckPoint = startTime.nanoseconds() + ARM_FOLLOWING_CHECKING;
struct timespec timeout;
timeout.tv_sec = 0;
timeout.tv_nsec = ARM_FOLLOWING_CHECKING; // 20 milliseconds
int step = 0;
bool busy = false;
int64_t maxUsedTime = 0;
// Execute the goal
while (rclcpp::ok()) {
if (armSimulaor_ != NULL) {
if (!busy) {
int result = sem_timedwait(&sem_, &timeout);
if (leftArmHandler_ != NULL && (leftArmHandler_->armStatus == ARM_STATUS_READY)) {
StartNewGoal(leftArmHandler_);
}
if (rightArmHandler_ != NULL && (rightArmHandler_->armStatus == ARM_STATUS_READY)) {
StartNewGoal(rightArmHandler_);
}
}
int64_t currentTimeNs = get_clock()->now().nanoseconds();
if (currentTimeNs >= nextCheckPoint) {
step++;
nextCheckPoint += ARM_FOLLOWING_CHECKING;
int coll = armSimulator_->CheckCollision();
g_checkCnt++;
int64_t durationCol = (get_clock()->now().nanoseconds() - currentTimeNs);
g_totalColTime += durationCol;
if (g_maxColTime < durationCol) {
g_maxColTime = durationCol;
}
if (g_checkCnt % 2000 == 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()),
"Collision check avg time: %ld us, max time: %ld us",
g_totalColTime / g_checkCnt / 1000, g_maxColTime / 1000);
g_checkCnt = 0;
g_totalColTime = 0;
}
if (coll != SAFE) {
// if (coll == ARM_AIM_CANNOT_REACH) {
leftArmHandler_->ArmStop(ARM_COLLISION);
rightArmHandler_->ArmStop(ARM_COLLISION);
ArmActionFeedback(coll);
step = 0;
} else if (step >= 5) {
ArmActionFeedback(coll);
step = 0;
}
}
int64_t afterCheckTimeNs = get_clock()->now().nanoseconds();
if (afterCheckTimeNs >= nextCheckPoint) {
busy = true;
int64_t usedTime = afterCheckTimeNs - currentTimeNs;
if (usedTime > 10 * 1e6) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm action check loop is too slow!, %ld",
usedTime);
}
} else {
timeout.tv_nsec = nextCheckPoint - afterCheckTimeNs;
busy = false;
}
}
}
void RmArmNode::ArmActionExecute()
{
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Executing\n");
rclcpp::Time startTime = get_clock()->now();
int64_t startTimeNs = startTime.nanoseconds();
// Execute the goal
while (rclcpp::ok()) {
if (armSimulator_ != NULL) {
if (leftArmHandler_ != NULL) {
leftArmHandler_->ArmSimu();
leftArmHandler_->ArmRun(startTimeNs);
}
if (rightArmHandler_ != NULL) {
rightArmHandler_->ArmSimu();
rightArmHandler_->ArmRun(startTimeNs);
}
}
startTimeNs += ARM_FOLLOWING_PERIOD;
int64_t currentTimeNs = get_clock()->now().nanoseconds();
int64_t sleepTime = (startTimeNs - (currentTimeNs)) / 1e3;
if (sleepTime > 0) {
std::this_thread::sleep_for(std::chrono::microseconds(sleepTime));
} else {
if (sleepTime < -10 * 1e3) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm action execute loop is too slow!, %ld",
-sleepTime);
}
int collisionStatus = armSimulaor_->CheckCollision();
if ((collisionStatus & LEFT_ARM_COLLISION) == 0) {
leftArmHandler_->ArmRun();
}
if ((collisionStatus & RIGHT_ARM_COLLISION) == 0) {
rightArmHandler_->ArmRun();
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
void RmArmNode::ArmHandleAccepted(const std::shared_ptr<GoalHandleArm> goal_handle)
void RmArmNode::ArmActionAccepted(const std::shared_ptr<GoalHandleArm> goal_handle)
{
printf("Goal accepted\n");
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Goal accepted. Index: %ld", goal_handle->get_goal()->command_id);
// Start executing the goal
goal_handles_[goal_handle->get_goal()->command_id] = goal_handle;
}
void RmArmNode::CallbackRmRealtimeArmJointState(rm_realtime_arm_joint_state_t data)
void RmArmNode::CallbackRealtimeArmJointState(ArmStatusCallbackData *data)
{
/*if (strcmp(data.arm_ip, "192.168.1.16") == 0 && leftArmHandler_ != NULL) {
leftArmHandler_->ArmDealCallBackInfo(&data);
if (data->armId == LEFT_ARM && leftArmHandler_ != NULL) {
leftArmHandler_->ArmDealCallBackInfo(data);
}
else if (strcmp(data.arm_ip, "192.168.1.18") == 0 && rightArmHandler_ != NULL) {
rightArmHandler_->ArmDealCallBackInfo(&data);
}*/
if (armStatesPub_ != NULL && armSimulaor_ != NULL) {
else if (data->armId == RIGHT_ARM && rightArmHandler_ != NULL) {
rightArmHandler_->ArmDealCallBackInfo(data);
}
if (armStatesPub_ != NULL && armSimulator_ != NULL) {
armStates.position = {leftArmHandler_->nowJoints_[0], leftArmHandler_->nowJoints_[1],
leftArmHandler_->nowJoints_[2], leftArmHandler_->nowJoints_[3],
leftArmHandler_->nowJoints_[4], leftArmHandler_->nowJoints_[5],
@@ -306,27 +450,3 @@ void RmArmNode::CallbackRmRealtimeArmJointState(rm_realtime_arm_joint_state_t da
armStatesPub_->publish(armStates);
}
}
void RmArmNode::custom_api_log(const char* message, va_list args)
{
if (!message) {
fprintf(stderr, "Error: message is a null pointer\n");
return;
}
char buffer[1024];
vsnprintf(buffer, sizeof(buffer), message, args);
printf(" %s\n", buffer);
}
int main(int argc, char *argv[]) {
printf("Starting rm_arm_control node...\n");
rclcpp::init(argc, argv);
auto rm_arm_control = std::make_shared<RmArmNode>();
rclcpp::spin(rm_arm_control);
rclcpp::shutdown();
return 0;
}

758
HiveCoreR1/src/rm_arm_control/src/rm_arm_simulator.cpp
View File

@@ -1,758 +0,0 @@
#include <memory>
#include <vector>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/collision_detection/collision_common.h>
#include <moveit/collision_detection_bullet/collision_detector_allocator_bullet.h>
#include "rm_arm_simulator.hpp"
using namespace Eigen;
#define MAX_ARM_POLYHEDRONS_CNT 8
#define ARM_POLYHEDRONS_POINT 8
#define ARM_POLYHEDRONS_CNT 15
static const bool LinkCollisionDetecMatrix[ARM_DOF * 2][ARM_DOF * 2] = {
// BASE L1 L2 L3 L4 L5 L6 BASE R1 R2 R3 R4 R5 R6
{ false, false, false, false, true, true, true, false, false, false, true, true, true, true }, // BASE
{ false, false, false, false, false, true, true, false, true, true, true, true, true, true, }, // L1
{ false, false, false, false, false, true, true, false, true, true, true, true, true, true, }, // L2
{ false, false, false, false, false, false, false, true, true, true, true, true, true, true, }, // L3
{ true , false, false, false, false, false, false, true, true, true, true, true, true, true, }, // L4
{ true , true, true, false, false, false, false, true, true, true, true, true, true, true, }, // L5
{ true , true, true, false, false, false, false, true, true, true, true, true, true, true, }, // L6
{ false, false, false, true , true , true , true , false , false , false , false , true , true , true, }, // BASE
{ false, true , true , true , true , true , true , false , false , false , false , false, true, true, }, // R1
{ false, true , true , true , true , true , true , false , false , false , false , false , true , true, }, // R2
{ true , true , true , true , true , true , true , false , false , false , false , false , false , false, }, // R3
{ true , true , true , true , true , true , true , true , false , false , false , false , false , false, }, // R4
{ true , true , true , true , true , true , true , true , true , true , false , false , false , false, }, // R5
{ true , true , true , true , true , true , true , true , true , true , false , false , false , false, } // R6
};
static std::map<std::string, int> g_linkMap = {
{"base_link_leftarm", 0},
{"LeftLink1", 1},
{"LeftLink2", 2},
{"LeftLink3", 3},
{"LeftLink4", 4},
{"LeftLink5", 5},
{"LeftLink6", 6},
{"base_link_rightarm", 7},
{"RightLink1", 8},
{"RightLink2", 9},
{"RightLink3", 10},
{"RightLink4", 11},
{"RightLink5", 12},
{"RightLink6", 13},
{"base_link", 14}
};
enum OriginBoxIndex {
BASE = 14,
LEFT = 0,
RIGHT = 7
};
using namespace Eigen;
std::vector<Vector3d> g_origin = {
Vector3d(0, 0, 0), // BASE
Vector3d(0, 0, 0), // L1
Vector3d(0, 0, 0), // L2
Vector3d(0, 0, 0), // L3
Vector3d(0, 0, 0), // L4
Vector3d(0, 0, 0), // L5
Vector3d(0, 0, 0), // L6
Vector3d(0, 0, 0), // BASE
Vector3d(0, 0, 0), // R1
Vector3d(0, 0, 0), // R2
Vector3d(0, 0, 0), // R3
Vector3d(0, 0, 0), // R4
Vector3d(0, 0, 0), // R5
Vector3d(0, 0, 0) // R6
};
std::vector<Vector3d> g_offset = {
Vector3d(0, 0, 0), // BASE
Vector3d(0, 0, 0), // L1
Vector3d(0, 0, 0), // L2
Vector3d(0, 0, 0), // L3
Vector3d(0, 0, 0), // L4
Vector3d(0, 0, 0), // L5
Vector3d(0, 0, 0), // L6
Vector3d(0, 0, 0), // BASE
Vector3d(0, 0, 0), // R1
Vector3d(0, 0, 0), // R2
Vector3d(0, 0, 0), // R3
Vector3d(0, 0, 0), // R4
Vector3d(0, 0, 0), // R5
Vector3d(0, 0, 0) // R6
};
std::vector<Vector3d> g_dir = {
Vector3d(0, 0, 0), // BASE
Vector3d::UnitX() * -1, // L1
Vector3d::UnitZ(), // L2
Vector3d::UnitZ(), // L3
Vector3d::UnitX() * -1, // L4
Vector3d::UnitZ(), // L5
Vector3d::UnitX() * -1, // L6
Vector3d(0, 0, 0), // BASE
Vector3d::UnitX(), // R1
Vector3d::UnitZ() * -1, // R2
Vector3d::UnitZ() * -1, // R3
Vector3d::UnitX(), // R4
Vector3d::UnitZ() * -1, // R5
Vector3d::UnitX(), // R6
};
static void InitArmSinglePolyhedron(ArmSimulator *simulator, int index, Polyhedron *poly,
Vector3d &origin, const Vector3d &offset, const Matrix3d &rotation)
{
float length = simulator->collision_structures[index].dimensions[0]; // 长度（沿u方向）
float width = simulator->collision_structures[index].dimensions[1]; // 宽度（沿v方向）
float height = simulator->collision_structures[index].dimensions[2]; // 高度（沿法向量方向）
Vector3d lenVec = rotation * Vector3d(length, 0, 0); // 长度方向
Vector3d widVec = rotation * Vector3d(0, width, 0); // 宽度方向
Vector3d heiVec = rotation * Vector3d(0, 0, height); // 高度方向
Vector3d bottomLeft = rotation * offset + origin;
(*poly)[0] = bottomLeft;
(*poly)[1] = bottomLeft + lenVec;
(*poly)[2] = bottomLeft + widVec;
(*poly)[3] = (*poly)[1] + widVec;
(*poly)[4] = bottomLeft + heiVec;
(*poly)[5] = (*poly)[1] + heiVec;
(*poly)[6] = (*poly)[2] + heiVec;
(*poly)[7] = (*poly)[3] + heiVec;
if (index < BASE - 1) {
origin = origin + rotation * g_origin[index + 1];
}
}
static void InitArmPolyhedronsList(ArmSimulator *simulator)
{
simulator->armPolyhedrons_.resize(ARM_POLYHEDRONS_CNT);
simulator->armPolyhedronsNext_.resize(ARM_POLYHEDRONS_CNT);
simulator->addedOBBs_.resize(ARM_POLYHEDRONS_CNT);
simulator->addedOBBsNext_.resize(ARM_POLYHEDRONS_CNT);
simulator->armPolyhedrons_[BASE].clear();
/*printf("origin:\n");
for (int i = 0; i < BASE; ++i) {
printf("%d: %.4f %.4f %.4f\n", i, g_origin[i].x(), g_origin[i].y(), g_origin[i].z());
}
printf("offset:\n");
for (int i = 0; i < BASE; ++i) {
printf("%d: %.4f %.4f %.4f\n", i, g_offset[i].x(), g_offset[i].y(), g_offset[i].z());
}*/
for (int i = 0; i < ARM_POLYHEDRONS_CNT; i++) {
for (int j = 0; j < ARM_POLYHEDRONS_POINT; j++) {
simulator->armPolyhedrons_[i].push_back(Vector3d(0, 0, 0));
simulator->armPolyhedronsNext_[i].push_back(Vector3d(0, 0, 0));
}
}
Vector3d offset_temp = {0, 0 ,0};
Matrix3d rotation = Matrix3d::Identity();
InitArmSinglePolyhedron(simulator, BASE, &simulator->armPolyhedrons_[BASE],
offset_temp,
Vector3d(simulator->collision_structures[BASE].dimensions[0] / 2,
simulator->collision_structures[BASE].dimensions[1] / 2,
simulator->collision_structures[BASE].dimensions[2] / 2),
rotation);
InitArmSinglePolyhedron(simulator, BASE, &simulator->armPolyhedronsNext_[BASE],
offset_temp,
Vector3d(simulator->collision_structures[BASE].dimensions[0] / 2,
simulator->collision_structures[BASE].dimensions[1] / 2,
simulator->collision_structures[BASE].dimensions[2] / 2),
rotation);
Vector3d offset_vec = g_origin[LEFT];
for (int i = LEFT; i < RIGHT; i++) {
InitArmSinglePolyhedron(simulator, i, &simulator->armPolyhedrons_[i],
offset_vec,
g_offset[i],
rotation);
InitArmSinglePolyhedron(simulator, i, &simulator->armPolyhedronsNext_[i],
offset_vec,
g_offset[i],
rotation);
}
offset_vec = g_origin[RIGHT];
for (int i = RIGHT; i < BASE; i++) {
InitArmSinglePolyhedron(simulator, i, &simulator->armPolyhedrons_[i],
offset_vec,
g_offset[i],
rotation);
InitArmSinglePolyhedron(simulator, i, &simulator->armPolyhedronsNext_[i],
offset_vec,
g_offset[i],
rotation);
}
}
static int RemakeCollisionStructureList(ArmSimulator *simulator,
std::vector<CollisionStructure> &collision_structures_temp)
{
printf("共提取到 %zu 个碰撞结构\n", collision_structures_temp.size());
simulator->collision_structures.resize(g_linkMap.size());
for (const auto& col : collision_structures_temp) {
int index = g_linkMap[col.link_name];
simulator->collision_structures[index] = col;
}
for (const auto& col : simulator->collision_structures) {
printf("链接: %s, 类型: %s, ",
col.link_name.c_str(),
col.geometry_type.c_str());
printf("尺寸: %.2f x %.2f x %.2f\n",
col.dimensions[0],
col.dimensions[1],
col.dimensions[2]);
}
return 0;
}
static double toRadians(float degrees) {
return (static_cast<double>(degrees)) * M_PI / 180.0;
}
static void UpdateArmPolyhedrons(ArmSimulator *simulator)
{
if (simulator == NULL) return;
if (simulator->armPolyhedrons_.size() != ARM_POLYHEDRONS_CNT) return;
Vector3d offset_vec = g_origin[LEFT] + g_origin[LEFT + 1];
Vector3d normal = Vector3d(-1, 0, 0);
// Matrix3d rotation = AngleAxisf(-M_PI / 2, Vector3d::UnitX()) * Matrix3d::Identity();
Matrix3d rotation = Matrix3d::Identity();
Vector3d new_normal;
simulator->addedOBBs_[LEFT] = gjk_interface::createOBBFromVertices(simulator->armPolyhedrons_[LEFT],
simulator->collision_structures[LEFT].dimensions[0],
simulator->collision_structures[LEFT].dimensions[1],
simulator->collision_structures[LEFT].dimensions[2]);
simulator->addedOBBsNext_[LEFT] = gjk_interface::createOBBFromVertices(simulator->armPolyhedronsNext_[LEFT],
simulator->collision_structures[LEFT].dimensions[0],
simulator->collision_structures[LEFT].dimensions[1],
simulator->collision_structures[LEFT].dimensions[2]);
simulator->addedOBBs_[RIGHT] = gjk_interface::createOBBFromVertices(simulator->armPolyhedrons_[RIGHT],
simulator->collision_structures[RIGHT].dimensions[0],
simulator->collision_structures[RIGHT].dimensions[1],
simulator->collision_structures[RIGHT].dimensions[2]);
simulator->addedOBBsNext_[RIGHT] = gjk_interface::createOBBFromVertices(simulator->armPolyhedronsNext_[RIGHT],
simulator->collision_structures[RIGHT].dimensions[0],
simulator->collision_structures[RIGHT].dimensions[1],
simulator->collision_structures[RIGHT].dimensions[2]);
for (int i = LEFT + 1; i < RIGHT; i++) {
double angleRad = toRadians(simulator->armNode_->leftArmHandler_->nowJoints_[i - LEFT - 1]);
rotation = AngleAxisd(angleRad, rotation * g_dir[i]) * rotation;
new_normal = rotation * normal;
/*printf("Link %d normal: (%.4f, %.4f, %.4f) rotation: \n"
"(%.4f, %.4f, %.4f)\n"
"(%.4f, %.4f, %.4f)\n"
"(%.4f, %.4f, %.4f)\n",
i, new_normal.x(), new_normal.y(), new_normal.z(),
rotation(0, 0), rotation(0, 1), rotation(0, 2),
rotation(1, 0), rotation(1, 1), rotation(1, 2),
rotation(2, 0), rotation(2, 1), rotation(2, 2));*/
InitArmSinglePolyhedron(simulator, i, &simulator->armPolyhedrons_[i],
offset_vec,
g_offset[i],
rotation);
simulator->addedOBBs_[i] = gjk_interface::createOBBFromVertices(simulator->armPolyhedrons_[i],
simulator->collision_structures[i].dimensions[0],
simulator->collision_structures[i].dimensions[1],
simulator->collision_structures[i].dimensions[2]);
}
offset_vec = g_origin[LEFT] + g_origin[LEFT + 1];
normal = Vector3d(-1, 0, 0);
rotation = Matrix3d::Identity();
for (int i = LEFT + 1; i < RIGHT; i++) {
double angleRad = toRadians(simulator->armNode_->leftArmHandler_->nextJoints_[i - LEFT - 1]);
rotation = AngleAxisd(angleRad, rotation * g_dir[i]) * rotation;
new_normal = rotation * normal;
InitArmSinglePolyhedron(simulator, i, &simulator->armPolyhedronsNext_[i],
offset_vec,
g_offset[i],
rotation);
simulator->addedOBBsNext_[i] = gjk_interface::createOBBFromVertices(simulator->armPolyhedronsNext_[i],
simulator->collision_structures[i].dimensions[0],
simulator->collision_structures[i].dimensions[1],
simulator->collision_structures[i].dimensions[2]);
}
offset_vec = g_origin[RIGHT] + g_origin[RIGHT + 1];
normal = Vector3d(1, 0, 0);
rotation = Matrix3d::Identity();
for (int i = RIGHT + 1; i < BASE; i++) {
double angleRad = toRadians(simulator->armNode_->rightArmHandler_->nowJoints_[i - LEFT - 1]);
rotation = AngleAxisd(angleRad, rotation * g_dir[i]) * rotation;
new_normal = rotation * normal;
InitArmSinglePolyhedron(simulator, i, &simulator->armPolyhedrons_[i],
offset_vec,
g_offset[i],
rotation);
simulator->addedOBBs_[i] = gjk_interface::createOBBFromVertices(simulator->armPolyhedrons_[i],
simulator->collision_structures[i].dimensions[0],
simulator->collision_structures[i].dimensions[1],
simulator->collision_structures[i].dimensions[2]);
}
offset_vec = g_origin[RIGHT] + g_origin[RIGHT + 1];
normal = Vector3d(1, 0, 0);
rotation = Matrix3d::Identity();
for (int i = RIGHT + 1; i < BASE; i++) {
double angleRad = toRadians(simulator->armNode_->rightArmHandler_->nextJoints_[i - LEFT - 1]);
rotation = AngleAxisd(angleRad, rotation * g_dir[i]) * rotation;
new_normal = rotation * normal;
InitArmSinglePolyhedron(simulator, i, &simulator->armPolyhedronsNext_[i],
offset_vec,
g_offset[i],
rotation);
simulator->addedOBBsNext_[i] = gjk_interface::createOBBFromVertices(simulator->armPolyhedronsNext_[i],
simulator->collision_structures[i].dimensions[0],
simulator->collision_structures[i].dimensions[1],
simulator->collision_structures[i].dimensions[2]);
}
/*printf("last left object:\n");
for (int i = 0; i < ARM_POLYHEDRONS_POINT; ++i) {
printf("%d: %.4f %.4f %.4f\n", i,
simulator->armPolyhedrons_[0][i].x(),
simulator->armPolyhedrons_[0][i].y(),
simulator->armPolyhedrons_[0][i].z());
}
printf("last right object:\n");
for (int i = 0; i < ARM_POLYHEDRONS_POINT; ++i) {
printf("%d: %.4f %.4f %.4f\n", i,
simulator->armPolyhedrons_[4][i].x(),
simulator->armPolyhedrons_[4][i].y(),
simulator->armPolyhedrons_[4][i].z());
}
printf("last left object:\n");
for (int i = 0; i < ARM_POLYHEDRONS_POINT; ++i) {
printf("%d: %.4f %.4f %.4f\n", i,
simulator->armPolyhedronsNext_[0][i].x(),
simulator->armPolyhedronsNext_[0][i].y(),
simulator->armPolyhedronsNext_[0][i].z());
}
printf("last right object:\n");
for (int i = 0; i < ARM_POLYHEDRONS_POINT; ++i) {
printf("%d: %.4f %.4f %.4f\n", i,
simulator->armPolyhedronsNext_[4][i].x(),
simulator->armPolyhedronsNext_[4][i].y(),
simulator->armPolyhedronsNext_[4][i].z());
}*/
}
static void InitPointsFromBoundsAndRotation(JointInfo &joint, CollisionStructure& col_struct)
{
std::vector<urdf::Vector3> now = {
urdf::Vector3(col_struct.mesh_bounds[0], col_struct.mesh_bounds[2], col_struct.mesh_bounds[4]),
urdf::Vector3(col_struct.mesh_bounds[0], col_struct.mesh_bounds[2], col_struct.mesh_bounds[5]),
urdf::Vector3(col_struct.mesh_bounds[0], col_struct.mesh_bounds[3], col_struct.mesh_bounds[4]),
urdf::Vector3(col_struct.mesh_bounds[0], col_struct.mesh_bounds[3], col_struct.mesh_bounds[5]),
urdf::Vector3(col_struct.mesh_bounds[1], col_struct.mesh_bounds[2], col_struct.mesh_bounds[4]),
urdf::Vector3(col_struct.mesh_bounds[1], col_struct.mesh_bounds[2], col_struct.mesh_bounds[5]),
urdf::Vector3(col_struct.mesh_bounds[1], col_struct.mesh_bounds[3], col_struct.mesh_bounds[4]),
urdf::Vector3(col_struct.mesh_bounds[1], col_struct.mesh_bounds[3], col_struct.mesh_bounds[5])
};
double x,y,z;
joint.rotationAll.getRPY(x,y,z);
// printf("dir now (%.2f,%.2f,%.2f))\n", x,y,z);
int leftBottom = -1, xIndex = 0, yIndex = 0, zIndex = 0;
double xMax = 0, yMax = 0, zMax = 0;
for (int i = 0; i < ARM_POLYHEDRONS_POINT; ++i) {
// printf ("ori %.4f, %.4f, %.4f\n", now[i].x, now[i].y, now[i].z);
}
for (int i = 0; i < ARM_POLYHEDRONS_POINT; ++i) {
now[i] = joint.rotationAll * now[i];
// printf ("aft %.4f, %.4f, %.4f\n", now[i].x, now[i].y, now[i].z);
if (leftBottom != -1) {
if (now[i].x - now[leftBottom].x<= 1e-2 && now[i].y - now[leftBottom].y<= 1e-2 && now[i].z - now[leftBottom].z<= 1e-2) {
leftBottom = i;
}
if (xMax <= now[i].x) {
xMax = now[i].x;
xIndex = i;
}
if (yMax <= now[i].y) {
yMax = now[i].y;
yIndex = i;
}
if (zMax <= now[i].z) {
zMax = now[i].z;
zIndex = i;
}
} else {
leftBottom = 0;
}
}
urdf::Vector3 leftBottomPoint = now[0];
now[0] = now[leftBottom];
now[leftBottom] = leftBottomPoint;
g_offset[joint.child_link] = (Vector3d(now[0].x, now[0].y, now[0].z));
for (int i = 0; i < ARM_POLYHEDRONS_POINT; ++i) {
col_struct.origin_points.push_back(Vector3d(now[i].x, now[i].y, now[i].z));
}
col_struct.mesh_bounds = {now[0].x, xMax, now[0].y, yMax, now[0].z, zMax};
col_struct.dimensions[0] = xMax - now[0].x;
col_struct.dimensions[1] = yMax - now[0].y;
col_struct.dimensions[2] = zMax - now[0].z;
}
// 加载网格并计算边界盒
static bool loadMeshDimensions(const std::string& filename, JointInfo &joint, CollisionStructure& col_struct)
{
// 加载网格文件
shapes::Mesh* mesh = shapes::createMeshFromResource(filename);
if (!mesh) {
printf("无法加载网格文件: %s\n", filename.c_str());
return false;
}
// 计算边界盒
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];
int min_index_x = 0, min_index_y = 0, min_index_z = 0;
int max_index_x = 0, max_index_y = 0, max_index_z = 0;
for (unsigned int i = 1; i < mesh->vertex_count; ++i) {
if (min_x > mesh->vertices[3*i]) {
min_x = mesh->vertices[3*i];
min_index_x = 3*i;
}
if (min_y > mesh->vertices[3*i + 1]) {
min_y = mesh->vertices[3*i + 1];
min_index_y = 3*i;
}
if (min_z > mesh->vertices[3*i + 2]) {
min_z = mesh->vertices[3*i + 2];
min_index_z = 3*i;
}
if (max_x < mesh->vertices[3*i]) {
max_x = mesh->vertices[3*i];
max_index_x = 3*i;
}
if (max_y < mesh->vertices[3*i + 1]) {
max_y = mesh->vertices[3*i + 1];
max_index_y = 3*i;
}
if (max_z < mesh->vertices[3*i + 2]) {
max_z = mesh->vertices[3*i + 2];
max_index_z = 3*i;
}
}
// 存储原始边界盒
col_struct.mesh_bounds = {min_x, max_x, min_y, max_y, min_z, max_z};
// 计算原始尺寸
double size_x = max_x - min_x;
double size_y = max_y - min_y;
double size_z = max_z - min_z;
col_struct.dimensions = {size_x, size_y, size_z};
InitPointsFromBoundsAndRotation(joint, col_struct);
printf("顶点数量: %u, 三角形数量: %u 大小:(%.4f, %.4f, %.4f)\n", mesh->vertex_count, mesh->triangle_count,
col_struct.dimensions[0], col_struct.dimensions[1], col_struct.dimensions[2]);
// 释放网格资源
delete mesh;
return true;
}
static int InitCollisionStructure(urdf::Model &urdf_model, ArmSimulator *simulator,
std::vector<CollisionStructure> &collision_structures_temp)
{
for (const auto& link_pair : urdf_model.links_) {
const std::string& link_name = link_pair.first;
const urdf::LinkConstSharedPtr& link = link_pair.second;
// 检查链接是否有碰撞元素
if (link->collision) {
CollisionStructure col_struct;
col_struct.link_name = link_name;
// 获取原点位置和姿态
if (link->collision != NULL) {
col_struct.origin_xyz = {
link->collision->origin.position.x,
link->collision->origin.position.y,
link->collision->origin.position.z
};
col_struct.origin_rpy = {
link->collision->origin.rotation.x,
link->collision->origin.rotation.y,
link->collision->origin.rotation.z
};
}
// 获取几何形状和尺寸
if (link->collision->geometry) {
switch (link->collision->geometry->type) {
case urdf::Geometry::BOX: {
col_struct.geometry_type = "box";
auto box = static_cast<urdf::Box*>(link->collision->geometry.get());
col_struct.dimensions = {box->dim.x, box->dim.y, box->dim.z};
break;
}
case urdf::Geometry::CYLINDER: {
col_struct.geometry_type = "cylinder";
auto cylinder = static_cast<urdf::Cylinder*>(link->collision->geometry.get());
col_struct.dimensions = {cylinder->radius, cylinder->length};
break;
}
case urdf::Geometry::SPHERE: {
col_struct.geometry_type = "sphere";
auto sphere = static_cast<urdf::Sphere*>(link->collision->geometry.get());
col_struct.dimensions = {sphere->radius};
break;
}
case urdf::Geometry::MESH: {
col_struct.geometry_type = "mesh";
auto mesh = static_cast<urdf::Mesh*>(link->collision->geometry.get());
// 存储STL文件路径（关键信息）
col_struct.mesh_filename = mesh->filename; // 需要在结构体中添加这个成员
auto index = g_linkMap.find(link_name);
if (index == g_linkMap.end()) {
printf("no link\n");
break;
}
int key = 0;
if (index->second == LEFT || index->second == RIGHT) {
key = (BASE << 8) | index->second;
} else {
key = ((index->second - 1) << 8) | (index->second);
}
auto joint = simulator->jointMap.find(key);
if (joint == simulator->jointMap.end()) {
printf("no joint\n");
break;
}
loadMeshDimensions(mesh->filename, joint->second, col_struct);
break;
}
default:
col_struct.geometry_type = "unknown";
break;
}
}
collision_structures_temp.push_back(col_struct);
}
}
return 0;
}
static int InitJoint(urdf::Model &urdf_model, std::map<int, JointInfo> &jointMap)
{
for (const auto& joint_pair : urdf_model.joints_) {
JointInfo jointInfo;
// joint_pair是一个键值对: 键为关节名称，值为关节指针
jointInfo.name = joint_pair.first;
urdf::JointConstSharedPtr joint = joint_pair.second;
jointInfo.type = joint->type;
jointInfo.parent_link = -1;
jointInfo.child_link = -1;
auto link = g_linkMap.find(joint->parent_link_name);
if (link != g_linkMap.end()) {
jointInfo.parent_link = link->second;
}
link = g_linkMap.find(joint->child_link_name);
if (link != g_linkMap.end()) {
jointInfo.child_link = link->second;
}
jointInfo.origin = joint->parent_to_joint_origin_transform;
if (jointInfo.limit_lower = joint->limits != NULL) {
jointInfo.limit_lower = joint->limits->lower;
jointInfo.limit_upper = joint->limits->upper;
jointInfo.limit_v = joint->limits->velocity;
jointInfo.limit_e = joint->limits->effort;
}
jointInfo.angle = 0;
int key = ((jointInfo.parent_link % 256) << 8) | (jointInfo.child_link % 256);
jointMap[key] = jointInfo;
}
int parent = BASE;
urdf::Rotation rotation;
for (int i = LEFT; i < RIGHT; ++i) {
int key = (parent << 8) | (i % 256);
parent = i;
auto jointPair = jointMap.find(key);
if (jointPair == jointMap.end()) {
printf("lose joint\n");
return -1;
}
urdf::Pose next = jointPair->second.origin;
urdf::Vector3 position = rotation * next.position;
g_origin[i] = Vector3d{position.x, position.y, position.z};
rotation = rotation * jointPair->second.origin.rotation;
jointPair->second.rotationAll = rotation;
double x,y,z;
rotation.getRPY(x,y,z);
/*printf("dir change (%.2f,%.2f,%.2f) origin (%.2f,%.2f,%.2f)\n", x,y,z,
position.x, position.y, position.z);*/
}
parent = BASE;
rotation.clear();
for (int i = RIGHT; i < BASE; ++i) {
int key = (parent << 8) | (i % 256);
parent = i;
auto jointPair = jointMap.find(key);
if (jointPair == jointMap.end()) {
printf("lose joint\n");
return -1;
}
urdf::Pose next = jointPair->second.origin;
urdf::Vector3 position = rotation * next.position;
g_origin[i] = Vector3d{position.x, position.y, position.z};
rotation = rotation * jointPair->second.origin.rotation;
jointPair->second.rotationAll = rotation;
double x,y,z;
rotation.getRPY(x,y,z);
/*printf("dir change (%.2f,%.2f,%.2f) origin (%.2f,%.2f,%.2f)\n", x,y,z,
position.x, position.y, position.z);*/
}
return 0;
}
static int InitArmPolyhedrons(ArmSimulator *simulator, urdf::Model &urdf_model)
{
std::vector<CollisionStructure> collision_structures_temp;
InitJoint(urdf_model, simulator->jointMap);
InitCollisionStructure(urdf_model, simulator, collision_structures_temp);
return RemakeCollisionStructureList(simulator, collision_structures_temp);
}
ArmSimulator::ArmSimulator(RmArmNode *armNode, std_msgs::msg::String::SharedPtr urdf_string_ptr) : armNode_(armNode)
{
printf("ArmSimulator: 初始化机械臂碰撞检测模块...\n");
if (armNode_ == nullptr) {
printf("ArmSimulator: armNode is null.\n");
return;
}
std::string urdf_string = urdf_string_ptr->data;
// 解析URDF模型
urdf::Model urdf_model;
if (!urdf_model.initString(urdf_string)) {
printf("无法解析URDF模型\n");
return;
}
InitArmPolyhedrons(this, urdf_model);
InitArmPolyhedronsList(this);
UpdateArmPolyhedrons(this);
}
ArmSimulator::~ArmSimulator()
{
}
int checkCnt = 0;
int64_t totalColTime = 0;
int64_t maxColTime = 0;
int64_t totalUpdateTime = 0;
int64_t maxUpdateTime = 0;
int ArmSimulator::CheckCollision()
{
int result = SAFE;
rclcpp::Time startTime = this->armNode_->get_clock()->now();
UpdateArmPolyhedrons(this);
rclcpp::Time endUpdateTime = this->armNode_->get_clock()->now();
int64_t durationUpdate = (endUpdateTime - startTime).nanoseconds();
totalUpdateTime += durationUpdate;
if (maxUpdateTime < durationUpdate) {
maxUpdateTime = durationUpdate;
}
for (int i = LEFT; i < RIGHT; ++i) {
for (int j = i + 1; j < ARM_POLYHEDRONS_CNT; ++j) {
if (!LinkCollisionDetecMatrix[i][j]) {
continue;
}
/*bool inCollision = gjk_interface::checkOBBCollision(addedOBBs_[i], addedOBBs_[j]);
if (inCollision) {
printf("dect left %d and %d collision a\n", i, j);
result |= LEFT_ARM_COLLISION;
}*/
bool inCollision = gjk_interface::checkOBBCollision(addedOBBsNext_[i], addedOBBsNext_[j]);
if (inCollision) {
printf("dect left %d and %d collision b\n", i, j);
result |= LEFT_ARM_COLLISION;
}
inCollision = gjk_interface::checkOBBCollision(addedOBBs_[i], addedOBBsNext_[j]);
if (inCollision) {
printf("dect left %d and %d collision c\n", i, j);
result |= LEFT_ARM_COLLISION;
}
}
}
for (int i = RIGHT; i < BASE; ++i) {
for (int j = i + 1; j < ARM_POLYHEDRONS_CNT; ++j) {
if (!LinkCollisionDetecMatrix[i][j]) {
continue;
}
/*bool inCollision = gjk_interface::checkOBBCollision(addedOBBs_[i], addedOBBs_[j]);
if (inCollision) {
printf("dect right %d and %d collision a\n", i, j);
result |= RIGHT_ARM_COLLISION;
}*/
bool inCollision = gjk_interface::checkOBBCollision(addedOBBsNext_[i], addedOBBsNext_[j]);
if (inCollision) {
printf("dect right %d and %d collision b\n", i, j);
result |= RIGHT_ARM_COLLISION;
}
inCollision = gjk_interface::checkOBBCollision(addedOBBs_[i], addedOBBsNext_[j]);
if (inCollision) {
printf("dect right %d and %d collision c\n", i, j);
result |= RIGHT_ARM_COLLISION;
}
}
}
rclcpp::Time endColTime = this->armNode_->get_clock()->now();
int64_t duration = (endColTime - startTime).nanoseconds();
totalColTime += duration;
if (maxColTime < duration) {
maxColTime = duration;
}
checkCnt++;
if (checkCnt >= 100) {
printf("CheckCollision avg time: %.2f us, max time: %.2f us\n",
(totalColTime / checkCnt) / 1e3, maxColTime / 1e3);
printf("UpdatePolyhedrons avg time: %.2f us, max time: %.2f us\n",
(totalUpdateTime / checkCnt) / 1e3, maxUpdateTime / 1e3);
checkCnt = 0;
totalColTime = 0;
totalUpdateTime = 0;
}
return result;
}
int ArmSimulator::AddPolyhedron(int id, const Polyhedron& poly)
{
if (addedPolyhedrons_.find(id) != addedPolyhedrons_.end()) {
return -1; // 已存在相同ID的多面体
}
addedPolyhedrons_[id] = poly;
return 0; // 成功添加
}
int ArmSimulator::RemovePolyhedron(int id)
{
auto it = addedPolyhedrons_.find(id);
if (it != addedPolyhedrons_.end()) {
addedPolyhedrons_.erase(it);
return 0; // 成功移除
}
return -1; // 未找到对应ID的多面体
}

0
HiveCoreR1/src/arm_description/urdf/leftarm.urdf → HiveCoreR1/src/rm_arm_control/urdf/leftarm.urdf
View File

0
HiveCoreR1/src/arm_description/urdf/rightarm.urdf → HiveCoreR1/src/rm_arm_control/urdf/rightarm.urdf
View File

0
HiveCoreR1/src/arm_description/urdf/rm_65_b.urdf → HiveCoreR1/src/rm_arm_control/urdf/rm_65_b.urdf
View File

0
HiveCoreR1/src/arm_description/urdf/rm_65_b_description.csv → HiveCoreR1/src/rm_arm_control/urdf/rm_65_b_description.csv
View File

58
HiveCoreR1/src/arm_description/urdf/rm_65_b_description.urdf → HiveCoreR1/src/rm_arm_control/urdf/rm_65_b_description.urdf
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="utf-8"?>
<robot
name="arm_description">
name="rm_arm_control">
<link name="world" />
<link
name="base_link">
@@ -52,7 +52,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/base_link.STL" />
filename="package://rm_arm_control/meshes/base_link.STL" />
</geometry>
<material
name="">
@@ -66,7 +66,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/base_link.STL" />
filename="package://rm_arm_control/meshes/base_link.STL" />
</geometry>
</collision>
</link>
@@ -101,7 +101,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link1.STL" />
filename="package://rm_arm_control/meshes/link1.STL" />
</geometry>
<material
name="">
@@ -115,7 +115,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link1.STL" />
filename="package://rm_arm_control/meshes/link1.STL" />
</geometry>
</collision>
</link>
@@ -159,7 +159,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link2.STL" />
filename="package://rm_arm_control/meshes/link2.STL" />
</geometry>
<material
name="">
@@ -173,7 +173,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link2.STL" />
filename="package://rm_arm_control/meshes/link2.STL" />
</geometry>
</collision>
</link>
@@ -217,7 +217,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link3.STL" />
filename="package://rm_arm_control/meshes/link3.STL" />
</geometry>
<material
name="">
@@ -231,7 +231,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link3.STL" />
filename="package://rm_arm_control/meshes/link3.STL" />
</geometry>
</collision>
</link>
@@ -275,7 +275,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link4.STL" />
filename="package://rm_arm_control/meshes/link4.STL" />
</geometry>
<material
name="">
@@ -289,7 +289,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link4.STL" />
filename="package://rm_arm_control/meshes/link4.STL" />
</geometry>
</collision>
</link>
@@ -333,7 +333,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link5.STL" />
filename="package://rm_arm_control/meshes/link5.STL" />
</geometry>
<material
name="">
@@ -347,7 +347,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link5.STL" />
filename="package://rm_arm_control/meshes/link5.STL" />
</geometry>
</collision>
</link>
@@ -391,7 +391,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link6.STL" />
filename="package://rm_arm_control/meshes/link6.STL" />
</geometry>
<material
name="">
@@ -405,7 +405,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link6.STL" />
filename="package://rm_arm_control/meshes/link6.STL" />
</geometry>
</collision>
</link>
@@ -449,7 +449,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/base_link.STL" />
filename="package://rm_arm_control/meshes/base_link.STL" />
</geometry>
<material
name="">
@@ -463,7 +463,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/base_link.STL" />
filename="package://rm_arm_control/meshes/base_link.STL" />
</geometry>
</collision>
</link>
@@ -498,7 +498,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link1.STL" />
filename="package://rm_arm_control/meshes/link1.STL" />
</geometry>
<material
name="">
@@ -512,7 +512,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link1.STL" />
filename="package://rm_arm_control/meshes/link1.STL" />
</geometry>
</collision>
</link>
@@ -556,7 +556,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link2.STL" />
filename="package://rm_arm_control/meshes/link2.STL" />
</geometry>
<material
name="">
@@ -570,7 +570,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link2.STL" />
filename="package://rm_arm_control/meshes/link2.STL" />
</geometry>
</collision>
</link>
@@ -614,7 +614,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link3.STL" />
filename="package://rm_arm_control/meshes/link3.STL" />
</geometry>
<material
name="">
@@ -628,7 +628,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link3.STL" />
filename="package://rm_arm_control/meshes/link3.STL" />
</geometry>
</collision>
</link>
@@ -672,7 +672,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link4.STL" />
filename="package://rm_arm_control/meshes/link4.STL" />
</geometry>
<material
name="">
@@ -686,7 +686,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link4.STL" />
filename="package://rm_arm_control/meshes/link4.STL" />
</geometry>
</collision>
</link>
@@ -730,7 +730,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link5.STL" />
filename="package://rm_arm_control/meshes/link5.STL" />
</geometry>
<material
name="">
@@ -744,7 +744,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link5.STL" />
filename="package://rm_arm_control/meshes/link5.STL" />
</geometry>
</collision>
</link>
@@ -788,7 +788,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link6.STL" />
filename="package://rm_arm_control/meshes/link6.STL" />
</geometry>
<material
name="">
@@ -802,7 +802,7 @@
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm_description/meshes/link6.STL" />
filename="package://rm_arm_control/meshes/link6.STL" />
</geometry>
</collision>
</link>

0
HiveCoreR1/src/arm_description/urdf/robot.urdf → HiveCoreR1/src/rm_arm_control/urdf/robot.urdf
View File