Compare commits
2 Commits
main
...
zhengjie_s
| Author | SHA1 | Date | |
|---|---|---|---|
| ac6fad19c2 | |||
| 4594fd4fbc |
Submodule zhengjie_simulator/hivecore_robot_simulator/IFRA_LinkAttacher added at b056289ba9
@@ -0,0 +1,48 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(body_controller)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(interfaces REQUIRED)
|
||||
|
||||
add_executable(body_controller src/body_controller.cpp)
|
||||
target_include_directories(body_controller PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
target_compile_features(body_controller PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
|
||||
|
||||
ament_target_dependencies(body_controller
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
interfaces
|
||||
)
|
||||
|
||||
install(TARGETS body_controller
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION share/${PROJECT_NAME}/launch)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -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.
|
||||
@@ -0,0 +1,93 @@
|
||||
#ifndef BODY_CONTROLLER_HPP
|
||||
#define BODY_CONTROLLER_HPP
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include <sensor_msgs/msg/joint_state.hpp>
|
||||
#include <std_msgs/msg/float64_multi_array.hpp>
|
||||
#include <interfaces/action/body.hpp>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <atomic>
|
||||
#include <vector>
|
||||
|
||||
namespace body_controller
|
||||
{
|
||||
using BodyAction = interfaces::action::Body;
|
||||
using GoalHandleBody = rclcpp_action::ServerGoalHandle<BodyAction>;
|
||||
|
||||
// 关节数量
|
||||
constexpr size_t BODY_JOINT_COUNT = 17;
|
||||
|
||||
// 位置误差阈值
|
||||
constexpr double POSITION_ERROR_THRESHOLD = 0.01;
|
||||
// 速度阈值
|
||||
constexpr double VELOCITY_THRESHOLD = 0.01;
|
||||
|
||||
class BodyController : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
BodyController();
|
||||
~BodyController();
|
||||
|
||||
private:
|
||||
// Action服务器
|
||||
rclcpp_action::Server<BodyAction>::SharedPtr action_server_;
|
||||
|
||||
// 发布器 - 向body_position_controller发送命令
|
||||
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr command_pub_;
|
||||
|
||||
// 订阅器 - 订阅body_state_broadcaster的关节状态
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr state_sub_;
|
||||
|
||||
// 关节名称列表
|
||||
std::vector<std::string> joint_names_;
|
||||
|
||||
// 当前关节状态
|
||||
std::vector<double> current_positions_;
|
||||
std::vector<double> current_velocities_;
|
||||
std::mutex state_mutex_;
|
||||
|
||||
// 目标位置
|
||||
std::vector<double> target_positions_;
|
||||
|
||||
// 执行状态
|
||||
std::atomic<bool> is_executing_{false};
|
||||
std::mutex execute_mutex_;
|
||||
|
||||
// 当前goal handle
|
||||
std::shared_ptr<GoalHandleBody> current_goal_handle_;
|
||||
rclcpp::TimerBase::SharedPtr control_timer_;
|
||||
|
||||
// 关节状态回调
|
||||
void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
|
||||
|
||||
// Action回调
|
||||
rclcpp_action::GoalResponse handleGoal(
|
||||
const rclcpp_action::GoalUUID& uuid,
|
||||
std::shared_ptr<const BodyAction::Goal> goal);
|
||||
|
||||
rclcpp_action::CancelResponse handleCancel(
|
||||
const std::shared_ptr<GoalHandleBody> goal_handle);
|
||||
|
||||
void handleAccepted(const std::shared_ptr<GoalHandleBody> goal_handle);
|
||||
|
||||
// 执行body动作
|
||||
void executeBody(const std::shared_ptr<GoalHandleBody> goal_handle);
|
||||
|
||||
// 发布命令
|
||||
void publishCommand();
|
||||
|
||||
// 检查是否到达目标位置
|
||||
bool hasReachedTarget();
|
||||
|
||||
// 尝试开始执行
|
||||
bool tryStartExecuting();
|
||||
|
||||
// 停止执行
|
||||
void stopExecuting();
|
||||
};
|
||||
}
|
||||
|
||||
#endif // BODY_CONTROLLER_HPP
|
||||
@@ -0,0 +1,20 @@
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
# GripperController 节点
|
||||
body_controller_node = Node(
|
||||
package='body_controller',
|
||||
executable='body_controller',
|
||||
name='body_controller',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': False
|
||||
}]
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
body_controller_node,
|
||||
])
|
||||
|
||||
@@ -0,0 +1,21 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>body_controller</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="zhengjie@hivecore.cn">zj</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>control_msgs</depend>
|
||||
<depend>interfaces</depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,274 @@
|
||||
#include "body_controller/body_controller.hpp"
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
#include <algorithm>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
using namespace std::placeholders;
|
||||
|
||||
namespace body_controller
|
||||
{
|
||||
|
||||
BodyController::BodyController()
|
||||
: Node("body_controller")
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Initializing BodyController...");
|
||||
|
||||
// 初始化关节名称列表 (与joint_controllers.yaml中body_position_controller配置一致)
|
||||
joint_names_ = {
|
||||
// 身体关节
|
||||
"head_joint",
|
||||
"body_joint",
|
||||
"body_2_joint",
|
||||
// 后轮关节
|
||||
"left_behind_hip_joint",
|
||||
"left_behind_leg_joint",
|
||||
"left_behind_wheel_joint",
|
||||
"right_behind_hip_joint",
|
||||
"right_behind_leg_joint",
|
||||
"right_behind_wheel_joint",
|
||||
// 前轮关节
|
||||
"left_front_roll_joint",
|
||||
"left_front_hip_joint",
|
||||
"left_front_knee_joint",
|
||||
"left_front_wheel_joint",
|
||||
"right_front_roll_joint",
|
||||
"right_front_hip_joint",
|
||||
"right_front_knee_joint",
|
||||
"right_front_wheel_joint"
|
||||
};
|
||||
|
||||
// 初始化状态向量
|
||||
current_positions_.resize(BODY_JOINT_COUNT, 0.0);
|
||||
current_velocities_.resize(BODY_JOINT_COUNT, 0.0);
|
||||
target_positions_.resize(BODY_JOINT_COUNT, 0.0);
|
||||
|
||||
// 创建命令发布器
|
||||
command_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
|
||||
"/body_position_controller/commands", 10);
|
||||
RCLCPP_INFO(this->get_logger(), "Created command publisher: /body_position_controller/commands");
|
||||
|
||||
// 创建状态订阅器
|
||||
state_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"/body_state_broadcaster/joint_states", 10,
|
||||
std::bind(&BodyController::jointStateCallback, this, _1));
|
||||
RCLCPP_INFO(this->get_logger(), "Subscribed to: /body_state_broadcaster/joint_states");
|
||||
|
||||
// 创建Action服务器
|
||||
action_server_ = rclcpp_action::create_server<BodyAction>(
|
||||
this,
|
||||
"body_action",
|
||||
std::bind(&BodyController::handleGoal, this, _1, _2),
|
||||
std::bind(&BodyController::handleCancel, this, _1),
|
||||
std::bind(&BodyController::handleAccepted, this, _1));
|
||||
|
||||
control_timer_ = this->create_wall_timer(
|
||||
20ms,
|
||||
std::bind(&BodyController::publishCommand, this));
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "BodyController initialized with %zu joints", BODY_JOINT_COUNT);
|
||||
}
|
||||
|
||||
BodyController::~BodyController()
|
||||
{
|
||||
if (control_timer_) {
|
||||
control_timer_->cancel();
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "Destroying BodyController...");
|
||||
}
|
||||
|
||||
void BodyController::jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||||
|
||||
// 根据关节名称更新位置和速度
|
||||
for (size_t i = 0; i < msg->name.size(); ++i) {
|
||||
// 查找关节在我们列表中的索引
|
||||
auto it = std::find(joint_names_.begin(), joint_names_.end(), msg->name[i]);
|
||||
if (it != joint_names_.end()) {
|
||||
size_t idx = std::distance(joint_names_.begin(), it);
|
||||
if (i < msg->position.size()) {
|
||||
current_positions_[idx] = msg->position[i];
|
||||
}
|
||||
if (i < msg->velocity.size()) {
|
||||
current_velocities_[idx] = msg->velocity[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 如果正在执行，发布feedback
|
||||
if (is_executing_ && current_goal_handle_) {
|
||||
auto feedback = std::make_shared<BodyAction::Feedback>();
|
||||
feedback->float_length = static_cast<int16_t>(current_positions_.size());
|
||||
feedback->current_position = current_positions_;
|
||||
current_goal_handle_->publish_feedback(feedback);
|
||||
}
|
||||
}
|
||||
|
||||
rclcpp_action::GoalResponse BodyController::handleGoal(
|
||||
const rclcpp_action::GoalUUID& uuid,
|
||||
std::shared_ptr<const BodyAction::Goal> goal)
|
||||
{
|
||||
(void)uuid;
|
||||
|
||||
// 验证目标位置数量
|
||||
if (goal->data_length != static_cast<int16_t>(BODY_JOINT_COUNT)) {
|
||||
RCLCPP_WARN(this->get_logger(),
|
||||
"Invalid data_length: expected %zu, got %d",
|
||||
BODY_JOINT_COUNT, goal->data_length);
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
|
||||
if (goal->target_position.size() != BODY_JOINT_COUNT) {
|
||||
RCLCPP_WARN(this->get_logger(),
|
||||
"Invalid target_position size: expected %zu, got %zu",
|
||||
BODY_JOINT_COUNT, goal->target_position.size());
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
|
||||
// 尝试开始执行
|
||||
if (!tryStartExecuting()) {
|
||||
RCLCPP_WARN(this->get_logger(), "Body controller is already executing");
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Received body goal with %zu joint positions",
|
||||
goal->target_position.size());
|
||||
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
|
||||
rclcpp_action::CancelResponse BodyController::handleCancel(
|
||||
const std::shared_ptr<GoalHandleBody> goal_handle)
|
||||
{
|
||||
(void)goal_handle;
|
||||
RCLCPP_INFO(this->get_logger(), "Received cancel request");
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void BodyController::handleAccepted(const std::shared_ptr<GoalHandleBody> goal_handle)
|
||||
{
|
||||
current_goal_handle_ = goal_handle;
|
||||
std::thread{std::bind(&BodyController::executeBody, this, std::placeholders::_1), goal_handle}.detach();
|
||||
}
|
||||
|
||||
void BodyController::executeBody(const std::shared_ptr<GoalHandleBody> goal_handle)
|
||||
{
|
||||
auto goal = goal_handle->get_goal();
|
||||
auto result = std::make_shared<BodyAction::Result>();
|
||||
|
||||
// 设置目标位置
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||||
target_positions_ = goal->target_position;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Executing body movement...");
|
||||
|
||||
// 等待达到目标位置
|
||||
rclcpp::Rate rate(50); // 50Hz检查频率
|
||||
auto start_time = this->now();
|
||||
auto timeout = rclcpp::Duration::from_seconds(10.0); // 10秒超时
|
||||
|
||||
while (rclcpp::ok()) {
|
||||
// 检查取消请求
|
||||
if (goal_handle->is_canceling()) {
|
||||
result->result = -1; // 取消
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||||
result->float_length = static_cast<int16_t>(current_positions_.size());
|
||||
result->final_position = current_positions_;
|
||||
}
|
||||
goal_handle->canceled(result);
|
||||
RCLCPP_INFO(this->get_logger(), "Body goal canceled");
|
||||
stopExecuting();
|
||||
return;
|
||||
}
|
||||
|
||||
// 检查超时
|
||||
if (this->now() - start_time > timeout) {
|
||||
RCLCPP_WARN(this->get_logger(), "Body goal timeout");
|
||||
result->result = -2; // 超时
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||||
result->float_length = static_cast<int16_t>(current_positions_.size());
|
||||
result->final_position = current_positions_;
|
||||
}
|
||||
goal_handle->abort(result);
|
||||
stopExecuting();
|
||||
return;
|
||||
}
|
||||
|
||||
// 检查是否到达目标位置
|
||||
if (hasReachedTarget()) {
|
||||
RCLCPP_INFO(this->get_logger(), "Reached target position");
|
||||
result->result = 0; // 成功
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||||
result->float_length = static_cast<int16_t>(current_positions_.size());
|
||||
result->final_position = current_positions_;
|
||||
}
|
||||
goal_handle->succeed(result);
|
||||
stopExecuting();
|
||||
return;
|
||||
}
|
||||
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
// ROS shutdown
|
||||
stopExecuting();
|
||||
}
|
||||
|
||||
void BodyController::publishCommand()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||||
auto msg = std_msgs::msg::Float64MultiArray();
|
||||
msg.data = target_positions_;
|
||||
command_pub_->publish(msg);
|
||||
}
|
||||
|
||||
bool BodyController::hasReachedTarget()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||||
|
||||
for (size_t i = 0; i < BODY_JOINT_COUNT; ++i) {
|
||||
double error = std::abs(target_positions_[i] - current_positions_[i]);
|
||||
double velocity = std::abs(current_velocities_[i]);
|
||||
|
||||
// 如果任意关节未到达目标，返回false
|
||||
if (error > POSITION_ERROR_THRESHOLD || velocity > VELOCITY_THRESHOLD) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool BodyController::tryStartExecuting()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(execute_mutex_);
|
||||
if (is_executing_) {
|
||||
return false;
|
||||
}
|
||||
is_executing_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
void BodyController::stopExecuting()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(execute_mutex_);
|
||||
is_executing_ = false;
|
||||
current_goal_handle_ = nullptr;
|
||||
}
|
||||
|
||||
} // namespace body_controller
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<body_controller::BodyController>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
project(camera_monitor)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
if(POLICY CMP0148)
|
||||
cmake_policy(SET CMP0148 OLD)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(interfaces REQUIRED)
|
||||
find_package(cv_bridge REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
|
||||
add_executable(camera_monitor src/camera_monitor.cpp)
|
||||
ament_target_dependencies(camera_monitor
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
interfaces
|
||||
cv_bridge
|
||||
OpenCV
|
||||
)
|
||||
|
||||
target_include_directories(camera_monitor PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
target_compile_features(camera_monitor PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
|
||||
|
||||
install(TARGETS camera_monitor
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -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.
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>camera_monitor</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="zhengjie@hivecore.cn">zj</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>interfaces</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>OpenCV</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,80 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <interfaces/msg/img_msg.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <chrono>
|
||||
#include <map>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
class TestCameraNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
TestCameraNode()
|
||||
: Node("test_camera_node")
|
||||
{
|
||||
// Gazebo 相机话题名称
|
||||
// 根据 URDF 中的配置，话题映射关系如下：
|
||||
// head_camera -> /head_camera/image_raw
|
||||
// left_arm_camera -> /left_arm_camera/image_raw
|
||||
// right_arm_camera -> /right_arm_camera/image_raw
|
||||
|
||||
// 创建订阅者
|
||||
// 使用 lambda 表达式来捕获 source 名称，避免 std::bind 导致的类型匹配问题
|
||||
sub_head_ = this->create_subscription<sensor_msgs::msg::Image>(
|
||||
"/head_camera/image_raw", 10,
|
||||
[this](const sensor_msgs::msg::Image::SharedPtr msg) {
|
||||
this->image_callback(msg, "head");
|
||||
});
|
||||
|
||||
sub_left_ = this->create_subscription<sensor_msgs::msg::Image>(
|
||||
"/left_arm_camera/image_raw", 10,
|
||||
[this](const sensor_msgs::msg::Image::SharedPtr msg) {
|
||||
this->image_callback(msg, "left");
|
||||
});
|
||||
|
||||
sub_right_ = this->create_subscription<sensor_msgs::msg::Image>(
|
||||
"/right_arm_camera/image_raw", 10,
|
||||
[this](const sensor_msgs::msg::Image::SharedPtr msg) {
|
||||
this->image_callback(msg, "right");
|
||||
});
|
||||
|
||||
image_count_["head"] = 0;
|
||||
image_count_["left"] = 0;
|
||||
image_count_["right"] = 0;
|
||||
RCLCPP_INFO(this->get_logger(), "TestCameraNode initialized and subscribing to Gazebo camera topics.");
|
||||
}
|
||||
|
||||
private:
|
||||
void image_callback(const sensor_msgs::msg::Image::SharedPtr msg, const std::string& source_name)
|
||||
{
|
||||
auto message = interfaces::msg::ImgMsg();
|
||||
message.source = source_name;
|
||||
message.image_color = *msg; // 直接转发图像数据
|
||||
|
||||
// 可以在这里添加深度图处理逻辑，如果有深度相机话题的话
|
||||
|
||||
image_count_[source_name]++;
|
||||
|
||||
// 降低日志频率，避免刷屏
|
||||
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000,
|
||||
"Forwarded image count: head:%d, left:%d, right:%d", image_count_["head"], image_count_["left"], image_count_["right"]);
|
||||
}
|
||||
|
||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_head_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_left_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_right_;
|
||||
|
||||
std::map<std::string, int> image_count_;
|
||||
};
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<TestCameraNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,46 @@
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
project(gripper_controller)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(interfaces REQUIRED)
|
||||
find_package(linkattacher_msgs REQUIRED)
|
||||
|
||||
add_executable(gripper_controller src/gripper_controller.cpp)
|
||||
target_include_directories(gripper_controller PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
target_compile_features(gripper_controller PUBLIC c_std_99 cxx_std_17)
|
||||
|
||||
ament_target_dependencies(gripper_controller
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
interfaces
|
||||
linkattacher_msgs
|
||||
)
|
||||
|
||||
install(TARGETS gripper_controller
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
# 安装 launch 文件
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION share/${PROJECT_NAME}/launch)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -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.
|
||||
@@ -0,0 +1,145 @@
|
||||
#ifndef GRIPPER_CONTROLLER_HPP
|
||||
#define GRIPPER_CONTROLLER_HPP
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include <sensor_msgs/msg/joint_state.hpp>
|
||||
#include <std_msgs/msg/float64_multi_array.hpp>
|
||||
#include <interfaces/action/gripper.hpp>
|
||||
#include <linkattacher_msgs/srv/attach_link.hpp>
|
||||
#include <linkattacher_msgs/srv/detach_link.hpp>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <atomic>
|
||||
#include <vector>
|
||||
|
||||
enum GripperType {
|
||||
GRIPPER_TYPE_LEFT,
|
||||
GRIPPER_TYPE_RIGHT,
|
||||
GRIPPER_TYPE_CNT,
|
||||
};
|
||||
|
||||
struct GripperConfig {
|
||||
std::string name; // "left" 或 "right"
|
||||
std::string command_topic; // effort 命令话题
|
||||
std::string state_topic; // 状态话题名称
|
||||
std::vector<std::string> joint_names; // 关节名称列表
|
||||
|
||||
// 位置范围
|
||||
double min_position; // 最小位置 (闭合) = 0.0
|
||||
double max_position; // 最大位置 (张开) = 0.015
|
||||
|
||||
// 当前状态 (来自 joint_states)
|
||||
double current_position; // 当前位置 (关节空间，取第一个关节)
|
||||
double current_velocity; // 当前速度
|
||||
double current_effort; // 当前力矩
|
||||
|
||||
// 目标状态
|
||||
double target_position; // 目标位置 (关节空间)
|
||||
double max_effort; // 最大夹持力
|
||||
|
||||
int callback_cnt; // 回调计数
|
||||
int debug_cnt;
|
||||
|
||||
// 执行状态
|
||||
std::atomic<bool> is_executing;
|
||||
std::mutex execute_mutex;
|
||||
|
||||
// Action 相关
|
||||
interfaces::action::Gripper::Feedback::SharedPtr feedback;
|
||||
std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::Gripper>> goal_handle;
|
||||
|
||||
// Publisher
|
||||
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr effort_pub;
|
||||
|
||||
// Subscriber
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr state_sub;
|
||||
|
||||
// 物体检测
|
||||
bool object_detected;
|
||||
int stall_count; // 停滞计数
|
||||
|
||||
// 物体附加状态
|
||||
bool is_attached; // 是否已附加物体
|
||||
std::string attached_model; // 附加的物体模型名
|
||||
std::string attached_link; // 附加的物体 link 名
|
||||
};
|
||||
|
||||
class GripperController : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
using GripperAction = interfaces::action::Gripper;
|
||||
using GoalHandleGripper = rclcpp_action::ServerGoalHandle<GripperAction>;
|
||||
|
||||
GripperController();
|
||||
~GripperController();
|
||||
|
||||
private:
|
||||
// Action Server 回调函数
|
||||
rclcpp_action::GoalResponse handleGoal(
|
||||
const rclcpp_action::GoalUUID& uuid,
|
||||
std::shared_ptr<const GripperAction::Goal> goal);
|
||||
|
||||
rclcpp_action::CancelResponse handleCancel(
|
||||
const std::shared_ptr<GoalHandleGripper> goal_handle);
|
||||
|
||||
void handleAccepted(const std::shared_ptr<GoalHandleGripper> goal_handle);
|
||||
|
||||
// 执行抓取动作
|
||||
void executeGripper(const std::shared_ptr<GoalHandleGripper> goal_handle);
|
||||
|
||||
// PD 控制循环 (定时器回调)
|
||||
void controlLoopCallback();
|
||||
|
||||
// 计算单个夹爪的 PD 控制力矩
|
||||
void computeAndPublishEffort(int gripper_type);
|
||||
|
||||
// 关节状态回调
|
||||
void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg, int gripper_type);
|
||||
|
||||
// 将归一化位置 (0-1) 转换为实际关节位置
|
||||
double normalizedToJointPosition(double normalized, int gripper_type);
|
||||
|
||||
// 将实际关节位置转换为归一化位置 (0-1)
|
||||
double jointToNormalizedPosition(double joint_pos, int gripper_type);
|
||||
|
||||
// 检测是否夹住物体
|
||||
bool detectObjectGrasped(int gripper_type);
|
||||
|
||||
// 设置执行状态 (线程安全)
|
||||
bool tryStartExecuting(int gripper_type);
|
||||
void stopExecuting(int gripper_type);
|
||||
|
||||
// 附加/分离物体
|
||||
void attachObject(int gripper_type, const std::string& object_model, const std::string& object_link);
|
||||
void detachObject(int gripper_type);
|
||||
|
||||
// Action Server
|
||||
rclcpp_action::Server<GripperAction>::SharedPtr action_server_;
|
||||
|
||||
// 控制循环定时器 (50Hz)
|
||||
rclcpp::TimerBase::SharedPtr control_timer_;
|
||||
|
||||
// 夹爪配置
|
||||
GripperConfig grippers_[GRIPPER_TYPE_CNT];
|
||||
|
||||
// PD 控制参数
|
||||
double kp_; // 比例增益
|
||||
double kd_; // 微分增益
|
||||
double default_max_effort_; // 默认最大力矩
|
||||
|
||||
// 物体检测参数
|
||||
static constexpr double VELOCITY_THRESHOLD = 0.002; // 速度阈值
|
||||
static constexpr double POSITION_ERROR_THRESHOLD = 0.001; // 位置误差阈值
|
||||
static constexpr int STALL_COUNT_THRESHOLD = 10; // 停滞计数阈值
|
||||
|
||||
// Link Attacher 服务客户端
|
||||
rclcpp::Client<linkattacher_msgs::srv::AttachLink>::SharedPtr attach_client_;
|
||||
rclcpp::Client<linkattacher_msgs::srv::DetachLink>::SharedPtr detach_client_;
|
||||
|
||||
// 机器人模型名称
|
||||
std::string robot_model_name_;
|
||||
};
|
||||
|
||||
#endif // GRIPPER_CONTROLLER_HPP
|
||||
@@ -0,0 +1,24 @@
|
||||
"""
|
||||
GripperController 独立启动文件
|
||||
启动夹爪控制器 Action Server
|
||||
"""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
# GripperController 节点
|
||||
gripper_controller_node = Node(
|
||||
package='gripper_controller',
|
||||
executable='gripper_controller',
|
||||
name='gripper_controller',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': False
|
||||
}]
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
gripper_controller_node,
|
||||
])
|
||||
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>gripper_controller</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Gripper controller with ROS2 Action interface for Gazebo simulation</description>
|
||||
<maintainer email="zhengjie@hivecore.cn">zj</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>control_msgs</depend>
|
||||
<depend>interfaces</depend>
|
||||
<depend>linkattacher_msgs</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,481 @@
|
||||
#include "gripper_controller/gripper_controller.hpp"
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
#include <algorithm>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
using namespace std::placeholders;
|
||||
using AttachLink = linkattacher_msgs::srv::AttachLink;
|
||||
using DetachLink = linkattacher_msgs::srv::DetachLink;
|
||||
|
||||
GripperController::GripperController()
|
||||
: Node("gripper_controller"),
|
||||
kp_(1000.0),
|
||||
kd_(50.0),
|
||||
default_max_effort_(15.0),
|
||||
robot_model_name_("fhrobot")
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Initializing GripperController (PD Effort Control)...");
|
||||
|
||||
// 创建 Link Attacher 服务客户端
|
||||
attach_client_ = this->create_client<AttachLink>("/ATTACHLINK");
|
||||
detach_client_ = this->create_client<DetachLink>("/DETACHLINK");
|
||||
RCLCPP_INFO(this->get_logger(), "Created link attacher service clients");
|
||||
|
||||
// 初始化左夹爪配置
|
||||
grippers_[GRIPPER_TYPE_LEFT].name = "left";
|
||||
grippers_[GRIPPER_TYPE_LEFT].command_topic = "/left_gripper_effort_controller/commands";
|
||||
grippers_[GRIPPER_TYPE_LEFT].state_topic = "/left_gripper_state_broadcaster/joint_states";
|
||||
grippers_[GRIPPER_TYPE_LEFT].joint_names = {
|
||||
"left_gripper_finger_left_joint",
|
||||
"left_gripper_finger_right_joint"
|
||||
};
|
||||
|
||||
// 初始化右夹爪配置
|
||||
grippers_[GRIPPER_TYPE_RIGHT].name = "right";
|
||||
grippers_[GRIPPER_TYPE_RIGHT].command_topic = "/right_gripper_effort_controller/commands";
|
||||
grippers_[GRIPPER_TYPE_RIGHT].state_topic = "/right_gripper_state_broadcaster/joint_states";
|
||||
grippers_[GRIPPER_TYPE_RIGHT].joint_names = {
|
||||
"right_gripper_finger_left_joint",
|
||||
"right_gripper_finger_right_joint"
|
||||
};
|
||||
|
||||
// 初始化夹爪状态
|
||||
for (int i = 0; i < GRIPPER_TYPE_CNT; i++) {
|
||||
grippers_[i].min_position = 0.0;
|
||||
grippers_[i].max_position = 0.015;
|
||||
grippers_[i].current_position = 0.0;
|
||||
grippers_[i].current_velocity = 0.0;
|
||||
grippers_[i].current_effort = 0.0;
|
||||
grippers_[i].target_position = 0.0; // 初始目标位置 (闭合)
|
||||
grippers_[i].max_effort = default_max_effort_;
|
||||
grippers_[i].is_executing = false;
|
||||
grippers_[i].object_detected = false;
|
||||
grippers_[i].stall_count = 0;
|
||||
grippers_[i].is_attached = false;
|
||||
grippers_[i].attached_model = "";
|
||||
grippers_[i].attached_link = "";
|
||||
grippers_[i].feedback = std::make_shared<GripperAction::Feedback>();
|
||||
grippers_[i].goal_handle = nullptr;
|
||||
grippers_[i].debug_cnt = 0;
|
||||
|
||||
// 创建 effort publisher
|
||||
grippers_[i].effort_pub = this->create_publisher<std_msgs::msg::Float64MultiArray>(
|
||||
grippers_[i].command_topic, 10);
|
||||
RCLCPP_INFO(this->get_logger(), "Created effort publisher for %s gripper: %s",
|
||||
grippers_[i].name.c_str(), grippers_[i].command_topic.c_str());
|
||||
|
||||
// 创建状态订阅者
|
||||
grippers_[i].state_sub = this->create_subscription<sensor_msgs::msg::JointState>(
|
||||
grippers_[i].state_topic, 10,
|
||||
[this, i](const sensor_msgs::msg::JointState::SharedPtr msg) {
|
||||
this->jointStateCallback(msg, i);
|
||||
});
|
||||
auto msg = std_msgs::msg::Float64MultiArray();
|
||||
msg.data = {0, 0};
|
||||
grippers_[i].effort_pub->publish(msg);
|
||||
RCLCPP_INFO(this->get_logger(), "Subscribed to %s", grippers_[i].state_topic.c_str());
|
||||
}
|
||||
|
||||
// 创建 Action Server
|
||||
action_server_ = rclcpp_action::create_server<GripperAction>(
|
||||
this,
|
||||
"gripper_action",
|
||||
std::bind(&GripperController::handleGoal, this, _1, _2),
|
||||
std::bind(&GripperController::handleCancel, this, _1),
|
||||
std::bind(&GripperController::handleAccepted, this, _1));
|
||||
|
||||
// 创建 PD 控制循环定时器 (50Hz)
|
||||
control_timer_ = this->create_wall_timer(
|
||||
20ms,
|
||||
std::bind(&GripperController::controlLoopCallback, this));
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "GripperController initialized. Kp=%.1f, Kd=%.1f", kp_, kd_);
|
||||
}
|
||||
|
||||
GripperController::~GripperController()
|
||||
{
|
||||
if (control_timer_) {
|
||||
control_timer_->cancel();
|
||||
}
|
||||
|
||||
for (int i = 0; i < GRIPPER_TYPE_CNT; i++) {
|
||||
grippers_[i].goal_handle = nullptr;
|
||||
grippers_[i].feedback = nullptr;
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "GripperController destroyed");
|
||||
}
|
||||
|
||||
void GripperController::jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg, int gripper_type)
|
||||
{
|
||||
GripperConfig& config = grippers_[gripper_type];
|
||||
|
||||
// 获取第一个关节的状态作为参考
|
||||
if (!msg->position.empty()) {
|
||||
config.current_position = msg->position[0];
|
||||
}
|
||||
if (!msg->velocity.empty()) {
|
||||
config.current_velocity = msg->velocity[0];
|
||||
}
|
||||
if (!msg->effort.empty()) {
|
||||
config.current_effort = msg->effort[0];
|
||||
}
|
||||
|
||||
// 如果正在执行，发布 feedback
|
||||
if (config.is_executing && config.goal_handle) {
|
||||
config.feedback->current_position = jointToNormalizedPosition(config.current_position, gripper_type);
|
||||
config.feedback->effort = config.current_effort;
|
||||
config.goal_handle->publish_feedback(config.feedback);
|
||||
}
|
||||
|
||||
++config.callback_cnt;
|
||||
if (config.callback_cnt == 500) {
|
||||
RCLCPP_INFO(this->get_logger(), "JointStateCallback: gripper_type=%d, effort=%.2f, position=%.2f",
|
||||
gripper_type, config.current_effort, config.current_position);
|
||||
config.callback_cnt = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void GripperController::controlLoopCallback()
|
||||
{
|
||||
// 对所有夹爪执行 PD 控制
|
||||
for (int i = 0; i < GRIPPER_TYPE_CNT; i++) {
|
||||
GripperConfig* config = &(grippers_[i]);
|
||||
if (config->is_executing == true) {
|
||||
computeAndPublishEffort(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void GripperController::computeAndPublishEffort(int gripper_type)
|
||||
{
|
||||
GripperConfig& config = grippers_[gripper_type];
|
||||
|
||||
// 计算位置误差
|
||||
double error = config.target_position - config.current_position;
|
||||
|
||||
// PD 控制计算力矩
|
||||
double effort = (kp_ * error - kd_ * config.current_velocity) * config.max_effort / default_max_effort_;
|
||||
|
||||
effort = std::clamp(effort, -config.max_effort, config.max_effort);
|
||||
|
||||
++config.debug_cnt;
|
||||
if (config.debug_cnt == 50) {
|
||||
RCLCPP_INFO(this->get_logger(), "computeAndPublishEffort: gripper_type=%d, effort=%.4f target_position=%.4f, current_position=%.4f velocity=%.4f",
|
||||
gripper_type, effort, config.target_position, config.current_position, config.current_velocity);
|
||||
config.debug_cnt = 0;
|
||||
}
|
||||
|
||||
auto msg = std_msgs::msg::Float64MultiArray();
|
||||
msg.data = {effort, effort};
|
||||
config.effort_pub->publish(msg);
|
||||
}
|
||||
|
||||
bool GripperController::detectObjectGrasped(int gripper_type)
|
||||
{
|
||||
GripperConfig& config = grippers_[gripper_type];
|
||||
|
||||
// 计算位置误差
|
||||
double error = std::abs(config.target_position - config.current_position);
|
||||
|
||||
// 检测条件:
|
||||
// 1. 力矩接近饱和
|
||||
// 2. 速度接近 0
|
||||
// 3. 位置误差大于阈值 (未到达目标)
|
||||
bool velocity_low = std::abs(config.current_velocity) < VELOCITY_THRESHOLD;
|
||||
bool position_error = error > POSITION_ERROR_THRESHOLD;
|
||||
|
||||
if (velocity_low && position_error) {
|
||||
config.stall_count++;
|
||||
if (config.stall_count >= STALL_COUNT_THRESHOLD) {
|
||||
return true;
|
||||
}
|
||||
} else {
|
||||
config.stall_count = 0;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
rclcpp_action::GoalResponse GripperController::handleGoal(
|
||||
const rclcpp_action::GoalUUID& uuid,
|
||||
std::shared_ptr<const GripperAction::Goal> goal)
|
||||
{
|
||||
(void)uuid;
|
||||
|
||||
// 验证夹爪类型
|
||||
if (goal->gripper_type < 0 || goal->gripper_type >= GRIPPER_TYPE_CNT) {
|
||||
RCLCPP_WARN(this->get_logger(), "Unknown gripper type: %d", goal->gripper_type);
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
|
||||
const int gripper_type = goal->gripper_type;
|
||||
GripperConfig& config = grippers_[gripper_type];
|
||||
|
||||
// 尝试开始执行
|
||||
if (!tryStartExecuting(gripper_type)) {
|
||||
RCLCPP_WARN(this->get_logger(), "%s gripper is already executing", config.name.c_str());
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Received gripper goal: name=%s, position=%.2f, max_effort=%.2f",
|
||||
config.name.c_str(), goal->position, goal->max_effort);
|
||||
|
||||
// 验证位置范围
|
||||
if (goal->position < 0.0 || goal->position > 1.0) {
|
||||
RCLCPP_WARN(this->get_logger(), "Position out of range [0, 1]: %.2f", goal->position);
|
||||
stopExecuting(gripper_type);
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
|
||||
rclcpp_action::CancelResponse GripperController::handleCancel(
|
||||
const std::shared_ptr<GoalHandleGripper> goal_handle)
|
||||
{
|
||||
(void)goal_handle;
|
||||
RCLCPP_INFO(this->get_logger(), "Received cancel request");
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void GripperController::handleAccepted(const std::shared_ptr<GoalHandleGripper> goal_handle)
|
||||
{
|
||||
GripperConfig& config = grippers_[goal_handle->get_goal()->gripper_type];
|
||||
config.goal_handle = goal_handle;
|
||||
std::thread{std::bind(&GripperController::executeGripper, this, goal_handle)}.detach();
|
||||
}
|
||||
|
||||
void GripperController::executeGripper(const std::shared_ptr<GoalHandleGripper> goal_handle)
|
||||
{
|
||||
auto goal = goal_handle->get_goal();
|
||||
const int gripper_type = goal->gripper_type;
|
||||
GripperConfig& config = grippers_[gripper_type];
|
||||
auto result = std::make_shared<GripperAction::Result>();
|
||||
|
||||
// 设置目标位置和最大力矩
|
||||
config.target_position = normalizedToJointPosition(goal->position, gripper_type);
|
||||
config.max_effort = goal->max_effort > 0 ? goal->max_effort : default_max_effort_;
|
||||
config.object_detected = false;
|
||||
config.stall_count = 0;
|
||||
|
||||
// 判断是张开还是闭合动作
|
||||
bool is_opening = goal->position > 0.5; // > 0.5 认为是张开
|
||||
|
||||
// 如果是张开动作，先分离物体
|
||||
if (is_opening && config.is_attached) {
|
||||
detachObject(gripper_type);
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Executing %s gripper: target=%.4f (%.2f), max_effort=%.1f, opening=%d",
|
||||
config.name.c_str(), config.target_position, goal->position, config.max_effort, is_opening);
|
||||
|
||||
// 等待达到目标或检测到物体
|
||||
rclcpp::Rate rate(50); // 50Hz 检查频率
|
||||
auto start_time = this->now();
|
||||
auto timeout = rclcpp::Duration::from_seconds(5.0);
|
||||
|
||||
while (rclcpp::ok()) {
|
||||
// 检查取消请求
|
||||
if (goal_handle->is_canceling()) {
|
||||
result->success = false;
|
||||
result->object_detected = false;
|
||||
result->final_position = jointToNormalizedPosition(config.current_position, gripper_type);
|
||||
goal_handle->canceled(result);
|
||||
RCLCPP_INFO(this->get_logger(), "Goal canceled");
|
||||
stopExecuting(gripper_type);
|
||||
return;
|
||||
}
|
||||
|
||||
// 检查超时
|
||||
if (this->now() - start_time > timeout) {
|
||||
RCLCPP_WARN(this->get_logger(), "Goal timeout");
|
||||
result->success = false;
|
||||
result->object_detected = config.object_detected;
|
||||
result->final_position = jointToNormalizedPosition(config.current_position, gripper_type);
|
||||
goal_handle->abort(result);
|
||||
stopExecuting(gripper_type);
|
||||
return;
|
||||
}
|
||||
|
||||
// 检测物体
|
||||
if (detectObjectGrasped(gripper_type)) {
|
||||
config.object_detected = true;
|
||||
RCLCPP_INFO(this->get_logger(), "Object detected! effort=%.2f, position=%.4f",
|
||||
config.current_effort, config.current_position);
|
||||
|
||||
// 附加物体 (使用默认物体名称，后续可从 goal 中获取)
|
||||
attachObject(gripper_type, "red_cylinder", "link");
|
||||
|
||||
result->success = true;
|
||||
result->object_detected = true;
|
||||
result->final_position = jointToNormalizedPosition(config.current_position, gripper_type);
|
||||
goal_handle->succeed(result);
|
||||
stopExecuting(gripper_type);
|
||||
return;
|
||||
}
|
||||
|
||||
// 检查是否到达目标位置
|
||||
double error = std::abs(config.target_position - config.current_position);
|
||||
if (error < POSITION_ERROR_THRESHOLD && std::abs(config.current_velocity) < VELOCITY_THRESHOLD) {
|
||||
RCLCPP_INFO(this->get_logger(), "Reached target position: %.4f", config.current_position);
|
||||
result->success = true;
|
||||
result->object_detected = false;
|
||||
result->final_position = jointToNormalizedPosition(config.current_position, gripper_type);
|
||||
goal_handle->succeed(result);
|
||||
stopExecuting(gripper_type);
|
||||
return;
|
||||
}
|
||||
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
// ROS shutdown
|
||||
stopExecuting(gripper_type);
|
||||
}
|
||||
|
||||
bool GripperController::tryStartExecuting(int gripper_type)
|
||||
{
|
||||
GripperConfig& config = grippers_[gripper_type];
|
||||
std::lock_guard<std::mutex> lock(config.execute_mutex);
|
||||
if (config.is_executing) {
|
||||
return false;
|
||||
}
|
||||
config.is_executing = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
void GripperController::stopExecuting(int gripper_type)
|
||||
{
|
||||
GripperConfig& config = grippers_[gripper_type];
|
||||
std::lock_guard<std::mutex> lock(config.execute_mutex);
|
||||
config.is_executing = false;
|
||||
}
|
||||
|
||||
double GripperController::normalizedToJointPosition(double normalized, int gripper_type)
|
||||
{
|
||||
const GripperConfig& config = grippers_[gripper_type];
|
||||
// 0.0 = 闭合 (min), 1.0 = 张开 (max)
|
||||
return config.min_position + normalized * (config.max_position - config.min_position);
|
||||
}
|
||||
|
||||
double GripperController::jointToNormalizedPosition(double joint_pos, int gripper_type)
|
||||
{
|
||||
const GripperConfig& config = grippers_[gripper_type];
|
||||
double range = config.max_position - config.min_position;
|
||||
if (range < 1e-6) {
|
||||
return 0.0;
|
||||
}
|
||||
return (joint_pos - config.min_position) / range;
|
||||
}
|
||||
|
||||
void GripperController::attachObject(int gripper_type, const std::string& object_model, const std::string& object_link)
|
||||
{
|
||||
GripperConfig& config = grippers_[gripper_type];
|
||||
|
||||
if (config.is_attached) {
|
||||
RCLCPP_WARN(this->get_logger(), "%s gripper already has an attached object", config.name.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
if (!attach_client_->wait_for_service(std::chrono::seconds(1))) {
|
||||
RCLCPP_WARN(this->get_logger(), "ATTACHLINK service not available");
|
||||
return;
|
||||
}
|
||||
|
||||
// 使用夹爪的第一个手指作为附加点
|
||||
std::string gripper_link = config.joint_names[0];
|
||||
// 从关节名获取 link 名 (去掉 _joint 后缀)
|
||||
size_t pos = gripper_link.find("_joint");
|
||||
if (pos != std::string::npos) {
|
||||
gripper_link = gripper_link.substr(0, pos);
|
||||
}
|
||||
|
||||
auto request = std::make_shared<AttachLink::Request>();
|
||||
request->model1_name = robot_model_name_;
|
||||
request->link1_name = gripper_link;
|
||||
request->model2_name = object_model;
|
||||
request->link2_name = object_link;
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Attaching %s::%s to %s::%s",
|
||||
request->model1_name.c_str(), request->link1_name.c_str(),
|
||||
request->model2_name.c_str(), request->link2_name.c_str());
|
||||
|
||||
auto future = attach_client_->async_send_request(request);
|
||||
|
||||
// 异步处理结果
|
||||
std::thread([this, future = std::move(future), gripper_type, object_model, object_link]() mutable {
|
||||
try {
|
||||
auto result = future.get();
|
||||
if (result->success) {
|
||||
grippers_[gripper_type].is_attached = true;
|
||||
grippers_[gripper_type].attached_model = object_model;
|
||||
grippers_[gripper_type].attached_link = object_link;
|
||||
RCLCPP_INFO(this->get_logger(), "Object attached successfully: %s", result->message.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to attach object: %s", result->message.c_str());
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Attach service call failed: %s", e.what());
|
||||
}
|
||||
}).detach();
|
||||
}
|
||||
|
||||
void GripperController::detachObject(int gripper_type)
|
||||
{
|
||||
GripperConfig& config = grippers_[gripper_type];
|
||||
|
||||
if (!config.is_attached) {
|
||||
return; // 没有附加物体，静默返回
|
||||
}
|
||||
|
||||
if (!detach_client_->wait_for_service(std::chrono::seconds(1))) {
|
||||
RCLCPP_WARN(this->get_logger(), "DETACHLINK service not available");
|
||||
return;
|
||||
}
|
||||
|
||||
// 使用夹爪的第一个手指作为附加点
|
||||
std::string gripper_link = config.joint_names[0];
|
||||
size_t pos = gripper_link.find("_joint");
|
||||
if (pos != std::string::npos) {
|
||||
gripper_link = gripper_link.substr(0, pos);
|
||||
}
|
||||
|
||||
auto request = std::make_shared<DetachLink::Request>();
|
||||
request->model1_name = robot_model_name_;
|
||||
request->link1_name = gripper_link;
|
||||
request->model2_name = config.attached_model;
|
||||
request->link2_name = config.attached_link;
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Detaching %s::%s from %s::%s",
|
||||
request->model1_name.c_str(), request->link1_name.c_str(),
|
||||
request->model2_name.c_str(), request->link2_name.c_str());
|
||||
|
||||
auto future = detach_client_->async_send_request(request);
|
||||
|
||||
// 异步处理结果
|
||||
std::thread([this, future = std::move(future), gripper_type]() mutable {
|
||||
try {
|
||||
auto result = future.get();
|
||||
if (result->success) {
|
||||
grippers_[gripper_type].is_attached = false;
|
||||
grippers_[gripper_type].attached_model = "";
|
||||
grippers_[gripper_type].attached_link = "";
|
||||
RCLCPP_INFO(this->get_logger(), "Object detached successfully: %s", result->message.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to detach object: %s", result->message.c_str());
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Detach service call failed: %s", e.what());
|
||||
}
|
||||
}).detach();
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<GripperController>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,110 @@
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
project(robot_simulator)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
add_compile_options(-g)
|
||||
|
||||
set(CMAKE_BUILD_TYPE Debug CACHE STRING "Build type" FORCE)
|
||||
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O0 -g")
|
||||
set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -O0 -g")
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(Eigen3 REQUIRED) # 查找Eigen3库
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(trajectory_msgs REQUIRED)
|
||||
find_package(control_msgs REQUIRED)
|
||||
find_package(gjk_interface REQUIRED)
|
||||
find_package(interfaces REQUIRED)
|
||||
find_package(PkgConfig REQUIRED)
|
||||
find_package(cv_bridge REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
find_package(yaml-cpp REQUIRED)
|
||||
find_package(ament_index_cpp REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
find_package(urdf REQUIRED)
|
||||
find_package(urdfdom REQUIRED)
|
||||
find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log)
|
||||
find_package(pinocchio REQUIRED)
|
||||
find_package(ocs2_pinocchio_interface REQUIRED)
|
||||
find_package(ocs2_oc REQUIRED)
|
||||
find_package(ocs2_core REQUIRED)
|
||||
|
||||
add_executable(robot_simulator
|
||||
src/robot_simulator.cpp
|
||||
src/data_collector.cpp
|
||||
src/robot_info_loader.cpp
|
||||
src/reference_manager_for_gazebo.cpp)
|
||||
target_include_directories(robot_simulator PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
target_compile_features(robot_simulator PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
|
||||
|
||||
ament_target_dependencies(robot_simulator
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
trajectory_msgs
|
||||
control_msgs
|
||||
gjk_interface
|
||||
interfaces
|
||||
cv_bridge
|
||||
ament_index_cpp
|
||||
tf2
|
||||
tf2_ros
|
||||
tf2_geometry_msgs
|
||||
urdf
|
||||
ocs2_pinocchio_interface
|
||||
ocs2_oc
|
||||
ocs2_core
|
||||
)
|
||||
|
||||
target_link_libraries(robot_simulator
|
||||
${OpenCV_LIBS}
|
||||
yaml-cpp
|
||||
Boost::system
|
||||
Boost::filesystem
|
||||
Boost::log_setup
|
||||
Boost::log
|
||||
pinocchio::pinocchio
|
||||
)
|
||||
|
||||
install(TARGETS robot_simulator
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
# Install launch files
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION share/${PROJECT_NAME}/launch
|
||||
)
|
||||
|
||||
# Install config files
|
||||
install(DIRECTORY config/
|
||||
DESTINATION share/${PROJECT_NAME}/config
|
||||
)
|
||||
|
||||
# Install urdf files
|
||||
install(DIRECTORY urdf/
|
||||
DESTINATION share/${PROJECT_NAME}/urdf
|
||||
)
|
||||
|
||||
# Install mesh files
|
||||
install(DIRECTORY meshes/
|
||||
DESTINATION share/${PROJECT_NAME}/meshes
|
||||
)
|
||||
|
||||
# Install world files
|
||||
install(DIRECTORY worlds/
|
||||
DESTINATION share/${PROJECT_NAME}/worlds
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -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.
|
||||
@@ -0,0 +1,168 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 1000 # Hz
|
||||
use_sim_time: true
|
||||
|
||||
left_arm_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
right_arm_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
left_gripper_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
right_gripper_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
body_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
left_arm_position_controller:
|
||||
type: forward_command_controller/ForwardCommandController
|
||||
|
||||
right_arm_position_controller:
|
||||
type: forward_command_controller/ForwardCommandController
|
||||
|
||||
left_gripper_effort_controller:
|
||||
type: forward_command_controller/ForwardCommandController
|
||||
|
||||
right_gripper_effort_controller:
|
||||
type: forward_command_controller/ForwardCommandController
|
||||
|
||||
body_position_controller:
|
||||
type: forward_command_controller/ForwardCommandController
|
||||
|
||||
left_arm_state_broadcaster:
|
||||
ros__parameters:
|
||||
use_local_topics: true # 发布到 /left_arm_state_broadcaster/joint_states
|
||||
interfaces: ["position", "velocity", "effort"]
|
||||
joints:
|
||||
- left_joint1
|
||||
- left_joint2
|
||||
- left_joint3
|
||||
- left_joint4
|
||||
- left_joint5
|
||||
- left_joint6
|
||||
|
||||
right_arm_state_broadcaster:
|
||||
ros__parameters:
|
||||
use_local_topics: true # 发布到 /right_arm_state_broadcaster/joint_states
|
||||
interfaces: ["position", "velocity", "effort"]
|
||||
joints:
|
||||
- right_joint1
|
||||
- right_joint2
|
||||
- right_joint3
|
||||
- right_joint4
|
||||
- right_joint5
|
||||
- right_joint6
|
||||
|
||||
left_gripper_state_broadcaster:
|
||||
ros__parameters:
|
||||
use_local_topics: true # 发布到 /left_gripper_state_broadcaster/joint_states
|
||||
interfaces: ["position", "velocity", "effort"]
|
||||
joints:
|
||||
- left_gripper_finger_left_joint
|
||||
- left_gripper_finger_right_joint
|
||||
|
||||
right_gripper_state_broadcaster:
|
||||
ros__parameters:
|
||||
use_local_topics: true # 发布到 /right_gripper_state_broadcaster/joint_states
|
||||
interfaces: ["position", "velocity", "effort"]
|
||||
joints:
|
||||
- right_gripper_finger_left_joint
|
||||
- right_gripper_finger_right_joint
|
||||
|
||||
body_state_broadcaster:
|
||||
ros__parameters:
|
||||
use_local_topics: true # 发布到 /body_state_broadcaster/joint_states
|
||||
interfaces: ["position", "velocity", "effort"]
|
||||
joints:
|
||||
# 身体关节
|
||||
- head_joint
|
||||
- body_joint
|
||||
- body_2_joint
|
||||
# 后轮关节
|
||||
- left_behind_hip_joint
|
||||
- left_behind_leg_joint
|
||||
- left_behind_wheel_joint
|
||||
- right_behind_hip_joint
|
||||
- right_behind_leg_joint
|
||||
- right_behind_wheel_joint
|
||||
# 前轮关节
|
||||
- left_front_roll_joint
|
||||
- left_front_hip_joint
|
||||
- left_front_knee_joint
|
||||
- left_front_wheel_joint
|
||||
- right_front_roll_joint
|
||||
- right_front_hip_joint
|
||||
- right_front_knee_joint
|
||||
- right_front_wheel_joint
|
||||
|
||||
left_arm_position_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- left_joint1
|
||||
- left_joint2
|
||||
- left_joint3
|
||||
- left_joint4
|
||||
- left_joint5
|
||||
- left_joint6
|
||||
interface_name: position
|
||||
command_interfaces: ["position"]
|
||||
state_interfaces: ["position"]
|
||||
|
||||
right_arm_position_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- right_joint1
|
||||
- right_joint2
|
||||
- right_joint3
|
||||
- right_joint4
|
||||
- right_joint5
|
||||
- right_joint6
|
||||
interface_name: position
|
||||
command_interfaces: ["position"]
|
||||
state_interfaces: ["position"]
|
||||
|
||||
left_gripper_effort_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- left_gripper_finger_left_joint
|
||||
- left_gripper_finger_right_joint
|
||||
interface_name: effort
|
||||
|
||||
right_gripper_effort_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- right_gripper_finger_left_joint
|
||||
- right_gripper_finger_right_joint
|
||||
interface_name: effort
|
||||
|
||||
body_position_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
# 身体关节
|
||||
- head_joint
|
||||
- body_joint
|
||||
- body_2_joint
|
||||
# 后轮关节
|
||||
- left_behind_hip_joint
|
||||
- left_behind_leg_joint
|
||||
- left_behind_wheel_joint
|
||||
- right_behind_hip_joint
|
||||
- right_behind_leg_joint
|
||||
- right_behind_wheel_joint
|
||||
# 前轮关节
|
||||
- left_front_roll_joint
|
||||
- left_front_hip_joint
|
||||
- left_front_knee_joint
|
||||
- left_front_wheel_joint
|
||||
- right_front_roll_joint
|
||||
- right_front_hip_joint
|
||||
- right_front_knee_joint
|
||||
- right_front_wheel_joint
|
||||
interface_name: position
|
||||
command_interfaces: ["position"]
|
||||
state_interfaces: ["position"]
|
||||
|
||||
@@ -0,0 +1,36 @@
|
||||
robot_parts:
|
||||
leg_joints:
|
||||
- "LF_HAA"
|
||||
- "LF_HFE"
|
||||
- "LF_KFE"
|
||||
- "RF_HAA"
|
||||
- "RF_HFE"
|
||||
- "RF_KFE"
|
||||
- "LH_HAA"
|
||||
- "LH_HFE"
|
||||
- "LH_KFE"
|
||||
- "RH_HAA"
|
||||
- "RH_HFE"
|
||||
- "RH_KFE"
|
||||
arm_joints:
|
||||
- "L_ARM_SHOULDER_PAN"
|
||||
- "L_ARM_SHOULDER_LIFT"
|
||||
- "L_ARM_ELBOW"
|
||||
- "L_ARM_WRIST_1"
|
||||
- "L_ARM_WRIST_2"
|
||||
- "L_ARM_WRIST_3"
|
||||
- "R_ARM_SHOULDER_PAN"
|
||||
- "R_ARM_SHOULDER_LIFT"
|
||||
- "R_ARM_ELBOW"
|
||||
- "R_ARM_WRIST_1"
|
||||
- "R_ARM_WRIST_2"
|
||||
- "R_ARM_WRIST_3"
|
||||
leg_contacts:
|
||||
- "LF_FOOT"
|
||||
- "RF_FOOT"
|
||||
- "LH_FOOT"
|
||||
- "RH_FOOT"
|
||||
arm_contacts:
|
||||
- "LEFT_HAND"
|
||||
- "RIGHT_HAND"
|
||||
|
||||
@@ -0,0 +1,102 @@
|
||||
#ifndef COL_SIMULATOR_HPP
|
||||
#define COL_SIMULATOR_HPP
|
||||
|
||||
#include <unordered_map>
|
||||
#include <map>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <urdf_model/pose.h>
|
||||
#include <std_msgs/msg/string.hpp>
|
||||
#include "arm_action_define.h"
|
||||
#include "gjk_interface.hpp"
|
||||
|
||||
namespace collision_check {
|
||||
typedef enum {
|
||||
BODY_TYPE_LEFT_ARM = 0,
|
||||
BODY_TYPE_RIGHT_ARM,
|
||||
BODY_TYPE_OTHERS,
|
||||
BODY_CNT
|
||||
} BodyListE;
|
||||
|
||||
/*typedef enum {
|
||||
FRAME_CHANGE_TYPE_ARM_BASE_TO_ORI,
|
||||
FRAME_CHANGE_TYPE_ORI_TO_ARM_BASE,
|
||||
FRAME_CHANGE_TYPE_ARM_END_TO_ORI,
|
||||
FRAME_CHANGE_TYPE_ORI_TO_ARM_END,
|
||||
FRAME_CHANGE_TYPE_ERR
|
||||
} FrameChangeTypeE;*/
|
||||
|
||||
typedef std::vector<Eigen::Vector3d> Polyhedron;
|
||||
|
||||
struct JointsInfo {
|
||||
std::string name; // 关节名称
|
||||
int type; // 关节类型
|
||||
int parent_link; // 父连杆
|
||||
int child_link; // 子连杆
|
||||
Eigen::Vector3d axis; // 关节轴
|
||||
|
||||
Eigen::Vector3d nextLinkOffset; // 下一个连杆关于关节原点偏移
|
||||
Eigen::Quaterniond rotation; // 关节旋转
|
||||
double limit_lower;
|
||||
double limit_upper;
|
||||
double limit_v;
|
||||
double limit_e;
|
||||
float *angle; // 关节角度
|
||||
};
|
||||
|
||||
struct CollisionStructureInfo {
|
||||
std::string link_name; // 所属链接名称
|
||||
int geometry_type; // 几何类型
|
||||
std::string mesh_filename; // mesh文件路径（如STL）
|
||||
std::vector<OBB> obbModelList; // 网格边界盒 [min_x, max_x, min_y, max_y, min_z, max_z]
|
||||
std::vector<int> obbLinkIndexList; // 网格边界盒所属链接索引
|
||||
std::vector<int> jointIndexList; // 网格边界盒所属关节索引
|
||||
|
||||
Eigen::Quaterniond rotation; // 链接旋转
|
||||
Eigen::Vector3d offset; // 链接偏移
|
||||
|
||||
int joint_count;
|
||||
int link_index;
|
||||
int parent_link_index;
|
||||
};
|
||||
|
||||
class CollisionChecktor
|
||||
{
|
||||
public:
|
||||
CollisionChecktor(std::string robot_string, std::string srdf_string, float **jointsList);
|
||||
~CollisionChecktor();
|
||||
|
||||
int CheckCollision(rclcpp::Node *node);
|
||||
int AddPolyhedron(int id, const Polyhedron& poly);
|
||||
int RemovePolyhedron(int id);
|
||||
|
||||
void SetJoints(float **joints);
|
||||
private:
|
||||
int CheckCollisionInner(std::vector<OBB> *addedOBBs);
|
||||
void UpdatePolyhedrons(int bodyType);
|
||||
int InitLinkCollisionDetectMatrix(const std::string& srdf_str);
|
||||
void InitPolyhedronsList();
|
||||
|
||||
int InitXmlStrFromFile(const std::string& robot_string);
|
||||
int InitJoint();
|
||||
|
||||
std::vector<Polyhedron> polyhedrons_;
|
||||
std::vector<OBB> addedOBBs_;
|
||||
std::map<int, std::string> obbNameMap_;
|
||||
|
||||
std::vector<CollisionStructureInfo> collision_structures_;
|
||||
std::vector<JointsInfo> jointMap_;
|
||||
|
||||
int rootLinkIndex_;
|
||||
int maxObbCnt;
|
||||
float *joints_[BODY_CNT];
|
||||
|
||||
std::unordered_map<int, Polyhedron> addedPolyhedrons_;
|
||||
std::map<std::string, int> linkCollisionMap_;
|
||||
|
||||
// Collision pair enable matrix (flattened upper-triangle including i<j)
|
||||
std::vector<uint8_t> linkCollisionDetectMatrix_;
|
||||
// Precomputed list of enabled OBB pairs to check
|
||||
std::vector<std::tuple<int, int, AxisStats*>> tuplesToCheck_;
|
||||
};
|
||||
}
|
||||
#endif // COL_SIMULATOR_HPP
|
||||
@@ -0,0 +1,19 @@
|
||||
#ifndef COMMON_H
|
||||
#define COMMON_H
|
||||
|
||||
enum
|
||||
{
|
||||
UNKNOWN = 0,
|
||||
REVOLUTE,
|
||||
CONTINUOUS,
|
||||
PRISMATIC,
|
||||
FLOATING,
|
||||
PLANAR,
|
||||
FIXED
|
||||
} SimulatorJointTypeE;
|
||||
|
||||
#define GET_FUNC_LINE_STR() \
|
||||
(std::string(__func__ ) + ":" + std::to_string(__LINE__))
|
||||
#define MAX_PRE_TARGET_TRAJ_CNT 32
|
||||
|
||||
#endif // COMMON_H
|
||||
@@ -0,0 +1,72 @@
|
||||
#ifndef DATA_COLLECTOR_HPP
|
||||
#define DATA_COLLECTOR_HPP
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <sensor_msgs/msg/joint_state.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <tf2_ros/buffer.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
#include <Eigen/Dense>
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#include <fstream>
|
||||
|
||||
// 关节组信息结构体
|
||||
struct JointGroupInfo {
|
||||
std::string name; // 关节组名称
|
||||
std::string topic; // 订阅话题
|
||||
std::vector<std::string> joints; // 关节名称列表
|
||||
sensor_msgs::msg::JointState state; // 最新关节状态
|
||||
bool received; // 是否已接收
|
||||
};
|
||||
|
||||
// 机器人基座位姿信息
|
||||
struct RobotBasePose {
|
||||
Eigen::Vector3d position;
|
||||
Eigen::Quaterniond rotation;
|
||||
bool valid = false;
|
||||
};
|
||||
|
||||
class DataCollector
|
||||
{
|
||||
public:
|
||||
DataCollector(const std::string& yaml_path, const std::string& joint_data_log_path, rclcpp::Node* node);
|
||||
~DataCollector();
|
||||
|
||||
// 获取机器人基座位姿
|
||||
RobotBasePose getRobotBasePose() const { return robot_base_pose_; }
|
||||
|
||||
private:
|
||||
// 关节状态相关
|
||||
std::map<std::string, JointGroupInfo> joint_groups_;
|
||||
std::map<std::string, rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr> joint_state_subs_;
|
||||
rclcpp::Node* node_;
|
||||
std::map<std::string, std::ofstream> joint_data_files_;
|
||||
std::string joint_data_output_dir_;
|
||||
|
||||
// TF 相关
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
rclcpp::TimerBase::SharedPtr tf_timer_;
|
||||
RobotBasePose robot_base_pose_;
|
||||
std::string world_frame_;
|
||||
std::string base_frame_;
|
||||
std::ofstream base_pose_file_;
|
||||
|
||||
void loadJointControllersConfig(const std::string& yaml_path);
|
||||
void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg, const std::string& groupName);
|
||||
void writeJointDataToFile(const std::string& groupName, const sensor_msgs::msg::JointState& state);
|
||||
void initJointDataFiles(const std::string& output_dir);
|
||||
|
||||
// TF 相关方法
|
||||
void initTfListener();
|
||||
void writeBasePoseToFile();
|
||||
|
||||
// Ground truth 回调
|
||||
void posionCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
|
||||
};
|
||||
|
||||
#endif // DATA_COLLECTOR_HPP
|
||||
@@ -0,0 +1,16 @@
|
||||
#ifndef MPC_CONTROLLER_HPP
|
||||
#define MPC_CONTROLLER_HPP
|
||||
|
||||
#include <ocs2_core/Types.h>
|
||||
#include <ocs2_core/reference/TargetTrajectories.h>
|
||||
#include <ocs2_core/reference/ModeSchedule.h>
|
||||
#include <ocs2_core/Types.h>
|
||||
|
||||
class MpcController {
|
||||
public:
|
||||
MpcController(const std::string& config_file);
|
||||
~MpcController();
|
||||
|
||||
};
|
||||
|
||||
#endif // MPC_CONTROLLER_HPP
|
||||
@@ -0,0 +1,100 @@
|
||||
#ifndef REFERENCE_MANAGER_FOR_GAZEBO_HPP
|
||||
#define REFERENCE_MANAGER_FOR_GAZEBO_HPP
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include "common.h"
|
||||
#include "robot_info_loader.hpp"
|
||||
#include <ocs2_oc/synchronized_module/ReferenceManager.h>
|
||||
#include <ocs2_core/reference/TargetTrajectories.h>
|
||||
#include <ocs2_core/reference/ModeSchedule.h>
|
||||
#include <ocs2_core/Types.h>
|
||||
|
||||
struct PreTargetTraj{
|
||||
int joint_id;
|
||||
int next_point_index;
|
||||
int final_point_index;
|
||||
double position[MAX_PRE_TARGET_TRAJ_CNT];
|
||||
double aim_time[MAX_PRE_TARGET_TRAJ_CNT];
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 机器人部件配置结构体
|
||||
* 用于区分腿部和手臂的关节和接触点
|
||||
*/
|
||||
struct RobotPartConfig {
|
||||
std::vector<std::string> leg_joint_names; // 腿部关节名称
|
||||
std::vector<std::string> arm_joint_names; // 手臂关节名称
|
||||
std::vector<std::string> leg_contact_names; // 腿部接触点名称（如 "LF_FOOT"）
|
||||
std::vector<std::string> arm_contact_names; // 手臂接触点名称（如 "LEFT_HAND"）
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Gazebo 参考管理器
|
||||
* 继承自 OCS2 的 ReferenceManager，实现手部和腿部的独立管理（方案A）
|
||||
*/
|
||||
class ReferenceManagerForGazebo : public ocs2::ReferenceManager
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
* @param urdf_content URDF 文件内容
|
||||
* @param config_file 配置文件路径（可选，用于加载关节分类）
|
||||
*/
|
||||
ReferenceManagerForGazebo(const std::string& urdf_content,
|
||||
const std::string& config_file = "");
|
||||
|
||||
~ReferenceManagerForGazebo() override;
|
||||
|
||||
/**
|
||||
* @brief 获取机器人信息加载器
|
||||
*/
|
||||
const RobotInfoLoader* getRobotInfoLoader() const { return robot_info_loader_.get(); }
|
||||
|
||||
/**
|
||||
* @brief 获取配置信息
|
||||
*/
|
||||
const RobotPartConfig& getConfig() const { return config_; }
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief 修改参考轨迹和模式调度
|
||||
* 在每次 MPC 优化前调用，用于更新参考轨迹和模式调度
|
||||
*/
|
||||
void modifyReferences(ocs2::scalar_t initTime,
|
||||
ocs2::scalar_t finalTime,
|
||||
const ocs2::vector_t& initState,
|
||||
ocs2::TargetTrajectories& targetTrajectories,
|
||||
ocs2::ModeSchedule& modeSchedule) override;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 从配置文件加载关节分类
|
||||
* @param config_file 配置文件路径
|
||||
*/
|
||||
void loadConfig(const std::string& config_file);
|
||||
|
||||
/**
|
||||
* @brief 初始化手臂轨迹规划器
|
||||
*/
|
||||
void initializeArmPlanner();
|
||||
|
||||
/* 待需求 */
|
||||
ocs2::ModeSchedule generateBasicLegModeSchedule(ocs2::scalar_t initTime,
|
||||
ocs2::scalar_t finalTime,
|
||||
const ocs2::vector_t& initState);
|
||||
|
||||
void UpdateTargetTraj
|
||||
(ocs2::scalar_t initTime, ocs2::scalar_t finalTime, const ocs2::vector_t& initState, ocs2::TargetTrajectories& targetTrajectories);
|
||||
ocs2::scalar_array_t getUniqueTimePoints(ocs2::scalar_t initTime, ocs2::scalar_t finalTime);
|
||||
|
||||
// 机器人信息加载器
|
||||
std::unique_ptr<RobotInfoLoader> robot_info_loader_;
|
||||
std::vector<PreTargetTraj> pre_target_traj_list_;
|
||||
|
||||
// 配置信息
|
||||
RobotPartConfig config_;
|
||||
};
|
||||
|
||||
#endif // REFERENCE_MANAGER_FOR_GAZEBO_HPP
|
||||
@@ -0,0 +1,149 @@
|
||||
#ifndef ROBOT_INFO_LOADER_HPP
|
||||
#define ROBOT_INFO_LOADER_HPP
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <urdf/model.h>
|
||||
#include <urdf_parser/urdf_parser.h>
|
||||
#include <ocs2_pinocchio_interface/PinocchioInterface.h>
|
||||
#include <ocs2_pinocchio_interface/urdf.h>
|
||||
#include <pinocchio/fwd.hpp> // forward declarations must be included first.
|
||||
#include <pinocchio/multibody/model.hpp>
|
||||
#include "common.h"
|
||||
|
||||
/**
|
||||
* @brief 机器人信息结构体
|
||||
*/
|
||||
struct RobotInfo {
|
||||
std::string robot_name; // 机器人名称
|
||||
std::vector<std::string> joint_names; // 关节名称列表（仅包含可动关节，排除fixed）
|
||||
std::vector<std::string> link_names; // 链接名称列表
|
||||
std::vector<int> joint_types; // 关节类型列表
|
||||
std::vector<double> joint_limits_lower; // 关节下限列表
|
||||
std::vector<double> joint_limits_upper; // 关节上限列表
|
||||
std::vector<double> joint_limits_velocity; // 关节速度限制列表
|
||||
std::vector<double> joint_limits_effort; // 关节力矩限制列表
|
||||
std::map<std::string, int> joint_name_to_index; // 关节名称 -> 索引映射
|
||||
|
||||
// 基座链接名称（通常是第一个链接或root link）
|
||||
std::string base_link_name;
|
||||
|
||||
// URDF原始内容（可选，用于调试）
|
||||
std::string urdf_content;
|
||||
|
||||
int joint_count;
|
||||
};
|
||||
|
||||
class RobotInfoLoader {
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
*/
|
||||
RobotInfoLoader(const std::string& urdf_content);
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~RobotInfoLoader();
|
||||
|
||||
/**
|
||||
* @brief 获取机器人信息
|
||||
* @return 机器人信息结构体（const引用）
|
||||
*/
|
||||
const RobotInfo& getRobotInfo() const { return robot_info_; }
|
||||
|
||||
/**
|
||||
* @brief 获取URDF模型指针
|
||||
* @return URDF模型指针
|
||||
*/
|
||||
std::shared_ptr<urdf::ModelInterface> getUrdfModel() const { return urdf_model_; }
|
||||
|
||||
/**
|
||||
* @brief 获取PinocchioInterface（用于MPC控制）
|
||||
* @return PinocchioInterface引用
|
||||
* @throw std::runtime_error 如果PinocchioInterface未创建
|
||||
*/
|
||||
const ocs2::PinocchioInterface& getPinocchioInterface() const {
|
||||
if (!pinocchio_interface_.has_value()) {
|
||||
throw std::runtime_error("[RobotInfoLoader] PinocchioInterface not created yet");
|
||||
}
|
||||
return pinocchio_interface_.value();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取PinocchioInterface（非const版本，用于更新数据）
|
||||
* @return PinocchioInterface引用
|
||||
* @throw std::runtime_error 如果PinocchioInterface未创建
|
||||
*/
|
||||
ocs2::PinocchioInterface& getPinocchioInterface() {
|
||||
if (!pinocchio_interface_.has_value()) {
|
||||
throw std::runtime_error("[RobotInfoLoader] PinocchioInterface not created yet");
|
||||
}
|
||||
return pinocchio_interface_.value();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 检查是否已加载
|
||||
* @return 是否已加载
|
||||
*/
|
||||
bool isLoaded() const { return is_loaded_; }
|
||||
|
||||
/**
|
||||
* @brief 获取关节数量（仅可动关节）
|
||||
* @return 关节数量
|
||||
*/
|
||||
size_t getJointCount() const { return robot_info_.joint_names.size(); }
|
||||
|
||||
/**
|
||||
* @brief 获取链接数量
|
||||
* @return 链接数量
|
||||
*/
|
||||
size_t getLinkCount() const { return robot_info_.link_names.size(); }
|
||||
|
||||
/**
|
||||
* @brief 根据关节名称获取关节索引
|
||||
* @param joint_name 关节名称
|
||||
* @return 关节索引，如果不存在返回-1
|
||||
*/
|
||||
int getJointIndex(const std::string& joint_name) const;
|
||||
|
||||
/**
|
||||
* @brief 根据关节索引获取关节名称
|
||||
* @param joint_index 关节索引
|
||||
* @return 关节名称，如果不存在返回空字符串
|
||||
*/
|
||||
std::string getJointName(int joint_index) const;
|
||||
|
||||
/**
|
||||
* @brief 打印机器人信息（用于调试）
|
||||
*/
|
||||
void printRobotInfo() const;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 解析URDF模型并提取信息
|
||||
* @return 是否解析成功
|
||||
*/
|
||||
bool parseUrdfModel();
|
||||
|
||||
/**
|
||||
* @brief 创建PinocchioInterface
|
||||
* @param joint_names 可选的关节名称列表，用于过滤关节（如果为空则使用所有关节）
|
||||
*/
|
||||
void createPinocchioInterface();
|
||||
|
||||
/**
|
||||
* @brief 清理已加载的数据
|
||||
*/
|
||||
void clear();
|
||||
|
||||
std::shared_ptr<urdf::ModelInterface> urdf_model_; // URDF模型
|
||||
RobotInfo robot_info_; // 机器人信息
|
||||
std::optional<ocs2::PinocchioInterface> pinocchio_interface_; // Pinocchio接口（用于MPC控制，延迟初始化）
|
||||
bool is_loaded_; // 是否已加载
|
||||
};
|
||||
|
||||
#endif // ROBOT_INFO_LOADER_HPP
|
||||
@@ -0,0 +1,121 @@
|
||||
#ifndef ROBOT_SIMULATOR_HPP
|
||||
#define ROBOT_SIMULATOR_HPP
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
#include <sensor_msgs/msg/joint_state.hpp>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include <std_msgs/msg/float64_multi_array.hpp>
|
||||
#include <interfaces/msg/img_msg.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <ament_index_cpp/get_package_share_directory.hpp>
|
||||
#include "data_collector.hpp"
|
||||
#include "reference_manager_for_gazebo.hpp"
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <array>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
enum ImageType {
|
||||
IMAGE_TYPE_RGB,
|
||||
IMAGE_TYPE_DEPTH,
|
||||
IMAGE_TYPE_CNT,
|
||||
};
|
||||
|
||||
enum CameraType {
|
||||
CAMERA_TYPE_HEAD,
|
||||
CAMERA_TYPE_LEFT_ARM,
|
||||
CAMERA_TYPE_RIGHT_ARM,
|
||||
CAMERA_TYPE_CNT,
|
||||
};
|
||||
|
||||
struct JointEffort {
|
||||
double max_effort;
|
||||
double min_effort;
|
||||
double avg_effort;
|
||||
double first_effort;
|
||||
double effort_sum;
|
||||
double position_max;
|
||||
double position_min;
|
||||
int get_cnt;
|
||||
};
|
||||
|
||||
class RobotSimulator : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
RobotSimulator();
|
||||
~RobotSimulator();
|
||||
|
||||
private:
|
||||
|
||||
void GetImageCallback(const sensor_msgs::msg::Image::SharedPtr msg, int cameraType, int imageType);
|
||||
|
||||
// CameraInfo 回调函数
|
||||
void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr msg, int cameraType, int cameraInfoType);
|
||||
|
||||
// 发布 ImgMsg 消息
|
||||
void publishImgMsg(int cameraType);
|
||||
|
||||
// ImgMsg 发布者
|
||||
rclcpp::Publisher<interfaces::msg::ImgMsg>::SharedPtr img_msg_pub_;
|
||||
|
||||
// Head camera 订阅者
|
||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr head_rgb_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr head_depth_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr head_rgb_info_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr head_depth_info_sub_;
|
||||
|
||||
// Left arm camera 订阅者
|
||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr left_arm_rgb_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr left_arm_depth_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr left_arm_rgb_info_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr left_arm_depth_info_sub_;
|
||||
|
||||
// Right arm camera 订阅者
|
||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr right_arm_rgb_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr right_arm_depth_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr right_arm_rgb_info_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr right_arm_depth_info_sub_;
|
||||
|
||||
// IMU 订阅者 - 用于监测 base_link 受力情况
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr base_link_imu_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr body_link_imu_sub_;
|
||||
|
||||
// IMU 回调函数
|
||||
void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg, const char* linkName, bool boardcast_start);
|
||||
|
||||
// ========== 关节力矩监测 ==========
|
||||
// 关节状态订阅者
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr left_arm_effort_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr right_arm_effort_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr body_effort_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr left_gripper_effort_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr right_gripper_effort_sub_;
|
||||
|
||||
std::map<std::string, JointEffort> joint_effort_map_;
|
||||
|
||||
// 关节力矩回调函数
|
||||
void jointEffortCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
|
||||
|
||||
sensor_msgs::msg::Image image_[CAMERA_TYPE_CNT][IMAGE_TYPE_CNT];
|
||||
int image_count_[CAMERA_TYPE_CNT][IMAGE_TYPE_CNT];
|
||||
|
||||
// 相机内参 K 矩阵 (3x3 = 9 个元素)
|
||||
std::array<double, 9> karr_[CAMERA_TYPE_CNT][IMAGE_TYPE_CNT];
|
||||
// 畸变系数 D 数组 (通常 5 个元素)
|
||||
std::vector<double> darr_[CAMERA_TYPE_CNT][IMAGE_TYPE_CNT];
|
||||
bool camera_info_received_[CAMERA_TYPE_CNT][IMAGE_TYPE_CNT];
|
||||
DataCollector *data_collector_;
|
||||
std::mutex boardcast_mutex_;
|
||||
|
||||
int baseLinkCallbackCnt;
|
||||
int bodyLinkCallbackCnt;
|
||||
|
||||
std::unique_ptr<ReferenceManagerForGazebo> reference_manager_for_gazebo_;
|
||||
};
|
||||
|
||||
#endif // ROBOT_SIMULATOR_HPP
|
||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,273 @@
|
||||
# Gazebo 仿真启动文件 - 带摄像头视觉
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
IncludeLaunchDescription,
|
||||
TimerAction,
|
||||
SetEnvironmentVariable,
|
||||
)
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.actions import OpaqueFunction
|
||||
import os
|
||||
import re
|
||||
|
||||
|
||||
def open_file(file_name, context):
|
||||
try:
|
||||
package_share = FindPackageShare(package='robot_simulator').perform(context)
|
||||
# 1. 计算SRDF文件路径并解析
|
||||
file_path = PathJoinSubstitution(
|
||||
[package_share, 'urdf', file_name] # 替换为实际的SRDF路径
|
||||
).perform(context) # context 是LaunchContext对象，通常在generate_launch_description中可用
|
||||
|
||||
# 2. 检查路径是否存在
|
||||
if not os.path.exists(file_path):
|
||||
raise FileNotFoundError(f"SRDF文件不存在: {file_path}")
|
||||
|
||||
# 3. 检查路径是否为文件（避免目录路径）
|
||||
if not os.path.isfile(file_path):
|
||||
raise IsADirectoryError(f"指定路径是目录，不是文件: {file_path}")
|
||||
|
||||
# 4. 尝试打开并读取文件
|
||||
with open(file_path, 'r') as f:
|
||||
file_content = f.read()
|
||||
|
||||
# 5. 检查文件内容是否为空
|
||||
if not file_content.strip(): # 忽略纯空白内容
|
||||
raise ValueError(f"文件内容为空: {file_path}")
|
||||
|
||||
# 6. （可选）简单验证XML格式是否合法（检查根节点）
|
||||
if '<robot' not in file_content:
|
||||
raise SyntaxError(f"文件格式无效，未找到<robot>根节点: {file_path}")
|
||||
|
||||
# 读取成功后的逻辑（如加载到参数服务器）
|
||||
print(f"成功读取SRDF文件: {file_path}")
|
||||
|
||||
return file_content # 返回读取的内容，或根据需要处理
|
||||
|
||||
except FileNotFoundError as e:
|
||||
# 文件不存在错误
|
||||
print(f"错误: {str(e)}")
|
||||
print("请检查路径是否正确，确保SRDF文件存在于指定目录")
|
||||
# 可根据需要抛出异常终止launch，或使用return None等方式处理
|
||||
raise # 抛出异常终止launch启动
|
||||
|
||||
except IsADirectoryError as e:
|
||||
# 路径是目录而非文件
|
||||
print(f"错误: {str(e)}")
|
||||
print("请检查路径是否指向具体的SRDF文件，而非目录")
|
||||
raise
|
||||
|
||||
except PermissionError:
|
||||
# 无权限读取文件
|
||||
print(f"错误: 没有权限读取SRDF文件: {file_path}")
|
||||
print("请检查文件权限，确保当前用户有读取权限")
|
||||
raise
|
||||
|
||||
except ValueError as e:
|
||||
# 文件内容为空
|
||||
print(f"错误: {str(e)}")
|
||||
print("请检查SRDF文件是否被正确生成，内容不能为空")
|
||||
raise
|
||||
|
||||
except SyntaxError as e:
|
||||
# XML格式初步验证失败
|
||||
print(f"错误: {str(e)}")
|
||||
print("请检查SRDF文件是否为有效的XML格式")
|
||||
raise
|
||||
|
||||
except Exception as e:
|
||||
# 捕获其他未预料的错误
|
||||
print(f"读取SRDF文件时发生未知错误: {str(e)}")
|
||||
raise
|
||||
|
||||
def load_robot_with_gazebo(context, *args, **kwargs):
|
||||
"""加载机器人模型并启动 Gazebo 仿真"""
|
||||
|
||||
# 获取启动参数
|
||||
joint_data_log_path = LaunchConfiguration('joint_data_log_path').perform(context)
|
||||
joint_controllers_yaml = LaunchConfiguration('joint_controllers_yaml').perform(context)
|
||||
world_name = LaunchConfiguration('world').perform(context)
|
||||
|
||||
# 展开 ~ 为用户 home 目录
|
||||
if joint_data_log_path.startswith('~'):
|
||||
joint_data_log_path = os.path.expanduser(joint_data_log_path)
|
||||
if joint_controllers_yaml.startswith('~'):
|
||||
joint_controllers_yaml = os.path.expanduser(joint_controllers_yaml)
|
||||
|
||||
# 获取包路径
|
||||
package_share = FindPackageShare(package='robot_simulator').perform(context)
|
||||
|
||||
# 如果 yaml 路径为空，使用默认路径
|
||||
if not joint_controllers_yaml:
|
||||
joint_controllers_yaml = os.path.join(package_share, 'config', 'joint_controllers.yaml')
|
||||
|
||||
# 读取带摄像头的 URDF 文件
|
||||
urdf_file = os.path.join(package_share, 'urdf', 'FHrobot_with_camera.urdf')
|
||||
|
||||
with open(urdf_file, 'r') as f:
|
||||
urdf_content = f.read()
|
||||
|
||||
# 移除 XML 声明（spawn_entity.py 不支持带 encoding 声明的 XML）
|
||||
urdf_content = re.sub(r'<\?xml[^?]*\?>\s*', '', urdf_content)
|
||||
# 移除注释
|
||||
urdf_content = re.sub(r'<!--.*?-->', '', urdf_content, flags=re.DOTALL)
|
||||
# 移除多余空白字符（避免换行符导致参数解析错误）
|
||||
urdf_content = re.sub(r'\s+', ' ', urdf_content).strip()
|
||||
|
||||
# 将 package:// URI 替换为实际文件路径（Gazebo 需要）
|
||||
urdf_content = urdf_content.replace(
|
||||
'package://robot_simulator/',
|
||||
f'file://{package_share}/'
|
||||
)
|
||||
|
||||
# 替换 $(find robot_simulator)/config/joint_controllers.yaml 为实际路径
|
||||
# 使用传入的 joint_controllers_yaml 参数
|
||||
urdf_content = urdf_content.replace('$(find robot_simulator)/config/joint_controllers.yaml', joint_controllers_yaml)
|
||||
|
||||
print(f"已加载 URDF 文件: {urdf_file}")
|
||||
print(f"使用控制器配置文件: {joint_controllers_yaml}")
|
||||
|
||||
# 获取世界文件路径
|
||||
world_file = os.path.join(package_share, 'worlds', f'{world_name}.world')
|
||||
print(f"使用世界文件: {world_file}")
|
||||
|
||||
# 1. Robot State Publisher - 发布处理过的 URDF（覆盖 robot_simulator 的发布）
|
||||
robot_state_publisher = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'robot_description': urdf_content,
|
||||
'use_sim_time': True
|
||||
}]
|
||||
)
|
||||
|
||||
# 2. 启动 Gazebo 仿真器
|
||||
gazebo_args = {
|
||||
'verbose': 'true',
|
||||
'pause': 'false'
|
||||
}
|
||||
|
||||
if os.path.exists(world_file):
|
||||
gazebo_args['world'] = world_file
|
||||
print(f"使用世界文件: {world_file}")
|
||||
|
||||
gazebo = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('gazebo_ros'),
|
||||
'launch',
|
||||
'gazebo.launch.py'
|
||||
])
|
||||
]),
|
||||
launch_arguments=gazebo_args.items()
|
||||
)
|
||||
|
||||
# 3. 在 Gazebo 中生成机器人模型（延迟5秒等待 Gazebo 启动）
|
||||
spawn_entity = Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
name='spawn_entity',
|
||||
output='screen',
|
||||
arguments=[
|
||||
'-file', '/home/zj/hivecore/hivecore_robot_simulator/robot_simulator/urdf/no_controller_fhrobot.sdf',
|
||||
'-entity', 'fhrobot',
|
||||
'-x', '0.0',
|
||||
'-y', '0.0',
|
||||
'-z', '0.72', # base_link 中心点离地高度
|
||||
'-timeout', '120' # 增加超时时间
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
|
||||
# 使用 TimerAction 延迟 spawn
|
||||
delayed_spawn = TimerAction(
|
||||
period=5.0, # 延迟5秒
|
||||
actions=[spawn_entity]
|
||||
)
|
||||
|
||||
# 4. RobotSimulator 节点 - 接收图像和关节状态，记录数据
|
||||
robot_simulator_node = Node(
|
||||
package='robot_simulator',
|
||||
executable='robot_simulator',
|
||||
name='robot_simulator',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': False,
|
||||
'joint_data_log_path': joint_data_log_path,
|
||||
'joint_controllers_yaml': joint_controllers_yaml,
|
||||
'robot_description': urdf_content # 传递URDF内容给节点
|
||||
}]
|
||||
)
|
||||
|
||||
# 延迟启动 RobotSimulator，等待 Gazebo 和控制器准备好
|
||||
delayed_robot_simulator = TimerAction(
|
||||
period=10.0, # 延迟10秒，确保控制器和相机已启动
|
||||
actions=[robot_simulator_node]
|
||||
)
|
||||
|
||||
return [
|
||||
robot_state_publisher,
|
||||
gazebo,
|
||||
delayed_spawn, # 延迟启动 spawn
|
||||
# RobotSimulator 节点
|
||||
delayed_robot_simulator,
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
# 设置 Gazebo 插件路径
|
||||
gazebo_plugin_path = '/opt/ros/humble/lib:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins'
|
||||
existing_plugin_path = os.environ.get('GAZEBO_PLUGIN_PATH', '')
|
||||
if existing_plugin_path:
|
||||
full_plugin_path = f"{gazebo_plugin_path}:{existing_plugin_path}"
|
||||
else:
|
||||
full_plugin_path = gazebo_plugin_path
|
||||
|
||||
# 设置动态库搜索路径（libCameraPlugin.so 等依赖）
|
||||
ld_library_path = '/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/lib'
|
||||
existing_ld_path = os.environ.get('LD_LIBRARY_PATH', '')
|
||||
if existing_ld_path:
|
||||
full_ld_path = f"{ld_library_path}:{existing_ld_path}"
|
||||
else:
|
||||
full_ld_path = ld_library_path
|
||||
|
||||
return LaunchDescription([
|
||||
# 设置 Gazebo 插件路径环境变量
|
||||
SetEnvironmentVariable('GAZEBO_PLUGIN_PATH', full_plugin_path),
|
||||
# 设置动态库搜索路径
|
||||
SetEnvironmentVariable('LD_LIBRARY_PATH', full_ld_path),
|
||||
|
||||
# 声明启动参数
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='true',
|
||||
description='使用 Gazebo 仿真时间'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'joint_data_log_path',
|
||||
default_value='~/hivecore/simulator_log',
|
||||
description='关节数据日志输出目录'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'joint_controllers_yaml',
|
||||
default_value='',
|
||||
description='关节控制器配置文件路径 (空则使用默认: robot_simulator/config/joint_controllers.yaml)'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'world',
|
||||
default_value='camera_test',
|
||||
description='Gazebo 世界文件名 (不含 .world 后缀): camera_test, grasp_test'
|
||||
),
|
||||
|
||||
# 加载机器人和启动仿真
|
||||
OpaqueFunction(function=load_robot_with_gazebo)
|
||||
])
|
||||
@@ -0,0 +1,492 @@
|
||||
# Gazebo 仿真启动文件 - 带摄像头视觉
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
IncludeLaunchDescription,
|
||||
TimerAction,
|
||||
SetEnvironmentVariable,
|
||||
RegisterEventHandler,
|
||||
)
|
||||
from launch.event_handlers import OnProcessExit, OnProcessStart
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.actions import OpaqueFunction
|
||||
from controller_manager_msgs.srv import SwitchController
|
||||
from launch.actions import ExecuteProcess
|
||||
import rclpy
|
||||
import os
|
||||
import re
|
||||
|
||||
start_node = None
|
||||
start_future = None
|
||||
target_controllers = [
|
||||
"left_arm_position_controller", "left_arm_state_broadcaster",
|
||||
"right_arm_position_controller", "right_arm_state_broadcaster",
|
||||
"left_gripper_effort_controller", "left_gripper_state_broadcaster",
|
||||
"right_gripper_effort_controller", "right_gripper_state_broadcaster",
|
||||
"body_position_controller", "body_state_broadcaster"
|
||||
]
|
||||
|
||||
def open_file(file_name, context):
|
||||
try:
|
||||
package_share = FindPackageShare(package='robot_simulator').perform(context)
|
||||
# 1. 计算SRDF文件路径并解析
|
||||
file_path = PathJoinSubstitution(
|
||||
[package_share, 'urdf', file_name] # 替换为实际的SRDF路径
|
||||
).perform(context) # context 是LaunchContext对象，通常在generate_launch_description中可用
|
||||
|
||||
# 2. 检查路径是否存在
|
||||
if not os.path.exists(file_path):
|
||||
raise FileNotFoundError(f"SRDF文件不存在: {file_path}")
|
||||
|
||||
# 3. 检查路径是否为文件（避免目录路径）
|
||||
if not os.path.isfile(file_path):
|
||||
raise IsADirectoryError(f"指定路径是目录，不是文件: {file_path}")
|
||||
|
||||
# 4. 尝试打开并读取文件
|
||||
with open(file_path, 'r') as f:
|
||||
file_content = f.read()
|
||||
|
||||
# 5. 检查文件内容是否为空
|
||||
if not file_content.strip(): # 忽略纯空白内容
|
||||
raise ValueError(f"文件内容为空: {file_path}")
|
||||
|
||||
# 6. （可选）简单验证XML格式是否合法（检查根节点）
|
||||
if '<robot' not in file_content:
|
||||
raise SyntaxError(f"文件格式无效，未找到<robot>根节点: {file_path}")
|
||||
|
||||
# 读取成功后的逻辑（如加载到参数服务器）
|
||||
print(f"成功读取SRDF文件: {file_path}")
|
||||
|
||||
return file_content # 返回读取的内容，或根据需要处理
|
||||
|
||||
except FileNotFoundError as e:
|
||||
# 文件不存在错误
|
||||
print(f"错误: {str(e)}")
|
||||
print("请检查路径是否正确，确保SRDF文件存在于指定目录")
|
||||
# 可根据需要抛出异常终止launch，或使用return None等方式处理
|
||||
raise # 抛出异常终止launch启动
|
||||
|
||||
except IsADirectoryError as e:
|
||||
# 路径是目录而非文件
|
||||
print(f"错误: {str(e)}")
|
||||
print("请检查路径是否指向具体的SRDF文件，而非目录")
|
||||
raise
|
||||
|
||||
except PermissionError:
|
||||
# 无权限读取文件
|
||||
print(f"错误: 没有权限读取SRDF文件: {file_path}")
|
||||
print("请检查文件权限，确保当前用户有读取权限")
|
||||
raise
|
||||
|
||||
except ValueError as e:
|
||||
# 文件内容为空
|
||||
print(f"错误: {str(e)}")
|
||||
print("请检查SRDF文件是否被正确生成，内容不能为空")
|
||||
raise
|
||||
|
||||
except SyntaxError as e:
|
||||
# XML格式初步验证失败
|
||||
print(f"错误: {str(e)}")
|
||||
print("请检查SRDF文件是否为有效的XML格式")
|
||||
raise
|
||||
|
||||
except Exception as e:
|
||||
# 捕获其他未预料的错误
|
||||
print(f"读取SRDF文件时发生未知错误: {str(e)}")
|
||||
raise
|
||||
|
||||
def load_robot_with_gazebo(context, *args, **kwargs):
|
||||
"""加载机器人模型并启动 Gazebo 仿真"""
|
||||
|
||||
# 获取启动参数
|
||||
joint_data_log_path = LaunchConfiguration('joint_data_log_path').perform(context)
|
||||
joint_controllers_yaml = LaunchConfiguration('joint_controllers_yaml').perform(context)
|
||||
world_name = LaunchConfiguration('world').perform(context)
|
||||
|
||||
# 展开 ~ 为用户 home 目录
|
||||
if joint_data_log_path.startswith('~'):
|
||||
joint_data_log_path = os.path.expanduser(joint_data_log_path)
|
||||
if joint_controllers_yaml.startswith('~'):
|
||||
joint_controllers_yaml = os.path.expanduser(joint_controllers_yaml)
|
||||
|
||||
# 获取包路径
|
||||
package_share = FindPackageShare(package='robot_simulator').perform(context)
|
||||
|
||||
# 如果 yaml 路径为空，使用默认路径
|
||||
if not joint_controllers_yaml:
|
||||
joint_controllers_yaml = os.path.join(package_share, 'config', 'joint_controllers.yaml')
|
||||
|
||||
# 读取带摄像头的 URDF 文件
|
||||
urdf_file = os.path.join(package_share, 'urdf', 'FHrobot_with_camera.urdf')
|
||||
|
||||
with open(urdf_file, 'r') as f:
|
||||
urdf_content = f.read()
|
||||
|
||||
# 移除 XML 声明（spawn_entity.py 不支持带 encoding 声明的 XML）
|
||||
urdf_content = re.sub(r'<\?xml[^?]*\?>\s*', '', urdf_content)
|
||||
# 移除注释
|
||||
urdf_content = re.sub(r'<!--.*?-->', '', urdf_content, flags=re.DOTALL)
|
||||
# 移除多余空白字符（避免换行符导致参数解析错误）
|
||||
urdf_content = re.sub(r'\s+', ' ', urdf_content).strip()
|
||||
|
||||
# 将 package:// URI 替换为实际文件路径（Gazebo 需要）
|
||||
urdf_content = urdf_content.replace(
|
||||
'package://robot_simulator/',
|
||||
f'file://{package_share}/'
|
||||
)
|
||||
|
||||
# 替换 $(find robot_simulator)/config/joint_controllers.yaml 为实际路径
|
||||
# 使用传入的 joint_controllers_yaml 参数
|
||||
urdf_content = urdf_content.replace('$(find robot_simulator)/config/joint_controllers.yaml', joint_controllers_yaml)
|
||||
|
||||
print(f"已加载 URDF 文件: {urdf_file}")
|
||||
print(f"使用控制器配置文件: {joint_controllers_yaml}")
|
||||
|
||||
# 检查 URDF 中的硬件插件类型
|
||||
import re as regex
|
||||
plugin_match = regex.search(r'<plugin>([^<]+GazeboSystem[^<]*)</plugin>', urdf_content)
|
||||
if plugin_match:
|
||||
plugin_name = plugin_match.group(1)
|
||||
print(f"[DEBUG] URDF 中的硬件插件: {plugin_name}")
|
||||
if 'my_gazebo_ros2_control' in plugin_name:
|
||||
print("[DEBUG] ✓ 插件名称正确: my_gazebo_ros2_control/GazeboSystem")
|
||||
else:
|
||||
print(f"[DEBUG] ✗ 警告: 插件名称不正确! 期望 my_gazebo_ros2_control/GazeboSystem")
|
||||
else:
|
||||
print("[DEBUG] ✗ 未在 URDF 中找到 GazeboSystem 插件定义")
|
||||
|
||||
# 获取世界文件路径
|
||||
world_file = os.path.join(package_share, 'worlds', f'{world_name}.world')
|
||||
print(f"使用世界文件: {world_file}")
|
||||
|
||||
# 1. Robot State Publisher - 发布处理过的 URDF（覆盖 robot_simulator 的发布）
|
||||
robot_state_publisher = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'robot_description': urdf_content,
|
||||
'use_sim_time': True
|
||||
}]
|
||||
)
|
||||
|
||||
# 2. 启动 Gazebo 仿真器
|
||||
gazebo_args = {
|
||||
'verbose': 'true',
|
||||
'pause': 'true',
|
||||
# 'extra_gazebo_args': '--record'
|
||||
}
|
||||
|
||||
if os.path.exists(world_file):
|
||||
gazebo_args['world'] = world_file
|
||||
print(f"使用世界文件: {world_file}")
|
||||
|
||||
gazebo = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('gazebo_ros'),
|
||||
'launch',
|
||||
'gazebo.launch.py'
|
||||
])
|
||||
]),
|
||||
launch_arguments=gazebo_args.items()
|
||||
)
|
||||
|
||||
# 3. 在 Gazebo 中生成机器人模型（延迟5秒等待 Gazebo 启动）
|
||||
spawn_entity = Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
name='spawn_entity',
|
||||
output='screen',
|
||||
arguments=[
|
||||
'-file', '/home/zj/hivecore/hivecore_robot_simulator/robot_simulator/urdf/fhrobot.sdf',
|
||||
'-entity', 'fhrobot',
|
||||
'-x', '-1.0',
|
||||
'-y', '0.0',
|
||||
'-z', '0.72', # base_link 中心点离地高度
|
||||
'-timeout', '120' # 增加超时时间
|
||||
]
|
||||
)
|
||||
|
||||
# 使用 TimerAction 延迟 spawn
|
||||
delayed_spawn = TimerAction(
|
||||
period=5.0, # 延迟5秒
|
||||
actions=[spawn_entity]
|
||||
)
|
||||
|
||||
load_all_state_broadcasters_controllers = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"left_arm_state_broadcaster",
|
||||
"right_arm_state_broadcaster",
|
||||
"left_gripper_state_broadcaster",
|
||||
"right_gripper_state_broadcaster",
|
||||
"body_state_broadcaster",
|
||||
"left_arm_position_controller",
|
||||
"right_arm_position_controller",
|
||||
"left_gripper_effort_controller",
|
||||
"right_gripper_effort_controller",
|
||||
"body_position_controller",
|
||||
"--controller-manager", "/controller_manager",
|
||||
"--controller-manager-timeout", "60",
|
||||
"--inactive"
|
||||
],
|
||||
output="screen"
|
||||
)
|
||||
|
||||
unpause_gazebo_task = ExecuteProcess(
|
||||
cmd=[
|
||||
"bash",
|
||||
"-lc",
|
||||
"echo 'unpausing gazebo';"
|
||||
"ros2 service call /unpause_physics std_srvs/srv/Empty {} || echo 'unpause_physics service call failed';"
|
||||
"echo 'unpausing gazebo done'",
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
unpause_gazebo_task2 = ExecuteProcess(
|
||||
cmd=[
|
||||
"bash",
|
||||
"-lc",
|
||||
"echo 'unpausing gazebo';"
|
||||
"ros2 service call /unpause_physics std_srvs/srv/Empty {} || echo 'unpause_physics service call failed';"
|
||||
"echo 'unpausing gazebo done'",
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
# 使用可被 OnProcessExit 监听的进程型 action，避免 OpaqueFunction 作为 target_action 不触发的问题
|
||||
# 注意：这里容错处理（wait/call 失败不阻塞后续链路）
|
||||
pause_gazebo_task = ExecuteProcess(
|
||||
cmd=[
|
||||
"bash",
|
||||
"-lc",
|
||||
"echo 'pausing gazebo';"
|
||||
"ros2 service call /pause_physics std_srvs/srv/Empty {} || echo 'pause_physics service call failed';"
|
||||
"echo 'pausing gazebo done'",
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
sleep_task = ExecuteProcess(
|
||||
cmd=[
|
||||
"bash",
|
||||
"-lc",
|
||||
"echo 'sleeping 0.5s...';"
|
||||
"sleep 0.5s;",
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
activate_controllers_task = ExecuteProcess(
|
||||
cmd=[
|
||||
"bash", "-lc",
|
||||
"echo 'Activating all controllers...';"
|
||||
"ros2 control switch_controllers "
|
||||
"--activate "
|
||||
"left_arm_position_controller left_arm_state_broadcaster "
|
||||
"right_arm_position_controller right_arm_state_broadcaster "
|
||||
"left_gripper_effort_controller left_gripper_state_broadcaster "
|
||||
"right_gripper_effort_controller right_gripper_state_broadcaster "
|
||||
"body_position_controller body_state_broadcaster "
|
||||
"--controller-manager /controller_manager;"
|
||||
"echo 'All controllers activated!'",
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
|
||||
# 串行化启动顺序（关键）：spawn 完成 -> 加载 state broadcasters -> 加载 controllers -> pause_gazebo -> 激活并解除Gazebo暂停
|
||||
start_state_broadcasters_after_spawn = RegisterEventHandler(
|
||||
OnProcessExit(
|
||||
target_action=spawn_entity,
|
||||
on_exit=[load_all_state_broadcasters_controllers],
|
||||
)
|
||||
)
|
||||
|
||||
activate_controllers_after_broadcasters_loaded = RegisterEventHandler(
|
||||
OnProcessExit(
|
||||
target_action=load_all_state_broadcasters_controllers,
|
||||
on_exit=[activate_controllers_task],
|
||||
)
|
||||
)
|
||||
|
||||
unpause_gazebo_after_controllers_loaded = RegisterEventHandler(
|
||||
OnProcessExit(
|
||||
target_action=load_all_state_broadcasters_controllers,
|
||||
on_exit=[unpause_gazebo_task],
|
||||
)
|
||||
)
|
||||
|
||||
sleep_after_unpause_gazebo = RegisterEventHandler(
|
||||
OnProcessExit(
|
||||
target_action=unpause_gazebo_task,
|
||||
on_exit=[sleep_task],
|
||||
)
|
||||
)
|
||||
|
||||
pause_gazebo_after_sleep = RegisterEventHandler(
|
||||
OnProcessExit(
|
||||
target_action=sleep_task,
|
||||
on_exit=[pause_gazebo_task],
|
||||
)
|
||||
)
|
||||
|
||||
send_start_command_after_pause_gazebo = RegisterEventHandler(
|
||||
OnProcessExit(
|
||||
target_action=activate_controllers_task,
|
||||
on_exit=[unpause_gazebo_task2],
|
||||
)
|
||||
)
|
||||
|
||||
# 4. RobotSimulator 节点 - 接收图像和关节状态，记录数据
|
||||
robot_simulator_node = Node(
|
||||
package='robot_simulator',
|
||||
executable='robot_simulator',
|
||||
name='robot_simulator',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': False,
|
||||
'joint_data_log_path': joint_data_log_path,
|
||||
'joint_controllers_yaml': joint_controllers_yaml,
|
||||
'robot_description': urdf_content # 传递URDF内容给节点
|
||||
}]
|
||||
)
|
||||
|
||||
# 延迟启动 RobotSimulator，等待 Gazebo 和控制器准备好
|
||||
delayed_robot_simulator = TimerAction(
|
||||
period=20.0, # 延迟10秒，确保控制器和相机已启动
|
||||
actions=[robot_simulator_node]
|
||||
)
|
||||
|
||||
return [
|
||||
SetEnvironmentVariable('ROS2_LOG_SEVERITY', 'DEBUG'),
|
||||
robot_state_publisher,
|
||||
gazebo,
|
||||
delayed_spawn, # 延迟启动 spawn
|
||||
start_state_broadcasters_after_spawn,
|
||||
activate_controllers_after_broadcasters_loaded,
|
||||
unpause_gazebo_after_controllers_loaded,
|
||||
sleep_after_unpause_gazebo,
|
||||
pause_gazebo_after_sleep,
|
||||
send_start_command_after_pause_gazebo,
|
||||
delayed_robot_simulator,
|
||||
]
|
||||
|
||||
def activate_controllers(context, *args, **kwargs):
|
||||
if start_node is None:
|
||||
rclpy.init(args=[])
|
||||
start_node = rclpy.create_node("controller_activation_node")
|
||||
# 创建switch_controller服务客户端
|
||||
switch_client = start_node.create_client(SwitchController, "/controller_manager/switch_controller")
|
||||
|
||||
# 1. 等待服务就绪（容错：最多等待10秒）
|
||||
if not switch_client.wait_for_service(timeout_sec=30.0):
|
||||
start_node.get_logger().error("切换控制器服务未就绪，30秒超时！")
|
||||
start_node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
start_node = None
|
||||
# 重试激活（2秒后重新尝试）
|
||||
return [TimerAction(period=2.0, actions=[OpaqueFunction(function=activate_controllers)])]
|
||||
|
||||
# 2. 构造规范的服务请求（适配ROS 2 Humble格式）
|
||||
request = SwitchController.Request()
|
||||
request.activate_controllers = target_controllers # 批量激活目标控制器
|
||||
request.stop_controllers = [] # 无需停止其他控制器
|
||||
request.strictness = SwitchController.Request.STRICT # 严格模式：一个激活失败则整体失败
|
||||
request.activate_asap = True # 立即激活
|
||||
request.timeout.sec = 60
|
||||
|
||||
# 3. 发送服务请求并等待结果
|
||||
start_future = switch_client.call_async(request)
|
||||
|
||||
return []
|
||||
|
||||
# 激活所有控制器并解除Gazebo暂停
|
||||
def start_simulation(context, *args, **kwargs):
|
||||
if start_node is None or start_future is None:
|
||||
return [TimerAction(period=2.0, actions=[OpaqueFunction(function=activate_controllers)])]
|
||||
|
||||
rclpy.spin_until_future_complete(start_node, start_future)
|
||||
|
||||
# 4. 结果处理与二次校验
|
||||
if start_future.result() and start_future.result().ok:
|
||||
start_node.get_logger().info("所有控制器激活成功！")
|
||||
else:
|
||||
start_node.get_logger().error(f"控制器激活失败：{start_future.exception()}")
|
||||
start_node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
start_node = None
|
||||
start_future = None
|
||||
# 重试激活（2秒后重新尝试）
|
||||
return [TimerAction(period=2.0, actions=[OpaqueFunction(function=start_simulation)])]
|
||||
|
||||
# 清理节点资源
|
||||
start_node.destroy_node()
|
||||
start_node = None
|
||||
start_future = None
|
||||
rclpy.shutdown()
|
||||
return []
|
||||
|
||||
def generate_launch_description():
|
||||
# 设置 Gazebo 插件路径
|
||||
gazebo_plugin_path = '/home/zj/hivecore/install/my_gazebo_ros2_control/lib:/opt/ros/humble/lib:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins'
|
||||
existing_plugin_path = os.environ.get('GAZEBO_PLUGIN_PATH', '')
|
||||
if existing_plugin_path:
|
||||
full_plugin_path = f"{gazebo_plugin_path}:{existing_plugin_path}"
|
||||
else:
|
||||
full_plugin_path = gazebo_plugin_path
|
||||
|
||||
# 设置动态库搜索路径（libCameraPlugin.so 等依赖）
|
||||
ld_library_path = '/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/lib'
|
||||
existing_ld_path = os.environ.get('LD_LIBRARY_PATH', '')
|
||||
if existing_ld_path:
|
||||
full_ld_path = f"{ld_library_path}:{existing_ld_path}"
|
||||
else:
|
||||
full_ld_path = ld_library_path
|
||||
|
||||
return LaunchDescription([
|
||||
# 设置 Gazebo 插件路径环境变量
|
||||
SetEnvironmentVariable('GAZEBO_PLUGIN_PATH', full_plugin_path),
|
||||
# 设置动态库搜索路径
|
||||
SetEnvironmentVariable('LD_LIBRARY_PATH', full_ld_path),
|
||||
|
||||
# 物理引擎调试环境变量
|
||||
SetEnvironmentVariable('GAZEBO_VERBOSE', '1'), # 开启 Gazebo 详细输出
|
||||
SetEnvironmentVariable('GAZEBO_DEBUG', '1'), # 开启调试模式（含物理引擎信息）
|
||||
SetEnvironmentVariable('ODE_DEBUG', '1'), # ODE 物理引擎调试输出
|
||||
|
||||
# 声明启动参数
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='true',
|
||||
description='使用 Gazebo 仿真时间'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'joint_data_log_path',
|
||||
default_value='~/hivecore/simulator_log',
|
||||
description='关节数据日志输出目录'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'joint_controllers_yaml',
|
||||
default_value='',
|
||||
description='关节控制器配置文件路径 (空则使用默认: robot_simulator/config/joint_controllers.yaml)'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'world',
|
||||
default_value='camera_test',
|
||||
description='Gazebo 世界文件名 (不含 .world 后缀): camera_test, grasp_test'
|
||||
),
|
||||
|
||||
# 加载机器人和启动仿真
|
||||
OpaqueFunction(function=load_robot_with_gazebo)
|
||||
])
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,43 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>robot_simulator</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Robot simulator with ros2_control (without Gazebo)</description>
|
||||
<maintainer email="zhengjie@hivecore.cn">zj</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<!-- ROS2 dependencies -->
|
||||
<build_depend>ament_cmake</build_depend>
|
||||
<depend>interfaces</depend>
|
||||
<depend>gjk_interface</depend>
|
||||
<exec_depend>rclcpp</exec_depend>
|
||||
<exec_depend>rclcpp_action</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
<exec_depend>ros2launch</exec_depend>
|
||||
<exec_depend>ros2_controllers</exec_depend>
|
||||
<exec_depend>ros2_control</exec_depend>
|
||||
<depend>gazebo_ros</depend>
|
||||
<depend>gazebo_ros2_control</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>libopencv-dev</depend>
|
||||
<depend>yaml-cpp</depend>
|
||||
<depend>ament_index_cpp</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
|
||||
<!-- Execution dependencies -->
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
<exec_depend>rm_arm_control</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,501 @@
|
||||
#include <Eigen/Dense>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <tinyxml2.h>
|
||||
#include "collision_check.hpp"
|
||||
#include "common.h"
|
||||
|
||||
using namespace Eigen;
|
||||
using namespace collision_check;
|
||||
const int g_bodyLinkCnt[BODY_CNT] = {USED_ARM_DOF + 1, USED_ARM_DOF + 1, USED_OTHER_DOF + 1};
|
||||
const int g_bodyJointCnt[BODY_CNT] = {USED_ARM_DOF, USED_ARM_DOF, USED_OTHER_DOF};
|
||||
float g_leftArmJoints[USED_ARM_DOF] = {0};
|
||||
float g_rightArmJoints[USED_ARM_DOF] = {0};
|
||||
float g_otherJoints[USED_OTHER_DOF] = {0};
|
||||
|
||||
const std::string g_bodyStartName[BODY_CNT] = {"base_link_leftarm", "base_link_rightarm", "base_link"};
|
||||
|
||||
static float zeroFixedAngle = 0;
|
||||
|
||||
int g_linkStartIndex[BODY_CNT] = {0};
|
||||
|
||||
#define POLYHEDRONS_POINTS_CNT (8)
|
||||
|
||||
double truncate_to_4_decimal(double num) {
|
||||
double int_part; // 存储整数部分
|
||||
double frac_part = std::modf(num, &int_part); // 分离小数部分
|
||||
|
||||
// 截断小数部分到前 4 位
|
||||
double truncated_frac = std::trunc(frac_part * 10000.0) * 0.0001;
|
||||
|
||||
// 合并整数部分和截断后的小数部分
|
||||
return int_part + truncated_frac;
|
||||
}
|
||||
|
||||
static double toRadians(float degrees) {
|
||||
return (static_cast<double>(degrees)) * (M_PI / 180.0);
|
||||
}
|
||||
|
||||
int CollisionChecktor::InitJoint()
|
||||
{
|
||||
const std::map<std::string, int> jointNameToIndexMap = {
|
||||
{"left_joint1", 0}, {"left_joint2", 1}, {"left_joint3", 2}, {"left_joint4", 3}, {"left_joint5", 4}, {"left_joint6", 5},
|
||||
{"right_joint1", 0}, {"right_joint2", 1}, {"right_joint3", 2}, {"right_joint4", 3}, {"right_joint5", 4}, {"right_joint6", 5},
|
||||
{"left_behind_hip_joint", 0}, {"left_behind_leg_joint", 1}, {"left_behind_wheel_joint", 2}, {"right_behind_hip_joint", 3},
|
||||
{"right_behind_leg_joint", 4}, {"right_behind_wheel_joint", 5}, {"body_joint", 6}, {"body_2_joint", 7}, {"head_joint", 8},
|
||||
{"left_front_roll_joint", 9}, {"left_front_hip_joint", 10}, {"left_front_knee_joint", 11},
|
||||
{"left_front_wheel_joint", 12}, {"right_front_roll_joint", 13}, {"right_front_hip_joint", 14}, {"right_front_knee_joint", 15}, {"right_front_wheel_joint", 16}
|
||||
};
|
||||
const std::map<std::string, int> jointNameToBodyType = {
|
||||
{"left_joint1", BODY_TYPE_LEFT_ARM}, {"left_joint2", BODY_TYPE_LEFT_ARM}, {"left_joint3", BODY_TYPE_LEFT_ARM}, {"left_joint4", BODY_TYPE_LEFT_ARM},
|
||||
{"left_joint5", BODY_TYPE_LEFT_ARM}, {"left_joint6", BODY_TYPE_LEFT_ARM},
|
||||
{"right_joint1", BODY_TYPE_RIGHT_ARM}, {"right_joint2", BODY_TYPE_RIGHT_ARM}, {"right_joint3", BODY_TYPE_RIGHT_ARM}, {"right_joint4", BODY_TYPE_RIGHT_ARM},
|
||||
{"right_joint5", BODY_TYPE_RIGHT_ARM}, {"right_joint6", BODY_TYPE_RIGHT_ARM},
|
||||
{"left_behind_hip_joint", BODY_TYPE_OTHERS}, {"left_behind_leg_joint", BODY_TYPE_OTHERS}, {"left_behind_wheel_joint", BODY_TYPE_OTHERS},
|
||||
{"right_behind_hip_joint", BODY_TYPE_OTHERS}, {"right_behind_leg_joint", BODY_TYPE_OTHERS}, {"right_behind_wheel_joint", BODY_TYPE_OTHERS},
|
||||
{"body_joint", BODY_TYPE_OTHERS}, {"body_2_joint", BODY_TYPE_OTHERS}, {"head_joint", BODY_TYPE_OTHERS}, {"left_front_roll_joint", BODY_TYPE_OTHERS},
|
||||
{"left_front_hip_joint", BODY_TYPE_OTHERS}, {"left_front_knee_joint", BODY_TYPE_OTHERS}, {"left_front_wheel_joint", BODY_TYPE_OTHERS},
|
||||
{"right_front_roll_joint", BODY_TYPE_OTHERS}, {"right_front_hip_joint", BODY_TYPE_OTHERS}, {"right_front_knee_joint", BODY_TYPE_OTHERS},
|
||||
{"right_front_wheel_joint", BODY_TYPE_OTHERS}
|
||||
};
|
||||
for (JointsInfo &joint : jointMap_) {
|
||||
if (joint.type != FIXED) {
|
||||
int jointAngleIndex = jointNameToIndexMap.at(joint.name);
|
||||
int bodyType = jointNameToBodyType.at(joint.name);
|
||||
joint.angle = &joints_[bodyType][jointAngleIndex];
|
||||
} else {
|
||||
joint.angle = &zeroFixedAngle;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int CollisionChecktor::InitLinkCollisionDetectMatrix(const std::string& srdf_str)
|
||||
{
|
||||
if (maxObbCnt <= 0) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "InitLinkCollisionDetectMatrix: invalid maxObbCnt=%d", maxObbCnt);
|
||||
return -1;
|
||||
}
|
||||
linkCollisionDetectMatrix_.assign(static_cast<size_t>(maxObbCnt) * static_cast<size_t>(maxObbCnt), 0);
|
||||
for (int i = 0; i < maxObbCnt; ++i) {
|
||||
for (int j = i + 1; j < maxObbCnt; ++j) {
|
||||
linkCollisionDetectMatrix_[static_cast<size_t>(i) * static_cast<size_t>(maxObbCnt) + static_cast<size_t>(j)] = 1;
|
||||
}
|
||||
}
|
||||
tinyxml2::XMLDocument doc;
|
||||
tinyxml2::XMLError error = doc.Parse(srdf_str.c_str());
|
||||
if (error != tinyxml2::XML_SUCCESS) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to parse SRDF: %s, length: %ld", doc.ErrorStr(), srdf_str.length());
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 获取根节点<robot>
|
||||
tinyxml2::XMLElement* robot_root = doc.FirstChildElement("robot");
|
||||
if (!robot_root) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "SRDF has no <robot> root node");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int disable_count = 0;
|
||||
for (tinyxml2::XMLElement* dc_elem = robot_root->FirstChildElement("disable_collisions");
|
||||
dc_elem;
|
||||
dc_elem = dc_elem->NextSiblingElement("disable_collisions")) {
|
||||
const char* link1_c = dc_elem->Attribute("link1");
|
||||
const char* link2_c = dc_elem->Attribute("link2");
|
||||
if (!link1_c || !link2_c) {
|
||||
RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()), "disable_collisions missing link1/link2 attribute, skipping.");
|
||||
continue;
|
||||
}
|
||||
std::string link1 = link1_c;
|
||||
std::string link2 = link2_c;
|
||||
int link1Index = linkCollisionMap_.find(link1) != linkCollisionMap_.end() ? linkCollisionMap_[link1] : -1;
|
||||
int link2Index = linkCollisionMap_.find(link2) != linkCollisionMap_.end() ? linkCollisionMap_[link2] : -1;
|
||||
if (link1Index != -1 && link2Index != -1) {
|
||||
CollisionStructureInfo* colStruct1 = &collision_structures_[link1Index];
|
||||
CollisionStructureInfo* colStruct2 = &collision_structures_[link2Index];
|
||||
for (int obb1Idx : colStruct1->obbLinkIndexList) {
|
||||
for (int obb2Idx : colStruct2->obbLinkIndexList) {
|
||||
if (obb1Idx >= 0 && obb1Idx < maxObbCnt && obb2Idx >= 0 && obb2Idx < maxObbCnt) {
|
||||
linkCollisionDetectMatrix_[
|
||||
static_cast<size_t>(obb1Idx) * static_cast<size_t>(maxObbCnt) + static_cast<size_t>(obb2Idx)] = 0;
|
||||
linkCollisionDetectMatrix_[
|
||||
static_cast<size_t>(obb2Idx) * static_cast<size_t>(maxObbCnt) + static_cast<size_t>(obb1Idx)] = 0;
|
||||
} else {
|
||||
RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"disable_collisions references out-of-range OBB index (%d,%d) with maxObbCnt=%d",
|
||||
obb1Idx, obb2Idx, maxObbCnt);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
disable_count++;
|
||||
}
|
||||
// Build tuplesToCheck_ list
|
||||
tuplesToCheck_.clear();
|
||||
tuplesToCheck_.reserve(static_cast<size_t>(maxObbCnt) * (static_cast<size_t>(maxObbCnt) - 1) / 2);
|
||||
for (int i = 0; i < maxObbCnt; ++i) {
|
||||
for (int j = i + 1; j < maxObbCnt; ++j) {
|
||||
if (linkCollisionDetectMatrix_[static_cast<size_t>(i) * static_cast<size_t>(maxObbCnt) + static_cast<size_t>(j)]) {
|
||||
AxisStats *stats = new AxisStats();
|
||||
tuplesToCheck_.emplace_back(i, j, stats);
|
||||
}
|
||||
}
|
||||
}
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"Initialized link collision detect matrix, disabled %d collision pairs, enabled pairs: %zu.",
|
||||
disable_count, tuplesToCheck_.size());
|
||||
return 0;
|
||||
}
|
||||
|
||||
int CollisionChecktor::InitXmlStrFromFile(const std::string& robot_string)
|
||||
{
|
||||
tinyxml2::XMLDocument doc;
|
||||
tinyxml2::XMLError error = doc.Parse(robot_string.c_str());
|
||||
if (error != tinyxml2::XML_SUCCESS) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to parse SRDF: %s, length: %ld", doc.ErrorStr(), robot_string.length());
|
||||
return -1;
|
||||
}
|
||||
|
||||
tinyxml2::XMLElement* robot_root = doc.FirstChildElement("robot");
|
||||
if (!robot_root) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "SRDF has no <robot> root node");
|
||||
return -1;
|
||||
}
|
||||
if (!robot_root->Attribute("maxObbCnt")) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "SRDF <robot> missing required attribute maxObbCnt");
|
||||
return -1;
|
||||
}
|
||||
rootLinkIndex_ = robot_root->IntAttribute("rootLinkIndex_", 0);
|
||||
maxObbCnt = robot_root->IntAttribute("maxObbCnt", 0);
|
||||
if (maxObbCnt <= 0) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Invalid maxObbCnt=%d parsed from SRDF", maxObbCnt);
|
||||
return -1;
|
||||
}
|
||||
addedOBBs_.resize(maxObbCnt);
|
||||
int collision_structure_count = robot_root->IntAttribute("collision_structure_count", 0);
|
||||
int joint_count = robot_root->IntAttribute("joint_count", 0);
|
||||
if (collision_structure_count <= 0 || joint_count <= 0) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Invalid counts: collision_structure_count=%d, joint_count=%d", collision_structure_count, joint_count);
|
||||
return -1;
|
||||
}
|
||||
collision_structures_.resize(collision_structure_count);
|
||||
jointMap_.resize(joint_count);
|
||||
|
||||
int obbCnt = 0;
|
||||
for (tinyxml2::XMLElement* link_elem = robot_root->FirstChildElement("collision_model");
|
||||
link_elem;
|
||||
link_elem = link_elem->NextSiblingElement("collision_model")) {
|
||||
CollisionStructureInfo col_struct;
|
||||
int linkIndex = col_struct.link_index = link_elem->IntAttribute("link_index", -1);
|
||||
const char* link_name_c = link_elem->Attribute("link_name");
|
||||
if (!link_name_c || linkIndex < 0 || linkIndex >= collision_structure_count) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "collision_model missing/invalid link_name or link_index=%d", linkIndex);
|
||||
return -1;
|
||||
}
|
||||
std::string linkName = col_struct.link_name = link_name_c;
|
||||
col_struct.geometry_type = (link_elem->IntAttribute("geometry_type", 0));
|
||||
col_struct.mesh_filename = link_elem->Attribute("mesh_filename") ? link_elem->Attribute("mesh_filename") : "";
|
||||
col_struct.joint_count = 0;
|
||||
col_struct.parent_link_index = link_elem->IntAttribute("parent_link_index", -1);
|
||||
|
||||
for (tinyxml2::XMLElement* obb_elem = link_elem->FirstChildElement("obb");
|
||||
obb_elem;
|
||||
obb_elem = obb_elem->NextSiblingElement("obb")) {
|
||||
OBB obb;
|
||||
obb.center = Vector3d(
|
||||
obb_elem->DoubleAttribute("center_x", 0.0),
|
||||
obb_elem->DoubleAttribute("center_y", 0.0),
|
||||
obb_elem->DoubleAttribute("center_z", 0.0));
|
||||
obb.axis[0] = Vector3d(
|
||||
obb_elem->DoubleAttribute("axis_0_x", 1.0),
|
||||
obb_elem->DoubleAttribute("axis_0_y", 0.0),
|
||||
obb_elem->DoubleAttribute("axis_0_z", 0.0));
|
||||
obb.axis[1] = Vector3d(
|
||||
obb_elem->DoubleAttribute("axis_1_x", 0.0),
|
||||
obb_elem->DoubleAttribute("axis_1_y", 1.0),
|
||||
obb_elem->DoubleAttribute("axis_1_z", 0.0));
|
||||
obb.axis[2] = Vector3d(
|
||||
obb_elem->DoubleAttribute("axis_2_x", 0.0),
|
||||
obb_elem->DoubleAttribute("axis_2_y", 0.0),
|
||||
obb_elem->DoubleAttribute("axis_2_z", 1.0));
|
||||
obb.halfExtent[0] = obb_elem->DoubleAttribute("half_extent_x", 0.5);
|
||||
obb.halfExtent[1] = obb_elem->DoubleAttribute("half_extent_y", 0.5);
|
||||
obb.halfExtent[2] = obb_elem->DoubleAttribute("half_extent_z", 0.5);
|
||||
obb.maxRadius = obb_elem->DoubleAttribute("max_radius", 0.0);
|
||||
obb.selfMinProject[0] = obb_elem->DoubleAttribute("self_min_project_x", 0.0);
|
||||
obb.selfMinProject[1] = obb_elem->DoubleAttribute("self_min_project_y", 0.0);
|
||||
obb.selfMinProject[2] = obb_elem->DoubleAttribute("self_min_project_z", 0.0);
|
||||
obb.selfMaxProject[0] = obb_elem->DoubleAttribute("self_max_project_x", 0.0);
|
||||
obb.selfMaxProject[1] = obb_elem->DoubleAttribute("self_max_project_y", 0.0);
|
||||
obb.selfMaxProject[2] = obb_elem->DoubleAttribute("self_max_project_z", 0.0);
|
||||
gjk_interface::getSelfProject(obb);
|
||||
col_struct.obbModelList.push_back(obb);
|
||||
obbCnt++;
|
||||
int obbIndex = obb_elem->IntAttribute("index", -1);
|
||||
if (obbIndex < 0 || obbIndex >= maxObbCnt) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "OBB index out of range: %d (max %d) for link %s", obbIndex, maxObbCnt, linkName.c_str());
|
||||
return -1;
|
||||
}
|
||||
col_struct.obbLinkIndexList.push_back(obbIndex);
|
||||
obbNameMap_[obbIndex] = linkName;
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Loaded OBB for link %s: center(%.2f, %.2f, %.2f), halfExtent(%.2f, %.2f, %.2f).",
|
||||
linkName.c_str(),
|
||||
obb.center.x(), obb.center.y(), obb.center.z(),
|
||||
obb.halfExtent[0], obb.halfExtent[1], obb.halfExtent[2]);
|
||||
}
|
||||
|
||||
for (tinyxml2::XMLElement* joint_elem = link_elem->FirstChildElement("joint_index");
|
||||
joint_elem;
|
||||
joint_elem = joint_elem->NextSiblingElement("joint_index")) {
|
||||
int idx = joint_elem->IntAttribute("index", -1);
|
||||
if (idx < 0 || idx >= joint_count) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "joint_index out of range: %d (joint_count %d) for link %s", idx, joint_count, linkName.c_str());
|
||||
return -1;
|
||||
}
|
||||
col_struct.jointIndexList.push_back(idx);
|
||||
col_struct.joint_count++;
|
||||
}
|
||||
|
||||
collision_structures_[linkIndex] = col_struct;
|
||||
linkCollisionMap_[linkName] = linkIndex;
|
||||
for (int i = 0; i < BODY_CNT; i++) {
|
||||
if (g_bodyStartName[i] == linkName) {
|
||||
g_linkStartIndex[i] = linkIndex;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (tinyxml2::XMLElement* joint_elem = robot_root->FirstChildElement("joint_model");
|
||||
joint_elem;
|
||||
joint_elem = joint_elem->NextSiblingElement("joint_model")) {
|
||||
JointsInfo joint_info;
|
||||
int joint_index = joint_elem->IntAttribute("joint_index", -1);
|
||||
const char* joint_name_c = joint_elem->Attribute("joint_name");
|
||||
if (!joint_name_c || joint_index < 0 || joint_index >= joint_count) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "joint_model missing/invalid joint_name or joint_index=%d", joint_index);
|
||||
return -1;
|
||||
}
|
||||
joint_info.name = joint_name_c;
|
||||
joint_info.type = (joint_elem->IntAttribute("type", 0));
|
||||
joint_info.parent_link = joint_elem->IntAttribute("parent_link", -1);
|
||||
joint_info.child_link = joint_elem->IntAttribute("child_link", -1);
|
||||
joint_info.limit_lower = joint_elem->DoubleAttribute("limit_lower", 0.0);
|
||||
joint_info.limit_upper = joint_elem->DoubleAttribute("limit_upper", 0.0);
|
||||
joint_info.limit_v = joint_elem->DoubleAttribute("limit_v", 0.0);
|
||||
joint_info.limit_e = joint_elem->DoubleAttribute("limit_e", 0.0);
|
||||
joint_info.nextLinkOffset = Vector3d(
|
||||
joint_elem->DoubleAttribute("nextLinkOffset_x", 0.0),
|
||||
joint_elem->DoubleAttribute("nextLinkOffset_y", 0.0),
|
||||
joint_elem->DoubleAttribute("nextLinkOffset_z", 0.0));
|
||||
joint_info.rotation = Quaterniond(
|
||||
joint_elem->DoubleAttribute("rotation_w", 1.0),
|
||||
joint_elem->DoubleAttribute("rotation_x", 0.0),
|
||||
joint_elem->DoubleAttribute("rotation_y", 0.0),
|
||||
joint_elem->DoubleAttribute("rotation_z", 0.0));
|
||||
joint_info.axis = Vector3d(
|
||||
joint_elem->DoubleAttribute("axis_x", 1.0),
|
||||
joint_elem->DoubleAttribute("axis_y", 0.0),
|
||||
joint_elem->DoubleAttribute("axis_z", 0.0));
|
||||
if (joint_info.child_link < 0 || joint_info.child_link >= collision_structure_count ||
|
||||
joint_info.parent_link < 0 || joint_info.parent_link >= collision_structure_count) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "joint_model link indices out of range for joint %s", joint_info.name.c_str());
|
||||
return -1;
|
||||
}
|
||||
jointMap_[joint_index] = joint_info;
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Loaded joint %s: axis(%.2f, %.2f, %.2f), offset(%.2f, %.2f, %.2f).",
|
||||
joint_info.name.c_str(), joint_info.axis.x(), joint_info.axis.y(), joint_info.axis.z(),
|
||||
joint_info.nextLinkOffset.x(), joint_info.nextLinkOffset.y(), joint_info.nextLinkOffset.z());
|
||||
}
|
||||
|
||||
collision_structures_[rootLinkIndex_].rotation = Quaterniond::Identity();
|
||||
collision_structures_[rootLinkIndex_].offset = Vector3d::Zero();
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Initialized collision structures from XML, total links: %ld, total OBBs: %d.",
|
||||
collision_structures_.size(), obbCnt);
|
||||
return 0;
|
||||
}
|
||||
|
||||
CollisionChecktor::CollisionChecktor(std::string robot_string, std::string srdf_string, float **jointsList)
|
||||
{
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CollisionChecktor: 初始化机械臂碰撞检测模块...");
|
||||
|
||||
|
||||
joints_[BODY_TYPE_LEFT_ARM] = g_leftArmJoints;
|
||||
joints_[BODY_TYPE_RIGHT_ARM] = g_rightArmJoints;
|
||||
joints_[BODY_TYPE_OTHERS] = g_otherJoints;
|
||||
for (int j = 0; j < BODY_CNT; j++) {
|
||||
for (int k = 0; k < g_bodyJointCnt[j]; k++) {
|
||||
joints_[j][k] = jointsList[j][k];
|
||||
}
|
||||
}
|
||||
|
||||
InitXmlStrFromFile(robot_string);
|
||||
|
||||
InitJoint();
|
||||
InitPolyhedronsList();
|
||||
InitLinkCollisionDetectMatrix(srdf_string);
|
||||
}
|
||||
|
||||
CollisionChecktor::~CollisionChecktor()
|
||||
{
|
||||
for (const auto& p : tuplesToCheck_) {
|
||||
delete std::get<2>(p);
|
||||
}
|
||||
tuplesToCheck_.clear();
|
||||
}
|
||||
|
||||
int flashStateIndex = 0;
|
||||
int CollisionChecktor::CheckCollisionInner(std::vector<OBB> *addedOBBs)
|
||||
{
|
||||
bool inCollision = false;
|
||||
OBB *obb1 = NULL;
|
||||
OBB *obb2 = NULL;
|
||||
for (const auto& p : tuplesToCheck_) {
|
||||
const int i = std::get<0>(p);
|
||||
const int j = std::get<1>(p);
|
||||
AxisStats *stats = std::get<2>(p);
|
||||
inCollision = gjk_interface::checkOBBCollisionNew((*addedOBBs)[i], (*addedOBBs)[j], stats);
|
||||
if (inCollision) {
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "dect %s and %s collision b", obbNameMap_[i].c_str(), obbNameMap_[j].c_str());
|
||||
obb1 = &(*addedOBBs)[i];
|
||||
obb2 = &(*addedOBBs)[j];
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "obb1: center(%.4f, %.4f, %.4f), axis1(%.4f, %.4f, %.4f), axis2(%.4f, %.4f, %.4f), axis3(%.4f, %.4f, %.4f), half_lengths(%.4f, %.4f, %.4f)",
|
||||
obb1->center.x(), obb1->center.y(), obb1->center.z(),
|
||||
obb1->axis[0].x(), obb1->axis[0].y(), obb1->axis[0].z(),
|
||||
obb1->axis[1].x(), obb1->axis[1].y(), obb1->axis[1].z(),
|
||||
obb1->axis[2].x(), obb1->axis[2].y(), obb1->axis[2].z(),
|
||||
obb1->halfExtent[0], obb1->halfExtent[1], obb1->halfExtent[2]);
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "obb2: center(%.4f, %.4f, %.4f), axis1(%.4f, %.4f, %.4f), axis2(%.4f, %.4f, %.4f), axis3(%.4f, %.4f, %.4f), half_lengths(%.4f, %.4f, %.4f)",
|
||||
obb2->center.x(), obb2->center.y(), obb2->center.z(),
|
||||
obb2->axis[0].x(), obb2->axis[0].y(), obb2->axis[0].z(),
|
||||
obb2->axis[1].x(), obb2->axis[1].y(), obb2->axis[1].z(),
|
||||
obb2->axis[2].x(), obb2->axis[2].y(), obb2->axis[2].z(),
|
||||
obb2->halfExtent[0], obb2->halfExtent[1], obb2->halfExtent[2]);
|
||||
}
|
||||
}
|
||||
AxisStats *stats = std::get<2>(tuplesToCheck_[flashStateIndex]);
|
||||
flashStateIndex = (flashStateIndex + 1) % tuplesToCheck_.size();
|
||||
gjk_interface::BubbleSortAxisByHitCount(stats);
|
||||
return OK;
|
||||
}
|
||||
|
||||
void CollisionChecktor::UpdatePolyhedrons(int bodyType)
|
||||
{
|
||||
int linkIndex = g_linkStartIndex[bodyType];
|
||||
CollisionStructureInfo *colStruct = &collision_structures_[linkIndex];
|
||||
std::vector<CollisionStructureInfo *> child = {colStruct};
|
||||
int childIndex = 0;
|
||||
int childCnt = 1;
|
||||
while (childIndex < childCnt) {
|
||||
colStruct = child[childIndex];
|
||||
childIndex++;
|
||||
for (int i = 0; i < colStruct->joint_count; i++) {
|
||||
int jointIndex = colStruct->jointIndexList[i];
|
||||
JointsInfo *jointInfo = &jointMap_[jointIndex];
|
||||
Quaterniond rotation = colStruct->rotation;
|
||||
Vector3d offset = colStruct->offset;
|
||||
|
||||
linkIndex = jointInfo->child_link;
|
||||
CollisionStructureInfo *nowChild = &collision_structures_[linkIndex];
|
||||
|
||||
offset = offset + rotation * jointInfo->nextLinkOffset;
|
||||
if (jointInfo->type == REVOLUTE) {
|
||||
double angleRad = toRadians(jointInfo->angle[0]);
|
||||
rotation = AngleAxisd(angleRad, rotation * jointInfo->axis) * rotation;
|
||||
} else if (jointInfo->type == PRISMATIC) {
|
||||
double displacement = jointInfo->angle[0];
|
||||
offset = offset + rotation * jointInfo->axis * displacement;
|
||||
}
|
||||
|
||||
nowChild->rotation = rotation;
|
||||
nowChild->offset = offset;
|
||||
|
||||
for (int i = 0; i < (int)nowChild->obbModelList.size(); i++) {
|
||||
int obbIndex = nowChild->obbLinkIndexList[i];
|
||||
addedOBBs_[obbIndex] = gjk_interface::updateOBBFromPoints(nowChild->obbModelList[i],
|
||||
offset,
|
||||
rotation);
|
||||
}
|
||||
|
||||
if (nowChild->joint_count > 0) {
|
||||
childCnt++;
|
||||
child.push_back(nowChild);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CollisionChecktor::InitPolyhedronsList()
|
||||
{
|
||||
UpdatePolyhedrons(BODY_TYPE_OTHERS);
|
||||
for (int i = 0; i < maxObbCnt; i++) {
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "InitPolyhedronsList obb %d: center(%.2f, %.2f, %.2f), axis1(%.2f, %.2f, %.2f), axis2(%.2f, %.2f, %.2f), axis3(%.2f, %.2f, %.2f), half_lengths(%.2f, %.2f, %.2f)",
|
||||
i,
|
||||
addedOBBs_[i].center.x(), addedOBBs_[i].center.y(), addedOBBs_[i].center.z(),
|
||||
addedOBBs_[i].axis[0].x(), addedOBBs_[i].axis[0].y(), addedOBBs_[i].axis[0].z(),
|
||||
addedOBBs_[i].axis[1].x(), addedOBBs_[i].axis[1].y(), addedOBBs_[i].axis[1].z(),
|
||||
addedOBBs_[i].axis[2].x(), addedOBBs_[i].axis[2].y(), addedOBBs_[i].axis[2].z(),
|
||||
addedOBBs_[i].halfExtent[0], addedOBBs_[i].halfExtent[1], addedOBBs_[i].halfExtent[2]);
|
||||
}
|
||||
}
|
||||
|
||||
int checkCnt = 0;
|
||||
int64_t prepareTime = 0;
|
||||
int64_t checkTime = 0;
|
||||
int64_t checkTime2 = 0;
|
||||
int CollisionChecktor::CheckCollision(rclcpp::Node *node)
|
||||
{
|
||||
int64_t currentTimeNs = node->get_clock()->now().nanoseconds();
|
||||
UpdatePolyhedrons(BODY_TYPE_OTHERS);
|
||||
int64_t updateNs = node->get_clock()->now().nanoseconds();
|
||||
prepareTime += updateNs - currentTimeNs;
|
||||
int result = CheckCollisionInner(&addedOBBs_);
|
||||
|
||||
int64_t time1 = node->get_clock()->now().nanoseconds();
|
||||
checkTime += time1 - updateNs;
|
||||
checkCnt++;
|
||||
if (checkCnt == 2000) {
|
||||
double checkAxisAve;
|
||||
gjk_interface::GetAxisLog(checkAxisAve);
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"Update time:%ld, check time1:%ld, check axis ave:%.4f.", prepareTime/checkCnt/1000,
|
||||
checkTime/checkCnt/1000, checkAxisAve);
|
||||
checkTime = 0;
|
||||
prepareTime = 0;
|
||||
checkCnt = 0;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
int CollisionChecktor::AddPolyhedron(int id, const Polyhedron& poly)
|
||||
{
|
||||
if (addedPolyhedrons_.find(id) != addedPolyhedrons_.end()) {
|
||||
return -1; // 已存在相同ID的多面体
|
||||
}
|
||||
addedPolyhedrons_[id] = poly;
|
||||
return 0; // 成功添加
|
||||
}
|
||||
|
||||
int CollisionChecktor::RemovePolyhedron(int id)
|
||||
{
|
||||
auto it = addedPolyhedrons_.find(id);
|
||||
if (it != addedPolyhedrons_.end()) {
|
||||
addedPolyhedrons_.erase(it);
|
||||
return 0; // 成功移除
|
||||
}
|
||||
return -1; // 未找到对应ID的多面体
|
||||
}
|
||||
|
||||
void CollisionChecktor::SetJoints(float **joints)
|
||||
{
|
||||
for (int i = 0; i < BODY_CNT; i++) {
|
||||
for (int j = 0; j < g_bodyJointCnt[i]; j++) {
|
||||
joints_[i][j] = joints[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,274 @@
|
||||
#include <filesystem>
|
||||
#include "data_collector.hpp"
|
||||
#include "common.h"
|
||||
#include <tf2/utils.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
|
||||
|
||||
DataCollector::DataCollector(const std::string& yaml_path, const std::string& joint_data_log_path, rclcpp::Node* node)
|
||||
: node_(node)
|
||||
, world_frame_("world")
|
||||
, base_frame_("base_link")
|
||||
{
|
||||
loadJointControllersConfig(yaml_path);
|
||||
|
||||
std::string output_dir;
|
||||
|
||||
// 优先使用参数传入的路径
|
||||
if (!joint_data_log_path.empty()) {
|
||||
output_dir = joint_data_log_path;
|
||||
} else {
|
||||
// 默认使用 ~/hivecore/simulator_log
|
||||
const char* home_dir = std::getenv("HOME");
|
||||
if (home_dir) {
|
||||
output_dir = std::string(home_dir) + "/hivecore/simulator_log";
|
||||
} else {
|
||||
output_dir = "/tmp/robot_simulator/joint_states";
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Joint data log path: %s", output_dir.c_str());
|
||||
initJointDataFiles(output_dir);
|
||||
|
||||
// 初始化 TF 监听器
|
||||
initTfListener();
|
||||
}
|
||||
|
||||
DataCollector::~DataCollector()
|
||||
{
|
||||
for (auto& [name, file] : joint_data_files_) {
|
||||
if (file.is_open()) {
|
||||
file.close();
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Closed joint data file for: %s", name.c_str());
|
||||
}
|
||||
}
|
||||
joint_data_files_.clear();
|
||||
|
||||
// 关闭基座位姿文件
|
||||
if (base_pose_file_.is_open()) {
|
||||
base_pose_file_.close();
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Closed base pose file");
|
||||
}
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "DataCollector destroyed");
|
||||
}
|
||||
|
||||
void DataCollector::loadJointControllersConfig(const std::string& yaml_path)
|
||||
{
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Loading joint controllers config from: %s", yaml_path.c_str());
|
||||
|
||||
YAML::Node config = YAML::LoadFile(yaml_path);
|
||||
|
||||
// 遍历 controller_manager 下的所有控制器
|
||||
if (!config["controller_manager"] || !config["controller_manager"]["ros__parameters"]) {
|
||||
RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()), "No controller_manager found in config file");
|
||||
return;
|
||||
}
|
||||
|
||||
YAML::Node controllers = config["controller_manager"]["ros__parameters"];
|
||||
|
||||
for (auto it = controllers.begin(); it != controllers.end(); ++it) {
|
||||
std::string controller_name = it->first.as<std::string>();
|
||||
|
||||
// 跳过 update_rate 等非控制器参数
|
||||
if (!it->second.IsMap() || !it->second["type"]) {
|
||||
continue;
|
||||
}
|
||||
|
||||
std::string controller_type = it->second["type"].as<std::string>();
|
||||
|
||||
// 只处理 JointStateBroadcaster 类型
|
||||
if (controller_type.find("JointStateBroadcaster") != std::string::npos) {
|
||||
// 读取关节列表
|
||||
std::vector<std::string> joints;
|
||||
if (config[controller_name] &&
|
||||
config[controller_name]["ros__parameters"] &&
|
||||
config[controller_name]["ros__parameters"]["joints"]) {
|
||||
|
||||
YAML::Node joint_list = config[controller_name]["ros__parameters"]["joints"];
|
||||
for (size_t i = 0; i < joint_list.size(); i++) {
|
||||
joints.push_back(joint_list[i].as<std::string>());
|
||||
}
|
||||
}
|
||||
|
||||
// 构建话题名称
|
||||
std::string topic = "/" + controller_name + "/joint_states";
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Topic: %s", topic.c_str());
|
||||
|
||||
// 创建关节组信息
|
||||
JointGroupInfo group_info;
|
||||
group_info.name = controller_name;
|
||||
group_info.topic = topic;
|
||||
group_info.joints = joints;
|
||||
group_info.received = false;
|
||||
|
||||
joint_groups_[controller_name] = group_info;
|
||||
|
||||
// 创建订阅者
|
||||
std::string group_name = controller_name; // 捕获用于 lambda
|
||||
joint_state_subs_[controller_name] = node_->create_subscription<sensor_msgs::msg::JointState>(
|
||||
topic, 10,
|
||||
[this, group_name](const sensor_msgs::msg::JointState::SharedPtr msg) {
|
||||
this->jointStateCallback(msg, group_name);
|
||||
});
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Subscribed to %s (%zu joints)",
|
||||
topic.c_str(), joints.size());
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Loaded %zu joint groups from config", joint_groups_.size());
|
||||
}
|
||||
|
||||
void DataCollector::jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg, const std::string& groupName)
|
||||
{
|
||||
auto it = joint_groups_.find(groupName);
|
||||
if (it == joint_groups_.end()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// 存储关节状态
|
||||
it->second.state = *msg;
|
||||
|
||||
// 写入文件
|
||||
writeJointDataToFile(groupName, *msg);
|
||||
|
||||
if (!it->second.received) {
|
||||
it->second.received = true;
|
||||
|
||||
std::string joint_names;
|
||||
for (size_t i = 0; i < msg->name.size(); i++) {
|
||||
joint_names += msg->name[i];
|
||||
if (i < msg->name.size() - 1) {
|
||||
joint_names += ", ";
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s JointState received: %zu joints [%s]",
|
||||
groupName.c_str(),
|
||||
msg->name.size(),
|
||||
joint_names.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void DataCollector::writeJointDataToFile(const std::string& groupName, const sensor_msgs::msg::JointState& state)
|
||||
{
|
||||
auto it = joint_groups_.find(groupName);
|
||||
if (it == joint_groups_.end()) {
|
||||
return;
|
||||
}
|
||||
|
||||
auto& file = joint_data_files_[groupName];
|
||||
|
||||
// 写入时间戳 (使用消息中的时间戳)
|
||||
double timestamp = state.header.stamp.sec + state.header.stamp.nanosec * 1e-9;
|
||||
file << std::fixed << std::setprecision(6) << timestamp;
|
||||
|
||||
// 写入每个关节的位置和速度
|
||||
for (size_t i = 0; i < state.position.size(); i++) {
|
||||
double pos = (i < state.position.size()) ? state.position[i] : 0.0;
|
||||
double vel = (i < state.velocity.size()) ? state.velocity[i] : 0.0;
|
||||
file << "," << std::setprecision(6) << pos << "," << vel;
|
||||
}
|
||||
file << std::endl;
|
||||
}
|
||||
|
||||
void DataCollector::initJointDataFiles(const std::string& output_dir)
|
||||
{
|
||||
joint_data_output_dir_ = output_dir;
|
||||
|
||||
// 创建输出目录
|
||||
try {
|
||||
std::filesystem::create_directories(output_dir);
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Created joint data output directory: %s", output_dir.c_str());
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to create output directory: %s", e.what());
|
||||
return;
|
||||
}
|
||||
|
||||
// 获取当前时间戳作为文件名前缀
|
||||
auto now = std::chrono::system_clock::now();
|
||||
auto time_t = std::chrono::system_clock::to_time_t(now);
|
||||
std::stringstream timestamp;
|
||||
timestamp << std::put_time(std::localtime(&time_t), "%Y%m%d_%H%M%S");
|
||||
|
||||
// 为每个关节组创建文件
|
||||
for (const auto& [group_name, group_info] : joint_groups_) {
|
||||
// 将话题名转换为文件名（去掉 / 替换为 _）
|
||||
std::string file_name = group_name;
|
||||
|
||||
// 构建完整文件路径
|
||||
std::string file_path = output_dir + "/" + timestamp.str() + "_" + file_name + ".csv";
|
||||
|
||||
// 打开文件
|
||||
joint_data_files_[group_name].open(file_path, std::ios::out | std::ios::trunc);
|
||||
|
||||
if (joint_data_files_[group_name].is_open()) {
|
||||
// 写入 CSV 表头
|
||||
joint_data_files_[group_name] << "timestamp";
|
||||
for (const auto& joint : group_info.joints) {
|
||||
joint_data_files_[group_name] << "," << joint << "_pos," << joint << "_vel";
|
||||
}
|
||||
joint_data_files_[group_name] << std::endl;
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Created joint data file: %s", file_path.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to open file: %s", file_path.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
// 创建基座位姿文件
|
||||
std::string base_pose_path = output_dir + "/" + timestamp.str() + "_base_pose.csv";
|
||||
base_pose_file_.open(base_pose_path, std::ios::out | std::ios::trunc);
|
||||
if (base_pose_file_.is_open()) {
|
||||
base_pose_file_ << "timestamp,position_x,position_y,position_z,rotation_x,rotation_y,rotation_z,rotation_w" << std::endl;
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Created base pose file: %s", base_pose_path.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to open base pose file: %s", base_pose_path.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
void DataCollector::initTfListener()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void DataCollector::posionCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
|
||||
{
|
||||
// 从 P3D 插件的 Odometry 消息中提取位姿
|
||||
robot_base_pose_.position = Eigen::Vector3d(
|
||||
msg->pose.pose.position.x,
|
||||
msg->pose.pose.position.y,
|
||||
msg->pose.pose.position.z);
|
||||
|
||||
robot_base_pose_.rotation = Eigen::Quaterniond(
|
||||
msg->pose.pose.orientation.w,
|
||||
msg->pose.pose.orientation.x,
|
||||
msg->pose.pose.orientation.y,
|
||||
msg->pose.pose.orientation.z);
|
||||
|
||||
robot_base_pose_.valid = true;
|
||||
|
||||
// 写入文件
|
||||
writeBasePoseToFile();
|
||||
}
|
||||
|
||||
void DataCollector::writeBasePoseToFile()
|
||||
{
|
||||
if (!base_pose_file_.is_open() || !robot_base_pose_.valid) {
|
||||
return;
|
||||
}
|
||||
|
||||
auto now = node_->get_clock()->now();
|
||||
double timestamp = now.seconds();
|
||||
|
||||
base_pose_file_ << std::fixed << std::setprecision(6)
|
||||
<< timestamp << ","
|
||||
<< robot_base_pose_.position.x() << ","
|
||||
<< robot_base_pose_.position.y() << ","
|
||||
<< robot_base_pose_.position.z() << ","
|
||||
<< robot_base_pose_.rotation.x() << ","
|
||||
<< robot_base_pose_.rotation.y() << ","
|
||||
<< robot_base_pose_.rotation.z() << ","
|
||||
<< robot_base_pose_.rotation.w() << std::endl;
|
||||
}
|
||||
@@ -0,0 +1,229 @@
|
||||
#include "reference_manager_for_gazebo.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <ocs2_core/misc/LinearInterpolation.h>
|
||||
#include <set>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
ReferenceManagerForGazebo::ReferenceManagerForGazebo(const std::string& urdf_content,
|
||||
const std::string& config_file)
|
||||
: ocs2::ReferenceManager(ocs2::TargetTrajectories(), ocs2::ModeSchedule())
|
||||
{
|
||||
try {
|
||||
// 初始化 RobotInfoLoader
|
||||
robot_info_loader_ = std::make_unique<RobotInfoLoader>(urdf_content);
|
||||
if (robot_info_loader_->isLoaded()) {
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Successfully loaded robot info from URDF");
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Robot name: %s", robot_info_loader_->getRobotInfo().robot_name.c_str());
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Number of joints: %zu", robot_info_loader_->getJointCount());
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Number of links: %zu", robot_info_loader_->getLinkCount());
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Base link: %s", robot_info_loader_->getRobotInfo().base_link_name.c_str());
|
||||
|
||||
// 获取PinocchioInterface信息（用于MPC控制）
|
||||
const auto& pinocchio_interface = robot_info_loader_->getPinocchioInterface();
|
||||
const auto& model = pinocchio_interface.getModel();
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "PinocchioInterface created successfully");
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Pinocchio model - nq: %d, nv: %d, njoints: %d",
|
||||
(model.nq), (model.nv), (model.njoints));
|
||||
|
||||
// 加载配置
|
||||
if (!config_file.empty()) {
|
||||
loadConfig(config_file);
|
||||
} else {
|
||||
RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"No config file provided, using default configuration");
|
||||
}
|
||||
|
||||
// 初始化手臂规划器
|
||||
initializeArmPlanner();
|
||||
|
||||
// 可选：打印详细信息（用于调试）
|
||||
// robot_info_loader_->printRobotInfo();
|
||||
} else {
|
||||
RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()), "RobotInfoLoader created but not loaded");
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to load robot info from URDF: %s", e.what());
|
||||
}
|
||||
}
|
||||
|
||||
ReferenceManagerForGazebo::~ReferenceManagerForGazebo()
|
||||
{
|
||||
}
|
||||
|
||||
void ReferenceManagerForGazebo::loadConfig(const std::string& config_file) {
|
||||
try {
|
||||
YAML::Node config = YAML::LoadFile(config_file);
|
||||
|
||||
if (config["robot_parts"]) {
|
||||
const auto& robot_parts = config["robot_parts"];
|
||||
|
||||
// 加载腿部关节名称
|
||||
if (robot_parts["leg_joints"]) {
|
||||
config_.leg_joint_names = robot_parts["leg_joints"].as<std::vector<std::string>>();
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"Loaded %zu leg joints from config", config_.leg_joint_names.size());
|
||||
}
|
||||
|
||||
// 加载手臂关节名称
|
||||
if (robot_parts["arm_joints"]) {
|
||||
config_.arm_joint_names = robot_parts["arm_joints"].as<std::vector<std::string>>();
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"Loaded %zu arm joints from config", config_.arm_joint_names.size());
|
||||
}
|
||||
|
||||
// 加载腿部接触点名称
|
||||
if (robot_parts["leg_contacts"]) {
|
||||
config_.leg_contact_names = robot_parts["leg_contacts"].as<std::vector<std::string>>();
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"Loaded %zu leg contacts from config", config_.leg_contact_names.size());
|
||||
}
|
||||
|
||||
// 加载手臂接触点名称
|
||||
if (robot_parts["arm_contacts"]) {
|
||||
config_.arm_contact_names = robot_parts["arm_contacts"].as<std::vector<std::string>>();
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"Loaded %zu arm contacts from config", config_.arm_contact_names.size());
|
||||
}
|
||||
} else {
|
||||
RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"Config file does not contain 'robot_parts' section");
|
||||
}
|
||||
} catch (const YAML::Exception& e) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"Failed to parse config file '%s': %s", config_file.c_str(), e.what());
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"Error loading config file '%s': %s", config_file.c_str(), e.what());
|
||||
}
|
||||
}
|
||||
|
||||
void ReferenceManagerForGazebo::initializeArmPlanner() {
|
||||
if (config_.arm_joint_names.empty()) {
|
||||
RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"No arm joints configured, arm planner will not be initialized");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!robot_info_loader_ || !robot_info_loader_->isLoaded()) {
|
||||
RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"RobotInfoLoader not available, cannot initialize arm planner");
|
||||
return;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"Arm trajectory planner initialized with %zu joints",
|
||||
config_.arm_joint_names.size());
|
||||
}
|
||||
|
||||
ocs2::scalar_array_t ReferenceManagerForGazebo::getUniqueTimePoints(ocs2::scalar_t initTime, ocs2::scalar_t finalTime) {
|
||||
std::set<ocs2::scalar_t> time_set;
|
||||
for (const auto& pre_traj : pre_target_traj_list_) {
|
||||
for (int i = pre_traj.next_point_index; i != pre_traj.final_point_index; i = (i + 1) % MAX_PRE_TARGET_TRAJ_CNT) {
|
||||
if (pre_traj.aim_time[i] > initTime - 1e-9) {
|
||||
if (pre_traj.aim_time[i] < finalTime + 1e-9) {
|
||||
time_set.insert(pre_traj.aim_time[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return ocs2::scalar_array_t(time_set.begin(), time_set.end());
|
||||
}
|
||||
|
||||
void ReferenceManagerForGazebo::UpdateTargetTraj
|
||||
(ocs2::scalar_t initTime, ocs2::scalar_t finalTime, const ocs2::vector_t& initState, ocs2::TargetTrajectories& targetTrajectories)
|
||||
{
|
||||
ocs2::scalar_array_t time_trajectory = getUniqueTimePoints(initTime, finalTime);
|
||||
if (time_trajectory.empty()) {
|
||||
return;
|
||||
}
|
||||
std::sort(time_trajectory.begin(), time_trajectory.end());
|
||||
// 获取状态维度
|
||||
const size_t state_dim = initState.size();
|
||||
|
||||
// 构建状态轨迹
|
||||
ocs2::vector_array_t state_trajectory;
|
||||
state_trajectory.reserve(time_trajectory.size());
|
||||
ocs2::vector_t state = initState;
|
||||
|
||||
for (const auto& time : time_trajectory) {
|
||||
|
||||
// 对每个 PreTargetTraj，根据时间插值获取关节位置
|
||||
for (auto& pre_traj : pre_target_traj_list_) {
|
||||
// 检查 joint_id 是否有效
|
||||
if (pre_traj.joint_id < 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 计算状态向量中的关节位置索引（从12开始）
|
||||
const size_t state_joint_idx = 12 + static_cast<size_t>(pre_traj.joint_id);
|
||||
if (state_joint_idx >= state_dim) {
|
||||
RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"Joint index %d maps to state index %zu which exceeds state dimension %zu",
|
||||
pre_traj.joint_id, state_joint_idx, state_dim);
|
||||
continue;
|
||||
}
|
||||
|
||||
for (int i = pre_traj.next_point_index; i != pre_traj.final_point_index; i = (i + 1) % MAX_PRE_TARGET_TRAJ_CNT) {
|
||||
const double t0 = pre_traj.aim_time[i];
|
||||
const double t1 = pre_traj.aim_time[(i + 1) % MAX_PRE_TARGET_TRAJ_CNT];
|
||||
|
||||
// 如果时间在区间内，进行线性插值
|
||||
if (time >= t0 - 1e-9 && time <= t1 + 1e-9) {
|
||||
const double p0 = pre_traj.position[i];
|
||||
const double p1 = pre_traj.position[(i + 1) % MAX_PRE_TARGET_TRAJ_CNT];
|
||||
|
||||
if (std::abs(t1 - t0) > 1e-9) {
|
||||
const double alpha = (time - t0) / (t1 - t0);
|
||||
state(state_joint_idx) = p0 + alpha * (p1 - p0);
|
||||
} else {
|
||||
state(state_joint_idx) = p0;
|
||||
}
|
||||
pre_traj.next_point_index = (i + 1) % MAX_PRE_TARGET_TRAJ_CNT;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
state_trajectory.push_back(std::move(state));
|
||||
}
|
||||
|
||||
// 构建 TargetTrajectories
|
||||
targetTrajectories.timeTrajectory = time_trajectory;
|
||||
targetTrajectories.stateTrajectory = state_trajectory;
|
||||
// inputTrajectory 可以为空，如果需要可以后续添加
|
||||
|
||||
RCLCPP_DEBUG(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"Updated target trajectories with %zu time points", time_trajectory.size());
|
||||
}
|
||||
|
||||
void ReferenceManagerForGazebo::modifyReferences(ocs2::scalar_t initTime,
|
||||
ocs2::scalar_t finalTime,
|
||||
const ocs2::vector_t& initState,
|
||||
ocs2::TargetTrajectories& targetTrajectories,
|
||||
ocs2::ModeSchedule& modeSchedule) {
|
||||
// 1. 处理腿部模式（基本模式管理）
|
||||
modeSchedule = generateBasicLegModeSchedule(initTime, finalTime, initState);
|
||||
|
||||
UpdateTargetTraj(initTime, finalTime, initState, targetTrajectories);
|
||||
}
|
||||
|
||||
ocs2::ModeSchedule ReferenceManagerForGazebo::generateBasicLegModeSchedule(
|
||||
ocs2::scalar_t /* initTime */,
|
||||
ocs2::scalar_t /* finalTime */,
|
||||
const ocs2::vector_t& /* initState */) {
|
||||
|
||||
// 基本实现：生成一个简单的四腿支撑模式
|
||||
// 可以根据需要扩展为更复杂的步态模式
|
||||
|
||||
// 使用 STANCE 模式（所有腿都接触地面）
|
||||
// 模式编号 15 对应 STANCE（所有四条腿都接触）
|
||||
constexpr size_t STANCE_MODE = 15;
|
||||
|
||||
ocs2::ModeSchedule modeSchedule;
|
||||
modeSchedule.eventTimes = {}; // 没有模式切换
|
||||
modeSchedule.modeSequence = {STANCE_MODE}; // 单一模式
|
||||
|
||||
return modeSchedule;
|
||||
}
|
||||
@@ -0,0 +1,165 @@
|
||||
#include "robot_info_loader.hpp"
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
|
||||
RobotInfoLoader::RobotInfoLoader(const std::string& urdf_content)
|
||||
: is_loaded_(false)
|
||||
{
|
||||
// 保存URDF内容
|
||||
robot_info_.urdf_content = urdf_content;
|
||||
createPinocchioInterface();
|
||||
|
||||
// 解析URDF - 使用urdf_parser
|
||||
urdf_model_ = urdf::parseURDF(urdf_content);
|
||||
|
||||
if (!urdf_model_) {
|
||||
throw std::runtime_error("[RobotInfoLoader] Failed to parse URDF content");
|
||||
}
|
||||
|
||||
// 提取机器人信息
|
||||
if (!parseUrdfModel()) {
|
||||
throw std::runtime_error("[RobotInfoLoader] Failed to parse URDF model");
|
||||
}
|
||||
|
||||
is_loaded_ = true;
|
||||
}
|
||||
|
||||
RobotInfoLoader::~RobotInfoLoader()
|
||||
{
|
||||
clear();
|
||||
}
|
||||
|
||||
bool RobotInfoLoader::parseUrdfModel()
|
||||
{
|
||||
if (!urdf_model_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto& pinocchio_model = pinocchio_interface_->getModel();
|
||||
// 获取机器人名称
|
||||
robot_info_.robot_name = urdf_model_->getName();
|
||||
robot_info_.joint_count = pinocchio_model.njoints;
|
||||
|
||||
robot_info_.joint_limits_lower.resize(robot_info_.joint_count, -M_PI);
|
||||
robot_info_.joint_limits_upper.resize(robot_info_.joint_count, M_PI);
|
||||
robot_info_.joint_limits_velocity.resize(robot_info_.joint_count, 10.0);
|
||||
robot_info_.joint_limits_effort.resize(robot_info_.joint_count, 100.0);
|
||||
robot_info_.joint_names.resize(robot_info_.joint_count, "");
|
||||
robot_info_.joint_types.resize(robot_info_.joint_count, 0);
|
||||
int link_count = pinocchio_model.nframes;
|
||||
robot_info_.link_names.resize(link_count, "");
|
||||
|
||||
// 获取根链接（基座链接）
|
||||
if (urdf_model_->getRoot()) {
|
||||
robot_info_.base_link_name = urdf_model_->getRoot()->name;
|
||||
}
|
||||
|
||||
// 遍历所有关节
|
||||
std::map<std::string, urdf::JointSharedPtr> joints = urdf_model_->joints_;
|
||||
|
||||
for (const auto& joint_pair : joints) {
|
||||
const std::string& joint_name = joint_pair.first;
|
||||
urdf::JointSharedPtr joint = joint_pair.second;
|
||||
|
||||
// 跳过固定关节（fixed joints）
|
||||
if (joint->type == urdf::Joint::FIXED) {
|
||||
continue;
|
||||
}
|
||||
int joint_index = pinocchio_model.getJointId(joint_name);
|
||||
if (joint_index == -1) {
|
||||
continue;
|
||||
}
|
||||
robot_info_.joint_names[joint_index] = joint_name;
|
||||
robot_info_.joint_types[joint_index] = joint->type;
|
||||
robot_info_.joint_limits_lower[joint_index] = joint->limits->lower;
|
||||
robot_info_.joint_limits_upper[joint_index] = joint->limits->upper;
|
||||
robot_info_.joint_limits_velocity[joint_index] = joint->limits->velocity;
|
||||
robot_info_.joint_limits_effort[joint_index] = joint->limits->effort;
|
||||
// 建立索引映射
|
||||
robot_info_.joint_name_to_index[joint_name] = joint_index;
|
||||
}
|
||||
// 遍历所有链接
|
||||
std::map<std::string, urdf::LinkSharedPtr> links = urdf_model_->links_;
|
||||
for (const auto& link_pair : links) {
|
||||
int link_index = pinocchio_model.getFrameId(link_pair.first);
|
||||
if (link_index == -1) {
|
||||
continue;
|
||||
}
|
||||
robot_info_.link_names[link_index] = link_pair.first;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int RobotInfoLoader::getJointIndex(const std::string& joint_name) const
|
||||
{
|
||||
auto it = robot_info_.joint_name_to_index.find(joint_name);
|
||||
if (it != robot_info_.joint_name_to_index.end()) {
|
||||
return static_cast<int>(it->second);
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::string RobotInfoLoader::getJointName(int joint_index) const
|
||||
{
|
||||
if (joint_index < 0 || joint_index >= robot_info_.joint_count) {
|
||||
return "";
|
||||
}
|
||||
return robot_info_.joint_names[joint_index];
|
||||
}
|
||||
|
||||
void RobotInfoLoader::printRobotInfo() const
|
||||
{
|
||||
if (!is_loaded_) {
|
||||
std::cout << "[RobotInfoLoader] No robot info loaded" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
std::cout << "\n========== Robot Information ==========" << std::endl;
|
||||
std::cout << "Robot Name: " << robot_info_.robot_name << std::endl;
|
||||
std::cout << "Base Link: " << robot_info_.base_link_name << std::endl;
|
||||
std::cout << "Number of Joints: " << robot_info_.joint_names.size() << std::endl;
|
||||
std::cout << "Number of Links: " << robot_info_.link_names.size() << std::endl;
|
||||
|
||||
std::cout << "\n--- Joints ---" << std::endl;
|
||||
for (size_t i = 0; i < robot_info_.joint_names.size(); i++) {
|
||||
const std::string& joint_name = robot_info_.joint_names[i];
|
||||
std::cout << " [" << i << "] " << joint_name;
|
||||
|
||||
std::cout << " (type: " << robot_info_.joint_types[i] << ")";
|
||||
|
||||
std::cout << " [limits: " << robot_info_.joint_limits_lower[i]
|
||||
<< " to " << robot_info_.joint_limits_upper[i] << "]";
|
||||
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
std::cout << "\n--- Links ---" << std::endl;
|
||||
for (size_t i = 0; i < robot_info_.link_names.size(); i++) {
|
||||
std::cout << " [" << i << "] " << robot_info_.link_names[i] << std::endl;
|
||||
}
|
||||
|
||||
std::cout << "========================================\n" << std::endl;
|
||||
}
|
||||
|
||||
void RobotInfoLoader::createPinocchioInterface()
|
||||
{
|
||||
try {
|
||||
pinocchio_interface_ = ocs2::getPinocchioInterfaceFromUrdfString(robot_info_.urdf_content);
|
||||
} catch (const std::exception& e) {
|
||||
throw std::runtime_error("[RobotInfoLoader] Failed to create PinocchioInterface: " + std::string(e.what()));
|
||||
}
|
||||
}
|
||||
|
||||
void RobotInfoLoader::clear()
|
||||
{
|
||||
robot_info_ = RobotInfo();
|
||||
urdf_model_.reset();
|
||||
pinocchio_interface_.reset(); // 重置 optional
|
||||
is_loaded_ = false;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,369 @@
|
||||
#include <cstdio>
|
||||
#include <sstream>
|
||||
#include <iomanip>
|
||||
#include "common.h"
|
||||
#include "robot_simulator.hpp"
|
||||
|
||||
RobotSimulator::RobotSimulator()
|
||||
: Node("robot_simulator")
|
||||
{
|
||||
// 初始化图像计数器
|
||||
for (int i = 0; i < CAMERA_TYPE_CNT; i++) {
|
||||
for (int j = 0; j < IMAGE_TYPE_CNT; j++) {
|
||||
image_count_[i][j] = 0;
|
||||
}
|
||||
}
|
||||
// 初始化相机信息接收标志
|
||||
for (int i = 0; i < CAMERA_TYPE_CNT; i++) {
|
||||
for (int j = 0; j < IMAGE_TYPE_CNT; j++) {
|
||||
camera_info_received_[i][j] = false;
|
||||
}
|
||||
}
|
||||
|
||||
// 创建 ImgMsg 发布者
|
||||
img_msg_pub_ = this->create_publisher<interfaces::msg::ImgMsg>("/img_msg", 10);
|
||||
|
||||
// 声明并获取参数
|
||||
this->declare_parameter<std::string>("joint_data_log_path", "");
|
||||
this->declare_parameter<std::string>("joint_controllers_yaml", "");
|
||||
this->declare_parameter<std::string>("robot_description", "");
|
||||
|
||||
std::string joint_data_log_path;
|
||||
std::string joint_controllers_yaml;
|
||||
std::string urdf_content;
|
||||
this->get_parameter("joint_data_log_path", joint_data_log_path);
|
||||
this->get_parameter("joint_controllers_yaml", joint_controllers_yaml);
|
||||
this->get_parameter("robot_description", urdf_content);
|
||||
|
||||
// 加载机器人信息
|
||||
if (!urdf_content.empty()) {
|
||||
reference_manager_for_gazebo_ = std::make_unique<ReferenceManagerForGazebo>(urdf_content);
|
||||
} else {
|
||||
RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()), "robot_description parameter is empty, skipping robot info loading");
|
||||
}
|
||||
|
||||
data_collector_ = new DataCollector(joint_controllers_yaml, joint_data_log_path, this);
|
||||
|
||||
// ========== Head Camera 订阅者 ==========
|
||||
head_rgb_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
|
||||
"/head_camera/image_raw", 10,
|
||||
[this](const sensor_msgs::msg::Image::SharedPtr msg) {
|
||||
this->GetImageCallback(msg, CAMERA_TYPE_HEAD, IMAGE_TYPE_RGB);
|
||||
});
|
||||
|
||||
head_depth_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
|
||||
"/head_camera/depth/image_raw", 10,
|
||||
[this](const sensor_msgs::msg::Image::SharedPtr msg) {
|
||||
this->GetImageCallback(msg, CAMERA_TYPE_HEAD, IMAGE_TYPE_DEPTH);
|
||||
});
|
||||
|
||||
head_rgb_info_sub_ = this->create_subscription<sensor_msgs::msg::CameraInfo>(
|
||||
"/head_camera/camera_info", 10,
|
||||
[this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
|
||||
this->cameraInfoCallback(msg, CAMERA_TYPE_HEAD, IMAGE_TYPE_RGB);
|
||||
});
|
||||
|
||||
head_depth_info_sub_ = this->create_subscription<sensor_msgs::msg::CameraInfo>(
|
||||
"/head_camera/depth/camera_info", 10,
|
||||
[this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
|
||||
this->cameraInfoCallback(msg, CAMERA_TYPE_HEAD, IMAGE_TYPE_DEPTH);
|
||||
});
|
||||
|
||||
// ========== Left Arm Camera 订阅者 ==========
|
||||
left_arm_rgb_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
|
||||
"/left_arm_camera/image_raw", 10,
|
||||
[this](const sensor_msgs::msg::Image::SharedPtr msg) {
|
||||
this->GetImageCallback(msg, CAMERA_TYPE_LEFT_ARM, IMAGE_TYPE_RGB);
|
||||
});
|
||||
|
||||
left_arm_depth_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
|
||||
"/left_arm_camera/depth/image_raw", 10,
|
||||
[this](const sensor_msgs::msg::Image::SharedPtr msg) {
|
||||
this->GetImageCallback(msg, CAMERA_TYPE_LEFT_ARM, IMAGE_TYPE_DEPTH);
|
||||
});
|
||||
|
||||
left_arm_rgb_info_sub_ = this->create_subscription<sensor_msgs::msg::CameraInfo>(
|
||||
"/left_arm_camera/camera_info", 10,
|
||||
[this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
|
||||
this->cameraInfoCallback(msg, CAMERA_TYPE_LEFT_ARM, IMAGE_TYPE_RGB);
|
||||
});
|
||||
|
||||
left_arm_depth_info_sub_ = this->create_subscription<sensor_msgs::msg::CameraInfo>(
|
||||
"/left_arm_camera/depth/camera_info", 10,
|
||||
[this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
|
||||
this->cameraInfoCallback(msg, CAMERA_TYPE_LEFT_ARM, IMAGE_TYPE_DEPTH);
|
||||
});
|
||||
|
||||
// ========== Right Arm Camera 订阅者 ==========
|
||||
right_arm_rgb_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
|
||||
"/right_arm_camera/image_raw", 10,
|
||||
[this](const sensor_msgs::msg::Image::SharedPtr msg) {
|
||||
this->GetImageCallback(msg, CAMERA_TYPE_RIGHT_ARM, IMAGE_TYPE_RGB);
|
||||
});
|
||||
|
||||
right_arm_depth_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
|
||||
"/right_arm_camera/depth/image_raw", 10,
|
||||
[this](const sensor_msgs::msg::Image::SharedPtr msg) {
|
||||
this->GetImageCallback(msg, CAMERA_TYPE_RIGHT_ARM, IMAGE_TYPE_DEPTH);
|
||||
});
|
||||
|
||||
right_arm_rgb_info_sub_ = this->create_subscription<sensor_msgs::msg::CameraInfo>(
|
||||
"/right_arm_camera/camera_info", 10,
|
||||
[this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
|
||||
this->cameraInfoCallback(msg, CAMERA_TYPE_RIGHT_ARM, IMAGE_TYPE_RGB);
|
||||
});
|
||||
|
||||
right_arm_depth_info_sub_ = this->create_subscription<sensor_msgs::msg::CameraInfo>(
|
||||
"/right_arm_camera/depth/camera_info", 10,
|
||||
[this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
|
||||
this->cameraInfoCallback(msg, CAMERA_TYPE_RIGHT_ARM, IMAGE_TYPE_DEPTH);
|
||||
});
|
||||
|
||||
const char* linkName[] = {"baselink", "bodylink"};
|
||||
baseLinkCallbackCnt = 0;
|
||||
bodyLinkCallbackCnt = 0;
|
||||
// ========== IMU 订阅者 - 监测 base_link 受力情况 ==========
|
||||
base_link_imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
|
||||
"/base_link_imu", 10,
|
||||
[this, linkName](const sensor_msgs::msg::Imu::SharedPtr msg) {
|
||||
bool boardcast_start = false;
|
||||
baseLinkCallbackCnt++;
|
||||
if (baseLinkCallbackCnt >= 1000) {
|
||||
boardcast_start = true;
|
||||
baseLinkCallbackCnt = 0;
|
||||
}
|
||||
this->imuCallback(msg, linkName[0], boardcast_start);
|
||||
});
|
||||
body_link_imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
|
||||
"/body_link_imu", 10,
|
||||
[this, linkName](const sensor_msgs::msg::Imu::SharedPtr msg) {
|
||||
bool boardcast_start = false;
|
||||
bodyLinkCallbackCnt++;
|
||||
if (bodyLinkCallbackCnt >= 1000) {
|
||||
boardcast_start = true;
|
||||
bodyLinkCallbackCnt = 0;
|
||||
}
|
||||
this->imuCallback(msg, linkName[1], boardcast_start);
|
||||
});
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Subscribed to /base_link_imu and /body_link_imu for force monitoring");
|
||||
|
||||
// ========== 关节力矩监测订阅者 ==========
|
||||
left_arm_effort_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"/left_arm_state_broadcaster/joint_states", 10,
|
||||
[this](const sensor_msgs::msg::JointState::SharedPtr msg) {
|
||||
this->jointEffortCallback(msg);
|
||||
});
|
||||
|
||||
right_arm_effort_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"/right_arm_state_broadcaster/joint_states", 10,
|
||||
[this](const sensor_msgs::msg::JointState::SharedPtr msg) {
|
||||
this->jointEffortCallback(msg);
|
||||
});
|
||||
|
||||
body_effort_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"/body_state_broadcaster/joint_states", 10,
|
||||
[this](const sensor_msgs::msg::JointState::SharedPtr msg) {
|
||||
this->jointEffortCallback(msg);
|
||||
});
|
||||
|
||||
left_gripper_effort_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"/left_gripper_state_broadcaster/joint_states", 10,
|
||||
[this](const sensor_msgs::msg::JointState::SharedPtr msg) {
|
||||
this->jointEffortCallback(msg);
|
||||
});
|
||||
|
||||
right_gripper_effort_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"/right_gripper_state_broadcaster/joint_states", 10,
|
||||
[this](const sensor_msgs::msg::JointState::SharedPtr msg) {
|
||||
this->jointEffortCallback(msg);
|
||||
});
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Subscribed to joint effort topics for torque monitoring");
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "RobotSimulator initialized");
|
||||
}
|
||||
|
||||
RobotSimulator::~RobotSimulator()
|
||||
{
|
||||
delete data_collector_;
|
||||
}
|
||||
|
||||
void RobotSimulator::cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr msg, int cameraType, int imageType)
|
||||
{
|
||||
// 只在第一次接收时保存，相机参数通常不会变化
|
||||
if (!camera_info_received_[cameraType][imageType]) {
|
||||
// 复制 K 矩阵 (3x3 = 9 个元素)
|
||||
for (size_t i = 0; i < 9 && i < msg->k.size(); i++) {
|
||||
karr_[cameraType][imageType][i] = msg->k[i];
|
||||
}
|
||||
|
||||
// 复制 D 畸变系数
|
||||
darr_[cameraType][imageType].assign(msg->d.begin(), msg->d.end());
|
||||
|
||||
camera_info_received_[cameraType][imageType] = true;
|
||||
|
||||
static const char* camera_names[CAMERA_TYPE_CNT][IMAGE_TYPE_CNT] = {
|
||||
{"Head RGB", "Head Depth"},
|
||||
{"Left Arm RGB", "Left Arm Depth"},
|
||||
{"Right Arm RGB", "Right Arm Depth"}
|
||||
};
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s CameraInfo received: K[0]=%f, K[4]=%f, D size=%zu",
|
||||
camera_names[cameraType][imageType],
|
||||
karr_[cameraType][imageType][0], karr_[cameraType][imageType][4],
|
||||
darr_[cameraType][imageType].size());
|
||||
}
|
||||
}
|
||||
|
||||
const std::string position[CAMERA_TYPE_CNT] = {"top", "left", "right"};
|
||||
|
||||
void RobotSimulator::publishImgMsg(int cameraType)
|
||||
{
|
||||
// 检查图像是否已接收
|
||||
if (image_count_[cameraType][0] == 0 || image_count_[cameraType][1] == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (camera_info_received_[cameraType][0] == false || camera_info_received_[cameraType][1] == false) {
|
||||
RCLCPP_WARN_THROTTLE(rclcpp::get_logger(GET_FUNC_LINE_STR()), *this->get_clock(), 5000,
|
||||
"Cannot publish ImgMsg: camera info not ready (rgb=%s, depth=%s)",
|
||||
camera_info_received_[cameraType][0] ? "ok" : "empty",
|
||||
camera_info_received_[cameraType][1] ? "ok" : "empty");
|
||||
return;
|
||||
}
|
||||
|
||||
auto msg = interfaces::msg::ImgMsg();
|
||||
|
||||
// 填充 RGB 图像
|
||||
msg.image_color = image_[cameraType][0];
|
||||
|
||||
// 填充深度图像
|
||||
msg.image_depth = image_[cameraType][1];
|
||||
|
||||
// 填充相机内外参
|
||||
msg.karr.assign(karr_[cameraType][0].begin(), karr_[cameraType][0].end());
|
||||
msg.darr.assign(darr_[cameraType][0].begin(), darr_[cameraType][0].end());
|
||||
|
||||
msg.source = "realsence";
|
||||
msg.position = position[cameraType];
|
||||
msg.type = "myType";
|
||||
|
||||
// 发布消息
|
||||
img_msg_pub_->publish(msg);
|
||||
image_count_[cameraType][0] = 0;
|
||||
image_count_[cameraType][1] = 0;
|
||||
|
||||
RCLCPP_INFO_THROTTLE(rclcpp::get_logger(GET_FUNC_LINE_STR()), *this->get_clock(), 5000,
|
||||
"Published ImgMsg: position=%s, rgb=%dx%d, depth=%dx%d",
|
||||
position[cameraType].c_str(),
|
||||
msg.image_color.width, msg.image_color.height,
|
||||
msg.image_depth.width, msg.image_depth.height);
|
||||
}
|
||||
|
||||
void RobotSimulator::GetImageCallback(const sensor_msgs::msg::Image::SharedPtr msg, int cameraType, int imageType)
|
||||
{
|
||||
image_[cameraType][imageType] = *msg;
|
||||
image_count_[cameraType][imageType]++;
|
||||
|
||||
publishImgMsg(cameraType);
|
||||
}
|
||||
|
||||
void RobotSimulator::jointEffortCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(boardcast_mutex_);
|
||||
// 检查是否有力矩数据
|
||||
if (msg->effort.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
bool boardcast_start = false;
|
||||
|
||||
for (size_t i = 0; i < msg->name.size(); i++) {
|
||||
std::string joint_name = msg->name[i];
|
||||
auto it = joint_effort_map_.find(joint_name);
|
||||
if (it == joint_effort_map_.end()) {
|
||||
joint_effort_map_[joint_name] = JointEffort();
|
||||
joint_effort_map_[joint_name].first_effort = msg->effort[i];
|
||||
joint_effort_map_[joint_name].effort_sum = msg->effort[i];
|
||||
joint_effort_map_[joint_name].get_cnt = 1;
|
||||
joint_effort_map_[joint_name].avg_effort = msg->effort[i];
|
||||
joint_effort_map_[joint_name].max_effort = msg->effort[i];
|
||||
joint_effort_map_[joint_name].min_effort = msg->effort[i];
|
||||
joint_effort_map_[joint_name].position_max = msg->position[i];
|
||||
joint_effort_map_[joint_name].position_min = msg->position[i];
|
||||
} else {
|
||||
joint_effort_map_[joint_name].max_effort = std::max(joint_effort_map_[joint_name].max_effort, msg->effort[i]);
|
||||
joint_effort_map_[joint_name].min_effort = std::min(joint_effort_map_[joint_name].min_effort, msg->effort[i]);
|
||||
joint_effort_map_[joint_name].effort_sum += msg->effort[i];
|
||||
joint_effort_map_[joint_name].get_cnt++;
|
||||
joint_effort_map_[joint_name].avg_effort = joint_effort_map_[joint_name].effort_sum / joint_effort_map_[joint_name].get_cnt;
|
||||
joint_effort_map_[joint_name].position_max = std::max(joint_effort_map_[joint_name].position_max, msg->position[i]);
|
||||
joint_effort_map_[joint_name].position_min = std::min(joint_effort_map_[joint_name].position_min, msg->position[i]);
|
||||
if (joint_effort_map_[joint_name].get_cnt >= 1000) {
|
||||
boardcast_start = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (boardcast_start) {
|
||||
for (auto it = joint_effort_map_.begin(); it != joint_effort_map_.end(); it++) {
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"Joint Effort: %s, Max: %f, Min: %f, Avg: %f, pos_max: %f, pos_min: %f",
|
||||
it->first.c_str(), it->second.max_effort, it->second.min_effort,
|
||||
it->second.avg_effort, it->second.position_max, it->second.position_min);
|
||||
it->second.effort_sum = 0;
|
||||
it->second.get_cnt = 0;
|
||||
it->second.avg_effort = 0;
|
||||
double posion_max = it->second.position_max;
|
||||
it->second.position_max = it->second.position_min;
|
||||
it->second.position_min = posion_max;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void RobotSimulator::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg, const char* linkName, bool boardcast_start)
|
||||
{
|
||||
// 获取线性加速度 (可以通过 F = m * a 推算力)
|
||||
double ax = msg->linear_acceleration.x;
|
||||
double ay = msg->linear_acceleration.y;
|
||||
double az = msg->linear_acceleration.z;
|
||||
|
||||
// 获取角速度
|
||||
double wx = msg->angular_velocity.x;
|
||||
double wy = msg->angular_velocity.y;
|
||||
double wz = msg->angular_velocity.z;
|
||||
|
||||
// 计算加速度幅值 (正常情况下应该接近重力加速度 9.8 m/s^2)
|
||||
double accel_magnitude = std::sqrt(ax*ax + ay*ay + az*az);
|
||||
|
||||
// 计算角速度幅值
|
||||
double angular_magnitude = std::sqrt(wx*wx + wy*wy + wz*wz);
|
||||
|
||||
// 阈值检测：如果加速度过大或角速度过大，打印警告
|
||||
const double ACCEL_THRESHOLD = 50.0; // m/s^2，超过这个值认为异常
|
||||
const double ANGULAR_THRESHOLD = 10.0; // rad/s，超过这个值认为异常
|
||||
|
||||
if (boardcast_start) {
|
||||
if (accel_magnitude > ACCEL_THRESHOLD || angular_magnitude > ANGULAR_THRESHOLD) {
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"%s Accel: (%.2f, %.2f, %.2f) |a|=%.2f m/s^2, "
|
||||
"Angular: (%.2f, %.2f, %.2f) |w|=%.2f rad/s",
|
||||
linkName, ax, ay, az, accel_magnitude,
|
||||
wx, wy, wz, angular_magnitude);
|
||||
} else {
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()),
|
||||
"%s Accel: (%.2f, %.2f, %.2f) |a|=%.2f m/s^2, Angular: (%.2f, %.2f, %.2f) |w|=%.2f rad/s",
|
||||
linkName, ax, ay, az, accel_magnitude,
|
||||
wx, wy, wz, angular_magnitude);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<RobotSimulator>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,55 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version='1.7'>
|
||||
<model name='FHrobot_test'>
|
||||
<link name='base_link'>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<pose>-1e-06 0.024255 0 0 -0 0</pose>
|
||||
<mass>0.789297</mass>
|
||||
<inertia>
|
||||
<ixx>0.001617</ixx>
|
||||
<ixy>-2.41988e-09</ixy>
|
||||
<ixz>-3.3642e-10</ixz>
|
||||
<iyy>0.001617</iyy>
|
||||
<iyz>-5.13093e-10</iyz>
|
||||
<izz>0.002946</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='base_link_test_collision'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>/home/zj/hivecore/hivecore_robot_simulator/robot_simulator/meshes/right_front_wheel_link.STL</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>6.0</mu>
|
||||
<mu2>6.0</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode>
|
||||
<kp>5e7</kp>
|
||||
<kd>10.0</kd>
|
||||
<min_depth>0.003</min_depth>
|
||||
<max_vel>1.0</max_vel>
|
||||
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='base_link_test_visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>/home/zj/hivecore/hivecore_robot_simulator/robot_simulator/meshes/right_front_wheel_link.STL</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,248 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.6">
|
||||
<world name="camera_test_world">
|
||||
|
||||
<!-- Link Attacher 插件 - 用于模拟抓取 -->
|
||||
<plugin name="gazebo_link_attacher" filename="libgazebo_link_attacher.so"/>
|
||||
|
||||
<!-- 光源 -->
|
||||
<light name="sun" type="directional">
|
||||
<cast_shadows>true</cast_shadows>
|
||||
<pose>0 0 10 0 0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
<attenuation>
|
||||
<range>1000</range>
|
||||
<constant>0.9</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<direction>-0.5 0.1 -0.9</direction>
|
||||
</light>
|
||||
|
||||
<!-- 地面 -->
|
||||
<model name="ground_plane">
|
||||
<static>true</static>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>10</mu>
|
||||
<mu2>10</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode>
|
||||
<kp>1e9</kp>
|
||||
<kd>1000.0</kd>
|
||||
<min_depth>0.003</min_depth>
|
||||
<max_vel>1.0</max_vel>
|
||||
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<cast_shadows>false</cast_shadows>
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.3 0.3 0.3 1</ambient>
|
||||
<diffuse>0.7 0.7 0.7 1</diffuse>
|
||||
<specular>0.01 0.01 0.01 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<!-- 红色立方体 - 前方 -->
|
||||
<model name="red_box">
|
||||
<static>true</static>
|
||||
<pose>3.0 0 0.15 0 0 0</pose>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.3 0.3 0.3</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.3 0.3 0.3</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.8 0.1 0.1 1</ambient>
|
||||
<diffuse>0.8 0.1 0.1 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<model name="red_box_2">
|
||||
<static>false</static>
|
||||
<pose>0.685 -0.355 1.077 0 0 0</pose>
|
||||
<link name="link">
|
||||
<inertial>
|
||||
<mass>0.05</mass>
|
||||
<inertia>
|
||||
<ixx>0.000042</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.000042</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.0000008</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.02 0.02 0.1</size> <!-- 宽 x 深 x 高 -->
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>5.0</mu>
|
||||
<mu2>4.0</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode>
|
||||
<kp>100000.0</kp>
|
||||
<kd>1000.0</kd>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.02 0.02 0.1</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.8 0.1 0.1 1</ambient>
|
||||
<diffuse>0.8 0.1 0.1 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<model name="red_cylinder">
|
||||
<static>false</static>
|
||||
<pose>0.65 -0.355 1.077 0 0 0</pose>
|
||||
<link name="link">
|
||||
<inertial>
|
||||
<mass>0.05</mass>
|
||||
<inertia>
|
||||
<ixx>0.0000425</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.0000425</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.0000025</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.01</radius>
|
||||
<length>0.1</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>5</mu>
|
||||
<mu2>4</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode>
|
||||
<kp>100000.0</kp>
|
||||
<kd>200.0</kd>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.01</radius>
|
||||
<length>0.1</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.8 0.1 0.1 1</ambient>
|
||||
<diffuse>0.8 0.1 0.1 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<!-- 新增：桌子 -->
|
||||
<include>
|
||||
<uri>file:///home/zj/hivecore/hivecore_lerobot_brain/lerobot_brain/props/table</uri>
|
||||
<name>table_at_1_5m</name>
|
||||
<pose>0.98 0 0 0 0 1.57</pose>
|
||||
</include>
|
||||
|
||||
<!-- 新增：桌子上的纸箱 -->
|
||||
<include>
|
||||
<uri>file:///home/zj/hivecore/hivecore_lerobot_brain/lerobot_brain/props/wooden_case</uri>
|
||||
<name>box_on_table</name>
|
||||
<pose>1.2 0 1.077 0 -1.5707 0</pose>
|
||||
</include>
|
||||
|
||||
<!-- 新增：桌子上的可乐罐 -->
|
||||
<include>
|
||||
<uri>file:///home/zj/hivecore/hivecore_lerobot_brain/lerobot_brain/props/coke_can</uri>
|
||||
<name>coke_on_table</name>
|
||||
<pose>0.8 0.4 1.077 0 0 0</pose>
|
||||
</include>
|
||||
|
||||
<!-- 物理引擎设置 - 使用 ODE 引擎 -->
|
||||
<physics type="ode">
|
||||
<real_time_update_rate>1000.0</real_time_update_rate>
|
||||
<max_step_size>0.001</max_step_size> <!-- 减小步长提高精度 -->
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
<ode>
|
||||
<solver>
|
||||
<type>quick</type>
|
||||
<iters>200</iters> <!-- 增加迭代次数 -->
|
||||
<sor>1.3</sor>
|
||||
</solver>
|
||||
<constraints>
|
||||
<cfm>1e-7</cfm>
|
||||
<erp>0.2</erp>
|
||||
<contact_max_correcting_vel>100</contact_max_correcting_vel>
|
||||
<contact_surface_layer>0.001</contact_surface_layer>
|
||||
</constraints>
|
||||
</ode>
|
||||
</physics>
|
||||
|
||||
<!-- 场景设置 -->
|
||||
<scene>
|
||||
<ambient>0.4 0.4 0.4 1</ambient>
|
||||
<background>0.7 0.7 0.7 1</background>
|
||||
<shadows>false</shadows> <!-- 禁用阴影减少 GPU 负载 -->
|
||||
<grid>false</grid> <!-- 禁用网格 -->
|
||||
</scene>
|
||||
|
||||
</world>
|
||||
</sdf>
|
||||
|
||||
207
zhengjie_simulator/my_gazebo_ros2_control/CHANGELOG.rst
Normal file
207
zhengjie_simulator/my_gazebo_ros2_control/CHANGELOG.rst
Normal file
@@ -0,0 +1,207 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package gazebo_ros2_control
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.4.12 (2025-10-17)
|
||||
-------------------
|
||||
* Override gazebo reset method (`#413 <https://github.com/ros-controls/gazebo_ros2_control/issues/413>`_) (`#414 <https://github.com/ros-controls/gazebo_ros2_control/issues/414>`_)
|
||||
* Contributors: mergify[bot]
|
||||
|
||||
0.4.11 (2025-05-18)
|
||||
-------------------
|
||||
* Fix namespacing for multiple instances of gazebo_ros2_control plugin (`#181 <https://github.com/ros-controls/gazebo_ros2_control/issues/181>`_) (`#398 <https://github.com/ros-controls/gazebo_ros2_control/issues/398>`_)
|
||||
* Contributors: Ben Holden, Alejandro Hernández Cordero, Christoph Froehlich
|
||||
|
||||
0.4.10 (2024-09-17)
|
||||
-------------------
|
||||
* Add support for getting PID parameters from loaded parameters (`#374 <https://github.com/ros-controls/gazebo_ros2_control//issues/374>`_) (`#375 <https://github.com/ros-controls/gazebo_ros2_control//issues/375>`_)
|
||||
Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>
|
||||
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
|
||||
(cherry picked from commit 6a4cc84344ed1a86807dc77f23f199598a205296)
|
||||
Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>
|
||||
* Add `hold_joints` parameter (backport `#251 <https://github.com/ros-controls/gazebo_ros2_control//issues/251>`_) (`#353 <https://github.com/ros-controls/gazebo_ros2_control//issues/353>`_)
|
||||
Co-authored-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>
|
||||
* Contributors: mergify[bot]
|
||||
|
||||
0.4.9 (2024-07-02)
|
||||
------------------
|
||||
* Initialize antiwindup variable properly (`#326 <https://github.com/ros-controls/gazebo_ros2_control/issues/326>`_) (`#327 <https://github.com/ros-controls/gazebo_ros2_control/issues/327>`_)
|
||||
(cherry picked from commit 1ef9652ac34ed883dbf8fed27bcf393f78f53d52)
|
||||
Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>
|
||||
* Contributors: mergify[bot]
|
||||
|
||||
0.4.8 (2024-05-14)
|
||||
------------------
|
||||
* Add PID controller to control joint using effort (`#294 <https://github.com/ros-controls/gazebo_ros2_control//issues/294>`_)
|
||||
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
|
||||
Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>
|
||||
* Update precommit config (`#298 <https://github.com/ros-controls/gazebo_ros2_control//issues/298>`_) (`#301 <https://github.com/ros-controls/gazebo_ros2_control//issues/301>`_)
|
||||
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
|
||||
(cherry picked from commit 105c0ba5b786a43e1e9266399ab027a12011c643)
|
||||
Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>
|
||||
* Fix incorrect force-torque sensor vec population (`#296 <https://github.com/ros-controls/gazebo_ros2_control//issues/296>`_) (`#299 <https://github.com/ros-controls/gazebo_ros2_control//issues/299>`_)
|
||||
(cherry picked from commit fdcd7aa8c67ea57f44bbf2f8fba90a28d7f04b5d)
|
||||
Co-authored-by: Mateus Menezes <mateusmenezes95@gmail.com>
|
||||
* Contributors: chameau5050, mergify[bot]
|
||||
|
||||
0.4.7 (2024-03-21)
|
||||
------------------
|
||||
* Update gazebo_ros2_control_plugin.cpp (`#286 <https://github.com/ros-controls/gazebo_ros2_control/issues/286>`_) (`#287 <https://github.com/ros-controls/gazebo_ros2_control/issues/287>`_)
|
||||
(cherry picked from commit 5e1f9a52bcd1bf4164186dbb3c8b5cf070ed156e)
|
||||
Co-authored-by: Tobias Fischer <info@tobiasfischer.info>
|
||||
* set the robot description parameter (`#277 <https://github.com/ros-controls/gazebo_ros2_control/issues/277>`_)
|
||||
* Fix crashing due to an invalid parameter in the initial value. (backport `#271 <https://github.com/ros-controls/gazebo_ros2_control/issues/271>`_) (`#282 <https://github.com/ros-controls/gazebo_ros2_control/issues/282>`_)
|
||||
* Fix crashing due to an invalid parameter in the initial value. (`#271 <https://github.com/ros-controls/gazebo_ros2_control/issues/271>`_)
|
||||
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
|
||||
(cherry picked from commit cdae6b8a8f638d87146482e99bf76cf36530e5a6)
|
||||
Co-authored-by: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com>
|
||||
Co-authored-by: Alejandro Hernandez Cordero <ahcorde@gmail.com>
|
||||
* Contributors: AB, mergify[bot]
|
||||
|
||||
0.4.6 (2024-01-24)
|
||||
------------------
|
||||
* Load the URDF to the resource_manager before parsing it to CM (`#262 <https://github.com/ros-controls/gazebo_ros2_control//issues/262>`_) (`#266 <https://github.com/ros-controls/gazebo_ros2_control//issues/266>`_)
|
||||
* Load the URDF to the resource_manager before parsing it to CM constructor (fixes https://github.com/ros-controls/ros2_control/issues/1299)
|
||||
(cherry picked from commit f5baf71c4c7cb3c0a0af52f988c107b356c95ed0)
|
||||
Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>
|
||||
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
|
||||
* Fix links in documentation (`#263 <https://github.com/ros-controls/gazebo_ros2_control//issues/263>`_) (`#264 <https://github.com/ros-controls/gazebo_ros2_control//issues/264>`_)
|
||||
(cherry picked from commit d44b879615a539fc7c6c53707ec518df7bfd4f47)
|
||||
Co-authored-by: Silvio Traversaro <silvio@traversaro.it>
|
||||
* Contributors: mergify[bot]
|
||||
|
||||
0.4.5 (2024-01-04)
|
||||
------------------
|
||||
* Fix stuck passive joints (`#237 <https://github.com/ros-controls/gazebo_ros2_control/issues/237>`_) (`#238 <https://github.com/ros-controls/gazebo_ros2_control/issues/238>`_)
|
||||
(cherry picked from commit 5dba0f95a03b136f39145c846ec5ebbfb5a09599)
|
||||
Co-authored-by: Johannes Huemer <johannes.huemer@ait.ac.at>
|
||||
* Contributors: mergify[bot]
|
||||
|
||||
0.4.4 (2023-08-21)
|
||||
------------------
|
||||
* Catch pluginlib exceptions (backport `#229 <https://github.com/ros-controls/gazebo_ros2_control/issues/229>`_) (`#230 <https://github.com/ros-controls/gazebo_ros2_control/issues/230>`_)
|
||||
* Catch pluginlib exceptions (`#229 <https://github.com/ros-controls/gazebo_ros2_control/issues/229>`_)
|
||||
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
|
||||
(cherry picked from commit f8a475d3092e67b77846d76738ffad0861c680c1)
|
||||
* Set the C++ version to 17 (`#221 <https://github.com/ros-controls/gazebo_ros2_control/issues/221>`_) (`#228 <https://github.com/ros-controls/gazebo_ros2_control/issues/228>`_)
|
||||
(cherry picked from commit 6da415cf82a75e2a5e9f9a41400957ad45b2be84)
|
||||
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
|
||||
* Removed unused var (`#220 <https://github.com/ros-controls/gazebo_ros2_control/issues/220>`_) (`#226 <https://github.com/ros-controls/gazebo_ros2_control/issues/226>`_)
|
||||
(cherry picked from commit 174e6b85f82774e9e802a5540382999066734421)
|
||||
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
|
||||
* Remove plugin export from ROS 1 (`#212 <https://github.com/ros-controls/gazebo_ros2_control/issues/212>`_) (`#215 <https://github.com/ros-controls/gazebo_ros2_control/issues/215>`_)
|
||||
(cherry picked from commit c15af63cb036cd1f36cffbc56e5e5bdb5224c7e2)
|
||||
Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>
|
||||
* Forced zero vel in position mode to avoid sagging (`#213 <https://github.com/ros-controls/gazebo_ros2_control/issues/213>`_) (`#214 <https://github.com/ros-controls/gazebo_ros2_control/issues/214>`_)
|
||||
(cherry picked from commit 3e950618a1f82c72097f7c90a6b5d2ea2e32b7b8)
|
||||
Co-authored-by: gwalck <guillaume.walck@stoglrobotics.de>
|
||||
* Various bug fixes (`#177 <https://github.com/ros-controls/gazebo_ros2_control/issues/177>`_) (`#208 <https://github.com/ros-controls/gazebo_ros2_control/issues/208>`_)
|
||||
Co-authored-by: AndyZe <andyz@utexas.edu>
|
||||
* Add pre-commit and CI-format (`#206 <https://github.com/ros-controls/gazebo_ros2_control/issues/206>`_) (`#207 <https://github.com/ros-controls/gazebo_ros2_control/issues/207>`_)
|
||||
* Add pre-commit and ci-format
|
||||
(cherry picked from commit f2cf686a1a97cefc9b5e3daa115e0c4854ea5707)
|
||||
Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>
|
||||
* Contributors: Alejandro Hernández Cordero, Christoph Fröhlich, mergify[bot]
|
||||
|
||||
0.4.3 (2023-05-23)
|
||||
------------------
|
||||
* add copy operator to SafeEnum (`#197 <https://github.com/ros-controls/gazebo_ros2_control/issues/197>`_) (`#198 <https://github.com/ros-controls/gazebo_ros2_control/issues/198>`_)
|
||||
* Contributors: mergify[bot]
|
||||
|
||||
0.4.2 (2023-03-02)
|
||||
------------------
|
||||
* Export all dependencies (`#183 <https://github.com/ros-controls/gazebo_ros2_control/issues/183>`_)
|
||||
The ament_export_dependencies exports dependencies to downstream
|
||||
packages. This is necessary so that the user of the library does
|
||||
not have to call find_package for those dependencies.
|
||||
* Contributors: Adrian Zwiener
|
||||
|
||||
0.4.1 (2023-02-07)
|
||||
------------------
|
||||
* Force setting use_sim_time parameter when using plugin. (`#171 <https://github.com/ros-controls/gazebo_ros2_control/issues/171>`_)
|
||||
* Removed warning (`#162 <https://github.com/ros-controls/gazebo_ros2_control/issues/162>`_)
|
||||
* Mimic joint should have the same control mode as mimicked joint. (`#154 <https://github.com/ros-controls/gazebo_ros2_control/issues/154>`_)
|
||||
* Enable loading params from multiple yaml files (`#149 <https://github.com/ros-controls/gazebo_ros2_control/issues/149>`_)
|
||||
* Contributors: Alejandro Hernández Cordero, Denis Štogl, Tony Najjar
|
||||
|
||||
0.4.0 (2022-08-09)
|
||||
------------------
|
||||
* Implemented perform_command_mode_switch override in GazeboSystem (`#136 <https://github.com/ros-simulation/gazebo_ros2_control/issues/136>`_)
|
||||
* added namespace to controller manager (`#147 <https://github.com/ros-simulation/gazebo_ros2_control/issues/147>`_)
|
||||
* Activate all hardware in URDF (`#144 <https://github.com/ros-simulation/gazebo_ros2_control/issues/144>`_)
|
||||
* activated all hardware by default (`#143 <https://github.com/ros-simulation/gazebo_ros2_control/issues/143>`_)
|
||||
* Fix setting initial values if command interfaces are not defined. (`#110 <https://github.com/ros-simulation/gazebo_ros2_control/issues/110>`_)
|
||||
* changed name to GazeboSystem (`#142 <https://github.com/ros-simulation/gazebo_ros2_control/issues/142>`_)
|
||||
* Contributors: Denis Štogl, Keegan Sotebeer, Maciej Bednarczyk
|
||||
|
||||
0.3.1 (2022-07-05)
|
||||
------------------
|
||||
* Added logic for activating hardware interfaces (`#139 <https://github.com/ros-simulation/gazebo_ros2_control/issues/139>`_)
|
||||
* Adjust repo URL (`#134 <https://github.com/ros-simulation/gazebo_ros2_control/issues/134>`_)
|
||||
* Contributors: Alejandro Hernández Cordero, Bence Magyar
|
||||
|
||||
0.3.0 (2022-05-27)
|
||||
------------------
|
||||
* Merge pull request `#120 <https://github.com/ros-simulation/gazebo_ros2_control/issues/120>`_ from ros-simulation/ahcorde/main/117
|
||||
Adapted to Humble
|
||||
* make linters happy
|
||||
* Merge remote-tracking branch 'denis/using-under-namespace' into ahcorde/main/117
|
||||
* update read/write interface functions of ros2_control parts
|
||||
This is needed since the ros2_control interfaces have been update
|
||||
* Declare dependency of gazebo_hardware_plugins to urdf in CMakeLists.txt (`#117 <https://github.com/ros-simulation/gazebo_ros2_control/issues/117>`_)
|
||||
* ros2_control is now having usings under its namespace.
|
||||
* Fix mimic joint for effort command (`#109 <https://github.com/ros-simulation/gazebo_ros2_control/issues/109>`_)
|
||||
* Support for mimic joints and example with gripper. (`#107 <https://github.com/ros-simulation/gazebo_ros2_control/issues/107>`_)
|
||||
* Contributors: Alejandro Hernández Cordero, Christoph Fröhlich, Denis Štogl, Manuel M, Martin Wudenka, ahcorde
|
||||
|
||||
0.0.8 (2022-01-28)
|
||||
------------------
|
||||
* Enable setting default position of the simulated robot using ros2_control URDF tag. (`#100 <https://github.com/ros-simulation/gazebo_ros2_control//issues/100>`_)
|
||||
* Contributors: Denis Štogl
|
||||
|
||||
0.0.7 (2021-12-03)
|
||||
------------------
|
||||
* Pass ROS time instead of SYSTEM time to update function (`#97 <https://github.com/ros-simulation/gazebo_ros2_control//issues/97>`_)
|
||||
* Contributors: Błażej Sowa
|
||||
|
||||
0.0.6 (2021-11-18)
|
||||
------------------
|
||||
* Fix ros2_control resource manager in galatic (`#96 <https://github.com/ros-simulation/gazebo_ros2_control//issues/96>`_)
|
||||
* Contributors: Alejandro Hernández Cordero
|
||||
|
||||
0.0.4 (2021-10-26)
|
||||
------------------
|
||||
* Added testing CI (`#93 <https://github.com/ros-simulation/gazebo_ros2_control//issues/93>`_)
|
||||
Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>
|
||||
Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>
|
||||
* fix maintainer email (`#92 <https://github.com/ros-simulation/gazebo_ros2_control//issues/92>`_)
|
||||
* Galactic: Pass time and period to update function (`#88 <https://github.com/ros-simulation/gazebo_ros2_control//issues/88>`_)
|
||||
* Export interfaces created in init (`#83 <https://github.com/ros-simulation/gazebo_ros2_control//issues/83>`_)
|
||||
* Add Imu and FT state interfaces (`#65 <https://github.com/ros-simulation/gazebo_ros2_control//issues/65>`_)
|
||||
Co-authored-by: Jordan Palacios <jordan.palacios@pal-robotics.com>
|
||||
* Contributors: Alejandro Hernández Cordero, Bence Magyar, Błażej Sowa, Victor Lopez
|
||||
|
||||
0.0.3 (2021-06-16)
|
||||
------------------
|
||||
* Forward sdf ros remappings to loaded controllers (`#80 <https://github.com/ros-simulation/gazebo_ros2_control/issues/80>`_)
|
||||
Co-authored-by: Jonatan Olofsson <jonatan.olofsson@saabgroup.com>
|
||||
* Join with the controller manager's executor thread on exit (`#79 <https://github.com/ros-simulation/gazebo_ros2_control/issues/79>`_)
|
||||
* Ensure that sim_joints\_ always has the same number of elements as the… (`#77 <https://github.com/ros-simulation/gazebo_ros2_control/issues/77>`_)
|
||||
* Write joints on each simulation update period (`#78 <https://github.com/ros-simulation/gazebo_ros2_control/issues/78>`_)
|
||||
* Contributors: Jonatan Olofsson, Kenneth Bogert, Victor Lopez
|
||||
|
||||
0.0.2 (2021-04-19)
|
||||
------------------
|
||||
* add ros parameters file to node context (`#60 <https://github.com/ros-simulation/gazebo_ros2_control//issues/60>`_)
|
||||
Co-authored-by: ahcorde <ahcorde@gmail.com>
|
||||
* Expose include path (`#58 <https://github.com/ros-simulation/gazebo_ros2_control//issues/58>`_)
|
||||
* Added License file (`#55 <https://github.com/ros-simulation/gazebo_ros2_control//issues/55>`_)
|
||||
* Fixed state interfaces (`#53 <https://github.com/ros-simulation/gazebo_ros2_control//issues/53>`_)
|
||||
* Contributors: Alejandro Hernández Cordero, Chen Bainian, Karsten Knese
|
||||
|
||||
0.0.1 (2021-02-05)
|
||||
------------------
|
||||
* Updated with ros2-control Foxy API (`#44 <https://github.com/ros-simulation/gazebo_ros2_control/issues/44>`_)
|
||||
Co-authored-by: Karsten Knese <Karsten1987@users.noreply.github.com>
|
||||
* Added initial version of gazebo_ros2_control (`#1 <https://github.com/ros-simulation/gazebo_ros2_control/issues/1>`_)
|
||||
* Contributors: Alejandro Hernández Cordero, Louise Poubel, Karsten Knese, Bence Magyar
|
||||
101
zhengjie_simulator/my_gazebo_ros2_control/CMakeLists.txt
Normal file
101
zhengjie_simulator/my_gazebo_ros2_control/CMakeLists.txt
Normal file
@@ -0,0 +1,101 @@
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
cmake_policy(SET CMP0167 OLD)
|
||||
set(CMAKE_POLICY_VERSION_MINIMUM 3.5)
|
||||
project(my_gazebo_ros2_control)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(angles REQUIRED)
|
||||
find_package(controller_manager REQUIRED)
|
||||
find_package(control_toolbox REQUIRED)
|
||||
find_package(gazebo_dev REQUIRED)
|
||||
find_package(gazebo_ros REQUIRED)
|
||||
find_package(hardware_interface REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(yaml_cpp_vendor REQUIRED)
|
||||
|
||||
# Default to C++17
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wconversion -Wno-sign-conversion -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual)
|
||||
endif()
|
||||
|
||||
include_directories(include)
|
||||
link_directories(
|
||||
${gazebo_dev_LIBRARY_DIRS}
|
||||
)
|
||||
|
||||
# Libraries
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
src/gazebo_ros2_control_plugin.cpp
|
||||
)
|
||||
ament_target_dependencies(${PROJECT_NAME}
|
||||
angles
|
||||
controller_manager
|
||||
control_toolbox
|
||||
gazebo_dev
|
||||
gazebo_ros
|
||||
hardware_interface
|
||||
pluginlib
|
||||
rclcpp
|
||||
yaml_cpp_vendor
|
||||
)
|
||||
|
||||
add_library(my_gazebo_hardware_plugins SHARED
|
||||
src/gazebo_system.cpp
|
||||
)
|
||||
ament_target_dependencies(my_gazebo_hardware_plugins
|
||||
angles
|
||||
control_toolbox
|
||||
gazebo_dev
|
||||
hardware_interface
|
||||
rclcpp
|
||||
)
|
||||
|
||||
## Install
|
||||
install(TARGETS
|
||||
${PROJECT_NAME}
|
||||
my_gazebo_hardware_plugins
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
|
||||
ament_export_include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
ament_export_dependencies(
|
||||
ament_cmake
|
||||
angles
|
||||
controller_manager
|
||||
gazebo_dev
|
||||
gazebo_ros
|
||||
hardware_interface
|
||||
pluginlib
|
||||
rclcpp
|
||||
yaml_cpp_vendor
|
||||
)
|
||||
|
||||
ament_export_libraries(
|
||||
${PROJECT_NAME}
|
||||
my_gazebo_hardware_plugins
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION include/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
pluginlib_export_plugin_description_file(my_gazebo_ros2_control my_gazebo_hardware_plugins.xml)
|
||||
|
||||
ament_package()
|
||||
7
zhengjie_simulator/my_gazebo_ros2_control/README.md
Normal file
7
zhengjie_simulator/my_gazebo_ros2_control/README.md
Normal file
@@ -0,0 +1,7 @@
|
||||
# Gazebo ros_control Interfaces
|
||||
|
||||
This is a ROS 2 package for integrating the [ros2_control](https://github.com/ros-controls/ros2_control) controller architecture
|
||||
with the [Gazebo Classic](https://classic.gazebosim.org/) simulator.
|
||||
|
||||
This package provides a Gazebo plugin which instantiates a ros_control
|
||||
controller manager and connects it to a Gazebo model.
|
||||
@@ -0,0 +1,53 @@
|
||||
#ifndef MY_GAZEBO_ROS2_CONTROL_AUTO_ADJUST_GROUP_HPP
|
||||
#define MY_GAZEBO_ROS2_CONTROL_AUTO_ADJUST_GROUP_HPP
|
||||
|
||||
#include "gazebo/physics/Joint.hh"
|
||||
#include "gazebo/physics/Model.hh"
|
||||
#include "gazebo/physics/Link.hh"
|
||||
#include "gazebo/physics/PhysicsTypes.hh"
|
||||
#include <Eigen/Dense>
|
||||
|
||||
namespace my_gazebo_ros2_control
|
||||
{
|
||||
|
||||
struct JointDynamicsParam
|
||||
{
|
||||
double mass; // 子连接杆质量 (kg)
|
||||
Eigen::Vector3d com_local; // 质心在连接杆坐标系的位置 (m)
|
||||
double viscous_damping; // 粘滞阻尼系数
|
||||
double coulomb_friction; // 库仑摩擦系数
|
||||
};
|
||||
|
||||
struct AutoAdjustGroupConfig
|
||||
{
|
||||
std::vector<gazebo::physics::JointPtr> sim_joints;
|
||||
gazebo::physics::LinkPtr parent_link;
|
||||
std::vector<gazebo::physics::LinkPtr> child_links;
|
||||
std::vector<JointDynamicsParam> joint_params;
|
||||
|
||||
// PD 控制增益
|
||||
std::vector<double> Kp;
|
||||
std::vector<double> Kd;
|
||||
};
|
||||
|
||||
class AutoAdjustGroup
|
||||
{
|
||||
public:
|
||||
AutoAdjustGroup(std::shared_ptr<AutoAdjustGroupConfig> config);
|
||||
~AutoAdjustGroup();
|
||||
|
||||
void get_adjust_effort(double *aimAngle, double *effort);
|
||||
void update_param();
|
||||
|
||||
private:
|
||||
std::shared_ptr<AutoAdjustGroupConfig> config_;
|
||||
std::vector<double> velocity_;
|
||||
std::vector<double> position_;
|
||||
|
||||
std::vector<double> aim_angle_history_[3];
|
||||
std::vector<double> aim_velocity_;
|
||||
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,68 @@
|
||||
// Copyright (c) 2013, Open Source Robotics Foundation. All rights reserved.
|
||||
// Copyright (c) 2013, The Johns Hopkins University. All rights reserved.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
//
|
||||
// * Redistributions of source code must retain the above copyright
|
||||
// notice, this list of conditions and the following disclaimer.
|
||||
//
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
//
|
||||
// * Neither the name of the Open Source Robotics Foundation nor the names of its
|
||||
// contributors may be used to endorse or promote products derived from
|
||||
// this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
// POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
/* Author: Dave Coleman, Jonathan Bohren
|
||||
Desc: Gazebo plugin for ros_control that allows 'hardware_interfaces' to be plugged in
|
||||
using pluginlib
|
||||
*/
|
||||
|
||||
#ifndef MY_GAZEBO_ROS2_CONTROL__GAZEBO_ROS2_CONTROL_PLUGIN_HPP_
|
||||
#define MY_GAZEBO_ROS2_CONTROL__GAZEBO_ROS2_CONTROL_PLUGIN_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "controller_manager/controller_manager.hpp"
|
||||
|
||||
#include "gazebo/common/common.hh"
|
||||
#include "gazebo/physics/Model.hh"
|
||||
|
||||
namespace my_gazebo_ros2_control
|
||||
{
|
||||
class GazeboRosControlPrivate;
|
||||
|
||||
class GazeboRosControlPlugin : public gazebo::ModelPlugin
|
||||
{
|
||||
public:
|
||||
GazeboRosControlPlugin();
|
||||
|
||||
~GazeboRosControlPlugin();
|
||||
|
||||
// Overloaded Gazebo entry point
|
||||
void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) override;
|
||||
void Reset() override;
|
||||
|
||||
private:
|
||||
/// Private data pointer
|
||||
std::unique_ptr<GazeboRosControlPrivate> impl_;
|
||||
};
|
||||
} // namespace my_gazebo_ros2_control
|
||||
|
||||
#endif // MY_GAZEBO_ROS2_CONTROL__GAZEBO_ROS2_CONTROL_PLUGIN_HPP_
|
||||
@@ -0,0 +1,106 @@
|
||||
// Copyright 2021 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// 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.
|
||||
|
||||
|
||||
#ifndef MY_GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_
|
||||
#define MY_GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_
|
||||
|
||||
#define VELOCITY_PID_PARAMS_PREFIX "vel_"
|
||||
#define POSITION_PID_PARAMS_PREFIX "pos_"
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "angles/angles.h"
|
||||
|
||||
#include "control_toolbox/pid.hpp"
|
||||
#include "my_gazebo_ros2_control/gazebo_system_interface.hpp"
|
||||
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
|
||||
namespace my_gazebo_ros2_control
|
||||
{
|
||||
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
|
||||
|
||||
// Forward declaration
|
||||
class GazeboSystemPrivate;
|
||||
|
||||
// These class must inherit `my_gazebo_ros2_control::GazeboSystemInterface` which implements a
|
||||
// simulated `ros2_control` `hardware_interface::SystemInterface`.
|
||||
|
||||
class GazeboSystem : public GazeboSystemInterface
|
||||
{
|
||||
public:
|
||||
// Documentation Inherited
|
||||
CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info)
|
||||
override;
|
||||
|
||||
// Documentation Inherited
|
||||
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
|
||||
|
||||
// Documentation Inherited
|
||||
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
|
||||
|
||||
// Documentation Inherited
|
||||
CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
|
||||
|
||||
// Documentation Inherited
|
||||
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
|
||||
|
||||
// Documentation Inherited
|
||||
hardware_interface::return_type perform_command_mode_switch(
|
||||
const std::vector<std::string> & start_interfaces,
|
||||
const std::vector<std::string> & stop_interfaces) override;
|
||||
|
||||
// Documentation Inherited
|
||||
hardware_interface::return_type read(
|
||||
const rclcpp::Time & time,
|
||||
const rclcpp::Duration & period) override;
|
||||
|
||||
// Documentation Inherited
|
||||
hardware_interface::return_type write(
|
||||
const rclcpp::Time & time,
|
||||
const rclcpp::Duration & period) override;
|
||||
|
||||
// Documentation Inherited
|
||||
bool initSim(
|
||||
rclcpp::Node::SharedPtr & model_nh,
|
||||
gazebo::physics::ModelPtr parent_model,
|
||||
const hardware_interface::HardwareInfo & hardware_info,
|
||||
sdf::ElementPtr sdf) override;
|
||||
|
||||
private:
|
||||
void registerJoints(
|
||||
const hardware_interface::HardwareInfo & hardware_info,
|
||||
gazebo::physics::ModelPtr parent_model);
|
||||
|
||||
void registerSensors(
|
||||
const hardware_interface::HardwareInfo & hardware_info,
|
||||
gazebo::physics::ModelPtr parent_model);
|
||||
|
||||
bool extractPID(
|
||||
const std::string & prefix,
|
||||
const hardware_interface::ComponentInfo & joint_info, control_toolbox::Pid & pid);
|
||||
|
||||
bool extractPIDFromParameters(
|
||||
const std::string & control_mode, const std::string & joint_name, control_toolbox::Pid & pid);
|
||||
|
||||
/// \brief Private data class
|
||||
std::unique_ptr<GazeboSystemPrivate> dataPtr;
|
||||
};
|
||||
|
||||
} // namespace my_gazebo_ros2_control
|
||||
|
||||
#endif // MY_GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_
|
||||
@@ -0,0 +1,95 @@
|
||||
// Copyright 2021 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// 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.
|
||||
|
||||
|
||||
#ifndef MY_GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_INTERFACE_HPP_
|
||||
#define MY_GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_INTERFACE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "gazebo/physics/Joint.hh"
|
||||
#include "gazebo/physics/Model.hh"
|
||||
#include "gazebo/physics/physics.hh"
|
||||
|
||||
#include "hardware_interface/system_interface.hpp"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
|
||||
namespace my_gazebo_ros2_control
|
||||
{
|
||||
|
||||
template<class ENUM, class UNDERLYING = typename std::underlying_type<ENUM>::type>
|
||||
class SafeEnum
|
||||
{
|
||||
public:
|
||||
SafeEnum()
|
||||
: mFlags(0) {}
|
||||
explicit SafeEnum(ENUM singleFlag)
|
||||
: mFlags(singleFlag) {}
|
||||
SafeEnum(const SafeEnum & original)
|
||||
: mFlags(original.mFlags) {}
|
||||
~SafeEnum() = default;
|
||||
|
||||
SafeEnum & operator=(const SafeEnum & original) = default;
|
||||
SafeEnum & operator|=(ENUM addValue) {mFlags |= addValue; return *this;}
|
||||
SafeEnum operator|(ENUM addValue) {SafeEnum result(*this); result |= addValue; return result;}
|
||||
SafeEnum & operator&=(ENUM maskValue) {mFlags &= maskValue; return *this;}
|
||||
SafeEnum operator&(ENUM maskValue) {SafeEnum result(*this); result &= maskValue; return result;}
|
||||
SafeEnum operator~() {SafeEnum result(*this); result.mFlags = ~result.mFlags; return result;}
|
||||
explicit operator bool() {return mFlags != 0;}
|
||||
// UNDERLYING getFlags() {return mFlags;}
|
||||
|
||||
protected:
|
||||
UNDERLYING mFlags;
|
||||
};
|
||||
|
||||
// SystemInterface provides API-level access to read and command joint properties.
|
||||
class GazeboSystemInterface
|
||||
: public hardware_interface::SystemInterface
|
||||
{
|
||||
public:
|
||||
/// \brief Initialize the system interface
|
||||
/// param[in] model_nh pointer to the ros2 node
|
||||
/// param[in] parent_model pointer to the model
|
||||
/// param[in] control_hardware vector filled with information about robot's control resources
|
||||
/// param[in] sdf pointer to the SDF
|
||||
virtual bool initSim(
|
||||
rclcpp::Node::SharedPtr & model_nh,
|
||||
gazebo::physics::ModelPtr parent_model,
|
||||
const hardware_interface::HardwareInfo & hardware_info,
|
||||
sdf::ElementPtr sdf) = 0;
|
||||
|
||||
// Methods used to control a joint.
|
||||
enum ControlMethod_
|
||||
{
|
||||
NONE = 0,
|
||||
POSITION = (1 << 0),
|
||||
VELOCITY = (1 << 1),
|
||||
EFFORT = (1 << 2),
|
||||
VELOCITY_PID = (1 << 3),
|
||||
POSITION_PID = (1 << 4),
|
||||
};
|
||||
|
||||
typedef SafeEnum<enum ControlMethod_> ControlMethod;
|
||||
|
||||
protected:
|
||||
rclcpp::Node::SharedPtr nh_;
|
||||
};
|
||||
|
||||
} // namespace my_gazebo_ros2_control
|
||||
|
||||
#endif // MY_GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_INTERFACE_HPP_
|
||||
@@ -0,0 +1,10 @@
|
||||
<library path="my_gazebo_hardware_plugins">
|
||||
<class
|
||||
name="my_gazebo_ros2_control/GazeboSystem"
|
||||
type="my_gazebo_ros2_control::GazeboSystem"
|
||||
base_class_type="my_gazebo_ros2_control::GazeboSystemInterface">
|
||||
<description>
|
||||
GazeboPositionJoint
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
40
zhengjie_simulator/my_gazebo_ros2_control/package.xml
Normal file
40
zhengjie_simulator/my_gazebo_ros2_control/package.xml
Normal file
@@ -0,0 +1,40 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>my_gazebo_ros2_control</name>
|
||||
<version>0.4.12</version>
|
||||
<description>my_gazebo_ros2_control</description>
|
||||
|
||||
<maintainer email="alejandro@osrfoundation.org">Alejandro Hernandez</maintainer>
|
||||
|
||||
<license>BSD</license>
|
||||
<license>Apache License 2.0</license>
|
||||
|
||||
<url type="website">http://ros.org/wiki/gazebo_ros_control</url>
|
||||
<url type="bugtracker">https://github.com/ros-controls/gazebo_ros2_control/issues</url>
|
||||
<url type="repository">https://github.com/ros-controls/gazebo_ros2_control/</url>
|
||||
|
||||
<author>Jonathan Bohren</author>
|
||||
<author>Dave Coleman</author>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>angles</depend>
|
||||
<depend>controller_manager</depend>
|
||||
<depend>control_toolbox</depend>
|
||||
<depend>gazebo_dev</depend>
|
||||
<depend>gazebo_ros</depend>
|
||||
<depend>hardware_interface</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>yaml_cpp_vendor</depend>
|
||||
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
<my_gazebo_ros2_control plugin="${prefix}/my_gazebo_hardware_plugins.xml"/>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,31 @@
|
||||
#include "my_gazebo_ros2_control/auto_adjust_group.hpp"
|
||||
|
||||
namespace my_gazebo_ros2_control
|
||||
{
|
||||
|
||||
AutoAdjustGroup::AutoAdjustGroup(std::shared_ptr<AutoAdjustGroupConfig> config)
|
||||
: config_(config)
|
||||
{
|
||||
}
|
||||
|
||||
AutoAdjustGroup::~AutoAdjustGroup()
|
||||
{
|
||||
}
|
||||
|
||||
void AutoAdjustGroup::get_adjust_effort(double *aimAngle, double *effort)
|
||||
{
|
||||
ignition::math::Pose3d pose = config_->parent_link->WorldCoGPose();
|
||||
ignition::math::Vector3d parent_effort_all = config_->parent_link->WorldForce();
|
||||
for (size_t i = 0; i < config_->sim_joints.size(); i++) {
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void AutoAdjustGroup::update_param()
|
||||
{
|
||||
for (size_t i = 0; i < config_->sim_joints.size(); i++) {
|
||||
velocity_[i] = config_->sim_joints[i]->GetVelocity(0);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,519 @@
|
||||
// Copyright (c) 2013, Open Source Robotics Foundation. All rights reserved.
|
||||
// Copyright (c) 2013, The Johns Hopkins University. All rights reserved.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
//
|
||||
// * Redistributions of source code must retain the above copyright
|
||||
// notice, this list of conditions and the following disclaimer.
|
||||
//
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
//
|
||||
// * Neither the name of the Open Source Robotics Foundation nor the names of its
|
||||
// contributors may be used to endorse or promote products derived from
|
||||
// this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
// POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
/* Author: Dave Coleman, Jonathan Bohren
|
||||
Desc: Gazebo plugin for ros_control that allows 'hardware_interfaces' to be plugged in
|
||||
using pluginlib
|
||||
*/
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "gazebo_ros/node.hpp"
|
||||
|
||||
#include "my_gazebo_ros2_control/gazebo_ros2_control_plugin.hpp"
|
||||
#include "my_gazebo_ros2_control/gazebo_system.hpp"
|
||||
|
||||
#include "pluginlib/class_loader.hpp"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "hardware_interface/resource_manager.hpp"
|
||||
#include "hardware_interface/component_parser.hpp"
|
||||
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
||||
|
||||
#include "yaml-cpp/yaml.h"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
namespace my_gazebo_ros2_control
|
||||
{
|
||||
|
||||
class GazeboRosControlPrivate
|
||||
{
|
||||
public:
|
||||
GazeboRosControlPrivate() = default;
|
||||
|
||||
virtual ~GazeboRosControlPrivate() = default;
|
||||
|
||||
// Called by the world update start event
|
||||
void Update();
|
||||
|
||||
// Called on world reset
|
||||
virtual void Reset();
|
||||
|
||||
// Get the URDF XML from the parameter server
|
||||
std::string getURDF(std::string param_name) const;
|
||||
|
||||
// Node Handles
|
||||
gazebo_ros::Node::SharedPtr model_nh_;
|
||||
|
||||
// Pointer to the model
|
||||
gazebo::physics::ModelPtr parent_model_;
|
||||
|
||||
// Pointer to the update event connection
|
||||
gazebo::event::ConnectionPtr update_connection_;
|
||||
|
||||
// Interface loader
|
||||
boost::shared_ptr<pluginlib::ClassLoader<
|
||||
my_gazebo_ros2_control::GazeboSystemInterface>> robot_hw_sim_loader_;
|
||||
|
||||
// String with the robot description
|
||||
std::string robot_description_;
|
||||
|
||||
// String with the name of the node that contains the robot_description
|
||||
std::string robot_description_node_;
|
||||
|
||||
// Executor to spin the controller
|
||||
rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_;
|
||||
|
||||
// Thread where the executor will sping
|
||||
std::thread thread_executor_spin_;
|
||||
|
||||
// Flag to stop the executor thread when this plugin is exiting
|
||||
bool stop_;
|
||||
|
||||
// Controller manager
|
||||
std::shared_ptr<controller_manager::ControllerManager> controller_manager_;
|
||||
|
||||
// Available controllers
|
||||
std::vector<std::shared_ptr<controller_interface::ControllerInterface>> controllers_;
|
||||
|
||||
// Timing
|
||||
rclcpp::Duration control_period_ = rclcpp::Duration(1, 0);
|
||||
|
||||
// Last time the update method was called
|
||||
rclcpp::Time last_update_sim_time_ros_ = rclcpp::Time((int64_t)0, RCL_ROS_TIME);
|
||||
};
|
||||
|
||||
GazeboRosControlPlugin::GazeboRosControlPlugin()
|
||||
: impl_(std::make_unique<GazeboRosControlPrivate>())
|
||||
{
|
||||
}
|
||||
|
||||
GazeboRosControlPlugin::~GazeboRosControlPlugin()
|
||||
{
|
||||
// Stop controller manager thread (only if properly initialized)
|
||||
impl_->stop_ = true;
|
||||
|
||||
if (impl_->executor_) {
|
||||
if (impl_->controller_manager_) {
|
||||
impl_->executor_->remove_node(impl_->controller_manager_);
|
||||
}
|
||||
impl_->executor_->cancel();
|
||||
}
|
||||
|
||||
if (impl_->thread_executor_spin_.joinable()) {
|
||||
impl_->thread_executor_spin_.join();
|
||||
}
|
||||
|
||||
// Disconnect from gazebo events
|
||||
impl_->update_connection_.reset();
|
||||
}
|
||||
|
||||
// Overloaded Gazebo entry point
|
||||
void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
rclcpp::get_logger("my_gazebo_ros2_control"),
|
||||
"Loading my_gazebo_ros2_control plugin");
|
||||
|
||||
// Save pointers to the model
|
||||
impl_->parent_model_ = parent;
|
||||
|
||||
// Get parameters/settings for controllers from ROS param server
|
||||
// Initialize ROS node
|
||||
impl_->model_nh_ = gazebo_ros::Node::Get(sdf);
|
||||
|
||||
RCLCPP_INFO(
|
||||
impl_->model_nh_->get_logger(), "Starting my_gazebo_ros2_control plugin in namespace: %s",
|
||||
impl_->model_nh_->get_namespace());
|
||||
|
||||
RCLCPP_INFO(
|
||||
impl_->model_nh_->get_logger(), "Starting my_gazebo_ros2_control plugin in ros 2 node: %s",
|
||||
impl_->model_nh_->get_name());
|
||||
|
||||
// Error message if the model couldn't be found
|
||||
if (!impl_->parent_model_) {
|
||||
RCLCPP_ERROR_STREAM(impl_->model_nh_->get_logger(), "parent model is NULL");
|
||||
return;
|
||||
}
|
||||
|
||||
// Check that ROS has been initialized
|
||||
if (!rclcpp::ok()) {
|
||||
RCLCPP_FATAL_STREAM(
|
||||
impl_->model_nh_->get_logger(),
|
||||
"A ROS node for Gazebo has not been initialized, unable to load plugin. " <<
|
||||
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
|
||||
return;
|
||||
}
|
||||
|
||||
// Get robot_description ROS param name
|
||||
if (sdf->HasElement("robot_param")) {
|
||||
impl_->robot_description_ = sdf->GetElement("robot_param")->Get<std::string>();
|
||||
} else {
|
||||
impl_->robot_description_ = "robot_description"; // default
|
||||
}
|
||||
|
||||
// Get robot_description ROS param name
|
||||
if (sdf->HasElement("robot_param_node")) {
|
||||
impl_->robot_description_node_ =
|
||||
sdf->GetElement("robot_param_node")->Get<std::string>();
|
||||
} else {
|
||||
impl_->robot_description_node_ = "robot_state_publisher"; // default
|
||||
}
|
||||
// Hold joints if no control mode is active?
|
||||
bool hold_joints = true; // default
|
||||
if (sdf->HasElement("hold_joints")) {
|
||||
hold_joints =
|
||||
sdf->GetElement("hold_joints")->Get<bool>();
|
||||
}
|
||||
|
||||
// Read urdf from ros parameter server
|
||||
std::string urdf_string;
|
||||
urdf_string = impl_->getURDF(impl_->robot_description_);
|
||||
if (urdf_string.empty()) {
|
||||
RCLCPP_ERROR_STREAM(impl_->model_nh_->get_logger(), "An empty URDF was passed. Exiting.");
|
||||
return;
|
||||
}
|
||||
|
||||
// There's currently no direct way to set parameters to the plugin's node
|
||||
// So we have to parse the plugin file manually and set it to the node's context.
|
||||
auto rcl_context = impl_->model_nh_->get_node_base_interface()->get_context()->get_rcl_context();
|
||||
std::vector<std::string> arguments = {"--ros-args"};
|
||||
|
||||
if (sdf->HasElement("parameters")) {
|
||||
sdf::ElementPtr argument_sdf = sdf->GetElement("parameters");
|
||||
while (argument_sdf) {
|
||||
std::string argument = argument_sdf->Get<std::string>();
|
||||
RCLCPP_INFO(impl_->model_nh_->get_logger(), "Loading parameter files %s", argument.c_str());
|
||||
arguments.push_back(RCL_PARAM_FILE_FLAG);
|
||||
arguments.push_back(argument);
|
||||
argument_sdf = argument_sdf->GetNextElement("parameters");
|
||||
}
|
||||
} else {
|
||||
RCLCPP_ERROR(
|
||||
impl_->model_nh_->get_logger(), "No parameter file provided. Configuration might be wrong");
|
||||
}
|
||||
|
||||
// set the robot description parameter
|
||||
// to propagate it among controller manager and controllers
|
||||
std::string rb_arg = std::string("robot_description:=") + urdf_string;
|
||||
arguments.push_back(RCL_PARAM_FLAG);
|
||||
arguments.push_back(rb_arg);
|
||||
|
||||
if (sdf->HasElement("ros")) {
|
||||
sdf = sdf->GetElement("ros");
|
||||
// Get list of remapping rules from SDF
|
||||
if (sdf->HasElement("remapping")) {
|
||||
sdf::ElementPtr argument_sdf = sdf->GetElement("remapping");
|
||||
|
||||
while (argument_sdf) {
|
||||
std::string argument = argument_sdf->Get<std::string>();
|
||||
arguments.push_back(RCL_REMAP_FLAG);
|
||||
arguments.push_back(argument);
|
||||
argument_sdf = argument_sdf->GetNextElement("remapping");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<const char *> argv;
|
||||
for (const auto & arg : arguments) {
|
||||
argv.push_back(reinterpret_cast<const char *>(arg.data()));
|
||||
}
|
||||
rcl_arguments_t rcl_args = rcl_get_zero_initialized_arguments();
|
||||
rcl_ret_t rcl_ret = rcl_parse_arguments(
|
||||
static_cast<int>(argv.size()),
|
||||
argv.data(), rcl_get_default_allocator(), &rcl_args);
|
||||
rcl_context->global_arguments = rcl_args;
|
||||
if (rcl_ret != RCL_RET_OK) {
|
||||
RCLCPP_ERROR(impl_->model_nh_->get_logger(), "parser error %s\n", rcl_get_error_string().str);
|
||||
rcl_reset_error();
|
||||
return;
|
||||
}
|
||||
if (rcl_arguments_get_param_files_count(&rcl_args) < 1) {
|
||||
RCLCPP_ERROR(
|
||||
impl_->model_nh_->get_logger(), "failed to parse input yaml file(s)");
|
||||
return;
|
||||
}
|
||||
|
||||
// Get the Gazebo simulation period
|
||||
rclcpp::Duration gazebo_period(
|
||||
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::duration<double>(
|
||||
impl_->parent_model_->GetWorld()->Physics()->GetMaxStepSize())));
|
||||
RCLCPP_INFO(impl_->model_nh_->get_logger(), "gazebo_period: %ld", gazebo_period.nanoseconds());
|
||||
// setup actuators and mechanism control node.
|
||||
// This call will block if ROS is not properly initialized.
|
||||
std::vector<hardware_interface::HardwareInfo> control_hardware_info;
|
||||
try {
|
||||
control_hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf_string);
|
||||
} catch (const std::runtime_error & ex) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
impl_->model_nh_->get_logger(),
|
||||
"Error parsing URDF in my_gazebo_ros2_control plugin, plugin not active : " << ex.what());
|
||||
return;
|
||||
}
|
||||
|
||||
std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ =
|
||||
std::make_unique<hardware_interface::ResourceManager>();
|
||||
|
||||
try {
|
||||
resource_manager_->load_urdf(urdf_string, false, false);
|
||||
} catch (...) {
|
||||
// This error should be normal as the resource manager is not supposed to load and initialize
|
||||
// them
|
||||
RCLCPP_ERROR(impl_->model_nh_->get_logger(), "Error initializing URDF to resource manager!");
|
||||
}
|
||||
|
||||
try {
|
||||
RCLCPP_INFO(impl_->model_nh_->get_logger(), "loading robot simulation interface loader");
|
||||
auto p = new pluginlib::ClassLoader<my_gazebo_ros2_control::GazeboSystemInterface>(
|
||||
"my_gazebo_ros2_control",
|
||||
"my_gazebo_ros2_control::GazeboSystemInterface");
|
||||
RCLCPP_INFO(impl_->model_nh_->get_logger(), "pluginlib::ClassLoader<my_gazebo_ros2_control::GazeboSystemInterface> created");
|
||||
impl_->robot_hw_sim_loader_.reset(p);
|
||||
RCLCPP_INFO(impl_->model_nh_->get_logger(), "robot_hw_sim_loader_ reset");
|
||||
} catch (pluginlib::LibraryLoadException & ex) {
|
||||
RCLCPP_ERROR(
|
||||
impl_->model_nh_->get_logger(), "Failed to create robot simulation interface loader: %s ",
|
||||
ex.what());
|
||||
}
|
||||
|
||||
RCLCPP_INFO(impl_->model_nh_->get_logger(), "control_hardware_info.size(): %ld", control_hardware_info.size());
|
||||
|
||||
for (unsigned int i = 0; i < control_hardware_info.size(); i++) {
|
||||
std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_class_type;
|
||||
RCLCPP_INFO(
|
||||
impl_->model_nh_->get_logger(), "Trying to load hardware interface: [%s]",
|
||||
robot_hw_sim_type_str_.c_str());
|
||||
std::unique_ptr<my_gazebo_ros2_control::GazeboSystemInterface> gazeboSystem;
|
||||
try {
|
||||
gazeboSystem = std::unique_ptr<my_gazebo_ros2_control::GazeboSystemInterface>(
|
||||
impl_->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_));
|
||||
} catch (pluginlib::PluginlibException & ex) {
|
||||
RCLCPP_ERROR(
|
||||
impl_->model_nh_->get_logger(), "The plugin failed to load for some reason. Error: %s\n",
|
||||
ex.what());
|
||||
continue;
|
||||
}
|
||||
rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast<rclcpp::Node>(impl_->model_nh_);
|
||||
RCLCPP_INFO(
|
||||
impl_->model_nh_->get_logger(), "Loaded hardware interface %s!",
|
||||
robot_hw_sim_type_str_.c_str());
|
||||
try {
|
||||
node_ros2->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints));
|
||||
} catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
|
||||
RCLCPP_ERROR(
|
||||
impl_->model_nh_->get_logger(), "Parameter 'hold_joints' has already been declared, %s",
|
||||
e.what());
|
||||
} catch (const rclcpp::exceptions::InvalidParametersException & e) {
|
||||
RCLCPP_ERROR(
|
||||
impl_->model_nh_->get_logger(), "Parameter 'hold_joints' has invalid name, %s", e.what());
|
||||
} catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
|
||||
RCLCPP_ERROR(
|
||||
impl_->model_nh_->get_logger(), "Parameter 'hold_joints' value is invalid, %s", e.what());
|
||||
} catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
|
||||
RCLCPP_ERROR(
|
||||
impl_->model_nh_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s",
|
||||
e.what());
|
||||
}
|
||||
if (!gazeboSystem->initSim(
|
||||
node_ros2,
|
||||
impl_->parent_model_,
|
||||
control_hardware_info[i],
|
||||
sdf))
|
||||
{
|
||||
RCLCPP_FATAL(
|
||||
impl_->model_nh_->get_logger(), "Could not initialize robot simulation interface");
|
||||
return;
|
||||
}
|
||||
RCLCPP_DEBUG(
|
||||
impl_->model_nh_->get_logger(), "Initialized robot simulation interface %s!",
|
||||
robot_hw_sim_type_str_.c_str());
|
||||
|
||||
resource_manager_->import_component(std::move(gazeboSystem), control_hardware_info[i]);
|
||||
|
||||
// activate all components
|
||||
rclcpp_lifecycle::State state(
|
||||
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
|
||||
hardware_interface::lifecycle_state_names::ACTIVE);
|
||||
resource_manager_->set_component_state(control_hardware_info[i].name, state);
|
||||
}
|
||||
|
||||
impl_->executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
|
||||
|
||||
// Create the controller manager
|
||||
RCLCPP_INFO(impl_->model_nh_->get_logger(), "Loading controller_manager");
|
||||
impl_->controller_manager_.reset(
|
||||
new controller_manager::ControllerManager(
|
||||
std::move(resource_manager_),
|
||||
impl_->executor_,
|
||||
"controller_manager",
|
||||
impl_->model_nh_->get_namespace()));
|
||||
impl_->executor_->add_node(impl_->controller_manager_);
|
||||
|
||||
if (!impl_->controller_manager_->has_parameter("update_rate")) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
impl_->model_nh_->get_logger(), "controller manager doesn't have an update_rate parameter");
|
||||
return;
|
||||
}
|
||||
|
||||
auto cm_update_rate = impl_->controller_manager_->get_parameter("update_rate").as_int();
|
||||
impl_->control_period_ = rclcpp::Duration(
|
||||
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::duration<double>(1.0 / static_cast<double>(cm_update_rate))));
|
||||
// Check the period against the simulation period
|
||||
if (impl_->control_period_ < gazebo_period) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
impl_->model_nh_->get_logger(),
|
||||
"Desired controller update period (" << impl_->control_period_.seconds() <<
|
||||
" s) is faster than the gazebo simulation period (" <<
|
||||
gazebo_period.seconds() << " s).");
|
||||
} else if (impl_->control_period_ > gazebo_period) {
|
||||
RCLCPP_WARN_STREAM(
|
||||
impl_->model_nh_->get_logger(),
|
||||
" Desired controller update period (" << impl_->control_period_.seconds() <<
|
||||
" s) is slower than the gazebo simulation period (" <<
|
||||
gazebo_period.seconds() << " s).");
|
||||
}
|
||||
// Force setting of use_sime_time parameter
|
||||
impl_->controller_manager_->set_parameter(
|
||||
rclcpp::Parameter("use_sim_time", rclcpp::ParameterValue(true)));
|
||||
|
||||
impl_->stop_ = false;
|
||||
auto spin = [this]()
|
||||
{
|
||||
while (rclcpp::ok() && !impl_->stop_) {
|
||||
impl_->executor_->spin_once();
|
||||
}
|
||||
};
|
||||
impl_->thread_executor_spin_ = std::thread(spin);
|
||||
|
||||
// Listen to the update event. This event is broadcast every simulation iteration.
|
||||
impl_->update_connection_ =
|
||||
gazebo::event::Events::ConnectWorldUpdateBegin(
|
||||
boost::bind(
|
||||
&GazeboRosControlPrivate::Update,
|
||||
impl_.get()));
|
||||
|
||||
RCLCPP_INFO(impl_->model_nh_->get_logger(), "Loaded my_gazebo_ros2_control.");
|
||||
}
|
||||
|
||||
// Called by the world update start event
|
||||
void GazeboRosControlPrivate::Update()
|
||||
{
|
||||
// Get the simulation time and period
|
||||
gazebo::common::Time gz_time_now = parent_model_->GetWorld()->SimTime();
|
||||
rclcpp::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec, RCL_ROS_TIME);
|
||||
rclcpp::Duration sim_period = sim_time_ros - last_update_sim_time_ros_;
|
||||
|
||||
if (sim_period >= control_period_) {
|
||||
controller_manager_->read(sim_time_ros, sim_period);
|
||||
controller_manager_->update(sim_time_ros, sim_period);
|
||||
last_update_sim_time_ros_ = sim_time_ros;
|
||||
}
|
||||
|
||||
// Always set commands on joints, otherwise at low control frequencies the joints tremble
|
||||
// as they are updated at a fraction of gazebo sim time
|
||||
// use same time as for read and update call - this is how it is done is ros2_control_node
|
||||
controller_manager_->write(sim_time_ros, sim_period);
|
||||
}
|
||||
|
||||
// Called on world reset
|
||||
void GazeboRosControlPlugin::Reset()
|
||||
{
|
||||
impl_->Reset();
|
||||
}
|
||||
|
||||
void GazeboRosControlPrivate::Reset()
|
||||
{
|
||||
// Reset timing variables to not pass negative update periods to controllers on world reset
|
||||
last_update_sim_time_ros_ = rclcpp::Time((int64_t)0, RCL_ROS_TIME);
|
||||
}
|
||||
|
||||
// Get the URDF XML from the parameter server
|
||||
std::string GazeboRosControlPrivate::getURDF(std::string param_name) const
|
||||
{
|
||||
std::string urdf_string;
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
auto parameters_client = std::make_shared<rclcpp::AsyncParametersClient>(
|
||||
model_nh_, robot_description_node_);
|
||||
while (!parameters_client->wait_for_service(0.5s)) {
|
||||
if (!rclcpp::ok()) {
|
||||
RCLCPP_ERROR(
|
||||
model_nh_->get_logger(), "Interrupted while waiting for %s service. Exiting.",
|
||||
robot_description_node_.c_str());
|
||||
return "";
|
||||
}
|
||||
RCLCPP_ERROR(
|
||||
model_nh_->get_logger(), "%s service not available, waiting again...",
|
||||
robot_description_node_.c_str());
|
||||
}
|
||||
|
||||
RCLCPP_INFO(
|
||||
model_nh_->get_logger(), "connected to service!! %s", robot_description_node_.c_str());
|
||||
|
||||
// search and wait for robot_description on param server
|
||||
while (urdf_string.empty()) {
|
||||
std::string search_param_name;
|
||||
RCLCPP_DEBUG(model_nh_->get_logger(), "param_name %s", param_name.c_str());
|
||||
|
||||
try {
|
||||
auto f = parameters_client->get_parameters({param_name});
|
||||
f.wait();
|
||||
std::vector<rclcpp::Parameter> values = f.get();
|
||||
urdf_string = values[0].as_string();
|
||||
} catch (const std::exception & e) {
|
||||
RCLCPP_ERROR(model_nh_->get_logger(), "%s", e.what());
|
||||
}
|
||||
|
||||
if (!urdf_string.empty()) {
|
||||
break;
|
||||
} else {
|
||||
RCLCPP_ERROR(
|
||||
model_nh_->get_logger(), "my_gazebo_ros2_control plugin is waiting for model"
|
||||
" URDF in parameter [%s] on the ROS param server.", search_param_name.c_str());
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::microseconds(100000));
|
||||
}
|
||||
RCLCPP_INFO(
|
||||
model_nh_->get_logger(), "Received urdf from param server, parsing...");
|
||||
|
||||
return urdf_string;
|
||||
}
|
||||
|
||||
// Register this plugin with the simulator
|
||||
GZ_REGISTER_MODEL_PLUGIN(GazeboRosControlPlugin)
|
||||
} // namespace my_gazebo_ros2_control
|
||||
958
zhengjie_simulator/my_gazebo_ros2_control/src/gazebo_system.cpp
Normal file
958
zhengjie_simulator/my_gazebo_ros2_control/src/gazebo_system.cpp
Normal file
@@ -0,0 +1,958 @@
|
||||
// Copyright 2021 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
|
||||
#include "control_toolbox/pid.hpp"
|
||||
#include "my_gazebo_ros2_control/gazebo_system.hpp"
|
||||
#include "gazebo/sensors/ImuSensor.hh"
|
||||
#include "gazebo/sensors/ForceTorqueSensor.hh"
|
||||
#include "gazebo/sensors/SensorManager.hh"
|
||||
|
||||
#include "hardware_interface/hardware_info.hpp"
|
||||
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
||||
|
||||
struct MimicJoint
|
||||
{
|
||||
std::size_t joint_index;
|
||||
std::size_t mimicked_joint_index;
|
||||
double multiplier = 1.0;
|
||||
};
|
||||
|
||||
class my_gazebo_ros2_control::GazeboSystemPrivate
|
||||
{
|
||||
public:
|
||||
GazeboSystemPrivate() = default;
|
||||
|
||||
~GazeboSystemPrivate() = default;
|
||||
|
||||
/// \brief Degrees od freedom.
|
||||
size_t n_dof_;
|
||||
|
||||
/// \brief Number of sensors.
|
||||
size_t n_sensors_;
|
||||
|
||||
/// \brief Gazebo Model Ptr.
|
||||
gazebo::physics::ModelPtr parent_model_;
|
||||
|
||||
/// \brief last time the write method was called.
|
||||
rclcpp::Time last_update_sim_time_ros_;
|
||||
|
||||
/// \brief vector with the joint's names.
|
||||
std::vector<std::string> joint_names_;
|
||||
|
||||
/// \brief vector with the control method defined in the URDF for each joint.
|
||||
std::vector<GazeboSystemInterface::ControlMethod> joint_control_methods_;
|
||||
|
||||
/// \brief vector with indication for actuated joints (vs. passive joints)
|
||||
std::vector<bool> is_joint_actuated_;
|
||||
|
||||
/// \brief handles to the joints from within Gazebo
|
||||
std::vector<gazebo::physics::JointPtr> sim_joints_;
|
||||
|
||||
/// \brief vector with the current joint position
|
||||
std::vector<double> joint_position_;
|
||||
|
||||
/// \brief vector with the current joint velocity
|
||||
std::vector<double> joint_velocity_;
|
||||
|
||||
/// \brief vector with the current joint effort
|
||||
std::vector<double> joint_effort_;
|
||||
|
||||
/// \brief vector with the current cmd joint position
|
||||
std::vector<double> joint_position_cmd_;
|
||||
|
||||
/// \brief vector with the current cmd joint velocity
|
||||
std::vector<double> joint_velocity_cmd_;
|
||||
|
||||
/// \brief vector with the current cmd joint effort
|
||||
std::vector<double> joint_effort_cmd_;
|
||||
|
||||
/// \brief Joint velocity PID utils
|
||||
std::vector<control_toolbox::Pid> vel_pid;
|
||||
|
||||
/// \brief Joint position PID utils
|
||||
std::vector<control_toolbox::Pid> pos_pid;
|
||||
|
||||
// \brief control flag
|
||||
std::vector<bool> is_pos_pid;
|
||||
|
||||
// \brief control flag
|
||||
std::vector<bool> is_vel_pid;
|
||||
|
||||
/// \brief handles to the imus from within Gazebo
|
||||
std::vector<gazebo::sensors::ImuSensorPtr> sim_imu_sensors_;
|
||||
|
||||
/// \brief An array per IMU with 4 orientation, 3 angular velocity and 3 linear acceleration
|
||||
std::vector<std::array<double, 10>> imu_sensor_data_;
|
||||
|
||||
/// \brief handles to the FT sensors from within Gazebo
|
||||
std::vector<gazebo::sensors::ForceTorqueSensorPtr> sim_ft_sensors_;
|
||||
|
||||
/// \brief An array per FT sensor for 3D force and torquee
|
||||
std::vector<std::array<double, 6>> ft_sensor_data_;
|
||||
|
||||
/// \brief state interfaces that will be exported to the Resource Manager
|
||||
std::vector<hardware_interface::StateInterface> state_interfaces_;
|
||||
|
||||
/// \brief command interfaces that will be exported to the Resource Manager
|
||||
std::vector<hardware_interface::CommandInterface> command_interfaces_;
|
||||
|
||||
/// \brief mapping of mimicked joints to index of joint they mimic
|
||||
std::vector<MimicJoint> mimic_joints_;
|
||||
|
||||
// Should hold the joints if no control_mode is active
|
||||
bool hold_joints_ = true;
|
||||
|
||||
// Current position and velocity control method
|
||||
GazeboSystemInterface::ControlMethod_ position_control_method_ =
|
||||
GazeboSystemInterface::ControlMethod_::POSITION;
|
||||
GazeboSystemInterface::ControlMethod_ velocity_control_method_ =
|
||||
GazeboSystemInterface::ControlMethod_::VELOCITY;
|
||||
};
|
||||
|
||||
namespace my_gazebo_ros2_control
|
||||
{
|
||||
|
||||
bool GazeboSystem::initSim(
|
||||
rclcpp::Node::SharedPtr & model_nh,
|
||||
gazebo::physics::ModelPtr parent_model,
|
||||
const hardware_interface::HardwareInfo & hardware_info,
|
||||
sdf::ElementPtr sdf)
|
||||
{
|
||||
this->dataPtr = std::make_unique<GazeboSystemPrivate>();
|
||||
this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time();
|
||||
|
||||
this->nh_ = model_nh;
|
||||
this->dataPtr->parent_model_ = parent_model;
|
||||
|
||||
gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics();
|
||||
|
||||
std::string physics_type_ = physics->GetType();
|
||||
if (physics_type_.empty()) {
|
||||
RCLCPP_ERROR(this->nh_->get_logger(), "No physics engine configured in Gazebo.");
|
||||
return false;
|
||||
}
|
||||
|
||||
try {
|
||||
this->dataPtr->hold_joints_ = this->nh_->get_parameter("hold_joints").as_bool();
|
||||
} catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
|
||||
RCLCPP_ERROR(
|
||||
this->nh_->get_logger(),
|
||||
"Parameter 'hold_joints' not initialized, with error %s", ex.what());
|
||||
RCLCPP_WARN_STREAM(
|
||||
this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
|
||||
} catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
|
||||
RCLCPP_ERROR(
|
||||
this->nh_->get_logger(),
|
||||
"Parameter 'hold_joints' not declared, with error %s", ex.what());
|
||||
RCLCPP_WARN_STREAM(
|
||||
this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
|
||||
} catch (rclcpp::ParameterTypeException & ex) {
|
||||
RCLCPP_ERROR(
|
||||
this->nh_->get_logger(),
|
||||
"Parameter 'hold_joints' has wrong type: %s", ex.what());
|
||||
RCLCPP_WARN_STREAM(
|
||||
this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
|
||||
}
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"hold_joints (system): " << std::boolalpha << this->dataPtr->hold_joints_ << std::endl);
|
||||
|
||||
registerJoints(hardware_info, parent_model);
|
||||
registerSensors(hardware_info, parent_model);
|
||||
|
||||
if (this->dataPtr->n_dof_ == 0 && this->dataPtr->n_sensors_ == 0) {
|
||||
RCLCPP_WARN_STREAM(this->nh_->get_logger(), "There is no joint or sensor available");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GazeboSystem::extractPID(
|
||||
const std::string & prefix,
|
||||
const hardware_interface::ComponentInfo & joint_info, control_toolbox::Pid & pid)
|
||||
{
|
||||
double kp;
|
||||
double ki;
|
||||
double kd;
|
||||
double max_integral_error;
|
||||
double min_integral_error;
|
||||
bool antiwindup = false;
|
||||
|
||||
bool are_pids_set = false;
|
||||
if (joint_info.parameters.find(prefix + "kp") != joint_info.parameters.end()) {
|
||||
kp = std::stod(joint_info.parameters.at(prefix + "kp"));
|
||||
are_pids_set = true;
|
||||
} else {
|
||||
kp = 0.0;
|
||||
}
|
||||
|
||||
if (joint_info.parameters.find(prefix + "ki") != joint_info.parameters.end()) {
|
||||
ki = std::stod(joint_info.parameters.at(prefix + "ki"));
|
||||
are_pids_set = true;
|
||||
} else {
|
||||
ki = 0.0;
|
||||
}
|
||||
|
||||
if (joint_info.parameters.find(prefix + "kd") != joint_info.parameters.end()) {
|
||||
kd = std::stod(joint_info.parameters.at(prefix + "kd"));
|
||||
are_pids_set = true;
|
||||
} else {
|
||||
kd = 0.0;
|
||||
}
|
||||
|
||||
if (are_pids_set) {
|
||||
if (joint_info.parameters.find(prefix + "max_integral_error") != joint_info.parameters.end()) {
|
||||
max_integral_error = std::stod(joint_info.parameters.at(prefix + "max_integral_error"));
|
||||
} else {
|
||||
max_integral_error = std::numeric_limits<double>::max();
|
||||
}
|
||||
|
||||
if (joint_info.parameters.find(prefix + "min_integral_error") != joint_info.parameters.end()) {
|
||||
min_integral_error = std::stod(joint_info.parameters.at(prefix + "min_integral_error"));
|
||||
} else {
|
||||
min_integral_error = std::numeric_limits<double>::min();
|
||||
}
|
||||
|
||||
if (joint_info.parameters.find(prefix + "antiwindup") != joint_info.parameters.end()) {
|
||||
if (joint_info.parameters.at(prefix + "antiwindup") == "true" ||
|
||||
joint_info.parameters.at(prefix + "antiwindup") == "True")
|
||||
{
|
||||
antiwindup = true;
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"Setting kp = " << kp << "\t"
|
||||
<< " ki = " << ki << "\t"
|
||||
<< " kd = " << kd << "\t"
|
||||
<< " max_integral_error = " << max_integral_error << "\t"
|
||||
<< "antiwindup =" << std::boolalpha << antiwindup);
|
||||
|
||||
pid.initPid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup);
|
||||
}
|
||||
return are_pids_set;
|
||||
}
|
||||
|
||||
|
||||
bool GazeboSystem::extractPIDFromParameters(
|
||||
const std::string & control_mode, const std::string & joint_name,
|
||||
control_toolbox::Pid & pid)
|
||||
{
|
||||
const std::string parameter_prefix = "pid_gains." + control_mode + "." + joint_name;
|
||||
auto get_pid_entry = [this, parameter_prefix](const std::string & entry, double & value) -> bool {
|
||||
try {
|
||||
// Check if the parameter is declared, if not, declare the default value NaN
|
||||
if (!this->nh_->has_parameter(parameter_prefix + "." + entry)) {
|
||||
this->nh_->declare_parameter<double>(
|
||||
parameter_prefix + "." + entry,
|
||||
std::numeric_limits<double>::quiet_NaN());
|
||||
}
|
||||
value = this->nh_->get_parameter(parameter_prefix + "." + entry).as_double();
|
||||
} catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
|
||||
RCLCPP_ERROR(
|
||||
this->nh_->get_logger(),
|
||||
"Parameter '%s' not declared, with error %s", entry.c_str(), ex.what());
|
||||
return false;
|
||||
} catch (rclcpp::exceptions::InvalidParameterTypeException & ex) {
|
||||
RCLCPP_ERROR(
|
||||
this->nh_->get_logger(),
|
||||
"Parameter '%s' has wrong type: %s", entry.c_str(), ex.what());
|
||||
return false;
|
||||
}
|
||||
return std::isfinite(value);
|
||||
};
|
||||
bool are_pids_set = true;
|
||||
double kp, ki, kd, max_integral_error, min_integral_error;
|
||||
bool antiwindup;
|
||||
are_pids_set &= get_pid_entry("kp", kp);
|
||||
are_pids_set &= get_pid_entry("ki", ki);
|
||||
are_pids_set &= get_pid_entry("kd", kd);
|
||||
if (are_pids_set) {
|
||||
get_pid_entry("max_integral_error", max_integral_error);
|
||||
get_pid_entry("min_integral_error", min_integral_error);
|
||||
const std::string antiwindup_str = "antiwindup";
|
||||
if (!this->nh_->has_parameter(parameter_prefix + "." + antiwindup_str)) {
|
||||
this->nh_->declare_parameter(
|
||||
parameter_prefix + "." + antiwindup_str,
|
||||
false);
|
||||
}
|
||||
try {
|
||||
antiwindup = this->nh_->get_parameter(parameter_prefix + "." + antiwindup_str).as_bool();
|
||||
} catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
|
||||
RCLCPP_ERROR(
|
||||
this->nh_->get_logger(),
|
||||
"Parameter '%s' not declared, with error %s", antiwindup_str.c_str(), ex.what());
|
||||
} catch (rclcpp::ParameterTypeException & ex) {
|
||||
RCLCPP_ERROR(
|
||||
this->nh_->get_logger(),
|
||||
"Parameter '%s' has wrong type: %s", antiwindup_str.c_str(), ex.what());
|
||||
}
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"Setting kp = " << kp << "\t"
|
||||
<< " ki = " << ki << "\t"
|
||||
<< " kd = " << kd << "\t"
|
||||
<< " max_integral_error = " << max_integral_error << "\t"
|
||||
<< "antiwindup =" << std::boolalpha << antiwindup << " from node parameters");
|
||||
pid.initPid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup);
|
||||
}
|
||||
|
||||
return are_pids_set;
|
||||
}
|
||||
|
||||
void GazeboSystem::registerJoints(
|
||||
const hardware_interface::HardwareInfo & hardware_info,
|
||||
gazebo::physics::ModelPtr parent_model)
|
||||
{
|
||||
this->dataPtr->n_dof_ = hardware_info.joints.size();
|
||||
|
||||
this->dataPtr->joint_names_.resize(this->dataPtr->n_dof_);
|
||||
this->dataPtr->joint_control_methods_.resize(this->dataPtr->n_dof_);
|
||||
this->dataPtr->is_joint_actuated_.resize(this->dataPtr->n_dof_);
|
||||
this->dataPtr->joint_position_.resize(this->dataPtr->n_dof_);
|
||||
this->dataPtr->joint_velocity_.resize(this->dataPtr->n_dof_);
|
||||
this->dataPtr->joint_effort_.resize(this->dataPtr->n_dof_);
|
||||
this->dataPtr->joint_position_cmd_.resize(this->dataPtr->n_dof_);
|
||||
this->dataPtr->joint_velocity_cmd_.resize(this->dataPtr->n_dof_);
|
||||
this->dataPtr->joint_effort_cmd_.resize(this->dataPtr->n_dof_);
|
||||
this->dataPtr->vel_pid.resize(this->dataPtr->n_dof_);
|
||||
this->dataPtr->pos_pid.resize(this->dataPtr->n_dof_);
|
||||
this->dataPtr->is_pos_pid.resize(this->dataPtr->n_dof_);
|
||||
this->dataPtr->is_vel_pid.resize(this->dataPtr->n_dof_);
|
||||
|
||||
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "registerJoints: n_dof_ = [" << this->dataPtr->n_dof_ << "]");
|
||||
|
||||
for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) {
|
||||
auto & joint_info = hardware_info.joints[j];
|
||||
const std::string joint_name = this->dataPtr->joint_names_[j] = joint_info.name;
|
||||
|
||||
gazebo::physics::JointPtr simjoint = parent_model->GetJoint(joint_name);
|
||||
this->dataPtr->sim_joints_.push_back(simjoint);
|
||||
if (!simjoint) {
|
||||
RCLCPP_WARN_STREAM(
|
||||
this->nh_->get_logger(), "Skipping joint in the URDF named '" << joint_name <<
|
||||
"' which is not in the gazebo model.");
|
||||
continue;
|
||||
}
|
||||
|
||||
// Accept this joint and continue configuration
|
||||
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name);
|
||||
|
||||
std::string suffix = "";
|
||||
|
||||
// check if joint is mimicked
|
||||
if (joint_info.parameters.find("mimic") != joint_info.parameters.end()) {
|
||||
const auto mimicked_joint = joint_info.parameters.at("mimic");
|
||||
const auto mimicked_joint_it = std::find_if(
|
||||
hardware_info.joints.begin(), hardware_info.joints.end(),
|
||||
[&mimicked_joint](const hardware_interface::ComponentInfo & info) {
|
||||
return info.name == mimicked_joint;
|
||||
});
|
||||
if (mimicked_joint_it == hardware_info.joints.end()) {
|
||||
throw std::runtime_error(
|
||||
std::string("Mimicked joint '") + mimicked_joint + "' not found");
|
||||
}
|
||||
MimicJoint mimic_joint;
|
||||
mimic_joint.joint_index = j;
|
||||
mimic_joint.mimicked_joint_index = std::distance(
|
||||
hardware_info.joints.begin(), mimicked_joint_it);
|
||||
auto param_it = joint_info.parameters.find("multiplier");
|
||||
if (param_it != joint_info.parameters.end()) {
|
||||
mimic_joint.multiplier = std::stod(joint_info.parameters.at("multiplier"));
|
||||
} else {
|
||||
mimic_joint.multiplier = 1.0;
|
||||
}
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"Joint '" << joint_name << "'is mimicking joint '" << mimicked_joint <<
|
||||
"' with multiplier: " << mimic_joint.multiplier);
|
||||
this->dataPtr->mimic_joints_.push_back(mimic_joint);
|
||||
suffix = "_mimic";
|
||||
}
|
||||
|
||||
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:");
|
||||
|
||||
auto get_initial_value =
|
||||
[this, joint_name](const hardware_interface::InterfaceInfo & interface_info) {
|
||||
double initial_value{0.0};
|
||||
if (!interface_info.initial_value.empty()) {
|
||||
try {
|
||||
initial_value = std::stod(interface_info.initial_value);
|
||||
RCLCPP_INFO(this->nh_->get_logger(), "\t\t\t found initial value: %f", initial_value);
|
||||
} catch (std::invalid_argument &) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"Failed converting initial_value string to real number for the joint "
|
||||
<< joint_name
|
||||
<< " and state interface " << interface_info.name
|
||||
<< ". Actual value of parameter: " << interface_info.initial_value
|
||||
<< ". Initial value will be set to 0.0");
|
||||
throw std::invalid_argument("Failed converting initial_value string");
|
||||
}
|
||||
}
|
||||
return initial_value;
|
||||
};
|
||||
|
||||
double initial_position = std::numeric_limits<double>::quiet_NaN();
|
||||
double initial_velocity = std::numeric_limits<double>::quiet_NaN();
|
||||
double initial_effort = std::numeric_limits<double>::quiet_NaN();
|
||||
|
||||
// register the state handles
|
||||
for (unsigned int i = 0; i < joint_info.state_interfaces.size(); i++) {
|
||||
if (joint_info.state_interfaces[i].name == "position") {
|
||||
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position");
|
||||
this->dataPtr->state_interfaces_.emplace_back(
|
||||
joint_name + suffix,
|
||||
hardware_interface::HW_IF_POSITION,
|
||||
&this->dataPtr->joint_position_[j]);
|
||||
initial_position = get_initial_value(joint_info.state_interfaces[i]);
|
||||
this->dataPtr->joint_position_[j] = initial_position;
|
||||
}
|
||||
if (joint_info.state_interfaces[i].name == "velocity") {
|
||||
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity");
|
||||
this->dataPtr->state_interfaces_.emplace_back(
|
||||
joint_name + suffix,
|
||||
hardware_interface::HW_IF_VELOCITY,
|
||||
&this->dataPtr->joint_velocity_[j]);
|
||||
initial_velocity = get_initial_value(joint_info.state_interfaces[i]);
|
||||
this->dataPtr->joint_velocity_[j] = initial_velocity;
|
||||
}
|
||||
if (joint_info.state_interfaces[i].name == "effort") {
|
||||
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort");
|
||||
this->dataPtr->state_interfaces_.emplace_back(
|
||||
joint_name + suffix,
|
||||
hardware_interface::HW_IF_EFFORT,
|
||||
&this->dataPtr->joint_effort_[j]);
|
||||
initial_effort = get_initial_value(joint_info.state_interfaces[i]);
|
||||
this->dataPtr->joint_effort_[j] = initial_effort;
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tCommand:");
|
||||
|
||||
// register the command handles
|
||||
bool has_already_registered_vel = false;
|
||||
bool has_already_registered_pos = false;
|
||||
|
||||
for (unsigned int i = 0; i < joint_info.command_interfaces.size(); i++) {
|
||||
if (joint_info.command_interfaces[i].name == "position" ||
|
||||
joint_info.command_interfaces[i].name == "position_pid")
|
||||
{
|
||||
if (has_already_registered_pos) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"can't have position and position_pid"
|
||||
<< "command_interface at same time !!!");
|
||||
continue;
|
||||
}
|
||||
has_already_registered_pos = true;
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(), "\t\t "
|
||||
<< joint_info.command_interfaces[i].name);
|
||||
|
||||
this->dataPtr->is_pos_pid[j] = this->extractPID(
|
||||
POSITION_PID_PARAMS_PREFIX, joint_info,
|
||||
this->dataPtr->pos_pid[j]) || this->extractPIDFromParameters(
|
||||
joint_info.command_interfaces[i].name, joint_name, this->dataPtr->pos_pid[j]);
|
||||
|
||||
if (this->dataPtr->is_pos_pid[j]) {
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(), "Found position PIDs for joint: " << joint_name << "!");
|
||||
this->dataPtr->position_control_method_ = POSITION_PID;
|
||||
}
|
||||
|
||||
this->dataPtr->command_interfaces_.emplace_back(
|
||||
joint_name + suffix,
|
||||
hardware_interface::HW_IF_POSITION,
|
||||
&this->dataPtr->joint_position_cmd_[j]);
|
||||
if (!std::isnan(initial_position)) {
|
||||
this->dataPtr->joint_position_cmd_[j] = initial_position;
|
||||
}
|
||||
}
|
||||
// independently of existence of command interface set initial value if defined
|
||||
if (!std::isnan(initial_position)) {
|
||||
this->dataPtr->sim_joints_[j]->SetPosition(0, initial_position, true);
|
||||
}
|
||||
if (joint_info.command_interfaces[i].name == "velocity" ||
|
||||
joint_info.command_interfaces[i].name == "velocity_pid")
|
||||
{
|
||||
if (has_already_registered_vel) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
this->nh_->get_logger(), "can't have velocity and velocity_pid "
|
||||
<< "command_interface at same time !!!");
|
||||
continue;
|
||||
}
|
||||
has_already_registered_vel = true;
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(), "\t\t "
|
||||
<< joint_info.command_interfaces[i].name);
|
||||
|
||||
this->dataPtr->is_vel_pid[j] = this->extractPID(
|
||||
VELOCITY_PID_PARAMS_PREFIX, joint_info,
|
||||
this->dataPtr->vel_pid[j]) || this->extractPIDFromParameters(
|
||||
joint_info.command_interfaces[i].name, joint_name, this->dataPtr->vel_pid[j]);
|
||||
|
||||
if (this->dataPtr->is_vel_pid[j]) {
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(), "Found velocity PIDs for joint: " << joint_name << "!");
|
||||
this->dataPtr->velocity_control_method_ = VELOCITY_PID;
|
||||
}
|
||||
|
||||
this->dataPtr->command_interfaces_.emplace_back(
|
||||
joint_name + suffix,
|
||||
hardware_interface::HW_IF_VELOCITY,
|
||||
&this->dataPtr->joint_velocity_cmd_[j]);
|
||||
if (!std::isnan(initial_velocity)) {
|
||||
this->dataPtr->joint_velocity_cmd_[j] = initial_velocity;
|
||||
}
|
||||
}
|
||||
// independently of existence of command interface set initial value if defined
|
||||
if (!std::isnan(initial_velocity)) {
|
||||
this->dataPtr->sim_joints_[j]->SetVelocity(0, initial_velocity);
|
||||
}
|
||||
if (joint_info.command_interfaces[i].name == "effort") {
|
||||
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort");
|
||||
this->dataPtr->command_interfaces_.emplace_back(
|
||||
joint_name + suffix,
|
||||
hardware_interface::HW_IF_EFFORT,
|
||||
&this->dataPtr->joint_effort_cmd_[j]);
|
||||
if (!std::isnan(initial_effort)) {
|
||||
this->dataPtr->joint_effort_cmd_[j] = initial_effort;
|
||||
}
|
||||
}
|
||||
// independently of existence of command interface set initial value if defined
|
||||
if (!std::isnan(initial_effort)) {
|
||||
this->dataPtr->sim_joints_[j]->SetForce(0, initial_effort);
|
||||
}
|
||||
}
|
||||
|
||||
// check if joint is actuated (has command interfaces) or passive
|
||||
this->dataPtr->is_joint_actuated_[j] = (joint_info.command_interfaces.size() > 0);
|
||||
}
|
||||
}
|
||||
|
||||
void GazeboSystem::registerSensors(
|
||||
const hardware_interface::HardwareInfo & hardware_info,
|
||||
gazebo::physics::ModelPtr parent_model)
|
||||
{
|
||||
// Collect gazebo sensor handles
|
||||
size_t n_sensors = hardware_info.sensors.size();
|
||||
std::vector<hardware_interface::ComponentInfo> imu_components_;
|
||||
std::vector<hardware_interface::ComponentInfo> ft_sensor_components_;
|
||||
|
||||
// This is split in two steps: Count the number and type of sensor and associate the interfaces
|
||||
// So we have resize only once the structures where the data will be stored, and we can safely
|
||||
// use pointers to the structures
|
||||
for (unsigned int j = 0; j < n_sensors; j++) {
|
||||
hardware_interface::ComponentInfo component = hardware_info.sensors[j];
|
||||
std::string sensor_name = component.name;
|
||||
|
||||
// This can't be used, because it only looks for sensor in links, but force_torque_sensor
|
||||
// must be in a joint, as in not found by SensorScopedName
|
||||
// std::vector<std::string> gz_sensor_names = parent_model->SensorScopedName(sensor_name);
|
||||
|
||||
// Workaround to find sensors whose parent is a link or joint of parent_model
|
||||
std::vector<std::string> gz_sensor_names;
|
||||
for (const auto & s : gazebo::sensors::SensorManager::Instance()->GetSensors()) {
|
||||
const std::string sensor_parent = s->ParentName();
|
||||
if (s->Name() != sensor_name) {
|
||||
continue;
|
||||
}
|
||||
if ((parent_model->GetJoint(sensor_parent) != nullptr) ||
|
||||
parent_model->GetLink(sensor_parent) != nullptr)
|
||||
{
|
||||
gz_sensor_names.push_back(s->ScopedName());
|
||||
}
|
||||
}
|
||||
if (gz_sensor_names.empty()) {
|
||||
RCLCPP_WARN_STREAM(
|
||||
this->nh_->get_logger(), "Skipping sensor in the URDF named '" << sensor_name <<
|
||||
"' which is not in the gazebo model.");
|
||||
continue;
|
||||
}
|
||||
if (gz_sensor_names.size() > 1) {
|
||||
RCLCPP_WARN_STREAM(
|
||||
this->nh_->get_logger(), "Sensor in the URDF named '" << sensor_name <<
|
||||
"' has more than one gazebo sensor with the " <<
|
||||
"same name, only using the first. It has " << gz_sensor_names.size() << " sensors");
|
||||
}
|
||||
|
||||
gazebo::sensors::SensorPtr simsensor = gazebo::sensors::SensorManager::Instance()->GetSensor(
|
||||
gz_sensor_names[0]);
|
||||
if (!simsensor) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"Error retrieving sensor '" << sensor_name << " from the sensor manager");
|
||||
continue;
|
||||
}
|
||||
if (simsensor->Type() == "imu") {
|
||||
gazebo::sensors::ImuSensorPtr imu_sensor =
|
||||
std::dynamic_pointer_cast<gazebo::sensors::ImuSensor>(simsensor);
|
||||
if (!imu_sensor) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"Error retrieving casting sensor '" << sensor_name << " to ImuSensor");
|
||||
continue;
|
||||
}
|
||||
imu_components_.push_back(component);
|
||||
this->dataPtr->sim_imu_sensors_.push_back(imu_sensor);
|
||||
} else if (simsensor->Type() == "force_torque") {
|
||||
gazebo::sensors::ForceTorqueSensorPtr ft_sensor =
|
||||
std::dynamic_pointer_cast<gazebo::sensors::ForceTorqueSensor>(simsensor);
|
||||
if (!ft_sensor) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"Error retrieving casting sensor '" << sensor_name << " to ForceTorqueSensor");
|
||||
continue;
|
||||
}
|
||||
ft_sensor_components_.push_back(component);
|
||||
this->dataPtr->sim_ft_sensors_.push_back(ft_sensor);
|
||||
}
|
||||
}
|
||||
|
||||
this->dataPtr->imu_sensor_data_.resize(this->dataPtr->sim_imu_sensors_.size());
|
||||
this->dataPtr->ft_sensor_data_.resize(this->dataPtr->sim_ft_sensors_.size());
|
||||
this->dataPtr->n_sensors_ = this->dataPtr->sim_imu_sensors_.size() +
|
||||
this->dataPtr->sim_ft_sensors_.size();
|
||||
|
||||
for (unsigned int i = 0; i < imu_components_.size(); i++) {
|
||||
const std::string & sensor_name = imu_components_[i].name;
|
||||
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << sensor_name);
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(), "\tState:");
|
||||
for (const auto & state_interface : imu_components_[i].state_interfaces) {
|
||||
static const std::map<std::string, size_t> interface_name_map = {
|
||||
{"orientation.x", 0},
|
||||
{"orientation.y", 1},
|
||||
{"orientation.z", 2},
|
||||
{"orientation.w", 3},
|
||||
{"angular_velocity.x", 4},
|
||||
{"angular_velocity.y", 5},
|
||||
{"angular_velocity.z", 6},
|
||||
{"linear_acceleration.x", 7},
|
||||
{"linear_acceleration.y", 8},
|
||||
{"linear_acceleration.z", 9},
|
||||
};
|
||||
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << state_interface.name);
|
||||
|
||||
size_t data_index = interface_name_map.at(state_interface.name);
|
||||
this->dataPtr->state_interfaces_.emplace_back(
|
||||
sensor_name,
|
||||
state_interface.name,
|
||||
&this->dataPtr->imu_sensor_data_[i][data_index]);
|
||||
}
|
||||
}
|
||||
for (unsigned int i = 0; i < ft_sensor_components_.size(); i++) {
|
||||
const std::string & sensor_name = ft_sensor_components_[i].name;
|
||||
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << sensor_name);
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(), "\tState:");
|
||||
for (const auto & state_interface : ft_sensor_components_[i].state_interfaces) {
|
||||
static const std::map<std::string, size_t> interface_name_map = {
|
||||
{"force.x", 0},
|
||||
{"force.y", 1},
|
||||
{"force.z", 2},
|
||||
{"torque.x", 3},
|
||||
{"torque.y", 4},
|
||||
{"torque.z", 5}
|
||||
};
|
||||
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << state_interface.name);
|
||||
|
||||
size_t data_index = interface_name_map.at(state_interface.name);
|
||||
this->dataPtr->state_interfaces_.emplace_back(
|
||||
sensor_name,
|
||||
state_interface.name,
|
||||
&this->dataPtr->ft_sensor_data_[i][data_index]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
CallbackReturn
|
||||
GazeboSystem::on_init(const hardware_interface::HardwareInfo & system_info)
|
||||
{
|
||||
if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) {
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
std::vector<hardware_interface::StateInterface>
|
||||
GazeboSystem::export_state_interfaces()
|
||||
{
|
||||
return std::move(this->dataPtr->state_interfaces_);
|
||||
}
|
||||
|
||||
std::vector<hardware_interface::CommandInterface>
|
||||
GazeboSystem::export_command_interfaces()
|
||||
{
|
||||
return std::move(this->dataPtr->command_interfaces_);
|
||||
}
|
||||
|
||||
CallbackReturn GazeboSystem::on_activate(const rclcpp_lifecycle::State & previous_state)
|
||||
{
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn GazeboSystem::on_deactivate(const rclcpp_lifecycle::State & previous_state)
|
||||
{
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::return_type
|
||||
GazeboSystem::perform_command_mode_switch(
|
||||
const std::vector<std::string> & start_interfaces,
|
||||
const std::vector<std::string> & stop_interfaces)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"=== perform_command_mode_switch called ===");
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"Start interfaces: " << start_interfaces.size());
|
||||
for (const auto & iface : start_interfaces) {
|
||||
RCLCPP_INFO_STREAM(this->nh_->get_logger(), " - " << iface);
|
||||
}
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"Stop interfaces: " << stop_interfaces.size());
|
||||
for (const auto & iface : stop_interfaces) {
|
||||
RCLCPP_INFO_STREAM(this->nh_->get_logger(), " - " << iface);
|
||||
}
|
||||
|
||||
for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) {
|
||||
for (const std::string & interface_name : stop_interfaces) {
|
||||
// Clear joint control method bits corresponding to stop interfaces
|
||||
if (interface_name == // NOLINT
|
||||
(this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION))
|
||||
{
|
||||
this->dataPtr->joint_control_methods_[j] &=
|
||||
static_cast<ControlMethod_>(VELOCITY & VELOCITY_PID & EFFORT);
|
||||
|
||||
} else if (interface_name == // NOLINT
|
||||
(this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY))
|
||||
{
|
||||
this->dataPtr->joint_control_methods_[j] &=
|
||||
static_cast<ControlMethod_>(POSITION & POSITION_PID & EFFORT);
|
||||
|
||||
} else if (interface_name == // NOLINT
|
||||
(this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT))
|
||||
{
|
||||
this->dataPtr->joint_control_methods_[j] &=
|
||||
static_cast<ControlMethod_>(POSITION & POSITION_PID & VELOCITY_PID & VELOCITY);
|
||||
}
|
||||
}
|
||||
|
||||
// Set joint control method bits corresponding to start interfaces
|
||||
for (const std::string & interface_name : start_interfaces) {
|
||||
if (interface_name ==
|
||||
(this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION))
|
||||
{
|
||||
this->dataPtr->joint_control_methods_[j] |=
|
||||
static_cast<ControlMethod_>(this->dataPtr->position_control_method_);
|
||||
} else if (interface_name == // NOLINT
|
||||
(this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY))
|
||||
{
|
||||
this->dataPtr->joint_control_methods_[j] |=
|
||||
static_cast<ControlMethod_>(this->dataPtr->velocity_control_method_);
|
||||
} else if (interface_name == // NOLINT
|
||||
(this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT))
|
||||
{
|
||||
this->dataPtr->joint_control_methods_[j] |= EFFORT;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// mimic joint has the same control mode as mimicked joint
|
||||
for (const auto & mimic_joint : this->dataPtr->mimic_joints_) {
|
||||
this->dataPtr->joint_control_methods_[mimic_joint.joint_index] =
|
||||
this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index];
|
||||
}
|
||||
|
||||
// Print joint_control_methods_ after mode switch
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"=== Joint Control Methods After Mode Switch ===");
|
||||
for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) {
|
||||
std::string method_str = "";
|
||||
auto & method = this->dataPtr->joint_control_methods_[j];
|
||||
|
||||
// Check each control method using bitwise operations
|
||||
if (!static_cast<bool>(method)) {
|
||||
method_str = "NONE";
|
||||
} else {
|
||||
if (method & POSITION) method_str += "POSITION ";
|
||||
if (method & VELOCITY) method_str += "VELOCITY ";
|
||||
if (method & EFFORT) method_str += "EFFORT ";
|
||||
if (method & VELOCITY_PID) method_str += "VELOCITY_PID ";
|
||||
if (method & POSITION_PID) method_str += "POSITION_PID ";
|
||||
}
|
||||
|
||||
// Get the underlying value for display by checking each bit
|
||||
int method_value = 0;
|
||||
if (method & POSITION) method_value |= static_cast<int>(POSITION);
|
||||
if (method & VELOCITY) method_value |= static_cast<int>(VELOCITY);
|
||||
if (method & EFFORT) method_value |= static_cast<int>(EFFORT);
|
||||
if (method & VELOCITY_PID) method_value |= static_cast<int>(VELOCITY_PID);
|
||||
if (method & POSITION_PID) method_value |= static_cast<int>(POSITION_PID);
|
||||
|
||||
if (method_value != 0) { // Only print non-zero values
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"Joint [" << j << "] " << this->dataPtr->joint_names_[j]
|
||||
<< ": control_method = 0x" << std::hex << method_value << std::dec
|
||||
<< " (" << method_str << ")");
|
||||
}
|
||||
}
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->nh_->get_logger(),
|
||||
"=== End Joint Control Methods After Mode Switch ===");
|
||||
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
hardware_interface::return_type GazeboSystem::read(
|
||||
const rclcpp::Time & time,
|
||||
const rclcpp::Duration & period)
|
||||
{
|
||||
for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) {
|
||||
if (this->dataPtr->sim_joints_[j]) {
|
||||
this->dataPtr->joint_position_[j] = this->dataPtr->sim_joints_[j]->Position(0);
|
||||
this->dataPtr->joint_velocity_[j] = this->dataPtr->sim_joints_[j]->GetVelocity(0);
|
||||
this->dataPtr->joint_effort_[j] = this->dataPtr->sim_joints_[j]->GetForce(0u);
|
||||
}
|
||||
}
|
||||
|
||||
for (unsigned int j = 0; j < this->dataPtr->sim_imu_sensors_.size(); j++) {
|
||||
auto sim_imu = this->dataPtr->sim_imu_sensors_[j];
|
||||
this->dataPtr->imu_sensor_data_[j][0] = sim_imu->Orientation().X();
|
||||
this->dataPtr->imu_sensor_data_[j][1] = sim_imu->Orientation().Y();
|
||||
this->dataPtr->imu_sensor_data_[j][2] = sim_imu->Orientation().Z();
|
||||
this->dataPtr->imu_sensor_data_[j][3] = sim_imu->Orientation().W();
|
||||
|
||||
this->dataPtr->imu_sensor_data_[j][4] = sim_imu->AngularVelocity().X();
|
||||
this->dataPtr->imu_sensor_data_[j][5] = sim_imu->AngularVelocity().Y();
|
||||
this->dataPtr->imu_sensor_data_[j][6] = sim_imu->AngularVelocity().Z();
|
||||
|
||||
this->dataPtr->imu_sensor_data_[j][7] = sim_imu->LinearAcceleration().X();
|
||||
this->dataPtr->imu_sensor_data_[j][8] = sim_imu->LinearAcceleration().Y();
|
||||
this->dataPtr->imu_sensor_data_[j][9] = sim_imu->LinearAcceleration().Z();
|
||||
}
|
||||
|
||||
for (unsigned int j = 0; j < this->dataPtr->sim_ft_sensors_.size(); j++) {
|
||||
auto sim_ft_sensor = this->dataPtr->sim_ft_sensors_[j];
|
||||
this->dataPtr->ft_sensor_data_[j][0] = sim_ft_sensor->Force().X();
|
||||
this->dataPtr->ft_sensor_data_[j][1] = sim_ft_sensor->Force().Y();
|
||||
this->dataPtr->ft_sensor_data_[j][2] = sim_ft_sensor->Force().Z();
|
||||
|
||||
this->dataPtr->ft_sensor_data_[j][3] = sim_ft_sensor->Torque().X();
|
||||
this->dataPtr->ft_sensor_data_[j][4] = sim_ft_sensor->Torque().Y();
|
||||
this->dataPtr->ft_sensor_data_[j][5] = sim_ft_sensor->Torque().Z();
|
||||
}
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
int debug_cnt = 0;
|
||||
int sim_joints_cnt = 0;
|
||||
int write_cnt = 0;
|
||||
hardware_interface::return_type GazeboSystem::write(
|
||||
const rclcpp::Time & time,
|
||||
const rclcpp::Duration & period)
|
||||
{
|
||||
// Get the simulation time and period
|
||||
gazebo::common::Time gz_time_now = this->dataPtr->parent_model_->GetWorld()->SimTime();
|
||||
rclcpp::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
|
||||
rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_;
|
||||
|
||||
// set values of all mimic joints with respect to mimicked joint
|
||||
for (const auto & mimic_joint : this->dataPtr->mimic_joints_) {
|
||||
if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION &&
|
||||
this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION)
|
||||
{
|
||||
this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] =
|
||||
mimic_joint.multiplier *
|
||||
this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index];
|
||||
} else if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY && // NOLINT
|
||||
this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY)
|
||||
{
|
||||
this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] =
|
||||
mimic_joint.multiplier *
|
||||
this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index];
|
||||
} else if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT && // NOLINT
|
||||
this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT)
|
||||
{
|
||||
this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] =
|
||||
mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index];
|
||||
}
|
||||
}
|
||||
|
||||
uint64_t dt = sim_period.nanoseconds();
|
||||
debug_cnt++;
|
||||
|
||||
for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) {
|
||||
if (this->dataPtr->sim_joints_[j]) {
|
||||
sim_joints_cnt++;
|
||||
if (this->dataPtr->joint_control_methods_[j] & POSITION) {
|
||||
double cmd = this->dataPtr->joint_position_cmd_[j];
|
||||
this->dataPtr->sim_joints_[j]->SetPosition(0, cmd, false);
|
||||
write_cnt++;
|
||||
// this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
|
||||
} else if (this->dataPtr->joint_control_methods_[j] & VELOCITY) {
|
||||
this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]);
|
||||
} else if (this->dataPtr->joint_control_methods_[j] & EFFORT) {
|
||||
this->dataPtr->sim_joints_[j]->SetForce(0, this->dataPtr->joint_effort_cmd_[j]);
|
||||
} else if (this->dataPtr->joint_control_methods_[j] & VELOCITY_PID) {
|
||||
double vel_goal = this->dataPtr->joint_velocity_cmd_[j];
|
||||
double vel = this->dataPtr->sim_joints_[j]->GetVelocity(0);
|
||||
double cmd = this->dataPtr->vel_pid[j].computeCommand(vel_goal - vel, dt);
|
||||
this->dataPtr->sim_joints_[j]->SetForce(0, cmd);
|
||||
} else if (this->dataPtr->joint_control_methods_[j] & POSITION_PID) {
|
||||
double pos_goal = this->dataPtr->joint_position_cmd_[j];
|
||||
double pos = this->dataPtr->sim_joints_[j]->Position(0);
|
||||
double cmd = this->dataPtr->pos_pid[j].computeCommand(pos_goal - pos, dt);
|
||||
this->dataPtr->sim_joints_[j]->SetForce(0, cmd);
|
||||
} else if (this->dataPtr->is_joint_actuated_[j] && this->dataPtr->hold_joints_) {
|
||||
// Fallback case is a velocity command of zero (only for actuated joints)
|
||||
// this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (debug_cnt >= 1000) {
|
||||
RCLCPP_INFO(rclcpp::get_logger("GazeboSystem"), "check update period, debug_cnt: %d, write_cnt: %d",
|
||||
debug_cnt, write_cnt);
|
||||
debug_cnt = 0;
|
||||
sim_joints_cnt = 0;
|
||||
write_cnt = 0;
|
||||
}
|
||||
|
||||
this->dataPtr->last_update_sim_time_ros_ = sim_time_ros;
|
||||
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
} // namespace my_gazebo_ros2_control
|
||||
|
||||
#include "pluginlib/class_list_macros.hpp" // NOLINT
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
my_gazebo_ros2_control::GazeboSystem, my_gazebo_ros2_control::GazeboSystemInterface)
|
||||
1
zhengjie_simulator/ocs2
Submodule
1
zhengjie_simulator/ocs2
Submodule
Submodule zhengjie_simulator/ocs2 added at 5b46a9b887
1
zhengjie_simulator/ocs2_robotic_assets
Submodule
1
zhengjie_simulator/ocs2_robotic_assets
Submodule
Submodule zhengjie_simulator/ocs2_robotic_assets added at 9feab348a1
@@ -0,0 +1,15 @@
|
||||
material Beer/Diffuse
|
||||
{
|
||||
technique
|
||||
{
|
||||
pass
|
||||
{
|
||||
texture_unit
|
||||
{
|
||||
texture beer.png
|
||||
filtering anistropic
|
||||
max_anisotropy 16
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
BIN
zhengjie_simulator/props/beer/materials/textures/beer.png
Normal file
BIN
zhengjie_simulator/props/beer/materials/textures/beer.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 2.1 MiB |
44
zhengjie_simulator/props/beer/model-1_4.sdf
Normal file
44
zhengjie_simulator/props/beer/model-1_4.sdf
Normal file
@@ -0,0 +1,44 @@
|
||||
<?xml version='1.0'?>
|
||||
<sdf version='1.4'>
|
||||
<model name="beer">
|
||||
<link name='link'>
|
||||
<pose>0 0 0.115 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.390</mass>
|
||||
<inertia>
|
||||
<ixx>0.00058</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.00058</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.00019</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='collision'>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.055000</radius>
|
||||
<length>0.230000</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name='visual'>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.055000</radius>
|
||||
<length>0.230000</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
|
||||
<material>
|
||||
<script>
|
||||
<uri>model://beer/materials/scripts</uri>
|
||||
<uri>model://beer/materials/textures</uri>
|
||||
<name>Beer/Diffuse</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
16
zhengjie_simulator/props/beer/model.config
Normal file
16
zhengjie_simulator/props/beer/model.config
Normal file
@@ -0,0 +1,16 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<model>
|
||||
<name>Beer</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.4">model-1_4.sdf</sdf>
|
||||
<sdf version="1.5">model.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>Maurice Fallon</name>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
Beer
|
||||
</description>
|
||||
</model>
|
||||
42
zhengjie_simulator/props/beer/model.sdf
Normal file
42
zhengjie_simulator/props/beer/model.sdf
Normal file
@@ -0,0 +1,42 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.5">
|
||||
<model name="beer">
|
||||
<link name="link">
|
||||
<pose>0 0 0.115 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.390</mass>
|
||||
<inertia>
|
||||
<ixx>0.00058</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.00058</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.00019</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.055000</radius>
|
||||
<length>0.230000</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.055000</radius>
|
||||
<length>0.230000</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>model://beer/materials/scripts</uri>
|
||||
<uri>model://beer/materials/textures</uri>
|
||||
<name>Beer/Diffuse</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user