From 189713c7d997714dde4ce219dc26cbf0992cd1e7 Mon Sep 17 00:00:00 2001 From: Ar-Ray-code Date: Sun, 25 May 2025 18:30:24 +0900 Subject: [PATCH 1/5] init packages --- minecraft_utils/CMakeLists.txt | 6 ++++++ minecraft_utils/package.xml | 20 +++++++++++++++++++ minecraft_utils_visualization/CMakeLists.txt | 12 +++++++++++ minecraft_utils_visualization/package.xml | 21 ++++++++++++++++++++ 4 files changed, 59 insertions(+) create mode 100644 minecraft_utils/CMakeLists.txt create mode 100644 minecraft_utils/package.xml create mode 100644 minecraft_utils_visualization/CMakeLists.txt create mode 100644 minecraft_utils_visualization/package.xml diff --git a/minecraft_utils/CMakeLists.txt b/minecraft_utils/CMakeLists.txt new file mode 100644 index 0000000..3e34fe5 --- /dev/null +++ b/minecraft_utils/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 3.5) +project(minecraft_utils) + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_package() \ No newline at end of file diff --git a/minecraft_utils/package.xml b/minecraft_utils/package.xml new file mode 100644 index 0000000..7b2816a --- /dev/null +++ b/minecraft_utils/package.xml @@ -0,0 +1,20 @@ + + + + minecraft_utils + 0.0.1 + The minecraft_utils package + Ar-Ray-code + Apache License 2.0 + + ament_cmake_auto + + minecraft_utils_visualization + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/minecraft_utils_visualization/CMakeLists.txt b/minecraft_utils_visualization/CMakeLists.txt new file mode 100644 index 0000000..ed2d4c9 --- /dev/null +++ b/minecraft_utils_visualization/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.5) +project(minecraft_utils_visualization) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() \ No newline at end of file diff --git a/minecraft_utils_visualization/package.xml b/minecraft_utils_visualization/package.xml new file mode 100644 index 0000000..d44c348 --- /dev/null +++ b/minecraft_utils_visualization/package.xml @@ -0,0 +1,21 @@ + + + + minecraft_utils_visualization + 0.0.1 + The minecraft_utils_visualization package + Ar-Ray-code + Apache License 2.0 + + ament_cmake_auto + + minecraft_msgs + visualization_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From 3fbb17e28cbea71ecb4ffbd6df505567b52d71c9 Mon Sep 17 00:00:00 2001 From: Ar-Ray-code Date: Sun, 25 May 2025 23:39:22 +0900 Subject: [PATCH 2/5] Add MobMarker class for visualizing mobs in the game --- minecraft_utils_visualization/CMakeLists.txt | 11 + .../mob_marker.hpp | 70 +++++ minecraft_utils_visualization/package.xml | 4 + .../src/mob_marker.cpp | 246 ++++++++++++++++++ 4 files changed, 331 insertions(+) create mode 100644 minecraft_utils_visualization/include/minecraft_utils_visualization/mob_marker.hpp create mode 100644 minecraft_utils_visualization/src/mob_marker.cpp diff --git a/minecraft_utils_visualization/CMakeLists.txt b/minecraft_utils_visualization/CMakeLists.txt index ed2d4c9..11aa415 100644 --- a/minecraft_utils_visualization/CMakeLists.txt +++ b/minecraft_utils_visualization/CMakeLists.txt @@ -4,6 +4,17 @@ project(minecraft_utils_visualization) find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() +set(TARGET mob_marker) +ament_auto_add_library(${TARGET} SHARED ./src/${TARGET}.cpp) +rclcpp_components_register_node( + ${TARGET} + PLUGIN "minecraft_utils_visualization::MobMarker" + EXECUTABLE ${TARGET}_exec) +target_include_directories( + ${TARGET} PRIVATE + $ + $) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/minecraft_utils_visualization/include/minecraft_utils_visualization/mob_marker.hpp b/minecraft_utils_visualization/include/minecraft_utils_visualization/mob_marker.hpp new file mode 100644 index 0000000..abe5e23 --- /dev/null +++ b/minecraft_utils_visualization/include/minecraft_utils_visualization/mob_marker.hpp @@ -0,0 +1,70 @@ +// Copyright 2025 minecraft-ros2 +// +// 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 +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace minecraft_utils_visualization +{ +class MobMarker : public rclcpp::Node +{ +public: + explicit MobMarker(rclcpp::NodeOptions); + ~MobMarker() = default; + +private: + void mobCallback(const minecraft_msgs::msg::LivingEntityArray::SharedPtr); + + void setMarkerColor(const uint8_t, visualization_msgs::msg::Marker &); + + void addNameHealthMarker( + const minecraft_msgs::msg::LivingEntity &, + const std_msgs::msg::Header &, + const float, const float, const float, + const geometry_msgs::msg::Quaternion &, + visualization_msgs::msg::MarkerArray &); + + tf2::Vector3 transformToPlayerFrame( + const geometry_msgs::msg::Quaternion& player_orientation, + const float, const float, const float); + + rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Subscription::SharedPtr mob_sub_; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + std::string world_frame_id_; + std::string player_frame_id_; + std::string mob_namespace_; + std::string mob_text_namespace_; +}; + +} // namespace minecraft_utils_visualization diff --git a/minecraft_utils_visualization/package.xml b/minecraft_utils_visualization/package.xml index d44c348..13c5c27 100644 --- a/minecraft_utils_visualization/package.xml +++ b/minecraft_utils_visualization/package.xml @@ -10,6 +10,10 @@ ament_cmake_auto minecraft_msgs + rclcpp + rclcpp_components + std_msgs + tf2_ros visualization_msgs ament_lint_auto diff --git a/minecraft_utils_visualization/src/mob_marker.cpp b/minecraft_utils_visualization/src/mob_marker.cpp new file mode 100644 index 0000000..30099c9 --- /dev/null +++ b/minecraft_utils_visualization/src/mob_marker.cpp @@ -0,0 +1,246 @@ +// Copyright 2025 minecraft-ros2 +// +// 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 "minecraft_utils_visualization/mob_marker.hpp" + +namespace minecraft_utils_visualization +{ + +MobMarker::MobMarker(rclcpp::NodeOptions options) +: rclcpp::Node("mob_marker", options) +{ + this->declare_parameter("world_frame_id", "world"); + this->declare_parameter("player_frame_id", "player"); + this->declare_parameter("mob_namespace", "mob"); + this->declare_parameter("mob_text_namespace", "mob_text"); + + world_frame_id_ = this->get_parameter("world_frame_id").as_string(); + player_frame_id_ = this->get_parameter("player_frame_id").as_string(); + mob_namespace_ = this->get_parameter("mob_namespace").as_string(); + mob_text_namespace_ = this->get_parameter("mob_text_namespace").as_string(); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).best_effort(); + + marker_pub_ = this->create_publisher("mob_markers", 10); + mob_sub_ = this->create_subscription( + "player/nearby_living_entities", qos, std::bind(&MobMarker::mobCallback, this, std::placeholders::_1)); +} + +void MobMarker::mobCallback(const minecraft_msgs::msg::LivingEntityArray::SharedPtr msg) +{ + visualization_msgs::msg::MarkerArray marker_array; + + visualization_msgs::msg::Marker delete_marker; + delete_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array.markers.push_back(delete_marker); + + geometry_msgs::msg::TransformStamped transform_stamped; + try { + transform_stamped = tf_buffer_->lookupTransform( + world_frame_id_, player_frame_id_, tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + return; + } + + float player_x = transform_stamped.transform.translation.x; + float player_y = transform_stamped.transform.translation.y; + float player_z = transform_stamped.transform.translation.z; + + auto player_orientation = transform_stamped.transform.rotation; + + RCLCPP_DEBUG(this->get_logger(), "Player position: x=%f, y=%f, z=%f", + player_x, player_y, player_z); + + for (size_t i = 0; i < msg->entities.size(); ++i) { + const auto & entity = msg->entities[i]; + + visualization_msgs::msg::Marker marker; + marker.header = msg->header; + + marker.header.frame_id = player_frame_id_; + marker.ns = mob_namespace_; + marker.id = static_cast(entity.id); + marker.lifetime = rclcpp::Duration(1, 0); + + marker.type = visualization_msgs::msg::Marker::CUBE; + this->setMarkerColor(entity.category.mob_category, marker); + + marker.scale.x = entity.hit_box.x; + marker.scale.y = entity.hit_box.y; + marker.scale.z = entity.hit_box.z; + + marker.pose = entity.pose; + + auto transformed_position = transformToPlayerFrame( + player_orientation, + entity.pose.position.x - player_x, + entity.pose.position.y - player_y, + entity.pose.position.z - player_z + ); + + marker.pose.position.x = transformed_position.getX(); + marker.pose.position.y = transformed_position.getY(); + marker.pose.position.z = transformed_position.getZ(); + + RCLCPP_DEBUG(this->get_logger(), "Mob %d position (player relative): x=%f, y=%f, z=%f", + entity.id, marker.pose.position.x, marker.pose.position.y, marker.pose.position.z); + + marker_array.markers.push_back(marker); + + this->addNameHealthMarker(entity, marker.header, player_x, player_y, player_z, player_orientation, marker_array); + } + + marker_pub_->publish(marker_array); +} + +void MobMarker::setMarkerColor( + const uint8_t mob_category, + visualization_msgs::msg::Marker & marker) +{ + switch (mob_category) { + case minecraft_msgs::msg::MobCategory::MONSTER: + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + marker.color.a = 0.8; + break; + + case minecraft_msgs::msg::MobCategory::CREATURE: + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.color.a = 0.8; + break; + + case minecraft_msgs::msg::MobCategory::AMBIENT: + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + marker.color.a = 0.8; + break; + + case minecraft_msgs::msg::MobCategory::AXOLOTLS: + marker.color.r = 1.0; + marker.color.g = 0.4; + marker.color.b = 0.7; + marker.color.a = 0.8; + break; + + case minecraft_msgs::msg::MobCategory::UNDERGROUND_WATER_CREATURE: + marker.color.r = 0.6; + marker.color.g = 0.3; + marker.color.b = 0.1; + marker.color.a = 0.8; + break; + + case minecraft_msgs::msg::MobCategory::WATER_CREATURE: + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.color.a = 0.8; + break; + + case minecraft_msgs::msg::MobCategory::WATER_AMBIENT: + marker.color.r = 0.5; + marker.color.g = 0.5; + marker.color.b = 1.0; + marker.color.a = 0.8; + break; + + case minecraft_msgs::msg::MobCategory::MISC: + default: + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.color.a = 0.8; + break; + } +} + +void MobMarker::addNameHealthMarker( + const minecraft_msgs::msg::LivingEntity & entity, + const std_msgs::msg::Header & header, + const float player_x, + const float player_y, + const float player_z, + const geometry_msgs::msg::Quaternion & player_orientation, + visualization_msgs::msg::MarkerArray & marker_array) +{ + visualization_msgs::msg::Marker text_marker; + text_marker.header = header; + text_marker.header.frame_id = player_frame_id_; + text_marker.ns = mob_text_namespace_; + text_marker.id = static_cast(entity.id); + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.action = visualization_msgs::msg::Marker::ADD; + + text_marker.pose = entity.pose; + + auto transformed_position = this->transformToPlayerFrame( + player_orientation, + entity.pose.position.x - player_x, + entity.pose.position.y - player_y, + entity.pose.position.z - player_z + ); + + text_marker.pose.position.x = transformed_position.getX(); + text_marker.pose.position.y = transformed_position.getY(); + text_marker.pose.position.z = transformed_position.getZ(); + text_marker.pose.position.z += entity.hit_box.z / 2.0 + 0.5; + + text_marker.scale.z = 0.3; + + text_marker.color.r = 1.0; + text_marker.color.g = 1.0; + text_marker.color.b = 1.0; + text_marker.color.a = 1.0; + + std::stringstream ss; + ss << entity.name << " (" + << static_cast(entity.health) << "/" + << static_cast(entity.max_health) << ")"; + text_marker.text = ss.str(); + + text_marker.lifetime = rclcpp::Duration(1, 0); + + marker_array.markers.push_back(text_marker); +} + +tf2::Vector3 MobMarker::transformToPlayerFrame( + const geometry_msgs::msg::Quaternion& player_orientation, + const float x, const float y, const float z) +{ + tf2::Quaternion tf_quat; + tf_quat.setValue( + player_orientation.x, + player_orientation.y, + player_orientation.z, + player_orientation.w + ); + + tf2::Matrix3x3 rot_matrix(tf_quat.inverse()); + tf2::Vector3 point(x, y, z); + tf2::Vector3 transformed_point = rot_matrix * point; + + return transformed_point; +} + +} // namespace minecraft_utils_visualization + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(minecraft_utils_visualization::MobMarker) From b659b70a26e53d0b8de0380017eb7823b97c3341 Mon Sep 17 00:00:00 2001 From: Ar-Ray-code Date: Sun, 25 May 2025 23:42:20 +0900 Subject: [PATCH 3/5] Add CI workflow for 'humble' branch to automate build and test processes --- .github/workflows/ci_humble.yaml | 35 ++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 .github/workflows/ci_humble.yaml diff --git a/.github/workflows/ci_humble.yaml b/.github/workflows/ci_humble.yaml new file mode 100644 index 0000000..ec769e4 --- /dev/null +++ b/.github/workflows/ci_humble.yaml @@ -0,0 +1,35 @@ +name: ci_humble + +on: + push: + branches: + - "humble" + pull_request: + types: [opened, synchronize, labeled] + +jobs: + ci: + runs-on: ${{ matrix.os }} + if: | + ((github.event.action == 'labeled') && (github.event.label.name == 'TESTING') && (github.base_ref == 'humble' )) || + ((github.event.action == 'synchronize') && (github.base_ref == 'humble') && contains(github.event.pull_request.labels.*.name, 'TESTING')) || + (github.ref_name == 'humble') + container: + image: osrf/ros:${{ matrix.ros_distribution }}-desktop + timeout-minutes: 20 + strategy: + fail-fast: false + matrix: + os: [ubuntu-22.04] + ros_distribution: [humble] + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.7 + - name: Build and Test + uses: ros-tooling/action-ros-ci@v0.3 + with: + target-ros2-distro: ${{ matrix.ros_distribution }} + import-token: ${{ secrets.GITHUB_TOKEN }} + package-name: | + minecraft_utils + minecraft_utils_visualization \ No newline at end of file From 5addd15a58a994f501bc8a247a56225272d43b10 Mon Sep 17 00:00:00 2001 From: Ar-Ray-code Date: Sun, 25 May 2025 23:52:08 +0900 Subject: [PATCH 4/5] Add build dependencies configuration for Minecraft ROS2 integration --- .github/workflows/ci_humble.yaml | 3 ++- build_depends.repos | 5 +++++ 2 files changed, 7 insertions(+), 1 deletion(-) create mode 100644 build_depends.repos diff --git a/.github/workflows/ci_humble.yaml b/.github/workflows/ci_humble.yaml index ec769e4..099ef7b 100644 --- a/.github/workflows/ci_humble.yaml +++ b/.github/workflows/ci_humble.yaml @@ -32,4 +32,5 @@ jobs: import-token: ${{ secrets.GITHUB_TOKEN }} package-name: | minecraft_utils - minecraft_utils_visualization \ No newline at end of file + minecraft_utils_visualization + vcs-repo-file-url: build_depends.repos \ No newline at end of file diff --git a/build_depends.repos b/build_depends.repos new file mode 100644 index 0000000..77aa7eb --- /dev/null +++ b/build_depends.repos @@ -0,0 +1,5 @@ +repositories: + minecraft-ros2/minecraft_msgs: + type: git + url: https://github.com/minecraft-ros2/minecraft_msgs.git + version: humble From 419a6895b7095069374cf9f8f3e7a72a52d80d63 Mon Sep 17 00:00:00 2001 From: Ar-Ray-code Date: Mon, 26 May 2025 00:37:04 +0900 Subject: [PATCH 5/5] fix colcon-test error --- .../mob_marker.hpp | 68 +-- .../src/mob_marker.cpp | 401 +++++++++--------- 2 files changed, 236 insertions(+), 233 deletions(-) diff --git a/minecraft_utils_visualization/include/minecraft_utils_visualization/mob_marker.hpp b/minecraft_utils_visualization/include/minecraft_utils_visualization/mob_marker.hpp index abe5e23..60b15e3 100644 --- a/minecraft_utils_visualization/include/minecraft_utils_visualization/mob_marker.hpp +++ b/minecraft_utils_visualization/include/minecraft_utils_visualization/mob_marker.hpp @@ -12,9 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifndef MINECRAFT_UTILS_VISUALIZATION__MOB_MARKER_HPP_ +#define MINECRAFT_UTILS_VISUALIZATION__MOB_MARKER_HPP_ + +#include +#include +#include +#include +#include + #include #include +#include #include +#include #include #include @@ -22,13 +33,8 @@ #include #include #include -#include -#include #include #include -#include -#include -#include namespace minecraft_utils_visualization @@ -36,35 +42,37 @@ namespace minecraft_utils_visualization class MobMarker : public rclcpp::Node { public: - explicit MobMarker(rclcpp::NodeOptions); - ~MobMarker() = default; + explicit MobMarker(rclcpp::NodeOptions); + ~MobMarker() = default; private: - void mobCallback(const minecraft_msgs::msg::LivingEntityArray::SharedPtr); - - void setMarkerColor(const uint8_t, visualization_msgs::msg::Marker &); - - void addNameHealthMarker( - const minecraft_msgs::msg::LivingEntity &, - const std_msgs::msg::Header &, - const float, const float, const float, - const geometry_msgs::msg::Quaternion &, - visualization_msgs::msg::MarkerArray &); - - tf2::Vector3 transformToPlayerFrame( - const geometry_msgs::msg::Quaternion& player_orientation, - const float, const float, const float); + void mobCallback(const minecraft_msgs::msg::LivingEntityArray::SharedPtr); + + void setMarkerColor(const uint8_t, visualization_msgs::msg::Marker &); - rclcpp::Publisher::SharedPtr marker_pub_; - rclcpp::Subscription::SharedPtr mob_sub_; + void addNameHealthMarker( + const minecraft_msgs::msg::LivingEntity &, + const std_msgs::msg::Header &, + const float, const float, const float, + const geometry_msgs::msg::Quaternion &, + visualization_msgs::msg::MarkerArray &); - std::unique_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - std::string world_frame_id_; - std::string player_frame_id_; - std::string mob_namespace_; - std::string mob_text_namespace_; + tf2::Vector3 transformToPlayerFrame( + const geometry_msgs::msg::Quaternion & player_orientation, + const float, const float, const float); + + rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Subscription::SharedPtr mob_sub_; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + std::string world_frame_id_; + std::string player_frame_id_; + std::string mob_namespace_; + std::string mob_text_namespace_; }; } // namespace minecraft_utils_visualization + +#endif // MINECRAFT_UTILS_VISUALIZATION__MOB_MARKER_HPP_ diff --git a/minecraft_utils_visualization/src/mob_marker.cpp b/minecraft_utils_visualization/src/mob_marker.cpp index 30099c9..b97c65c 100644 --- a/minecraft_utils_visualization/src/mob_marker.cpp +++ b/minecraft_utils_visualization/src/mob_marker.cpp @@ -20,227 +20,222 @@ namespace minecraft_utils_visualization MobMarker::MobMarker(rclcpp::NodeOptions options) : rclcpp::Node("mob_marker", options) { - this->declare_parameter("world_frame_id", "world"); - this->declare_parameter("player_frame_id", "player"); - this->declare_parameter("mob_namespace", "mob"); - this->declare_parameter("mob_text_namespace", "mob_text"); - - world_frame_id_ = this->get_parameter("world_frame_id").as_string(); - player_frame_id_ = this->get_parameter("player_frame_id").as_string(); - mob_namespace_ = this->get_parameter("mob_namespace").as_string(); - mob_text_namespace_ = this->get_parameter("mob_text_namespace").as_string(); - - tf_buffer_ = std::make_unique(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).best_effort(); - - marker_pub_ = this->create_publisher("mob_markers", 10); - mob_sub_ = this->create_subscription( - "player/nearby_living_entities", qos, std::bind(&MobMarker::mobCallback, this, std::placeholders::_1)); + this->declare_parameter("world_frame_id", "world"); + this->declare_parameter("player_frame_id", "player"); + this->declare_parameter("mob_namespace", "mob"); + this->declare_parameter("mob_text_namespace", "mob_text"); + + world_frame_id_ = this->get_parameter("world_frame_id").as_string(); + player_frame_id_ = this->get_parameter("player_frame_id").as_string(); + mob_namespace_ = this->get_parameter("mob_namespace").as_string(); + mob_text_namespace_ = this->get_parameter("mob_text_namespace").as_string(); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).best_effort(); + + marker_pub_ = this->create_publisher("mob_markers", 10); + mob_sub_ = this->create_subscription( + "player/nearby_living_entities", qos, + std::bind(&MobMarker::mobCallback, this, std::placeholders::_1)); } + void MobMarker::mobCallback(const minecraft_msgs::msg::LivingEntityArray::SharedPtr msg) { - visualization_msgs::msg::MarkerArray marker_array; - - visualization_msgs::msg::Marker delete_marker; - delete_marker.action = visualization_msgs::msg::Marker::DELETEALL; - marker_array.markers.push_back(delete_marker); - - geometry_msgs::msg::TransformStamped transform_stamped; - try { - transform_stamped = tf_buffer_->lookupTransform( - world_frame_id_, player_frame_id_, tf2::TimePointZero); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "%s", ex.what()); - return; - } - - float player_x = transform_stamped.transform.translation.x; - float player_y = transform_stamped.transform.translation.y; - float player_z = transform_stamped.transform.translation.z; - - auto player_orientation = transform_stamped.transform.rotation; - - RCLCPP_DEBUG(this->get_logger(), "Player position: x=%f, y=%f, z=%f", - player_x, player_y, player_z); - - for (size_t i = 0; i < msg->entities.size(); ++i) { - const auto & entity = msg->entities[i]; - - visualization_msgs::msg::Marker marker; - marker.header = msg->header; - - marker.header.frame_id = player_frame_id_; - marker.ns = mob_namespace_; - marker.id = static_cast(entity.id); - marker.lifetime = rclcpp::Duration(1, 0); - - marker.type = visualization_msgs::msg::Marker::CUBE; - this->setMarkerColor(entity.category.mob_category, marker); - - marker.scale.x = entity.hit_box.x; - marker.scale.y = entity.hit_box.y; - marker.scale.z = entity.hit_box.z; - - marker.pose = entity.pose; - - auto transformed_position = transformToPlayerFrame( - player_orientation, - entity.pose.position.x - player_x, - entity.pose.position.y - player_y, - entity.pose.position.z - player_z - ); - - marker.pose.position.x = transformed_position.getX(); - marker.pose.position.y = transformed_position.getY(); - marker.pose.position.z = transformed_position.getZ(); - - RCLCPP_DEBUG(this->get_logger(), "Mob %d position (player relative): x=%f, y=%f, z=%f", - entity.id, marker.pose.position.x, marker.pose.position.y, marker.pose.position.z); - - marker_array.markers.push_back(marker); - - this->addNameHealthMarker(entity, marker.header, player_x, player_y, player_z, player_orientation, marker_array); - } - - marker_pub_->publish(marker_array); + visualization_msgs::msg::MarkerArray marker_array; + + visualization_msgs::msg::Marker delete_marker; + delete_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array.markers.push_back(delete_marker); + + geometry_msgs::msg::TransformStamped transform_stamped; + try { + transform_stamped = tf_buffer_->lookupTransform( + world_frame_id_, player_frame_id_, tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + return; + } + + float player_x = transform_stamped.transform.translation.x; + float player_y = transform_stamped.transform.translation.y; + float player_z = transform_stamped.transform.translation.z; + + auto player_orientation = transform_stamped.transform.rotation; + + for (size_t i = 0; i < msg->entities.size(); ++i) { + const auto & entity = msg->entities[i]; + + visualization_msgs::msg::Marker marker; + marker.header = msg->header; + + marker.header.frame_id = player_frame_id_; + marker.ns = mob_namespace_; + marker.id = static_cast(entity.id); + marker.lifetime = rclcpp::Duration(1, 0); + + marker.type = visualization_msgs::msg::Marker::CUBE; + this->setMarkerColor(entity.category.mob_category, marker); + + marker.scale.x = entity.hit_box.x; + marker.scale.y = entity.hit_box.y; + marker.scale.z = entity.hit_box.z; + + marker.pose = entity.pose; + + auto transformed_position = transformToPlayerFrame( + player_orientation, + entity.pose.position.x - player_x, + entity.pose.position.y - player_y, + entity.pose.position.z - player_z + ); + + marker.pose.position.x = transformed_position.getX(); + marker.pose.position.y = transformed_position.getY(); + marker.pose.position.z = transformed_position.getZ(); + + marker_array.markers.push_back(marker); + + this->addNameHealthMarker( + entity, marker.header, player_x, player_y, player_z, + player_orientation, marker_array); + } + + marker_pub_->publish(marker_array); } - + + void MobMarker::setMarkerColor( - const uint8_t mob_category, - visualization_msgs::msg::Marker & marker) + const uint8_t mob_category, + visualization_msgs::msg::Marker & marker) { - switch (mob_category) { - case minecraft_msgs::msg::MobCategory::MONSTER: - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 0.0; - marker.color.a = 0.8; - break; - - case minecraft_msgs::msg::MobCategory::CREATURE: - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - marker.color.a = 0.8; - break; - - case minecraft_msgs::msg::MobCategory::AMBIENT: - marker.color.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - marker.color.a = 0.8; - break; - - case minecraft_msgs::msg::MobCategory::AXOLOTLS: - marker.color.r = 1.0; - marker.color.g = 0.4; - marker.color.b = 0.7; - marker.color.a = 0.8; - break; - - case minecraft_msgs::msg::MobCategory::UNDERGROUND_WATER_CREATURE: - marker.color.r = 0.6; - marker.color.g = 0.3; - marker.color.b = 0.1; - marker.color.a = 0.8; - break; - - case minecraft_msgs::msg::MobCategory::WATER_CREATURE: - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 1.0; - marker.color.a = 0.8; - break; - - case minecraft_msgs::msg::MobCategory::WATER_AMBIENT: - marker.color.r = 0.5; - marker.color.g = 0.5; - marker.color.b = 1.0; - marker.color.a = 0.8; - break; - - case minecraft_msgs::msg::MobCategory::MISC: - default: - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 1.0; - marker.color.a = 0.8; - break; - } + switch (mob_category) { + case minecraft_msgs::msg::MobCategory::MONSTER: + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + marker.color.a = 0.8; + break; + case minecraft_msgs::msg::MobCategory::CREATURE: + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.color.a = 0.8; + break; + case minecraft_msgs::msg::MobCategory::AMBIENT: + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + marker.color.a = 0.8; + break; + case minecraft_msgs::msg::MobCategory::AXOLOTLS: + marker.color.r = 1.0; + marker.color.g = 0.4; + marker.color.b = 0.7; + marker.color.a = 0.8; + break; + case minecraft_msgs::msg::MobCategory::UNDERGROUND_WATER_CREATURE: + marker.color.r = 0.6; + marker.color.g = 0.3; + marker.color.b = 0.1; + marker.color.a = 0.8; + break; + case minecraft_msgs::msg::MobCategory::WATER_CREATURE: + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.color.a = 0.8; + break; + case minecraft_msgs::msg::MobCategory::WATER_AMBIENT: + marker.color.r = 0.5; + marker.color.g = 0.5; + marker.color.b = 1.0; + marker.color.a = 0.8; + break; + case minecraft_msgs::msg::MobCategory::MISC: + default: + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.color.a = 0.8; + break; + } } + void MobMarker::addNameHealthMarker( - const minecraft_msgs::msg::LivingEntity & entity, - const std_msgs::msg::Header & header, - const float player_x, - const float player_y, - const float player_z, - const geometry_msgs::msg::Quaternion & player_orientation, - visualization_msgs::msg::MarkerArray & marker_array) + const minecraft_msgs::msg::LivingEntity & entity, + const std_msgs::msg::Header & header, + const float player_x, + const float player_y, + const float player_z, + const geometry_msgs::msg::Quaternion & player_orientation, + visualization_msgs::msg::MarkerArray & marker_array) { - visualization_msgs::msg::Marker text_marker; - text_marker.header = header; - text_marker.header.frame_id = player_frame_id_; - text_marker.ns = mob_text_namespace_; - text_marker.id = static_cast(entity.id); - text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - text_marker.action = visualization_msgs::msg::Marker::ADD; - - text_marker.pose = entity.pose; - - auto transformed_position = this->transformToPlayerFrame( - player_orientation, - entity.pose.position.x - player_x, - entity.pose.position.y - player_y, - entity.pose.position.z - player_z - ); - - text_marker.pose.position.x = transformed_position.getX(); - text_marker.pose.position.y = transformed_position.getY(); - text_marker.pose.position.z = transformed_position.getZ(); - text_marker.pose.position.z += entity.hit_box.z / 2.0 + 0.5; - - text_marker.scale.z = 0.3; - - text_marker.color.r = 1.0; - text_marker.color.g = 1.0; - text_marker.color.b = 1.0; - text_marker.color.a = 1.0; - - std::stringstream ss; - ss << entity.name << " (" - << static_cast(entity.health) << "/" - << static_cast(entity.max_health) << ")"; - text_marker.text = ss.str(); - - text_marker.lifetime = rclcpp::Duration(1, 0); - - marker_array.markers.push_back(text_marker); + visualization_msgs::msg::Marker text_marker; + text_marker.header = header; + text_marker.header.frame_id = player_frame_id_; + text_marker.ns = mob_text_namespace_; + text_marker.id = static_cast(entity.id); + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.action = visualization_msgs::msg::Marker::ADD; + + text_marker.pose = entity.pose; + + auto transformed_position = this->transformToPlayerFrame( + player_orientation, + entity.pose.position.x - player_x, + entity.pose.position.y - player_y, + entity.pose.position.z - player_z + ); + + text_marker.pose.position.x = transformed_position.getX(); + text_marker.pose.position.y = transformed_position.getY(); + text_marker.pose.position.z = transformed_position.getZ(); + text_marker.pose.position.z += entity.hit_box.z / 2.0 + 0.5; + + text_marker.scale.z = 0.3; + + text_marker.color.r = 1.0; + text_marker.color.g = 1.0; + text_marker.color.b = 1.0; + text_marker.color.a = 1.0; + + std::stringstream ss; + ss << entity.name << " (" + << static_cast(entity.health) << "/" + << static_cast(entity.max_health) << ")"; + text_marker.text = ss.str(); + + text_marker.lifetime = rclcpp::Duration(1, 0); + + marker_array.markers.push_back(text_marker); } + tf2::Vector3 MobMarker::transformToPlayerFrame( - const geometry_msgs::msg::Quaternion& player_orientation, - const float x, const float y, const float z) + const geometry_msgs::msg::Quaternion & player_orientation, + const float x, const float y, const float z) { - tf2::Quaternion tf_quat; - tf_quat.setValue( - player_orientation.x, - player_orientation.y, - player_orientation.z, - player_orientation.w - ); - - tf2::Matrix3x3 rot_matrix(tf_quat.inverse()); - tf2::Vector3 point(x, y, z); - tf2::Vector3 transformed_point = rot_matrix * point; - - return transformed_point; + tf2::Quaternion tf_quat; + tf_quat.setValue( + player_orientation.x, + player_orientation.y, + player_orientation.z, + player_orientation.w + ); + + tf2::Matrix3x3 rot_matrix(tf_quat.inverse()); + tf2::Vector3 point(x, y, z); + tf2::Vector3 transformed_point = rot_matrix * point; + + return transformed_point; } } // namespace minecraft_utils_visualization + #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(minecraft_utils_visualization::MobMarker)