diff --git a/.github/workflows/ci_humble.yaml b/.github/workflows/ci_humble.yaml new file mode 100644 index 0000000..099ef7b --- /dev/null +++ b/.github/workflows/ci_humble.yaml @@ -0,0 +1,36 @@ +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 + 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 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..11aa415 --- /dev/null +++ b/minecraft_utils_visualization/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.5) +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() +endif() + +ament_auto_package() \ No newline at end of file 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..60b15e3 --- /dev/null +++ b/minecraft_utils_visualization/include/minecraft_utils_visualization/mob_marker.hpp @@ -0,0 +1,78 @@ +// 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. + +#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 +#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 + +#endif // MINECRAFT_UTILS_VISUALIZATION__MOB_MARKER_HPP_ diff --git a/minecraft_utils_visualization/package.xml b/minecraft_utils_visualization/package.xml new file mode 100644 index 0000000..13c5c27 --- /dev/null +++ b/minecraft_utils_visualization/package.xml @@ -0,0 +1,25 @@ + + + + minecraft_utils_visualization + 0.0.1 + The minecraft_utils_visualization package + Ar-Ray-code + Apache License 2.0 + + ament_cmake_auto + + minecraft_msgs + rclcpp + rclcpp_components + std_msgs + tf2_ros + visualization_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/minecraft_utils_visualization/src/mob_marker.cpp b/minecraft_utils_visualization/src/mob_marker.cpp new file mode 100644 index 0000000..b97c65c --- /dev/null +++ b/minecraft_utils_visualization/src/mob_marker.cpp @@ -0,0 +1,241 @@ +// 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; + + 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) +{ + 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)