diff --git a/visualization/tier4_planning_factor_rviz_plugin/CMakeLists.txt b/visualization/tier4_planning_factor_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..db1fa63c128fb --- /dev/null +++ b/visualization/tier4_planning_factor_rviz_plugin/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_planning_factor_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(tier4_planning_factor_rviz_plugin SHARED + src/planning_factor_rviz_plugin.cpp +) + +target_link_libraries(tier4_planning_factor_rviz_plugin + ${QT_LIBRARIES} +) + +pluginlib_export_plugin_description_file(rviz_common plugins.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons +) diff --git a/visualization/tier4_planning_factor_rviz_plugin/README.md b/visualization/tier4_planning_factor_rviz_plugin/README.md new file mode 100644 index 0000000000000..7b65c89d5c22d --- /dev/null +++ b/visualization/tier4_planning_factor_rviz_plugin/README.md @@ -0,0 +1 @@ +# tier4_planning_factor_rviz_plugin diff --git a/visualization/tier4_planning_factor_rviz_plugin/icons/classes/PlanningFactorRvizPlugin.png b/visualization/tier4_planning_factor_rviz_plugin/icons/classes/PlanningFactorRvizPlugin.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/visualization/tier4_planning_factor_rviz_plugin/icons/classes/PlanningFactorRvizPlugin.png differ diff --git a/visualization/tier4_planning_factor_rviz_plugin/package.xml b/visualization/tier4_planning_factor_rviz_plugin/package.xml new file mode 100644 index 0000000000000..f1d2ce7df6ef4 --- /dev/null +++ b/visualization/tier4_planning_factor_rviz_plugin/package.xml @@ -0,0 +1,33 @@ + + + + tier4_planning_factor_rviz_plugin + 0.40.0 + The tier4_planning_factor_rviz_plugin package + Satoshi Ota + Mamoru Sobue + Shumpei Wakabayashi + Apache 2.0 + + ament_cmake + autoware_cmake + + qtbase5-dev + + autoware_motion_utils + autoware_universe_utils + autoware_vehicle_info_utils + rviz_common + rviz_default_plugins + tier4_planning_msgs + + libqt5-widgets + rviz2 + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/visualization/tier4_planning_factor_rviz_plugin/plugins.xml b/visualization/tier4_planning_factor_rviz_plugin/plugins.xml new file mode 100644 index 0000000000000..7e5b53d2b1d61 --- /dev/null +++ b/visualization/tier4_planning_factor_rviz_plugin/plugins.xml @@ -0,0 +1,8 @@ + + + + tier4_planning_factor_rviz_plugin + tier4_planning_msgs/msg/PlanningFactorArray + + + diff --git a/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp new file mode 100644 index 0000000000000..9530368421596 --- /dev/null +++ b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp @@ -0,0 +1,189 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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 "planning_factor_rviz_plugin.hpp" + +#include +#include +#include +#include + +#include + +namespace autoware::rviz_plugins +{ + +using autoware::motion_utils::createDeadLineVirtualWallMarker; +using autoware::motion_utils::createSlowDownVirtualWallMarker; +using autoware::motion_utils::createStopVirtualWallMarker; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerScale; + +namespace +{ + +std_msgs::msg::ColorRGBA convertFromColorCode(const uint64_t code, const float alpha) +{ + const float r = static_cast(code >> 16) / 255.0; + const float g = static_cast((code << 48) >> 56) / 255.0; + const float b = static_cast((code << 56) >> 56) / 255.0; + + return autoware::universe_utils::createMarkerColor(r, g, b, alpha); +} + +std_msgs::msg::ColorRGBA getGreen(const float alpha) +{ + constexpr uint64_t code = 0x00e676; + return convertFromColorCode(code, alpha); +} + +std_msgs::msg::ColorRGBA getRed(const float alpha) +{ + constexpr uint64_t code = 0xff3d00; + return convertFromColorCode(code, alpha); +} + +std_msgs::msg::ColorRGBA getGray(const float alpha) +{ + constexpr uint64_t code = 0xbdbdbd; + return convertFromColorCode(code, alpha); +} + +visualization_msgs::msg::Marker createArrowMarker( + const size_t id, const geometry_msgs::msg::Point & position, + const std_msgs::msg::ColorRGBA & color, const std::string & name, const double height_offset, + const double arrow_length = 1.0) +{ + const double line_width = 0.25 * arrow_length; + const double head_width = 0.5 * arrow_length; + const double head_height = 0.5 * arrow_length; + + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_arrow", id, + visualization_msgs::msg::Marker::ARROW, createMarkerScale(line_width, head_width, head_height), + color); + + geometry_msgs::msg::Point src, dst; + src = position; + src.z += height_offset + arrow_length; + dst = position; + dst.z += height_offset; + + marker.points.push_back(src); + marker.points.push_back(dst); + + return marker; +} + +visualization_msgs::msg::Marker createCircleMarker( + const size_t id, const geometry_msgs::msg::Point & position, + const std_msgs::msg::ColorRGBA & color, const std::string & name, const double radius, + const double height_offset, const double line_width = 0.1) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name, id, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(line_width, 0.0, 0.0), color); + + constexpr size_t num_points = 20; + for (size_t i = 0; i < num_points; ++i) { + geometry_msgs::msg::Point point; + const double ratio = static_cast(i) / static_cast(num_points); + const double theta = 2 * autoware::universe_utils::pi * ratio; + point.x = position.x + radius * autoware::universe_utils::cos(theta); + point.y = position.y + radius * autoware::universe_utils::sin(theta); + point.z = position.z + height_offset; + marker.points.push_back(point); + } + marker.points.push_back(marker.points.front()); + + return marker; +} + +visualization_msgs::msg::Marker createNameTextMarker( + const size_t id, const geometry_msgs::msg::Point & position, const std::string & name, + const double height_offset, const double text_size = 0.5) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_name_text", id, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, text_size), + getGray(0.999)); + + marker.text = name; + + marker.pose.position = position; + marker.pose.position.z += height_offset; + + return marker; +} + +visualization_msgs::msg::MarkerArray createTargetMarker( + const size_t id, const geometry_msgs::msg::Point & position, + const std_msgs::msg::ColorRGBA & color, const std::string & name, + const double height_offset = 2.0, const double arrow_length = 1.0, const double line_width = 0.1) +{ + visualization_msgs::msg::MarkerArray marker_array; + marker_array.markers.push_back( + createArrowMarker(id, position, color, name, height_offset, arrow_length)); + marker_array.markers.push_back(createCircleMarker( + id, position, color, name + "_circle1", 0.5 * arrow_length, height_offset + 0.75 * arrow_length, + line_width)); + marker_array.markers.push_back(createCircleMarker( + id, position, color, name + "_circle2", 0.75 * arrow_length, + height_offset + 0.75 * arrow_length, line_width)); + marker_array.markers.push_back(createNameTextMarker( + id, position, name, height_offset + 1.5 * arrow_length, 0.5 * arrow_length)); + + return marker_array; +} +} // namespace + +void PlanningFactorRvizPlugin::processMessage( + const tier4_planning_msgs::msg::PlanningFactorArray::ConstSharedPtr msg) +{ + size_t i = 0L; + for (const auto & factor : msg->factors) { + const auto text = factor.module + (factor.detail.empty() ? "" : " (" + factor.detail + ")"); + + switch (factor.behavior) { + case tier4_planning_msgs::msg::PlanningFactor::STOP: + for (const auto & control_point : factor.control_points) { + const auto virtual_wall = createStopVirtualWallMarker( + control_point.pose, text, msg->header.stamp, i++, baselink2front_.getFloat()); + add_marker(std::make_shared(virtual_wall)); + } + break; + + case tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN: + for (const auto & control_point : factor.control_points) { + const auto virtual_wall = createSlowDownVirtualWallMarker( + control_point.pose, text, msg->header.stamp, i++, baselink2front_.getFloat()); + add_marker(std::make_shared(virtual_wall)); + } + break; + } + + for (const auto & safety_factor : factor.safety_factors.factors) { + const auto color = safety_factor.is_safe ? getGreen(0.999) : getRed(0.999); + for (const auto & point : safety_factor.points) { + const auto safety_factor_marker = createTargetMarker(i++, point, color, factor.module); + add_marker(std::make_shared(safety_factor_marker)); + } + } + } +} +} // namespace autoware::rviz_plugins + +// Export the plugin +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(autoware::rviz_plugins::PlanningFactorRvizPlugin, rviz_common::Display) diff --git a/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.hpp b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.hpp new file mode 100644 index 0000000000000..f2af16e3e06b1 --- /dev/null +++ b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.hpp @@ -0,0 +1,103 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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 PLANNING_FACTOR_RVIZ_PLUGIN_HPP_ +#define PLANNING_FACTOR_RVIZ_PLUGIN_HPP_ + +#include +#include +#include +#include +#include + +#include + +#include + +namespace autoware::rviz_plugins +{ + +using RosTopicDisplay = rviz_common::RosTopicDisplay; + +class PlanningFactorRvizPlugin +: public rviz_common::RosTopicDisplay +{ +public: + PlanningFactorRvizPlugin() + : marker_common_{this}, + baselink2front_{"Baselink To Front", 0.0, "Length between base link to front.", this}, + topic_name_{"planning_factors"} + { + } + + void onInitialize() override + { + RosTopicDisplay::RTDClass::onInitialize(); + marker_common_.initialize(this->context_, this->scene_node_); + QString message_type = QString::fromStdString( + rosidl_generator_traits::name()); + this->topic_property_->setMessageType(message_type); + this->topic_property_->setValue(topic_name_.c_str()); + this->topic_property_->setDescription("Topic to subscribe to."); + + const auto vehicle_info = + autoware::vehicle_info_utils::VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()) + .getVehicleInfo(); + baselink2front_.setValue(vehicle_info.max_longitudinal_offset_m); + } + + void load(const rviz_common::Config & config) override + { + RosTopicDisplay::Display::load(config); + marker_common_.load(config); + } + + void update(float wall_dt, float ros_dt) override { marker_common_.update(wall_dt, ros_dt); } + + void reset() override + { + RosTopicDisplay::reset(); + marker_common_.clearMarkers(); + } + + void clear_markers() { marker_common_.clearMarkers(); } + + void add_marker(visualization_msgs::msg::Marker::ConstSharedPtr marker_ptr) + { + marker_common_.addMessage(marker_ptr); + } + + void add_marker(visualization_msgs::msg::MarkerArray::ConstSharedPtr markers_ptr) + { + marker_common_.addMessage(markers_ptr); + } + + void delete_marker(rviz_default_plugins::displays::MarkerID marker_id) + { + marker_common_.deleteMarker(marker_id); + } + +private: + void processMessage( + const tier4_planning_msgs::msg::PlanningFactorArray::ConstSharedPtr msg) override; + + rviz_default_plugins::displays::MarkerCommon marker_common_; + + rviz_common::properties::FloatProperty baselink2front_; + + std::string topic_name_; +}; +} // namespace autoware::rviz_plugins + +#endif // PLANNING_FACTOR_RVIZ_PLUGIN_HPP_