diff --git a/nexus_visualization/CMakeLists.txt b/nexus_visualization/CMakeLists.txt deleted file mode 100644 index a3c2eeb..0000000 --- a/nexus_visualization/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -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 deleted file mode 100644 index c1b5030..0000000 --- a/nexus_visualization/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - 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 deleted file mode 100644 index a139abd..0000000 --- a/nexus_visualization/src/workcell_state_visualizer.cpp +++ /dev/null @@ -1,257 +0,0 @@ -/* - * 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 deleted file mode 100644 index f1536d7..0000000 --- a/nexus_visualization/src/workcell_state_visualizer.hpp +++ /dev/null @@ -1,79 +0,0 @@ -/* - * 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