diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index cac45cabb096a..cc976d668d7d7 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -83,4 +83,4 @@ repos: args: ["--quiet"] exclude: ".cu" -exclude: ".svg" +exclude: ".svg|common/autoware_auto_perception_rviz_plugin" diff --git a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt b/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..d85b408f4cdb6 --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt @@ -0,0 +1,84 @@ +# Copyright 2019 The Autoware 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. +# +# Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +cmake_minimum_required(VERSION 3.5) +project(autoware_auto_perception_rviz_plugin) + +#dependencies +find_package(ament_cmake_auto REQUIRED) +find_package(Qt5 REQUIRED COMPONENTS Widgets) +ament_auto_find_build_dependencies() + +set(OD_PLUGIN_LIB_SRC + src/object_detection/detected_objects_display.cpp + src/object_detection/tracked_objects_display.cpp + src/object_detection/predicted_objects_display.cpp +) + +set(OD_PLUGIN_LIB_HEADERS + include/visibility_control.hpp +) +set(OD_PLUGIN_LIB_HEADERS_TO_WRAP + include/object_detection/detected_objects_display.hpp + include/object_detection/tracked_objects_display.hpp + include/object_detection/predicted_objects_display.hpp +) + +set(COMMON_HEADERS + include/common/color_alpha_property.hpp + include/object_detection/object_polygon_detail.hpp + include/object_detection/object_polygon_display_base.hpp +) + +set(COMMON_SRC + src/common/color_alpha_property.cpp + src/object_detection/object_polygon_detail.cpp +) + +foreach(header "${OD_PLUGIN_LIB_HEADERS_TO_WRAP}") + qt5_wrap_cpp(OD_PLUGIN_LIB_HEADERS_MOC "${header}") +endforeach() + +ament_auto_add_library(${PROJECT_NAME} SHARED + ${COMMON_HEADERS} + ${COMMON_SRC} + ${OD_PLUGIN_LIB_HEADERS} + ${OD_PLUGIN_LIB_HEADERS_MOC} + ${OD_PLUGIN_LIB_SRC} +) +target_link_libraries(${PROJECT_NAME} + rviz_common::rviz_common + Qt5::Widgets +) +target_include_directories(${PROJECT_NAME} PRIVATE include) + +# Settings to improve the build as suggested on https://github.com/ros2/rviz/blob/ros2/docs/plugin_development.md +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +target_compile_definitions(${PROJECT_NAME} PRIVATE "OBJECT_DETECTION_PLUGINS_BUILDING_LIBRARY") + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) + +if(BUILD_TESTING) + # run linters + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + icons +) diff --git a/common/autoware_auto_perception_rviz_plugin/README.md b/common/autoware_auto_perception_rviz_plugin/README.md new file mode 100644 index 0000000000000..68f96d654e258 --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/README.md @@ -0,0 +1,62 @@ +# autoware_auto_perception_plugin + +## Purpose + +It is an rviz plugin for visualizing the result from perception module. This package is based on the implementation of the rviz plugin developed by Autoware.Auto. + +See Autoware.Auto design documentation for the original design philosophy. [[1]](https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/blob/master/src/tools/visualization/autoware_rviz_plugins) + + + +## Input Types / Visualization Results + +### DetectedObjects + +#### Input Types + +| Name | Type | Description | +| ---- | ----------------------------------------------------- | ---------------------- | +| | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection result array | + +#### Visualization Result + +![](./images/detected-object-visualization-description.jpg) + +### TrackedObjects + +#### Input Types + +| Name | Type | Description | +| ---- | ---------------------------------------------------- | --------------------- | +| | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking result array | + +#### Visualization Result + +Overwrite tracking results with detection results. + +![](./images/tracked-object-visualization-description.jpg) + +### PredictedObjects + +#### Input Types + +| Name | Type | Description | +| ---- | ------------------------------------------------------ | ----------------------- | +| | `autoware_auto_perception_msgs::msg::PredictedObjects` | prediction result array | + +#### Visualization Result + +Overwrite prediction results with tracking results. + +![](./images/predicted-object-visualization-description.jpg) + +## References/External links + +[1] https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/tree/master/src/tools/visualization/autoware_rviz_plugins + +## Future extensions / Unimplemented parts diff --git a/common/autoware_auto_perception_rviz_plugin/icons/classes/DetectedObjects.png b/common/autoware_auto_perception_rviz_plugin/icons/classes/DetectedObjects.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_auto_perception_rviz_plugin/icons/classes/DetectedObjects.png differ diff --git a/common/autoware_auto_perception_rviz_plugin/icons/classes/PredictedObjects.png b/common/autoware_auto_perception_rviz_plugin/icons/classes/PredictedObjects.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_auto_perception_rviz_plugin/icons/classes/PredictedObjects.png differ diff --git a/common/autoware_auto_perception_rviz_plugin/icons/classes/TrackedObjects.png b/common/autoware_auto_perception_rviz_plugin/icons/classes/TrackedObjects.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_auto_perception_rviz_plugin/icons/classes/TrackedObjects.png differ diff --git a/common/autoware_auto_perception_rviz_plugin/images/detected-object-visualization-description.jpg b/common/autoware_auto_perception_rviz_plugin/images/detected-object-visualization-description.jpg new file mode 100644 index 0000000000000..a7e3257fd5138 Binary files /dev/null and b/common/autoware_auto_perception_rviz_plugin/images/detected-object-visualization-description.jpg differ diff --git a/common/autoware_auto_perception_rviz_plugin/images/predicted-object-visualization-description.jpg b/common/autoware_auto_perception_rviz_plugin/images/predicted-object-visualization-description.jpg new file mode 100644 index 0000000000000..93420ab345f4c Binary files /dev/null and b/common/autoware_auto_perception_rviz_plugin/images/predicted-object-visualization-description.jpg differ diff --git a/common/autoware_auto_perception_rviz_plugin/images/tracked-object-visualization-description.jpg b/common/autoware_auto_perception_rviz_plugin/images/tracked-object-visualization-description.jpg new file mode 100644 index 0000000000000..158365876aa9f Binary files /dev/null and b/common/autoware_auto_perception_rviz_plugin/images/tracked-object-visualization-description.jpg differ diff --git a/common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp b/common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp new file mode 100644 index 0000000000000..50edcf844e6a6 --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp @@ -0,0 +1,56 @@ +// Copyright 2021 Apex.AI, 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 COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#define COMMON__COLOR_ALPHA_PROPERTY_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace common +{ +/// \brief Class to define Color and Alpha values as plugin properties +class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ColorAlphaProperty +{ +public: + /// \brief Constructor + /// \param color_default Default value for color property + /// \param alpha_default Default value for alpha property + /// \param parent_property Parent property for the color and alpha properties. Memory managed + /// by the caller + ColorAlphaProperty( + const QColor & color_default, const float alpha_default, + rviz_common::properties::Property * parent_property); + + /// \brief Convert color and alpha to ColorRGBA type + /// \return color and alpha values as ColorRGBA type + operator std_msgs::msg::ColorRGBA() const; + +private: + rviz_common::properties::ColorProperty m_color_property; + rviz_common::properties::FloatProperty m_alpha_property; +}; + +} // namespace common +} // namespace rviz_plugins +} // namespace autoware + +#endif // COMMON__COLOR_ALPHA_PROPERTY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp new file mode 100644 index 0000000000000..9ff8adac9d2e4 --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -0,0 +1,45 @@ +// Copyright 2021 Apex.AI, 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 OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#define OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ + +#include +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace object_detection +{ +/// \brief Class defining rviz plugin to visualize DetectedObjects +class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay + : public ObjectPolygonDisplayBase +{ + Q_OBJECT + +public: + using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects; + + DetectedObjectsDisplay(); + +private: + void processMessage(DetectedObjects::ConstSharedPtr msg) override; +}; + +} // namespace object_detection +} // namespace rviz_plugins +} // namespace autoware + +#endif // OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp new file mode 100644 index 0000000000000..54414ab442b1b --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp @@ -0,0 +1,208 @@ +// Copyright 2021 Apex.AI, 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. +/// \brief This file defines some helper functions used by ObjectPolygonDisplayBase class +#ifndef OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#define OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace object_detection +{ +namespace detail +{ +// Struct to define all the configurable visual properties of an object of a particular +// classification type +struct ObjectPropertyValues +{ + // Classified type of the object + std::string label; + // Color for the type of the object + std::array color; + // Alpha values for the type of the object + float alpha{0.999F}; +}; + +// Map defining colors according to value of label field in ObjectClassification msg +const std::map< + autoware_auto_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues> +// Color map is based on cityscapes color +kDefaultObjectPropertyValues = { + {autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, + {"UNKNOWN", {255, 255, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + {"PEDESTRIAN", {255, 192, 203}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + {"MOTORCYCLE", {119, 11, 32}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER, + {"TRAILER", {30, 144, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; + +/// \brief Convert the given polygon into a marker representing the shape in 3d +/// \param shape_msg Shape msg to be converted. Corners should be in object-local frame +/// \param centroid Centroid position of the shape in Object.header.frame_id frame +/// \param orientation Orientation of the shape in Object.header.frame_id frame +/// \param color_rgba Color and alpha values to use for the marker +/// \return Marker ptr. Id and header will have to be set by the caller +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_shape_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba); + +/// \brief Convert the given polygon into a marker representing the shape in 3d +/// \param centroid Centroid position of the shape in Object.header.frame_id frame +/// \return Marker ptr. Id and header will have to be set by the caller +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_label_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std::string label, const std_msgs::msg::ColorRGBA & color_rgba); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_uuid_marker_ptr( + const std::string & uuid, const geometry_msgs::msg::Point & centroid, + const std_msgs::msg::ColorRGBA & color_rgba); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_pose_with_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_velocity_text_marker_ptr( + const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, + const std_msgs::msg::ColorRGBA & color_rgba); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_twist_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_predicted_path_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const std_msgs::msg::ColorRGBA & predicted_path_color); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_path_confidence_marker_ptr( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const std_msgs::msg::ColorRGBA & path_confidence_color); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( + const geometry_msgs::msg::Point center, const double radius, + std::vector & points, const int n); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( + const autoware_auto_perception_msgs::msg::PredictedPath & paths, + std::vector & points); + +/// \brief Convert Point32 to Point +/// \param val Point32 to be converted +/// \return Point type +inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Point to_point( + const geometry_msgs::msg::Point32 & val) +{ + geometry_msgs::msg::Point ret; + ret.x = static_cast(val.x); + ret.y = static_cast(val.y); + ret.z = static_cast(val.z); + return ret; +} + +/// \brief Convert to Pose from Point and Quaternion +/// \param point +/// \param orientation +/// \return Pose type +inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose to_pose( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Quaternion & orientation) +{ + geometry_msgs::msg::Pose ret; + ret.position = point; + ret.orientation = orientation; + return ret; +} + +inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose initPose() +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + pose.position.z = 0.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + return pose; +} + +/// \brief Get the best classification from the list of classifications based on max probability +/// \tparam ClassificationContainerT List type with ObjectClassificationMsg +/// \param labels List of ObjectClassificationMsg objects +/// \param logger_name Name to use for logger in case of a warning (if labels is empty) +/// \return Id of the best classification, Unknown if there is no best label +template +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC +autoware_auto_perception_msgs::msg::ObjectClassification::_label_type +get_best_label(ClassificationContainerT labels, const std::string & logger_name) +{ + const auto best_class_label = std::max_element( + labels.begin(), labels.end(), + [](const auto & a, const auto & b) -> bool {return a.probability < b.probability;}); + if (best_class_label == labels.end()) { + RCLCPP_WARN( + rclcpp::get_logger(logger_name), + "Empty classification field. " + "Treating as unknown"); + return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + } + return best_class_label->label; +} + +} // namespace detail +} // namespace object_detection +} // namespace rviz_plugins +} // namespace autoware + +#endif // OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp new file mode 100644 index 0000000000000..1222d4c82d12e --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -0,0 +1,368 @@ +// Copyright 2021 Apex.AI, 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 OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace object_detection +{ +/// \brief Base rviz plugin class for all object msg types. The class defines common properties +/// for the plugin and also defines common helper functions that can be used by its derived +/// classes. +/// \tparam MsgT PredictedObjects or TrackedObjects or DetectedObjects type +template +class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase + : public rviz_common::RosTopicDisplay +{ +public: + using Color = std::array; + using Marker = visualization_msgs::msg::Marker; + using MarkerCommon = rviz_default_plugins::displays::MarkerCommon; + using ObjectClassificationMsg = autoware_auto_perception_msgs::msg::ObjectClassification; + using RosTopicDisplay = rviz_common::RosTopicDisplay; + + using PolygonPropertyMap = + std::unordered_map; + + explicit ObjectPolygonDisplayBase(const std::string & default_topic) + : m_marker_common(this), + m_display_3d_property{ + "Display 3d polygon", true, "Enable/disable height visualization of the polygon", this}, + m_display_label_property{"Display Label", true, "Enable/disable label visualization", this}, + m_display_uuid_property{"Display UUID", true, "Enable/disable uuid visualization", this}, + m_display_pose_with_covariance_property{ + "Display PoseWithCovariance", true, "Enable/disable pose with covariance visualization", + this}, + m_display_velocity_text_property{ + "Display Velocity", true, "Enable/disable velocity text visualization", this}, + m_display_twist_property{"Display Twist", true, "Enable/disable twist visualization", this}, + m_display_predicted_paths_property{ + "Display Predicted Paths", true, "Enable/disable predicted paths visualization", this}, + m_display_path_confidence_property{ + "Display Predicted Path Confidence", true, "Enable/disable predicted paths visualization", + this}, + m_default_topic{default_topic} + { + // iterate over default values to create and initialize the properties. + for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { + const auto & class_property_values = map_property_it.second; + const auto & color = class_property_values.color; + // This is just a parent property to contain the necessary properties for the given class: + m_class_group_properties.emplace_back( + class_property_values.label.c_str(), QVariant(), + "Groups polygon properties for the given class", this); + auto & parent_property = m_class_group_properties.back(); + // Associate a color and opacity property for the given class and attach them to the + // parent property of the class so they can have a drop down view from the label property: + m_polygon_properties.emplace( + std::piecewise_construct, std::forward_as_tuple(map_property_it.first), + std::forward_as_tuple( + QColor{color[0], color[1], color[2]}, class_property_values.alpha, &parent_property)); + } + init_color_list(predicted_path_colors); + } + + void onInitialize() override + { + RosTopicDisplay::RTDClass::onInitialize(); + m_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(m_default_topic.c_str()); + this->topic_property_->setDescription("Topic to subscribe to."); + } + + void load(const rviz_common::Config & config) override + { + RosTopicDisplay::Display::load(config); + m_marker_common.load(config); + } + + void update(float wall_dt, float ros_dt) override {m_marker_common.update(wall_dt, ros_dt);} + + void reset() override + { + RosTopicDisplay::reset(); + m_marker_common.clearMarkers(); + } + + void clear_markers() {m_marker_common.clearMarkers();} + + void add_marker(visualization_msgs::msg::Marker::ConstSharedPtr marker_ptr) + { + m_marker_common.addMessage(marker_ptr); + } + +protected: + /// \brief Convert given shape msg into a Marker + /// \tparam ClassificationContainerT List type with ObjectClassificationMsg + /// \param shape_msg Shape msg to be converted + /// \param centroid Centroid position of the shape in Object.header.frame_id frame + /// \param orientation Orientation of the shape in Object.header.frame_id frame + /// \param labels List of ObjectClassificationMsg objects + /// \return Marker ptr. Id and header will have to be set by the caller + template + std::optional get_shape_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const ClassificationContainerT & labels) const + { + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + + if (m_display_3d_property.getBool()) { + return detail::get_shape_marker_ptr(shape_msg, centroid, orientation, color_rgba); + } else { + return std::nullopt; + } + } + + /// \brief Convert given shape msg into a Marker to visualize label name + /// \tparam ClassificationContainerT List type with ObjectClassificationMsg + /// \param centroid Centroid position of the shape in Object.header.frame_id frame + /// \param labels List of ObjectClassificationMsg objects + /// \return Marker ptr. Id and header will have to be set by the caller + template + std::optional get_label_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const ClassificationContainerT & labels) const + { + if (m_display_label_property.getBool()) { + const std::string label = get_best_label(labels); + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + return detail::get_label_marker_ptr(centroid, orientation, label, color_rgba); + } else { + return std::nullopt; + } + } + + template + std::optional get_uuid_marker_ptr( + const unique_identifier_msgs::msg::UUID & uuid, const geometry_msgs::msg::Point & centroid, + const ClassificationContainerT & labels) const + { + if (m_display_uuid_property.getBool()) { + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + const std::string uuid_str = uuid_to_string(uuid); + return detail::get_uuid_marker_ptr(uuid_str, centroid, color_rgba); + } else { + return std::nullopt; + } + } + + std::optional get_pose_with_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) const + { + if (m_display_pose_with_covariance_property.getBool()) { + return detail::get_pose_with_covariance_marker_ptr(pose_with_covariance); + } else { + return std::nullopt; + } + } + + template + std::optional get_velocity_text_marker_ptr( + const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, + const ClassificationContainerT & labels) const + { + if (m_display_velocity_text_property.getBool()) { + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + return detail::get_velocity_text_marker_ptr(twist, vis_pos, color_rgba); + } else { + return std::nullopt; + } + } + + std::optional get_twist_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) const + { + if (m_display_twist_property.getBool()) { + return detail::get_twist_marker_ptr(pose_with_covariance, twist_with_covariance); + } else { + return std::nullopt; + } + } + + std::optional get_predicted_path_marker_ptr( + const unique_identifier_msgs::msg::UUID & uuid, + const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const + { + if (m_display_predicted_paths_property.getBool()) { + const std::string uuid_str = uuid_to_string(uuid); + const std_msgs::msg::ColorRGBA predicted_path_color = get_color_from_uuid(uuid_str); + return detail::get_predicted_path_marker_ptr(shape, predicted_path, predicted_path_color); + } else { + return std::nullopt; + } + } + + std::optional get_path_confidence_marker_ptr( + const unique_identifier_msgs::msg::UUID & uuid, + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const + { + if (m_display_path_confidence_property.getBool()) { + const std::string uuid_str = uuid_to_string(uuid); + const std_msgs::msg::ColorRGBA path_confidence_color = get_color_from_uuid(uuid_str); + return detail::get_path_confidence_marker_ptr(predicted_path, path_confidence_color); + } else { + return std::nullopt; + } + } + + /// \brief Get color and alpha values based on the given list of classification values + /// \tparam ClassificationContainerT Container of ObjectClassification + /// \param labels list of classifications + /// \return Color and alpha for the best class in the given list. Unknown class is used in + /// degenerate cases + template + std_msgs::msg::ColorRGBA get_color_rgba(const ClassificationContainerT & labels) const + { + static const std::string kLoggerName("ObjectPolygonDisplayBase"); + const auto label = detail::get_best_label(labels, kLoggerName); + auto it = m_polygon_properties.find(label); + if (it == m_polygon_properties.end()) { + it = m_polygon_properties.find(ObjectClassificationMsg::UNKNOWN); + } + return it->second; + } + + /// \brief Get color and alpha values based on the given list of classification values + /// \tparam ClassificationContainerT Container of ObjectClassification + /// \param labels list of classifications + /// \return best label string + template + std::string get_best_label(const ClassificationContainerT & labels) const + { + static const std::string kLoggerName("ObjectPolygonDisplayBase"); + const auto label = detail::get_best_label(labels, kLoggerName); + auto it = detail::kDefaultObjectPropertyValues.find(label); + if (it == detail::kDefaultObjectPropertyValues.end()) { + it = detail::kDefaultObjectPropertyValues.find(ObjectClassificationMsg::UNKNOWN); + } + return (it->second).label; + } + + std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const + { + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +u.uuid[i]; + } + return ss.str(); + } + + std_msgs::msg::ColorRGBA AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC + get_color_from_uuid(const std::string & uuid) const + { + int i = (static_cast(uuid.at(0)) * 4 + static_cast(uuid.at(1))) % + static_cast(predicted_path_colors.size()); + + std_msgs::msg::ColorRGBA color; + color.r = predicted_path_colors.at(i).r; + color.g = predicted_path_colors.at(i).g; + color.b = predicted_path_colors.at(i).b; + return color; + } + + void init_color_list(std::vector & colors) const + { + std_msgs::msg::ColorRGBA sample_color; + sample_color.r = 1.0; + sample_color.g = 0.0; + sample_color.b = 1.0; + colors.push_back(sample_color); // magenta + sample_color.r = 0.69; + sample_color.g = 1.0; + sample_color.b = 0.18; + colors.push_back(sample_color); // green yellow + sample_color.r = 0.59; + sample_color.g = 1.0; + sample_color.b = 0.59; + colors.push_back(sample_color); // pale green + sample_color.r = 0.5; + sample_color.g = 1.0; + sample_color.b = 0.0; + colors.push_back(sample_color); // chartreuse green + sample_color.r = 0.12; + sample_color.g = 0.56; + sample_color.b = 1.0; + colors.push_back(sample_color); // dodger blue + sample_color.r = 0.0; + sample_color.g = 1.0; + sample_color.b = 1.0; + colors.push_back(sample_color); // cyan + sample_color.r = 0.54; + sample_color.g = 0.168; + sample_color.b = 0.886; + colors.push_back(sample_color); // blueviolet + sample_color.r = 0.0; + sample_color.g = 1.0; + sample_color.b = 0.5; + colors.push_back(sample_color); // spring green + } + +private: + // All rviz plugins should have this. Should be initialized with pointer to this class + MarkerCommon m_marker_common; + // List is used to store the properties for classification in case we need to access them: + std::list m_class_group_properties; + // Map to store class labels and its corresponding properties + PolygonPropertyMap m_polygon_properties; + // Property to enable/disable height visualization of the polygon + rviz_common::properties::BoolProperty m_display_3d_property; + // Property to enable/disable label visualization + rviz_common::properties::BoolProperty m_display_label_property; + // Property to enable/disable uuid visualization + rviz_common::properties::BoolProperty m_display_uuid_property; + // Property to enable/disable pose with covariance visualization + rviz_common::properties::BoolProperty m_display_pose_with_covariance_property; + // Property to enable/disable velocity text visualization + rviz_common::properties::BoolProperty m_display_velocity_text_property; + // Property to enable/disable twist visualization + rviz_common::properties::BoolProperty m_display_twist_property; + // Property to enable/disable predicted paths visualization + rviz_common::properties::BoolProperty m_display_predicted_paths_property; + // Property to enable/disable predicted path confidence visualization + rviz_common::properties::BoolProperty m_display_path_confidence_property; + // Default topic name to be visualized + std::string m_default_topic; + + std::vector predicted_path_colors; +}; +} // namespace object_detection +} // namespace rviz_plugins +} // namespace autoware + +#endif // OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp new file mode 100644 index 0000000000000..82c0898718f4b --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -0,0 +1,104 @@ +// Copyright 2020 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 OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#define OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace object_detection +{ +/// \brief Class defining rviz plugin to visualize PredictedObjects +class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay + : public ObjectPolygonDisplayBase +{ + Q_OBJECT + +public: + using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; + + PredictedObjectsDisplay(); + +private: + void processMessage(PredictedObjects::ConstSharedPtr msg) override; + + boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) + { + const std::string uuid_str = uuid_to_string(uuid_msg); + boost::uuids::string_generator gen; + boost::uuids::uuid uuid = gen(uuid_str); + return uuid; + } + + void update_id_map(const PredictedObjects::ConstSharedPtr & msg) + { + std::vector new_uuids; + std::vector tracked_uuids; + new_uuids.reserve(msg->objects.size()); + tracked_uuids.reserve(msg->objects.size()); + for (const auto & object : msg->objects) { + const auto uuid = to_boost_uuid(object.object_id); + ((id_map.find(uuid) != id_map.end()) ? tracked_uuids : new_uuids).push_back(uuid); + } + + auto itr = id_map.begin(); + while (itr != id_map.end()) { + if ( + std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) + { + unused_marker_ids.push_back(itr->second); + itr = id_map.erase(itr); + } else { + ++itr; + } + } + + for (const auto & new_uuid : new_uuids) { + if (unused_marker_ids.empty()) { + id_map.emplace(new_uuid, marker_id); + marker_id++; + } else { + id_map.emplace(new_uuid, unused_marker_ids.front()); + unused_marker_ids.pop_front(); + } + } + } + + int32_t uuid_to_marker_id(const unique_identifier_msgs::msg::UUID & uuid_msg) + { + auto uuid = to_boost_uuid(uuid_msg); + return id_map.at(uuid); + } + + std::map id_map; + std::list unused_marker_ids; + int32_t marker_id = 0; + const int32_t PATH_ID_CONSTANT = 1e3; +}; + +} // namespace object_detection +} // namespace rviz_plugins +} // namespace autoware + +#endif // OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp new file mode 100644 index 0000000000000..43077ce1ce32e --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -0,0 +1,103 @@ +// Copyright 2021 Apex.AI, 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 OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#define OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace object_detection +{ +/// \brief Class defining rviz plugin to visualize TrackedObjects +class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay + : public ObjectPolygonDisplayBase +{ + Q_OBJECT + +public: + using TrackedObjects = autoware_auto_perception_msgs::msg::TrackedObjects; + + TrackedObjectsDisplay(); + +private: + void processMessage(TrackedObjects::ConstSharedPtr msg) override; + + boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) + { + const std::string uuid_str = uuid_to_string(uuid_msg); + boost::uuids::string_generator gen; + boost::uuids::uuid uuid = gen(uuid_str); + return uuid; + } + + void update_id_map(const TrackedObjects::ConstSharedPtr & msg) + { + std::vector new_uuids; + std::vector tracked_uuids; + new_uuids.reserve(msg->objects.size()); + tracked_uuids.reserve(msg->objects.size()); + for (const auto & object : msg->objects) { + const auto uuid = to_boost_uuid(object.object_id); + ((id_map.find(uuid) != id_map.end()) ? tracked_uuids : new_uuids).push_back(uuid); + } + + auto itr = id_map.begin(); + while (itr != id_map.end()) { + if ( + std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) + { + unused_marker_ids.push_back(itr->second); + itr = id_map.erase(itr); + } else { + ++itr; + } + } + + for (const auto & new_uuid : new_uuids) { + if (unused_marker_ids.empty()) { + id_map.emplace(new_uuid, marker_id); + marker_id++; + } else { + id_map.emplace(new_uuid, unused_marker_ids.front()); + unused_marker_ids.pop_front(); + } + } + } + + int32_t uuid_to_marker_id(const unique_identifier_msgs::msg::UUID & uuid_msg) + { + auto uuid = to_boost_uuid(uuid_msg); + return id_map.at(uuid); + } + + std::map id_map; + std::list unused_marker_ids; + int32_t marker_id = 0; +}; + +} // namespace object_detection +} // namespace rviz_plugins +} // namespace autoware + +#endif // OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/visibility_control.hpp b/common/autoware_auto_perception_rviz_plugin/include/visibility_control.hpp new file mode 100644 index 0000000000000..d14a1898a9b80 --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/include/visibility_control.hpp @@ -0,0 +1,43 @@ +// Copyright 2019 the Autoware 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef VISIBILITY_CONTROL_HPP_ +#define VISIBILITY_CONTROL_HPP_ + +#if defined(__WIN32) +#if defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || \ + defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllexport) +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL +// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || +// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#else +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllimport) +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL +// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || +// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#endif +#elif defined(__linux__) +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) +#else // defined(_LINUX) +#error "Unsupported Build Configuration" +#endif // defined(_WINDOWS) + +#endif // VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml new file mode 100644 index 0000000000000..24d48fe5e3ee4 --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -0,0 +1,26 @@ + + + + autoware_auto_perception_rviz_plugin + 1.0.0 + Contains plugins to visualize object detection outputs + Apex.AI, Inc. + Taichi Higashide + Apache 2.0 + + ament_cmake + + qtbase5-dev + + autoware_auto_perception_msgs + rviz_common + rviz_default_plugins + + libqt5-widgets + rviz2 + + ament_lint_auto + ament_lint_common + + ament_cmake + diff --git a/common/autoware_auto_perception_rviz_plugin/plugins_description.xml b/common/autoware_auto_perception_rviz_plugin/plugins_description.xml new file mode 100644 index 0000000000000..3f56a43558494 --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/plugins_description.xml @@ -0,0 +1,26 @@ + + + + + + + Convert a PredictedObjects to markers and display them. + + autoware_auto_perception_msgs/msg/PredictedObjects + + + + + Convert a TrackedObjects to markers and display them. + + autoware_auto_perception_msgs/msg/TrackedObjects + + + + + Convert a DetectedObjects to markers and display them. + + autoware_auto_perception_msgs/msg/DetectedObjects + + + diff --git a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp new file mode 100644 index 0000000000000..ebb3783845567 --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp @@ -0,0 +1,50 @@ +// Copyright 2021 Apex.AI, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace common +{ +ColorAlphaProperty::ColorAlphaProperty( + const QColor & color_default, const float alpha_default, + rviz_common::properties::Property * parent_property) +: m_color_property("Color", color_default, "Set color value.", parent_property), + m_alpha_property( + "Alpha", alpha_default, "Set transparency value. Should be between 0 and 1.", parent_property) +{ + m_alpha_property.setMax(1.0F); + m_alpha_property.setMin(0.0F); +} + +ColorAlphaProperty::operator std_msgs::msg::ColorRGBA() const +{ + std_msgs::msg::ColorRGBA ret; + ret.r = static_cast(m_color_property.getColor().redF()); + ret.g = static_cast(m_color_property.getColor().greenF()); + ret.b = static_cast(m_color_property.getColor().blueF()); + ret.a = m_alpha_property.getFloat(); + + return ret; +} + +} // namespace common +} // namespace rviz_plugins +} // namespace autoware diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp new file mode 100644 index 0000000000000..b3c645e8eff48 --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -0,0 +1,65 @@ +// Copyright 2021 Apex.AI, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace object_detection +{ +DetectedObjectsDisplay::DetectedObjectsDisplay() +: ObjectPolygonDisplayBase("detected_objects") {} + +void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) +{ + clear_markers(); + int id = 0; + for (const auto & object : msg->objects) { + // Get marker for shape + auto shape_marker = get_shape_marker_ptr( + object.shape, object.kinematics.pose_with_covariance.pose.position, + object.kinematics.pose_with_covariance.pose.orientation, object.classification); + if (shape_marker) { + auto shape_marker_ptr = shape_marker.value(); + shape_marker_ptr->header = msg->header; + shape_marker_ptr->id = id++; + add_marker(shape_marker_ptr); + } + + // Get marker for label + auto label_marker = get_label_marker_ptr( + object.kinematics.pose_with_covariance.pose.position, + object.kinematics.pose_with_covariance.pose.orientation, object.classification); + if (label_marker) { + auto label_marker_ptr = label_marker.value(); + label_marker_ptr->header = msg->header; + label_marker_ptr->id = id++; + add_marker(label_marker_ptr); + } + } +} + +} // namespace object_detection +} // namespace rviz_plugins +} // namespace autoware + +// Export the plugin +#include // NOLINT +PLUGINLIB_EXPORT_CLASS( + autoware::rviz_plugins::object_detection::DetectedObjectsDisplay, rviz_common::Display) diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp new file mode 100644 index 0000000000000..5f65fce8c72ea --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -0,0 +1,508 @@ +// Copyright 2021 Apex.AI, 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 +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace object_detection +{ +namespace detail +{ +using Marker = visualization_msgs::msg::Marker; + +visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const std_msgs::msg::ColorRGBA & path_confidence_color) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker_ptr->ns = std::string("path confidence"); + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->scale.x = 0.5; + marker_ptr->scale.y = 0.5; + marker_ptr->scale.z = 0.5; + marker_ptr->pose = initPose(); + marker_ptr->color = path_confidence_color; + marker_ptr->pose.position = predicted_path.path.back().position; + marker_ptr->text = std::to_string(predicted_path.confidence); + marker_ptr->color.a = std::max( + static_cast(std::min(static_cast(predicted_path.confidence), 0.999)), 0.5); + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const std_msgs::msg::ColorRGBA & predicted_path_color) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + marker_ptr->ns = std::string("path"); + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->pose = initPose(); + marker_ptr->color = predicted_path_color; + marker_ptr->color.a = std::max( + static_cast(std::min(static_cast(predicted_path.confidence), 0.999)), 0.5); + marker_ptr->scale.x = 0.03 * marker_ptr->color.a; + calc_path_line_list(predicted_path, marker_ptr->points); + for (size_t k = 0; k < marker_ptr->points.size(); ++k) { + marker_ptr->points.at(k).z -= shape.dimensions.z / 2.0; + } + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + marker_ptr->ns = std::string("twist"); + marker_ptr->scale.x = 0.03; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + + geometry_msgs::msg::Point pt_s; + pt_s.x = 0.0; + pt_s.y = 0.0; + pt_s.z = 0.0; + marker_ptr->points.push_back(pt_s); + + geometry_msgs::msg::Point pt_e; + pt_e.x = twist_with_covariance.twist.linear.x; + pt_e.y = twist_with_covariance.twist.linear.y; + pt_e.z = twist_with_covariance.twist.linear.z; + marker_ptr->points.push_back(pt_e); + + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->color.a = 0.999; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 0.0; + marker_ptr->color.b = 0.0; + + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( + const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, + const std_msgs::msg::ColorRGBA & color_rgba) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker_ptr->ns = std::string("velocity"); + marker_ptr->scale.x = 0.5; + marker_ptr->scale.z = 0.5; + + double vel = std::sqrt( + twist.linear.x * twist.linear.x + twist.linear.y * twist.linear.y + + twist.linear.z * twist.linear.z); + marker_ptr->text = std::to_string(static_cast(vel * 3.6)) + std::string("[km/h]"); + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose.position = vis_pos; + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->color = color_rgba; + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + marker_ptr->ns = std::string("position covariance"); + marker_ptr->scale.x = 0.03; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + marker_ptr->pose.orientation.x = 0.0; + marker_ptr->pose.orientation.y = 0.0; + marker_ptr->pose.orientation.z = 0.0; + marker_ptr->pose.orientation.w = 1.0; + geometry_msgs::msg::Point point; + Eigen::Matrix2d eigen_pose_with_covariance; + eigen_pose_with_covariance << pose_with_covariance.covariance[0], + pose_with_covariance.covariance[1], pose_with_covariance.covariance[6], + pose_with_covariance.covariance[7]; + Eigen::SelfAdjointEigenSolver solver(eigen_pose_with_covariance); + double sigma1 = 2.448 * std::sqrt(solver.eigenvalues().x()); // 2.448 sigma is 95% + double sigma2 = 2.448 * std::sqrt(solver.eigenvalues().y()); // 2.448 sigma is 95% + Eigen::Vector2d e1 = solver.eigenvectors().col(0); + Eigen::Vector2d e2 = solver.eigenvectors().col(1); + point.x = -e1.x() * sigma1; + point.y = -e1.y() * sigma1; + point.z = 0; + marker_ptr->points.push_back(point); + point.x = e1.x() * sigma1; + point.y = e1.y() * sigma1; + point.z = 0; + marker_ptr->points.push_back(point); + point.x = -e2.x() * sigma2; + point.y = -e2.y() * sigma2; + point.z = 0; + marker_ptr->points.push_back(point); + point.x = e2.x() * sigma2; + point.y = e2.y() * sigma2; + point.z = 0; + marker_ptr->points.push_back(point); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->color.a = 0.999; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 1.0; + marker_ptr->color.b = 1.0; + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_uuid_marker_ptr( + const std::string & uuid, const geometry_msgs::msg::Point & centroid, + const std_msgs::msg::ColorRGBA & color_rgba) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = Marker::TEXT_VIEW_FACING; + marker_ptr->ns = std::string("uuid"); + marker_ptr->text = uuid.substr(0, 4); + marker_ptr->action = Marker::MODIFY; + marker_ptr->scale.z = 0.5; + marker_ptr->color = color_rgba; + marker_ptr->pose.position = centroid; + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std::string label, const std_msgs::msg::ColorRGBA & color_rgba) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker_ptr->ns = std::string("label"); + marker_ptr->scale.x = 0.5; + marker_ptr->scale.z = 0.5; + marker_ptr->text = label; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->color = color_rgba; + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->ns = std::string("shape"); + + using autoware_auto_perception_msgs::msg::Shape; + if (shape_msg.type == Shape::BOUNDING_BOX) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_bounding_box_line_list(shape_msg, marker_ptr->points); + } else if (shape_msg.type == Shape::CYLINDER) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_cylinder_line_list(shape_msg, marker_ptr->points); + } else if (shape_msg.type == Shape::POLYGON) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_polygon_line_list(shape_msg, marker_ptr->points); + } else { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_polygon_line_list(shape_msg, marker_ptr->points); + } + + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = to_pose(centroid, orientation); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->scale.x = 0.03; + marker_ptr->color = color_rgba; + + return marker_ptr; +} + +void calc_bounding_box_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + geometry_msgs::msg::Point point; + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + // up surface + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + + // down surface + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); +} + +void calc_cylinder_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + const double radius = shape.dimensions.x * 0.5; + { + constexpr int n = 20; + geometry_msgs::msg::Point center; + center.x = 0.0; + center.y = 0.0; + center.z = shape.dimensions.z * 0.5; + calc_circle_line_list(center, radius, points, n); + center.z = -shape.dimensions.z * 0.5; + calc_circle_line_list(center, radius, points, n); + } + { + constexpr int n = 4; + for (int i = 0; i < n; ++i) { + geometry_msgs::msg::Point point; + point.x = std::cos( + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; + point.y = std::sin( + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; + point.z = shape.dimensions.z * 0.5; + points.push_back(point); + point.x = std::cos( + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; + point.y = std::sin( + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; + point.z = -shape.dimensions.z * 0.5; + points.push_back(point); + } + } +} + +void calc_circle_line_list( + const geometry_msgs::msg::Point center, const double radius, + std::vector & points, const int n) +{ + for (int i = 0; i < n; ++i) { + geometry_msgs::msg::Point point; + point.x = std::cos( + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.x; + point.y = std::sin( + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.y; + point.z = center.z; + points.push_back(point); + point.x = std::cos( + (static_cast(i + 1.0) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.x; + point.y = std::sin( + (static_cast(i + 1.0) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.y; + point.z = center.z; + points.push_back(point); + } +} + +void calc_polygon_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + if (shape.footprint.points.size() < 2) { + RCLCPP_WARN( + rclcpp::get_logger("ObjectPolygonDisplayBase"), + "there are no enough footprint to visualize polygon"); + return; + } + for (size_t i = 0; i < shape.footprint.points.size(); ++i) { + geometry_msgs::msg::Point point; + point.x = shape.footprint.points.at(i).x; + point.y = shape.footprint.points.at(i).y; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.footprint.points + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .x; + point.y = shape.footprint.points + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .y; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + } + for (size_t i = 0; i < shape.footprint.points.size(); ++i) { + geometry_msgs::msg::Point point; + point.x = shape.footprint.points.at(i).x; + point.y = shape.footprint.points.at(i).y; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.footprint.points + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .x; + point.y = shape.footprint.points + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .y; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + } + for (size_t i = 0; i < shape.footprint.points.size(); ++i) { + geometry_msgs::msg::Point point; + point.x = shape.footprint.points.at(i).x; + point.y = shape.footprint.points.at(i).y; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.footprint.points.at(i).x; + point.y = shape.footprint.points.at(i).y; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + } +} + +void calc_path_line_list( + const autoware_auto_perception_msgs::msg::PredictedPath & paths, + std::vector & points) +{ + for (int i = 0; i < static_cast(paths.path.size()) - 1; ++i) { + geometry_msgs::msg::Point point; + point.x = paths.path.at(i).position.x; + point.y = paths.path.at(i).position.y; + point.z = paths.path.at(i).position.z; + points.push_back(point); + point.x = paths.path.at(i + 1).position.x; + point.y = paths.path.at(i + 1).position.y; + point.z = paths.path.at(i + 1).position.z; + points.push_back(point); + calc_circle_line_list(point, 0.25, points, 10); + } +} + +} // namespace detail +} // namespace object_detection +} // namespace rviz_plugins +} // namespace autoware diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp new file mode 100644 index 0000000000000..9bb4e79f5f75d --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -0,0 +1,149 @@ +// Copyright 2020 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 +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace object_detection +{ +PredictedObjectsDisplay::PredictedObjectsDisplay() +: ObjectPolygonDisplayBase("tracks") {} + +void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr msg) +{ + clear_markers(); + update_id_map(msg); + + for (const auto & object : msg->objects) { + // Get marker for shape + auto shape_marker = get_shape_marker_ptr( + object.shape, object.kinematics.initial_pose_with_covariance.pose.position, + object.kinematics.initial_pose_with_covariance.pose.orientation, object.classification); + if (shape_marker) { + auto shape_marker_ptr = shape_marker.value(); + shape_marker_ptr->header = msg->header; + shape_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(shape_marker_ptr); + } + + // Get marker for label + auto label_marker = get_label_marker_ptr( + object.kinematics.initial_pose_with_covariance.pose.position, + object.kinematics.initial_pose_with_covariance.pose.orientation, object.classification); + if (label_marker) { + auto label_marker_ptr = label_marker.value(); + label_marker_ptr->header = msg->header; + label_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(label_marker_ptr); + } + + // Get marker for id + geometry_msgs::msg::Point uuid_vis_position; + uuid_vis_position.x = object.kinematics.initial_pose_with_covariance.pose.position.x - 0.5; + uuid_vis_position.y = object.kinematics.initial_pose_with_covariance.pose.position.y; + uuid_vis_position.z = object.kinematics.initial_pose_with_covariance.pose.position.z - 0.5; + + auto id_marker = + get_uuid_marker_ptr(object.object_id, uuid_vis_position, object.classification); + if (id_marker) { + auto id_marker_ptr = id_marker.value(); + id_marker_ptr->header = msg->header; + id_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(id_marker_ptr); + } + + // Get marker for pose with covariance + auto pose_with_covariance_marker = + get_pose_with_covariance_marker_ptr(object.kinematics.initial_pose_with_covariance); + if (pose_with_covariance_marker) { + auto pose_with_covariance_marker_ptr = pose_with_covariance_marker.value(); + pose_with_covariance_marker_ptr->header = msg->header; + pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(pose_with_covariance_marker_ptr); + } + + // Get marker for velocity text + geometry_msgs::msg::Point vel_vis_position; + vel_vis_position.x = uuid_vis_position.x - 0.5; + vel_vis_position.y = uuid_vis_position.y; + vel_vis_position.z = uuid_vis_position.z - 0.5; + auto velocity_text_marker = get_velocity_text_marker_ptr( + object.kinematics.initial_twist_with_covariance.twist, vel_vis_position, + object.classification); + if (velocity_text_marker) { + auto velocity_text_marker_ptr = velocity_text_marker.value(); + velocity_text_marker_ptr->header = msg->header; + velocity_text_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(velocity_text_marker_ptr); + } + + // Get marker for twist + auto twist_marker = get_twist_marker_ptr( + object.kinematics.initial_pose_with_covariance, + object.kinematics.initial_twist_with_covariance); + if (twist_marker) { + auto twist_marker_ptr = twist_marker.value(); + twist_marker_ptr->header = msg->header; + twist_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(twist_marker_ptr); + } + + // Add marker for each candidate path + int32_t path_count = 0; + for (const auto & predicted_path : object.kinematics.predicted_paths) { + // Get marker for predicted path + auto predicted_path_marker = + get_predicted_path_marker_ptr(object.object_id, object.shape, predicted_path); + if (predicted_path_marker) { + auto predicted_path_marker_ptr = predicted_path_marker.value(); + predicted_path_marker_ptr->header = msg->header; + predicted_path_marker_ptr->id = + uuid_to_marker_id(object.object_id) + path_count * PATH_ID_CONSTANT; + add_marker(predicted_path_marker_ptr); + path_count++; + } + } + + // Add confidence text marker for each candidate path + path_count = 0; + for (const auto & predicted_path : object.kinematics.predicted_paths) { + if (predicted_path.path.empty()) { + continue; + } + auto path_confidence_marker = + get_path_confidence_marker_ptr(object.object_id, predicted_path); + if (path_confidence_marker) { + auto path_confidence_marker_ptr = path_confidence_marker.value(); + path_confidence_marker_ptr->header = msg->header; + path_confidence_marker_ptr->id = + uuid_to_marker_id(object.object_id) + path_count * PATH_ID_CONSTANT; + add_marker(path_confidence_marker_ptr); + path_count++; + } + } + } +} + +} // namespace object_detection +} // namespace rviz_plugins +} // namespace autoware + +// Export the plugin +#include // NOLINT +PLUGINLIB_EXPORT_CLASS( + autoware::rviz_plugins::object_detection::PredictedObjectsDisplay, rviz_common::Display) diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp new file mode 100644 index 0000000000000..bbdcca607bf35 --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -0,0 +1,115 @@ +// Copyright 2021 Apex.AI, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace object_detection +{ +TrackedObjectsDisplay::TrackedObjectsDisplay() +: ObjectPolygonDisplayBase("tracks") {} + +void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) +{ + clear_markers(); + update_id_map(msg); + + for (const auto & object : msg->objects) { + // Get marker for shape + auto shape_marker = get_shape_marker_ptr( + object.shape, object.kinematics.pose_with_covariance.pose.position, + object.kinematics.pose_with_covariance.pose.orientation, object.classification); + if (shape_marker) { + auto shape_marker_ptr = shape_marker.value(); + shape_marker_ptr->header = msg->header; + shape_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(shape_marker_ptr); + } + + // Get marker for label + auto label_marker = get_label_marker_ptr( + object.kinematics.pose_with_covariance.pose.position, + object.kinematics.pose_with_covariance.pose.orientation, object.classification); + if (label_marker) { + auto label_marker_ptr = label_marker.value(); + label_marker_ptr->header = msg->header; + label_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(label_marker_ptr); + } + + // Get marker for id + geometry_msgs::msg::Point uuid_vis_position; + uuid_vis_position.x = object.kinematics.pose_with_covariance.pose.position.x - 0.5; + uuid_vis_position.y = object.kinematics.pose_with_covariance.pose.position.y; + uuid_vis_position.z = object.kinematics.pose_with_covariance.pose.position.z - 0.5; + + auto id_marker = + get_uuid_marker_ptr(object.object_id, uuid_vis_position, object.classification); + if (id_marker) { + auto id_marker_ptr = id_marker.value(); + id_marker_ptr->header = msg->header; + id_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(id_marker_ptr); + } + + // Get marker for pose with covariance + auto pose_with_covariance_marker = + get_pose_with_covariance_marker_ptr(object.kinematics.pose_with_covariance); + if (pose_with_covariance_marker) { + auto pose_with_covariance_marker_ptr = pose_with_covariance_marker.value(); + pose_with_covariance_marker_ptr->header = msg->header; + pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(pose_with_covariance_marker_ptr); + } + + // Get marker for velocity text + geometry_msgs::msg::Point vel_vis_position; + vel_vis_position.x = uuid_vis_position.x - 0.5; + vel_vis_position.y = uuid_vis_position.y; + vel_vis_position.z = uuid_vis_position.z - 0.5; + auto velocity_text_marker = get_velocity_text_marker_ptr( + object.kinematics.twist_with_covariance.twist, vel_vis_position, object.classification); + if (velocity_text_marker) { + auto velocity_text_marker_ptr = velocity_text_marker.value(); + velocity_text_marker_ptr->header = msg->header; + velocity_text_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(velocity_text_marker_ptr); + } + + // Get marker for twist + auto twist_marker = get_twist_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + if (twist_marker) { + auto twist_marker_ptr = twist_marker.value(); + twist_marker_ptr->header = msg->header; + twist_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(twist_marker_ptr); + } + } +} + +} // namespace object_detection +} // namespace rviz_plugins +} // namespace autoware + +// Export the plugin +#include // NOLINT +PLUGINLIB_EXPORT_CLASS( + autoware::rviz_plugins::object_detection::TrackedObjectsDisplay, rviz_common::Display) diff --git a/common/autoware_datetime_rviz_plugin/CMakeLists.txt b/common/autoware_datetime_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..2d9168b0f1843 --- /dev/null +++ b/common/autoware_datetime_rviz_plugin/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_datetime_rviz_plugin) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Qt5 ${rviz_QT_VERSION} EXACT 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(${PROJECT_NAME} SHARED + src/autoware_datetime_panel.hpp + src/autoware_datetime_panel.cpp +) +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/common/autoware_datetime_rviz_plugin/README.md b/common/autoware_datetime_rviz_plugin/README.md new file mode 100644 index 0000000000000..b8e6bf77a75b4 --- /dev/null +++ b/common/autoware_datetime_rviz_plugin/README.md @@ -0,0 +1,16 @@ +# autoware_datetime_rviz_plugin + +## Purpose + +This plugin displays the ROS Time and Wall Time in rviz. + +## Assumptions / Known limits + +TBD. + +## Usage + +1. Start rviz and select panels/Add new panel. + ![select_panel](./images/select_panels.png) +2. Select autoware_datetime_rviz_plugin/AutowareDateTimePanel and press OK. + ![select_datetime_plugin](./images/select_datetime_plugin.png) diff --git a/common/autoware_datetime_rviz_plugin/icons/classes/AutowareDateTimePanel.png b/common/autoware_datetime_rviz_plugin/icons/classes/AutowareDateTimePanel.png new file mode 100644 index 0000000000000..9431c2d422363 Binary files /dev/null and b/common/autoware_datetime_rviz_plugin/icons/classes/AutowareDateTimePanel.png differ diff --git a/common/autoware_datetime_rviz_plugin/images/select_datetime_plugin.png b/common/autoware_datetime_rviz_plugin/images/select_datetime_plugin.png new file mode 100644 index 0000000000000..6485b97379e72 Binary files /dev/null and b/common/autoware_datetime_rviz_plugin/images/select_datetime_plugin.png differ diff --git a/common/autoware_datetime_rviz_plugin/images/select_panels.png b/common/autoware_datetime_rviz_plugin/images/select_panels.png new file mode 100644 index 0000000000000..a691602c42c3c Binary files /dev/null and b/common/autoware_datetime_rviz_plugin/images/select_panels.png differ diff --git a/common/autoware_datetime_rviz_plugin/package.xml b/common/autoware_datetime_rviz_plugin/package.xml new file mode 100644 index 0000000000000..747e7a0204553 --- /dev/null +++ b/common/autoware_datetime_rviz_plugin/package.xml @@ -0,0 +1,26 @@ + + + + autoware_datetime_rviz_plugin + 0.0.0 + The autoware_datetime_rviz_plugin package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + + diff --git a/common/autoware_datetime_rviz_plugin/plugins/plugin_description.xml b/common/autoware_datetime_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..98ed9bb7b0242 --- /dev/null +++ b/common/autoware_datetime_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + AutowareDateTimePanel + + + diff --git a/common/autoware_datetime_rviz_plugin/src/autoware_datetime_panel.cpp b/common/autoware_datetime_rviz_plugin/src/autoware_datetime_panel.cpp new file mode 100644 index 0000000000000..e2c6be4d00a60 --- /dev/null +++ b/common/autoware_datetime_rviz_plugin/src/autoware_datetime_panel.cpp @@ -0,0 +1,66 @@ +// Copyright 2021 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 "autoware_datetime_panel.hpp" + +#include +#include +#include +#include +#include + +#include + +void setFormatTime(QLineEdit * line, double time) +{ + char buffer[128]; + time_t seconds = static_cast(time); + strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", localtime(&seconds)); + line->setText(QString(buffer) + QString::number((time - seconds), 'f', 3).rightRef(4)); +} + +AutowareDateTimePanel::AutowareDateTimePanel(QWidget * parent) : rviz_common::Panel(parent) +{ + ros_time_label_ = new QLineEdit; + ros_time_label_->setReadOnly(true); + + wall_time_label_ = new QLineEdit; + wall_time_label_->setReadOnly(true); + + QHBoxLayout * layout = new QHBoxLayout(this); + layout->addWidget(new QLabel("ROS Time:")); + layout->addWidget(ros_time_label_); + layout->addWidget(new QLabel("Wall Time:")); + layout->addWidget(wall_time_label_); + setLayout(layout); + + QTimer * timer = new QTimer(this); + connect(timer, &QTimer::timeout, this, &AutowareDateTimePanel::update); + timer->start(60); +} + +void AutowareDateTimePanel::onInitialize() +{ + rviz_ros_node_ = getDisplayContext()->getRosNodeAbstraction(); +} + +void AutowareDateTimePanel::update() +{ + setFormatTime( + ros_time_label_, rviz_ros_node_.lock()->get_raw_node()->get_clock()->now().seconds()); + setFormatTime(wall_time_label_, rclcpp::Clock().now().seconds()); +} + +#include +PLUGINLIB_EXPORT_CLASS(AutowareDateTimePanel, rviz_common::Panel) diff --git a/common/autoware_datetime_rviz_plugin/src/autoware_datetime_panel.hpp b/common/autoware_datetime_rviz_plugin/src/autoware_datetime_panel.hpp new file mode 100644 index 0000000000000..d1f7087efae27 --- /dev/null +++ b/common/autoware_datetime_rviz_plugin/src/autoware_datetime_panel.hpp @@ -0,0 +1,41 @@ +// Copyright 2021 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 AUTOWARE_DATETIME_PANEL_HPP_ +#define AUTOWARE_DATETIME_PANEL_HPP_ + +#include +#include +#include + +class QLineEdit; + +class AutowareDateTimePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit AutowareDateTimePanel(QWidget * parent = nullptr); + void update(); + void onInitialize() override; + +private: + QLineEdit * ros_time_label_; + QLineEdit * wall_time_label_; + +protected: + rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node_; +}; + +#endif // AUTOWARE_DATETIME_PANEL_HPP_ diff --git a/common/autoware_localization_rviz_plugin/CMakeLists.txt b/common/autoware_localization_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..0b1854c34d7d3 --- /dev/null +++ b/common/autoware_localization_rviz_plugin/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_localization_rviz_plugin) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Qt5 ${rviz_QT_VERSION} EXACT 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(${PROJECT_NAME} SHARED + src/pose_history/pose_history_display.hpp + src/pose_history/pose_history_display.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/common/autoware_localization_rviz_plugin/README.md b/common/autoware_localization_rviz_plugin/README.md new file mode 100644 index 0000000000000..d26c0e71a5b82 --- /dev/null +++ b/common/autoware_localization_rviz_plugin/README.md @@ -0,0 +1,38 @@ +# autoware_localization_rviz_plugin + +## Purpose + +This plugin can display the history of the localization obtained by ekf_localizer or ndt_scan_matching. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------ | --------------------------------- | ---------------------------------------------------------------------------------------------- | +| `input/pose` | `geometry_msgs::msg::PoseStamped` | In input/pose, put the result of localization calculated by ekf_localizer or ndt_scan_matching | + +## Parameters + +### Core Parameters + +| Name | Type | Default Value | Description | +| ----------------------- | ------ | ------------- | -------------------------- | +| `property_buffer_size_` | int | 100 | Buffer size of topic | +| `property_line_view_` | bool | true | Use Line property or not | +| `property_line_width_` | float | 0.1 | Width of Line property [m] | +| `property_line_alpha_` | float | 1.0 | Alpha of Line property | +| `property_line_color_` | QColor | Qt::white | Color of Line property | + +## Assumptions / Known limits + +TBD. + +## Usage + +1. Start rviz and select Add under the Displays panel. + ![select_add](./images/select_add.png) +2. Select autoware_localization_rviz_plugin/PoseHistory and press OK. + ![select_localization_plugin](./images/select_localization_plugin.png) +3. Enter the name of the topic where you want to view the trajectory. + ![select_topic_name](./images/select_topic_name.png) diff --git a/common/autoware_localization_rviz_plugin/icons/classes/PoseHistory.png b/common/autoware_localization_rviz_plugin/icons/classes/PoseHistory.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_localization_rviz_plugin/icons/classes/PoseHistory.png differ diff --git a/common/autoware_localization_rviz_plugin/images/select_add.png b/common/autoware_localization_rviz_plugin/images/select_add.png new file mode 100644 index 0000000000000..c5130b6092384 Binary files /dev/null and b/common/autoware_localization_rviz_plugin/images/select_add.png differ diff --git a/common/autoware_localization_rviz_plugin/images/select_localization_plugin.png b/common/autoware_localization_rviz_plugin/images/select_localization_plugin.png new file mode 100644 index 0000000000000..0646c77bde24a Binary files /dev/null and b/common/autoware_localization_rviz_plugin/images/select_localization_plugin.png differ diff --git a/common/autoware_localization_rviz_plugin/images/select_topic_name.png b/common/autoware_localization_rviz_plugin/images/select_topic_name.png new file mode 100644 index 0000000000000..ed9ebc752f7e6 Binary files /dev/null and b/common/autoware_localization_rviz_plugin/images/select_topic_name.png differ diff --git a/common/autoware_localization_rviz_plugin/package.xml b/common/autoware_localization_rviz_plugin/package.xml new file mode 100644 index 0000000000000..e3283062da819 --- /dev/null +++ b/common/autoware_localization_rviz_plugin/package.xml @@ -0,0 +1,29 @@ + + + + autoware_localization_rviz_plugin + 0.1.0 + The autoware_localization_rviz_plugin package + Apache License 2.0 + + Isamu Takagi + + ament_cmake_auto + geometry_msgs + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + rviz_default_plugins + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + + diff --git a/common/autoware_localization_rviz_plugin/plugins/plugin_description.xml b/common/autoware_localization_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..c06af3364eb2e --- /dev/null +++ b/common/autoware_localization_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + Display pose history + + + diff --git a/common/autoware_localization_rviz_plugin/src/pose_history/pose_history_display.cpp b/common/autoware_localization_rviz_plugin/src/pose_history/pose_history_display.cpp new file mode 100644 index 0000000000000..f31eb833419fb --- /dev/null +++ b/common/autoware_localization_rviz_plugin/src/pose_history/pose_history_display.cpp @@ -0,0 +1,141 @@ +// Copyright 2020 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 "pose_history_display.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace rviz_plugins +{ +PoseHistory::PoseHistory() : last_stamp_(0, 0, RCL_ROS_TIME) +{ + property_buffer_size_ = new rviz_common::properties::IntProperty("Buffer Size", 100, "", this); + property_line_view_ = new rviz_common::properties::BoolProperty("Line", true, "", this); + property_line_width_ = + new rviz_common::properties::FloatProperty("Width", 0.1, "", property_line_view_); + property_line_alpha_ = + new rviz_common::properties::FloatProperty("Alpha", 1.0, "", property_line_view_); + property_line_alpha_->setMin(0.0); + property_line_alpha_->setMax(1.0); + property_line_color_ = + new rviz_common::properties::ColorProperty("Color", Qt::white, "", property_line_view_); + + property_buffer_size_->setMin(0); + property_buffer_size_->setMax(16000); + property_line_width_->setMin(0.0); +} + +PoseHistory::~PoseHistory() +{ + // Properties are deleted by Qt +} + +void PoseHistory::onInitialize() +{ + MFDClass::onInitialize(); + lines_.reset(new rviz_rendering::BillboardLine(scene_manager_, scene_node_)); +} + +void PoseHistory::onEnable() { subscribe(); } + +void PoseHistory::onDisable() { unsubscribe(); } + +void PoseHistory::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + if (!history_.empty()) { + lines_->clear(); + if (property_line_view_->getBool()) { + updateLines(); + } + } +} + +void PoseHistory::subscribe() { MFDClass::subscribe(); } + +void PoseHistory::unsubscribe() +{ + MFDClass::unsubscribe(); + + history_.clear(); + lines_->clear(); +} + +void PoseHistory::processMessage(const geometry_msgs::msg::PoseStamped::ConstSharedPtr message) +{ + if (!rviz_common::validateFloats(message->pose)) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + if (target_frame_ != message->header.frame_id) { + history_.clear(); + target_frame_ = message->header.frame_id; + } + history_.emplace_back(message); + last_stamp_ = message->header.stamp; + + updateHistory(); +} + +void PoseHistory::updateHistory() +{ + const auto buffer_size = static_cast(property_buffer_size_->getInt()); + while (buffer_size < history_.size()) { + history_.pop_front(); + } +} + +void PoseHistory::updateLines() +{ + Ogre::ColourValue color = rviz_common::properties::qtToOgre(property_line_color_->getColor()); + color.a = property_line_alpha_->getFloat(); + Ogre::Vector3 position; + Ogre::Quaternion orientation; + + auto frame_manager = context_->getFrameManager(); + if (!frame_manager->getTransform(target_frame_, last_stamp_, position, orientation)) { + setMissingTransformToFixedFrame(target_frame_); + return; + } + + setTransformOk(); + lines_->setMaxPointsPerLine(history_.size()); + lines_->setLineWidth(property_line_width_->getFloat()); + lines_->setPosition(position); + lines_->setOrientation(orientation); + lines_->setColor(color.r, color.g, color.b, color.a); + + for (const auto & message : history_) { + Ogre::Vector3 point; + point.x = message->pose.position.x; + point.y = message->pose.position.y; + point.z = message->pose.position.z; + lines_->addPoint(point); + } +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::PoseHistory, rviz_common::Display) diff --git a/common/autoware_localization_rviz_plugin/src/pose_history/pose_history_display.hpp b/common/autoware_localization_rviz_plugin/src/pose_history/pose_history_display.hpp new file mode 100644 index 0000000000000..0dfc666c64dea --- /dev/null +++ b/common/autoware_localization_rviz_plugin/src/pose_history/pose_history_display.hpp @@ -0,0 +1,81 @@ +// Copyright 2020 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 POSE_HISTORY__POSE_HISTORY_DISPLAY_HPP_ +#define POSE_HISTORY__POSE_HISTORY_DISPLAY_HPP_ + +#include + +#include + +#include +#include +#include + +namespace rviz_rendering +{ +class BillboardLine; +} // namespace rviz_rendering +namespace rviz_common +{ +namespace properties +{ +class ColorProperty; +class FloatProperty; +class IntProperty; +class BoolProperty; +} // namespace properties +} // namespace rviz_common + +namespace rviz_plugins +{ +class PoseHistory : public rviz_common::MessageFilterDisplay +{ + Q_OBJECT + +public: + PoseHistory(); + ~PoseHistory() override; + +protected: + void onInitialize() override; + void onEnable() override; + void onDisable() override; + void update(float wall_dt, float ros_dt) override; + +private Q_SLOTS: + void subscribe(); + void unsubscribe(); + void processMessage(const geometry_msgs::msg::PoseStamped::ConstSharedPtr message); + +private: + void updateHistory(); + void updateLines(); + +private: + std::string target_frame_; + std::deque history_; + std::unique_ptr lines_; + rclcpp::Time last_stamp_; + + rviz_common::properties::IntProperty * property_buffer_size_; + rviz_common::properties::BoolProperty * property_line_view_; + rviz_common::properties::FloatProperty * property_line_width_; + rviz_common::properties::FloatProperty * property_line_alpha_; + rviz_common::properties::ColorProperty * property_line_color_; +}; + +} // namespace rviz_plugins + +#endif // POSE_HISTORY__POSE_HISTORY_DISPLAY_HPP_ diff --git a/common/autoware_perception_rviz_plugin/CMakeLists.txt b/common/autoware_perception_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..186ca20c1efa1 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_perception_rviz_plugin) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) + +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +## Declare a C++ library +ament_auto_add_library(autoware_perception_rviz_plugin SHARED + src/tools/pedestrian_pose.cpp + src/tools/car_pose.cpp + src/tools/unknown_pose.cpp + src/tools/delete_all_objects.cpp +) + +target_link_libraries(autoware_perception_rviz_plugin + ${QT_LIBRARIES}) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/common/autoware_perception_rviz_plugin/README.md b/common/autoware_perception_rviz_plugin/README.md new file mode 100644 index 0000000000000..792f3ec4cc459 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/README.md @@ -0,0 +1,79 @@ +# autoware_perception_rviz_plugin + +## Purpose + +This plugin is used to generate dummy pedestrians, cars, and obstacles in planning simulator. + +## Overview + +The CarInitialPoseTool sends a topic for generating a dummy car. +The PedestrianInitialPoseTool sends a topic for generating a dummy pedestrian. +The UnknownInitialPoseTool sends a topic for generating a dummy obstacle. +The DeleteAllObjectsTool deletes the dummy cars, pedestrians, and obstacles displayed by the above three tools. + +## Inputs / Outputs + +### Output + +| Name | Type | Description | +| ---------------------------------------------------- | ----------------------------------------- | ----------------------------------------------- | +| `/simulation/dummy_perception_publisher/object_info` | `dummy_perception_publisher::msg::Object` | The topic on which to publish dummy object info | + +## Parameter + +### Core Parameters + +#### CarPose + +| Name | Type | Default Value | Description | +| ----------------- | ------ | ---------------------------------------------------- | ----------------------------------------------- | +| `topic_property_` | string | `/simulation/dummy_perception_publisher/object_info` | The topic on which to publish dummy object info | +| `std_dev_x_` | float | 0.03 | X standard deviation for initial pose [m] | +| `std_dev_y_` | float | 0.03 | Y standard deviation for initial pose [m] | +| `std_dev_z_` | float | 0.03 | Z standard deviation for initial pose [m] | +| `std_dev_theta_` | float | 5.0 \* M_PI / 180.0 | Theta standard deviation for initial pose [rad] | +| `position_z_` | float | 0.0 | Z position for initial pose [m] | +| `velocity_` | float | 0.0 | Velocity [m/s] | + +#### PedestrianPose + +| Name | Type | Default Value | Description | +| ----------------- | ------ | ---------------------------------------------------- | ----------------------------------------------- | +| `topic_property_` | string | `/simulation/dummy_perception_publisher/object_info` | The topic on which to publish dummy object info | +| `std_dev_x_` | float | 0.03 | X standard deviation for initial pose [m] | +| `std_dev_y_` | float | 0.03 | Y standard deviation for initial pose [m] | +| `std_dev_z_` | float | 0.03 | Z standard deviation for initial pose [m] | +| `std_dev_theta_` | float | 5.0 \* M_PI / 180.0 | Theta standard deviation for initial pose [rad] | +| `position_z_` | float | 0.0 | Z position for initial pose [m] | +| `velocity_` | float | 0.0 | Velocity [m/s] | + +#### UnknownPose + +| Name | Type | Default Value | Description | +| ----------------- | ------ | ---------------------------------------------------- | ----------------------------------------------- | +| `topic_property_` | string | `/simulation/dummy_perception_publisher/object_info` | The topic on which to publish dummy object info | +| `std_dev_x_` | float | 0.03 | X standard deviation for initial pose [m] | +| `std_dev_y_` | float | 0.03 | Y standard deviation for initial pose [m] | +| `std_dev_z_` | float | 0.03 | Z standard deviation for initial pose [m] | +| `std_dev_theta_` | float | 5.0 \* M_PI / 180.0 | Theta standard deviation for initial pose [rad] | +| `position_z_` | float | 0.0 | Z position for initial pose [m] | +| `velocity_` | float | 0.0 | Velocity [m/s] | + +#### DeleteAllObjects + +| Name | Type | Default Value | Description | +| ----------------- | ------ | ---------------------------------------------------- | ----------------------------------------------- | +| `topic_property_` | string | `/simulation/dummy_perception_publisher/object_info` | The topic on which to publish dummy object info | + +## Assumptions / Known limits + +Using a planning simulator + +## Usage + +1. Start rviz and select + on the tool tab. + ![select_add](./images/select_add.png) +2. Select one of the following: autoware_perception_rviz_plugin and press OK. + ![select_plugin](./images/select_plugin.png) +3. Select the new item in the tool tab (2D Dummy Car in the example) and click on it in rviz. + ![select_dummy_car](./images/select_dummy_car.png) diff --git a/common/autoware_perception_rviz_plugin/icons/classes/CarInitialPoseTool.png b/common/autoware_perception_rviz_plugin/icons/classes/CarInitialPoseTool.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_perception_rviz_plugin/icons/classes/CarInitialPoseTool.png differ diff --git a/common/autoware_perception_rviz_plugin/icons/classes/DeleteAllObjectsTool.png b/common/autoware_perception_rviz_plugin/icons/classes/DeleteAllObjectsTool.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_perception_rviz_plugin/icons/classes/DeleteAllObjectsTool.png differ diff --git a/common/autoware_perception_rviz_plugin/icons/classes/PedestrianInitialPoseTool.png b/common/autoware_perception_rviz_plugin/icons/classes/PedestrianInitialPoseTool.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_perception_rviz_plugin/icons/classes/PedestrianInitialPoseTool.png differ diff --git a/common/autoware_perception_rviz_plugin/icons/classes/UnknownInitialPoseTool.png b/common/autoware_perception_rviz_plugin/icons/classes/UnknownInitialPoseTool.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_perception_rviz_plugin/icons/classes/UnknownInitialPoseTool.png differ diff --git a/common/autoware_perception_rviz_plugin/images/select_add.png b/common/autoware_perception_rviz_plugin/images/select_add.png new file mode 100644 index 0000000000000..f4b73e874b230 Binary files /dev/null and b/common/autoware_perception_rviz_plugin/images/select_add.png differ diff --git a/common/autoware_perception_rviz_plugin/images/select_dummy_car.png b/common/autoware_perception_rviz_plugin/images/select_dummy_car.png new file mode 100644 index 0000000000000..6a08a2b37ee16 Binary files /dev/null and b/common/autoware_perception_rviz_plugin/images/select_dummy_car.png differ diff --git a/common/autoware_perception_rviz_plugin/images/select_plugin.png b/common/autoware_perception_rviz_plugin/images/select_plugin.png new file mode 100644 index 0000000000000..387dc6fa962ce Binary files /dev/null and b/common/autoware_perception_rviz_plugin/images/select_plugin.png differ diff --git a/common/autoware_perception_rviz_plugin/package.xml b/common/autoware_perception_rviz_plugin/package.xml new file mode 100644 index 0000000000000..a57aab8a314b2 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/package.xml @@ -0,0 +1,29 @@ + + + autoware_perception_rviz_plugin + 0.1.0 + The autoware_perception_rviz_plugin package + + Yukihiro Saito + Apache License 2.0 + + ament_cmake + + dummy_perception_publisher + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rviz_common + rviz_default_plugins + tf2_geometry_msgs + tf2_ros + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/autoware_perception_rviz_plugin/plugins/plugin_description.xml b/common/autoware_perception_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..fcbbd27f65eb6 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,18 @@ + + + + + + + + + + diff --git a/common/autoware_perception_rviz_plugin/src/tools/car_pose.cpp b/common/autoware_perception_rviz_plugin/src/tools/car_pose.cpp new file mode 100644 index 0000000000000..75038d85619de --- /dev/null +++ b/common/autoware_perception_rviz_plugin/src/tools/car_pose.cpp @@ -0,0 +1,166 @@ +// Copyright 2020 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "car_pose.hpp" + +#include + +#include +#include + +#include +#include +#include + +namespace rviz_plugins +{ +CarInitialPoseTool::CarInitialPoseTool() +{ + shortcut_key_ = 'k'; + + topic_property_ = new rviz_common::properties::StringProperty( + "Pose Topic", "/simulation/dummy_perception_publisher/object_info", + "The topic on which to publish dummy object info.", getPropertyContainer(), SLOT(updateTopic()), + this); + std_dev_x_ = new rviz_common::properties::FloatProperty( + "X std deviation", 0.03, "X standard deviation for initial pose [m]", getPropertyContainer()); + std_dev_y_ = new rviz_common::properties::FloatProperty( + "Y std deviation", 0.03, "Y standard deviation for initial pose [m]", getPropertyContainer()); + std_dev_z_ = new rviz_common::properties::FloatProperty( + "Z std deviation", 0.03, "Z standard deviation for initial pose [m]", getPropertyContainer()); + std_dev_theta_ = new rviz_common::properties::FloatProperty( + "Theta std deviation", 5.0 * M_PI / 180.0, "Theta standard deviation for initial pose [rad]", + getPropertyContainer()); + position_z_ = new rviz_common::properties::FloatProperty( + "Z position", 0.0, "Z position for initial pose [m]", getPropertyContainer()); + velocity_ = new rviz_common::properties::FloatProperty( + "Velocity", 0.0, "velocity [m/s]", getPropertyContainer()); + std_dev_x_->setMin(0); + std_dev_y_->setMin(0); + std_dev_z_->setMin(0); + std_dev_theta_->setMin(0); + position_z_->setMin(0); +} + +void CarInitialPoseTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("2D Dummy Car"); + updateTopic(); +} + +void CarInitialPoseTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + dummy_object_info_pub_ = raw_node->create_publisher( + topic_property_->getStdString(), 1); + clock_ = raw_node->get_clock(); +} + +void CarInitialPoseTool::onPoseSet(double x, double y, double theta) +{ + dummy_perception_publisher::msg::Object output_msg; + std::string fixed_frame = context_->getFixedFrame().toStdString(); + + // header + output_msg.header.frame_id = fixed_frame; + output_msg.header.stamp = clock_->now(); + + // semantic + output_msg.classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + output_msg.classification.probability = 1.0; + + // shape + output_msg.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + const double width = 1.8; + const double length = 4.0; + output_msg.shape.dimensions.x = length; + output_msg.shape.dimensions.y = width; + output_msg.shape.dimensions.z = 2.0; + + // initial state + // pose + output_msg.initial_state.pose_covariance.pose.position.x = x; + output_msg.initial_state.pose_covariance.pose.position.y = y; + output_msg.initial_state.pose_covariance.pose.position.z = position_z_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[0] = + std_dev_x_->getFloat() * std_dev_x_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[7] = + std_dev_y_->getFloat() * std_dev_y_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[14] = + std_dev_z_->getFloat() * std_dev_z_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[35] = + std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + output_msg.initial_state.pose_covariance.pose.orientation = tf2::toMsg(quat); + RCLCPP_INFO( + rclcpp::get_logger("CarInitialPoseTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", x, y, + position_z_->getFloat(), theta, fixed_frame.c_str()); + // twist + output_msg.initial_state.twist_covariance.twist.linear.x = velocity_->getFloat(); + output_msg.initial_state.twist_covariance.twist.linear.y = 0.0; + output_msg.initial_state.twist_covariance.twist.linear.z = 0.0; + RCLCPP_INFO( + rclcpp::get_logger("CarInitialPoseTool"), "Setting twist: %.3f %.3f %.3f [frame=%s]", + velocity_->getFloat(), 0.0, 0.0, fixed_frame.c_str()); + + // action + output_msg.action = dummy_perception_publisher::msg::Object::ADD; + + // id + std::mt19937 gen(std::random_device{}()); + std::independent_bits_engine bit_eng(gen); + std::generate(output_msg.id.uuid.begin(), output_msg.id.uuid.end(), bit_eng); + + dummy_object_info_pub_->publish(output_msg); +} + +} // end namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::CarInitialPoseTool, rviz_common::Tool) diff --git a/common/autoware_perception_rviz_plugin/src/tools/car_pose.hpp b/common/autoware_perception_rviz_plugin/src/tools/car_pose.hpp new file mode 100644 index 0000000000000..adc3a671676bd --- /dev/null +++ b/common/autoware_perception_rviz_plugin/src/tools/car_pose.hpp @@ -0,0 +1,94 @@ +// Copyright 2020 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TOOLS__CAR_POSE_HPP_ +#define TOOLS__CAR_POSE_HPP_ + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include +#include +#include +#include +#endif + +#include + +namespace rviz_plugins +{ +class CarInitialPoseTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + CarInitialPoseTool(); + virtual ~CarInitialPoseTool() {} + virtual void onInitialize(); + +protected: + virtual void onPoseSet(double x, double y, double theta); + +private Q_SLOTS: + void updateTopic(); + +private: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr dummy_object_info_pub_; + + rviz_common::properties::StringProperty * topic_property_; + rviz_common::properties::FloatProperty * std_dev_x_; + rviz_common::properties::FloatProperty * std_dev_y_; + rviz_common::properties::FloatProperty * std_dev_z_; + rviz_common::properties::FloatProperty * std_dev_theta_; + rviz_common::properties::FloatProperty * position_z_; + rviz_common::properties::FloatProperty * velocity_; +}; + +} // namespace rviz_plugins + +#endif // TOOLS__CAR_POSE_HPP_ diff --git a/common/autoware_perception_rviz_plugin/src/tools/delete_all_objects.cpp b/common/autoware_perception_rviz_plugin/src/tools/delete_all_objects.cpp new file mode 100644 index 0000000000000..40984facf4459 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/src/tools/delete_all_objects.cpp @@ -0,0 +1,100 @@ +// Copyright 2020 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "delete_all_objects.hpp" + +#include + +#include + +namespace rviz_plugins +{ +DeleteAllObjectsTool::DeleteAllObjectsTool() +{ + shortcut_key_ = 'd'; + + topic_property_ = new rviz_common::properties::StringProperty( + "Pose Topic", "/simulation/dummy_perception_publisher/object_info", + "The topic on which to publish dummy object info.", getPropertyContainer(), SLOT(updateTopic()), + this); +} + +void DeleteAllObjectsTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("Delete All Objects"); + updateTopic(); +} + +void DeleteAllObjectsTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + dummy_object_info_pub_ = raw_node->create_publisher( + topic_property_->getStdString(), 1); + clock_ = raw_node->get_clock(); +} + +void DeleteAllObjectsTool::onPoseSet( + [[maybe_unused]] double x, [[maybe_unused]] double y, [[maybe_unused]] double theta) +{ + dummy_perception_publisher::msg::Object output_msg; + std::string fixed_frame = context_->getFixedFrame().toStdString(); + + // header + output_msg.header.frame_id = fixed_frame; + output_msg.header.stamp = clock_->now(); + + // action + output_msg.action = dummy_perception_publisher::msg::Object::DELETEALL; + + dummy_object_info_pub_->publish(output_msg); +} + +} // end namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::DeleteAllObjectsTool, rviz_common::Tool) diff --git a/common/autoware_perception_rviz_plugin/src/tools/delete_all_objects.hpp b/common/autoware_perception_rviz_plugin/src/tools/delete_all_objects.hpp new file mode 100644 index 0000000000000..4c559445208da --- /dev/null +++ b/common/autoware_perception_rviz_plugin/src/tools/delete_all_objects.hpp @@ -0,0 +1,88 @@ +// Copyright 2020 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TOOLS__DELETE_ALL_OBJECTS_HPP_ +#define TOOLS__DELETE_ALL_OBJECTS_HPP_ + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include +#include +#include +#include +#endif + +#include + +namespace rviz_plugins +{ +class DeleteAllObjectsTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + DeleteAllObjectsTool(); + virtual ~DeleteAllObjectsTool() {} + virtual void onInitialize(); + +protected: + virtual void onPoseSet(double x, double y, double theta); + +private Q_SLOTS: + void updateTopic(); + +private: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr dummy_object_info_pub_; + + rviz_common::properties::StringProperty * topic_property_; +}; + +} // namespace rviz_plugins + +#endif // TOOLS__DELETE_ALL_OBJECTS_HPP_ diff --git a/common/autoware_perception_rviz_plugin/src/tools/pedestrian_pose.cpp b/common/autoware_perception_rviz_plugin/src/tools/pedestrian_pose.cpp new file mode 100644 index 0000000000000..85b91aeb64b41 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/src/tools/pedestrian_pose.cpp @@ -0,0 +1,171 @@ +// Copyright 2020 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "pedestrian_pose.hpp" + +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include + +namespace rviz_plugins +{ +PedestrianInitialPoseTool::PedestrianInitialPoseTool() +{ + shortcut_key_ = 'l'; + + topic_property_ = new rviz_common::properties::StringProperty( + "Pose Topic", "/simulation/dummy_perception_publisher/object_info", + "The topic on which to publish dummy object info.", getPropertyContainer(), SLOT(updateTopic()), + this); + std_dev_x_ = new rviz_common::properties::FloatProperty( + "X std deviation", 0.03, "X standard deviation for initial pose [m]", getPropertyContainer()); + std_dev_y_ = new rviz_common::properties::FloatProperty( + "Y std deviation", 0.03, "Y standard deviation for initial pose [m]", getPropertyContainer()); + std_dev_z_ = new rviz_common::properties::FloatProperty( + "Z std deviation", 0.03, "Z standard deviation for initial pose [m]", getPropertyContainer()); + std_dev_theta_ = new rviz_common::properties::FloatProperty( + "Theta std deviation", 5.0 * M_PI / 180.0, "Theta standard deviation for initial pose [rad]", + getPropertyContainer()); + position_z_ = new rviz_common::properties::FloatProperty( + "Z position", 0.0, "Z position for initial pose [m]", getPropertyContainer()); + velocity_ = new rviz_common::properties::FloatProperty( + "Velocity", 0.0, "velocity [m/s]", getPropertyContainer()); + std_dev_x_->setMin(0); + std_dev_y_->setMin(0); + std_dev_z_->setMin(0); + std_dev_theta_->setMin(0); + position_z_->setMin(0); +} + +void PedestrianInitialPoseTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("2D Dummy Pedestrian"); + updateTopic(); +} + +void PedestrianInitialPoseTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + dummy_object_info_pub_ = raw_node->create_publisher( + topic_property_->getStdString(), 1); + clock_ = raw_node->get_clock(); +} + +void PedestrianInitialPoseTool::onPoseSet(double x, double y, double theta) +{ + dummy_perception_publisher::msg::Object output_msg; + std::string fixed_frame = context_->getFixedFrame().toStdString(); + + // header + output_msg.header.frame_id = fixed_frame; + output_msg.header.stamp = clock_->now(); + + // semantic + output_msg.classification.label = + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + output_msg.classification.probability = 1.0; + + // shape + output_msg.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + const double width = 0.8; + const double length = 0.8; + output_msg.shape.dimensions.x = length; + output_msg.shape.dimensions.y = width; + output_msg.shape.dimensions.z = 2.0; + + // initial state + // pose + output_msg.initial_state.pose_covariance.pose.position.x = x; + output_msg.initial_state.pose_covariance.pose.position.y = y; + output_msg.initial_state.pose_covariance.pose.position.z = position_z_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[0] = + std_dev_x_->getFloat() * std_dev_x_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[7] = + std_dev_y_->getFloat() * std_dev_y_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[14] = + std_dev_z_->getFloat() * std_dev_z_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[35] = + std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + output_msg.initial_state.pose_covariance.pose.orientation = tf2::toMsg(quat); + RCLCPP_INFO( + rclcpp::get_logger("PedestrianInitialPoseTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", + x, y, position_z_->getFloat(), theta, fixed_frame.c_str()); + // twist + output_msg.initial_state.twist_covariance.twist.linear.x = velocity_->getFloat(); + output_msg.initial_state.twist_covariance.twist.linear.y = 0.0; + output_msg.initial_state.twist_covariance.twist.linear.z = 0.0; + RCLCPP_INFO( + rclcpp::get_logger("PedestrianInitialPoseTool"), "Setting twist: %.3f %.3f %.3f [frame=%s]", + velocity_->getFloat(), 0.0, 0.0, fixed_frame.c_str()); + + // action + output_msg.action = dummy_perception_publisher::msg::Object::ADD; + + // id + std::mt19937 gen(std::random_device{}()); + std::independent_bits_engine bit_eng(gen); + std::generate(output_msg.id.uuid.begin(), output_msg.id.uuid.end(), bit_eng); + + dummy_object_info_pub_->publish(output_msg); +} + +} // end namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::PedestrianInitialPoseTool, rviz_common::Tool) diff --git a/common/autoware_perception_rviz_plugin/src/tools/pedestrian_pose.hpp b/common/autoware_perception_rviz_plugin/src/tools/pedestrian_pose.hpp new file mode 100644 index 0000000000000..c463dbceeefd1 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/src/tools/pedestrian_pose.hpp @@ -0,0 +1,93 @@ +// Copyright 2020 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TOOLS__PEDESTRIAN_POSE_HPP_ +#define TOOLS__PEDESTRIAN_POSE_HPP_ + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include +#include +#include +#endif + +#include + +namespace rviz_plugins +{ +class PedestrianInitialPoseTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + PedestrianInitialPoseTool(); + virtual ~PedestrianInitialPoseTool() {} + virtual void onInitialize(); + +protected: + virtual void onPoseSet(double x, double y, double theta); + +private Q_SLOTS: + void updateTopic(); + +private: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr dummy_object_info_pub_; + + rviz_common::properties::StringProperty * topic_property_; + rviz_common::properties::FloatProperty * std_dev_x_; + rviz_common::properties::FloatProperty * std_dev_y_; + rviz_common::properties::FloatProperty * std_dev_z_; + rviz_common::properties::FloatProperty * std_dev_theta_; + rviz_common::properties::FloatProperty * position_z_; + rviz_common::properties::FloatProperty * velocity_; +}; + +} // namespace rviz_plugins + +#endif // TOOLS__PEDESTRIAN_POSE_HPP_ diff --git a/common/autoware_perception_rviz_plugin/src/tools/unknown_pose.cpp b/common/autoware_perception_rviz_plugin/src/tools/unknown_pose.cpp new file mode 100644 index 0000000000000..c88e631f361c0 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/src/tools/unknown_pose.cpp @@ -0,0 +1,166 @@ +// Copyright 2021 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. +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "unknown_pose.hpp" + +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include + +namespace rviz_plugins +{ +UnknownInitialPoseTool::UnknownInitialPoseTool() +{ + shortcut_key_ = 'u'; + + topic_property_ = new rviz_common::properties::StringProperty( + "Pose Topic", "/simulation/dummy_perception_publisher/object_info", + "The topic on which to publish dummy object info.", getPropertyContainer(), SLOT(updateTopic()), + this); + std_dev_x_ = new rviz_common::properties::FloatProperty( + "X std deviation", 0.03, "X standard deviation for initial pose [m]", getPropertyContainer()); + std_dev_y_ = new rviz_common::properties::FloatProperty( + "Y std deviation", 0.03, "Y standard deviation for initial pose [m]", getPropertyContainer()); + std_dev_z_ = new rviz_common::properties::FloatProperty( + "Z std deviation", 0.03, "Z standard deviation for initial pose [m]", getPropertyContainer()); + std_dev_theta_ = new rviz_common::properties::FloatProperty( + "Theta std deviation", 5.0 * M_PI / 180.0, "Theta standard deviation for initial pose [rad]", + getPropertyContainer()); + position_z_ = new rviz_common::properties::FloatProperty( + "Z position", 0.0, "Z position for initial pose [m]", getPropertyContainer()); + velocity_ = new rviz_common::properties::FloatProperty( + "Velocity", 0.0, "velocity [m/s]", getPropertyContainer()); + std_dev_x_->setMin(0); + std_dev_y_->setMin(0); + std_dev_z_->setMin(0); + std_dev_theta_->setMin(0); + position_z_->setMin(0); +} + +void UnknownInitialPoseTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("2D Dummy Unknown"); + updateTopic(); +} + +void UnknownInitialPoseTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + dummy_object_info_pub_ = raw_node->create_publisher( + topic_property_->getStdString(), 1); + clock_ = raw_node->get_clock(); +} + +void UnknownInitialPoseTool::onPoseSet(double x, double y, double theta) +{ + dummy_perception_publisher::msg::Object output_msg; + std::string fixed_frame = context_->getFixedFrame().toStdString(); + + // header + output_msg.header.frame_id = fixed_frame; + output_msg.header.stamp = clock_->now(); + + // semantic + output_msg.classification.label = + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + output_msg.classification.probability = 1.0; + + // shape + output_msg.shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + const double width = 0.8; + const double length = 0.8; + output_msg.shape.dimensions.x = length; + output_msg.shape.dimensions.y = width; + output_msg.shape.dimensions.z = 2.0; + + // initial state + // pose + output_msg.initial_state.pose_covariance.pose.position.x = x; + output_msg.initial_state.pose_covariance.pose.position.y = y; + output_msg.initial_state.pose_covariance.pose.position.z = position_z_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[0] = + std_dev_x_->getFloat() * std_dev_x_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[7] = + std_dev_y_->getFloat() * std_dev_y_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[14] = + std_dev_z_->getFloat() * std_dev_z_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[35] = + std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + output_msg.initial_state.pose_covariance.pose.orientation = tf2::toMsg(quat); + RCLCPP_INFO( + rclcpp::get_logger("PedestrianInitialPoseTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", + x, y, position_z_->getFloat(), theta, fixed_frame.c_str()); + // twist + output_msg.initial_state.twist_covariance.twist.linear.x = velocity_->getFloat(); + output_msg.initial_state.twist_covariance.twist.linear.y = 0.0; + output_msg.initial_state.twist_covariance.twist.linear.z = 0.0; + RCLCPP_INFO( + rclcpp::get_logger("PedestrianInitialPoseTool"), "Setting twist: %.3f %.3f %.3f [frame=%s]", + velocity_->getFloat(), 0.0, 0.0, fixed_frame.c_str()); + + // action + output_msg.action = dummy_perception_publisher::msg::Object::ADD; + + // id + std::mt19937 gen(std::random_device{}()); + std::independent_bits_engine bit_eng(gen); + std::generate(output_msg.id.uuid.begin(), output_msg.id.uuid.end(), bit_eng); + + dummy_object_info_pub_->publish(output_msg); +} + +} // end namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::UnknownInitialPoseTool, rviz_common::Tool) diff --git a/common/autoware_perception_rviz_plugin/src/tools/unknown_pose.hpp b/common/autoware_perception_rviz_plugin/src/tools/unknown_pose.hpp new file mode 100644 index 0000000000000..bdc3d40becbe9 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/src/tools/unknown_pose.hpp @@ -0,0 +1,88 @@ +// Copyright 2021 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. +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TOOLS__UNKNOWN_POSE_HPP_ +#define TOOLS__UNKNOWN_POSE_HPP_ + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include +#include +#include +#endif + +#include + +namespace rviz_plugins +{ +class UnknownInitialPoseTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + UnknownInitialPoseTool(); + virtual ~UnknownInitialPoseTool() {} + virtual void onInitialize(); + +protected: + virtual void onPoseSet(double x, double y, double theta); + +private Q_SLOTS: + void updateTopic(); + +private: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr dummy_object_info_pub_; + + rviz_common::properties::StringProperty * topic_property_; + rviz_common::properties::FloatProperty * std_dev_x_; + rviz_common::properties::FloatProperty * std_dev_y_; + rviz_common::properties::FloatProperty * std_dev_z_; + rviz_common::properties::FloatProperty * std_dev_theta_; + rviz_common::properties::FloatProperty * position_z_; + rviz_common::properties::FloatProperty * velocity_; +}; + +} // namespace rviz_plugins + +#endif // TOOLS__UNKNOWN_POSE_HPP_ diff --git a/common/autoware_planning_rviz_plugin/CMakeLists.txt b/common/autoware_planning_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..73d932ae94336 --- /dev/null +++ b/common/autoware_planning_rviz_plugin/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_planning_rviz_plugin) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) +find_package(Eigen3 REQUIRED) + +ament_auto_add_library(autoware_planning_rviz_plugin SHARED + include/path/display.hpp + src/path/display.cpp + include/path_footprint/display.hpp + src/path_footprint/display.cpp + include/trajectory/display.hpp + src/trajectory/display.cpp + include/trajectory_footprint/display.hpp + src/trajectory_footprint/display.cpp + include/mission_checkpoint/mission_checkpoint.hpp + src/mission_checkpoint/mission_checkpoint.cpp + src/tools/jsk_overlay_utils.cpp + src/tools/max_velocity.cpp +) + +target_link_libraries(autoware_planning_rviz_plugin + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/common/autoware_planning_rviz_plugin/README.md b/common/autoware_planning_rviz_plugin/README.md new file mode 100644 index 0000000000000..12520cc9a8bb4 --- /dev/null +++ b/common/autoware_planning_rviz_plugin/README.md @@ -0,0 +1,118 @@ +# autoware_planning_rviz_plugin + +This package is including jsk code. +Note that jsk_overlay_utils.cpp and jsk_overlay_utils.hpp are BSD license. + +## Purpose + +This plugin displays the path, trajectory, and maximum speed. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------------------------------------- | ---------------------------------------------- | ------------------------------------------ | +| `/input/path` | `autoware_auto_planning_msgs::msg::Path` | The topic on which to subscribe path | +| `/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | The topic on which to subscribe trajectory | +| `/planning/scenario_planning/current_max_velocity` | `autoware_planning_msgs/msg/VelocityLimit` | The topic on which to publish max velocity | + +### Output + +| Name | Type | Description | +| --------------------------------------- | ------------------------------- | ---------------------------------------- | +| `/planning/mission_planning/checkpoint` | `geometry_msgs/msg/PoseStamped` | The topic on which to publish checkpoint | + +## Parameter + +### Core Parameters + +#### MissionCheckpoint + +| Name 　 | Type | Default Value | Description | +| ------------------------ | ------ | -------------------- | -------------------------------------------------- | +| `pose_topic_property_`　 | string | `mission_checkpoint` | The topic on which to publish checkpoint | +| `std_dev_x_` 　 | float | 0.5 | X standard deviation for checkpoint pose [m] | +| `std_dev_y_` 　 | float | 0.5 | Y standard deviation for checkpoint pose [m] | +| `std_dev_theta_` 　 | float | M_PI / 12.0 | Theta standard deviation for checkpoint pose [rad] | +| `position_z_` 　 | float | 0.0 | Z position for checkpoint pose [m] | + +#### Path + +| Name | Type | Default Value | Description | +| ------------------------------- | ------ | ------------- | ---------------------------- | +| `property_path_view_` | bool | true | Use Path property or not | +| `property_path_width_` | float | 2.0 | Width of Path property [m] | +| `property_path_alpha_` | float | 1.0 | Alpha of Path property | +| `property_path_color_view_` | bool | false | Use Constant Color or not | +| `property_path_color_` | QColor | Qt::black | Color of Path property | +| `property_velocity_view_` | bool | true | Use Velocity property or not | +| `property_velocity_alpha_` | float | 1.0 | Alpha of Velocity property | +| `property_velocity_scale_` | float | 0.3 | Scale of Velocity property | +| `property_velocity_color_view_` | bool | false | Use Constant Color or not | +| `property_velocity_color_` | QColor | Qt::black | Color of Velocity property | +| `property_vel_max_` | float | 3.0 | Max velocity [m/s] | + +#### PathFootprint + +| Name | Type | Default Value | Description | +| -------------------------------- | ------ | ------------- | ---------------------------------- | +| `property_path_footprint_view_` | bool | true | Use Path Footprint property or not | +| `property_path_footprint_alpha_` | float | 1.0 | Alpha of Path Footprint property | +| `property_path_footprint_color_` | QColor | Qt::black | Color of Path Footprint property | +| `property_vehicle_length_` | float | 4.77 | Vehicle length [m] | +| `property_vehicle_width_` | float | 1.83 | Vehicle width [m] | +| `property_rear_overhang_` | float | 1.03 | Rear overhang [m] | + +#### Trajectory + +| Name | Type | Default Value | Description | +| ------------------------------- | ------ | ------------- | ---------------------------- | +| `property_path_view_` | bool | true | Use Path property or not | +| `property_path_width_` | float | 2.0 | Width of Path property [m] | +| `property_path_alpha_` | float | 1.0 | Alpha of Path property | +| `property_path_color_view_` | bool | false | Use Constant Color or not | +| `property_path_color_` | QColor | Qt::black | Color of Path property | +| `property_velocity_view_` | bool | true | Use Velocity property or not | +| `property_velocity_alpha_` | float | 1.0 | Alpha of Velocity property | +| `property_velocity_scale_` | float | 0.3 | Scale of Velocity property | +| `property_velocity_color_view_` | bool | false | Use Constant Color or not | +| `property_velocity_color_` | QColor | Qt::black | Color of Velocity property | +| `property_velocity_text_view_` | bool | false | View text Velocity | +| `property_velocity_text_scale_` | float | 0.3 | Scale of Velocity property | +| `property_vel_max_` | float | 3.0 | Max velocity [m/s] | + +#### TrajectoryFootprint + +| Name | Type | Default Value | Description | +| -------------------------------------- | ------ | -------------------- | ---------------------------------------- | +| `property_trajectory_footprint_view_` | bool | true | Use Trajectory Footprint property or not | +| `property_trajectory_footprint_alpha_` | float | 1.0 | Alpha of Trajectory Footprint property | +| `property_trajectory_footprint_color_` | QColor | QColor(230, 230, 50) | Color of Trajectory Footprint property | +| `property_vehicle_length_` | float | 4.77 | Vehicle length [m] | +| `property_vehicle_width_` | float | 1.83 | Vehicle width [m] | +| `property_rear_overhang_` | float | 1.03 | Rear overhang [m] | +| `property_trajectory_point_view_` | bool | false | Use Trajectory Point property or not | +| `property_trajectory_point_alpha_` | float | 1.0 | Alpha of Trajectory Point property | +| `property_trajectory_point_color_` | QColor | QColor(0, 60, 255) | Color of Trajectory Point property | +| `property_trajectory_point_radius_` | float | 0.1 | Radius of Trajectory Point property | + +#### MaxVelocity + +| Name | Type | Default Value | Description | +| ----------------------- | ------ | -------------------------------------------------- | -------------------------------------------- | +| `property_topic_name_` | string | `/planning/scenario_planning/current_max_velocity` | The topic on which to subscribe max velocity | +| `property_text_color_` | QColor | QColor(255, 255, 255) | Text color | +| `property_left_` | int | 128 | Left of the plotter window [px] | +| `property_top_` | int | 128 | Top of the plotter window [px] | +| `property_length_` | int | 96 | Length of the plotter window [px] | +| `property_value_scale_` | float | 1.0 / 4.0 | Value scale | + +## Usage + +1. Start rviz and select Add under the Displays panel. + ![select_add](./images/select_add.png) +2. Select any one of the autoware_planning_rviz_plugin and press OK. + ![select_planning_plugin](./images/select_planning_plugin.png) +3. Enter the name of the topic where you want to view the path or trajectory. + ![select_topic_name](./images/select_topic_name.png) diff --git a/common/autoware_planning_rviz_plugin/icons/classes/MaxVelocity.png b/common/autoware_planning_rviz_plugin/icons/classes/MaxVelocity.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_planning_rviz_plugin/icons/classes/MaxVelocity.png differ diff --git a/common/autoware_planning_rviz_plugin/icons/classes/MissionCheckpointTool.png b/common/autoware_planning_rviz_plugin/icons/classes/MissionCheckpointTool.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_planning_rviz_plugin/icons/classes/MissionCheckpointTool.png differ diff --git a/common/autoware_planning_rviz_plugin/icons/classes/Path.png b/common/autoware_planning_rviz_plugin/icons/classes/Path.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_planning_rviz_plugin/icons/classes/Path.png differ diff --git a/common/autoware_planning_rviz_plugin/icons/classes/PathFootprint.png b/common/autoware_planning_rviz_plugin/icons/classes/PathFootprint.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_planning_rviz_plugin/icons/classes/PathFootprint.png differ diff --git a/common/autoware_planning_rviz_plugin/icons/classes/Trajectory.png b/common/autoware_planning_rviz_plugin/icons/classes/Trajectory.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_planning_rviz_plugin/icons/classes/Trajectory.png differ diff --git a/common/autoware_planning_rviz_plugin/icons/classes/TrajectoryFootprint.png b/common/autoware_planning_rviz_plugin/icons/classes/TrajectoryFootprint.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_planning_rviz_plugin/icons/classes/TrajectoryFootprint.png differ diff --git a/common/autoware_planning_rviz_plugin/images/select_add.png b/common/autoware_planning_rviz_plugin/images/select_add.png new file mode 100644 index 0000000000000..c5130b6092384 Binary files /dev/null and b/common/autoware_planning_rviz_plugin/images/select_add.png differ diff --git a/common/autoware_planning_rviz_plugin/images/select_planning_plugin.png b/common/autoware_planning_rviz_plugin/images/select_planning_plugin.png new file mode 100644 index 0000000000000..b4043353de498 Binary files /dev/null and b/common/autoware_planning_rviz_plugin/images/select_planning_plugin.png differ diff --git a/common/autoware_planning_rviz_plugin/images/select_topic_name.png b/common/autoware_planning_rviz_plugin/images/select_topic_name.png new file mode 100644 index 0000000000000..0b14dc78617b9 Binary files /dev/null and b/common/autoware_planning_rviz_plugin/images/select_topic_name.png differ diff --git a/common/autoware_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp b/common/autoware_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp new file mode 100644 index 0000000000000..9fbc08fc339a8 --- /dev/null +++ b/common/autoware_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp @@ -0,0 +1,93 @@ +// Copyright 2020 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ +#define MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include +#include +#include +#include +#endif + +#include + +namespace rviz_plugins +{ +class MissionCheckpointTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + MissionCheckpointTool(); + virtual ~MissionCheckpointTool() {} + virtual void onInitialize(); + +protected: + virtual void onPoseSet(double x, double y, double theta); + +private Q_SLOTS: + void updateTopic(); + +private: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr pose_pub_; + + rviz_common::properties::StringProperty * pose_topic_property_; + rviz_common::properties::StringProperty * twist_topic_property_; + rviz_common::properties::FloatProperty * std_dev_x_; + rviz_common::properties::FloatProperty * std_dev_y_; + rviz_common::properties::FloatProperty * std_dev_theta_; + rviz_common::properties::FloatProperty * position_z_; +}; + +} // namespace rviz_plugins + +#endif // MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ diff --git a/common/autoware_planning_rviz_plugin/include/path/display.hpp b/common/autoware_planning_rviz_plugin/include/path/display.hpp new file mode 100644 index 0000000000000..2d7aeba5c7cfb --- /dev/null +++ b/common/autoware_planning_rviz_plugin/include/path/display.hpp @@ -0,0 +1,84 @@ +// Copyright 2020 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 PATH__DISPLAY_HPP_ +#define PATH__DISPLAY_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace rviz_plugins +{ +class AutowarePathDisplay +: public rviz_common::MessageFilterDisplay +{ + Q_OBJECT + +public: + AutowarePathDisplay(); + virtual ~AutowarePathDisplay(); + + void onInitialize() override; + void reset() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void processMessage( + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) override; + std::unique_ptr setColorDependsOnVelocity( + const double vel_max, const double cmd_vel); + std::unique_ptr gradation( + const QColor & color_min, const QColor & color_max, const double ratio); + Ogre::ManualObject * path_manual_object_; + Ogre::ManualObject * velocity_manual_object_; + rviz_common::properties::BoolProperty * property_path_view_; + rviz_common::properties::BoolProperty * property_velocity_view_; + rviz_common::properties::FloatProperty * property_path_width_; + rviz_common::properties::ColorProperty * property_path_color_; + rviz_common::properties::ColorProperty * property_velocity_color_; + rviz_common::properties::FloatProperty * property_path_alpha_; + rviz_common::properties::FloatProperty * property_velocity_alpha_; + rviz_common::properties::FloatProperty * property_velocity_scale_; + rviz_common::properties::BoolProperty * property_path_color_view_; + rviz_common::properties::BoolProperty * property_velocity_color_view_; + rviz_common::properties::FloatProperty * property_vel_max_; + +private: + autoware_auto_planning_msgs::msg::Path::ConstSharedPtr last_msg_ptr_; + bool validateFloats(const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & msg_ptr); +}; + +} // namespace rviz_plugins + +#endif // PATH__DISPLAY_HPP_ diff --git a/common/autoware_planning_rviz_plugin/include/path_footprint/display.hpp b/common/autoware_planning_rviz_plugin/include/path_footprint/display.hpp new file mode 100644 index 0000000000000..78f87bd6ec030 --- /dev/null +++ b/common/autoware_planning_rviz_plugin/include/path_footprint/display.hpp @@ -0,0 +1,84 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 PATH_FOOTPRINT__DISPLAY_HPP_ +#define PATH_FOOTPRINT__DISPLAY_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include + +namespace rviz_plugins +{ +class AutowarePathFootprintDisplay +: public rviz_common::MessageFilterDisplay +{ + Q_OBJECT + +public: + AutowarePathFootprintDisplay(); + virtual ~AutowarePathFootprintDisplay(); + + void onInitialize() override; + void reset() override; + +private Q_SLOTS: + void updateVisualization(); + void updateVehicleInfo(); + +protected: + void processMessage( + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) override; + Ogre::ManualObject * path_footprint_manual_object_; + rviz_common::properties::BoolProperty * property_path_footprint_view_; + rviz_common::properties::ColorProperty * property_path_footprint_color_; + rviz_common::properties::FloatProperty * property_path_footprint_alpha_; + rviz_common::properties::FloatProperty * property_vehicle_length_; + rviz_common::properties::FloatProperty * property_vehicle_width_; + rviz_common::properties::FloatProperty * property_rear_overhang_; + + struct VehicleFootprintInfo + { + VehicleFootprintInfo(const float l, const float w, const float r) + : length(l), width(w), rear_overhang(r) + { + } + float length, width, rear_overhang; + }; + std::shared_ptr vehicle_footprint_info_; + +private: + autoware_auto_planning_msgs::msg::Path::ConstSharedPtr last_msg_ptr_; + bool validateFloats(const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & msg_ptr); +}; + +} // namespace rviz_plugins + +#endif // PATH_FOOTPRINT__DISPLAY_HPP_ diff --git a/common/autoware_planning_rviz_plugin/include/trajectory/display.hpp b/common/autoware_planning_rviz_plugin/include/trajectory/display.hpp new file mode 100644 index 0000000000000..428774e1d3b90 --- /dev/null +++ b/common/autoware_planning_rviz_plugin/include/trajectory/display.hpp @@ -0,0 +1,90 @@ +// Copyright 2020 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 TRAJECTORY__DISPLAY_HPP_ +#define TRAJECTORY__DISPLAY_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace rviz_plugins +{ +class AutowareTrajectoryDisplay +: public rviz_common::MessageFilterDisplay +{ + Q_OBJECT + +public: + AutowareTrajectoryDisplay(); + virtual ~AutowareTrajectoryDisplay(); + + void onInitialize() override; + void reset() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void processMessage( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) override; + std::unique_ptr setColorDependsOnVelocity( + const double vel_max, const double cmd_vel); + std::unique_ptr gradation( + const QColor & color_min, const QColor & color_max, const double ratio); + Ogre::ManualObject * path_manual_object_; + Ogre::ManualObject * velocity_manual_object_; + std::vector velocity_texts_; + std::vector velocity_text_nodes_; + rviz_common::properties::BoolProperty * property_path_view_; + rviz_common::properties::BoolProperty * property_velocity_view_; + rviz_common::properties::FloatProperty * property_path_width_; + rviz_common::properties::ColorProperty * property_path_color_; + rviz_common::properties::ColorProperty * property_velocity_color_; + rviz_common::properties::FloatProperty * property_velocity_scale_; + rviz_common::properties::BoolProperty * property_velocity_text_view_; + rviz_common::properties::FloatProperty * property_velocity_text_scale_; + rviz_common::properties::FloatProperty * property_path_alpha_; + rviz_common::properties::FloatProperty * property_velocity_alpha_; + rviz_common::properties::BoolProperty * property_path_color_view_; + rviz_common::properties::BoolProperty * property_velocity_color_view_; + rviz_common::properties::FloatProperty * property_vel_max_; + +private: + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr last_msg_ptr_; + bool validateFloats(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & msg_ptr); +}; + +} // namespace rviz_plugins + +#endif // TRAJECTORY__DISPLAY_HPP_ diff --git a/common/autoware_planning_rviz_plugin/include/trajectory_footprint/display.hpp b/common/autoware_planning_rviz_plugin/include/trajectory_footprint/display.hpp new file mode 100644 index 0000000000000..5c2002160d832 --- /dev/null +++ b/common/autoware_planning_rviz_plugin/include/trajectory_footprint/display.hpp @@ -0,0 +1,91 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 TRAJECTORY_FOOTPRINT__DISPLAY_HPP_ +#define TRAJECTORY_FOOTPRINT__DISPLAY_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include + +namespace rviz_plugins +{ +class AutowareTrajectoryFootprintDisplay +: public rviz_common::MessageFilterDisplay +{ + Q_OBJECT + +public: + AutowareTrajectoryFootprintDisplay(); + virtual ~AutowareTrajectoryFootprintDisplay(); + + void onInitialize() override; + void reset() override; + +private Q_SLOTS: + void updateVisualization(); + void updateVehicleInfo(); + +protected: + void processMessage( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) override; + Ogre::ManualObject * trajectory_footprint_manual_object_; + rviz_common::properties::BoolProperty * property_trajectory_footprint_view_; + rviz_common::properties::ColorProperty * property_trajectory_footprint_color_; + rviz_common::properties::FloatProperty * property_trajectory_footprint_alpha_; + rviz_common::properties::FloatProperty * property_vehicle_length_; + rviz_common::properties::FloatProperty * property_vehicle_width_; + rviz_common::properties::FloatProperty * property_rear_overhang_; + + Ogre::ManualObject * trajectory_point_manual_object_; + rviz_common::properties::BoolProperty * property_trajectory_point_view_; + rviz_common::properties::ColorProperty * property_trajectory_point_color_; + rviz_common::properties::FloatProperty * property_trajectory_point_alpha_; + rviz_common::properties::FloatProperty * property_trajectory_point_radius_; + rviz_common::properties::FloatProperty * property_trajectory_point_offset_; + + struct VehicleFootprintInfo + { + VehicleFootprintInfo(const float l, const float w, const float r) + : length(l), width(w), rear_overhang(r) + { + } + float length, width, rear_overhang; + }; + std::shared_ptr vehicle_footprint_info_; + +private: + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr last_msg_ptr_; + bool validateFloats(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & msg_ptr); +}; + +} // namespace rviz_plugins + +#endif // TRAJECTORY_FOOTPRINT__DISPLAY_HPP_ diff --git a/common/autoware_planning_rviz_plugin/package.xml b/common/autoware_planning_rviz_plugin/package.xml new file mode 100644 index 0000000000000..d8e3a10e4e160 --- /dev/null +++ b/common/autoware_planning_rviz_plugin/package.xml @@ -0,0 +1,32 @@ + + + + autoware_planning_rviz_plugin + 0.1.0 + The autoware_planning_rviz_plugin package + Yukihiro Saito + Apache License 2.0 + + ament_cmake + autoware_auto_planning_msgs + autoware_planning_msgs + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + rviz_default_plugins + rviz_rendering + tf2_geometry_msgs + tf2_ros + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + + diff --git a/common/autoware_planning_rviz_plugin/plugins/plugin_description.xml b/common/autoware_planning_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..63f07bd9e562e --- /dev/null +++ b/common/autoware_planning_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,31 @@ + + + Display autoware_planning_msg::Path + + + Display autoware_planning_msg::PathFootprint + + + Display autoware_planning_msg::Trajectory + + + Display autoware_planning_msg::TrajectoryFootprint + + + Display external max velocity limit + + + + diff --git a/common/autoware_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp b/common/autoware_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp new file mode 100644 index 0000000000000..a3678158c32aa --- /dev/null +++ b/common/autoware_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp @@ -0,0 +1,117 @@ +// Copyright 2020 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include +#include + +#include + +namespace rviz_plugins +{ +MissionCheckpointTool::MissionCheckpointTool() +{ + shortcut_key_ = 'c'; + + pose_topic_property_ = new rviz_common::properties::StringProperty( + "Pose Topic", "mission_checkpoint", "The topic on which to publish checkpoint.", + getPropertyContainer(), SLOT(updateTopic()), this); + std_dev_x_ = new rviz_common::properties::FloatProperty( + "X std deviation", 0.5, "X standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_y_ = new rviz_common::properties::FloatProperty( + "Y std deviation", 0.5, "Y standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_theta_ = new rviz_common::properties::FloatProperty( + "Theta std deviation", M_PI / 12.0, "Theta standard deviation for checkpoint pose [rad]", + getPropertyContainer()); + position_z_ = new rviz_common::properties::FloatProperty( + "Z position", 0.0, "Z position for checkpoint pose [m]", getPropertyContainer()); + std_dev_x_->setMin(0); + std_dev_y_->setMin(0); + std_dev_theta_->setMin(0); + position_z_->setMin(0); +} + +void MissionCheckpointTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("2D Checkpoint Pose"); + updateTopic(); +} + +void MissionCheckpointTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + pose_pub_ = raw_node->create_publisher( + pose_topic_property_->getStdString(), 1); + clock_ = raw_node->get_clock(); +} + +void MissionCheckpointTool::onPoseSet(double x, double y, double theta) +{ + // pose + std::string fixed_frame = context_->getFixedFrame().toStdString(); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = fixed_frame; + pose.header.stamp = clock_->now(); + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = position_z_->getFloat(); + + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + pose.pose.orientation = tf2::toMsg(quat); + RCLCPP_INFO( + rclcpp::get_logger("MissionCheckpointTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", x, + y, position_z_->getFloat(), theta, fixed_frame.c_str()); + pose_pub_->publish(pose); +} + +} // end namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::MissionCheckpointTool, rviz_common::Tool) diff --git a/common/autoware_planning_rviz_plugin/src/path/display.cpp b/common/autoware_planning_rviz_plugin/src/path/display.cpp new file mode 100644 index 0000000000000..ee4b95dc8e1af --- /dev/null +++ b/common/autoware_planning_rviz_plugin/src/path/display.cpp @@ -0,0 +1,254 @@ +// Copyright 2020 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 + +#include +#define EIGEN_MPL2_ONLY +#include +#include + +namespace rviz_plugins +{ +std::unique_ptr AutowarePathDisplay::gradation( + const QColor & color_min, const QColor & color_max, const double ratio) +{ + std::unique_ptr color_ptr(new Ogre::ColourValue); + color_ptr->g = color_max.greenF() * ratio + color_min.greenF() * (1.0 - ratio); + color_ptr->r = color_max.redF() * ratio + color_min.redF() * (1.0 - ratio); + color_ptr->b = color_max.blueF() * ratio + color_min.blueF() * (1.0 - ratio); + + return color_ptr; +} + +std::unique_ptr AutowarePathDisplay::setColorDependsOnVelocity( + const double vel_max, const double cmd_vel) +{ + const double cmd_vel_abs = std::fabs(cmd_vel); + const double vel_min = 0.0; + + std::unique_ptr color_ptr(new Ogre::ColourValue()); + if (vel_min < cmd_vel_abs && cmd_vel_abs <= (vel_max / 2.0)) { + double ratio = (cmd_vel_abs - vel_min) / (vel_max / 2.0 - vel_min); + color_ptr = gradation(Qt::red, Qt::yellow, ratio); + } else if ((vel_max / 2.0) < cmd_vel_abs && cmd_vel_abs <= vel_max) { + double ratio = (cmd_vel_abs - vel_max / 2.0) / (vel_max - vel_max / 2.0); + color_ptr = gradation(Qt::yellow, Qt::green, ratio); + } else if (vel_max < cmd_vel_abs) { + *color_ptr = Ogre::ColourValue::Green; + } else { + *color_ptr = Ogre::ColourValue::Red; + } + + return color_ptr; +} + +AutowarePathDisplay::AutowarePathDisplay() +{ + property_path_view_ = new rviz_common::properties::BoolProperty( + "View Path", true, "", this, SLOT(updateVisualization())); + property_path_width_ = new rviz_common::properties::FloatProperty( + "Width", 2.0, "", property_path_view_, SLOT(updateVisualization()), this); + property_path_width_->setMin(0.0); + property_path_alpha_ = new rviz_common::properties::FloatProperty( + "Alpha", 1.0, "", property_path_view_, SLOT(updateVisualization()), this); + property_path_alpha_->setMin(0.0); + property_path_alpha_->setMax(1.0); + property_path_color_view_ = new rviz_common::properties::BoolProperty( + "Constant Color", false, "", property_path_view_, SLOT(updateVisualization()), this); + property_path_color_ = new rviz_common::properties::ColorProperty( + "Color", Qt::black, "", property_path_view_, SLOT(updateVisualization()), this); + + property_velocity_view_ = new rviz_common::properties::BoolProperty( + "View Velocity", true, "", this, SLOT(updateVisualization()), this); + property_velocity_alpha_ = new rviz_common::properties::FloatProperty( + "Alpha", 1.0, "", property_velocity_view_, SLOT(updateVisualization()), this); + property_velocity_alpha_->setMin(0.0); + property_velocity_alpha_->setMax(1.0); + property_velocity_scale_ = new rviz_common::properties::FloatProperty( + "Scale", 0.3, "", property_velocity_view_, SLOT(updateVisualization()), this); + property_velocity_scale_->setMin(0.1); + property_velocity_scale_->setMax(10.0); + property_velocity_color_view_ = new rviz_common::properties::BoolProperty( + "Constant Color", false, "", property_velocity_view_, SLOT(updateVisualization()), this); + property_velocity_color_ = new rviz_common::properties::ColorProperty( + "Color", Qt::black, "", property_velocity_view_, SLOT(updateVisualization()), this); + + property_vel_max_ = new rviz_common::properties::FloatProperty( + "Color Border Vel Max", 3.0, "[m/s]", this, SLOT(updateVisualization()), this); + property_vel_max_->setMin(0.0); +} + +AutowarePathDisplay::~AutowarePathDisplay() +{ + if (initialized()) { + scene_manager_->destroyManualObject(path_manual_object_); + scene_manager_->destroyManualObject(velocity_manual_object_); + } +} + +void AutowarePathDisplay::onInitialize() +{ + MFDClass::onInitialize(); + + path_manual_object_ = scene_manager_->createManualObject(); + velocity_manual_object_ = scene_manager_->createManualObject(); + path_manual_object_->setDynamic(true); + velocity_manual_object_->setDynamic(true); + scene_node_->attachObject(path_manual_object_); + scene_node_->attachObject(velocity_manual_object_); +} + +void AutowarePathDisplay::reset() +{ + MFDClass::reset(); + path_manual_object_->clear(); + velocity_manual_object_->clear(); +} + +bool AutowarePathDisplay::validateFloats( + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & msg_ptr) +{ + for (auto && path_point : msg_ptr->points) { + if ( + !rviz_common::validateFloats(path_point.pose) && + !rviz_common::validateFloats(path_point.longitudinal_velocity_mps)) { + return false; + } + } + return true; +} + +void AutowarePathDisplay::processMessage( + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) +{ + if (!validateFloats(msg_ptr)) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { + RCLCPP_DEBUG( + rclcpp::get_logger("AutowarePathDisplay"), "Error transforming from frame '%s' to frame '%s'", + msg_ptr->header.frame_id.c_str(), qPrintable(fixed_frame_)); + } + + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); + + path_manual_object_->clear(); + velocity_manual_object_->clear(); + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + + if (!msg_ptr->points.empty()) { + path_manual_object_->estimateVertexCount(msg_ptr->points.size() * 2); + velocity_manual_object_->estimateVertexCount(msg_ptr->points.size()); + path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); + // path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); + velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); + + for (auto && path_point : msg_ptr->points) { + /* + * Path + */ + if (property_path_view_->getBool()) { + Ogre::ColourValue color; + if (property_path_color_view_->getBool()) { + color = rviz_common::properties::qtToOgre(property_path_color_->getColor()); + } else { + /* color change depending on velocity */ + std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( + property_vel_max_->getFloat(), path_point.longitudinal_velocity_mps); + color = *dynamic_color_ptr; + } + color.a = property_path_alpha_->getFloat(); + Eigen::Vector3f vec_in, vec_out; + Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); + { + vec_in << 0, (property_path_width_->getFloat() / 2.0), 0; + Eigen::Quaternionf quat( + path_point.pose.orientation.w, path_point.pose.orientation.x, + path_point.pose.orientation.y, path_point.pose.orientation.z); + if (path_point.longitudinal_velocity_mps < 0) { + quat *= quat_yaw_reverse; + } + vec_out = quat * vec_in; + path_manual_object_->position( + path_point.pose.position.x + vec_out.x(), path_point.pose.position.y + vec_out.y(), + path_point.pose.position.z + vec_out.z()); + path_manual_object_->colour(color); + } + { + vec_in << 0, -(property_path_width_->getFloat() / 2.0), 0; + Eigen::Quaternionf quat( + path_point.pose.orientation.w, path_point.pose.orientation.x, + path_point.pose.orientation.y, path_point.pose.orientation.z); + if (path_point.longitudinal_velocity_mps < 0) { + quat *= quat_yaw_reverse; + } + vec_out = quat * vec_in; + path_manual_object_->position( + path_point.pose.position.x + vec_out.x(), path_point.pose.position.y + vec_out.y(), + path_point.pose.position.z + vec_out.z()); + path_manual_object_->colour(color); + } + } + /* + * Velocity + */ + if (property_velocity_view_->getBool()) { + Ogre::ColourValue color; + if (property_velocity_color_view_->getBool()) { + color = rviz_common::properties::qtToOgre(property_velocity_color_->getColor()); + } else { + /* color change depending on velocity */ + std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( + property_vel_max_->getFloat(), path_point.longitudinal_velocity_mps); + color = *dynamic_color_ptr; + } + color.a = property_velocity_alpha_->getFloat(); + + velocity_manual_object_->position( + path_point.pose.position.x, path_point.pose.position.y, + path_point.pose.position.z + + path_point.longitudinal_velocity_mps * property_velocity_scale_->getFloat()); + velocity_manual_object_->colour(color); + } + } + + path_manual_object_->end(); + velocity_manual_object_->end(); + } + last_msg_ptr_ = msg_ptr; +} + +void AutowarePathDisplay::updateVisualization() +{ + if (last_msg_ptr_ != nullptr) { + processMessage(last_msg_ptr_); + } +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathDisplay, rviz_common::Display) diff --git a/common/autoware_planning_rviz_plugin/src/path_footprint/display.cpp b/common/autoware_planning_rviz_plugin/src/path_footprint/display.cpp new file mode 100644 index 0000000000000..9806be61bdc35 --- /dev/null +++ b/common/autoware_planning_rviz_plugin/src/path_footprint/display.cpp @@ -0,0 +1,185 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 + +#define EIGEN_MPL2_ONLY +#include +#include +#include + +namespace rviz_plugins +{ +AutowarePathFootprintDisplay::AutowarePathFootprintDisplay() +{ + property_path_footprint_view_ = new rviz_common::properties::BoolProperty( + "View Path Footprint", true, "", this, SLOT(updateVisualization()), this); + property_path_footprint_alpha_ = new rviz_common::properties::FloatProperty( + "Alpha", 1.0, "", property_path_footprint_view_, SLOT(updateVisualization()), this); + property_path_footprint_alpha_->setMin(0.0); + property_path_footprint_alpha_->setMax(1.0); + property_path_footprint_color_ = new rviz_common::properties::ColorProperty( + "Color", QColor(230, 230, 50), "", property_path_footprint_view_, SLOT(updateVisualization()), + this); + property_vehicle_length_ = new rviz_common::properties::FloatProperty( + "Vehicle Length", 4.77, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); + property_vehicle_width_ = new rviz_common::properties::FloatProperty( + "Vehicle Width", 1.83, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); + property_rear_overhang_ = new rviz_common::properties::FloatProperty( + "Rear Overhang", 1.03, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); + property_vehicle_length_->setMin(0.0); + property_vehicle_width_->setMin(0.0); + property_rear_overhang_->setMin(0.0); + + updateVehicleInfo(); +} + +AutowarePathFootprintDisplay::~AutowarePathFootprintDisplay() +{ + if (initialized()) { + scene_manager_->destroyManualObject(path_footprint_manual_object_); + } +} + +void AutowarePathFootprintDisplay::onInitialize() +{ + MFDClass::onInitialize(); + + path_footprint_manual_object_ = scene_manager_->createManualObject(); + path_footprint_manual_object_->setDynamic(true); + scene_node_->attachObject(path_footprint_manual_object_); +} + +void AutowarePathFootprintDisplay::reset() +{ + MFDClass::reset(); + path_footprint_manual_object_->clear(); +} + +bool AutowarePathFootprintDisplay::validateFloats( + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & msg_ptr) +{ + for (auto && path_point : msg_ptr->points) { + if (!rviz_common::validateFloats(path_point.pose)) { + return false; + } + } + return true; +} + +void AutowarePathFootprintDisplay::processMessage( + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) +{ + if (!validateFloats(msg_ptr)) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { + RCLCPP_DEBUG( + rviz_ros_node_.lock()->get_raw_node()->get_logger(), + "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), + qPrintable(fixed_frame_)); + } + + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); + + path_footprint_manual_object_->clear(); + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + + if (!msg_ptr->points.empty()) { + path_footprint_manual_object_->estimateVertexCount(msg_ptr->points.size() * 4 * 2); + path_footprint_manual_object_->begin( + "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); + + for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { + const auto & path_point = msg_ptr->points.at(point_idx); + /* + * Footprint + */ + if (property_path_footprint_view_->getBool()) { + Ogre::ColourValue color; + color = rviz_common::properties::qtToOgre(property_path_footprint_color_->getColor()); + color.a = property_path_footprint_alpha_->getFloat(); + + const auto info = vehicle_footprint_info_; + const float top = info->length - info->rear_overhang; + const float bottom = -info->rear_overhang; + const float left = -info->width / 2.0; + const float right = info->width / 2.0; + + const std::array lon_offset_vec{top, top, bottom, bottom}; + const std::array lat_offset_vec{left, right, right, left}; + + for (int f_idx = 0; f_idx < 4; ++f_idx) { + const Eigen::Quaternionf quat( + path_point.pose.orientation.w, path_point.pose.orientation.x, + path_point.pose.orientation.y, path_point.pose.orientation.z); + + { + const Eigen::Vector3f offset_vec{ + lon_offset_vec.at(f_idx), lat_offset_vec.at(f_idx), 0.0}; + const auto offset_to_edge = quat * offset_vec; + path_footprint_manual_object_->position( + path_point.pose.position.x + offset_to_edge.x(), + path_point.pose.position.y + offset_to_edge.y(), path_point.pose.position.z); + path_footprint_manual_object_->colour(color); + } + { + const Eigen::Vector3f offset_vec{ + lon_offset_vec.at((f_idx + 1) % 4), lat_offset_vec.at((f_idx + 1) % 4), 0.0}; + const auto offset_to_edge = quat * offset_vec; + path_footprint_manual_object_->position( + path_point.pose.position.x + offset_to_edge.x(), + path_point.pose.position.y + offset_to_edge.y(), path_point.pose.position.z); + path_footprint_manual_object_->colour(color); + } + } + } + } + + path_footprint_manual_object_->end(); + } + last_msg_ptr_ = msg_ptr; +} + +void AutowarePathFootprintDisplay::updateVisualization() +{ + if (last_msg_ptr_ != nullptr) { + processMessage(last_msg_ptr_); + } +} + +void AutowarePathFootprintDisplay::updateVehicleInfo() +{ + float length{property_vehicle_length_->getFloat()}; + float width{property_vehicle_width_->getFloat()}; + float rear_overhang{property_rear_overhang_->getFloat()}; + + vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathFootprintDisplay, rviz_common::Display) diff --git a/common/autoware_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp b/common/autoware_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp new file mode 100644 index 0000000000000..e535ceadab9cd --- /dev/null +++ b/common/autoware_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp @@ -0,0 +1,193 @@ +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "jsk_overlay_utils.hpp" + +#include + +namespace jsk_rviz_plugins +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() { pixel_buffer_->unlock(); } + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() { return pixel_buffer_; } + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject( + Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name) +: name_(name), logger_(logger) +{ + rviz_rendering::RenderSystem::get()->prepareOverlays(manager); + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + hide(); + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + // mOverlayMgr->destroyOverlayElement(panel_); + // delete panel_; + // delete overlay_; +} + +std::string OverlayObject::getName() { return name_; } + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() { return static_cast(texture_); } + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RCLCPP_WARN(logger_, "width=0 is specified as texture size"); + width = 1; + } + if (height == 0) { + RCLCPP_WARN(logger_, "height=0 is specified as texture size"); + height = 1; + } + if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { + if (isTextureReady()) { + Ogre::TextureManager::getSingleton().remove(texture_name); + panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); + } + texture_ = Ogre::TextureManager::getSingleton().createManual( + texture_name, // name + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, // type + width, height, // width & height of the render window + 0, // number of mipmaps + Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use + Ogre::TU_DEFAULT // usage + ); + panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + + panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + } +} + +ScopedPixelBuffer OverlayObject::getBuffer() +{ + if (isTextureReady()) { + return ScopedPixelBuffer(texture_->getBuffer()); + } else { + return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); + } +} + +void OverlayObject::setPosition(double left, double top) { panel_->setPosition(left, top); } + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() { return overlay_->isVisible(); } + +unsigned int OverlayObject::getTextureWidth() +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} + +} // namespace jsk_rviz_plugins diff --git a/common/autoware_planning_rviz_plugin/src/tools/jsk_overlay_utils.hpp b/common/autoware_planning_rviz_plugin/src/tools/jsk_overlay_utils.hpp new file mode 100644 index 0000000000000..e0e885c9e340d --- /dev/null +++ b/common/autoware_planning_rviz_plugin/src/tools/jsk_overlay_utils.hpp @@ -0,0 +1,127 @@ +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef TOOLS__JSK_OVERLAY_UTILS_HPP_ +#define TOOLS__JSK_OVERLAY_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +// see OGRE/OgrePrerequisites.h +// #define OGRE_VERSION +// ((OGRE_VERSION_MAJOR << 16) | (OGRE_VERSION_MINOR << 8) | OGRE_VERSION_PATCH) +#if OGRE_VERSION < ((1 << 16) | (9 << 8) | 0) +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif + +#include +#include +#include +#include + +namespace jsk_rviz_plugins +{ +class OverlayObject; + +class ScopedPixelBuffer +{ +public: + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + virtual ~ScopedPixelBuffer(); + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + virtual QImage getQImage(unsigned int width, unsigned int height); + virtual QImage getQImage(OverlayObject & overlay); + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; + +private: +}; + +// this is a class for put overlay object on rviz 3D panel. +// This class suppose to be instantiated in onInitialize method +// of rviz::Display class. +class OverlayObject +{ +public: + typedef std::shared_ptr Ptr; + + OverlayObject(Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName(); + virtual void hide(); + virtual void show(); + virtual bool isTextureReady(); + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition(double left, double top); + virtual void setDimensions(double width, double height); + virtual bool isVisible(); + virtual unsigned int getTextureWidth(); + virtual unsigned int getTextureHeight(); + +protected: + const std::string name_; + rclcpp::Logger logger_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; + +private: +}; + +// Ogre::Overlay* createOverlay(std::string name); +// Ogre::PanelOverlayElement* createOverlayPanel(Ogre::Overlay* overlay); +// Ogre::MaterialPtr createOverlayMaterial(Ogre::Overlay* overlay); +} // namespace jsk_rviz_plugins + +#endif // TOOLS__JSK_OVERLAY_UTILS_HPP_ diff --git a/common/autoware_planning_rviz_plugin/src/tools/max_velocity.cpp b/common/autoware_planning_rviz_plugin/src/tools/max_velocity.cpp new file mode 100644 index 0000000000000..53750cb9db1be --- /dev/null +++ b/common/autoware_planning_rviz_plugin/src/tools/max_velocity.cpp @@ -0,0 +1,189 @@ +// Copyright 2020 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 "max_velocity.hpp" + +#include +#include +#include + +#include + +#include +#include +#include + +namespace rviz_plugins +{ +MaxVelocityDisplay::MaxVelocityDisplay() +{ + property_topic_name_ = new rviz_common::properties::StringProperty( + "Topic", "/planning/scenario_planning/current_max_velocity", + "The topic on which to publish max velocity.", this, SLOT(updateTopic()), this); + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(255, 255, 255), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", 128, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", 128, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_length_ = new rviz_common::properties::IntProperty( + "Length", 96, "Length of the plotter window", this, SLOT(updateVisualization()), this); + property_length_->setMin(10); + property_value_scale_ = new rviz_common::properties::FloatProperty( + "Value Scale", 1.0 / 4.0, "Value scale", this, SLOT(updateVisualization()), this); + property_value_scale_->setMin(0.01); +} + +MaxVelocityDisplay::~MaxVelocityDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void MaxVelocityDisplay::onInitialize() +{ + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "MaxVelocityDisplayObject" << count++; + auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); + overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); + + overlay_->show(); + + overlay_->updateTextureSize(property_length_->getInt(), property_length_->getInt()); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + + // QColor background_color; + // background_color.setAlpha(0); + // jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + // hud_ = buffer.getQImage(*overlay_); + // for (int i = 0; i < overlay_->getTextureWidth(); i++) + // { + // for (int j = 0; j < overlay_->getTextureHeight(); j++) + // { + // hud_.setPixel(i, j, background_color.rgba()); + // } + // } +} + +void MaxVelocityDisplay::updateTopic() +{ + unsubscribe(); + subscribe(); +} + +void MaxVelocityDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void MaxVelocityDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void MaxVelocityDisplay::subscribe() +{ + std::string topic_name = property_topic_name_->getStdString(); + if (topic_name.length() > 0 && topic_name != "/") { + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + max_vel_sub_ = raw_node->create_subscription( + topic_name, rclcpp::QoS{1}.transient_local(), + std::bind(&MaxVelocityDisplay::processMessage, this, std::placeholders::_1)); + } +} + +void MaxVelocityDisplay::unsubscribe() { max_vel_sub_.reset(); } + +void MaxVelocityDisplay::processMessage( + const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + if (!overlay_->isVisible()) { + return; + } + + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int line_width = property_length_->getInt() / 8; + + int w = overlay_->getTextureWidth() - line_width; + int h = overlay_->getTextureHeight() - line_width; + + // draw sign + // QColor white_color(Qt::red); + // white_color.setAlpha(255); + // const double min_range_theta = 2.0 * M_PI + M_PI_2; + // const double max_range_theta = 0.0 + M_PI_2; + // painter.setPen(QPen(white_color, line_width, Qt::SolidLine)); + // painter.drawLine( + // (w / 2) + (line_width * 0.5) + ((double)w / 2.0 - (line_width * 0.5)) * std::cos(M_PI_4), + // (h / 2) + (line_width * 0.5) - ((double)h / 2.0 - (line_width * 0.5)) * std::sin(M_PI_4), + // (w / 2) + (line_width * 0.5) - ((double)w / 2.0 - (line_width * 0.5)) * std::cos(M_PI_4), + // (h / 2) + (line_width * 0.5) + ((double)h / 2.0 - (line_width * 0.5)) * std::sin(M_PI_4)); + // painter.drawArc( + // line_width * 0.5, line_width * 0.5, w, h, 16 * ((min_range_theta - M_PI) * 180.0 / M_PI), + // 16 * ((max_range_theta - min_range_theta) * 180.0 / M_PI)); + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(255); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize( + std::max(static_cast(static_cast(w) * property_value_scale_->getFloat()), 1)); + font.setBold(true); + painter.setFont(font); + std::ostringstream velocity_ss; + velocity_ss << std::fixed << std::setprecision(0) << "limited" << std::endl + << msg_ptr->max_velocity * 3.6 << "km/h"; + painter.drawText( + static_cast(line_width * 0.5), std::min(static_cast(line_width * 0.5), h - 1), w, + std::max(h, 1), Qt::AlignCenter | Qt::AlignVCenter, velocity_ss.str().c_str()); + painter.end(); + last_msg_ptr_ = msg_ptr; +} + +void MaxVelocityDisplay::updateVisualization() +{ + overlay_->updateTextureSize(property_length_->getInt(), property_length_->getInt()); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + + if (last_msg_ptr_ != nullptr) { + processMessage(last_msg_ptr_); + } +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::MaxVelocityDisplay, rviz_common::Display) diff --git a/common/autoware_planning_rviz_plugin/src/tools/max_velocity.hpp b/common/autoware_planning_rviz_plugin/src/tools/max_velocity.hpp new file mode 100644 index 0000000000000..e379d1fd1b754 --- /dev/null +++ b/common/autoware_planning_rviz_plugin/src/tools/max_velocity.hpp @@ -0,0 +1,82 @@ +// Copyright 2020 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 TOOLS__MAX_VELOCITY_HPP_ +#define TOOLS__MAX_VELOCITY_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "jsk_overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#endif + +namespace rviz_plugins +{ +class MaxVelocityDisplay : public rviz_common::Display +{ + Q_OBJECT + +public: + MaxVelocityDisplay(); + virtual ~MaxVelocityDisplay(); + + void onInitialize() override; + void onDisable() override; + void onEnable() override; + void subscribe(); + void unsubscribe(); + +private Q_SLOTS: + void updateTopic(); + void updateVisualization(); + +protected: + void processMessage(const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr); + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_length_; + rviz_common::properties::StringProperty * property_topic_name_; + rviz_common::properties::FloatProperty * property_value_scale_; + +private: + rclcpp::Subscription::SharedPtr max_vel_sub_; + autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr last_msg_ptr_; +}; + +} // namespace rviz_plugins + +#endif // TOOLS__MAX_VELOCITY_HPP_ diff --git a/common/autoware_planning_rviz_plugin/src/trajectory/display.cpp b/common/autoware_planning_rviz_plugin/src/trajectory/display.cpp new file mode 100644 index 0000000000000..3e6ce699fc5d2 --- /dev/null +++ b/common/autoware_planning_rviz_plugin/src/trajectory/display.cpp @@ -0,0 +1,317 @@ +// Copyright 2020 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 + +#include +#define EIGEN_MPL2_ONLY +#include +#include + +namespace rviz_plugins +{ +std::unique_ptr AutowareTrajectoryDisplay::gradation( + const QColor & color_min, const QColor & color_max, const double ratio) +{ + std::unique_ptr color_ptr(new Ogre::ColourValue); + color_ptr->g = color_max.greenF() * ratio + color_min.greenF() * (1.0 - ratio); + color_ptr->r = color_max.redF() * ratio + color_min.redF() * (1.0 - ratio); + color_ptr->b = color_max.blueF() * ratio + color_min.blueF() * (1.0 - ratio); + + return color_ptr; +} + +std::unique_ptr AutowareTrajectoryDisplay::setColorDependsOnVelocity( + const double vel_max, const double cmd_vel) +{ + const double cmd_vel_abs = std::fabs(cmd_vel); + const double vel_min = 0.0; + + std::unique_ptr color_ptr(new Ogre::ColourValue()); + if (vel_min < cmd_vel_abs && cmd_vel_abs <= (vel_max / 2.0)) { + double ratio = (cmd_vel_abs - vel_min) / (vel_max / 2.0 - vel_min); + color_ptr = gradation(Qt::red, Qt::yellow, ratio); + } else if ((vel_max / 2.0) < cmd_vel_abs && cmd_vel_abs <= vel_max) { + double ratio = (cmd_vel_abs - vel_max / 2.0) / (vel_max - vel_max / 2.0); + color_ptr = gradation(Qt::yellow, Qt::green, ratio); + } else if (vel_max < cmd_vel_abs) { + *color_ptr = Ogre::ColourValue::Green; + } else { + *color_ptr = Ogre::ColourValue::Red; + } + + return color_ptr; +} + +AutowareTrajectoryDisplay::AutowareTrajectoryDisplay() +{ + property_path_view_ = new rviz_common::properties::BoolProperty( + "View Path", true, "", this, SLOT(updateVisualization())); + property_path_width_ = new rviz_common::properties::FloatProperty( + "Width", 2.0, "", property_path_view_, SLOT(updateVisualization()), this); + property_path_width_->setMin(0.0); + property_path_alpha_ = new rviz_common::properties::FloatProperty( + "Alpha", 1.0, "", property_path_view_, SLOT(updateVisualization()), this); + property_path_alpha_->setMin(0.0); + property_path_alpha_->setMax(1.0); + property_path_color_view_ = new rviz_common::properties::BoolProperty( + "Constant Color", false, "", property_path_view_, SLOT(updateVisualization()), this); + property_path_color_ = new rviz_common::properties::ColorProperty( + "Color", Qt::black, "", property_path_view_, SLOT(updateVisualization()), this); + + property_velocity_view_ = new rviz_common::properties::BoolProperty( + "View Velocity", true, "", this, SLOT(updateVisualization()), this); + property_velocity_alpha_ = new rviz_common::properties::FloatProperty( + "Alpha", 1.0, "", property_velocity_view_, SLOT(updateVisualization()), this); + property_velocity_alpha_->setMin(0.0); + property_velocity_alpha_->setMax(1.0); + property_velocity_scale_ = new rviz_common::properties::FloatProperty( + "Scale", 0.3, "", property_velocity_view_, SLOT(updateVisualization()), this); + property_velocity_scale_->setMin(0.1); + property_velocity_scale_->setMax(10.0); + property_velocity_color_view_ = new rviz_common::properties::BoolProperty( + "Constant Color", false, "", property_velocity_view_, SLOT(updateVisualization()), this); + property_velocity_color_ = new rviz_common::properties::ColorProperty( + "Color", Qt::black, "", property_velocity_view_, SLOT(updateVisualization()), this); + property_velocity_text_view_ = new rviz_common::properties::BoolProperty( + "View Text Velocity", false, "", this, SLOT(updateVisualization()), this); + property_velocity_text_scale_ = new rviz_common::properties::FloatProperty( + "Scale", 0.3, "", property_velocity_text_view_, SLOT(updateVisualization()), this); + property_vel_max_ = new rviz_common::properties::FloatProperty( + "Color Border Vel Max", 3.0, "[m/s]", this, SLOT(updateVisualization()), this); + property_vel_max_->setMin(0.0); +} + +AutowareTrajectoryDisplay::~AutowareTrajectoryDisplay() +{ + if (initialized()) { + scene_manager_->destroyManualObject(path_manual_object_); + scene_manager_->destroyManualObject(velocity_manual_object_); + for (size_t i = 0; i < velocity_text_nodes_.size(); i++) { + Ogre::SceneNode * node = velocity_text_nodes_.at(i); + node->removeAndDestroyAllChildren(); + node->detachAllObjects(); + scene_manager_->destroySceneNode(node); + } + } +} + +void AutowareTrajectoryDisplay::onInitialize() +{ + MFDClass::onInitialize(); + + path_manual_object_ = scene_manager_->createManualObject(); + velocity_manual_object_ = scene_manager_->createManualObject(); + path_manual_object_->setDynamic(true); + velocity_manual_object_->setDynamic(true); + scene_node_->attachObject(path_manual_object_); + scene_node_->attachObject(velocity_manual_object_); +} + +void AutowareTrajectoryDisplay::reset() +{ + MFDClass::reset(); + path_manual_object_->clear(); + velocity_manual_object_->clear(); + for (size_t i = 0; i < velocity_texts_.size(); i++) { + Ogre::SceneNode * node = velocity_text_nodes_.at(i); + node->detachAllObjects(); + node->removeAndDestroyAllChildren(); + scene_manager_->destroySceneNode(node); + } + velocity_text_nodes_.clear(); + velocity_texts_.clear(); +} + +bool AutowareTrajectoryDisplay::validateFloats( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & msg_ptr) +{ + for (auto && trajectory_point : msg_ptr->points) { + if ( + !rviz_common::validateFloats(trajectory_point.pose) && + !rviz_common::validateFloats(trajectory_point.longitudinal_velocity_mps)) { + return false; + } + } + return true; +} + +void AutowareTrajectoryDisplay::processMessage( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) +{ + if (!validateFloats(msg_ptr)) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { + RCLCPP_DEBUG( + rclcpp::get_logger("AutowareTrajectoryDisplay"), + "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), + qPrintable(fixed_frame_)); + } + + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); + + path_manual_object_->clear(); + velocity_manual_object_->clear(); + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + + if (!msg_ptr->points.empty()) { + path_manual_object_->estimateVertexCount(msg_ptr->points.size() * 2); + velocity_manual_object_->estimateVertexCount(msg_ptr->points.size()); + path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); + velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); + + if (msg_ptr->points.size() > velocity_texts_.size()) { + for (size_t i = velocity_texts_.size(); i < msg_ptr->points.size(); i++) { + Ogre::SceneNode * node = scene_node_->createChildSceneNode(); + rviz_rendering::MovableText * text = + new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1); + text->setVisible(false); + text->setTextAlignment( + rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); + node->attachObject(text); + velocity_texts_.push_back(text); + velocity_text_nodes_.push_back(node); + } + } else if (msg_ptr->points.size() < velocity_texts_.size()) { + for (size_t i = velocity_texts_.size() - 1; i >= msg_ptr->points.size(); i--) { + Ogre::SceneNode * node = velocity_text_nodes_.at(i); + node->detachAllObjects(); + node->removeAndDestroyAllChildren(); + scene_manager_->destroySceneNode(node); + } + velocity_texts_.resize(msg_ptr->points.size()); + velocity_text_nodes_.resize(msg_ptr->points.size()); + } + + for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { + const auto & path_point = msg_ptr->points.at(point_idx); + /* + * Path + */ + if (property_path_view_->getBool()) { + Ogre::ColourValue color; + if (property_path_color_view_->getBool()) { + color = rviz_common::properties::qtToOgre(property_path_color_->getColor()); + } else { + /* color change depending on velocity */ + std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( + property_vel_max_->getFloat(), path_point.longitudinal_velocity_mps); + color = *dynamic_color_ptr; + } + color.a = property_path_alpha_->getFloat(); + Eigen::Vector3f vec_in, vec_out; + Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); + { + vec_in << 0, (property_path_width_->getFloat() / 2.0), 0; + Eigen::Quaternionf quat( + path_point.pose.orientation.w, path_point.pose.orientation.x, + path_point.pose.orientation.y, path_point.pose.orientation.z); + if (path_point.longitudinal_velocity_mps < 0) { + quat *= quat_yaw_reverse; + } + vec_out = quat * vec_in; + path_manual_object_->position( + path_point.pose.position.x + vec_out.x(), path_point.pose.position.y + vec_out.y(), + path_point.pose.position.z + vec_out.z()); + path_manual_object_->colour(color); + } + { + vec_in << 0, -(property_path_width_->getFloat() / 2.0), 0; + Eigen::Quaternionf quat( + path_point.pose.orientation.w, path_point.pose.orientation.x, + path_point.pose.orientation.y, path_point.pose.orientation.z); + if (path_point.longitudinal_velocity_mps < 0) { + quat *= quat_yaw_reverse; + } + vec_out = quat * vec_in; + path_manual_object_->position( + path_point.pose.position.x + vec_out.x(), path_point.pose.position.y + vec_out.y(), + path_point.pose.position.z + vec_out.z()); + path_manual_object_->colour(color); + } + } + /* + * Velocity + */ + if (property_velocity_view_->getBool()) { + Ogre::ColourValue color; + if (property_velocity_color_view_->getBool()) { + color = rviz_common::properties::qtToOgre(property_velocity_color_->getColor()); + } else { + /* color change depending on velocity */ + std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( + property_vel_max_->getFloat(), path_point.longitudinal_velocity_mps); + color = *dynamic_color_ptr; + } + color.a = property_velocity_alpha_->getFloat(); + + velocity_manual_object_->position( + path_point.pose.position.x, path_point.pose.position.y, + path_point.pose.position.z + + path_point.longitudinal_velocity_mps * property_velocity_scale_->getFloat()); + velocity_manual_object_->colour(color); + } + /* + * Velocity Text + */ + if (property_velocity_text_view_->getBool()) { + Ogre::Vector3 position; + position.x = path_point.pose.position.x; + position.y = path_point.pose.position.y; + position.z = path_point.pose.position.z; + Ogre::SceneNode * node = velocity_text_nodes_.at(point_idx); + node->setPosition(position); + + rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); + double vel = path_point.longitudinal_velocity_mps; + text->setCaption( + std::to_string(static_cast(std::floor(vel))) + "." + + std::to_string(static_cast(std::floor(vel * 100)))); + text->setCharacterHeight(property_velocity_text_scale_->getFloat()); + text->setVisible(true); + } else { + rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); + text->setVisible(false); + } + } + + path_manual_object_->end(); + velocity_manual_object_->end(); + } + last_msg_ptr_ = msg_ptr; +} + +void AutowareTrajectoryDisplay::updateVisualization() +{ + if (last_msg_ptr_ != nullptr) { + processMessage(last_msg_ptr_); + } +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareTrajectoryDisplay, rviz_common::Display) diff --git a/common/autoware_planning_rviz_plugin/src/trajectory_footprint/display.cpp b/common/autoware_planning_rviz_plugin/src/trajectory_footprint/display.cpp new file mode 100644 index 0000000000000..4eb63fcd1201e --- /dev/null +++ b/common/autoware_planning_rviz_plugin/src/trajectory_footprint/display.cpp @@ -0,0 +1,250 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 + +#define EIGEN_MPL2_ONLY +#include +#include +#include + +#include + +namespace rviz_plugins +{ +AutowareTrajectoryFootprintDisplay::AutowareTrajectoryFootprintDisplay() +{ + // trajectory footprint + property_trajectory_footprint_view_ = new rviz_common::properties::BoolProperty( + "View Trajectory Footprint", true, "", this, SLOT(updateVisualization()), this); + property_trajectory_footprint_alpha_ = new rviz_common::properties::FloatProperty( + "Alpha", 1.0, "", property_trajectory_footprint_view_, SLOT(updateVisualization()), this); + property_trajectory_footprint_alpha_->setMin(0.0); + property_trajectory_footprint_alpha_->setMax(1.0); + property_trajectory_footprint_color_ = new rviz_common::properties::ColorProperty( + "Color", QColor(230, 230, 50), "", property_trajectory_footprint_view_, + SLOT(updateVisualization()), this); + property_vehicle_length_ = new rviz_common::properties::FloatProperty( + "Vehicle Length", 4.77, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), + this); + property_vehicle_width_ = new rviz_common::properties::FloatProperty( + "Vehicle Width", 1.83, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), + this); + property_rear_overhang_ = new rviz_common::properties::FloatProperty( + "Rear Overhang", 1.03, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), + this); + property_vehicle_length_->setMin(0.0); + property_vehicle_width_->setMin(0.0); + property_rear_overhang_->setMin(0.0); + + // trajectory point + property_trajectory_point_view_ = new rviz_common::properties::BoolProperty( + "View Trajectory Point", false, "", this, SLOT(updateVisualization()), this); + property_trajectory_point_alpha_ = new rviz_common::properties::FloatProperty( + "Alpha", 1.0, "", property_trajectory_point_view_, SLOT(updateVisualization()), this); + property_trajectory_point_alpha_->setMin(0.0); + property_trajectory_point_alpha_->setMax(1.0); + property_trajectory_point_color_ = new rviz_common::properties::ColorProperty( + "Color", QColor(0, 60, 255), "", property_trajectory_point_view_, SLOT(updateVisualization()), + this); + property_trajectory_point_radius_ = new rviz_common::properties::FloatProperty( + "Radius", 0.1, "", property_trajectory_point_view_, SLOT(updateVisualization()), this); + property_trajectory_point_offset_ = new rviz_common::properties::FloatProperty( + "Offset", 0.0, "", property_trajectory_point_view_, SLOT(updateVisualization()), this); + + updateVehicleInfo(); +} + +AutowareTrajectoryFootprintDisplay::~AutowareTrajectoryFootprintDisplay() +{ + if (initialized()) { + scene_manager_->destroyManualObject(trajectory_footprint_manual_object_); + scene_manager_->destroyManualObject(trajectory_point_manual_object_); + } +} + +void AutowareTrajectoryFootprintDisplay::onInitialize() +{ + MFDClass::onInitialize(); + + trajectory_footprint_manual_object_ = scene_manager_->createManualObject(); + trajectory_footprint_manual_object_->setDynamic(true); + scene_node_->attachObject(trajectory_footprint_manual_object_); + + trajectory_point_manual_object_ = scene_manager_->createManualObject(); + trajectory_point_manual_object_->setDynamic(true); + scene_node_->attachObject(trajectory_point_manual_object_); +} + +void AutowareTrajectoryFootprintDisplay::reset() +{ + MFDClass::reset(); + trajectory_footprint_manual_object_->clear(); + trajectory_point_manual_object_->clear(); +} + +bool AutowareTrajectoryFootprintDisplay::validateFloats( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & msg_ptr) +{ + for (auto && trajectory_point : msg_ptr->points) { + if (!rviz_common::validateFloats(trajectory_point.pose)) { + return false; + } + } + return true; +} + +void AutowareTrajectoryFootprintDisplay::processMessage( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) +{ + if (!validateFloats(msg_ptr)) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { + RCLCPP_DEBUG( + rviz_ros_node_.lock()->get_raw_node()->get_logger(), + "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), + qPrintable(fixed_frame_)); + } + + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); + + trajectory_footprint_manual_object_->clear(); + trajectory_point_manual_object_->clear(); + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + + if (!msg_ptr->points.empty()) { + trajectory_footprint_manual_object_->estimateVertexCount(msg_ptr->points.size() * 4 * 2); + trajectory_footprint_manual_object_->begin( + "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); + + trajectory_point_manual_object_->estimateVertexCount(msg_ptr->points.size() * 3 * 8); + trajectory_point_manual_object_->begin( + "BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST); + + for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { + const auto & path_point = msg_ptr->points.at(point_idx); + /* + * Footprint + */ + if (property_trajectory_footprint_view_->getBool()) { + Ogre::ColourValue color; + color = rviz_common::properties::qtToOgre(property_trajectory_footprint_color_->getColor()); + color.a = property_trajectory_footprint_alpha_->getFloat(); + + const auto info = vehicle_footprint_info_; + const float top = info->length - info->rear_overhang; + const float bottom = -info->rear_overhang; + const float left = -info->width / 2.0; + const float right = info->width / 2.0; + + const std::array lon_offset_vec{top, top, bottom, bottom}; + const std::array lat_offset_vec{left, right, right, left}; + + for (int f_idx = 0; f_idx < 4; ++f_idx) { + const Eigen::Quaternionf quat( + path_point.pose.orientation.w, path_point.pose.orientation.x, + path_point.pose.orientation.y, path_point.pose.orientation.z); + + { + const Eigen::Vector3f offset_vec{ + lon_offset_vec.at(f_idx), lat_offset_vec.at(f_idx), 0.0}; + const auto offset_to_edge = quat * offset_vec; + trajectory_footprint_manual_object_->position( + path_point.pose.position.x + offset_to_edge.x(), + path_point.pose.position.y + offset_to_edge.y(), path_point.pose.position.z); + trajectory_footprint_manual_object_->colour(color); + } + { + const Eigen::Vector3f offset_vec{ + lon_offset_vec.at((f_idx + 1) % 4), lat_offset_vec.at((f_idx + 1) % 4), 0.0}; + const auto offset_to_edge = quat * offset_vec; + trajectory_footprint_manual_object_->position( + path_point.pose.position.x + offset_to_edge.x(), + path_point.pose.position.y + offset_to_edge.y(), path_point.pose.position.z); + trajectory_footprint_manual_object_->colour(color); + } + } + } + + /* + * Point + */ + if (property_trajectory_point_view_->getBool()) { + Ogre::ColourValue color; + color = rviz_common::properties::qtToOgre(property_trajectory_point_color_->getColor()); + color.a = property_trajectory_point_alpha_->getFloat(); + + const double offset = property_trajectory_point_offset_->getFloat(); + const double yaw = tf2::getYaw(path_point.pose.orientation); + const double base_x = path_point.pose.position.x + offset * std::cos(yaw); + const double base_y = path_point.pose.position.y + offset * std::sin(yaw); + const double base_z = path_point.pose.position.z; + + const double radius = property_trajectory_point_radius_->getFloat(); + for (size_t s_idx = 0; s_idx < 8; ++s_idx) { + const double current_angle = static_cast(s_idx) / 8.0 * 2.0 * M_PI; + const double next_angle = static_cast(s_idx + 1) / 8.0 * 2.0 * M_PI; + trajectory_point_manual_object_->position( + base_x + radius * std::cos(current_angle), base_y + radius * std::sin(current_angle), + base_z); + trajectory_point_manual_object_->colour(color); + + trajectory_point_manual_object_->position( + base_x + radius * std::cos(next_angle), base_y + radius * std::sin(next_angle), base_z); + trajectory_point_manual_object_->colour(color); + + trajectory_point_manual_object_->position(base_x, base_y, base_z); + trajectory_point_manual_object_->colour(color); + } + } + } + + trajectory_footprint_manual_object_->end(); + trajectory_point_manual_object_->end(); + } + last_msg_ptr_ = msg_ptr; +} + +void AutowareTrajectoryFootprintDisplay::updateVisualization() +{ + if (last_msg_ptr_ != nullptr) { + processMessage(last_msg_ptr_); + } +} + +void AutowareTrajectoryFootprintDisplay::updateVehicleInfo() +{ + float length{property_vehicle_length_->getFloat()}; + float width{property_vehicle_width_->getFloat()}; + float rear_overhang{property_rear_overhang_->getFloat()}; + + vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareTrajectoryFootprintDisplay, rviz_common::Display) diff --git a/common/autoware_state_rviz_plugin/CMakeLists.txt b/common/autoware_state_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..9098124f01131 --- /dev/null +++ b/common/autoware_state_rviz_plugin/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_state_rviz_plugin) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Qt5 ${rviz_QT_VERSION} EXACT 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(${PROJECT_NAME} SHARED + src/autoware_state_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/common/autoware_state_rviz_plugin/README.md b/common/autoware_state_rviz_plugin/README.md new file mode 100644 index 0000000000000..425a586ea57b8 --- /dev/null +++ b/common/autoware_state_rviz_plugin/README.md @@ -0,0 +1,32 @@ +# autoware_state_rviz_plugin + +## Purpose + +This plugin displays the current status of autoware. +This plugin also can engage from the panel. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ----------------------------- | ----------------------------------------------- | -------------------------------------------------- | +| `/control/current_gate_mode` | `autoware_control_msgs::msg::GateMode` | The topic represents the state of AUTO or EXTERNAL | +| `/autoware/state` | `autoware_auto_system_msgs::msg::AutowareState` | The topic represents the state of Autoware | +| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic represents the state of Gear | +| `/api/external/get/engage` | `autoware_external_api_msgs::msg::EngageStatus` | The topic represents the state of Engage | + +### Output + +| Name | Type | Description | +| -------------------------- | ----------------------------------------- | ------------------------------ | +| `/api/external/set/engage` | `autoware_external_api_msgs::srv::Engage` | The service inputs engage true | + +## HowToUse + +1. Start rviz and select panels/Add new panel. + ![select_panel](./images/select_panels.png) +2. Select autoware_state_rviz_plugin/AutowareStatePanel and press OK. + ![select_state_plugin](./images/select_state_plugin.png) +3. If the AutowareState is WaitingForEngage, can engage by clicking the Engage button. + ![select_engage](./images/select_engage.png) diff --git a/common/autoware_state_rviz_plugin/icons/classes/AutowareStatePanel.png b/common/autoware_state_rviz_plugin/icons/classes/AutowareStatePanel.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_state_rviz_plugin/icons/classes/AutowareStatePanel.png differ diff --git a/common/autoware_state_rviz_plugin/images/select_engage.png b/common/autoware_state_rviz_plugin/images/select_engage.png new file mode 100644 index 0000000000000..9a842800a786c Binary files /dev/null and b/common/autoware_state_rviz_plugin/images/select_engage.png differ diff --git a/common/autoware_state_rviz_plugin/images/select_panels.png b/common/autoware_state_rviz_plugin/images/select_panels.png new file mode 100644 index 0000000000000..a691602c42c3c Binary files /dev/null and b/common/autoware_state_rviz_plugin/images/select_panels.png differ diff --git a/common/autoware_state_rviz_plugin/images/select_state_plugin.png b/common/autoware_state_rviz_plugin/images/select_state_plugin.png new file mode 100644 index 0000000000000..eff568e755307 Binary files /dev/null and b/common/autoware_state_rviz_plugin/images/select_state_plugin.png differ diff --git a/common/autoware_state_rviz_plugin/package.xml b/common/autoware_state_rviz_plugin/package.xml new file mode 100644 index 0000000000000..ad9ee379e7ce6 --- /dev/null +++ b/common/autoware_state_rviz_plugin/package.xml @@ -0,0 +1,30 @@ + + + + autoware_state_rviz_plugin + 0.0.0 + The autoware state rviz plugin package + Hiroki OTA + Apache License 2.0 + + ament_cmake_auto + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_external_api_msgs + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + + diff --git a/common/autoware_state_rviz_plugin/plugins/plugin_description.xml b/common/autoware_state_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..df5b5500b30fa --- /dev/null +++ b/common/autoware_state_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + AutowareStatePanel + + + diff --git a/common/autoware_state_rviz_plugin/src/autoware_state_panel.cpp b/common/autoware_state_rviz_plugin/src/autoware_state_panel.cpp new file mode 100644 index 0000000000000..4dad6cb742f6d --- /dev/null +++ b/common/autoware_state_rviz_plugin/src/autoware_state_panel.cpp @@ -0,0 +1,199 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 "autoware_state_panel.hpp" + +#include +#include +#include + +#include +#include + +inline std::string Bool2String(const bool var) { return var ? "True" : "False"; } + +using std::placeholders::_1; + +namespace rviz_plugins +{ +AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(parent) +{ + // Gate Mode + auto * gate_prefix_label_ptr = new QLabel("GATE: "); + gate_prefix_label_ptr->setAlignment(Qt::AlignRight); + gate_mode_label_ptr_ = new QLabel("INIT"); + gate_mode_label_ptr_->setAlignment(Qt::AlignCenter); + auto * gate_layout = new QHBoxLayout; + gate_layout->addWidget(gate_prefix_label_ptr); + gate_layout->addWidget(gate_mode_label_ptr_); + + // State + auto * state_prefix_label_ptr = new QLabel("STATE: "); + state_prefix_label_ptr->setAlignment(Qt::AlignRight); + autoware_state_label_ptr_ = new QLabel("INIT"); + autoware_state_label_ptr_->setAlignment(Qt::AlignCenter); + auto * state_layout = new QHBoxLayout; + state_layout->addWidget(state_prefix_label_ptr); + state_layout->addWidget(autoware_state_label_ptr_); + + // Gear + auto * gear_prefix_label_ptr = new QLabel("GEAR: "); + gear_prefix_label_ptr->setAlignment(Qt::AlignRight); + gear_label_ptr_ = new QLabel("INIT"); + gear_label_ptr_->setAlignment(Qt::AlignCenter); + auto * gear_layout = new QHBoxLayout; + gear_layout->addWidget(gear_prefix_label_ptr); + gear_layout->addWidget(gear_label_ptr_); + + // Engage Status + auto * engage_prefix_label_ptr = new QLabel("Engage: "); + engage_prefix_label_ptr->setAlignment(Qt::AlignRight); + engage_status_label_ptr_ = new QLabel("INIT"); + engage_status_label_ptr_->setAlignment(Qt::AlignCenter); + auto * engage_status_layout = new QHBoxLayout; + engage_status_layout->addWidget(engage_prefix_label_ptr); + engage_status_layout->addWidget(engage_status_label_ptr_); + + // Autoware Engage Button + engage_button_ptr_ = new QPushButton("Engage"); + connect(engage_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareEngage())); + + auto * v_layout = new QVBoxLayout; + v_layout->addLayout(gate_layout); + v_layout->addLayout(state_layout); + v_layout->addLayout(gear_layout); + v_layout->addLayout(engage_status_layout); + v_layout->addWidget(engage_button_ptr_); + setLayout(v_layout); +} + +void AutowareStatePanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + sub_gate_mode_ = raw_node_->create_subscription( + "/control/current_gate_mode", 10, std::bind(&AutowareStatePanel::onGateMode, this, _1)); + + sub_autoware_state_ = + raw_node_->create_subscription( + "/autoware/state", 10, std::bind(&AutowareStatePanel::onAutowareState, this, _1)); + + sub_gear_ = raw_node_->create_subscription( + "/vehicle/status/gear_status", 10, std::bind(&AutowareStatePanel::onShift, this, _1)); + + sub_engage_ = raw_node_->create_subscription( + "/api/external/get/engage", 10, std::bind(&AutowareStatePanel::onEngageStatus, this, _1)); + + client_engage_ = raw_node_->create_client( + "/api/external/set/engage", rmw_qos_profile_services_default); +} + +void AutowareStatePanel::onGateMode(const autoware_control_msgs::msg::GateMode::ConstSharedPtr msg) +{ + switch (msg->data) { + case autoware_control_msgs::msg::GateMode::AUTO: + gate_mode_label_ptr_->setText("AUTO"); + gate_mode_label_ptr_->setStyleSheet("background-color: #00FF00;"); + break; + + case autoware_control_msgs::msg::GateMode::EXTERNAL: + gate_mode_label_ptr_->setText("EXTERNAL"); + gate_mode_label_ptr_->setStyleSheet("background-color: #FFFF00;"); + break; + + default: + gate_mode_label_ptr_->setText("UNKNOWN"); + gate_mode_label_ptr_->setStyleSheet("background-color: #FF0000;"); + break; + } +} + +void AutowareStatePanel::onAutowareState( + const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg) +{ + if (msg->state == autoware_auto_system_msgs::msg::AutowareState::INITIALIZING) { + autoware_state_label_ptr_->setText("INITIALIZING"); + autoware_state_label_ptr_->setStyleSheet("background-color: #FFFF00;"); + } else if (msg->state == autoware_auto_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE) { + autoware_state_label_ptr_->setText("WAITING_FOR_ROUTE"); + autoware_state_label_ptr_->setStyleSheet("background-color: #FFFF00;"); + } else if (msg->state == autoware_auto_system_msgs::msg::AutowareState::PLANNING) { + autoware_state_label_ptr_->setText("PLANNING"); + autoware_state_label_ptr_->setStyleSheet("background-color: #FFFF00;"); + } else if (msg->state == autoware_auto_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE) { + autoware_state_label_ptr_->setText("WAITING_FOR_ENGAGE"); + autoware_state_label_ptr_->setStyleSheet("background-color: #00FFFF;"); + } else if (msg->state == autoware_auto_system_msgs::msg::AutowareState::DRIVING) { + autoware_state_label_ptr_->setText("DRIVING"); + autoware_state_label_ptr_->setStyleSheet("background-color: #00FF00;"); + } else if (msg->state == autoware_auto_system_msgs::msg::AutowareState::ARRIVED_GOAL) { + autoware_state_label_ptr_->setText("ARRIVED_GOAL"); + autoware_state_label_ptr_->setStyleSheet("background-color: #FF00FF;"); + } else if (msg->state == autoware_auto_system_msgs::msg::AutowareState::FINALIZING) { + autoware_state_label_ptr_->setText("FINALIZING"); + autoware_state_label_ptr_->setStyleSheet("background-color: #FFFF00;"); + } +} + +void AutowareStatePanel::onShift( + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) +{ + switch (msg->report) { + case autoware_auto_vehicle_msgs::msg::GearReport::PARK: + gear_label_ptr_->setText("PARKING"); + break; + case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE: + gear_label_ptr_->setText("REVERSE"); + break; + case autoware_auto_vehicle_msgs::msg::GearReport::DRIVE: + gear_label_ptr_->setText("DRIVE"); + break; + case autoware_auto_vehicle_msgs::msg::GearReport::LOW: + gear_label_ptr_->setText("LOW"); + break; + } +} + +void AutowareStatePanel::onEngageStatus( + const autoware_external_api_msgs::msg::EngageStatus::ConstSharedPtr msg) +{ + engage_status_label_ptr_->setText(QString::fromStdString(Bool2String(msg->engage))); +} + +void AutowareStatePanel::onClickAutowareEngage() +{ + auto req = std::make_shared(); + req->engage = true; + + RCLCPP_INFO(raw_node_->get_logger(), "client request"); + + if (!client_engage_->service_is_ready()) { + RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable"); + return; + } + + client_engage_->async_send_request( + req, [this](rclcpp::Client::SharedFuture result) { + RCLCPP_INFO( + raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code, + result.get()->status.message.c_str()); + }); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareStatePanel, rviz_common::Panel) diff --git a/common/autoware_state_rviz_plugin/src/autoware_state_panel.hpp b/common/autoware_state_rviz_plugin/src/autoware_state_panel.hpp new file mode 100644 index 0000000000000..c9d1f9bc97f2d --- /dev/null +++ b/common/autoware_state_rviz_plugin/src/autoware_state_panel.hpp @@ -0,0 +1,68 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 AUTOWARE_STATE_PANEL_HPP_ +#define AUTOWARE_STATE_PANEL_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace rviz_plugins +{ +class AutowareStatePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit AutowareStatePanel(QWidget * parent = nullptr); + void onInitialize() override; + +public Q_SLOTS: + void onClickAutowareEngage(); + +protected: + void onGateMode(const autoware_control_msgs::msg::GateMode::ConstSharedPtr msg); + void onAutowareState(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg); + void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); + void onEngageStatus(const autoware_external_api_msgs::msg::EngageStatus::ConstSharedPtr msg); + + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Subscription::SharedPtr sub_gate_mode_; + rclcpp::Subscription::SharedPtr + sub_autoware_state_; + rclcpp::Subscription::SharedPtr sub_gear_; + rclcpp::Subscription::SharedPtr sub_engage_; + + rclcpp::Client::SharedPtr client_engage_; + + QLabel * gate_mode_label_ptr_; + QLabel * autoware_state_label_ptr_; + QLabel * gear_label_ptr_; + QLabel * engage_status_label_ptr_; + QPushButton * engage_button_ptr_; +}; + +} // namespace rviz_plugins + +#endif // AUTOWARE_STATE_PANEL_HPP_ diff --git a/common/autoware_vehicle_rviz_plugin/CMakeLists.txt b/common/autoware_vehicle_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..0360d8b8a751a --- /dev/null +++ b/common/autoware_vehicle_rviz_plugin/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_vehicle_rviz_plugin) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) + +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +set(HEADERS + src/tools/turn_signal.hpp + src/tools/console_meter.hpp + src/tools/steering_angle.hpp + src/tools/jsk_overlay_utils.hpp + src/tools/velocity_history.hpp +) + +## Declare a C++ library +ament_auto_add_library(autoware_vehicle_rviz_plugin SHARED + src/tools/turn_signal.cpp + src/tools/console_meter.cpp + src/tools/steering_angle.cpp + src/tools/jsk_overlay_utils.cpp + src/tools/velocity_history.cpp + ${HEADERS} +) + +target_link_libraries(autoware_vehicle_rviz_plugin + ${QT_LIBRARIES}) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + icons + images + plugins +) diff --git a/common/autoware_vehicle_rviz_plugin/README.md b/common/autoware_vehicle_rviz_plugin/README.md new file mode 100644 index 0000000000000..c8a755604cbf3 --- /dev/null +++ b/common/autoware_vehicle_rviz_plugin/README.md @@ -0,0 +1,78 @@ +# autoware_vehicle_rviz_plugin + +This package is including jsk code. +Note that jsk_overlay_utils.cpp and jsk_overlay_utils.hpp are BSD license. + +## Purpose + +This plugin provides a visual and easy-to-understand display of vehicle speed, turn signal and steering status. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| --------------------------------- | ------------------------------------------------------- | ---------------------------------- | +| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle twist | +| `/control/turn_signal_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | +| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | + +## Parameter + +### Core Parameters + +#### ConsoleMeter + +| Name | Type | Default Value | Description | +| ------------------------------- | ------ | -------------------- | ---------------------------------------- | +| `property_text_color_` | QColor | QColor(25, 255, 240) | Text color | +| `property_left_` | int | 128 | Left of the plotter window [px] | +| `property_top_` | int | 128 | Top of the plotter window [px] | +| `property_length_` | int | 256 | Height of the plotter window [px] | +| `property_value_height_offset_` | int | 0 | Height offset of the plotter window [px] | +| `property_value_scale_` | float | 1.0 / 6.667 | Value scale | + +#### SteeringAngle + +| Name | Type | Default Value | Description | +| ------------------------------- | ------ | -------------------- | ---------------------------------------- | +| `property_text_color_` | QColor | QColor(25, 255, 240) | Text color | +| `property_left_` | int | 128 | Left of the plotter window [px] | +| `property_top_` | int | 128 | Top of the plotter window [px] | +| `property_length_` | int | 256 | Height of the plotter window [px] | +| `property_value_height_offset_` | int | 0 | Height offset of the plotter window [px] | +| `property_value_scale_` | float | 1.0 / 6.667 | Value scale | +| `property_handle_angle_scale_` | float | 3.0 | Scale is steering angle to handle angle | + +#### TurnSignal + +| Name | Type | Default Value | Description | +| ------------------ | ---- | ------------- | -------------------------------- | +| `property_left_` | int | 128 | Left of the plotter window [px] | +| `property_top_` | int | 128 | Top of the plotter window [px] | +| `property_width_` | int | 256 | Left of the plotter window [px] | +| `property_height_` | int | 256 | Width of the plotter window [px] | + +#### VelocityHistory + +| Name | Type | Default Value | Description | +| ------------------------------- | ------ | ------------- | -------------------------- | +| `property_velocity_timeout_` | float | 10.0 | Timeout of velocity [s] | +| `property_velocity_alpha_` | float | 1.0 | Alpha of velocity | +| `property_velocity_scale_` | float | 0.3 | Scale of velocity | +| `property_velocity_color_view_` | bool | false | Use Constant Color or not | +| `property_velocity_color_` | QColor | Qt::black | Color of velocity history | +| `property_vel_max_` | float | 3.0 | Color Border Vel Max [m/s] | + +## Assumptions / Known limits + +TBD. + +## Usage + +1. Start rviz and select Add under the Displays panel. + ![select_add](./images/select_add.png) +2. Select any one of the autoware_vehicle_rviz_plugin and press OK. + ![select_vehicle_plugin](./images/select_vehicle_plugin.png) +3. Enter the name of the topic where you want to view the status. + ![select_topic_name](./images/select_topic_name.png) diff --git a/common/autoware_vehicle_rviz_plugin/icons/classes/ConsoleMeter.png b/common/autoware_vehicle_rviz_plugin/icons/classes/ConsoleMeter.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_vehicle_rviz_plugin/icons/classes/ConsoleMeter.png differ diff --git a/common/autoware_vehicle_rviz_plugin/icons/classes/SteeringAngle.png b/common/autoware_vehicle_rviz_plugin/icons/classes/SteeringAngle.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_vehicle_rviz_plugin/icons/classes/SteeringAngle.png differ diff --git a/common/autoware_vehicle_rviz_plugin/icons/classes/TurnSignal.png b/common/autoware_vehicle_rviz_plugin/icons/classes/TurnSignal.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_vehicle_rviz_plugin/icons/classes/TurnSignal.png differ diff --git a/common/autoware_vehicle_rviz_plugin/icons/classes/VelocityHistory.png b/common/autoware_vehicle_rviz_plugin/icons/classes/VelocityHistory.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/autoware_vehicle_rviz_plugin/icons/classes/VelocityHistory.png differ diff --git a/common/autoware_vehicle_rviz_plugin/images/handle.png b/common/autoware_vehicle_rviz_plugin/images/handle.png new file mode 100644 index 0000000000000..8783b8ef67074 Binary files /dev/null and b/common/autoware_vehicle_rviz_plugin/images/handle.png differ diff --git a/common/autoware_vehicle_rviz_plugin/images/select_add.png b/common/autoware_vehicle_rviz_plugin/images/select_add.png new file mode 100644 index 0000000000000..c5130b6092384 Binary files /dev/null and b/common/autoware_vehicle_rviz_plugin/images/select_add.png differ diff --git a/common/autoware_vehicle_rviz_plugin/images/select_topic_name.png b/common/autoware_vehicle_rviz_plugin/images/select_topic_name.png new file mode 100644 index 0000000000000..39481fb38b407 Binary files /dev/null and b/common/autoware_vehicle_rviz_plugin/images/select_topic_name.png differ diff --git a/common/autoware_vehicle_rviz_plugin/images/select_vehicle_plugin.png b/common/autoware_vehicle_rviz_plugin/images/select_vehicle_plugin.png new file mode 100644 index 0000000000000..021da18907983 Binary files /dev/null and b/common/autoware_vehicle_rviz_plugin/images/select_vehicle_plugin.png differ diff --git a/common/autoware_vehicle_rviz_plugin/package.xml b/common/autoware_vehicle_rviz_plugin/package.xml new file mode 100644 index 0000000000000..f04bea3506c41 --- /dev/null +++ b/common/autoware_vehicle_rviz_plugin/package.xml @@ -0,0 +1,26 @@ + + + autoware_vehicle_rviz_plugin + 0.1.0 + The autoware_vehicle_rviz_plugin package + Yukihiro Saito + Apache License 2.0 + ament_cmake_auto + + autoware_auto_vehicle_msgs + autoware_utils + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rviz_common + rviz_default_plugins + rviz_ogre_vendor + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_vehicle_rviz_plugin/plugins/plugin_description.xml b/common/autoware_vehicle_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..1f809c748035b --- /dev/null +++ b/common/autoware_vehicle_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,18 @@ + + + + + + + + + + diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/console_meter.cpp b/common/autoware_vehicle_rviz_plugin/src/tools/console_meter.cpp new file mode 100644 index 0000000000000..632a74170ee54 --- /dev/null +++ b/common/autoware_vehicle_rviz_plugin/src/tools/console_meter.cpp @@ -0,0 +1,216 @@ +// Copyright 2020 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 "console_meter.hpp" + +#include +#include + +#include + +namespace rviz_plugins +{ +ConsoleMeterDisplay::ConsoleMeterDisplay() +{ + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", 128, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", 128, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_length_ = new rviz_common::properties::IntProperty( + "Length", 256, "Length of the plotter window", this, SLOT(updateVisualization()), this); + property_length_->setMin(10); + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_value_scale_ = new rviz_common::properties::FloatProperty( + "Value Scale", 1.0 / 6.667, "Value scale", this, SLOT(updateVisualization()), this); + property_value_scale_->setMin(0.01); +} + +ConsoleMeterDisplay::~ConsoleMeterDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void ConsoleMeterDisplay::onInitialize() +{ + RTDClass::onInitialize(); + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "ConsoleMeterDisplayObject" << count++; + overlay_.reset(new jsk_rviz_plugins::OverlayObject(ss.str())); + + overlay_->show(); + + overlay_->updateTextureSize(property_length_->getInt(), property_length_->getInt()); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void ConsoleMeterDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void ConsoleMeterDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void ConsoleMeterDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + double linear_x = 0; + { + std::lock_guard message_lock(mutex_); + if (!last_msg_ptr_) { + return; + } + linear_x = last_msg_ptr_->longitudinal_velocity; + } + + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // meter + QColor white_color(Qt::white); + white_color.setAlpha(255); + const float velocity_ratio = std::min( + std::max(std::fabs(linear_x) - meter_min_velocity_, 0.0) / + (meter_max_velocity_ - meter_min_velocity_), + 1.0); + const float theta = + (velocity_ratio * (meter_max_angle_ - meter_min_angle_)) + meter_min_angle_ + M_PI_2; + + painter.setPen(QPen(white_color, hand_width_, Qt::SolidLine)); + painter.drawLine( + w * 0.5, h * 0.5, + (w * 0.5) + + (static_cast(w) * 0.5 - (static_cast(hand_width_) * 0.5)) * std::cos(theta), + (h * 0.5) + + (static_cast(h) * 0.5 - (static_cast(hand_width_) * 0.5)) * std::sin(theta)); + + painter.setPen(QPen(white_color, line_width_, Qt::SolidLine)); + painter.drawLine(min_range_line_.x0, min_range_line_.y0, min_range_line_.x1, min_range_line_.y1); + painter.drawLine(max_range_line_.x0, max_range_line_.y0, max_range_line_.x1, max_range_line_.y1); + painter.drawArc( + outer_arc_.x0, outer_arc_.y0, outer_arc_.x1, outer_arc_.y1, outer_arc_.start_angle * 16, + outer_arc_.end_angle * 16); + painter.drawArc( + inner_arc_.x0, inner_arc_.y0, inner_arc_.x1, inner_arc_.y1, inner_arc_.start_angle * 16, + inner_arc_.end_angle * 16); + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(255); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize( + std::max(static_cast(static_cast(w) * property_value_scale_->getFloat()), 1)); + font.setBold(true); + painter.setFont(font); + std::ostringstream velocity_ss; + velocity_ss << std::fixed << std::setprecision(2) << linear_x * 3.6 << "km/h"; + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignCenter | Qt::AlignVCenter, + velocity_ss.str().c_str()); + painter.end(); + updateVisualization(); +} + +void ConsoleMeterDisplay::processMessage( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_ptr_ = msg_ptr; + } + + queueRender(); +} + +void ConsoleMeterDisplay::updateVisualization() +{ + overlay_->updateTextureSize(property_length_->getInt(), property_length_->getInt()); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + const float min_range_theta = meter_max_angle_ + M_PI_2; + const float max_range_theta = meter_min_angle_ + M_PI_2; + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + min_range_line_.x0 = + (w / 2.0) + line_width_ / 2.0 + (static_cast(w) / 8.0) * std::cos(min_range_theta); + min_range_line_.y0 = + (h / 2.0) + line_width_ / 2.0 + (static_cast(h) / 8.0) * std::sin(min_range_theta); + min_range_line_.x1 = + (w / 2.0) + line_width_ / 2.0 + + (static_cast(w) / 2.0 - (line_width_ / 2.0)) * std::cos(min_range_theta); + min_range_line_.y1 = + (h / 2.0) + line_width_ / 2.0 + + (static_cast(h) / 2.0 - (line_width_ / 2.0)) * std::sin(min_range_theta); + max_range_line_.x0 = + (w / 2.0) + line_width_ / 2.0 + (static_cast(w) / 8.0) * std::cos(max_range_theta); + max_range_line_.y0 = + (h / 2.0) + line_width_ / 2.0 + (static_cast(h) / 8.0) * std::sin(max_range_theta); + max_range_line_.x1 = + (w * 0.5) + line_width_ * 0.5 + + (static_cast(w) / 2.0 - (line_width_ * 0.5)) * std::cos(max_range_theta); + max_range_line_.y1 = + (h * 0.5) + line_width_ * 0.5 + + (static_cast(h) / 2.0 - (line_width_ * 0.5)) * std::sin(max_range_theta); + inner_arc_.x0 = line_width_ / 2.0; + inner_arc_.y0 = line_width_ / 2.0; + inner_arc_.x1 = w; + inner_arc_.y1 = h; + inner_arc_.start_angle = autoware_utils::rad2deg(min_range_theta - M_PI); + inner_arc_.end_angle = autoware_utils::rad2deg(max_range_theta - min_range_theta); + outer_arc_.x0 = w * 3 / 8; + outer_arc_.y0 = h * 3 / 8; + outer_arc_.x1 = w / 4; + outer_arc_.y1 = h / 4; + outer_arc_.start_angle = autoware_utils::rad2deg(min_range_theta - M_PI); + outer_arc_.end_angle = autoware_utils::rad2deg(max_range_theta - min_range_theta); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::ConsoleMeterDisplay, rviz_common::Display) diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/console_meter.hpp b/common/autoware_vehicle_rviz_plugin/src/tools/console_meter.hpp new file mode 100644 index 0000000000000..7847a5725274f --- /dev/null +++ b/common/autoware_vehicle_rviz_plugin/src/tools/console_meter.hpp @@ -0,0 +1,93 @@ +// Copyright 2020 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 TOOLS__CONSOLE_METER_HPP_ +#define TOOLS__CONSOLE_METER_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "jsk_overlay_utils.hpp" + +#include +#include +#include +#include +#include + +#include +#endif + +namespace rviz_plugins +{ +class ConsoleMeterDisplay +: public rviz_common::RosTopicDisplay +{ + Q_OBJECT + +public: + ConsoleMeterDisplay(); + ~ConsoleMeterDisplay() override; + + void onInitialize() override; + void onDisable() override; + void onEnable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_length_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::FloatProperty * property_value_scale_; + // QImage hud_; + +private: + static constexpr float meter_min_velocity_ = autoware_utils::kmph2mps(0.f); + static constexpr float meter_max_velocity_ = autoware_utils::kmph2mps(60.f); + static constexpr float meter_min_angle_ = autoware_utils::deg2rad(40.f); + static constexpr float meter_max_angle_ = autoware_utils::deg2rad(320.f); + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + struct Line // for drawLine + { + int x0, y0; + int x1, y1; + }; + Line min_range_line_; + Line max_range_line_; + struct Arc // for drawArc + { + int x0, y0; + int x1, y1; + float start_angle, end_angle; + }; + Arc inner_arc_; + Arc outer_arc_; + + std::mutex mutex_; + autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr last_msg_ptr_; +}; + +} // namespace rviz_plugins + +#endif // TOOLS__CONSOLE_METER_HPP_ diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp b/common/autoware_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp new file mode 100644 index 0000000000000..54e8583dc457c --- /dev/null +++ b/common/autoware_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp @@ -0,0 +1,190 @@ +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "jsk_overlay_utils.hpp" + +#include + +namespace jsk_rviz_plugins +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() { pixel_buffer_->unlock(); } + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() { return pixel_buffer_; } + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject(const std::string & name) : name_(name) +{ + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + hide(); + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + // mOverlayMgr->destroyOverlayElement(panel_); + // delete panel_; + // delete overlay_; +} + +std::string OverlayObject::getName() { return name_; } + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() { return static_cast(texture_); } + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RCLCPP_WARN(rclcpp::get_logger("OverlayObject"), "width=0 is specified as texture size"); + width = 1; + } + if (height == 0) { + RCLCPP_WARN(rclcpp::get_logger("OverlayObject"), "height=0 is specified as texture size"); + height = 1; + } + if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { + if (isTextureReady()) { + Ogre::TextureManager::getSingleton().remove(texture_name); + panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); + } + texture_ = Ogre::TextureManager::getSingleton().createManual( + texture_name, // name + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, // type + width, height, // width & height of the render window + 0, // number of mipmaps + Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use + Ogre::TU_DEFAULT // usage + ); + panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + + panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + } +} + +ScopedPixelBuffer OverlayObject::getBuffer() +{ + if (isTextureReady()) { + return ScopedPixelBuffer(texture_->getBuffer()); + } else { + return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); + } +} + +void OverlayObject::setPosition(double left, double top) { panel_->setPosition(left, top); } + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() { return overlay_->isVisible(); } + +unsigned int OverlayObject::getTextureWidth() +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} + +} // namespace jsk_rviz_plugins diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.hpp b/common/autoware_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.hpp new file mode 100644 index 0000000000000..374e126b0f24d --- /dev/null +++ b/common/autoware_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.hpp @@ -0,0 +1,124 @@ +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef TOOLS__JSK_OVERLAY_UTILS_HPP_ +#define TOOLS__JSK_OVERLAY_UTILS_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +// see OGRE/OgrePrerequisites.h +// #define OGRE_VERSION +// ((OGRE_VERSION_MAJOR << 16) | (OGRE_VERSION_MINOR << 8) | OGRE_VERSION_PATCH) +#if OGRE_VERSION < ((1 << 16) | (9 << 8) | 0) +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif + +#include +#include +#include + +namespace jsk_rviz_plugins +{ +class OverlayObject; + +class ScopedPixelBuffer +{ +public: + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + virtual ~ScopedPixelBuffer(); + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + virtual QImage getQImage(unsigned int width, unsigned int height); + virtual QImage getQImage(OverlayObject & overlay); + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; + +private: +}; + +// this is a class for put overlay object on rviz 3D panel. +// This class suppose to be instantiated in onInitialize method +// of rviz::Display class. +class OverlayObject +{ +public: + typedef std::shared_ptr Ptr; + + explicit OverlayObject(const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName(); + virtual void hide(); + virtual void show(); + virtual bool isTextureReady(); + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition(double left, double top); + virtual void setDimensions(double width, double height); + virtual bool isVisible(); + virtual unsigned int getTextureWidth(); + virtual unsigned int getTextureHeight(); + +protected: + const std::string name_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; + +private: +}; + +// Ogre::Overlay* createOverlay(std::string name); +// Ogre::PanelOverlayElement* createOverlayPanel(Ogre::Overlay* overlay); +// Ogre::MaterialPtr createOverlayMaterial(Ogre::Overlay* overlay); +} // namespace jsk_rviz_plugins + +#endif // TOOLS__JSK_OVERLAY_UTILS_HPP_ diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/steering_angle.cpp b/common/autoware_vehicle_rviz_plugin/src/tools/steering_angle.cpp new file mode 100644 index 0000000000000..f09c2c2f14f79 --- /dev/null +++ b/common/autoware_vehicle_rviz_plugin/src/tools/steering_angle.cpp @@ -0,0 +1,181 @@ +// Copyright 2020 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 "steering_angle.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +SteeringAngleDisplay::SteeringAngleDisplay() +: handle_image_(std::string( + ament_index_cpp::get_package_share_directory("autoware_vehicle_rviz_plugin") + + "/images/handle.png") + .c_str()) +{ + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", 128, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", 128, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_length_ = new rviz_common::properties::IntProperty( + "Length", 256, "Length of the plotter window", this, SLOT(updateVisualization()), this); + property_length_->setMin(10); + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_value_scale_ = new rviz_common::properties::FloatProperty( + "Value Scale", 1.0 / 6.667, "Value scale", this, SLOT(updateVisualization()), this); + property_value_scale_->setMin(0.01); + property_handle_angle_scale_ = new rviz_common::properties::FloatProperty( + "Scale", 3.0, "Scale is steering angle to handle angle ", this, SLOT(updateVisualization()), + this); + property_handle_angle_scale_->setMin(0.1); +} + +SteeringAngleDisplay::~SteeringAngleDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void SteeringAngleDisplay::onInitialize() +{ + RTDClass::onInitialize(); + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "SteeringAngleDisplayObject" << count++; + overlay_.reset(new jsk_rviz_plugins::OverlayObject(ss.str())); + + overlay_->show(); + + updateVisualization(); +} + +void SteeringAngleDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void SteeringAngleDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void SteeringAngleDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + double steering = 0; + { + std::lock_guard message_lock(mutex_); + if (!last_msg_ptr_) { + return; + } + steering = last_msg_ptr_->steering_tire_angle; + } + + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + if (!buffer.getPixelBuffer()) { + return; + } + + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(255); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + + const int w = overlay_->getTextureWidth(); + const int h = overlay_->getTextureHeight(); + + QMatrix rotation_matrix; + rotation_matrix.rotate( + std::round(property_handle_angle_scale_->getFloat() * (steering / M_PI) * -180.0)); + + // else + // rotation_matrix.rotate + // ((property_handle_angle_scale_->getFloat() * (msg_ptr->data / M_PI) * -180.0)); + int handle_image_width = handle_image_.width(), handle_image_height = handle_image_.height(); + QPixmap rotate_handle_image; + rotate_handle_image = handle_image_.transformed(rotation_matrix); + rotate_handle_image = rotate_handle_image.copy( + (rotate_handle_image.width() - handle_image_width) / 2, + (rotate_handle_image.height() - handle_image_height) / 2, handle_image_width, + handle_image_height); + painter.drawPixmap( + 0, 0, property_length_->getInt(), property_length_->getInt(), rotate_handle_image); + + QFont font = painter.font(); + font.setPixelSize( + std::max(static_cast((static_cast(w)) * property_value_scale_->getFloat()), 1)); + font.setBold(true); + painter.setFont(font); + std::ostringstream steering_angle_ss; + steering_angle_ss << std::fixed << std::setprecision(1) << steering * 180.0 / M_PI << "deg"; + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignCenter | Qt::AlignVCenter, + steering_angle_ss.str().c_str()); + + painter.end(); +} + +void SteeringAngleDisplay::processMessage( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_ptr_ = msg_ptr; + } + + queueRender(); +} + +void SteeringAngleDisplay::updateVisualization() +{ + overlay_->updateTextureSize(property_length_->getInt(), property_length_->getInt()); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::SteeringAngleDisplay, rviz_common::Display) diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/steering_angle.hpp b/common/autoware_vehicle_rviz_plugin/src/tools/steering_angle.hpp new file mode 100644 index 0000000000000..d56b5a8d742b9 --- /dev/null +++ b/common/autoware_vehicle_rviz_plugin/src/tools/steering_angle.hpp @@ -0,0 +1,73 @@ +// Copyright 2020 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 TOOLS__STEERING_ANGLE_HPP_ +#define TOOLS__STEERING_ANGLE_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "jsk_overlay_utils.hpp" + +#include +#include +#include +#include + +#include +#endif + +namespace rviz_plugins +{ +class SteeringAngleDisplay +: public rviz_common::RosTopicDisplay +{ + Q_OBJECT + +public: + SteeringAngleDisplay(); + ~SteeringAngleDisplay() override; + + void onInitialize() override; + void onDisable() override; + void onEnable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) override; + + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_length_; + rviz_common::properties::FloatProperty * property_handle_angle_scale_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::FloatProperty * property_value_scale_; + QPixmap handle_image_; + // QImage hud_; + +private: + std::mutex mutex_; + autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr last_msg_ptr_; +}; + +} // namespace rviz_plugins + +#endif // TOOLS__STEERING_ANGLE_HPP_ diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/turn_signal.cpp b/common/autoware_vehicle_rviz_plugin/src/tools/turn_signal.cpp new file mode 100644 index 0000000000000..19e3c8c9e0978 --- /dev/null +++ b/common/autoware_vehicle_rviz_plugin/src/tools/turn_signal.cpp @@ -0,0 +1,185 @@ +// Copyright 2020 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 "turn_signal.hpp" + +#include +#include +#include + +namespace rviz_plugins +{ +TurnSignalDisplay::TurnSignalDisplay() +{ + property_left_ = new rviz_common::properties::IntProperty( + "Left", 128, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", 128, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_width_ = new rviz_common::properties::IntProperty( + "Width", 256, "Width of the plotter window", this, SLOT(updateVisualization()), this); + property_width_->setMin(10); + property_height_ = new rviz_common::properties::IntProperty( + "Height", 256, "Height of the plotter window", this, SLOT(updateVisualization()), this); + property_height_->setMin(10); +} + +TurnSignalDisplay::~TurnSignalDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void TurnSignalDisplay::onInitialize() +{ + RTDClass::onInitialize(); + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "TurnSignalDisplayObject" << count++; + overlay_.reset(new jsk_rviz_plugins::OverlayObject(ss.str())); + + overlay_->show(); + + overlay_->updateTextureSize(property_width_->getInt(), property_height_->getInt()); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void TurnSignalDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void TurnSignalDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void TurnSignalDisplay::processMessage( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_ptr_ = msg_ptr; + } + + queueRender(); +} + +void TurnSignalDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + unsigned int signal_type; + { + std::lock_guard message_lock(mutex_); + if (!last_msg_ptr_) { + return; + } + signal_type = last_msg_ptr_->report; + } + + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + // turn signal color + QColor white_color(Qt::white); + white_color.setAlpha(255); + if (signal_type == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT) { + painter.setPen(QPen(white_color, static_cast(2), Qt::DotLine)); + painter.drawPolygon(left_arrow_polygon_, 7); + painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); + painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); + painter.drawPolygon(right_arrow_polygon_, 7); + } else if (signal_type == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT) { + painter.setPen(QPen(white_color, static_cast(2), Qt::DotLine)); + painter.drawPolygon(right_arrow_polygon_, 7); + painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); + painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); + painter.drawPolygon(left_arrow_polygon_, 7); + } else if (signal_type == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE) { + painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); + painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); + painter.drawPolygon(right_arrow_polygon_, 7); + painter.drawPolygon(left_arrow_polygon_, 7); + } else { + painter.setPen(QPen(white_color, static_cast(2), Qt::DotLine)); + painter.drawPolygon(right_arrow_polygon_, 7); + painter.drawPolygon(left_arrow_polygon_, 7); + } + painter.end(); + updateVisualization(); +} + +void TurnSignalDisplay::updateVisualization() +{ + overlay_->updateTextureSize(property_width_->getInt(), property_height_->getInt()); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + + const int w = overlay_->getTextureWidth(); + const int h = overlay_->getTextureHeight(); + + right_arrow_polygon_[0].setX(static_cast(w) * 5.0 / 5.0); + right_arrow_polygon_[0].setY(static_cast(h) * 1.0 / 2.0); + right_arrow_polygon_[1].setX(static_cast(w) * 4.0 / 5.0); + right_arrow_polygon_[1].setY(static_cast(h) * 1.0 / 5.0); + right_arrow_polygon_[2].setX(static_cast(w) * 4.0 / 5.0); + right_arrow_polygon_[2].setY(static_cast(h) * 2.0 / 5.0); + right_arrow_polygon_[3].setX(static_cast(w) * 3.0 / 5.0); + right_arrow_polygon_[3].setY(static_cast(h) * 2.0 / 5.0); + right_arrow_polygon_[4].setX(static_cast(w) * 3.0 / 5.0); + right_arrow_polygon_[4].setY(static_cast(h) * 3.0 / 5.0); + right_arrow_polygon_[5].setX(static_cast(w) * 4.0 / 5.0); + right_arrow_polygon_[5].setY(static_cast(h) * 3.0 / 5.0); + right_arrow_polygon_[6].setX(static_cast(w) * 4.0 / 5.0); + right_arrow_polygon_[6].setY(static_cast(h) * 4.0 / 5.0); + + left_arrow_polygon_[0].setX(static_cast(w) * 0.0 / 5.0); + left_arrow_polygon_[0].setY(static_cast(h) * 1.0 / 2.0); + left_arrow_polygon_[1].setX(static_cast(w) * 1.0 / 5.0); + left_arrow_polygon_[1].setY(static_cast(h) * 1.0 / 5.0); + left_arrow_polygon_[2].setX(static_cast(w) * 1.0 / 5.0); + left_arrow_polygon_[2].setY(static_cast(h) * 2.0 / 5.0); + left_arrow_polygon_[3].setX(static_cast(w) * 2.0 / 5.0); + left_arrow_polygon_[3].setY(static_cast(h) * 2.0 / 5.0); + left_arrow_polygon_[4].setX(static_cast(w) * 2.0 / 5.0); + left_arrow_polygon_[4].setY(static_cast(h) * 3.0 / 5.0); + left_arrow_polygon_[5].setX(static_cast(w) * 1.0 / 5.0); + left_arrow_polygon_[5].setY(static_cast(h) * 3.0 / 5.0); + left_arrow_polygon_[6].setX(static_cast(w) * 1.0 / 5.0); + left_arrow_polygon_[6].setY(static_cast(h) * 4.0 / 5.0); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::TurnSignalDisplay, rviz_common::Display) diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/turn_signal.hpp b/common/autoware_vehicle_rviz_plugin/src/tools/turn_signal.hpp new file mode 100644 index 0000000000000..06a1d2e5f0d54 --- /dev/null +++ b/common/autoware_vehicle_rviz_plugin/src/tools/turn_signal.hpp @@ -0,0 +1,70 @@ +// Copyright 2020 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 TOOLS__TURN_SIGNAL_HPP_ +#define TOOLS__TURN_SIGNAL_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "jsk_overlay_utils.hpp" + +#include +#include + +#include +#include +#endif + +namespace rviz_plugins +{ +class TurnSignalDisplay +: public rviz_common::RosTopicDisplay +{ + Q_OBJECT + +public: + TurnSignalDisplay(); + ~TurnSignalDisplay() override; + + void onInitialize() override; + void onDisable() override; + void onEnable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_width_; + rviz_common::properties::IntProperty * property_height_; + // QImage hud_; + +private: + QPointF right_arrow_polygon_[7]; + QPointF left_arrow_polygon_[7]; + + std::mutex mutex_; + autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr last_msg_ptr_; +}; + +} // namespace rviz_plugins + +#endif // TOOLS__TURN_SIGNAL_HPP_ diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/velocity_history.cpp b/common/autoware_vehicle_rviz_plugin/src/tools/velocity_history.cpp new file mode 100644 index 0000000000000..b5db539f437cb --- /dev/null +++ b/common/autoware_vehicle_rviz_plugin/src/tools/velocity_history.cpp @@ -0,0 +1,211 @@ +// Copyright 2020 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 "velocity_history.hpp" + +#include + +#include +#include +#define EIGEN_MPL2_ONLY +#include + +namespace rviz_plugins +{ +std::unique_ptr VelocityHistoryDisplay::gradation( + const QColor & color_min, const QColor & color_max, const double ratio) +{ + std::unique_ptr color_ptr(new Ogre::ColourValue); + color_ptr->g = color_max.greenF() * ratio + color_min.greenF() * (1.0 - ratio); + color_ptr->r = color_max.redF() * ratio + color_min.redF() * (1.0 - ratio); + color_ptr->b = color_max.blueF() * ratio + color_min.blueF() * (1.0 - ratio); + + return color_ptr; +} + +std::unique_ptr VelocityHistoryDisplay::setColorDependsOnVelocity( + const double vel_max, const double cmd_vel) +{ + const double cmd_vel_abs = std::fabs(cmd_vel); + const double vel_min = 0.0; + + std::unique_ptr color_ptr(new Ogre::ColourValue()); + if (vel_min < cmd_vel_abs && cmd_vel_abs <= (vel_max / 2.0)) { + double ratio = (cmd_vel_abs - vel_min) / (vel_max / 2.0 - vel_min); + color_ptr = gradation(Qt::red, Qt::yellow, ratio); + } else if ((vel_max / 2.0) < cmd_vel_abs && cmd_vel_abs <= vel_max) { + double ratio = (cmd_vel_abs - vel_max / 2.0) / (vel_max - vel_max / 2.0); + color_ptr = gradation(Qt::yellow, Qt::green, ratio); + } else if (vel_max < cmd_vel_abs) { + *color_ptr = Ogre::ColourValue::Green; + } else { + *color_ptr = Ogre::ColourValue::Red; + } + + return color_ptr; +} + +VelocityHistoryDisplay::VelocityHistoryDisplay() +{ + property_velocity_timeout_ = new rviz_common::properties::FloatProperty( + "Timeout", 10.0, "", this, SLOT(updateVisualization()), this); + property_velocity_timeout_->setMin(0.0); + property_velocity_timeout_->setMax(100000.0); + property_velocity_alpha_ = new rviz_common::properties::FloatProperty( + "Alpha", 1.0, "", this, SLOT(updateVisualization()), this); + property_velocity_alpha_->setMin(0.0); + property_velocity_alpha_->setMax(1.0); + property_velocity_scale_ = new rviz_common::properties::FloatProperty( + "Scale", 0.3, "", this, SLOT(updateVisualization()), this); + property_velocity_scale_->setMin(0.1); + property_velocity_scale_->setMax(10.0); + property_velocity_color_view_ = new rviz_common::properties::BoolProperty( + "Constant Color", false, "", this, SLOT(updateVisualization()), this); + property_velocity_color_ = new rviz_common::properties::ColorProperty( + "Color", Qt::black, "", property_velocity_color_view_, SLOT(updateVisualization()), this); + property_vel_max_ = new rviz_common::properties::FloatProperty( + "Color Border Vel Max", 3.0, "[m/s]", this, SLOT(updateVisualization()), this); + property_vel_max_->setMin(0.0); +} + +VelocityHistoryDisplay::~VelocityHistoryDisplay() +{ + if (initialized()) { + scene_manager_->destroyManualObject(velocity_manual_object_); + } +} + +void VelocityHistoryDisplay::onInitialize() +{ + RTDClass::onInitialize(); + + velocity_manual_object_ = scene_manager_->createManualObject(); + velocity_manual_object_->setDynamic(true); + scene_node_->attachObject(velocity_manual_object_); +} + +void VelocityHistoryDisplay::reset() +{ + RTDClass::reset(); + velocity_manual_object_->clear(); +} + +bool VelocityHistoryDisplay::validateFloats( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr) +{ + if (!rviz_common::validateFloats(msg_ptr->longitudinal_velocity)) { + return false; + } + + return true; +} + +void VelocityHistoryDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + updateVisualization(); +} + +void VelocityHistoryDisplay::processMessage( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + if (!validateFloats(msg_ptr)) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + std_msgs::msg::Header header; + header.stamp = msg_ptr->header.stamp; + header.frame_id = "base_link"; + if (!context_->getFrameManager()->getTransform(header, position, orientation)) { + RCLCPP_DEBUG( + rclcpp::get_logger("VelocityHistoryDisplay"), + "Error transforming from frame '%s' to frame '%s'", header.frame_id.c_str(), + qPrintable(fixed_frame_)); + } + + { + std::lock_guard message_lock(mutex_); + histories_.emplace_back(msg_ptr, position); + } + queueRender(); +} + +void VelocityHistoryDisplay::updateVisualization() +{ + std::lock_guard message_lock(mutex_); + if (histories_.empty()) { + return; + } + velocity_manual_object_->clear(); + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + rclcpp::Time current_time = rviz_ros_node_.lock()->get_raw_node()->get_clock()->now(); + + while (!histories_.empty()) { + if ( + property_velocity_timeout_->getFloat() < + (current_time - std::get<0>(histories_.front())->header.stamp).seconds()) { + histories_.pop_front(); + } else { + break; + } + } + + // std::cout << __LINE__ << ":" <(histories_.front()) <estimateVertexCount(histories_.size()); + velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_POINT_LIST); + + for (auto & history : histories_) { + Ogre::ColourValue color; + if (property_velocity_color_view_->getBool()) { + color = rviz_common::properties::qtToOgre(property_velocity_color_->getColor()); + } else { + /* color change depending on velocity */ + std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( + property_vel_max_->getFloat(), std::get<0>(history)->longitudinal_velocity); + color = *dynamic_color_ptr; + } + color.a = 1.0 - (current_time - std::get<0>(history)->header.stamp).seconds() / + property_velocity_timeout_->getFloat(); + color.a = std::min(std::max(color.a, 0.0f), 1.0f); + // std::cout << __LINE__ << ":" <(histories_.front()) <getFloat(); + velocity_manual_object_->position( + std::get<1>(history).x, std::get<1>(history).y, + std::get<1>(history).z + + std::get<0>(history)->longitudinal_velocity * property_velocity_scale_->getFloat()); + velocity_manual_object_->colour(color); + } + velocity_manual_object_->end(); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::VelocityHistoryDisplay, rviz_common::Display) diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/velocity_history.hpp b/common/autoware_vehicle_rviz_plugin/src/tools/velocity_history.hpp new file mode 100644 index 0000000000000..9b5819df98bef --- /dev/null +++ b/common/autoware_vehicle_rviz_plugin/src/tools/velocity_history.hpp @@ -0,0 +1,80 @@ +// Copyright 2020 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 TOOLS__VELOCITY_HISTORY_HPP_ +#define TOOLS__VELOCITY_HISTORY_HPP_ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +class VelocityHistoryDisplay +: public rviz_common::RosTopicDisplay +{ + Q_OBJECT + +public: + VelocityHistoryDisplay(); + ~VelocityHistoryDisplay() override; + + void onInitialize() override; + void reset() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; + std::unique_ptr setColorDependsOnVelocity( + const double vel_max, const double cmd_vel); + std::unique_ptr gradation( + const QColor & color_min, const QColor & color_max, const double ratio); + Ogre::ManualObject * velocity_manual_object_; + rviz_common::properties::FloatProperty * property_velocity_timeout_; + rviz_common::properties::FloatProperty * property_velocity_alpha_; + rviz_common::properties::FloatProperty * property_velocity_scale_; + rviz_common::properties::BoolProperty * property_velocity_color_view_; + rviz_common::properties::ColorProperty * property_velocity_color_; + rviz_common::properties::FloatProperty * property_vel_max_; + +private: + std::deque< + std::tuple> + histories_; + bool validateFloats( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr); + std::mutex mutex_; +}; + +} // namespace rviz_plugins + +#endif // TOOLS__VELOCITY_HISTORY_HPP_ diff --git a/common/polar_grid/CMakeLists.txt b/common/polar_grid/CMakeLists.txt new file mode 100644 index 0000000000000..8d9b69fe0e8c1 --- /dev/null +++ b/common/polar_grid/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.5) +project(polar_grid) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Qt5 ${rviz_QT_VERSION} EXACT 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(polar_grid SHARED + src/polar_grid_display.cpp + src/polar_grid_display.hpp +) + +target_link_libraries(polar_grid + ${QT_LIBRARIES} +) + +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/common/polar_grid/Readme.md b/common/polar_grid/Readme.md new file mode 100644 index 0000000000000..da89bdb458208 --- /dev/null +++ b/common/polar_grid/Readme.md @@ -0,0 +1,17 @@ +# Polar Grid + +## Purpose + +This plugin displays polar grid around ego vehicle in Rviz. + +### Core Parameters + +| Name | Type | Default Value | Explanation | +| --------------- | ----- | ------------- | -------------------------------------- | +| `Max Range` | float | 200.0f | max range for polar grid. [m] | +| `Wave Velocity` | float | 100.0f | wave ring velocity. [m/s] | +| `Delta Range` | float | 10.0f | wave ring distance for polar grid. [m] | + +## Assumptions / Known limits + +TBD. diff --git a/common/polar_grid/icons/classes/PolarGridDisplay.png b/common/polar_grid/icons/classes/PolarGridDisplay.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/polar_grid/icons/classes/PolarGridDisplay.png differ diff --git a/common/polar_grid/package.xml b/common/polar_grid/package.xml new file mode 100644 index 0000000000000..c11858c4dcc98 --- /dev/null +++ b/common/polar_grid/package.xml @@ -0,0 +1,28 @@ + + + polar_grid + 0.1.0 + The polar_grid package + + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rviz_common + rviz_default_plugins + rviz_ogre_vendor + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + + diff --git a/common/polar_grid/plugins/plugin_description.xml b/common/polar_grid/plugins/plugin_description.xml new file mode 100644 index 0000000000000..1ccaf8725d4de --- /dev/null +++ b/common/polar_grid/plugins/plugin_description.xml @@ -0,0 +1,6 @@ + + + + diff --git a/common/polar_grid/src/polar_grid_display.cpp b/common/polar_grid/src/polar_grid_display.cpp new file mode 100644 index 0000000000000..b397a21eb0d5f --- /dev/null +++ b/common/polar_grid/src/polar_grid_display.cpp @@ -0,0 +1,240 @@ +// Copyright 2020 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. +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "polar_grid_display.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace rviz_plugins +{ +PolarGridDisplay::PolarGridDisplay() : Display(), wave_range_(0.0) +{ + frame_property_ = new rviz_common::properties::TfFrameProperty( + "Reference Frame", rviz_common::properties::TfFrameProperty::FIXED_FRAME_STRING, + "The TF frame this grid will use for its origin.", this, 0, true); + + color_property_ = new rviz_common::properties::ColorProperty( + "Color", Qt::white, "The color of the grid lines.", this, SLOT(updatePlane())); + + d_range_property_ = new rviz_common::properties::FloatProperty( + "Delta Range", 10.0f, "Delta Range[m].", this, SLOT(updatePlane())); + d_range_property_->setMin(0.1f); + d_range_property_->setMax(100.0f); + + max_range_property_ = new rviz_common::properties::FloatProperty( + "Max Range", 200.0f, "Max Range[m].", this, SLOT(updatePlane())); + max_range_property_->setMin(0.0f); + max_range_property_->setMax(500.0f); + max_alpha_property_ = new rviz_common::properties::FloatProperty( + "Max Alpha", 1.0f, "The amount of transparency to apply to the grid lines.", this, + SLOT(updatePlane())); + max_alpha_property_->setMin(0.0f); + max_alpha_property_->setMax(1.0f); + min_alpha_property_ = new rviz_common::properties::FloatProperty( + "Min Alpha", 0.2f, "The amount of transparency to apply to the grid lines.", this, + SLOT(updatePlane())); + min_alpha_property_->setMin(0.0f); + min_alpha_property_->setMax(1.0f); + wave_velocity_property_ = new rviz_common::properties::FloatProperty( + "Wave Velocity", 100.0f, "Wave Velocity [m/s]", this, SLOT(updatePlane())); + wave_velocity_property_->setMin(0.0f); + wave_color_property_ = new rviz_common::properties::ColorProperty( + "Wave Color", Qt::white, "The color of the grid lines.", this, SLOT(updatePlane())); + + max_wave_alpha_property_ = new rviz_common::properties::FloatProperty( + "Max Wave Alpha", 1.0f, "The amount of transparency to apply to the grid lines.", this, + SLOT(updatePlane())); + max_wave_alpha_property_->setMin(0.0f); + max_wave_alpha_property_->setMax(1.0f); + min_wave_alpha_property_ = new rviz_common::properties::FloatProperty( + "Min Wave Alpha", 0.2f, "The amount of transparency to apply to the grid lines.", this, + SLOT(updatePlane())); + min_wave_alpha_property_->setMin(0.0f); + min_wave_alpha_property_->setMax(1.0f); +} + +PolarGridDisplay::~PolarGridDisplay() +{ + if (initialized()) { + scene_manager_->destroyManualObject(rings_manual_object_); + scene_manager_->destroyManualObject(wave_manual_object_); + } +} + +void PolarGridDisplay::onInitialize() +{ + rings_manual_object_ = scene_manager_->createManualObject(); + rings_manual_object_->setDynamic(true); + scene_node_->attachObject(rings_manual_object_); + wave_manual_object_ = scene_manager_->createManualObject(); + wave_manual_object_->setDynamic(true); + scene_node_->attachObject(wave_manual_object_); + + frame_property_->setFrameManager(context_->getFrameManager()); + updatePlane(); +} + +void PolarGridDisplay::reset() +{ + rings_manual_object_->clear(); + wave_manual_object_->clear(); +} + +void PolarGridDisplay::update(float /*dt*/, float ros_dt) +{ + QString qframe = frame_property_->getFrame(); + std::string frame = qframe.toStdString(); + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (context_->getFrameManager()->getTransform( + frame, rclcpp::Time(0, 0, context_->getClock()->get_clock_type()), position, orientation)) { + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); + setStatus(rviz_common::properties::StatusProperty::Ok, "Transform", "Transform OK"); + } else { + std::string error; + if (context_->getFrameManager()->transformHasProblems( + frame, rclcpp::Time(0, 0, context_->getClock()->get_clock_type()), error)) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Transform", QString::fromStdString(error)); + } else { + setStatus( + rviz_common::properties::StatusProperty::Error, "Transform", + "Could not transform from [" + qframe + "] to [" + fixed_frame_ + "]"); + } + } + wave_range_ += ros_dt / 1e9 /* sec */ * wave_velocity_property_->getFloat(); + wave_range_ = std::fmod(wave_range_, max_range_property_->getFloat()); + wave_manual_object_->clear(); + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + const float d_theta = 3.6 * M_PI / 180.0; + const float d_alpha = std::min( + std::max( + static_cast(max_wave_alpha_property_->getFloat()) - + static_cast(min_wave_alpha_property_->getFloat()), + 0.0f), + 1.0f); + Ogre::ColourValue color; + color = rviz_common::properties::qtToOgre(wave_color_property_->getColor()); + color.a = max_wave_alpha_property_->getFloat(); + wave_manual_object_->estimateVertexCount(static_cast(2 * M_PI / d_theta)); + wave_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); + + color.a = d_alpha * (1.0 - (wave_range_ / max_range_property_->getFloat())) + + min_wave_alpha_property_->getFloat(); + color.a = std::max(color.a, min_wave_alpha_property_->getFloat()); + for (float theta = 0.0; theta < 2.0 * M_PI + d_theta; theta += d_theta) { + wave_manual_object_->position( + wave_range_ * static_cast(std::cos(theta)), + wave_range_ * static_cast(std::sin(theta)), 0.0); + wave_manual_object_->colour(color); + } + wave_manual_object_->end(); + + context_->queueRender(); +} + +void PolarGridDisplay::updatePlane() +{ + rings_manual_object_->clear(); + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + + const float d_theta = 3.6 * M_PI / 180.0; + const float d_range = d_range_property_->getFloat(); + const float d_alpha = std::min( + std::max( + static_cast(max_alpha_property_->getFloat()) - + static_cast(min_alpha_property_->getFloat()), + 0.0f), + 1.0f); + const float epsilon = 0.001; + const float max_range = max_range_property_->getFloat() + epsilon; + Ogre::ColourValue color; + color = rviz_common::properties::qtToOgre(color_property_->getColor()); + color.a = max_alpha_property_->getFloat(); + rings_manual_object_->estimateVertexCount( + static_cast(max_range / d_range) + static_cast(2.0f * M_PI / d_theta)); + rings_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); + for (int r = d_range; r <= max_range; r += d_range) { + for (float theta = 0.0; theta < 2 * M_PI; theta += d_theta) { + { + rings_manual_object_->position( + static_cast(r) * std::cos(theta), static_cast(r) * std::sin(theta), 0.0); + rings_manual_object_->colour(color); + rings_manual_object_->position( + static_cast(r) * std::cos(theta + d_theta), + static_cast(r) * std::sin(theta + d_theta), 0.0); + rings_manual_object_->colour(color); + } + } + color.a -= d_alpha / (max_range / d_range); + color.a = std::max(color.a, min_alpha_property_->getFloat()); + } + + rings_manual_object_->end(); + + context_->queueRender(); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::PolarGridDisplay, rviz_common::Display) diff --git a/common/polar_grid/src/polar_grid_display.hpp b/common/polar_grid/src/polar_grid_display.hpp new file mode 100644 index 0000000000000..b42cc42a810e3 --- /dev/null +++ b/common/polar_grid/src/polar_grid_display.hpp @@ -0,0 +1,103 @@ +// Copyright 2020 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. +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef POLAR_GRID_DISPLAY_HPP_ +#define POLAR_GRID_DISPLAY_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +/** + * \class PolarGridDisplay + * \brief Displays a grid in either the XY, YZ, or XZ plane. + * + * For more information see Grid + */ +class PolarGridDisplay : public rviz_common::Display +{ + Q_OBJECT + +public: + PolarGridDisplay(); + virtual ~PolarGridDisplay(); + + // Overrides from Display + virtual void onInitialize(); + virtual void update(float dt, float ros_dt); + void reset() override; + +private Q_SLOTS: + void updatePlane(); + +private: + Ogre::ManualObject * rings_manual_object_; + Ogre::ManualObject * wave_manual_object_; + float wave_range_; + + rviz_common::properties::TfFrameProperty * frame_property_; + rviz_common::properties::FloatProperty * d_range_property_; + rviz_common::properties::FloatProperty * max_range_property_; + rviz_common::properties::FloatProperty * max_alpha_property_; + rviz_common::properties::FloatProperty * min_alpha_property_; + rviz_common::properties::ColorProperty * color_property_; + rviz_common::properties::FloatProperty * wave_velocity_property_; + rviz_common::properties::ColorProperty * wave_color_property_; + rviz_common::properties::FloatProperty * max_wave_alpha_property_; + rviz_common::properties::FloatProperty * min_wave_alpha_property_; +}; + +} // namespace rviz_plugins + +#endif // POLAR_GRID_DISPLAY_HPP_