diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..907def8 --- /dev/null +++ b/.gitignore @@ -0,0 +1,11 @@ +HiveCoreR0/build/** +HiveCoreR0/install/** +HiveCoreR0/log/** + +HiveCoreR1/build/** +HiveCoreR1/install/** +HiveCoreR1/log/** + +HiveCoreR2/build/** +HiveCoreR2/install/** +HiveCoreR2/log/** \ No newline at end of file diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 208c88a..cd0d391 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -4,7 +4,8 @@ "name": "Linux", "includePath": [ "${workspaceFolder}/**", - "/opt/ros/humble/include/**" + "/opt/ros/humble/include/**", + "/usr/include/**" ], "defines": [], "compilerPath": "/usr/bin/gcc", diff --git a/.vscode/settings.json b/.vscode/settings.json index 7b53f63..48f93c7 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -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" } } \ No newline at end of file diff --git a/HiveCoreR0/hive_core_bot0.gv b/HiveCoreR0/hive_core_bot0.gv deleted file mode 100644 index 2e869bf..0000000 --- a/HiveCoreR0/hive_core_bot0.gv +++ /dev/null @@ -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" -} diff --git a/HiveCoreR0/hive_core_bot0.pdf b/HiveCoreR0/hive_core_bot0.pdf deleted file mode 100644 index 7b61e90..0000000 Binary files a/HiveCoreR0/hive_core_bot0.pdf and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/config/bot0_ros2_controller.yaml b/HiveCoreR0/src/hive_core_r0/config/bot0_ros2_controller.yaml deleted file mode 100644 index f1234ef..0000000 --- a/HiveCoreR0/src/hive_core_r0/config/bot0_ros2_controller.yaml +++ /dev/null @@ -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 diff --git a/HiveCoreR0/src/hive_core_r0/launch/gazebo_sim_bot0.launch.py b/HiveCoreR0/src/hive_core_r0/launch/gazebo_sim_bot0.launch.py deleted file mode 100644 index bff4c6a..0000000 --- a/HiveCoreR0/src/hive_core_r0/launch/gazebo_sim_bot0.launch.py +++ /dev/null @@ -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],) - ), - - ]) \ No newline at end of file diff --git a/HiveCoreR0/src/hive_core_r0/src/arm_control_node.cpp b/HiveCoreR0/src/hive_core_r0/src/arm_control_node.cpp deleted file mode 100644 index e258d60..0000000 --- a/HiveCoreR0/src/hive_core_r0/src/arm_control_node.cpp +++ /dev/null @@ -1,116 +0,0 @@ -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/float64_multi_array.hpp" -#include -#include - -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( - "/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::SharedPtr cmd_pub_; - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Time start_time_; // 起始时间 - double amplitude_rad_; // 振幅（弧度） - double frequency_; // 频率（Hz） - std::vector arm_joint_indices_; // 手臂关节索引 - int total_joints_; // 总关节数 -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - - // 运行节点（按Ctrl+C退出） - rclcpp::spin(node); - - // 退出时发送复位指令（所有关节回0） - node->RestJoint(); - RCLCPP_INFO(node->get_logger(), "节点退出，发送复位指令"); - - rclcpp::shutdown(); - return 0; -} diff --git a/HiveCoreR0/src/hive_core_r0/src/keyboard_control_node.cpp b/HiveCoreR0/src/hive_core_r0/src/keyboard_control_node.cpp deleted file mode 100644 index 3513314..0000000 --- a/HiveCoreR0/src/hive_core_r0/src/keyboard_control_node.cpp +++ /dev/null @@ -1,140 +0,0 @@ -#include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include -#include -#include -#include - -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( - "/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::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(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/bot0.ros2_control.xacro b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/bot0.ros2_control.xacro deleted file mode 100644 index 01c05bb..0000000 --- a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/bot0.ros2_control.xacro +++ /dev/null @@ -1,165 +0,0 @@ - - - - - - - - - gazebo_ros2_control/GazeboSystem - - - - - - - -1.0 - 1.0 - - - - - - - - - -1.0 - 1.0 - - - - - - - - - -1.0 - 1.0 - - - - - - - - - -1.0 - 1.0 - - - - - - - - - - -1.0 - 1.0 - - - - - - - - - -1.0 - 1.0 - - - - - - - - - -1.0 - 1.0 - - - - - - - - - -1.0 - 1.0 - - - - - - - - - -1.0 - 1.0 - - - - - - - - - -1.0 - 1.0 - - - - - - - - - - -50 - 50 - - - - - - - - - -50 - 50 - - - - - - - - - -50 - 50 - - - - - - - - - -50 - 50 - - - - - - - - - - - - $(find hive_core_r0)/config/bot0_ros2_controller.yaml - - - - diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/generic_joint.xacro b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/generic_joint.xacro deleted file mode 100644 index b9a5b58..0000000 --- a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/generic_joint.xacro +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - \ No newline at end of file diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/generic_link.xacro b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/generic_link.xacro deleted file mode 100644 index e73dc45..0000000 --- a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/generic_link.xacro +++ /dev/null @@ -1,65 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1e3 - 1e2 - 0.8 - 0.8 - 0.001 - - - - 0.0 - 10000.0 - - - - - - - - \ No newline at end of file diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf deleted file mode 100755 index a7bc75f..0000000 --- a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf +++ /dev/null @@ -1,860 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf copy.xacro b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf copy.xacro deleted file mode 100644 index a45e144..0000000 --- a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf copy.xacro +++ /dev/null @@ -1,440 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf.xacro b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf.xacro deleted file mode 100644 index 2570beb..0000000 --- a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/hive_core_bot0.urdf.xacro +++ /dev/null @@ -1,438 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/base_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/base_link.STL deleted file mode 100755 index 9b83836..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/base_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_arm1_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_arm1_link.STL deleted file mode 100755 index b490296..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_arm1_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_arm2_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_arm2_link.STL deleted file mode 100755 index e86a41d..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_arm2_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_hip_pitch_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_hip_pitch_link.STL deleted file mode 100755 index a11933c..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_hip_pitch_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_leg_pitch_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_leg_pitch_link.STL deleted file mode 100755 index 0edcbb9..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_leg_pitch_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_shoulder_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_shoulder_link.STL deleted file mode 100755 index f111585..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_shoulder_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_wheel1_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_wheel1_link.STL deleted file mode 100755 index 2354012..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_wheel1_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_wheel2_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_wheel2_link.STL deleted file mode 100755 index 43387e6..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/left_wheel2_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_arm1_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_arm1_link.STL deleted file mode 100755 index 9030d2d..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_arm1_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_arm2_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_arm2_link.STL deleted file mode 100755 index bdc6572..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_arm2_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_hip_pitch_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_hip_pitch_link.STL deleted file mode 100755 index 69661bd..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_hip_pitch_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_leg_pitch_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_leg_pitch_link.STL deleted file mode 100755 index 1a24885..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_leg_pitch_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_shoulder_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_shoulder_link.STL deleted file mode 100755 index 7a0dff1..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_shoulder_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_wheel1_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_wheel1_link.STL deleted file mode 100755 index a2663e9..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_wheel1_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_wheel2_link.STL b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_wheel2_link.STL deleted file mode 100755 index 2a7b5e5..0000000 Binary files a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/right_wheel2_link.STL and /dev/null differ diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/robot_roll.py b/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/robot_roll.py deleted file mode 100755 index b0883ed..0000000 --- a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot0/meshes/robot_roll.py +++ /dev/null @@ -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() \ No newline at end of file diff --git a/HiveCoreR0/src/hive_core_r0/world/bot0.world b/HiveCoreR0/src/hive_core_r0/world/bot0.world deleted file mode 100644 index 1c21c77..0000000 --- a/HiveCoreR0/src/hive_core_r0/world/bot0.world +++ /dev/null @@ -1,324 +0,0 @@ - - - - 1 - 0 0 10 0 -0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.5 0.1 -0.9 - - 0 - 0 - 0 - - - - 1 - - - - - 0 0 1 - 100 100 - - - - - - 100 - 50 - - - - - - - - - - - 10 - - - 0 - - - 0 0 1 - 100 100 - - - - - - - 0 - 0 - 0 - - - 0 0 -9.8 - 6e-06 2.3e-05 -4.2e-05 - - - 0.001 - 1 - 1000 - - - 0.4 0.4 0.4 1 - 0.7 0.7 0.7 1 - 1 - - - - - EARTH_WGS84 - 0 - 0 - 0 - 0 - - - 1 - - - 0 0 1 0 -0 0 - - - 1.5 0.8 0.03 - - - - - - 0.6 - 0.6 - - - - - - - - - - - 10 - - - 0 0 1 0 -0 0 - - - 1.5 0.8 0.03 - - - - - - - - 0.68 0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - 10 - - - - - - - - - - - - - - - 0.68 0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - - - - - - 0.68 -0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - 10 - - - - - - - - - - - - - - - 0.68 -0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - - - - - - -0.68 -0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - 10 - - - - - - - - - - - - - - - -0.68 -0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - - - - - - -0.68 0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - 10 - - - - - - - - - - - - - - - -0.68 0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - - - - - 0 - 0 - 0 - - -0.772768 -1.3242 0 0 -0 0 - - - 83 658000000 - 83 738917407 - 1753783232 542949961 - 83658 - - 0 0 0 0 -0 0 - 1 1 1 - - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - -0.772768 -1.3242 0 0 -0 0 - 1 1 1 - - -0.772768 -1.3242 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - 0 0 10 0 -0 0 - - - - - 4.75138 -5.25474 1.98471 0 0.275643 2.35619 - orbit - perspective - - - - diff --git a/HiveCoreR0/src/hive_core_r0/world/table.world b/HiveCoreR0/src/hive_core_r0/world/table.world deleted file mode 100644 index 0c21f92..0000000 --- a/HiveCoreR0/src/hive_core_r0/world/table.world +++ /dev/null @@ -1,321 +0,0 @@ - - - - 1 - 0 0 10 0 -0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.5 0.1 -0.9 - - 0 - 0 - 0 - - - - 1 - - - - - 0 0 1 - 100 100 - - - - - - 1e6 - 5e4 - 10.0 - 10.0 - - - - 0.0 - - - - - 0 - - - 0 0 1 - 100 100 - - - - - - - 0 - 0 - 0 - - - 0 0 -9.8 - 6e-06 2.3e-05 -4.2e-05 - - - 0.001 - 1 - 1000 - - - 0.4 0.4 0.4 1 - 0.7 0.7 0.7 1 - 1 - - - - - EARTH_WGS84 - 0 - 0 - 0 - 0 - - - 1 - - - 0 0 1 0 -0 0 - - - 1.5 0.8 0.03 - - - - - - 0.6 - 0.6 - - - - - - - - - - - 10 - - - 0 0 1 0 -0 0 - - - 1.5 0.8 0.03 - - - - - - - - 0.68 0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - 10 - - - - - - - - - - - - - - - 0.68 0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - - - - - - 0.68 -0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - 10 - - - - - - - - - - - - - - - 0.68 -0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - - - - - - -0.68 -0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - 10 - - - - - - - - - - - - - - - -0.68 -0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - - - - - - -0.68 0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - 10 - - - - - - - - - - - - - - - -0.68 0.38 0.5 0 -0 0 - - - 0.02 - 1 - - - - - - - 0 - 0 - 0 - - 0.726316 -1.21623 0 0 -0 0 - - - 56 645000000 - 56 704774179 - 1753436842 65734558 - 56645 - - 0 0 0 0 -0 0 - 1 1 1 - - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - 0.726316 -1.21623 0 0 -0 0 - 1 1 1 - - 0.726316 -1.21623 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - 0 0 10 0 -0 0 - - - - - 4.85069 0.30326 2.30729 0 0.443643 -2.92699 - orbit - perspective - - - - diff --git a/HiveCoreR0/src/hive_core_r0/CMakeLists.txt b/HiveCoreR0/src/keyboard_control/CMakeLists.txt similarity index 61% rename from HiveCoreR0/src/hive_core_r0/CMakeLists.txt rename to HiveCoreR0/src/keyboard_control/CMakeLists.txt index a573bfd..a2353c7 100644 --- a/HiveCoreR0/src/hive_core_r0/CMakeLists.txt +++ b/HiveCoreR0/src/keyboard_control/CMakeLists.txt @@ -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( 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() diff --git a/HiveCoreR0/src/keyboard_control/include/keyboard_control.hpp b/HiveCoreR0/src/keyboard_control/include/keyboard_control.hpp new file mode 100644 index 0000000..1c8d51b --- /dev/null +++ b/HiveCoreR0/src/keyboard_control/include/keyboard_control.hpp @@ -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::SharedPtr twist_pub_; + rclcpp::TimerBase::SharedPtr timer_; + double linear_speed_; // 线速度（m/s） + double angular_speed_; // 角速度（rad/s） +}; + +#endif // KEY_BOARD_CONTROL_NODE__HPP_ \ No newline at end of file diff --git a/HiveCoreR0/src/keyboard_control/launch/keyboard_control.launch.py b/HiveCoreR0/src/keyboard_control/launch/keyboard_control.launch.py new file mode 100644 index 0000000..c871a37 --- /dev/null +++ b/HiveCoreR0/src/keyboard_control/launch/keyboard_control.launch.py @@ -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 + ]) diff --git a/HiveCoreR0/src/keyboard_control/package.xml b/HiveCoreR0/src/keyboard_control/package.xml new file mode 100644 index 0000000..64da8e4 --- /dev/null +++ b/HiveCoreR0/src/keyboard_control/package.xml @@ -0,0 +1,18 @@ + + + + keyboard_control + 1.0.0 + keyboard package + Your Name + Apache-2.0 + + ament_cmake + rclcpp + geometry_msgs + nav_msgs + + + ament_cmake + + diff --git a/HiveCoreR0/src/keyboard_control/src/keyboard_control.cpp b/HiveCoreR0/src/keyboard_control/src/keyboard_control.cpp new file mode 100644 index 0000000..46f0465 --- /dev/null +++ b/HiveCoreR0/src/keyboard_control/src/keyboard_control.cpp @@ -0,0 +1,129 @@ +#include "keyboard_control.hpp" +#include +#include +#include +#include + +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( + "/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(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/HiveCoreR0/src/robot_bringup/CMakeLists.txt b/HiveCoreR0/src/robot_bringup/CMakeLists.txt new file mode 100644 index 0000000..1b86bc8 --- /dev/null +++ b/HiveCoreR0/src/robot_bringup/CMakeLists.txt @@ -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() diff --git a/HiveCoreR0/src/hive_core_r0/LICENSE b/HiveCoreR0/src/robot_bringup/LICENSE similarity index 100% rename from HiveCoreR0/src/hive_core_r0/LICENSE rename to HiveCoreR0/src/robot_bringup/LICENSE diff --git a/HiveCoreR0/src/robot_bringup/launch/main.launch.py b/HiveCoreR0/src/robot_bringup/launch/main.launch.py new file mode 100644 index 0000000..5906ea0 --- /dev/null +++ b/HiveCoreR0/src/robot_bringup/launch/main.launch.py @@ -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 + ]) diff --git a/HiveCoreR0/src/robot_bringup/package.xml b/HiveCoreR0/src/robot_bringup/package.xml new file mode 100644 index 0000000..02969c3 --- /dev/null +++ b/HiveCoreR0/src/robot_bringup/package.xml @@ -0,0 +1,15 @@ + + + + robot_bringup + 1.0.0 + Robot control package + Your Name + Apache-2.0 + + ament_cmake + + + ament_cmake + + diff --git a/HiveCoreR0/src/robot_control/CMakeLists.txt b/HiveCoreR0/src/robot_control/CMakeLists.txt new file mode 100644 index 0000000..6ada095 --- /dev/null +++ b/HiveCoreR0/src/robot_control/CMakeLists.txt @@ -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() diff --git a/HiveCoreR0/src/robot_control/LICENSE b/HiveCoreR0/src/robot_control/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/HiveCoreR0/src/robot_control/LICENSE @@ -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. diff --git a/HiveCoreR0/src/robot_control/include/arm_control.hpp b/HiveCoreR0/src/robot_control/include/arm_control.hpp new file mode 100644 index 0000000..8bb7ea3 --- /dev/null +++ b/HiveCoreR0/src/robot_control/include/arm_control.hpp @@ -0,0 +1,38 @@ +#ifndef ROBOT_CONTROL__ARMCONTROL_HPP_ +#define ROBOT_CONTROL__ARMCONTROL_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include + +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& target_joint, double duration); + + void DefaultArmMovement(std::vector& joint_position_desired, double duration); + +private: + + int total_joints_; // 总关节数 + + std::vector joint_position_; // 机械臂关节位置 + std::vector joint_velocity_; // 机械臂关节速度 + std::vector joint_acceleration_; // 机械臂关节加速度 + std::vector joint_torque_; // 机械臂关节力矩 + std::vector joint_angle_; // 机械臂关节角度 + + std::vector joint_position_desired_; // 机械臂关节期望位置 + + double amplitude_rad_; // 振幅 （弧度） + double amplitude_deg_; // 振幅 （角度） + double frequency_; // 频率 +}; + +#endif // ROBOT_CONTROL__ARMCONTROL_HPP_ \ No newline at end of file diff --git a/HiveCoreR0/src/robot_control/include/leg_control.hpp b/HiveCoreR0/src/robot_control/include/leg_control.hpp new file mode 100644 index 0000000..26f1eb9 --- /dev/null +++ b/HiveCoreR0/src/robot_control/include/leg_control.hpp @@ -0,0 +1,24 @@ +#ifndef ROBOT_CONTROL__LEGCONTROL_HPP_ +#define ROBOT_CONTROL__LEGCONTROL_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include + +namespace robot_control +{ + +class LegControl +{ +public: + LegControl(); + ~LegControl(); + + void set_goal(double x, double y); + +private: + +}; + +} // namespace robot_control + +#endif // ROBOT_CONTROL__LEGCONTROL_HPP_ diff --git a/HiveCoreR0/src/robot_control/include/robot_control.hpp b/HiveCoreR0/src/robot_control/include/robot_control.hpp new file mode 100644 index 0000000..ee53674 --- /dev/null +++ b/HiveCoreR0/src/robot_control/include/robot_control.hpp @@ -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::SharedPtr cmd_pub_; + rclcpp::Subscription::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 leg_joint_indices_; // 腿部关节索引 + std::vector arm_joint_indices_; // 手臂关节索引 + std::vector wheel_joint_indices_; // 轮子关节索引 + std::vector joint_directions_; // 关节方向 + std::vector wheel_joint_directions_; // 轮子关节方向 + + // 轮子控制相关 + double wheel_radius_; // 轮子半径（米） + double wheel_separation_; // 轮距（米） + std::vector 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_ diff --git a/HiveCoreR0/src/robot_control/include/robot_kinematics.hpp b/HiveCoreR0/src/robot_control/include/robot_kinematics.hpp new file mode 100644 index 0000000..17bfe73 --- /dev/null +++ b/HiveCoreR0/src/robot_control/include/robot_kinematics.hpp @@ -0,0 +1,89 @@ +#ifndef ROBOT_MODEL__ROBOT_KINEMATICS_HPP_ +#define ROBOT_MODEL__ROBOT_KINEMATICS_HPP_ + +#include "robot_model.hpp" +#include +#include +#include + +namespace robot_model +{ + +class RobotKinematics +{ +public: + /** + * @brief 构造函数 + * @param robot_model 机器人模型指针 + */ + explicit RobotKinematics(std::shared_ptr robot_model); + + /** + * @brief 析构函数 + */ + ~RobotKinematics() = default; + + // 手臂运动学 + + /** + * @brief 计算手臂正运动学 + * @param joint_angles 关节角度数组 + * @return 末端执行器位置 (x, y, z) + */ + Eigen::Vector3d arm_forward_kinematics(const std::vector& 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& 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& 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& 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 robot_model_; // 机器人模型指针 +}; + +} // namespace robot_model + +#endif // ROBOT_MODEL__ROBOT_KINEMATICS_HPP_ diff --git a/HiveCoreR0/src/robot_control/include/robot_model.hpp b/HiveCoreR0/src/robot_control/include/robot_model.hpp new file mode 100644 index 0000000..f2af88c --- /dev/null +++ b/HiveCoreR0/src/robot_control/include/robot_model.hpp @@ -0,0 +1,114 @@ +#ifndef ROBOT_MODEL__ROBOT_MODEL_HPP_ +#define ROBOT_MODEL__ROBOT_MODEL_HPP_ + +#include +#include +#include + +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> joints; // 关节 + std::vector> links; // 连杆 + std::string side; // 机械臂位置（如"left", "right"） +}; + +// 腿部结构体 +struct Leg +{ + std::vector> joints; // 关节 + std::vector> 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 arms_; // 机械臂集合 （2个机械臂） + std::vector legs_; // 腿部集合（4条腿） +}; + +} // namespace robot_model + +#endif // ROBOT_MODEL__ROBOT_MODEL_HPP_ diff --git a/HiveCoreR0/src/robot_control/include/urdf_parser.hpp b/HiveCoreR0/src/robot_control/include/urdf_parser.hpp new file mode 100644 index 0000000..ec95bec --- /dev/null +++ b/HiveCoreR0/src/robot_control/include/urdf_parser.hpp @@ -0,0 +1,78 @@ +#ifndef ROBOT_MODEL__URDF_PARSER_HPP_ +#define ROBOT_MODEL__URDF_PARSER_HPP_ + +#include "robot_model.hpp" +#include +#include +#include + +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 parse_from_file(const std::string& urdf_file_path); + + /** + * @brief 从XML字符串解析URDF并构建RobotModel + * @param urdf_xml URDF XML字符串 + * @return 构建的RobotModel共享指针，失败返回nullptr + */ + std::shared_ptr parse_from_string(const std::string& urdf_xml); + +private: + /** + * @brief 解析URDF模型并构建RobotModel + * @param model URDF模型 + * @return 构建的RobotModel共享指针 + */ + std::shared_ptr build_robot_model(const urdf::Model& model); + + /** + * @brief 解析连杆信息 + * @param link URDF连杆 + * @return 构建的Link共享指针 + */ + std::shared_ptr parse_link(const urdf::LinkConstSharedPtr& link); + + /** + * @brief 解析关节信息 + * @param joint URDF关节 + * @return 构建的Joint共享指针 + */ + std::shared_ptr 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_ diff --git a/HiveCoreR0/src/robot_control/launch/robot_control.launch.py b/HiveCoreR0/src/robot_control/launch/robot_control.launch.py new file mode 100644 index 0000000..1605fa0 --- /dev/null +++ b/HiveCoreR0/src/robot_control/launch/robot_control.launch.py @@ -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 + ]) diff --git a/HiveCoreR0/src/robot_control/package.xml b/HiveCoreR0/src/robot_control/package.xml new file mode 100644 index 0000000..f9cab5d --- /dev/null +++ b/HiveCoreR0/src/robot_control/package.xml @@ -0,0 +1,18 @@ + + + + robot_control + 1.0.0 + Robot control package + Your Name + Apache-2.0 + + ament_cmake + rclcpp + geometry_msgs + nav_msgs + + + ament_cmake + + diff --git a/HiveCoreR0/src/robot_control/src/arm_control.cpp b/HiveCoreR0/src/robot_control/src/arm_control.cpp new file mode 100644 index 0000000..b0266db --- /dev/null +++ b/HiveCoreR0/src/robot_control/src/arm_control.cpp @@ -0,0 +1,62 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +#include +#include +#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& 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 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]; + } + + // 计算每个关节的所需时间 +} + diff --git a/HiveCoreR0/src/robot_control/src/leg_control.cpp b/HiveCoreR0/src/robot_control/src/leg_control.cpp new file mode 100644 index 0000000..d278557 --- /dev/null +++ b/HiveCoreR0/src/robot_control/src/leg_control.cpp @@ -0,0 +1,139 @@ +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include + +class MotionController : public rclcpp::Node +{ +public: + MotionController() : Node("motion_controller") + { + // 声明参数 + this->declare_parameter("wheel_radius", 0.05); + this->declare_parameter("wheel_separation", 0.2); + + // 获取参数 + this->get_parameter("wheel_radius", wheel_radius_); + this->get_parameter("wheel_separation", wheel_separation_); + + // 创建发布者和订阅者 + cmd_vel_pub_ = this->create_publisher("/cmd_vel", 10); + odom_sub_ = this->create_subscription( + "/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::SharedPtr cmd_vel_pub_; + rclcpp::Subscription::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()); + rclcpp::shutdown(); + return 0; +} diff --git a/HiveCoreR0/src/hive_core_r0/src/hiveBot1_control_node.cpp b/HiveCoreR0/src/robot_control/src/robot_control.cpp similarity index 57% rename from HiveCoreR0/src/hive_core_r0/src/hiveBot1_control_node.cpp rename to HiveCoreR0/src/robot_control/src/robot_control.cpp index 83b9617..7dbcae6 100644 --- a/HiveCoreR0/src/hive_core_r0/src/hiveBot1_control_node.cpp +++ b/HiveCoreR0/src/robot_control/src/robot_control.cpp @@ -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 #include #include #include #include -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( - "/bot1_position_controller/commands", 10 + "/robot_position_controller/commands", 10 ); // 创建订阅者（接收来自速度指令） twist_sub_ = this->create_subscription( "/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::SharedPtr cmd_pub_; - rclcpp::Subscription::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 leg_joint_indices_; // 腿部关节索引 - std::vector arm_joint_indices_; // 手臂关节索引 - std::vector wheel_joint_indices_; // 轮子关节索引 - std::vector joint_directions_; // 关节方向 - std::vector wheel_joint_directions_; // 轮子关节方向 - - // 轮子控制相关 - double wheel_radius_; // 轮子半径（米） - double wheel_separation_; // 轮距（米） - std::vector 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(); - rclcpp::spin(node); - node->resetJoints(); - RCLCPP_INFO(node->get_logger(), "节点退出，发送复位指令"); - rclcpp::shutdown(); - return 0; -} + { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + node->resetJoints(); + RCLCPP_INFO(node->get_logger(), "节点退出，发送复位指令"); + rclcpp::shutdown(); + return 0; + } diff --git a/HiveCoreR0/src/robot_control/src/robot_kinematics.cpp b/HiveCoreR0/src/robot_control/src/robot_kinematics.cpp new file mode 100644 index 0000000..04df666 --- /dev/null +++ b/HiveCoreR0/src/robot_control/src/robot_kinematics.cpp @@ -0,0 +1,215 @@ +#include "robot_kinematics.hpp" +#include +#include +#include + +namespace robot_model +{ + +RobotKinematics::RobotKinematics(std::shared_ptr 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& 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& 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& 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& 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 diff --git a/HiveCoreR0/src/robot_control/src/robot_model.cpp b/HiveCoreR0/src/robot_control/src/robot_model.cpp new file mode 100644 index 0000000..d9407a6 --- /dev/null +++ b/HiveCoreR0/src/robot_control/src/robot_model.cpp @@ -0,0 +1,186 @@ +#include "robot_model.hpp" +#include +#include + +namespace robot_model +{ + + RobotModel::RobotModel() + { + initialize_model(); + } + + void RobotModel::initialize_model() + { + // 初始化机械臂（4个关节，2个连杆） + // 添加关节 + arm_.joints.push_back(std::make_shared("arm_base_joint", -M_PI/2, M_PI/2, 10.0)); + arm_.joints.push_back(std::make_shared("arm_shoulder_joint", -M_PI/2, M_PI/2, 8.0)); + arm_.joints.push_back(std::make_shared("arm_elbow_joint", 0, M_PI/2, 6.0)); + arm_.joints.push_back(std::make_shared("arm_wrist_joint", -M_PI/4, M_PI/4, 4.0)); + + // 添加连杆 + arm_.links.push_back(std::make_shared("arm_upper_link", 0.3, 1.5)); + arm_.links.push_back(std::make_shared("arm_lower_link", 0.25, 1.0)); + + // 初始化腿部（4条腿，每条腿2个连杆） + std::vector 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(side + "_hip_joint", -M_PI/4, M_PI/4, 15.0)); + leg.joints.push_back(std::make_shared(side + "_knee_joint", 0, M_PI/2, 12.0)); + + // 为每条腿添加连杆 + leg.links.push_back(std::make_shared(side + "_thigh_link", 0.2, 2.0)); + leg.links.push_back(std::make_shared(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 diff --git a/HiveCoreR0/src/robot_control/src/urdf_parser.cpp b/HiveCoreR0/src/robot_control/src/urdf_parser.cpp new file mode 100644 index 0000000..26c2f5d --- /dev/null +++ b/HiveCoreR0/src/robot_control/src/urdf_parser.cpp @@ -0,0 +1,249 @@ +#include "urdf_parser.hpp" +#include +#include +#include +#include +#include + +namespace robot_model +{ + +std::shared_ptr 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 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 UrdfParser::build_robot_model(const urdf::Model& model) +{ + auto robot_model = std::make_shared(); + + // 我们需要访问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 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(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->name, length, mass); +} + +std::shared_ptr 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->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 diff --git a/HiveCoreR0/src/robot_description/CMakeLists.txt b/HiveCoreR0/src/robot_description/CMakeLists.txt new file mode 100644 index 0000000..98e4af0 --- /dev/null +++ b/HiveCoreR0/src/robot_description/CMakeLists.txt @@ -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() diff --git a/HiveCoreR0/src/robot_description/LICENSE b/HiveCoreR0/src/robot_description/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/HiveCoreR0/src/robot_description/LICENSE @@ -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. diff --git a/HiveCoreR0/src/hive_core_r0/config/bot1_ros2_controller.yaml b/HiveCoreR0/src/robot_description/config/robot_ros2_controller.yaml similarity index 95% rename from HiveCoreR0/src/hive_core_r0/config/bot1_ros2_controller.yaml rename to HiveCoreR0/src/robot_description/config/robot_ros2_controller.yaml index 7ea05fe..068507a 100644 --- a/HiveCoreR0/src/hive_core_r0/config/bot1_ros2_controller.yaml +++ b/HiveCoreR0/src/robot_description/config/robot_ros2_controller.yaml @@ -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 diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/base_link.STL b/HiveCoreR0/src/robot_description/meshes/base_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/base_link.STL rename to HiveCoreR0/src/robot_description/meshes/base_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_arm1_link.STL b/HiveCoreR0/src/robot_description/meshes/left_arm1_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_arm1_link.STL rename to HiveCoreR0/src/robot_description/meshes/left_arm1_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_arm2_link.STL b/HiveCoreR0/src/robot_description/meshes/left_arm2_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_arm2_link.STL rename to HiveCoreR0/src/robot_description/meshes/left_arm2_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_leg1_link.STL b/HiveCoreR0/src/robot_description/meshes/left_leg1_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_leg1_link.STL rename to HiveCoreR0/src/robot_description/meshes/left_leg1_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_leg2_link.STL b/HiveCoreR0/src/robot_description/meshes/left_leg2_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_leg2_link.STL rename to HiveCoreR0/src/robot_description/meshes/left_leg2_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_shoulder_link.STL b/HiveCoreR0/src/robot_description/meshes/left_shoulder_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_shoulder_link.STL rename to HiveCoreR0/src/robot_description/meshes/left_shoulder_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_wheel1_link.STL b/HiveCoreR0/src/robot_description/meshes/left_wheel1_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_wheel1_link.STL rename to HiveCoreR0/src/robot_description/meshes/left_wheel1_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_wheel2_link.STL b/HiveCoreR0/src/robot_description/meshes/left_wheel2_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/left_wheel2_link.STL rename to HiveCoreR0/src/robot_description/meshes/left_wheel2_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/pitch_body_link.STL b/HiveCoreR0/src/robot_description/meshes/pitch_body_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/pitch_body_link.STL rename to HiveCoreR0/src/robot_description/meshes/pitch_body_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_arm1_link.STL b/HiveCoreR0/src/robot_description/meshes/right_arm1_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_arm1_link.STL rename to HiveCoreR0/src/robot_description/meshes/right_arm1_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_arm2_link.STL b/HiveCoreR0/src/robot_description/meshes/right_arm2_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_arm2_link.STL rename to HiveCoreR0/src/robot_description/meshes/right_arm2_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_leg1_link.STL b/HiveCoreR0/src/robot_description/meshes/right_leg1_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_leg1_link.STL rename to HiveCoreR0/src/robot_description/meshes/right_leg1_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_leg2_link.STL b/HiveCoreR0/src/robot_description/meshes/right_leg2_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_leg2_link.STL rename to HiveCoreR0/src/robot_description/meshes/right_leg2_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_shoulder_link.STL b/HiveCoreR0/src/robot_description/meshes/right_shoulder_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_shoulder_link.STL rename to HiveCoreR0/src/robot_description/meshes/right_shoulder_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_wheel1_link.STL b/HiveCoreR0/src/robot_description/meshes/right_wheel1_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_wheel1_link.STL rename to HiveCoreR0/src/robot_description/meshes/right_wheel1_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_wheel2_link.STL b/HiveCoreR0/src/robot_description/meshes/right_wheel2_link.STL similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/meshes/right_wheel2_link.STL rename to HiveCoreR0/src/robot_description/meshes/right_wheel2_link.STL diff --git a/HiveCoreR0/src/hive_core_r0/package.xml b/HiveCoreR0/src/robot_description/package.xml similarity index 54% rename from HiveCoreR0/src/hive_core_r0/package.xml rename to HiveCoreR0/src/robot_description/package.xml index dae87e2..77e1252 100644 --- a/HiveCoreR0/src/hive_core_r0/package.xml +++ b/HiveCoreR0/src/robot_description/package.xml @@ -1,17 +1,16 @@ - hive_core_r0 - 0.0.0 - TODO: Package description - hivecore + robot_description + 1.0.0 + Robot model description package + Your Name Apache-2.0 ament_cmake - - ament_lint_auto - ament_lint_common - + urdf + xacro + ament_cmake diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/generic_joint.xacro b/HiveCoreR0/src/robot_description/urdf/generic_joint.xacro similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/generic_joint.xacro rename to HiveCoreR0/src/robot_description/urdf/generic_joint.xacro diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/generic_link.xacro b/HiveCoreR0/src/robot_description/urdf/generic_link.xacro similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/generic_link.xacro rename to HiveCoreR0/src/robot_description/urdf/generic_link.xacro diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/hive_core_bot1.urdf b/HiveCoreR0/src/robot_description/urdf/robot.urdf similarity index 100% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/hive_core_bot1.urdf rename to HiveCoreR0/src/robot_description/urdf/robot.urdf diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/hive_core_bot1.urdf.xacro b/HiveCoreR0/src/robot_description/urdf/robot.urdf.xacro similarity index 96% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/hive_core_bot1.urdf.xacro rename to HiveCoreR0/src/robot_description/urdf/robot.urdf.xacro index 036b193..e7ba307 100644 --- a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/hive_core_bot1.urdf.xacro +++ b/HiveCoreR0/src/robot_description/urdf/robot.urdf.xacro @@ -1,10 +1,10 @@ - + - + - - + + --> - - + + diff --git a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/bot1_ros2_control.xacro b/HiveCoreR0/src/robot_description/urdf/robot_ros2_control.xacro similarity index 95% rename from HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/bot1_ros2_control.xacro rename to HiveCoreR0/src/robot_description/urdf/robot_ros2_control.xacro index e883387..abd3208 100644 --- a/HiveCoreR0/src/hive_core_r0/urdf/hiveBot1/bot1_ros2_control.xacro +++ b/HiveCoreR0/src/robot_description/urdf/robot_ros2_control.xacro @@ -1,9 +1,9 @@ - + - + gazebo_ros2_control/GazeboSystem @@ -110,7 +110,7 @@ - $(find hive_core_r0)/config/bot1_ros2_controller.yaml + $(find robot_description)/config/robot_ros2_controller.yaml diff --git a/HiveCoreR0/src/robot_simulation/CMakeLists.txt b/HiveCoreR0/src/robot_simulation/CMakeLists.txt new file mode 100644 index 0000000..dcbde2b --- /dev/null +++ b/HiveCoreR0/src/robot_simulation/CMakeLists.txt @@ -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() diff --git a/HiveCoreR0/src/robot_simulation/LICENSE b/HiveCoreR0/src/robot_simulation/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/HiveCoreR0/src/robot_simulation/LICENSE @@ -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. diff --git a/HiveCoreR0/src/hive_core_r0/launch/gazebo_sim_bot1.launch.py b/HiveCoreR0/src/robot_simulation/launch/gazebo_simulation.launch.py similarity index 65% rename from HiveCoreR0/src/hive_core_r0/launch/gazebo_sim_bot1.launch.py rename to HiveCoreR0/src/robot_simulation/launch/gazebo_simulation.launch.py index 3c041b7..3732a0e 100644 --- a/HiveCoreR0/src/hive_core_r0/launch/gazebo_sim_bot1.launch.py +++ b/HiveCoreR0/src/robot_simulation/launch/gazebo_simulation.launch.py @@ -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, ]) \ No newline at end of file diff --git a/HiveCoreR0/src/robot_simulation/package.xml b/HiveCoreR0/src/robot_simulation/package.xml new file mode 100644 index 0000000..1bd1d61 --- /dev/null +++ b/HiveCoreR0/src/robot_simulation/package.xml @@ -0,0 +1,15 @@ + + + + robot_simulation + 1.0.0 + Robot simulation package + Your Name + Apache-2.0 + + ament_cmake + + + ament_cmake + + diff --git a/HiveCoreR0/src/hive_core_r0/world/custom_room.world b/HiveCoreR0/src/robot_simulation/world/custom_room.world similarity index 100% rename from HiveCoreR0/src/hive_core_r0/world/custom_room.world rename to HiveCoreR0/src/robot_simulation/world/custom_room.world diff --git a/HiveCoreR0/src/hive_core_r0/world/empty.world b/HiveCoreR0/src/robot_simulation/world/empty.world similarity index 100% rename from HiveCoreR0/src/hive_core_r0/world/empty.world rename to HiveCoreR0/src/robot_simulation/world/empty.world diff --git a/HiveCoreR0/src/hive_core_r0/world/room/model.config b/HiveCoreR0/src/robot_simulation/world/room/model.config similarity index 100% rename from HiveCoreR0/src/hive_core_r0/world/room/model.config rename to HiveCoreR0/src/robot_simulation/world/room/model.config diff --git a/HiveCoreR0/src/hive_core_r0/world/room/model.sdf b/HiveCoreR0/src/robot_simulation/world/room/model.sdf similarity index 100% rename from HiveCoreR0/src/hive_core_r0/world/room/model.sdf rename to HiveCoreR0/src/robot_simulation/world/room/model.sdf