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

code optimization

Browse Source
This commit is contained in:
Ray
2025-08-06 20:52:50 +08:00
parent 80a06ef898
commit 30179b48e7
91 changed files with 2390 additions and 3505 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
.gitignore vendored Normal file
View File

@@ -0,0 +1,11 @@
HiveCoreR0/build/**
HiveCoreR0/install/**
HiveCoreR0/log/**
HiveCoreR1/build/**
HiveCoreR1/install/**
HiveCoreR1/log/**
HiveCoreR2/build/**
HiveCoreR2/install/**
HiveCoreR2/log/**

3
.vscode/c_cpp_properties.json vendored
View File

@@ -4,7 +4,8 @@
"name": "Linux",
"includePath": [
"${workspaceFolder}/**",
"/opt/ros/humble/include/**"
"/opt/ros/humble/include/**",
"/usr/include/**"
],
"defines": [],
"compilerPath": "/usr/bin/gcc",

70
.vscode/settings.json vendored
View File

@@ -10,6 +10,74 @@
"initializer_list": "cpp",
"regex": "cpp",
"utility": "cpp",
"valarray": "cpp"
"valarray": "cpp",
"chrono": "cpp",
"cctype": "cpp",
"clocale": "cpp",
"cmath": "cpp",
"csignal": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"any": "cpp",
"atomic": "cpp",
"strstream": "cpp",
"bit": "cpp",
"codecvt": "cpp",
"compare": "cpp",
"complex": "cpp",
"concepts": "cpp",
"condition_variable": "cpp",
"cstdint": "cpp",
"deque": "cpp",
"forward_list": "cpp",
"list": "cpp",
"map": "cpp",
"set": "cpp",
"string": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"vector": "cpp",
"exception": "cpp",
"algorithm": "cpp",
"functional": "cpp",
"iterator": "cpp",
"memory": "cpp",
"memory_resource": "cpp",
"numeric": "cpp",
"optional": "cpp",
"random": "cpp",
"ratio": "cpp",
"system_error": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"fstream": "cpp",
"future": "cpp",
"iomanip": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"istream": "cpp",
"limits": "cpp",
"mutex": "cpp",
"new": "cpp",
"numbers": "cpp",
"ostream": "cpp",
"semaphore": "cpp",
"shared_mutex": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"stop_token": "cpp",
"streambuf": "cpp",
"thread": "cpp",
"cfenv": "cpp",
"cinttypes": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
"variant": "cpp"
}
}

47
HiveCoreR0/hive_core_bot0.gv
View File

@@ -1,47 +0,0 @@
digraph G {
node [shape=box];
"base_link" [label="base_link"];
"left_hip_pitch_link" [label="left_hip_pitch_link"];
"left_leg_pitch_link" [label="left_leg_pitch_link"];
"left_wheel2_link" [label="left_wheel2_link"];
"left_wheel1_link" [label="left_wheel1_link"];
"left_shoulder_link" [label="left_shoulder_link"];
"left_arm1_link" [label="left_arm1_link"];
"left_arm2_link" [label="left_arm2_link"];
"right_hip_pitch_link" [label="right_hip_pitch_link"];
"right_leg_pitch_link" [label="right_leg_pitch_link"];
"right_wheel2_link" [label="right_wheel2_link"];
"right_wheel1_link" [label="right_wheel1_link"];
"right_shoulder_link" [label="right_shoulder_link"];
"right_arm1_link" [label="right_arm1_link"];
"right_arm2_link" [label="right_arm2_link"];
node [shape=ellipse, color=blue, fontcolor=blue];
"base_link" -> "left_hip_pitch_joint" [label="xyz: -0.06 -0.1358 -0.037269 \nrpy: -1.5708 -0.0095732 0"]
"left_hip_pitch_joint" -> "left_hip_pitch_link"
"left_hip_pitch_link" -> "left_leg_pitch_joint" [label="xyz: 0 0 -0.0295 \nrpy: 0 -0 0.01205"]
"left_leg_pitch_joint" -> "left_leg_pitch_link"
"left_leg_pitch_link" -> "left_wheel2_joint" [label="xyz: 0 0.4805 0.004 \nrpy: 3.14159 -3.30872e-24 -0.0024764"]
"left_wheel2_joint" -> "left_wheel2_link"
"left_hip_pitch_link" -> "left_wheel1_joint" [label="xyz: 0 0.48 0.01 \nrpy: 0 -0 0.0095732"]
"left_wheel1_joint" -> "left_wheel1_link"
"base_link" -> "left_shoulder_joint" [label="xyz: -0.06 -0.0878 0.36288 \nrpy: 0 -0 0"]
"left_shoulder_joint" -> "left_shoulder_link"
"left_shoulder_link" -> "left_arm1_joint" [label="xyz: 0 0 0.0385 \nrpy: 1.5708 4.33681e-19 -0.0074682"]
"left_arm1_joint" -> "left_arm1_link"
"left_arm1_link" -> "left_arm2_joint" [label="xyz: 0.020127 0.0040341 0.17817 \nrpy: -1.6062 0.0074682 4.33693e-19"]
"left_arm2_joint" -> "left_arm2_link"
"base_link" -> "right_hip_pitch_joint" [label="xyz: -0.06 0.1358 -0.037269 \nrpy: 1.5708 -0.0029091 0"]
"right_hip_pitch_joint" -> "right_hip_pitch_link"
"right_hip_pitch_link" -> "right_leg_pitch_joint" [label="xyz: 0 0 -0.0295 \nrpy: 0 0 -0.0029091"]
"right_leg_pitch_joint" -> "right_leg_pitch_link"
"right_leg_pitch_link" -> "right_wheel2_joint" [label="xyz: 1.9386e-05 -0.4805 0.004 \nrpy: -3.14159 -0 0"]
"right_wheel2_joint" -> "right_wheel2_link"
"right_hip_pitch_link" -> "right_wheel1_joint" [label="xyz: 0 -0.48 0.01 \nrpy: 0 0 -0.0029091"]
"right_wheel1_joint" -> "right_wheel1_link"
"base_link" -> "right_shoulder_joint" [label="xyz: -0.06 0.0878 0.36288 \nrpy: 0 -0 0"]
"right_shoulder_joint" -> "right_shoulder_link"
"right_shoulder_link" -> "right_arm1_joint" [label="xyz: 0 0 0.0385 \nrpy: 1.5708 -0 0"]
"right_arm1_joint" -> "right_arm1_link"
"right_arm1_link" -> "right_arm2_joint" [label="xyz: 0.017969 0.0026972 -0.17843 \nrpy: -1.5793 0 0"]
"right_arm2_joint" -> "right_arm2_link"
}

BIN
HiveCoreR0/hive_core_bot0.pdf
View File

Binary file not shown.

73
HiveCoreR0/src/hive_core_r0/config/bot0_ros2_controller.yaml
View File

@@ -1,73 +0,0 @@
controller_manager:
ros__parameters:
update_rate: 100 # Hz
use_sim_time: true
# 关节状态广播器 - 必须配置，用于发布关节状态
bot0_joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
use_sim_time: true
# 关节位置控制器
bot0_position_controller:
type: position_controllers/JointGroupPositionController
# 轮子速度控制器（已从差速控制器改为速度控制器）
bot0_wheel_velocity_controller:
type: velocity_controllers/JointGroupVelocityController
# 腿部和手臂关节位置控制器配置
bot0_position_controller:
ros__parameters:
joints:
- left_hip_pitch_joint
- left_leg_pitch_joint
- right_hip_pitch_joint
- right_leg_pitch_joint
- left_shoulder_joint
- left_arm1_joint
- left_arm2_joint
- right_shoulder_joint
- right_arm1_joint
- right_arm2_joint
command_interfaces:
- position
state_interfaces:
- position
- velocity
# 位置控制器PID参数
gains:
left_hip_pitch_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
left_leg_pitch_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
right_hip_pitch_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
right_leg_pitch_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
left_shoulder_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
left_arm1_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
left_arm2_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
right_shoulder_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
right_arm1_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
right_arm2_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
# 位置控制允许的误差范围
allowable_position_error: 0.1 # 弧度
# 轮子速度控制器配置（替代原来的差速控制器）
bot0_wheel_velocity_controller:
ros__parameters:
joints:
- left_wheel1_joint
- left_wheel2_joint
- right_wheel1_joint
- right_wheel2_joint
command_interfaces:
- velocity
state_interfaces:
- position
- velocity
# 速度控制器PID参数
gains:
left_wheel1_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
left_wheel2_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
right_wheel1_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
right_wheel2_joint: {p: 1, i: 0.0, d: 0.1, i_clamp: 0.01}
# 速度控制允许的误差范围
allowable_velocity_error: 0.2 # rad/s

119
HiveCoreR0/src/hive_core_r0/launch/gazebo_sim_bot0.launch.py
View File

@@ -1,119 +0,0 @@
import os
import launch
import launch_ros
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler
from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution, FindExecutable
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.event_handlers import OnProcessStart
from launch.actions import TimerAction
def generate_launch_description():
# 声明参数
robot_name_in_model = "HiveBot0"
urdf_package = 'hive_core_r0'
urdf_tutorial_path = get_package_share_directory(urdf_package)
default_world_path = os.path.join(urdf_tutorial_path, 'world', 'table.world')
default_model_path = urdf_tutorial_path + '/urdf/hiveBot0/hive_core_bot0.urdf.xacro'
# 声明模型路径参数
action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
name='model', default_value=str(default_model_path),
description='URDF 的绝对路径')
# 获取文件内容生成新的参数
robot_description = launch_ros.parameter_descriptions.ParameterValue(
launch.substitutions.Command(
['xacro ', launch.substitutions.LaunchConfiguration('model')]),
value_type=str)
# 声明机器人状态发布节点
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': robot_description}],
)
# 启动Gazebo - 添加 paused=true 参数使仿真暂停
launch_gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(get_package_share_directory('gazebo_ros'), 'launch',
'gazebo.launch.py')
]),
launch_arguments={
# 'world': default_world_path,
'verbose': 'true',
}.items()
)
# 在Gazebo中生成机器人
spawn_entity_node = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=[
'-entity', robot_name_in_model, # 模型名称
'-topic', '/robot_description', # 从ROS话题获取URDF
'-x', '0.0', # 初始位置x
'-y', '0.0', # 初始位置y
'-z', '0.55', # 初始位置z (高于地面)
'-Y', '0.0' # 初始朝向
]
)
# 加载并激活 load_joint_state_controller 控制器
load_joint_state_controller = launch.actions.ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'bot0_joint_state_broadcaster'],
output='screen'
)
# 加载并激活 load_bot0_position_controller 控制器
load_bot0_position_controller = launch.actions.ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','bot0_position_controller'],
output='screen')
load_bot0_wheel_velocity_controller = launch.actions.ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','bot0_wheel_velocity_controller'],
output='screen')
action_node_arm_control = launch_ros.actions.Node(
package='hive_core_r0',
executable='arm_control_node',
name='arm_control_node',
output='screen'
)
return LaunchDescription([
action_declare_arg_mode_path,
# 先启动Gazebo
launch_gazebo,
# 然后启动状态发布器
robot_state_publisher_node,
# 延迟后生成机器人（确保Gazebo完全启动）
TimerAction(
period= 5.0,
actions=[spawn_entity_node]
),
# 事件动作，当加载机器人结束后执行
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=spawn_entity_node,
on_exit=[load_joint_state_controller, load_bot0_position_controller, load_bot0_wheel_velocity_controller],)
),
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=load_bot0_position_controller,
on_exit=[action_node_arm_control],)
),
])

116
HiveCoreR0/src/hive_core_r0/src/arm_control_node.cpp
View File

@@ -1,116 +0,0 @@
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include <cmath>
#include <chrono>
using namespace std::chrono_literals;
class ArmSineMotionNode : public rclcpp::Node
{
public:
ArmSineMotionNode() : Node("arm_sine_motion_node")
{
// 1. 声明并获取参数（振幅默认50度，频率默认0.5Hz）
this->declare_parameter("amplitude_deg", 50.0); // 振幅（度）
this->declare_parameter("frequency", 0.5); // 频率（Hz）
this->declare_parameter("control_period_ms", 1); // 控制周期（毫秒，默认20ms=50Hz）
// 2. 读取参数并转换单位（度→弧度）
double amplitude_deg = this->get_parameter("amplitude_deg").as_double();
amplitude_rad_ = amplitude_deg * M_PI / 180.0; // 50度→弧度
frequency_ = this->get_parameter("frequency").as_double();
int period_ms = this->get_parameter("control_period_ms").as_int();
// 3. 手臂关节在控制器指令数组中的索引（对应joints配置顺序）
// 控制器joints顺序：[0-3腿部, 4-9手臂]
arm_joint_indices_ = {4, 5, 6, 7, 8, 9}; // 6个手臂关节索引
total_joints_ = 10; // 控制器总关节数（4腿部+6手臂）
// 4. 创建发布者（发送位置指令到位置控制器）
cmd_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
"/bot0_position_controller/commands",
10
);
// 5. 创建定时器（周期性发送指令）
timer_ = this->create_wall_timer(
std::chrono::milliseconds(period_ms),
std::bind(&ArmSineMotionNode::timer_callback, this)
);
// 6. 记录起始时间（用于计算正弦运动相位）
start_time_ = this->now();
RCLCPP_INFO(this->get_logger(), "手臂正弦运动节点启动（振幅50度，频率%.2fHz）", frequency_);
}
void RestJoint()
{
auto reset_msg = std_msgs::msg::Float64MultiArray();
reset_msg.data.resize(10, 0.0);
cmd_pub_->publish(reset_msg);
}
private:
void timer_callback()
{
// 1. 计算当前运动时间（秒）
rclcpp::Duration elapsed = this->now() - start_time_;
double current_time = elapsed.seconds();
// 2. 计算正弦位置：y = A*sin(2πft)
// A=振幅（弧度），f=频率（Hz），t=时间（秒）
double sine_pos = amplitude_rad_ * std::sin(2 * M_PI * frequency_ * current_time);
// 3. 构建指令数组（10个关节：腿部保持0，手臂按正弦运动）
auto cmd_msg = std_msgs::msg::Float64MultiArray();
cmd_msg.data.resize(total_joints_, 0.0); // 初始化所有关节为0
// 4. 为手臂关节赋值正弦位置
// for (size_t i = 0; i < arm_joint_indices_.size(); ++i) {
// int joint_idx = arm_joint_indices_[i];
// cmd_msg.data[joint_idx] = 0;//sine_pos; // 所有手臂关节同步运动
// }
for (int i = 0; i < total_joints_; ++i) {
// int joint_idx = arm_joint_indices_[i];
if (i == 1 || i == 3)
{
cmd_msg.data[i] = sine_pos * 2;
}
else
{
cmd_msg.data[i] = sine_pos; // 所有手臂关节同步运动
}
}
cmd_msg.data.resize(total_joints_, 0.0); // 初始化所有关节为0
// 5. 发送指令
cmd_pub_->publish(cmd_msg);
}
// 成员变量
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr cmd_pub_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time start_time_; // 起始时间
double amplitude_rad_; // 振幅（弧度）
double frequency_; // 频率（Hz）
std::vector<int> arm_joint_indices_; // 手臂关节索引
int total_joints_; // 总关节数
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<ArmSineMotionNode>();
// 运行节点（按Ctrl+C退出）
rclcpp::spin(node);
// 退出时发送复位指令（所有关节回0）
node->RestJoint();
RCLCPP_INFO(node->get_logger(), "节点退出，发送复位指令");
rclcpp::shutdown();
return 0;
}

140
HiveCoreR0/src/hive_core_r0/src/keyboard_control_node.cpp
View File

@@ -1,140 +0,0 @@
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
#include <chrono>
using namespace std::chrono_literals;
// 键盘输入处理函数（非阻塞模式）
int kbhit()
{
struct termios oldt, newt;
int ch;
int oldf;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
fcntl(STDIN_FILENO, F_SETFL, oldf);
if(ch != EOF)
{
ungetc(ch, stdin);
return 1;
}
return 0;
}
class KeyboardControlNode : public rclcpp::Node
{
public:
KeyboardControlNode() : Node("keyboard_control_node")
{
// 声明参数：移动速度、旋转速度
this->declare_parameter("linear_speed", 0.2); // 线速度（m/s）
this->declare_parameter("angular_speed", 0.5); // 角速度（rad/s）
// 读取参数
linear_speed_ = this->get_parameter("linear_speed").as_double();
angular_speed_ = this->get_parameter("angular_speed").as_double();
// 创建发布者（发送速度指令）
twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
"/cmd_vel", 10
);
// 打印控制说明
print_help();
// 创建定时器（周期性检查键盘输入）
timer_ = this->create_wall_timer(
50ms, // 20Hz检查频率
std::bind(&KeyboardControlNode::timer_callback, this)
);
}
private:
void print_help()
{
RCLCPP_INFO(this->get_logger(), "键盘控制说明:");
RCLCPP_INFO(this->get_logger(), " w : 前进");
RCLCPP_INFO(this->get_logger(), " s : 后退");
RCLCPP_INFO(this->get_logger(), " a : 左转");
RCLCPP_INFO(this->get_logger(), " d : 右转");
RCLCPP_INFO(this->get_logger(), " x : 停止");
RCLCPP_INFO(this->get_logger(), " q : 退出");
}
void timer_callback()
{
geometry_msgs::msg::Twist twist_msg;
if(kbhit())
{
char key = getchar();
switch(key)
{
case 'w': // 前进
twist_msg.linear.x = linear_speed_;
twist_msg.angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "前进");
break;
case 's': // 后退
twist_msg.linear.x = -linear_speed_;
twist_msg.angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "后退");
break;
case 'a': // 左转
twist_msg.linear.x = 0.0;
twist_msg.angular.z = angular_speed_;
RCLCPP_INFO(this->get_logger(), "左转");
break;
case 'd': // 右转
twist_msg.linear.x = 0.0;
twist_msg.angular.z = -angular_speed_;
RCLCPP_INFO(this->get_logger(), "右转");
break;
case 'x': // 停止
twist_msg.linear.x = 0.0;
twist_msg.angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "停止");
break;
case 'q': // 退出
RCLCPP_INFO(this->get_logger(), "退出控制");
rclcpp::shutdown();
return;
default:
twist_msg.linear.x = 0.0;
twist_msg.angular.z = 0.0;
print_help();
break;
}
twist_pub_->publish(twist_msg);
}
}
// 成员变量
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
rclcpp::TimerBase::SharedPtr timer_;
double linear_speed_; // 线速度（m/s）
double angular_speed_; // 角速度（rad/s）
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<KeyboardControlNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

165
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/bot0.ros2_control.xacro
View File

@@ -1,165 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 宏定义：ros2_control硬件接口配置（适配位置控制器和轮子速度控制器） -->
<xacro:macro name="bot0_ros2_control">
<!-- 1. ros2_control系统配置：定义所有关节的控制接口 -->
<ros2_control name="Bot0GazeboSystem" type="system">
<!-- 硬件插件：关联Gazebo仿真 -->
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<!-- 2. 腿部+手臂关节（位置控制接口，对应bot0_position_controller） -->
<!-- 2.1 腿部关节 -->
<joint name="left_hip_pitch_joint">
<command_interface name="position"> <!-- 位置控制器的命令接口 -->
<param name="min">-1.0</param> <!-- 对应关节限位（弧度） -->
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" /> <!-- 反馈接口：位置 -->
<state_interface name="velocity" /> <!-- 反馈接口：速度 -->
<state_interface name="effort" /> <!-- 反馈接口：力（用于调试） -->
</joint>
<joint name="left_leg_pitch_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="right_hip_pitch_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="right_leg_pitch_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<!-- 2.2 手臂关节 -->
<joint name="left_shoulder_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="left_arm1_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="left_arm2_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="right_shoulder_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="right_arm1_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="right_arm2_joint">
<command_interface name="position">
<param name="min">-1.0</param>
<param name="max">1.0</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<!-- 3. 车轮关节（速度控制接口，对应bot0_wheel_velocity_controller） -->
<joint name="left_wheel1_joint">
<command_interface name="velocity"> <!-- 速度控制器的命令接口 -->
<param name="min">-50</param> <!-- 对应关节速度限位（rad/s） -->
<param name="max">50</param>
</command_interface>
<state_interface name="position" /> <!-- 反馈接口：位置 -->
<state_interface name="velocity" /> <!-- 反馈接口：速度 -->
<state_interface name="effort" /> <!-- 反馈接口：力 -->
</joint>
<joint name="left_wheel2_joint">
<command_interface name="velocity">
<param name="min">-50</param>
<param name="max">50</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="right_wheel1_joint">
<command_interface name="velocity">
<param name="min">-50</param>
<param name="max">50</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<joint name="right_wheel2_joint">
<command_interface name="velocity">
<param name="min">-50</param>
<param name="max">50</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
</ros2_control>
<!-- 4. Gazebo插件配置：关联控制器参数文件 -->
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<!-- 加载控制器配置文件（路径需替换为你的实际路径） -->
<parameters>$(find hive_core_r0)/config/bot0_ros2_controller.yaml</parameters>
</plugin>
</gazebo>
</xacro:macro>
</robot>

15
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/generic_joint.xacro
View File

@@ -1,15 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="generic_joint" params="joint_name type parent child
origin_xyz origin_rpy axis
limit_lower limit_upper effort velocity">
<joint name="${joint_name}" type="${type}">
<origin xyz="${origin_xyz}" rpy="${origin_rpy}" />
<parent link="${parent}" />
<child link="${child}" />
<axis xyz="${axis}" />
<limit lower="${limit_lower}" upper="${limit_upper}"
effort="${effort}" velocity="${velocity}" />
</joint>
</xacro:macro>
</robot>

65
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/generic_link.xacro
View File

@@ -1,65 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="generic_link" params="link_name mesh_file mass
inertia_ixx inertia_ixy inertia_ixz
inertia_iyy inertia_iyz inertia_izz
origin_xyz origin_rpy color_rgba enable_collision">
<link name="${link_name}">
<!-- 惯性参数 -->
<inertial>
<origin xyz="${origin_xyz}" rpy="${origin_rpy}" />
<mass value="${mass}" />
<inertia ixx="${inertia_ixx}" ixy="${inertia_ixy}" ixz="${inertia_ixz}"
iyy="${inertia_iyy}" iyz="${inertia_iyz}" izz="${inertia_izz}" />
</inertial>
<!-- 可视化模型 -->
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="${mesh_file}" />
</geometry>
<material name="${link_name}_material">
<color rgba="${color_rgba}" />
</material>
</visual>
<!-- 碰撞模型（与可视化模型一致） -->
<!-- 条件生成碰撞体：仅当enable_collision=true时添加 -->
<xacro:if value="${enable_collision}">
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.05" length="0.03"/>
</geometry>
<material name="blue">
<color rgba="0.1 0.1 1.0 0.5" />
</material>
</collision>
</xacro:if>
</link>
<!-- 仅当启用碰撞时，添加碰撞物理参数 -->
<xacro:if value="${enable_collision}">
<gazebo reference="${link_name}">
<collision>
<surface>
<contact>
<ode>
<kp>1e3</kp> <!-- 刚度：1e6~1e8（根据质量调整） -->
<kd>1e2</kd> <!-- 阻尼：与kp比例1:100左右 -->
<mu1>0.8</mu1> <!-- 静摩擦系数：0.5~1.0 -->
<mu2>0.8</mu2> <!-- 动摩擦系数：与mu1接近 -->
<min_depth>0.001</min_depth> <!-- 允许微小穿透，减少抖动 -->
</ode>
</contact>
<bounce>
<restitution_coefficient>0.0</restitution_coefficient> <!-- 反弹系数0 -->
<threshold>10000.0</threshold> <!-- 阈值设大，确保所有碰撞都不反弹 -->
</bounce>
</surface>
</collision>
</gazebo>
</xacro:if>
</xacro:macro>
</robot>

860
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf
View File

@@ -1,860 +0,0 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
name="hive_core_bot0">
<link
name="base_link">
<inertial>
<origin
xyz="-0.0598891820748674 5.31626531352636E-07 0.164869219254393"
rpy="0 0 0" />
<mass
value="5.4247767" />
<inertia
ixx="0.1131876"
ixy="0.0000008"
ixz="0.0001546"
iyy="0.0858545"
iyz="0.0000006"
izz="0.0325040" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.666666666666667 0.666666666666667 0.666666666666667 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/base_link.STL" />
</geometry>
</collision>
</link>
<link
name="left_hip_pitch_link">
<inertial>
<origin
xyz="3.93767161736252E-06 0.161490126813208 0.0158567790568734"
rpy="0 0 0" />
<mass
value="0.6475263" />
<inertia
ixx="0.0182516"
ixy="0.0000058"
ixz="0.0001729"
iyy="0.0181748"
iyz="-0.0006055"
izz="0.0003913" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_hip_pitch_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://urdf/meshes/meshes/left_hip_pitch_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_hip_pitch_joint"
type="revolute">
<origin
xyz="-0.06 -0.1358 -0.037269"
rpy="-1.5708 -0.0095732 0" />
<parent
link="base_link" />
<child
link="left_hip_pitch_link" />
<axis
xyz="0 0 -1" />
<limit
lower="-1.57"
upper="1.57"
effort="100"
velocity="50" />
</joint>
<link
name="left_leg_pitch_link">
<inertial>
<origin
xyz="-2.63464180747426E-05 0.197159145100908 0.000498203326348359"
rpy="0 0 0" />
<mass
value="0.5126336" />
<inertia
ixx="0.0135141"
ixy="0.0000005"
ixz="-0.0000325"
iyy="0.0135986"
iyz="0.0001480"
izz="0.0002025" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_leg_pitch_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 0.372549019607843 0.294117647058824 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_leg_pitch_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_leg_pitch_joint"
type="revolute">
<origin
xyz="0 0 -0.0295"
rpy="0 0 0.01205" />
<parent
link="left_hip_pitch_link" />
<child
link="left_leg_pitch_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1.57"
upper="1.57"
effort="100"
velocity="50" />
</joint>
<link
name="left_wheel2_link">
<inertial>
<origin
xyz="4.1585564079967E-08 -5.47447156262759E-06 0.0187838263233595"
rpy="0 0 0" />
<mass
value="0.745" />
<inertia
ixx="0.0004716"
ixy="0"
ixz="0"
iyy="0.0007945"
iyz="0.0000001"
izz="0.0004716" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_wheel2_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_wheel2_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_wheel2_joint"
type="revolute">
<origin
xyz="0 0.4805 0.004"
rpy="-3.1416 0 -0.0024764" />
<parent
link="left_leg_pitch_link" />
<child
link="left_wheel2_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1000"
upper="1000"
effort="100"
velocity="50" />
</joint>
<link
name="left_wheel1_link">
<inertial>
<origin
xyz="1.97042367054093E-06 5.10773910927798E-06 0.0187838263233595"
rpy="0 0 0" />
<mass
value="0.745" />
<inertia
ixx="0.0004716"
ixy="0"
ixz="0"
iyy="0.0007945"
iyz="0.0000001"
izz="0.0004716" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_wheel1_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_wheel1_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_wheel1_joint"
type="revolute">
<origin
xyz="0 0.48 0.01"
rpy="0 0 0.0095732" />
<parent
link="left_hip_pitch_link" />
<child
link="left_wheel1_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1000"
upper="1000"
effort="100"
velocity="50" />
</joint>
<link
name="right_hip_pitch_link">
<inertial>
<origin
xyz="-3.97209815898069E-06 -0.161490067811833 0.0158567790568734"
rpy="0 0 0" />
<mass
value="0.6475263" />
<inertia
ixx="0.0182531"
ixy="-0.0000017"
ixz="0.0000501"
iyy="0.0181748"
iyz="0.0006055"
izz="0.0003897" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_hip_pitch_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://urdf/meshes/meshes/right_hip_pitch_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_hip_pitch_joint"
type="revolute">
<origin
xyz="-0.06 0.1358 -0.037269"
rpy="1.5708 -0.0029091 0" />
<parent
link="base_link" />
<child
link="right_hip_pitch_link" />
<axis
xyz="0 0 -1" />
<limit
lower="-1.57"
upper="1.57"
effort="100"
velocity="50" />
</joint>
<link
name="right_leg_pitch_link">
<inertial>
<origin
xyz="3.43008600186093E-05 -0.197159143801203 0.000498203308628542"
rpy="0 0 0" />
<mass
value="0.5126336" />
<inertia
ixx="0.0135142"
ixy="0.0000001"
ixz="0.0000001"
iyy="0.0135986"
iyz="-0.0001480"
izz="0.0002024" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_leg_pitch_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_leg_pitch_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_leg_pitch_joint"
type="revolute">
<origin
xyz="0 0 -0.0295"
rpy="0 0 -0.0029091" />
<parent
link="right_hip_pitch_link" />
<child
link="right_leg_pitch_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1.57"
upper="1.57"
effort="100"
velocity="50" />
</joint>
<link
name="right_wheel2_link">
<inertial>
<origin
xyz="-5.49216228909084E-08 5.47435401360152E-06 0.0187838263233596"
rpy="0 0 0" />
<mass
value="0.745" />
<inertia
ixx="0.0004716"
ixy="0"
ixz="0"
iyy="0.0007945"
iyz="0.0000001"
izz="0.0004716" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_wheel2_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_wheel2_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_wheel2_joint"
type="revolute">
<origin
xyz="1.9386E-05 -0.4805 0.004"
rpy="3.1416 0 0" />
<parent
link="right_leg_pitch_link" />
<child
link="right_wheel2_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1000"
upper="1000"
effort="100"
velocity="50" />
</joint>
<link
name="right_wheel1_link">
<inertial>
<origin
xyz="-1.90651569380871E-06 -5.13193590762073E-06 0.0187838263233596"
rpy="0 0 0" />
<mass
value="0.745" />
<inertia
ixx="0.0004716"
ixy="0"
ixz="0"
iyy="0.0007945"
iyz="0.0000001"
izz="0.0004716" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_wheel1_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_wheel1_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_wheel1_joint"
type="revolute">
<origin
xyz="0 -0.48 0.01"
rpy="0 0 -0.0029091" />
<parent
link="right_hip_pitch_link" />
<child
link="right_wheel1_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1000"
upper="1000"
effort="100"
velocity="50" />
</joint>
<link
name="left_shoulder_link">
<inertial>
<origin
xyz="-0.0144837915216556 0.000108169744531657 0.0209530786562939"
rpy="0 0 0" />
<mass
value="0.0131461" />
<inertia
ixx="0.0000070"
ixy="0"
ixz="0.0000017"
iyy="0.0000071"
iyz="0"
izz="0.0000045" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_shoulder_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://urdf/meshes/meshes/left_shoulder_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_shoulder_joint"
type="revolute">
<origin
xyz="-0.06 -0.0878 0.36288"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="left_shoulder_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1.57"
upper="1.57"
effort="100"
velocity="50" />
</joint>
<link
name="left_arm1_link">
<inertial>
<origin
xyz="-0.00228631297282665 -0.000694770956028379 0.0248681244469774"
rpy="0 0 0" />
<mass
value="0.5854363" />
<inertia
ixx="0.0008783"
ixy="-0.0000561"
ixz="0.0000086"
iyy="0.0002821"
iyz="-0.0000017"
izz="0.0009299" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_arm1_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://urdf/meshes/meshes/left_arm1_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_arm1_joint"
type="revolute">
<origin
xyz="0 0 0.0385"
rpy="1.5708 0 -0.0074682" />
<parent
link="left_shoulder_link" />
<child
link="left_arm1_link" />
<axis
xyz="-1 0 0" />
<limit
lower="-1.5"
upper="1.5"
effort="42.8"
velocity="50" />
</joint>
<link
name="left_arm2_link">
<inertial>
<origin
xyz="-0.0237485342372672 -0.0546317049801525 1.0877545797372E-05"
rpy="0 0 0" />
<mass
value="0.7955302" />
<inertia
ixx="0.0035648"
ixy="-0.0000258"
ixz="0.0000009"
iyy="0.0002355"
iyz="-0.0001213"
izz="0.0036543" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/left_arm2_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://urdf/meshes/meshes/left_arm2_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_arm2_joint"
type="revolute">
<origin
xyz="0.020127 0.0040341 0.17817"
rpy="-1.6062 0.0074682 0" />
<parent
link="left_arm1_link" />
<child
link="left_arm2_link" />
<axis
xyz="-0.99997 0.0074634 0.0002647" />
<limit
lower="-1.5"
upper="1.5"
effort="18"
velocity="50" />
</joint>
<link
name="right_shoulder_link">
<inertial>
<origin
xyz="-0.0144831349305154 -0.000175271563297497 0.0209530786562937"
rpy="0 0 0" />
<mass
value="0.0131461" />
<inertia
ixx="0.0000070"
ixy="0"
ixz="0.0000017"
iyy="0.0000071"
iyz="0"
izz="0.0000045" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_shoulder_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://urdf/meshes/meshes/right_shoulder_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_shoulder_joint"
type="revolute">
<origin
xyz="-0.06 0.0878 0.36288"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="right_shoulder_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1.57"
upper="1.57"
effort="100"
velocity="50" />
</joint>
<link
name="right_arm1_link">
<inertial>
<origin
xyz="-0.00258654183724675 0.00163071193731268 -0.0247948407356193"
rpy="0 0 0" />
<mass
value="0.5854363" />
<inertia
ixx="0.0008778"
ixy="0.0000592"
ixz="-0.0000064"
iyy="0.0002836"
iyz="-0.0000262"
izz="0.0009288" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_arm1_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://urdf/meshes/meshes/right_arm1_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_arm1_joint"
type="revolute">
<origin
xyz="0 0 0.0385"
rpy="1.5708 0 0" />
<parent
link="right_shoulder_link" />
<child
link="right_arm1_link" />
<axis
xyz="1 0 0" />
<limit
lower="-1.5"
upper="1.5"
effort="42.8"
velocity="50" />
</joint>
<link
name="right_arm2_link">
<inertial>
<origin
xyz="-0.0240175529876588 0.0558375631647781 2.11385309717427E-06"
rpy="0 0 0" />
<mass
value="0.7955302" />
<inertia
ixx="0.0035728"
ixy="0.0000412"
ixz="-0.0000004"
iyy="0.0002317"
iyz="0.0000292"
izz="0.0036666" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://urdf/meshes/meshes/right_arm2_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://urdf/meshes/meshes/right_arm2_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_arm2_joint"
type="revolute">
<origin
xyz="0.017969 0.0026972 -0.17843"
rpy="-1.5793 0 0" />
<parent
link="right_arm1_link" />
<child
link="right_arm2_link" />
<axis
xyz="-0.99993 -0.0121 -0.00010316" />
<limit
lower="-1.5"
upper="1.5"
effort="18"
velocity="50" />
</joint>
</robot>

440
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf copy.xacro
View File

@@ -1,440 +0,0 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- 转换自 URDF 的 XACRO 文件，保留原模型参数并简化重复结构 -->
<robot name="hive_core_bot0" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find hive_core_r0)/urdf/generic_link.xacro" />
<xacro:include filename="$(find hive_core_r0)/urdf/generic_joint.xacro" />
<!-- 1. 基础链接（base_link） -->
<xacro:generic_link
link_name="base_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/base_link.STL"
mass="0.4247767"
inertia_ixx="0.1131876" inertia_ixy="0.0000008" inertia_ixz="0.0001546"
inertia_iyy="0.0858545" inertia_iyz="0.0000006" inertia_izz="0.0325040"
origin_xyz="-0.0598891820748674 5.31626531352636E-07 0.164869219254393"
origin_rpy="0 0 0"
color_rgba="0.666666666666667 0.666666666666667 0.666666666666667 1"
enable_collision="false"
/>
<!-- <link name="world">
<inertial>
<mass value="0.0"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0"
iyy="0.0" iyz="0.0"
izz="0.0"/>
</inertial>
</link>
<joint name="base_link_to_world_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="world" />
<child link="base_link" />
</joint> -->
<!-- 2. 左髋关节及腿部（left_hip/leg/wheel） -->
<!-- 2.1 左髋链接 -->
<xacro:generic_link
link_name="left_hip_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/left_hip_pitch_link.STL"
mass="0.6475263"
inertia_ixx="0.0182516" inertia_ixy="0.0000058" inertia_ixz="0.0001729"
inertia_iyy="0.0181748" inertia_iyz="-0.0006055" inertia_izz="0.0003913"
origin_xyz="3.93767161736252E-06 0.161490126813208 0.0158567790568734"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 2.2 左髋关节 -->
<xacro:generic_joint
joint_name="left_hip_pitch_joint"
type="revolute"
parent="base_link"
child="left_hip_pitch_link"
origin_xyz="-0.06 -0.1358 -0.037269"
origin_rpy="-1.5708 0.523 0.0"
axis="0 0 -1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2"
velocity="3"
/>
<!-- 2.3 左腿链接 -->
<xacro:generic_link
link_name="left_leg_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/left_leg_pitch_link.STL"
mass="0.5126336"
inertia_ixx="0.0135141" inertia_ixy="0.0000005" inertia_ixz="-0.0000325"
inertia_iyy="0.0135986" inertia_iyz="0.0001480" inertia_izz="0.0002025"
origin_xyz="-2.63464180747426E-05 0.197159145100908 0.000498203326348359"
origin_rpy="0 0 0"
color_rgba="1 0.372549019607843 0.294117647058824 1"
enable_collision="false"
/>
<!-- 2.4 左腿关节 -->
<xacro:generic_joint
joint_name="left_leg_pitch_joint"
type="revolute"
parent="left_hip_pitch_link"
child="left_leg_pitch_link"
origin_xyz="0 0 -0.0295"
origin_rpy="0 0 -1.046"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2"
velocity="3"
/>
<!-- 2.5 左车轮2链接 -->
<xacro:generic_link
link_name="left_wheel2_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/left_wheel2_link.STL"
mass="0.745"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="4.1585564079967E-08 -5.47447156262759E-06 0.0187838263233595"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 2.6 左车轮2关节 -->
<xacro:generic_joint
joint_name="left_wheel2_joint"
type="revolute"
parent="left_leg_pitch_link"
child="left_wheel2_link"
origin_xyz="0 0.4805 0.004"
origin_rpy="-3.1416 0 -0.0024764"
axis="0 0 1"
limit_lower="-1000"
limit_upper="1000"
effort="2"
velocity="3"
/>
<!-- 2.7 左车轮1链接 -->
<xacro:generic_link
link_name="left_wheel1_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/left_wheel1_link.STL"
mass="0.745"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="1.97042367054093E-06 5.10773910927798E-06 0.0187838263233595"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 2.8 左车轮1关节 -->
<xacro:generic_joint
joint_name="left_wheel1_joint"
type="revolute"
parent="left_hip_pitch_link"
child="left_wheel1_link"
origin_xyz="0 0.48 0.01"
origin_rpy="0 0 0.0095732"
axis="0 0 1"
limit_lower="-1000"
limit_upper="1000"
effort="2"
velocity="3"
/>
<!-- 3. 右髋关节及腿部（right_hip/leg/wheel） -->
<!-- 3.1 右髋链接 -->
<xacro:generic_link
link_name="right_hip_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/right_hip_pitch_link.STL"
mass="0.6475263"
inertia_ixx="0.0182531" inertia_ixy="-0.0000017" inertia_ixz="0.0000501"
inertia_iyy="0.0181748" inertia_iyz="0.0006055" inertia_izz="0.0003897"
origin_xyz="-3.97209815898069E-06 -0.161490067811833 0.0158567790568734"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 3.2 右髋关节 -->
<xacro:generic_joint
joint_name="right_hip_pitch_joint"
type="revolute"
parent="base_link"
child="right_hip_pitch_link"
origin_xyz="-0.06 0.1358 -0.037269"
origin_rpy="1.5708 0.523 0"
axis="0 0 -1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2"
velocity="3"
/>
<!-- 3.3 右腿链接 -->
<xacro:generic_link
link_name="right_leg_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/right_leg_pitch_link.STL"
mass="0.5126336"
inertia_ixx="0.0135142" inertia_ixy="0.0000001" inertia_ixz="0.0000001"
inertia_iyy="0.0135986" inertia_iyz="-0.0001480" inertia_izz="0.0002024"
origin_xyz="3.43008600186093E-05 -0.197159143801203 0.000498203308628542"
origin_rpy="0 0 0"
color_rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"
enable_collision="false"
/>
<!-- 3.4 右腿关节 -->
<xacro:generic_joint
joint_name="right_leg_pitch_joint"
type="revolute"
parent="right_hip_pitch_link"
child="right_leg_pitch_link"
origin_xyz="0 0 -0.0295"
origin_rpy="0 0 1.046"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2"
velocity="3"
/>
<!-- 3.5 右车轮2链接 -->
<xacro:generic_link
link_name="right_wheel2_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/right_wheel2_link.STL"
mass="0.745"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="-5.49216228909084E-08 5.47435401360152E-06 0.0187838263233596"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 3.6 右车轮2关节 -->
<xacro:generic_joint
joint_name="right_wheel2_joint"
type="revolute"
parent="right_leg_pitch_link"
child="right_wheel2_link"
origin_xyz="1.9386E-05 -0.4805 0.004"
origin_rpy="3.1416 0 0"
axis="0 0 1"
limit_lower="-1000"
limit_upper="1000"
effort="2"
velocity="3"
/>
<!-- 3.7 右车轮1链接 -->
<xacro:generic_link
link_name="right_wheel1_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/right_wheel1_link.STL"
mass="0.745"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="-1.90651569380871E-06 -5.13193590762073E-06 0.0187838263233596"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 3.8 右车轮1关节 -->
<xacro:generic_joint
joint_name="right_wheel1_joint"
type="revolute"
parent="right_hip_pitch_link"
child="right_wheel1_link"
origin_xyz="0 -0.48 0.01"
origin_rpy="0 0 -0.0029091"
axis="0 0 1"
limit_lower="-1000"
limit_upper="1000"
effort="2"
velocity="3"
/>
<!-- 4. 左手臂（left_shoulder/arm） -->
<!-- 4.1 左肩链接 -->
<xacro:generic_link
link_name="left_shoulder_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/left_shoulder_link.STL"
mass="0.0131461"
inertia_ixx="0.0000070" inertia_ixy="0" inertia_ixz="0.0000017"
inertia_iyy="0.0000071" inertia_iyz="0" inertia_izz="0.0000045"
origin_xyz="-0.0144837915216556 0.000108169744531657 0.0209530786562939"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 4.2 左肩关节 -->
<xacro:generic_joint
joint_name="left_shoulder_joint"
type="revolute"
parent="base_link"
child="left_shoulder_link"
origin_xyz="-0.06 -0.0878 0.36288"
origin_rpy="0 0 0"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2"
velocity="3"
/>
<!-- 4.3 左手臂1链接 -->
<xacro:generic_link
link_name="left_arm1_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/left_arm1_link.STL"
mass="0.5854363"
inertia_ixx="0.0008783" inertia_ixy="-0.0000561" inertia_ixz="0.0000086"
inertia_iyy="0.0002821" inertia_iyz="-0.0000017" inertia_izz="0.0009299"
origin_xyz="-0.00228631297282665 -0.000694770956028379 0.0248681244469774"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 4.4 左手臂1关节 -->
<xacro:generic_joint
joint_name="left_arm1_joint"
type="revolute"
parent="left_shoulder_link"
child="left_arm1_link"
origin_xyz="0 0 0.0385"
origin_rpy="1.5708 0 -0.0074682"
axis="-1 0 0"
limit_lower="-1.5"
limit_upper="1.5"
effort="2"
velocity="3"
/>
<!-- 4.5 左手臂2链接 -->
<xacro:generic_link
link_name="left_arm2_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/left_arm2_link.STL"
mass="0.7955302"
inertia_ixx="0.0035648" inertia_ixy="-0.0000258" inertia_ixz="0.0000009"
inertia_iyy="0.0002355" inertia_iyz="-0.0001213" inertia_izz="0.0036543"
origin_xyz="-0.0237485342372672 -0.0546317049801525 1.0877545797372E-05"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 4.6 左手臂2关节 -->
<xacro:generic_joint
joint_name="left_arm2_joint"
type="revolute"
parent="left_arm1_link"
child="left_arm2_link"
origin_xyz="0.020127 0.0040341 0.17817"
origin_rpy="-1.6062 0.0074682 0"
axis="-0.99997 0.0074634 0.0002647"
limit_lower="-1.5"
limit_upper="1.5"
effort="2"
velocity="3"
/>
<!-- 5. 右手臂（right_shoulder/arm） -->
<!-- 5.1 右肩链接 -->
<xacro:generic_link
link_name="right_shoulder_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/right_shoulder_link.STL"
mass="0.0131461"
inertia_ixx="0.0000070" inertia_ixy="0" inertia_ixz="0.0000017"
inertia_iyy="0.0000071" inertia_iyz="0" inertia_izz="0.0000045"
origin_xyz="-0.0144831349305154 -0.000175271563297497 0.0209530786562937"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 5.2 右肩关节 -->
<xacro:generic_joint
joint_name="right_shoulder_joint"
type="revolute"
parent="base_link"
child="right_shoulder_link"
origin_xyz="-0.06 0.0878 0.36288"
origin_rpy="0 0 0"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2"
velocity="3"
/>
<!-- 5.3 右手臂1链接 -->
<xacro:generic_link
link_name="right_arm1_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/right_arm1_link.STL"
mass="0.5854363"
inertia_ixx="0.0008778" inertia_ixy="0.0000592" inertia_ixz="-0.0000064"
inertia_iyy="0.0002836" inertia_iyz="-0.0000262" inertia_izz="0.0009288"
origin_xyz="-0.00258654183724675 0.00163071193731268 -0.0247948407356193"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 5.4 右手臂1关节 -->
<xacro:generic_joint
joint_name="right_arm1_joint"
type="revolute"
parent="right_shoulder_link"
child="right_arm1_link"
origin_xyz="0 0 0.0385"
origin_rpy="1.5708 0 0"
axis="1 0 0"
limit_lower="-1.5"
limit_upper="1.5"
effort="2"
velocity="3"
/>
<!-- 5.5 右手臂2链接 -->
<xacro:generic_link
link_name="right_arm2_link"
mesh_file="file://$(find hive_core_r0)/urdf/meshes/right_arm2_link.STL"
mass="0.7955302"
inertia_ixx="0.0035728" inertia_ixy="0.0000412" inertia_ixz="-0.0000004"
inertia_iyy="0.0002317" inertia_iyz="0.0000292" inertia_izz="0.0036666"
origin_xyz="-0.0240175529876588 0.0558375631647781 2.11385309717427E-06"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 5.6 右手臂2关节 -->
<xacro:generic_joint
joint_name="right_arm2_joint"
type="revolute"
parent="right_arm1_link"
child="right_arm2_link"
origin_xyz="0.017969 0.0026972 -0.17843"
origin_rpy="-1.5793 0 0"
axis="-0.99993 -0.0121 -0.00010316"
limit_lower="-1.5"
limit_upper="1.5"
effort="2"
velocity="3"
/>
<xacro:include filename="$(find hive_core_r0)/urdf/bot0.ros2_control.xacro" />
<xacro:bot0_ros2_control />
</robot>

438
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf.xacro
View File

@@ -1,438 +0,0 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- 转换自 URDF 的 XACRO 文件，保留原模型参数并简化重复结构 -->
<robot name="hive_core_bot0" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find hive_core_r0)/urdf/generic_link.xacro" />
<xacro:include filename="$(find hive_core_r0)/urdf/generic_joint.xacro" />
<!-- 1. 基础链接（base_link） -->
<xacro:generic_link
link_name="base_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/base_link.STL"
mass="0.2"
inertia_ixx="0.00076" inertia_ixy="0.0000008" inertia_ixz="0.00001546"
inertia_iyy="0.000858545" inertia_iyz="0.0000006" inertia_izz="0.000325040"
origin_xyz="-0.0598891820748674 5.31626531352636E-07 0.164869219254393"
origin_rpy="0 0 0"
color_rgba="0.666666666666667 0.666666666666667 0.666666666666667 1"
enable_collision="false"
/>
<link name="world">
<inertial>
<mass value="0.0"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0"
iyy="0.0" iyz="0.0"
izz="0.0"/>
</inertial>
</link>
<joint name="base_link_to_world_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="world" />
<child link="base_link" />
</joint>
<!-- 2. 左髋关节及腿部（left_hip/leg/wheel） -->
<!-- 2.1 左髋链接 -->
<xacro:generic_link
link_name="left_hip_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/left_hip_pitch_link.STL"
mass="0.1475263"
inertia_ixx="0.000182516" inertia_ixy="0.0000058" inertia_ixz="0.0001729"
inertia_iyy="0.000181748" inertia_iyz="-0.0006055" inertia_izz="0.0003913"
origin_xyz="3.93767161736252E-06 0.161490126813208 0.0158567790568734"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 2.2 左髋关节 -->
<xacro:generic_joint
joint_name="left_hip_pitch_joint"
type="revolute"
parent="base_link"
child="left_hip_pitch_link"
origin_xyz="-0.06 -0.1358 -0.037269"
origin_rpy="-1.5708 0.523 0.0"
axis="0 0 -1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 2.3 左腿链接 -->
<xacro:generic_link
link_name="left_leg_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/left_leg_pitch_link.STL"
mass="0.1126336"
inertia_ixx="0.000135141" inertia_ixy="0.0000005" inertia_ixz="-0.0000325"
inertia_iyy="0.000135986" inertia_iyz="0.0001480" inertia_izz="0.0002025"
origin_xyz="-2.63464180747426E-05 0.197159145100908 0.000498203326348359"
origin_rpy="0 0 0"
color_rgba="1 0.372549019607843 0.294117647058824 1"
enable_collision="false"
/>
<!-- 2.4 左腿关节 -->
<xacro:generic_joint
joint_name="left_leg_pitch_joint"
type="revolute"
parent="left_hip_pitch_link"
child="left_leg_pitch_link"
origin_xyz="0 0 -0.0295"
origin_rpy="0 0 -1.046"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 2.5 左车轮2链接 -->
<xacro:generic_link
link_name="left_wheel2_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/left_wheel2_link.STL"
mass="0.145"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="4.1585564079967E-08 -5.47447156262759E-06 0.0187838263233595"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 2.6 左车轮2关节 -->
<xacro:generic_joint
joint_name="left_wheel2_joint"
type="revolute"
parent="left_leg_pitch_link"
child="left_wheel2_link"
origin_xyz="0 0.4805 0.004"
origin_rpy="-3.1416 0 -0.0024764"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 2.7 左车轮1链接 -->
<xacro:generic_link
link_name="left_wheel1_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/left_wheel1_link.STL"
mass="0.145"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="1.97042367054093E-06 5.10773910927798E-06 0.0187838263233595"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 2.8 左车轮1关节 -->
<xacro:generic_joint
joint_name="left_wheel1_joint"
type="revolute"
parent="left_hip_pitch_link"
child="left_wheel1_link"
origin_xyz="0 0.48 0.01"
origin_rpy="0 0 0.0095732"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 3. 右髋关节及腿部（right_hip/leg/wheel） -->
<!-- 3.1 右髋链接 -->
<xacro:generic_link
link_name="right_hip_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/right_hip_pitch_link.STL"
mass="0.1475263"
inertia_ixx="0.000182531" inertia_ixy="-0.0000017" inertia_ixz="0.0000501"
inertia_iyy="0.000181748" inertia_iyz="0.0006055" inertia_izz="0.0003897"
origin_xyz="-3.97209815898069E-06 -0.161490067811833 0.0158567790568734"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 3.2 右髋关节 -->
<xacro:generic_joint
joint_name="right_hip_pitch_joint"
type="revolute"
parent="base_link"
child="right_hip_pitch_link"
origin_xyz="-0.06 0.1358 -0.037269"
origin_rpy="1.5708 0.523 0"
axis="0 0 -1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 3.3 右腿链接 -->
<xacro:generic_link
link_name="right_leg_pitch_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/right_leg_pitch_link.STL"
mass="0.1126336"
inertia_ixx="0.000135142" inertia_ixy="0.0000001" inertia_ixz="0.0000001"
inertia_iyy="0.000135986" inertia_iyz="-0.0001480" inertia_izz="0.0002024"
origin_xyz="3.43008600186093E-05 -0.197159143801203 0.000498203308628542"
origin_rpy="0 0 0"
color_rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"
enable_collision="false"
/>
<!-- 3.4 右腿关节 -->
<xacro:generic_joint
joint_name="right_leg_pitch_joint"
type="revolute"
parent="right_hip_pitch_link"
child="right_leg_pitch_link"
origin_xyz="0 0 -0.0295"
origin_rpy="0 0 1.046"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 3.5 右车轮2链接 -->
<xacro:generic_link
link_name="right_wheel2_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/right_wheel2_link.STL"
mass="0.145"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="-5.49216228909084E-08 5.47435401360152E-06 0.0187838263233596"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 3.6 右车轮2关节 -->
<xacro:generic_joint
joint_name="right_wheel2_joint"
type="revolute"
parent="right_leg_pitch_link"
child="right_wheel2_link"
origin_xyz="1.9386E-05 -0.4805 0.004"
origin_rpy="3.1416 0 0"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 3.7 右车轮1链接 -->
<xacro:generic_link
link_name="right_wheel1_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/right_wheel1_link.STL"
mass="0.145"
inertia_ixx="0.0004716" inertia_ixy="0" inertia_ixz="0"
inertia_iyy="0.0007945" inertia_iyz="0.0000001" inertia_izz="0.0004716"
origin_xyz="-1.90651569380871E-06 -5.13193590762073E-06 0.0187838263233596"
origin_rpy="0 0 0"
color_rgba="0.780392156862745 0.780392156862745 0.780392156862745 1"
enable_collision="true"
/>
<!-- 3.8 右车轮1关节 -->
<xacro:generic_joint
joint_name="right_wheel1_joint"
type="revolute"
parent="right_hip_pitch_link"
child="right_wheel1_link"
origin_xyz="0 -0.48 0.01"
origin_rpy="0 0 -0.0029091"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 4. 左手臂（left_shoulder/arm） -->
<!-- 4.1 左肩链接 -->
<xacro:generic_link
link_name="left_shoulder_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/left_shoulder_link.STL"
mass="0.0131461"
inertia_ixx="0.0000070" inertia_ixy="0" inertia_ixz="0.0000017"
inertia_iyy="0.0000071" inertia_iyz="0" inertia_izz="0.0000045"
origin_xyz="-0.0144837915216556 0.000108169744531657 0.0209530786562939"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 4.2 左肩关节 -->
<xacro:generic_joint
joint_name="left_shoulder_joint"
type="revolute"
parent="base_link"
child="left_shoulder_link"
origin_xyz="-0.06 -0.0878 0.36288"
origin_rpy="0 0 0"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 4.3 左手臂1链接 -->
<xacro:generic_link
link_name="left_arm1_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/left_arm1_link.STL"
mass="0.1854363"
inertia_ixx="0.0008783" inertia_ixy="-0.0000561" inertia_ixz="0.0000086"
inertia_iyy="0.0002821" inertia_iyz="-0.0000017" inertia_izz="0.0009299"
origin_xyz="-0.00228631297282665 -0.000694770956028379 0.0248681244469774"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 4.4 左手臂1关节 -->
<xacro:generic_joint
joint_name="left_arm1_joint"
type="revolute"
parent="left_shoulder_link"
child="left_arm1_link"
origin_xyz="0 0 0.0385"
origin_rpy="1.5708 0 -0.0074682"
axis="-1 0 0"
limit_lower="-1.5"
limit_upper="1.5"
effort="2000"
velocity="30"
/>
<!-- 4.5 左手臂2链接 -->
<xacro:generic_link
link_name="left_arm2_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/left_arm2_link.STL"
mass="0.1955302"
inertia_ixx="0.000035648" inertia_ixy="-0.0000258" inertia_ixz="0.0000009"
inertia_iyy="0.0002355" inertia_iyz="-0.0001213" inertia_izz="0.0036543"
origin_xyz="-0.0237485342372672 -0.0546317049801525 1.0877545797372E-05"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 4.6 左手臂2关节 -->
<xacro:generic_joint
joint_name="left_arm2_joint"
type="revolute"
parent="left_arm1_link"
child="left_arm2_link"
origin_xyz="0.020127 0.0040341 0.17817"
origin_rpy="-1.6062 0.0074682 0"
axis="-0.99997 0.0074634 0.0002647"
limit_lower="-1.5"
limit_upper="1.5"
effort="2000"
velocity="30"
/>
<!-- 5. 右手臂（right_shoulder/arm） -->
<!-- 5.1 右肩链接 -->
<xacro:generic_link
link_name="right_shoulder_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/right_shoulder_link.STL"
mass="0.0131461"
inertia_ixx="0.0000070" inertia_ixy="0" inertia_ixz="0.0000017"
inertia_iyy="0.0000071" inertia_iyz="0" inertia_izz="0.0000045"
origin_xyz="-0.0144831349305154 -0.000175271563297497 0.0209530786562937"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 5.2 右肩关节 -->
<xacro:generic_joint
joint_name="right_shoulder_joint"
type="revolute"
parent="base_link"
child="right_shoulder_link"
origin_xyz="-0.06 0.0878 0.36288"
origin_rpy="0 0 0"
axis="0 0 1"
limit_lower="-1.57"
limit_upper="1.57"
effort="2000"
velocity="30"
/>
<!-- 5.3 右手臂1链接 -->
<xacro:generic_link
link_name="right_arm1_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/right_arm1_link.STL"
mass="0.1854363"
inertia_ixx="0.0008778" inertia_ixy="0.0000592" inertia_ixz="-0.0000064"
inertia_iyy="0.0002836" inertia_iyz="-0.0000262" inertia_izz="0.0009288"
origin_xyz="-0.00258654183724675 0.00163071193731268 -0.0247948407356193"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 5.4 右手臂1关节 -->
<xacro:generic_joint
joint_name="right_arm1_joint"
type="revolute"
parent="right_shoulder_link"
child="right_arm1_link"
origin_xyz="0 0 0.0385"
origin_rpy="1.5708 0 0"
axis="1 0 0"
limit_lower="-1.5"
limit_upper="1.5"
effort="2000"
velocity="30"
/>
<!-- 5.5 右手臂2链接 -->
<xacro:generic_link
link_name="right_arm2_link"
mesh_file="file://$(find hive_core_r0)/urdf/hiveBot0/meshes/right_arm2_link.STL"
mass="0.1955302"
inertia_ixx="0.0035728" inertia_ixy="0.0000412" inertia_ixz="-0.0000004"
inertia_iyy="0.0002317" inertia_iyz="0.0000292" inertia_izz="0.0036666"
origin_xyz="-0.0240175529876588 0.0558375631647781 2.11385309717427E-06"
origin_rpy="0 0 0"
color_rgba="1 1 1 1"
enable_collision="false"
/>
<!-- 5.6 右手臂2关节 -->
<xacro:generic_joint
joint_name="right_arm2_joint"
type="revolute"
parent="right_arm1_link"
child="right_arm2_link"
origin_xyz="0.017969 0.0026972 -0.17843"
origin_rpy="-1.5793 0 0"
axis="-0.99993 -0.0121 -0.00010316"
limit_lower="-1.5"
limit_upper="1.5"
effort="2000"
velocity="30"
/>
<xacro:include filename="$(find hive_core_r0)/urdf/hiveBot0/bot0.ros2_control.xacro" />
<xacro:bot0_ros2_control />
</robot>

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/base_link.STL
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_arm1_link.STL
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_arm2_link.STL
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_hip_pitch_link.STL
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_leg_pitch_link.STL
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_shoulder_link.STL
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_wheel1_link.STL
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_wheel2_link.STL
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_arm1_link.STL
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_arm2_link.STL
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_hip_pitch_link.STL
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_leg_pitch_link.STL
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_shoulder_link.STL
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_wheel1_link.STL
View File

Binary file not shown.

BIN
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_wheel2_link.STL
View File

Binary file not shown.

152
HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/robot_roll.py
View File

@@ -1,152 +0,0 @@
import mujoco
import mujoco.viewer
import numpy as np
import time
import math
# 目标Roll倾斜角度（单位：度）
TARGET_ROLL_DEGREE = 7.0 # 目标倾斜7度
ROLL_TRANSITION_DURATION = 1.5 # 倾斜过渡时间（秒）
STABLE_DURATION = 1.0 # 稳定阶段持续时间（秒）
def main():
# 加载模型和创建模拟
model = mujoco.MjModel.from_xml_path("/home/mrji/mujoco_learning/robot3/urdf/robot3.xml")
data = mujoco.MjData(model)
# 完整重置机器人状态（第一次重置）
mujoco.mj_resetData(model, data)
# 设置初始关节角度为0
data.qpos[model.joint("left_hip_pitch_joint").id] = 0
data.qpos[model.joint("right_hip_pitch_joint").id] = 0
data.qpos[model.joint("left_leg_pitch_joint").id] = 0
data.qpos[model.joint("right_leg_pitch_joint").id] = 0
# 重置所有速度为0
data.qvel[:] = 0
# 第二次重置模型数据（规避加载问题）
mujoco.mj_resetData(model, data)
# 再次设置初始关节角度为0
data.qpos[model.joint("left_hip_pitch_joint").id] = 0
data.qpos[model.joint("right_hip_pitch_joint").id] = 0
data.qpos[model.joint("left_leg_pitch_joint").id] = 0
data.qpos[model.joint("right_leg_pitch_joint").id] = 0
# 创建查看器
with mujoco.viewer.launch_passive(model, data) as viewer:
# 设置摄像头位置
viewer.cam.azimuth = 180 # 面向机器人正面
viewer.cam.elevation = -20 # 稍微俯视
viewer.cam.distance = 2.0 # 距离
viewer.cam.lookat[:] = [0.0, 0.0, 0.65] # 看向机器人中心
# 获取执行器ID
hip_left = model.actuator("1_pos").id
leg_left = model.actuator("2_pos").id
hip_right = model.actuator("3_pos").id
leg_right = model.actuator("4_pos").id
# 设置控制信号初始值为0
data.ctrl[hip_left] = 0
data.ctrl[leg_left] = 0
data.ctrl[hip_right] = 0
data.ctrl[leg_right] = 0
# 修改：在最开始的时候额外重置一次
mujoco.mj_resetData(model, data)
# 状态控制变量
start_time = time.time()
current_phase = "STABLE" # STABLE, MOVING, DONE
transition_start_time = 0
# 控制参数
hip_adjustment = 0.1 # 髋关节调整量（根据实验确定）
leg_adjustment = 0.15 # 腿关节调整量（根据实验确定）
initial_roll = 0.0 # 初始Roll角度
current_roll = 0.0 # 当前Roll角度
# 初始稳定步骤 - 让物理引擎稳定
for _ in range(200):
mujoco.mj_step(model, data)
viewer.sync()
time.sleep(0.001)
# 模拟循环
while viewer.is_running():
step_start = time.time()
elapsed_time = time.time() - start_time
# 状态机控制
if current_phase == "STABLE":
# 稳定阶段 - 保持初始姿态
if elapsed_time >= STABLE_DURATION:
# 记录初始姿态（Roll角度）
quat = data.sensor("orientation").data.copy()
initial_roll = get_roll_from_quat(quat)
print(f"稳定阶段结束，初始Roll角度: {math.degrees(initial_roll):.2f}度")
current_phase = "MOVING"
transition_start_time = time.time()
print("开始Roll倾斜过渡...")
elif current_phase == "MOVING":
# 过渡阶段 - 平滑过渡到目标倾斜
transition_elapsed = time.time() - transition_start_time
if transition_elapsed >= ROLL_TRANSITION_DURATION:
# 达到目标倾斜
data.ctrl[hip_left] = 0 - hip_adjustment
data.ctrl[leg_left] = 0 - leg_adjustment
data.ctrl[hip_right] = 0 + hip_adjustment
data.ctrl[leg_right] = 0 + leg_adjustment
current_phase = "DONE"
print("达到目标倾斜状态")
else:
# 线性插值过渡
fraction = transition_elapsed / ROLL_TRANSITION_DURATION
data.ctrl[hip_left] = 0 - hip_adjustment * fraction
data.ctrl[leg_left] = 0 - leg_adjustment * fraction
data.ctrl[hip_right] = 0 + hip_adjustment * fraction
data.ctrl[leg_right] = 0 + leg_adjustment * fraction
elif current_phase == "DONE":
# 倾斜完成阶段 - 保持当前状态
# 计算并输出当前Roll角度
quat = data.sensor("orientation").data.copy()
current_roll = get_roll_from_quat(quat)
roll_angle_deg = math.degrees(current_roll - initial_roll)
print(f"当前Roll倾斜角度: {roll_angle_deg:.2f}度")
# 停留一段时间后退出
if time.time() - transition_start_time > ROLL_TRANSITION_DURATION + 2.0:
print(f"最终Roll倾斜角度: {roll_angle_deg:.2f}度 (目标: {TARGET_ROLL_DEGREE}度)")
break
# 执行物理模拟
mujoco.mj_step(model, data)
# 同步查看器
viewer.sync()
# 控制步长（约1000Hz）
time_until_next_step = 0.001 - (time.time() - step_start)
if time_until_next_step > 0:
time.sleep(time_until_next_step)
def get_roll_from_quat(quat):
"""从四元数中提取Roll角度（弧度）"""
# 四元数转换为旋转矩阵
rot_matrix = np.zeros(9)
mujoco.mju_quat2Mat(rot_matrix, quat)
# 从旋转矩阵中提取Roll角度（绕X轴的旋转）
roll = math.atan2(rot_matrix[7], rot_matrix[8])
return roll
if __name__ == "__main__":
main()

324
HiveCoreR0/src/hive_core_r0/world/bot0.world
View File

@@ -1,324 +0,0 @@
<sdf version='1.7'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<audio>
<device>default</device>
</audio>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='table'>
<static>1</static>
<link name='link'>
<collision name='surface'>
<pose>0 0 1 0 -0 0</pose>
<geometry>
<box>
<size>1.5 0.8 0.03</size>
</box>
</geometry>
<surface>
<friction>
<ode>
<mu>0.6</mu>
<mu2>0.6</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual1'>
<pose>0 0 1 0 -0 0</pose>
<geometry>
<box>
<size>1.5 0.8 0.03</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Wood</name>
</script>
</material>
</visual>
<collision name='front_left_leg_collision'>
<pose>0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='front_left_leg_visual'>
<pose>0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<collision name='front_right_leg_collision'>
<pose>0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='front_right_leg_visual'>
<pose>0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<collision name='back_right_leg_collision'>
<pose>-0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='back_right_leg_visual'>
<pose>-0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<collision name='back_left_leg_collision'>
<pose>-0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='back_left_leg_visual'>
<pose>-0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>-0.772768 -1.3242 0 0 -0 0</pose>
</model>
<state world_name='default'>
<sim_time>83 658000000</sim_time>
<real_time>83 738917407</real_time>
<wall_time>1753783232 542949961</wall_time>
<iterations>83658</iterations>
<model name='ground_plane'>
<pose>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='table'>
<pose>-0.772768 -1.3242 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-0.772768 -1.3242 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose>0 0 10 0 -0 0</pose>
</light>
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>4.75138 -5.25474 1.98471 0 0.275643 2.35619</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>

321
HiveCoreR0/src/hive_core_r0/world/table.world
View File

@@ -1,321 +0,0 @@
<sdf version='1.7'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link_0'>
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<contact>
<ode>
<kp>1e6</kp> <!-- 地面刚度可稍大于轮子 -->
<kd>5e4</kd>
<mu1>10.0</mu1>
<mu2>10.0</mu2>
</ode>
</contact>
<bounce>
<restitution_coefficient>0.0</restitution_coefficient>
</bounce>
</surface>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<audio>
<device>default</device>
</audio>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='table'>
<static>1</static>
<link name='link_1'>
<collision name='surface'>
<pose>0 0 1 0 -0 0</pose>
<geometry>
<box>
<size>1.5 0.8 0.03</size>
</box>
</geometry>
<surface>
<friction>
<ode>
<mu>0.6</mu>
<mu2>0.6</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual1'>
<pose>0 0 1 0 -0 0</pose>
<geometry>
<box>
<size>1.5 0.8 0.03</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Wood</name>
</script>
</material>
</visual>
<collision name='front_left_leg_collision'>
<pose>0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='front_left_leg_visual'>
<pose>0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<collision name='front_right_leg_collision'>
<pose>0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='front_right_leg_visual'>
<pose>0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<collision name='back_right_leg_collision'>
<pose>-0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='back_right_leg_visual'>
<pose>-0.68 -0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<collision name='back_left_leg_collision'>
<pose>-0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='back_left_leg_visual'>
<pose>-0.68 0.38 0.5 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.02</radius>
<length>1</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>0.726316 -1.21623 0 0 -0 0</pose>
</model>
<state world_name='default'>
<sim_time>56 645000000</sim_time>
<real_time>56 704774179</real_time>
<wall_time>1753436842 65734558</wall_time>
<iterations>56645</iterations>
<model name='ground_plane'>
<pose>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link_0'>
<pose>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='table'>
<pose>0.726316 -1.21623 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link_1'>
<pose>0.726316 -1.21623 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose>0 0 10 0 -0 0</pose>
</light>
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>4.85069 0.30326 2.30729 0 0.443643 -2.92699</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>

31
HiveCoreR0/src/hive_core_r0/CMakeLists.txt → HiveCoreR0/src/keyboard_control/CMakeLists.txt
View File

@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.10)
project(hive_core_r0)
project(keyboard_control)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
@@ -9,27 +9,14 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
add_executable(arm_control_node src/arm_control_node.cpp)
ament_target_dependencies(arm_control_node rclcpp geometry_msgs)
include_directories(
include
)
add_executable(hiveBot1_control_node src/hiveBot1_control_node.cpp)
ament_target_dependencies(hiveBot1_control_node rclcpp geometry_msgs)
add_executable(keyboard_control_node src/keyboard_control_node.cpp)
add_executable(keyboard_control_node src/keyboard_control.cpp)
ament_target_dependencies(keyboard_control_node rclcpp geometry_msgs)
install(TARGETS
arm_control_node
hiveBot1_control_node
keyboard_control_node
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
@@ -42,8 +29,12 @@ if(BUILD_TESTING)
ament_lint_auto_find_test_dependencies()
endif()
install(DIRECTORY launch urdf config world src
DESTINATION share/${PROJECT_NAME}
install(TARGETS
keyboard_control_node
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}/
)
ament_package()

24
HiveCoreR0/src/keyboard_control/include/keyboard_control.hpp Normal file
View File

@@ -0,0 +1,24 @@
#ifndef KEY_BOARD_CONTROL_NODE__HPP_
#define KEY_BOARD_CONTROL_NODE__HPP_
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
class KeyboardControlNode : public rclcpp::Node
{
public:
KeyboardControlNode(); // 构造函数
private:
void print_help();
void timer_callback();
// 成员变量
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
rclcpp::TimerBase::SharedPtr timer_;
double linear_speed_; // 线速度（m/s）
double angular_speed_; // 角速度（rad/s）
};
#endif // KEY_BOARD_CONTROL_NODE__HPP_

23
HiveCoreR0/src/keyboard_control/launch/keyboard_control.launch.py Normal file
View File

@@ -0,0 +1,23 @@
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import TimerAction
def generate_launch_description():
# 创建节点启动描述
keyboard_control_node = Node(
package='keyboard_control', # 功能包名称
executable='keyboard_control_node', # 可执行文件名称
name='keyboard_control_node', # 节点名称（可自定义）
output='screen', # 输出到屏幕
emulate_tty=True, # 模拟终端
)
start_keyboard_control_node = TimerAction(
period=1.0,
actions=[keyboard_control_node],
)
# 组装launch描述
return LaunchDescription([
start_keyboard_control_node
])

18
HiveCoreR0/src/keyboard_control/package.xml Normal file
View File

@@ -0,0 +1,18 @@
<?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>keyboard_control</name>
<version>1.0.0</version>
<description>keyboard package</description>
<maintainer email="Ray@email.com">Your Name</maintainer>
<license>Apache-2.0</license>
<build_depend>ament_cmake</build_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

129
HiveCoreR0/src/keyboard_control/src/keyboard_control.cpp Normal file
View File

@@ -0,0 +1,129 @@
#include "keyboard_control.hpp"
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
#include <chrono>
using namespace std::chrono_literals;
// 键盘输入处理函数（非阻塞模式）
int kbhit()
{
struct termios oldt, newt;
int ch;
int oldf;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
fcntl(STDIN_FILENO, F_SETFL, oldf);
if(ch != EOF)
{
ungetc(ch, stdin);
return 1;
}
return 0;
}
KeyboardControlNode::KeyboardControlNode() : Node("keyboard_control_node")
{
// 声明参数：移动速度、旋转速度
this->declare_parameter("linear_speed", 0.2); // 线速度（m/s）
this->declare_parameter("angular_speed", 0.5); // 角速度（rad/s）
// 读取参数
linear_speed_ = this->get_parameter("linear_speed").as_double();
angular_speed_ = this->get_parameter("angular_speed").as_double();
// 创建发布者（发送速度指令）
twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
"/cmd_vel", 10
);
// 打印控制说明
print_help();
// 创建定时器（周期性检查键盘输入）
timer_ = this->create_wall_timer(
50ms, // 20Hz检查频率
std::bind(&KeyboardControlNode::timer_callback, this)
);
}
void KeyboardControlNode::print_help()
{
RCLCPP_INFO(this->get_logger(), "键盘控制说明:");
RCLCPP_INFO(this->get_logger(), " w : 前进");
RCLCPP_INFO(this->get_logger(), " s : 后退");
RCLCPP_INFO(this->get_logger(), " a : 左转");
RCLCPP_INFO(this->get_logger(), " d : 右转");
RCLCPP_INFO(this->get_logger(), " x : 停止");
RCLCPP_INFO(this->get_logger(), " q : 退出");
}
void KeyboardControlNode::timer_callback()
{
geometry_msgs::msg::Twist twist_msg;
if(kbhit())
{
char key = getchar();
switch(key)
{
case 'w': // 前进
twist_msg.linear.x = linear_speed_;
twist_msg.angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "前进");
break;
case 's': // 后退
twist_msg.linear.x = -linear_speed_;
twist_msg.angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "后退");
break;
case 'a': // 左转
twist_msg.linear.x = 0.0;
twist_msg.angular.z = angular_speed_;
RCLCPP_INFO(this->get_logger(), "左转");
break;
case 'd': // 右转
twist_msg.linear.x = 0.0;
twist_msg.angular.z = -angular_speed_;
RCLCPP_INFO(this->get_logger(), "右转");
break;
case 'x': // 停止
twist_msg.linear.x = 0.0;
twist_msg.angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "停止");
break;
case 'q': // 退出
RCLCPP_INFO(this->get_logger(), "退出控制");
rclcpp::shutdown();
return;
default:
twist_msg.linear.x = 0.0;
twist_msg.angular.z = 0.0;
print_help();
break;
}
twist_pub_->publish(twist_msg);
}
}
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<KeyboardControlNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

16
HiveCoreR0/src/robot_bringup/CMakeLists.txt Normal file
View File

@@ -0,0 +1,16 @@
cmake_minimum_required(VERSION 3.10)
project(robot_bringup)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# 依赖
find_package(ament_cmake REQUIRED)
# 安装启动文件和世界文件
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}/
)
ament_package()

0
HiveCoreR0/src/hive_core_r0/LICENSE → HiveCoreR0/src/robot_bringup/LICENSE
View File

32
HiveCoreR0/src/robot_bringup/launch/main.launch.py Normal file
View File

@@ -0,0 +1,32 @@
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
def generate_launch_description():
sim_pkg_share = FindPackageShare(package="robot_simulation")
sim_launch_path = PathJoinSubstitution(
[sim_pkg_share, "launch", "gazebo_simulation.launch.py"]
)
control_pkg_share = FindPackageShare(package="robot_control")
control_launch_path = PathJoinSubstitution(
[control_pkg_share, "launch", "robot_control.launch.py"]
)
include_simulation = IncludeLaunchDescription(
PythonLaunchDescriptionSource(sim_launch_path),
)
include_control = IncludeLaunchDescription(
PythonLaunchDescriptionSource(control_launch_path),
)
return LaunchDescription([
include_simulation, # 启动仿真子launch
include_control # 启动控制子launch
])

15
HiveCoreR0/src/robot_bringup/package.xml Normal file
View File

@@ -0,0 +1,15 @@
<?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>robot_bringup</name>
<version>1.0.0</version>
<description>Robot control package</description>
<maintainer email="Ray@email.com">Your Name</maintainer>
<license>Apache-2.0</license>
<build_depend>ament_cmake</build_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

29
HiveCoreR0/src/robot_control/CMakeLists.txt Normal file
View File

@@ -0,0 +1,29 @@
cmake_minimum_required(VERSION 3.10)
project(robot_control)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# 依赖
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(geometry_msgs REQUIRED)
include_directories(
include
)
add_executable(robot_control_node src/robot_control.cpp)
ament_target_dependencies(robot_control_node rclcpp geometry_msgs)
install(TARGETS
robot_control_node
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}/
)
ament_package()

202
HiveCoreR0/src/robot_control/LICENSE Normal file
View File

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

38
HiveCoreR0/src/robot_control/include/arm_control.hpp Normal file
View File

@@ -0,0 +1,38 @@
#ifndef ROBOT_CONTROL__ARMCONTROL_HPP_
#define ROBOT_CONTROL__ARMCONTROL_HPP_
#include "rclcpp/rclcpp.hpp"
#include <Eigen/Dense>
class ArmControl
{
public:
ArmControl(int total_joints);
~ArmControl() = default;
void MoveToZeroPoint(double duration);
void MoveToTargetPoint(const Eigen::Vector3d& target_pos, double duration);
void MoveToTargetJoint(const std::vector<double>& target_joint, double duration);
void DefaultArmMovement(std::vector<double>& joint_position_desired, double duration);
private:
int total_joints_; // 总关节数
std::vector<double> joint_position_; // 机械臂关节位置
std::vector<double> joint_velocity_; // 机械臂关节速度
std::vector<double> joint_acceleration_; // 机械臂关节加速度
std::vector<double> joint_torque_; // 机械臂关节力矩
std::vector<double> joint_angle_; // 机械臂关节角度
std::vector<double> joint_position_desired_; // 机械臂关节期望位置
double amplitude_rad_; // 振幅 （弧度）
double amplitude_deg_; // 振幅 （角度）
double frequency_; // 频率
};
#endif // ROBOT_CONTROL__ARMCONTROL_HPP_

24
HiveCoreR0/src/robot_control/include/leg_control.hpp Normal file
View File

@@ -0,0 +1,24 @@
#ifndef ROBOT_CONTROL__LEGCONTROL_HPP_
#define ROBOT_CONTROL__LEGCONTROL_HPP_
#include "rclcpp/rclcpp.hpp"
#include <cmath>
namespace robot_control
{
class LegControl
{
public:
LegControl();
~LegControl();
void set_goal(double x, double y);
private:
};
} // namespace robot_control
#endif // ROBOT_CONTROL__LEGCONTROL_HPP_

60
HiveCoreR0/src/robot_control/include/robot_control.hpp Normal file
View File

@@ -0,0 +1,60 @@
#ifndef ROBOT_CONTROL__HPP_
#define ROBOT_CONTROL__HPP_
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "geometry_msgs/msg/twist.hpp"
namespace robot_control
{
class RobotControlNode : public rclcpp::Node
{
public:
RobotControlNode();
~RobotControlNode();
// 关节复位
void resetJoints();
private:
void print_help();
void twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
void timer_callback();
void handle_phase_motion(std_msgs::msg::Float64MultiArray& cmd_msg, double current_time);
void handle_wheel_control(std_msgs::msg::Float64MultiArray& cmd_msg, double dt_sec);
// 成员变量
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr cmd_pub_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time start_time_;
rclcpp::Time last_time_;
// 运动参数
double leg_target_rad_; // 腿部目标角度（弧度）
double arm_amplitude_rad_; // 手臂摆动振幅（弧度）
double arm_frequency_; // 手臂摆动频率（Hz）
int total_joints_; // 总关节数
bool phase1_notified_ = false; // 第一阶段结束提示标记
// 关节索引
std::vector<int> leg_joint_indices_; // 腿部关节索引
std::vector<int> arm_joint_indices_; // 手臂关节索引
std::vector<int> wheel_joint_indices_; // 轮子关节索引
std::vector<int> joint_directions_; // 关节方向
std::vector<int> wheel_joint_directions_; // 轮子关节方向
// 轮子控制相关
double wheel_radius_; // 轮子半径（米）
double wheel_separation_; // 轮距（米）
std::vector<double> wheel_targets_; // 轮子目标角度（弧度）
double linear_x_ = 0.0; // 线速度（m/s）
double angular_z_ = 0.0; // 角速度（rad/s）
double linear_speed_; // 最大线速度（m/s）
double angular_speed_; // 最大角速度（rad/s）
};
} // namespace robot_control
#endif // ROBOT_CONTROL__HPP_

89
HiveCoreR0/src/robot_control/include/robot_kinematics.hpp Normal file
View File

@@ -0,0 +1,89 @@
#ifndef ROBOT_MODEL__ROBOT_KINEMATICS_HPP_
#define ROBOT_MODEL__ROBOT_KINEMATICS_HPP_
#include "robot_model.hpp"
#include <Eigen/Dense>
#include <vector>
#include <memory>
namespace robot_model
{
class RobotKinematics
{
public:
/**
* @brief 构造函数
* @param robot_model 机器人模型指针
*/
explicit RobotKinematics(std::shared_ptr<RobotModel> robot_model);
/**
* @brief 析构函数
*/
~RobotKinematics() = default;
// 手臂运动学
/**
* @brief 计算手臂正运动学
* @param joint_angles 关节角度数组
* @return 末端执行器位置 (x, y, z)
*/
Eigen::Vector3d arm_forward_kinematics(const std::vector<double>& joint_angles);
/**
* @brief 计算手臂逆运动学
* @param target_pos 目标位置 (x, y, z)
* @param[out] joint_angles 计算得到的关节角度
* @return 成功返回true，失败返回false
*/
bool arm_inverse_kinematics(const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles);
// 腿部运动学
/**
* @brief 计算腿部正运动学
* @param leg_index 腿的索引 (0-3)
* @param joint_angles 关节角度数组
* @return 足部位置 (x, y, z)
*/
Eigen::Vector3d leg_forward_kinematics(size_t leg_index, const std::vector<double>& joint_angles);
/**
* @brief 计算腿部逆运动学
* @param leg_index 腿的索引 (0-3)
* @param target_pos 目标位置 (x, y, z)
* @param[out] joint_angles 计算得到的关节角度
* @return 成功返回true，失败返回false
*/
bool leg_inverse_kinematics(size_t leg_index, const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles);
private:
/**
* @brief 创建旋转矩阵 (绕Z轴)
* @param angle 旋转角度(弧度)
* @return 旋转矩阵
*/
Eigen::Matrix3d rotate_z(double angle);
/**
* @brief 创建旋转矩阵 (绕Y轴)
* @param angle 旋转角度(弧度)
* @return 旋转矩阵
*/
Eigen::Matrix3d rotate_y(double angle);
/**
* @brief 创建旋转矩阵 (绕X轴)
* @param angle 旋转角度(弧度)
* @return 旋转矩阵
*/
Eigen::Matrix3d rotate_x(double angle);
std::shared_ptr<RobotModel> robot_model_; // 机器人模型指针
};
} // namespace robot_model
#endif // ROBOT_MODEL__ROBOT_KINEMATICS_HPP_

114
HiveCoreR0/src/robot_control/include/robot_model.hpp Normal file
View File

@@ -0,0 +1,114 @@
#ifndef ROBOT_MODEL__ROBOT_MODEL_HPP_
#define ROBOT_MODEL__ROBOT_MODEL_HPP_
#include <vector>
#include <string>
#include <memory>
namespace robot_model
{
class UrdfParser;
// 关节结构体
struct Joint
{
std::string name; // 关节名称
double angle; // 关节角度（弧度）
double min_angle; // 最小角度限制
double max_angle; // 最大角度限制
double effort_limit; // 力/力矩限制
Joint(const std::string & joint_name, double min, double max, double effort)
: name(joint_name), angle(0.0), min_angle(min), max_angle(max), effort_limit(effort) {}
};
// 连杆结构体
struct Link
{
std::string name; // 连杆名称
double length; // 连杆长度
double mass; // 连杆质量
Link(const std::string & link_name, double len, double m)
: name(link_name), length(len), mass(m) {}
};
// 机械臂结构体
struct Arm
{
std::vector<std::shared_ptr<Joint>> joints; // 关节
std::vector<std::shared_ptr<Link>> links; // 连杆
std::string side; // 机械臂位置（如"left", "right"）
};
// 腿部结构体
struct Leg
{
std::vector<std::shared_ptr<Joint>> joints; // 关节
std::vector<std::shared_ptr<Link>> links; // 连杆
std::string side; // 腿部位置（如"front_left", "front_right", "rear_left", "rear_right"）
};
class RobotModel
{
// 声明UrdfParser为友元类，使其可以访问私有成员
friend class UrdfParser;
public:
/**
* @brief 构造函数
*/
RobotModel();
/**
* @brief 析构函数
*/
~RobotModel() = default;
/**
* @brief 获取机械臂
* @return 机械臂对象
*/
const Arm & get_arm(size_t index) const;
/**
* @brief 获取腿部
* @param index 腿的索引（0-3）
* @return 腿部对象
*/
const Leg & get_leg(size_t index) const;
/**
* @brief 设置关节角度
* @param joint_name 关节名称
* @param angle 目标角度（弧度）
* @return 成功返回true，失败返回false
*/
bool set_joint_angle(const std::string & joint_name, double angle);
/**
* @brief 获取关节角度
* @param joint_name 关节名称
* @return 关节角度（弧度），未找到返回0.0
*/
double get_joint_angle(const std::string & joint_name) const;
/**
* @brief 打印机器人模型信息
*/
void print_model_info() const;
private:
/**
* @brief 初始化机器人模型
*/
void initialize_model();
std::vector<Arm> arms_; // 机械臂集合 （2个机械臂）
std::vector<Leg> legs_; // 腿部集合（4条腿）
};
} // namespace robot_model
#endif // ROBOT_MODEL__ROBOT_MODEL_HPP_

78
HiveCoreR0/src/robot_control/include/urdf_parser.hpp Normal file
View File

@@ -0,0 +1,78 @@
#ifndef ROBOT_MODEL__URDF_PARSER_HPP_
#define ROBOT_MODEL__URDF_PARSER_HPP_
#include "robot_model.hpp"
#include <string>
#include <memory>
#include <urdf/model.h>
namespace robot_model
{
class UrdfParser
{
public:
/**
* @brief 构造函数
*/
UrdfParser() = default;
/**
* @brief 析构函数
*/
~UrdfParser() = default;
/**
* @brief 从文件解析URDF并构建RobotModel
* @param urdf_file_path URDF文件路径
* @return 构建的RobotModel共享指针，失败返回nullptr
*/
std::shared_ptr<RobotModel> parse_from_file(const std::string& urdf_file_path);
/**
* @brief 从XML字符串解析URDF并构建RobotModel
* @param urdf_xml URDF XML字符串
* @return 构建的RobotModel共享指针，失败返回nullptr
*/
std::shared_ptr<RobotModel> parse_from_string(const std::string& urdf_xml);
private:
/**
* @brief 解析URDF模型并构建RobotModel
* @param model URDF模型
* @return 构建的RobotModel共享指针
*/
std::shared_ptr<RobotModel> build_robot_model(const urdf::Model& model);
/**
* @brief 解析连杆信息
* @param link URDF连杆
* @return 构建的Link共享指针
*/
std::shared_ptr<Link> parse_link(const urdf::LinkConstSharedPtr& link);
/**
* @brief 解析关节信息
* @param joint URDF关节
* @return 构建的Joint共享指针
*/
std::shared_ptr<Joint> parse_joint(const urdf::JointConstSharedPtr& joint);
/**
* @brief 识别关节所属部分（手臂、腿部等）
* @param joint_name 关节名称
* @return 关节所属部分的字符串标识
*/
std::string identify_joint_part(const std::string& joint_name);
/**
* @brief 识别腿部位置（前左、前右、后左、后右）
* @param joint_name 关节名称
* @return 腿部位置字符串
*/
std::string identify_leg_side(const std::string& joint_name);
};
} // namespace robot_model
#endif // ROBOT_MODEL__URDF_PARSER_HPP_

22
HiveCoreR0/src/robot_control/launch/robot_control.launch.py Normal file
View File

@@ -0,0 +1,22 @@
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import TimerAction
def generate_launch_description():
# 创建节点启动描述
robot_control_node = Node(
package='robot_control', # 功能包名称
executable='robot_control_node', # 可执行文件名称
name='robot_control_node', # 节点名称（可自定义）
output='screen', # 输出到屏幕
)
start_robot_control_node = TimerAction(
period=1.0,
actions=[robot_control_node],
)
# 组装launch描述
return LaunchDescription([
start_robot_control_node
])

18
HiveCoreR0/src/robot_control/package.xml Normal file
View File

@@ -0,0 +1,18 @@
<?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>robot_control</name>
<version>1.0.0</version>
<description>Robot control package</description>
<maintainer email="Ray@email.com">Your Name</maintainer>
<license>Apache-2.0</license>
<build_depend>ament_cmake</build_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

62
HiveCoreR0/src/robot_control/src/arm_control.cpp Normal file
View File

@@ -0,0 +1,62 @@
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include <cmath>
#include <chrono>
#include "arm_control.hpp"
using namespace std::chrono_literals;
ArmControl::ArmControl(int total_joints)
{
// 初始化成员变量
total_joints_ = total_joints;
// 初始化正弦运动参数
amplitude_deg_ = 0.5; // 振幅（弧度）
frequency_ = 0.5; // 频率（Hz）
// 初始化成员数组
joint_position_.resize(total_joints_, 0.0);
joint_velocity_.resize(total_joints_, 0.0);
joint_acceleration_.resize(total_joints_, 0.0);
joint_torque_.resize(total_joints_, 0.0);
joint_position_desired_.resize(total_joints_, 0.0);
// 初始化正弦运动参数
amplitude_rad_ = amplitude_deg_ * M_PI / 180.0; // 将角度转换为弧度
}
ArmControl::~ArmControl()
{
// 析构函数
}
void ArmControl::DefaultArmMovement(std::vector<double>& joint_position_desired, double duration)
{
double sine_pos = amplitude_rad_ * std::sin(2 * M_PI * frequency_ * duration);
for (size_t i = 0; i < total_joints_; i++)
{
joint_position_desired_[i] = sine_pos;
joint_position_desired = joint_position_desired_;
}
}
void ArmControl::MoveToZeroPoint(double duration)
{
std::vector<double> joint_position_desired(total_joints_, 0.0);
MoveToTargetPoint(joint_position_desired, duration);
}
void ArmControl::MoveToTargetPoint(const Eigen::Vector3d& target_pos, double duration)
{
// 计算每个关节的目标位置
for (size_t i = 0; i < total_joints_; i++)
{
joint_position_desired_[i] = joint_position_desired[i];
}
// 计算每个关节的所需时间
}

139
HiveCoreR0/src/robot_control/src/leg_control.cpp Normal file
View File

@@ -0,0 +1,139 @@
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include <cmath>
class MotionController : public rclcpp::Node
{
public:
MotionController() : Node("motion_controller")
{
// 声明参数
this->declare_parameter<double>("wheel_radius", 0.05);
this->declare_parameter<double>("wheel_separation", 0.2);
// 获取参数
this->get_parameter("wheel_radius", wheel_radius_);
this->get_parameter("wheel_separation", wheel_separation_);
// 创建发布者和订阅者
cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
"/odom", 10,
std::bind(&MotionController::odom_callback, this, std::placeholders::_1)
);
// 创建定时器用于控制循环
control_timer_ = this->create_wall_timer(
std::chrono::milliseconds(100),
std::bind(&MotionController::control_loop, this)
);
RCLCPP_INFO(this->get_logger(), "Motion controller initialized");
}
private:
void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
// 保存当前里程计信息
current_pose_ = msg->pose.pose;
current_twist_ = msg->twist.twist;
}
void control_loop()
{
// 这里可以实现更复杂的控制算法
// 简单示例：如果设置了目标位置，则向目标位置移动
if (goal_set_)
{
// 计算当前位置与目标位置的差异
double dx = goal_x_ - current_pose_.position.x;
double dy = goal_y_ - current_pose_.position.y;
double distance = std::sqrt(dx*dx + dy*dy);
// 计算目标方向
double target_yaw = std::atan2(dy, dx);
// 从四元数获取当前偏航角
double current_yaw = get_yaw_from_quaternion(current_pose_.orientation);
// 计算角度差
double angle_diff = target_yaw - current_yaw;
// 归一化到[-π, π]
angle_diff = std::atan2(std::sin(angle_diff), std::cos(angle_diff));
// 控制参数
const double angle_tolerance = 0.1; // 弧度
const double distance_tolerance = 0.05; // 米
geometry_msgs::msg::Twist cmd_vel;
// 如果到达目标位置，停止
if (distance < distance_tolerance)
{
cmd_vel.linear.x = 0.0;
cmd_vel.angular.z = 0.0;
goal_set_ = false;
RCLCPP_INFO(this->get_logger(), "Reached goal position");
}
// 否则先旋转到目标方向
else if (std::abs(angle_diff) > angle_tolerance)
{
cmd_vel.linear.x = 0.0;
cmd_vel.angular.z = std::clamp(angle_diff, -0.5, 0.5); // 限制最大角速度
}
// 然后前进
else
{
cmd_vel.linear.x = std::clamp(distance, 0.0, 0.3); // 限制最大线速度
cmd_vel.angular.z = 0.0;
}
cmd_vel_pub_->publish(cmd_vel);
}
}
// 从四元数计算偏航角
double get_yaw_from_quaternion(const geometry_msgs::msg::Quaternion& quat)
{
double w = quat.w;
double x = quat.x;
double y = quat.y;
double z = quat.z;
return std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
}
// 设置目标位置（可以通过服务或动作接口实现）
void set_goal(double x, double y)
{
goal_x_ = x;
goal_y_ = y;
goal_set_ = true;
RCLCPP_INFO(this->get_logger(), "New goal set: (%f, %f)", x, y);
}
// 成员变量
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
rclcpp::TimerBase::SharedPtr control_timer_;
geometry_msgs::msg::Pose current_pose_;
geometry_msgs::msg::Twist current_twist_;
double wheel_radius_;
double wheel_separation_;
double goal_x_ = 0.0;
double goal_y_ = 0.0;
bool goal_set_ = false;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MotionController>());
rclcpp::shutdown();
return 0;
}

176
HiveCoreR0/src/hive_core_r0/src/hiveBot1_control_node.cpp → HiveCoreR0/src/robot_control/src/robot_control.cpp
View File

@@ -1,46 +1,16 @@
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "robot_control.hpp"
#include <cmath>
#include <chrono>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
using namespace std::chrono_literals;
// 键盘输入处理函数（非阻塞模式）
int kbhit()
namespace robot_control
{
struct termios oldt, newt;
int ch;
int oldf;
using namespace std::chrono_literals;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
fcntl(STDIN_FILENO, F_SETFL, oldf);
if(ch != EOF)
{
ungetc(ch, stdin);
return 1;
}
return 0;
}
class RobotPhaseMotionNode : public rclcpp::Node
{
public:
RobotPhaseMotionNode() : Node("robot_phase_motion_node")
RobotControlNode::RobotControlNode(): Node("robot_phase_motion_node")
{
// 声明并获取参数
this->declare_parameter("leg_target_deg", 0.0); // 腿部目标角度（度）
@@ -80,62 +50,51 @@ public:
// 创建发布者
cmd_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
"/bot1_position_controller/commands", 10
"/robot_position_controller/commands", 10
);
// 创建订阅者（接收来自速度指令）
twist_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel", 10,
std::bind(&RobotPhaseMotionNode::twist_callback, this, std::placeholders::_1)
std::bind(&RobotControlNode::twist_callback, this, std::placeholders::_1)
);
// 创建定时器
timer_ = this->create_wall_timer(
std::chrono::milliseconds(period_ms),
std::bind(&RobotPhaseMotionNode::timer_callback, this)
std::bind(&RobotControlNode::timer_callback, this)
);
// 记录起始时间
start_time_ = this->now();
last_time_ = this->now();
// 打印控制说明
print_help();
RCLCPP_INFO(this->get_logger(), "机器人分阶段运动节点启动（含轮子控制）");
}
// 关节复位
void resetJoints()
RobotControlNode::~RobotControlNode()
{
// 关闭节点时复位关节
resetJoints();
}
// 关节复位
void RobotControlNode::resetJoints()
{
auto reset_msg = std_msgs::msg::Float64MultiArray();
reset_msg.data.resize(total_joints_, 0.0);
cmd_pub_->publish(reset_msg);
}
private:
void print_help()
{
RCLCPP_INFO(this->get_logger(), "键盘控制说明:");
RCLCPP_INFO(this->get_logger(), " w : 前进");
RCLCPP_INFO(this->get_logger(), " s : 后退");
RCLCPP_INFO(this->get_logger(), " a : 左转");
RCLCPP_INFO(this->get_logger(), " d : 右转");
RCLCPP_INFO(this->get_logger(), " x : 停止");
RCLCPP_INFO(this->get_logger(), " q : 退出");
}
void twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
void RobotControlNode::twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
// 接收外部速度指令
linear_x_ = msg->linear.x;
angular_z_ = msg->angular.z;
}
void timer_callback()
void RobotControlNode::timer_callback()
{
// 处理键盘输入
handle_keyboard_input();
// 计算时间差
rclcpp::Time current_time = this->now();
rclcpp::Duration dt = current_time - last_time_;
@@ -157,7 +116,7 @@ private:
cmd_pub_->publish(cmd_msg);
}
void handle_phase_motion(std_msgs::msg::Float64MultiArray& cmd_msg, double current_time)
void RobotControlNode::handle_phase_motion(std_msgs::msg::Float64MultiArray& cmd_msg, double current_time)
{
// 第一阶段：前5秒，腿部关节缓慢张开
if (current_time <= 5.0)
@@ -222,12 +181,12 @@ private:
cmd_msg.data[arm_joint_indices_[i]] = arm_angle;
}
cmd_msg.data[arm_joint_indices_[i]] = 0;
// cmd_msg.data[arm_joint_indices_[i]] = 0;
}
}
}
void handle_wheel_control(std_msgs::msg::Float64MultiArray& cmd_msg, double dt_sec)
void RobotControlNode::handle_wheel_control(std_msgs::msg::Float64MultiArray& cmd_msg, double dt_sec)
{
// 计算左右轮目标线速度
double left_speed = linear_x_ + (angular_z_ * wheel_separation_) / 2.0;
@@ -247,90 +206,15 @@ private:
cmd_msg.data[wheel_joint_indices_[i]] = wheel_joint_directions_[i] * wheel_targets_[i];
}
}
void handle_keyboard_input()
{
if(kbhit())
{
char key = getchar();
switch(key)
{
case 'w': // 前进
linear_x_ = linear_speed_;
angular_z_ = 0.0;
RCLCPP_INFO(this->get_logger(), "前进");
break;
case 's': // 后退
linear_x_ = -linear_speed_;
angular_z_ = 0.0;
RCLCPP_INFO(this->get_logger(), "后退");
break;
case 'a': // 左转
linear_x_ = 0.0;
angular_z_ = angular_speed_;
RCLCPP_INFO(this->get_logger(), "左转");
break;
case 'd': // 右转
linear_x_ = 0.0;
angular_z_ = -angular_speed_;
RCLCPP_INFO(this->get_logger(), "右转");
break;
case 'x': // 停止
linear_x_ = 0.0;
angular_z_ = 0.0;
RCLCPP_INFO(this->get_logger(), "停止");
break;
case 'q': // 退出
RCLCPP_INFO(this->get_logger(), "退出控制");
resetJoints();
rclcpp::shutdown();
exit(0);
break;
default:
print_help();
break;
}
}
}
// 成员变量
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr cmd_pub_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time start_time_;
rclcpp::Time last_time_;
// 运动参数
double leg_target_rad_; // 腿部目标角度（弧度）
double arm_amplitude_rad_; // 手臂摆动振幅（弧度）
double arm_frequency_; // 手臂摆动频率（Hz）
int total_joints_; // 总关节数
bool phase1_notified_ = false; // 第一阶段结束提示标记
// 关节索引
std::vector<int> leg_joint_indices_; // 腿部关节索引
std::vector<int> arm_joint_indices_; // 手臂关节索引
std::vector<int> wheel_joint_indices_; // 轮子关节索引
std::vector<int> joint_directions_; // 关节方向
std::vector<int> wheel_joint_directions_; // 轮子关节方向
// 轮子控制相关
double wheel_radius_; // 轮子半径（米）
double wheel_separation_; // 轮距（米）
std::vector<double> wheel_targets_; // 轮子目标角度（弧度）
double linear_x_ = 0.0; // 线速度（m/s）
double angular_z_ = 0.0; // 角速度（rad/s）
double linear_speed_; // 最大线速度（m/s）
double angular_speed_; // 最大角速度（rad/s）
};
}
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<RobotPhaseMotionNode>();
rclcpp::spin(node);
node->resetJoints();
RCLCPP_INFO(node->get_logger(), "节点退出，发送复位指令");
rclcpp::shutdown();
return 0;
}
{
rclcpp::init(argc, argv);
auto node = std::make_shared<robot_control::RobotControlNode>();
rclcpp::spin(node);
node->resetJoints();
RCLCPP_INFO(node->get_logger(), "节点退出，发送复位指令");
rclcpp::shutdown();
return 0;
}

215
HiveCoreR0/src/robot_control/src/robot_kinematics.cpp Normal file
View File

@@ -0,0 +1,215 @@
#include "robot_kinematics.hpp"
#include <cmath>
#include <stdexcept>
#include <iostream>
namespace robot_model
{
RobotKinematics::RobotKinematics(std::shared_ptr<RobotModel> robot_model)
: robot_model_(robot_model)
{
if (!robot_model_)
{
throw std::invalid_argument("Robot model pointer cannot be null");
}
}
Eigen::Matrix3d RobotKinematics::rotate_z(double angle)
{
Eigen::Matrix3d mat;
mat << std::cos(angle), -std::sin(angle), 0,
std::sin(angle), std::cos(angle), 0,
0, 0, 1;
return mat;
}
Eigen::Matrix3d RobotKinematics::rotate_y(double angle)
{
Eigen::Matrix3d mat;
mat << std::cos(angle), 0, std::sin(angle),
0, 1, 0,
-std::sin(angle), 0, std::cos(angle);
return mat;
}
Eigen::Matrix3d RobotKinematics::rotate_x(double angle)
{
Eigen::Matrix3d mat;
mat << 1, 0, 0,
0, std::cos(angle), -std::sin(angle),
0, std::sin(angle), std::cos(angle);
return mat;
}
Eigen::Vector3d RobotKinematics::arm_forward_kinematics(const std::vector<double>& joint_angles)
{
const auto& arm = robot_model_->get_arm();
// 检查关节角度数量是否正确
if (joint_angles.size() != arm.joints.size())
{
throw std::invalid_argument("Invalid number of joint angles for arm");
}
// 连杆长度
double l1 = arm.links[0]->length; // 上臂长度
double l2 = arm.links[1]->length; // 下臂长度
// 关节角度
double theta1 = joint_angles[0]; // 基座关节 (绕Z轴)
double theta2 = joint_angles[1]; // 肩部关节 (绕Y轴)
double theta3 = joint_angles[2]; // 肘部关节 (绕Y轴)
double theta4 = joint_angles[3]; // 腕部关节 (绕Y轴) - 简化模型中不影响位置
// 计算末端位置
Eigen::Vector3d end_pos;
// 正运动学公式 (简化的3DOF模型)
double x = (l1 * std::cos(theta2) + l2 * std::cos(theta2 + theta3)) * std::cos(theta1);
double y = (l1 * std::cos(theta2) + l2 * std::cos(theta2 + theta3)) * std::sin(theta1);
double z = l1 * std::sin(theta2) + l2 * std::sin(theta2 + theta3);
end_pos << x, y, z;
return end_pos;
}
bool RobotKinematics::arm_inverse_kinematics(const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles)
{
const auto& arm = robot_model_->get_arm();
joint_angles.resize(4, 0.0); // 4个关节
// 连杆长度
double l1 = arm.links[0]->length;
double l2 = arm.links[1]->length;
// 目标位置坐标
double x = target_pos.x();
double y = target_pos.y();
double z = target_pos.z();
// 计算手腕位置到基座的距离
double r = std::sqrt(x*x + y*y);
double d = std::sqrt(r*r + z*z);
// 检查工作空间
if (d > l1 + l2 || d < std::fabs(l1 - l2))
{
std::cerr << "Target position out of arm workspace" << std::endl;
return false;
}
// 计算关节角度 (逆运动学解)
// 基座关节角度
joint_angles[0] = std::atan2(y, x);
// 肩部和肘部关节角度 (使用余弦定理)
double cos_theta3 = (d*d - l1*l1 - l2*l2) / (2*l1*l2);
cos_theta3 = std::clamp(cos_theta3, -1.0, 1.0); // 防止数值误差导致的问题
joint_angles[2] = std::acos(cos_theta3);
double alpha = std::atan2(z, r);
double beta = std::asin((l2 * std::sin(joint_angles[2])) / d);
joint_angles[1] = alpha + beta;
// 腕部关节 (简化为0)
joint_angles[3] = 0.0;
// 检查关节角度限制
for (size_t i = 0; i < joint_angles.size(); ++i)
{
if (joint_angles[i] < arm.joints[i]->min_angle ||
joint_angles[i] > arm.joints[i]->max_angle)
{
std::cerr << "Joint " << i << " angle out of range" << std::endl;
return false;
}
}
return true;
}
Eigen::Vector3d RobotKinematics::leg_forward_kinematics(size_t leg_index, const std::vector<double>& joint_angles)
{
const auto& leg = robot_model_->get_leg(leg_index);
// 检查关节角度数量是否正确
if (joint_angles.size() != leg.joints.size())
{
throw std::invalid_argument("Invalid number of joint angles for leg");
}
// 连杆长度
double l1 = leg.links[0]->length; // 大腿长度
double l2 = leg.links[1]->length; // 小腿长度
// 关节角度
double theta1 = joint_angles[0]; // 髋关节
double theta2 = joint_angles[1]; // 膝关节
// 根据腿部位置调整坐标系
double sign = (leg.side.find("right") != std::string::npos) ? -1.0 : 1.0;
// 计算足部位置
Eigen::Vector3d foot_pos;
// 正运动学计算
foot_pos.x() = sign * (l1 * std::cos(theta1) + l2 * std::cos(theta1 + theta2));
foot_pos.y() = 0.0; // 简化模型
foot_pos.z() = -(l1 * std::sin(theta1) + l2 * std::sin(theta1 + theta2)); // Z轴向下为正
return foot_pos;
}
bool RobotKinematics::leg_inverse_kinematics(size_t leg_index, const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles)
{
const auto& leg = robot_model_->get_leg(leg_index);
joint_angles.resize(2, 0.0); // 每条腿2个关节
// 连杆长度
double l1 = leg.links[0]->length; // 大腿长度
double l2 = leg.links[1]->length; // 小腿长度
// 根据腿部位置调整坐标系
double sign = (leg.side.find("right") != std::string::npos) ? -1.0 : 1.0;
// 目标位置坐标 (调整符号)
double x = sign * target_pos.x();
double z = -target_pos.z(); // 转换为腿部坐标系
// 计算髋关节到目标点的距离
double d = std::sqrt(x*x + z*z);
// 检查工作空间
if (d > l1 + l2 || d < std::fabs(l1 - l2))
{
std::cerr << "Target position out of leg workspace" << std::endl;
return false;
}
// 计算关节角度 (逆运动学解)
double cos_theta2 = (d*d - l1*l1 - l2*l2) / (2*l1*l2);
cos_theta2 = std::clamp(cos_theta2, -1.0, 1.0); // 防止数值误差导致的问题
joint_angles[1] = std::acos(cos_theta2);
double alpha = std::atan2(z, x);
double beta = std::asin((l2 * std::sin(joint_angles[1])) / d);
joint_angles[0] = alpha - beta;
// 检查关节角度限制
for (size_t i = 0; i < joint_angles.size(); ++i)
{
if (joint_angles[i] < leg.joints[i]->min_angle ||
joint_angles[i] > leg.joints[i]->max_angle)
{
std::cerr << "Leg joint " << i << " angle out of range: "
<< joint_angles[i] << " (min: " << leg.joints[i]->min_angle
<< ", max: " << leg.joints[i]->max_angle << ")" << std::endl;
return false;
}
}
return true;
}
} // namespace robot_model

186
HiveCoreR0/src/robot_control/src/robot_model.cpp Normal file
View File

@@ -0,0 +1,186 @@
#include "robot_model.hpp"
#include <iostream>
#include <cmath>
namespace robot_model
{
RobotModel::RobotModel()
{
initialize_model();
}
void RobotModel::initialize_model()
{
// 初始化机械臂（4个关节，2个连杆）
// 添加关节
arm_.joints.push_back(std::make_shared<Joint>("arm_base_joint", -M_PI/2, M_PI/2, 10.0));
arm_.joints.push_back(std::make_shared<Joint>("arm_shoulder_joint", -M_PI/2, M_PI/2, 8.0));
arm_.joints.push_back(std::make_shared<Joint>("arm_elbow_joint", 0, M_PI/2, 6.0));
arm_.joints.push_back(std::make_shared<Joint>("arm_wrist_joint", -M_PI/4, M_PI/4, 4.0));
// 添加连杆
arm_.links.push_back(std::make_shared<Link>("arm_upper_link", 0.3, 1.5));
arm_.links.push_back(std::make_shared<Link>("arm_lower_link", 0.25, 1.0));
// 初始化腿部（4条腿，每条腿2个连杆）
std::vector<std::string> leg_sides = {"front_left", "front_right", "rear_left", "rear_right"};
for (const auto & side : leg_sides)
{
Leg leg;
leg.side = side;
// 为每条腿添加关节
leg.joints.push_back(std::make_shared<Joint>(side + "_hip_joint", -M_PI/4, M_PI/4, 15.0));
leg.joints.push_back(std::make_shared<Joint>(side + "_knee_joint", 0, M_PI/2, 12.0));
// 为每条腿添加连杆
leg.links.push_back(std::make_shared<Link>(side + "_thigh_link", 0.2, 2.0));
leg.links.push_back(std::make_shared<Link>(side + "_shin_link", 0.2, 1.5));
legs_.push_back(leg);
}
}
const Arm & RobotModel::get_arm(size_t index) const
{
if (index >= arms_.size())
{
throw std::out_of_range("Arm index out of range");
}
return arms_[index];
}
const Leg & RobotModel::get_leg(size_t index) const
{
if (index >= legs_.size())
{
throw std::out_of_range("Leg index out of range");
}
return legs_[index];
}
bool RobotModel::set_joint_angle(const std::string & joint_name, double angle)
{
// 检查机械臂关节
for (auto & arm : arms_)
{
for (const auto & joint : arm.joints)
{
if (joint->name == joint_name)
{
// 检查角度限制
if (angle >= joint->min_angle && angle <= joint->max_angle)
{
joint->angle = angle;
return true;
}
return false;
}
}
}
// 检查腿部关节
for (auto & leg : legs_)
{
for (const auto & joint : leg.joints)
{
if (joint->name == joint_name)
{
// 检查角度限制
if (angle >= joint->min_angle && angle <= joint->max_angle)
{
joint->angle = angle;
return true;
}
return false;
}
}
}
// 未找到关节
return false;
}
double RobotModel::get_joint_angle(const std::string & joint_name) const
{
// 检查机械臂关节
for (auto & arm : arms_)
{
for (const auto & joint : arm.joints)
{
if (joint->name == joint_name)
{
return joint->angle;
}
}
}
// 检查腿部关节
for (const auto & leg : legs_)
{
for (const auto & joint : leg.joints)
{
if (joint->name == joint_name)
{
return joint->angle;
}
}
}
// 未找到关节
return 0.0;
}
void RobotModel::print_model_info() const
{
std::cout << "=== Robot Model Information ===" << std::endl;
// 打印机械臂信息
std::cout << "\nArm:" << std::endl;
std::cout << " Joints:" << std::endl;
for (size_t i = 0; i < arms_.size(); ++i)
{
const auto & arm = arms_[i];
std::cout << " Leg " << i << " (" << arm.side << "):" << std::endl;
std::cout << " Joints:" << std::endl;
for (const auto & joint : arm.joints)
{
std::cout << " " << joint->name << ": "
<< "Angle=" << joint->angle << " rad, "
<< "Range=[" << joint->min_angle << ", " << joint->max_angle << "] rad" << std::endl;
}
std::cout << " Links:" << std::endl;
for (const auto & link : arm.links)
{
std::cout << " " << link->name << ": "
<< "Length=" << link->length << " m, "
<< "Mass=" << link->mass << " kg" << std::endl;
}
}
// 打印腿部信息
std::cout << "\nLegs:" << std::endl;
for (size_t i = 0; i < legs_.size(); ++i)
{
const auto & leg = legs_[i];
std::cout << " Leg " << i << " (" << leg.side << "):" << std::endl;
std::cout << " Joints:" << std::endl;
for (const auto & joint : leg.joints)
{
std::cout << " " << joint->name << ": "
<< "Angle=" << joint->angle << " rad, "
<< "Range=[" << joint->min_angle << ", " << joint->max_angle << "] rad" << std::endl;
}
std::cout << " Links:" << std::endl;
for (const auto & link : leg.links)
{
std::cout << " " << link->name << ": "
<< "Length=" << link->length << " m, "
<< "Mass=" << link->mass << " kg" << std::endl;
}
}
}
} // namespace robot_model

249
HiveCoreR0/src/robot_control/src/urdf_parser.cpp Normal file
View File

@@ -0,0 +1,249 @@
#include "urdf_parser.hpp"
#include <fstream>
#include <sstream>
#include <algorithm>
#include <stdexcept>
#include <iostream>
namespace robot_model
{
std::shared_ptr<RobotModel> UrdfParser::parse_from_file(const std::string& urdf_file_path)
{
// 读取URDF文件内容
std::ifstream file(urdf_file_path);
if (!file.is_open())
{
std::cerr << "Failed to open URDF file: " << urdf_file_path << std::endl;
return nullptr;
}
std::stringstream ss;
ss << file.rdbuf();
std::string urdf_xml = ss.str();
file.close();
return parse_from_string(urdf_xml);
}
std::shared_ptr<RobotModel> UrdfParser::parse_from_string(const std::string& urdf_xml)
{
urdf::Model model;
if (!model.initString(urdf_xml))
{
std::cerr << "Failed to parse URDF XML string" << std::endl;
return nullptr;
}
return build_robot_model(model);
}
std::shared_ptr<RobotModel> UrdfParser::build_robot_model(const urdf::Model& model)
{
auto robot_model = std::make_shared<RobotModel>();
// 我们需要访问RobotModel的私有成员来构建模型
// 这里假设RobotModel类有一个友元类UrdfParser，或者提供了构建接口
// 为了演示，我们直接修改RobotModel的私有成员（实际项目中应使用适当的接口）
// 遍历所有关节和连杆
for (const auto& joint_pair : model.joints_)
{
const auto& joint = joint_pair.second;
auto parsed_joint = parse_joint(joint);
if (!parsed_joint)
continue;
// 确定关节属于哪个部分
std::string part = identify_joint_part(joint->name);
// 手臂关节
if (part == "arm")
{
robot_model->arm_.joints.push_back(parsed_joint);
}
// 腿部关节
else if (part == "leg")
{
std::string leg_side = identify_leg_side(joint->name);
// 查找或创建对应的腿
bool leg_found = false;
for (auto& leg : robot_model->legs_)
{
if (leg.side == leg_side)
{
leg.joints.push_back(parsed_joint);
leg_found = true;
break;
}
}
// 如果没有找到对应的腿，则创建新的
if (!leg_found)
{
Leg new_leg;
new_leg.side = leg_side;
new_leg.joints.push_back(parsed_joint);
robot_model->legs_.push_back(new_leg);
}
}
}
// 解析连杆
for (const auto& link_pair : model.links_)
{
const auto& link = link_pair.second;
auto parsed_link = parse_link(link);
if (!parsed_link)
continue;
// 确定连杆属于哪个部分
std::string part = identify_joint_part(link->name);
// 手臂连杆
if (part == "arm")
{
robot_model->arm_.links.push_back(parsed_link);
}
// 腿部连杆
else if (part == "leg")
{
std::string leg_side = identify_leg_side(link->name);
// 查找对应的腿
for (auto& leg : robot_model->legs_)
{
if (leg.side == leg_side)
{
leg.links.push_back(parsed_link);
break;
}
}
}
}
// 确保腿部数量为4
if (robot_model->legs_.size() != 4)
{
std::cerr << "Warning: Expected 4 legs, but found " << robot_model->legs_.size() << std::endl;
}
return robot_model;
}
std::shared_ptr<Link> UrdfParser::parse_link(const urdf::LinkConstSharedPtr& link)
{
if (!link)
return nullptr;
// 获取连杆长度（简化处理，假设是盒子形状）
double length = 0.1; // 默认长度
if (link->collision && link->collision->geometry &&
link->collision->geometry->type == urdf::Geometry::BOX)
{
auto box = std::dynamic_pointer_cast<urdf::Box>(link->collision->geometry);
if (box)
{
// 假设X轴为长度方向
length = box->dim.x;
}
}
// 获取质量
double mass = 1.0; // 默认质量
if (link->inertial)
{
mass = link->inertial->mass;
}
return std::make_shared<Link>(link->name, length, mass);
}
std::shared_ptr<Joint> UrdfParser::parse_joint(const urdf::JointConstSharedPtr& joint)
{
if (!joint)
return nullptr;
// 只处理旋转关节
if (joint->type != urdf::Joint::REVOLUTE && joint->type != urdf::Joint::CONTINUOUS)
return nullptr;
// 获取关节角度限制
double min_angle = -M_PI;
double max_angle = M_PI;
if (joint->type == urdf::Joint::REVOLUTE)
{
min_angle = joint->limits->lower;
max_angle = joint->limits->upper;
}
// 获取力限制
double effort_limit = 100.0; // 默认力限制
if (joint->limits)
{
effort_limit = joint->limits->effort;
}
return std::make_shared<Joint>(joint->name, min_angle, max_angle, effort_limit);
}
std::string UrdfParser::identify_joint_part(const std::string& name)
{
// 转换为小写以便不区分大小写匹配
std::string lower_name = name;
std::transform(lower_name.begin(), lower_name.end(), lower_name.begin(), ::tolower);
if (lower_name.find("arm") != std::string::npos ||
lower_name.find("shoulder") != std::string::npos ||
lower_name.find("elbow") != std::string::npos ||
lower_name.find("wrist") != std::string::npos)
{
return "arm";
}
else if (lower_name.find("leg") != std::string::npos ||
lower_name.find("hip") != std::string::npos ||
lower_name.find("knee") != std::string::npos ||
lower_name.find("thigh") != std::string::npos ||
lower_name.find("shin") != std::string::npos)
{
return "leg";
}
return "unknown";
}
std::string UrdfParser::identify_leg_side(const std::string& name)
{
// 转换为小写以便不区分大小写匹配
std::string lower_name = name;
std::transform(lower_name.begin(), lower_name.end(), lower_name.begin(), ::tolower);
bool is_front = (lower_name.find("front") != std::string::npos ||
lower_name.find("fore") != std::string::npos);
bool is_rear = (lower_name.find("rear") != std::string::npos ||
lower_name.find("hind") != std::string::npos);
bool is_left = (lower_name.find("left") != std::string::npos);
bool is_right = (lower_name.find("right") != std::string::npos);
std::string position;
if (is_front)
position = "front_";
else if (is_rear)
position = "rear_";
else
position = "unknown_";
std::string side;
if (is_left)
side = "left";
else if (is_right)
side = "right";
else
side = "unknown";
return position + side;
}
} // namespace robot_model

18
HiveCoreR0/src/robot_description/CMakeLists.txt Normal file
View File

@@ -0,0 +1,18 @@
cmake_minimum_required(VERSION 3.10)
project(robot_description)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# 依赖
find_package(ament_cmake REQUIRED)
find_package(urdf REQUIRED)
find_package(xacro REQUIRED)
# 安装模型文件
install(DIRECTORY urdf meshes config
DESTINATION share/${PROJECT_NAME}/
)
ament_package()

202
HiveCoreR0/src/robot_description/LICENSE Normal file
View File

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

6
HiveCoreR0/src/hive_core_r0/config/bot1_ros2_controller.yaml → HiveCoreR0/src/robot_description/config/robot_ros2_controller.yaml
View File

@@ -4,16 +4,16 @@ controller_manager:
use_sim_time: true
# 关节状态广播器 - 必须配置，用于发布关节状态
bot1_joint_state_broadcaster:
robot_joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
use_sim_time: true
# 关节位置控制器
bot1_position_controller:
robot_position_controller:
type: position_controllers/JointGroupPositionController
# 腿部和手臂关节位置控制器配置
bot1_position_controller:
robot_position_controller:
ros__parameters:
joints:
- left_leg1_joint

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/base_link.STL → HiveCoreR0/src/robot_description/meshes/base_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_arm1_link.STL → HiveCoreR0/src/robot_description/meshes/left_arm1_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_arm2_link.STL → HiveCoreR0/src/robot_description/meshes/left_arm2_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_leg1_link.STL → HiveCoreR0/src/robot_description/meshes/left_leg1_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_leg2_link.STL → HiveCoreR0/src/robot_description/meshes/left_leg2_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_shoulder_link.STL → HiveCoreR0/src/robot_description/meshes/left_shoulder_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_wheel1_link.STL → HiveCoreR0/src/robot_description/meshes/left_wheel1_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_wheel2_link.STL → HiveCoreR0/src/robot_description/meshes/left_wheel2_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/pitch_body_link.STL → HiveCoreR0/src/robot_description/meshes/pitch_body_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_arm1_link.STL → HiveCoreR0/src/robot_description/meshes/right_arm1_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_arm2_link.STL → HiveCoreR0/src/robot_description/meshes/right_arm2_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_leg1_link.STL → HiveCoreR0/src/robot_description/meshes/right_leg1_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_leg2_link.STL → HiveCoreR0/src/robot_description/meshes/right_leg2_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_shoulder_link.STL → HiveCoreR0/src/robot_description/meshes/right_shoulder_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_wheel1_link.STL → HiveCoreR0/src/robot_description/meshes/right_wheel1_link.STL
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_wheel2_link.STL → HiveCoreR0/src/robot_description/meshes/right_wheel2_link.STL
View File

15
HiveCoreR0/src/hive_core_r0/package.xml → HiveCoreR0/src/robot_description/package.xml
View File

@@ -1,17 +1,16 @@
<?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>hive_core_r0</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="ray@hivecore.cn">hivecore</maintainer>
<name>robot_description</name>
<version>1.0.0</version>
<description>Robot model description package</description>
<maintainer email="Ray@email.com">Your Name</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<depend>urdf</depend>
<depend>xacro</depend>
<export>
<build_type>ament_cmake</build_type>
</export>

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/generic_joint.xacro → HiveCoreR0/src/robot_description/urdf/generic_joint.xacro
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/generic_link.xacro → HiveCoreR0/src/robot_description/urdf/generic_link.xacro
View File

0
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/hive_core_bot1.urdf → HiveCoreR0/src/robot_description/urdf/robot.urdf
View File

12
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/hive_core_bot1.urdf.xacro → HiveCoreR0/src/robot_description/urdf/robot.urdf.xacro
View File

@@ -1,10 +1,10 @@
<?xml version="1.0" encoding="utf-8"?>
<robot name="hive_core_bot1" xmlns:xacro="http://www.ros.org/wiki/xacro">
<robot name="hive_core_bot0" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 引入XACRO命名空间 -->
<xacro:property name="meshes_path" value="file://$(find hive_core_r0)/urdf/hiveBot1/meshes" />
<xacro:property name="meshes_path" value="file://$(find robot_description)/meshes" />
<xacro:include filename="$(find hive_core_r0)/urdf/hiveBot1/generic_link.xacro" />
<xacro:include filename="$(find hive_core_r0)/urdf/hiveBot1/generic_joint.xacro" />
<xacro:include filename="$(find robot_description)/urdf/generic_link.xacro" />
<xacro:include filename="$(find robot_description)/urdf/generic_joint.xacro" />
<!-- 基础链接 -->
<xacro:generic_link
@@ -517,7 +517,7 @@
<child link="base_link" />
</joint> -->
<xacro:include filename="$(find hive_core_r0)/urdf/hiveBot1/bot1_ros2_control.xacro" />
<xacro:bot1_ros2_control />
<xacro:include filename="$(find robot_description)/urdf/robot_ros2_control.xacro" />
<xacro:robot_ros2_control />
</robot>

6
HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/bot1_ros2_control.xacro → HiveCoreR0/src/robot_description/urdf/robot_ros2_control.xacro
View File

@@ -1,9 +1,9 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 宏定义：ros2_control硬件接口配置（适配位置控制器和轮子速度控制器） -->
<xacro:macro name="bot1_ros2_control">
<xacro:macro name="robot_ros2_control">
<!-- 1. ros2_control系统配置：定义所有关节的控制接口 -->
<ros2_control name="Bot1GazeboSystem" type="system">
<ros2_control name="RobotGazeboSystem" type="system">
<!-- 硬件插件：关联Gazebo仿真 -->
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
@@ -110,7 +110,7 @@
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<!-- 加载控制器配置文件（路径需替换为你的实际路径） -->
<parameters>$(find hive_core_r0)/config/bot1_ros2_controller.yaml</parameters>
<parameters>$(find robot_description)/config/robot_ros2_controller.yaml</parameters>
</plugin>
</gazebo>
</xacro:macro>

16
HiveCoreR0/src/robot_simulation/CMakeLists.txt Normal file
View File

@@ -0,0 +1,16 @@
cmake_minimum_required(VERSION 3.10)
project(robot_simulation)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# 依赖
find_package(ament_cmake REQUIRED)
# 安装启动文件和世界文件
install(DIRECTORY launch world
DESTINATION share/${PROJECT_NAME}/
)
ament_package()

202
HiveCoreR0/src/robot_simulation/LICENSE Normal file
View File

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

55
HiveCoreR0/src/hive_core_r0/launch/gazebo_sim_bot1.launch.py → HiveCoreR0/src/robot_simulation/launch/gazebo_simulation.launch.py
View File

@@ -2,28 +2,27 @@ import os
import launch
import launch_ros
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler
from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution, FindExecutable
from launch.actions import ExecuteProcess
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.event_handlers import OnProcessStart
from launch.actions import TimerAction
def generate_launch_description():
# 声明参数
robot_name_in_model = "HiveBot0"
urdf_package = 'hive_core_r0'
urdf_tutorial_path = get_package_share_directory(urdf_package)
default_model_path = urdf_tutorial_path + '/urdf/hiveBot1/hive_core_bot1.urdf.xacro'
default_world_path = os.path.join(urdf_tutorial_path, 'world', 'empty.world')
robot_name_in_model = "hive_core_bot0"
robot_description_package = 'robot_description'
robot_description_path = get_package_share_directory(robot_description_package)
robot_model_path = os.path.join(robot_description_path,'urdf','robot.urdf.xacro')
robot_simulation_package = 'robot_simulation'
robot_simulation_path = get_package_share_directory(robot_simulation_package)
default_world_path = os.path.join(robot_simulation_path, 'world', 'empty.world')
# 声明模型路径参数
action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
name='model', default_value=str(default_model_path),
name='model', default_value=str(robot_model_path),
description='URDF 的绝对路径')
# 获取文件内容生成新的参数
@@ -71,7 +70,7 @@ def generate_launch_description():
executable="ros2_control_node",
parameters=[
{'robot_description': robot_description},
os.path.join(urdf_tutorial_path, 'config', 'bot1_ros2_controller.yaml') # 显式指定配置文件
os.path.join(robot_description_path, 'config', 'bot1_ros2_controller.yaml') # 显式指定配置文件
],
output="screen"
)
@@ -81,7 +80,7 @@ def generate_launch_description():
period=3.0, # 延迟3秒，等待初始化
actions=[ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'bot1_joint_state_broadcaster'],
'robot_joint_state_broadcaster'],
output='screen'
)]
)
@@ -91,36 +90,10 @@ def generate_launch_description():
period=2.0, # 延迟2秒
actions=[ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'bot1_position_controller'],
'robot_position_controller'],
output='screen'
)]
)
action_node_hiveBot1_control = launch_ros.actions.Node(
package='hive_core_r0',
executable='hiveBot1_control_node',
name='hiveBot1_control_node',
output='screen'
)
# 4. 延迟启动控制节点（在位置控制器加载后）
start_control_node = TimerAction(
period=2.0,
actions=[action_node_hiveBot1_control]
)
keyboard_control_node = launch_ros.actions.Node(
package='hive_core_r0',
executable='keyboard_control_node',
name='keyboard_control_node',
output='screen'
)
# 5. 延迟启动键盘控制节点（在控制节点启动后）
start_keyboard_control_node = TimerAction(
period=2.0,
actions=[keyboard_control_node]
)
# 在LaunchDescription中按顺序添加
return LaunchDescription([
@@ -135,6 +108,4 @@ def generate_launch_description():
controller_manager_node, # 显式启动控制器管理器
load_joint_state_controller,
load_bot1_position_controller,
start_control_node,
# start_keyboard_control_node,
])

15
HiveCoreR0/src/robot_simulation/package.xml Normal file
View File

@@ -0,0 +1,15 @@
<?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>robot_simulation</name>
<version>1.0.0</version>
<description>Robot simulation package</description>
<maintainer email="Ray@email.com">Your Name</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

0
HiveCoreR0/src/hive_core_r0/world/custom_room.world → HiveCoreR0/src/robot_simulation/world/custom_room.world
View File

0
HiveCoreR0/src/hive_core_r0/world/empty.world → HiveCoreR0/src/robot_simulation/world/empty.world
View File

0
HiveCoreR0/src/hive_core_r0/world/room/model.config → HiveCoreR0/src/robot_simulation/world/room/model.config
View File

0
HiveCoreR0/src/hive_core_r0/world/room/model.sdf → HiveCoreR0/src/robot_simulation/world/room/model.sdf
View File