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

add tf transform

Browse Source
This commit is contained in:
david
2025-10-14 13:54:59 +08:00
parent 75ba0f9429
commit 5d4aaff556
8 changed files with 201 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
src/brain/CMakeLists.txt
View File

@@ -27,6 +27,9 @@ find_package(ament_index_cpp REQUIRED)
find_package(Doxygen QUIET)
find_package(std_srvs REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
# 创建一个可执行文件，将两个节点编译到一个进程中
add_executable(brain_node
@@ -57,6 +60,9 @@ ament_target_dependencies(brain_node
ament_index_cpp
yaml-cpp
std_srvs
tf2
tf2_ros
tf2_geometry_msgs
)
target_link_libraries(brain_node
@@ -100,6 +106,9 @@ if(BUILD_TESTING)
brain_interfaces
nav2_msgs
std_srvs
tf2
tf2_ros
tf2_geometry_msgs
)
target_include_directories(test_action_registry PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
endif()
@@ -112,6 +121,9 @@ if(BUILD_TESTING)
rclcpp
behaviortree_cpp
std_srvs
tf2
tf2_ros
tf2_geometry_msgs
)
target_include_directories(test_bt_registry PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
endif()
@@ -124,6 +136,9 @@ if(BUILD_TESTING)
rclcpp
behaviortree_cpp
std_srvs
tf2
tf2_ros
tf2_geometry_msgs
)
target_include_directories(test_bt_registry_xml PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
endif()
@@ -146,6 +161,9 @@ if(BUILD_TESTING)
brain_interfaces
nav2_msgs
std_srvs
tf2
tf2_ros
tf2_geometry_msgs
)
target_include_directories(test_cerebellum_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(test_cerebellum_node Boost::thread)
@@ -171,6 +189,9 @@ if(BUILD_TESTING)
ament_index_cpp
nav2_msgs
std_srvs
tf2
tf2_ros
tf2_geometry_msgs
)
target_include_directories(test_cerebrum_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(test_cerebrum_node Boost::thread)
@@ -190,6 +211,9 @@ if(BUILD_TESTING)
brain_interfaces
nav2_msgs
std_srvs
tf2
tf2_ros
tf2_geometry_msgs
)
target_include_directories(test_sm_cerebellum PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
endif()
@@ -213,6 +237,9 @@ if(BUILD_TESTING)
ament_index_cpp
nav2_msgs
std_srvs
tf2
tf2_ros
tf2_geometry_msgs
)
target_include_directories(test_execute_bt_action PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(test_execute_bt_action Boost::thread yaml-cpp)
@@ -237,6 +264,9 @@ if(BUILD_TESTING)
ament_index_cpp
nav2_msgs
std_srvs
tf2
tf2_ros
tf2_geometry_msgs
)
target_include_directories(test_execute_bt_action_extended PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(test_execute_bt_action_extended Boost::thread yaml-cpp)
@@ -253,7 +283,7 @@ if(BUILD_TESTING)
if(TARGET ${test_name})
target_compile_definitions(${test_name} PRIVATE BRAIN_DISABLE_SM=1)
ament_target_dependencies(${test_name}
rclcpp rclcpp_action behaviortree_cpp interfaces brain_interfaces smacc2 smacc2_msgs ament_index_cpp nav2_msgs std_srvs)
rclcpp rclcpp_action behaviortree_cpp interfaces brain_interfaces smacc2 smacc2_msgs ament_index_cpp nav2_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs)
target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(${test_name} Boost::thread yaml-cpp)
endif()
@@ -268,7 +298,7 @@ if(BUILD_TESTING)
if(TARGET test_cerebrum_utils)
target_compile_definitions(test_cerebrum_utils PRIVATE BRAIN_DISABLE_SM=1)
ament_target_dependencies(test_cerebrum_utils
rclcpp rclcpp_action behaviortree_cpp interfaces brain_interfaces smacc2 smacc2_msgs ament_index_cpp nav2_msgs std_srvs)
rclcpp rclcpp_action behaviortree_cpp interfaces brain_interfaces smacc2 smacc2_msgs ament_index_cpp nav2_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs)
target_include_directories(test_cerebrum_utils PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(test_cerebrum_utils Boost::thread yaml-cpp)
endif()

12
src/brain/config/bt_vision_grasp.xml
View File

@@ -1,9 +1,15 @@
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<Sequence name="root">
<!-- <VisionObjectRecognition_H name="VisionObjectRecognition_H" />
<HandControl_H name="HandControl_H" /> -->
<Arm_H name="Arm_H" />
<!-- Retry vision + hand until HandControl_H succeeds -->
<RetryUntilSuccessful name="retry_vision_hand" num_attempts="5">
<Sequence>
<VisionObjectRecognition_H name="VisionObjectRecognition_H" />
<Arm_H name="Arm_H" />
</Sequence>
</RetryUntilSuccessful>
<!-- After Arm_H eventually succeeds, run the hand -->
<HandControl_H name="HandControl_H" />
</Sequence>
</BehaviorTree>
</root>

18
src/brain/include/brain/cerebellum_node.hpp
View File

@@ -3,14 +3,18 @@
#include <atomic>
#include <limits>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <std_msgs/msg/string.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include "brain_interfaces/action/execute_bt_action.hpp"
#include "brain/action_registry.hpp"
#include "brain/skill_manager.hpp"
#include <smacc2/smacc_signal_detector.hpp>
#include "brain/sm_cerebellum.hpp" // for CerebellumData::ExecResult
#include <std_msgs/msg/string.hpp>
namespace brain
{
@@ -67,6 +71,13 @@ private:
double stats_log_interval_sec_ {60.0};
std::mutex stats_mutex_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr stats_pub_;
std::unordered_map<std::string, geometry_msgs::msg::PoseStamped> target_pose_;
std::string target_frame_{"bottle"};
int64_t command_id_{0};
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::string arm_target_frame_{"base_link_leftarm"};
double arm_transform_timeout_sec_{0.2};
// Current accepted sequence epoch (from client) used to filter/reject stale goals
std::atomic<uint32_t> current_epoch_{0};
@@ -285,6 +296,11 @@ private:
/** @brief Register ExecuteBtAction server with goal/execute handlers. */
void SetupExecuteBtServer();
/** @brief 将检测到的姿态转换到机械臂参考坐标系。 */
bool TransformPoseToArmFrame(
const geometry_msgs::msg::PoseStamped & source,
geometry_msgs::msg::PoseStamped & transformed) const;
// NOTE: Additional future helpers (cancellation handling, concurrency) may be added here.
};

1
src/brain/include/brain/skill_manager.hpp
View File

@@ -347,6 +347,7 @@ struct SkillActionTrait<interfaces::action::Arm>
static bool success(const interfaces::action::Arm::Result & r) {return r.result;}
static std::string message(const interfaces::action::Arm::Result & r)
{
(void)r;
return "r.message";
}
};

130
src/brain/src/cerebellum_node.cpp
View File

@@ -42,6 +42,11 @@
#include "interfaces/srv/map_save.hpp"
#include "interfaces/srv/map_load.hpp"
#include <std_msgs/msg/string.hpp>
#include <tf2/exceptions.h>
#include <tf2/time.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
namespace brain
{
@@ -58,6 +63,10 @@ namespace brain
CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
: Node("cerebellum_node", options)
{
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
tf_buffer_->setUsingDedicatedThread(true);
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
RCLCPP_INFO(this->get_logger(), "Cerebellum node started");
action_registry_ = std::make_unique<ActionServerRegistry>(this);
action_clients_ = std::make_unique<ActionClientRegistry>(this);
@@ -152,30 +161,54 @@ void CerebellumNode::ConfigureActionHooks()
SkillManager::ActionHookOptions<interfaces::action::Arm> arm_hooks;
arm_hooks.make_goal = [this](const std::string & skill_name) {
interfaces::action::Arm::Goal goal{};
const std::string param = "arm.reference_frame";
if (!this->has_parameter(param)) {
this->declare_parameter<std::string>(param, std::string("base_link"));
goal.body_id = 0; //LEFT_ARM=0, RIGHT_ARM=1
goal.data_type = 3; //ARM_COMMAND_TYPE_POSE_DIRECT_MOVE
goal.command_id = ++command_id_;
if (target_pose_.empty()) {
RCLCPP_WARN(this->get_logger(), "[%s] 未检测到目标物体，使用默认位置", skill_name.c_str());
// default position
goal.data_array = {-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
goal.frame_time_stamp = this->get_clock()->now().nanoseconds();
} else {
RCLCPP_INFO(this->get_logger(), "[%s] 使用检测到的目标物体位置", skill_name.c_str());
const auto pose_it = target_pose_.find(target_frame_);
if (pose_it == target_pose_.end()) {
RCLCPP_WARN(this->get_logger(), "[%s] 未找到目标 %s 的姿态缓存，使用默认坐标",
skill_name.c_str(), target_frame_.c_str());
goal.data_array = {-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
goal.frame_time_stamp = this->get_clock()->now().nanoseconds();
} else {
geometry_msgs::msg::PoseStamped pose_in_arm;
const bool transformed = TransformPoseToArmFrame(pose_it->second, pose_in_arm);
const auto & pose_ref = transformed ? pose_in_arm : pose_it->second;
if (!transformed) {
RCLCPP_WARN(this->get_logger(), "[%s] 坐标系转换失败，继续使用 %s 坐标系数据",
skill_name.c_str(), pose_ref.header.frame_id.c_str());
} else {
RCLCPP_INFO(this->get_logger(), "[%s] 目标姿态已转换到 %s 坐标系",
skill_name.c_str(), arm_target_frame_.c_str());
}
goal.data_array = {
pose_ref.pose.position.x,
pose_ref.pose.position.y,
pose_ref.pose.position.z,
pose_ref.pose.orientation.x,
pose_ref.pose.orientation.y,
pose_ref.pose.orientation.z,
pose_ref.pose.orientation.w
};
goal.frame_time_stamp = pose_ref.header.stamp.sec * 1000000000LL
+ pose_ref.header.stamp.nanosec;
}
target_pose_.clear();
}
const auto frame = this->get_parameter(param).as_string();
RCLCPP_INFO(this->get_logger(), "[%s] Arm 使用参考坐标系 %s",
skill_name.c_str(), frame.c_str());
(void)frame;
goal.data_length = static_cast<int32_t>(goal.data_array.size());
static int64_t command_id = 0;
//TODO :
goal.body_id = 1;
goal.data_type = 1;
goal.data_length = 3;
goal.command_id = command_id++;
rclcpp::Time now = this->get_clock()->now();
goal.frame_time_stamp = now.nanoseconds();
goal.data_array = {0.1, 0.2, 0.3};
RCLCPP_INFO(this->get_logger(), "[%s] Arm 目标参数 body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.1f, %.1f, %.1f]",
RCLCPP_INFO(this->get_logger(), "[%s] 目标参数 body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f]",
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp,
goal.data_array[0], goal.data_array[1], goal.data_array[2]);
goal.data_array[0], goal.data_array[1], goal.data_array[2], goal.data_array[3], goal.data_array[4], goal.data_array[5], goal.data_array[6]);
return goal;
};
@@ -184,12 +217,12 @@ void CerebellumNode::ConfigureActionHooks()
ActionClientRegistry::GoalHandle<interfaces::action::Arm>::SharedPtr,
const std::shared_ptr<const interfaces::action::Arm::Feedback> & feedback) {
if (!feedback) {
RCLCPP_WARN(this->get_logger(), "[%s] Arm feedback 为空", skill_name.c_str());
RCLCPP_WARN(this->get_logger(), "[%s] feedback 为空", skill_name.c_str());
return;
}
(void)feedback;
//TODO :
RCLCPP_INFO(this->get_logger(), "[%s] command_id: %d, current progress: %d",
RCLCPP_INFO(this->get_logger(), "[%s] command_id: %ld, current progress: %d",
skill_name.c_str(), feedback->command_id, feedback->int_lenth);
};
@@ -199,11 +232,11 @@ void CerebellumNode::ConfigureActionHooks()
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->result;
const std::string message = res.result ? std::string("action end") : std::string("无返回信息");
if (success) {
RCLCPP_INFO(this->get_logger(), "[%s] Arm 完成: %s", skill_name.c_str(), message.c_str());
RCLCPP_INFO(this->get_logger(), "[%s] 完成: %s", skill_name.c_str(), message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(this->get_logger(), "[%s] Arm 被取消", skill_name.c_str());
RCLCPP_WARN(this->get_logger(), "[%s] 被取消", skill_name.c_str());
} else {
RCLCPP_ERROR(this->get_logger(), "[%s] Arm 失败 (code=%d): %s",
RCLCPP_ERROR(this->get_logger(), "[%s] 失败 (code=%d): %s",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
@@ -511,6 +544,14 @@ void CerebellumNode::ConfigureServiceHooks()
stamp_sec, resp->info.c_str());
for (size_t i = 0; i < resp->objects.size(); ++i) {
const auto & obj = resp->objects[i];
//判断target_pose_的key是否等于obj.class_name，如果相等，将value存入target_pose_的PostStamped
if (target_frame_ == obj.class_name) {
target_pose_[target_frame_].header = resp->header;
target_pose_[target_frame_].pose = obj.pose_list[0];
RCLCPP_INFO(this->get_logger(), "target_frame_: %s, updated target_pose", target_frame_.c_str());
}
RCLCPP_INFO(
this->get_logger(), " [%zu] class='%s' id=%d poses=%zu", i,
obj.class_name.c_str(), obj.class_id, obj.pose_list.size());
@@ -622,6 +663,8 @@ void CerebellumNode::DeclareAndLoadParameters()
this->declare_parameter<double>("navigate_to_pose.yaw_offset", nav_goal_yaw_offset_);
this->declare_parameter<double>("navigate_to_pose.distance_tolerance", nav_goal_distance_tolerance_);
this->declare_parameter<bool>("slam_mode.enable_mapping_default", slam_enable_mapping_default_);
this->declare_parameter<std::string>("arm.target_pose_target_frame", arm_target_frame_);
this->declare_parameter<double>("arm.target_pose_transform_timeout_sec", arm_transform_timeout_sec_);
// Read back
this->get_parameter("abort_on_failure", abort_on_failure_);
this->get_parameter("default_action_timeout_sec", default_action_timeout_sec_);
@@ -635,6 +678,9 @@ void CerebellumNode::DeclareAndLoadParameters()
this->get_parameter("navigate_to_pose.yaw_offset", nav_goal_yaw_offset_);
this->get_parameter("navigate_to_pose.distance_tolerance", nav_goal_distance_tolerance_);
this->get_parameter("slam_mode.enable_mapping_default", slam_enable_mapping_default_);
this->get_parameter("arm.target_pose_target_frame", arm_target_frame_);
this->get_parameter("arm.target_pose_transform_timeout_sec", arm_transform_timeout_sec_);
arm_transform_timeout_sec_ = std::max(0.01, arm_transform_timeout_sec_);
map_save_free_thresh_ = std::clamp(map_save_free_thresh_, 0.0, 1.0);
map_save_occupied_thresh_ = std::clamp(map_save_occupied_thresh_, 0.0, 1.0);
nav_goal_distance_tolerance_ = std::max(0.0, nav_goal_distance_tolerance_);
@@ -779,6 +825,38 @@ void CerebellumNode::SetupExecuteBtServer()
action_registry_->register_action_server<EBA>(brain::kExecuteBtActionName, h);
}
bool CerebellumNode::TransformPoseToArmFrame(
const geometry_msgs::msg::PoseStamped & source,
geometry_msgs::msg::PoseStamped & transformed) const
{
if (!tf_buffer_) {
RCLCPP_ERROR(this->get_logger(), "TF buffer 未初始化，无法转换目标点位");
return false;
}
if (source.header.frame_id.empty()) {
RCLCPP_WARN(this->get_logger(), "目标姿态缺少来源坐标系，跳过转换");
return false;
}
if (source.header.frame_id == arm_target_frame_) {
RCLCPP_INFO(this->get_logger(), "目标姿态已位于 %s 坐标系，跳过转换", arm_target_frame_.c_str());
transformed = source;
return true;
}
try {
transformed = tf_buffer_->transform(
source, arm_target_frame_, tf2::durationFromSec(arm_transform_timeout_sec_));
RCLCPP_INFO(
this->get_logger(), "目标姿态从 %s 转换至 %s",
source.header.frame_id.c_str(), arm_target_frame_.c_str());
return true;
} catch (const tf2::TransformException & ex) {
RCLCPP_ERROR(
this->get_logger(), "转换目标姿态失败: %s -> %s, 原因: %s",
source.header.frame_id.c_str(), arm_target_frame_.c_str(), ex.what());
}
return false;
}
/**
* @brief Parse a raw delimited skill list string into individual skill tokens.
*

16
src/brain/src/cerebrum_node.cpp
View File

@@ -159,6 +159,8 @@ void CerebrumNode::RegisterActionClient()
}
}
g.action_name = oss.str();
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction]Sending goal epoch=%u skills=%s",
g.epoch, g.action_name.c_str());
return g;
},
// Feedback
@@ -237,6 +239,11 @@ void CerebrumNode::RegisterActionClient()
info.awaiting_result = false;
info.result = ok; // authoritative final
info.result_code = ok ? ExecResultCode::SUCCESS : ExecResultCode::FAILURE;
RCLCPP_INFO(
this->get_logger(),
"[ExecuteBtAction][result] Finalizing node %s result=%s",
kv.first.c_str(),
info.result ? "SUCCESS" : "FAILURE");
}
}
});
@@ -431,6 +438,7 @@ void CerebrumNode::RegisterSkillBtActions()
return BT::NodeStatus::FAILURE;
}
action_registry_->send(brain::kExecuteBtActionName, this->get_logger());
RCLCPP_INFO(this->get_logger(), "[%s] BTAction on_start", bt_type.c_str());
// Clear override after send to avoid affecting other triggers.
single_skill_goal_override_.reset();
return BT::NodeStatus::RUNNING;
@@ -445,7 +453,9 @@ void CerebrumNode::RegisterSkillBtActions()
info.in_flight = false;
info.result = false;
info.result_code = ExecResultCode::FAILURE;
return BT::NodeStatus::FAILURE;
RCLCPP_WARN(this->get_logger(), "[%s] BTAction on_running timeout after %.2f sec", bt_type.c_str(), elapsed);
action_registry_->cancel(brain::kExecuteBtActionName, this->get_logger());
// return BT::NodeStatus::FAILURE;
}
return BT::NodeStatus::RUNNING;
}
@@ -462,10 +472,10 @@ void CerebrumNode::RegisterSkillBtActions()
return info.result ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
};
handlers.on_halted = [this, bt_type](rclcpp::Node *, BT::TreeNode &) {
RCLCPP_INFO(this->get_logger(), "Node halted %s", bt_type.c_str());
RCLCPP_INFO(this->get_logger(), "[%s] BTAction on_halted", bt_type.c_str());
};
try {RegisterBtAction(bt_type, handlers);} catch (const BT::BehaviorTreeException & e) {
RCLCPP_INFO(this->get_logger(), "BT type exists %s: %s", bt_type.c_str(), e.what());
RCLCPP_WARN(this->get_logger(), "BT type exists %s: %s", bt_type.c_str(), e.what());
}
}
}

1
src/brain/src/skill_manager.cpp
View File

@@ -167,6 +167,7 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
register_action_client_default<ArmSpaceControl>("ArmSpaceControl", topic, internal_skill);
}},
{"Arm", [this](const std::string & topic, const std::string & internal_skill) {
(void)topic;
register_action_client_default<Arm>("Arm", "ArmAction", internal_skill);
}},
{"HandControl", [this](const std::string & topic, const std::string & internal_skill) {

34
src/modules/vision/vision_object_recg/src/vision_object_recg_node.cpp
View File

@@ -34,7 +34,9 @@ private:
// 填充 header
res->header.stamp = now();
// 若调用方传 camera_position 作为一个逻辑位姿来源，可作为 frame_id；否则给一个默认
res->header.frame_id = req->camera_position.empty() ? "camera_link" : ("camera_" + req->camera_position);
// res->header.frame_id = req->camera_position.empty() ? "camera_link" : ("camera_" + req->camera_position);
res->header.frame_id = "base_link_camera";
// 简单逻辑: 只要 camera_position 非空且不是 unknown 就认为识别到两个目标
if (!req->camera_position.empty() && req->camera_position != "unknown") {
@@ -42,17 +44,29 @@ private:
// 伪造两类目标 (示例: cup / bottle)，每类给出 1~2 个 Pose
interfaces::msg::PoseClassAndID obj1;
obj1.class_name = "cup";
obj1.class_name = "bottle";
obj1.class_id = 1;
//{-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
geometry_msgs::msg::Pose pose1; // 基本位姿
pose1.position.x = 0.3;
pose1.position.y = 0.1;
pose1.position.z = 0.8;
pose1.orientation.x = 0.0;
pose1.orientation.y = 0.0;
pose1.orientation.z = 0.0;
pose1.orientation.w = 1.0;
// pose1.position.x = 0.9758;
// pose1.position.y = 0;
// pose1.position.z = 0.42;
// pose1.orientation.x = -0.5;
// pose1.orientation.y = 0.5;
// pose1.orientation.z = 0.5;
// pose1.orientation.w = -0.5;
//geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=-0.2851924786746129, y=-0.056353812689333635, z=1.073772448259523),
//orientation=geometry_msgs.msg.Quaternion(x=-0.16713377464857188, y=-0.42460237568763715, z=-0.26706232441180955, w=0.8487972895879773))
pose1.position.x = -0.2851924786746129;
pose1.position.y = -0.056353812689333635;
pose1.position.z = 1.073772448259523;
pose1.orientation.x = -0.16713377464857188;
pose1.orientation.y = -0.42460237568763715;
pose1.orientation.z = -0.26706232441180955;
pose1.orientation.w = 0.8487972895879773;
obj1.pose_list.push_back(pose1);
// 第二个同类实例(示例)
@@ -61,7 +75,7 @@ private:
obj1.pose_list.push_back(pose1b);
interfaces::msg::PoseClassAndID obj2;
obj2.class_name = "bottle";
obj2.class_name = "cup";
obj2.class_id = 2;
geometry_msgs::msg::Pose pose2;
pose2.position.x = 0.5;