From 349f4a7323e9c1a4d8422d5d74a244ad4b900760 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 10 Jan 2025 16:57:53 +0800 Subject: [PATCH] Add nexus_visualization package Signed-off-by: Luca Della Vedova --- nexus_visualization/CMakeLists.txt | 77 ++++++ nexus_visualization/package.xml | 26 ++ .../src/workcell_state_visualizer.cpp | 257 ++++++++++++++++++ .../src/workcell_state_visualizer.hpp | 79 ++++++ 4 files changed, 439 insertions(+) create mode 100644 nexus_visualization/CMakeLists.txt create mode 100644 nexus_visualization/package.xml create mode 100644 nexus_visualization/src/workcell_state_visualizer.cpp create mode 100644 nexus_visualization/src/workcell_state_visualizer.hpp diff --git a/nexus_visualization/CMakeLists.txt b/nexus_visualization/CMakeLists.txt new file mode 100644 index 0000000..a3c2eeb --- /dev/null +++ b/nexus_visualization/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.8) +project(nexus_visualization) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include(GNUInstallDirs) + +set(dep_pkgs + rclcpp + rclcpp_components + nexus_endpoints + nexus_orchestrator_msgs + rmf_building_map_msgs + rmf_visualization_msgs + geometry_msgs + visualization_msgs +) +foreach(pkg ${dep_pkgs}) + find_package(${pkg} REQUIRED) +endforeach() + +#=============================================================================== +add_library(workcell_state_visualizer SHARED src/workcell_state_visualizer.cpp) + +target_link_libraries(workcell_state_visualizer + PUBLIC + rclcpp::rclcpp + ${rclcpp_components_TARGETS} + nexus_endpoints::nexus_endpoints + ${nexus_orchestrator_msgs_TARGETS} + ${rmf_building_map_msgs_TARGETS} + ${rmf_visualization_msgs_TARGETS} + ${visualization_msgs_TARGETS} + ${geometry_msgs_TARGETS} +) + +target_include_directories(workcell_state_visualizer + PRIVATE + $ + $ +) + +target_compile_features(workcell_state_visualizer INTERFACE cxx_std_17) + +rclcpp_components_register_node(workcell_state_visualizer + PLUGIN "workcell_state_visualizer" + EXECUTABLE workcell_state_visualizer_node) + +#=============================================================================== +if(BUILD_TESTING) + find_package(ament_cmake_uncrustify REQUIRED) + find_package(rmf_utils REQUIRED) + find_file(uncrustify_config_file + NAMES "rmf_code_style.cfg" + PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/") + + + # TODO(luca) reintroduce uncrustify + #ament_uncrustify( + # ARGN src + # CONFIG_FILE ${uncrustify_config_file} + # MAX_LINE_LENGTH 80 + #) + +endif() + +#=============================================================================== +install( + TARGETS workcell_state_visualizer + RUNTIME DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) + +ament_package() diff --git a/nexus_visualization/package.xml b/nexus_visualization/package.xml new file mode 100644 index 0000000..c1b5030 --- /dev/null +++ b/nexus_visualization/package.xml @@ -0,0 +1,26 @@ + + + + nexus_visualization + 0.0.0 + Package to visualize workcells and their states in rviz + Luca Della Vedova + Apache License 2.0 + + ament_cmake + + nexus_endpoints + nexus_orchestrator_msgs + rclcpp + rclcpp_components + rmf_building_map_msgs + visualization_msgs + rmf_visualization_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/nexus_visualization/src/workcell_state_visualizer.cpp b/nexus_visualization/src/workcell_state_visualizer.cpp new file mode 100644 index 0000000..a139abd --- /dev/null +++ b/nexus_visualization/src/workcell_state_visualizer.cpp @@ -0,0 +1,257 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * 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 "workcell_state_visualizer.hpp" + +using namespace std::chrono_literals; + +namespace nexus::visualization { + +//============================================================================== +WorkcellStateVisualizer::WorkcellStateVisualizer(const rclcpp::NodeOptions& options) +: Node("workcell_state_visualizer", options), + _next_available_id(0) +{ + RCLCPP_INFO( + this->get_logger(), + "Configuring workcell_state_visualizer..." + ); + + _current_level = this->declare_parameter("initial_map_name", "L1"); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter initial_map_name to %s", _current_level.c_str() + ); + + auto transient_qos = rclcpp::QoS(1).transient_local(); + _map_sub = this->create_subscription( + "/map", + transient_qos, + [=](BuildingMap::ConstSharedPtr msg) + { + RCLCPP_INFO( + this->get_logger(), + "Received map" + ); + // Note that this implementation depends on vertices being in the navgraphs + // We should probably parse the map but it could be in pixel units which is tricky + for (const auto& level : msg->levels) + { + for (const auto& graph : level.nav_graphs) + { + for (const auto& vertex : graph.vertices) + { + for (const auto& param : vertex.params) + { + if (param.name == "pickup_dispenser") + { + RCLCPP_INFO( + this->get_logger(), + "Found dispenser [%s]", param.value_string.c_str() + ); + WorkcellDescription description; + description.workcell_id = vertex.name; + description.level_name = level.name; + description.location.position.x = vertex.x; + description.location.position.y = vertex.y; + _workcells.insert({vertex.name, Workcell {description, std::nullopt}}); + _ids.insert({vertex.name, _next_available_id}); + ++_next_available_id; + } + } + } + } + } + initialize_state_subscriptions(); + }); + + _param_sub = this->create_subscription( + "rmf_visualization/parameters", + rclcpp::SystemDefaultsQoS(), + [=](RvizParam::ConstSharedPtr msg) + { + if (msg->map_name.empty() || msg->map_name == _current_level) + return; + + _current_level = msg->map_name; + publish_markers(); + }); + + _marker_pub = this->create_publisher("/workcell_markers", transient_qos); + + RCLCPP_INFO( + this->get_logger(), + "Running workcell_state_visualizer..." + ); + +} + +void WorkcellStateVisualizer::initialize_state_subscriptions() +{ + for (const auto& [name, _] : _workcells) + { + _state_subs.emplace_back(nexus::endpoints::WorkcellStateTopic::create_subscription(this, name, + [this](nexus::endpoints::WorkcellStateTopic::MessageType::ConstSharedPtr msg) + { + auto state_it = _workcells.find(msg->workcell_id); + if (state_it == _workcells.end()) + { + RCLCPP_WARN(this->get_logger(), "Received state for workcell [%s] but it was not found in the map", msg->workcell_id.c_str()); + return; + } + state_it->second.state = *msg; + })); + } +} + +void WorkcellStateVisualizer::publish_markers() +{ + auto set_body_pose = + [](const Workcell& workcell, Marker& marker) + { + marker.pose.position.x = workcell.description.location.position.x; + marker.pose.position.y = workcell.description.location.position.y; + marker.pose.position.z = 0.0; + marker.pose.orientation.w = 1.0; + }; + + auto set_text_pose = + [](const Workcell& workcell, const double x_offset, Marker& marker) + { + marker.pose.position.x = workcell.description.location.position.x + x_offset; + marker.pose.position.y = workcell.description.location.position.y; + marker.pose.position.z = 0.0; + marker.pose.orientation.w = 1.0; + }; + + auto fill_marker = + [&]( + const std::string& name, + const Workcell& workcell, + const double radius, + MarkerArray& marker_array) + { + std::size_t id = _ids[name]; + Marker body_marker; + body_marker.header.frame_id = "map"; + body_marker.header.stamp = this->get_clock()->now(); + body_marker.ns = "body"; + body_marker.id = id; + body_marker.type = body_marker.CUBE; + body_marker.action = body_marker.ADD; + set_body_pose(workcell, body_marker); + body_marker.scale.x = radius; + body_marker.scale.y = radius; + body_marker.scale.z = radius; + // TODO(luca) color to match state + Color color; + std::string status; + color.a = 1.0; + if (!workcell.state.has_value()) + { + // Indeterminate, grey + color.r = 0.3; + color.g = 0.3; + color.b = 0.3; + status = "UNKNOWN"; + } else if (workcell.state.value().status == WorkcellState::STATUS_IDLE) { + // Idle, green + color.r = 0.0; + color.g = 0.8; + color.b = 0.0; + status = "IDLE"; + } else if (workcell.state.value().status == WorkcellState::STATUS_BUSY) { + // Busy, yellow + color.r = 0.5; + color.g = 0.5; + color.b = 0.0; + status = "BUSY"; + } + body_marker.color = color; + + auto text_marker = body_marker; + text_marker.ns = "name"; + text_marker.type = text_marker.TEXT_VIEW_FACING; + set_text_pose(workcell, radius * 2.0, text_marker); + std::string task_id = ""; + std::string message = ""; + const auto& state = workcell.state; + if (state.has_value()) + { + if (!state.value().message.empty()) + { + message = state.value().message; + } + if (!state.value().task_id.empty()) + { + task_id = state.value().task_id; + } + } + text_marker.text = name + "\n" + "STATUS:" + status + "\n" + "TASK:" + task_id + "\n" + "MESSAGE:" + message; + text_marker.scale.z = 0.3; + + marker_array.markers.push_back(std::move(body_marker)); + marker_array.markers.push_back(std::move(text_marker)); + }; + + auto delete_marker = + [&]( + const std::string& name, + MarkerArray& marker_array) + { + std::size_t id = _ids[name]; + Marker body_marker; + body_marker.header.frame_id = "map"; + body_marker.header.stamp = this->get_clock()->now(); + body_marker.ns = "body"; + body_marker.id = id; + body_marker.type = body_marker.CUBE; + body_marker.action = body_marker.DELETE; + auto text_marker = body_marker; + text_marker.ns = "name"; + text_marker.type = text_marker.TEXT_VIEW_FACING; + marker_array.markers.push_back(std::move(body_marker)); + marker_array.markers.push_back(std::move(text_marker)); + }; + + MarkerArray marker_array; + for (const auto& [name, workcell] : _workcells) + { + if (workcell.description.level_name != _current_level) + { + delete_marker(name, marker_array); + } + continue; + + double radius = 1.0; + fill_marker( + name, + workcell, + radius, + marker_array + ); + } + if (!marker_array.markers.empty()) + _marker_pub->publish(marker_array); +} +} + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(nexus::visualization::WorkcellStateVisualizer) diff --git a/nexus_visualization/src/workcell_state_visualizer.hpp b/nexus_visualization/src/workcell_state_visualizer.hpp new file mode 100644 index 0000000..f1536d7 --- /dev/null +++ b/nexus_visualization/src/workcell_state_visualizer.hpp @@ -0,0 +1,79 @@ +/* + * Copyright (C) 2025 Open Source Robotics Foundation + * + * 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 SRC__WORKCELL_STATE_VISUALIZER_HPP +#define SRC__WORKCELL_STATE_VISUALIZER_HPP + +#include + +#include +#include + +#include +#include + +#include +#include +#include + +#include + +namespace nexus::visualization { + +//============================================================================== +class WorkcellStateVisualizer : public rclcpp::Node +{ +public: + using BuildingMap = rmf_building_map_msgs::msg::BuildingMap; + using RvizParam = rmf_visualization_msgs::msg::RvizParam; + using WorkcellDescription = nexus_orchestrator_msgs::msg::WorkcellDescription; + using WorkcellState = nexus_orchestrator_msgs::msg::WorkcellState; + using Marker = visualization_msgs::msg::Marker; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using Color = std_msgs::msg::ColorRGBA; + +/// Constructor + WorkcellStateVisualizer( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + +private: + struct Workcell { + WorkcellDescription description; + std::optional state; + }; + + void initialize_state_subscriptions(); + + void publish_markers(); + + std::string _current_level; + + rclcpp::Subscription::SharedPtr _map_sub; + rclcpp::Subscription::SharedPtr _param_sub; + std::vector::SharedPtr> _state_subs; + rclcpp::Publisher::SharedPtr _marker_pub; + + // Map workcell name to a unique marker id + std::unordered_map _ids; + std::size_t _next_available_id; + + std::unordered_map _workcells; +}; + +} + +#endif // SRC__WORKCELLSTATEVISUALIZER_HPP