diff --git a/common/tier4_localization_rviz_plugin/CMakeLists.txt b/common/tier4_localization_rviz_plugin/CMakeLists.txt index 847c5e8af5fbc..eba48f27635e3 100644 --- a/common/tier4_localization_rviz_plugin/CMakeLists.txt +++ b/common/tier4_localization_rviz_plugin/CMakeLists.txt @@ -15,6 +15,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_history/pose_history_display.cpp src/pose_history_footprint/display.hpp src/pose_history_footprint/display.cpp + src/pose_with_covariance_history/pose_with_covariance_history_display.hpp + src/pose_with_covariance_history/pose_with_covariance_history_display.cpp ) target_link_libraries(${PROJECT_NAME} diff --git a/common/tier4_localization_rviz_plugin/README.md b/common/tier4_localization_rviz_plugin/README.md index 60a923f15b563..5f5e35ea3dc19 100644 --- a/common/tier4_localization_rviz_plugin/README.md +++ b/common/tier4_localization_rviz_plugin/README.md @@ -2,20 +2,31 @@ ## Purpose -This plugin can display the history of the localization obtained by ekf_localizer or ndt_scan_matching. +This plugin can display the localization history obtained by ekf_localizer, ndt_scan_matching, and GNSS. +If the uncertainty of the estimated pose is given, it can also be displayed. ## 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 | +### Pose History + +| Name | Type | Description | +| ------------ | --------------------------------- | ----------------------------------------------------------------------------------------------------- | +| `input/pose` | `geometry_msgs::msg::PoseStamped` | In input/pose, put the result of localization calculated by ekf_localizer, ndt_scan_matching, or GNSS | + +### Pose With Covariance History + +| Name | Type | Description | +| ---------------------------- | ----------------------------------------------- | --------------------------------------------------------------------------------------------------------------------- | +| `input/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | In input/pose_with_covariance, put the result of localization calculated by ekf_localizer, ndt_scan_matching, or GNSS | ## Parameters ### Core Parameters +### Pose History + | Name | Type | Default Value | Description | | ----------------------- | ------ | ------------- | -------------------------- | | `property_buffer_size_` | int | 100 | Buffer size of topic | @@ -24,6 +35,26 @@ This plugin can display the history of the localization obtained by ekf_localize | `property_line_alpha_` | float | 1.0 | Alpha of Line property | | `property_line_color_` | QColor | Qt::white | Color of Line property | +### Pose With Covariance History + +| Name | Type | Default Value | Description | +| ------------------------------- | ------ | -------------- | -------------------------------- | +| `property_buffer_size_` | int | 100 | Buffer size of topic | +| `property_path_view_` | bool | true | Use path property or not | +| `property_shape_type_` | string | Line | Line or Arrow | +| `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 | +| `property_arrow_shaft_length` | float | 0.3 | Shaft length of Arrow property | +| `property_arrow_shaft_diameter` | float | 0.15 | Shaft diameter of Arrow property | +| `property_arrow_head_length` | float | 0.2 | Head length of Arrow property | +| `property_arrow_head_diameter` | float | 0.3 | Head diameter of Arrow property | +| `property_arrow_alpha_` | float | 1.0 | Alpha of Arrow property | +| `property_arrow_color_` | QColor | Qt::white | Color of Arrow property | +| `property_sphere_scale_` | float | 1.0 | Scale of Sphere property | +| `property_sphere_alpha_` | float | 0.5 | Alpha of Sphere property | +| `property_sphere_color_` | QColor | (204, 51, 204) | Color of Sphere property | + ## Assumptions / Known limits TBD. @@ -32,7 +63,9 @@ TBD. 1. Start rviz and select Add under the Displays panel. ![select_add](./images/select_add.png) -2. Select tier4_localization_rviz_plugin/PoseHistory and press OK. +2. Select tier4_localization_rviz_plugin/PoseHistory or PoseWithCovarianceHistory. Next, press OK. ![select_localization_plugin](./images/select_localization_plugin.png) -3. Enter the name of the topic where you want to view the trajectory. +3. Enter the name of the topic where you want to view the trajectory and the covariance. ![select_topic_name](./images/select_topic_name.png) +4. You can view the trajectory and the covariance. + ![ex_pose_with_covariance_history](./images/ex_pose_with_covariance_history.png) diff --git a/common/tier4_localization_rviz_plugin/icons/classes/PoseWithCovarianceHistory.png b/common/tier4_localization_rviz_plugin/icons/classes/PoseWithCovarianceHistory.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_localization_rviz_plugin/icons/classes/PoseWithCovarianceHistory.png differ diff --git a/common/tier4_localization_rviz_plugin/images/ex_pose_with_covariance_history.png b/common/tier4_localization_rviz_plugin/images/ex_pose_with_covariance_history.png new file mode 100644 index 0000000000000..24f4be1c8546f Binary files /dev/null and b/common/tier4_localization_rviz_plugin/images/ex_pose_with_covariance_history.png differ diff --git a/common/tier4_localization_rviz_plugin/package.xml b/common/tier4_localization_rviz_plugin/package.xml index 291c7ff1617cc..ee771072f79ee 100644 --- a/common/tier4_localization_rviz_plugin/package.xml +++ b/common/tier4_localization_rviz_plugin/package.xml @@ -10,6 +10,7 @@ Apache License 2.0 Takagi, Isamu Takamasa Horibe + Yuhei Oshikubo ament_cmake_auto autoware_cmake diff --git a/common/tier4_localization_rviz_plugin/plugins/plugin_description.xml b/common/tier4_localization_rviz_plugin/plugins/plugin_description.xml index 2a58895354cad..01fc1a0ee488d 100644 --- a/common/tier4_localization_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_localization_rviz_plugin/plugins/plugin_description.xml @@ -10,4 +10,9 @@ base_class_type="rviz_common::Display"> Display footprint of geometry_msgs::Pose + + Display geometry_msgs::PoseWithCovarianceStamped + diff --git a/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp b/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp new file mode 100644 index 0000000000000..7708e6efaa104 --- /dev/null +++ b/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp @@ -0,0 +1,285 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_with_covariance_history_display.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace rviz_plugins +{ +PoseWithCovarianceHistory::PoseWithCovarianceHistory() : last_stamp_(0, 0, RCL_ROS_TIME) +{ + property_buffer_size_ = new rviz_common::properties::IntProperty("Buffer Size", 100, "", this); + + property_path_view_ = new rviz_common::properties::BoolProperty("Path", true, "", this); + property_shape_type_ = new rviz_common::properties::EnumProperty( + "Shape Type", "Line", "", property_path_view_, SLOT(updateShapeType())); + property_shape_type_->addOption("Line", 0); + property_shape_type_->addOption("Arrow", 1); + + property_line_width_ = + new rviz_common::properties::FloatProperty("Width", 0.1, "", property_shape_type_); + property_line_alpha_ = + new rviz_common::properties::FloatProperty("Alpha", 1.0, "", property_shape_type_); + property_line_alpha_->setMin(0.0); + property_line_alpha_->setMax(1.0); + property_line_color_ = + new rviz_common::properties::ColorProperty("Color", Qt::white, "", property_shape_type_); + + property_arrow_shaft_length_ = + new rviz_common::properties::FloatProperty("Shaft Length", 0.3, "", property_shape_type_); + property_arrow_shaft_diameter_ = + new rviz_common::properties::FloatProperty("Shaft diameter", 0.15, "", property_shape_type_); + property_arrow_head_length_ = + new rviz_common::properties::FloatProperty("Head Length", 0.2, "", property_shape_type_); + property_arrow_head_diameter_ = + new rviz_common::properties::FloatProperty("Head diameter", 0.3, "", property_shape_type_); + property_arrow_alpha_ = + new rviz_common::properties::FloatProperty("Alpha", 1.0, "", property_shape_type_); + property_arrow_alpha_->setMin(0.0); + property_arrow_alpha_->setMax(1.0); + property_arrow_color_ = + new rviz_common::properties::ColorProperty("Color", Qt::white, "", property_shape_type_); + + property_sphere_view_ = new rviz_common::properties::BoolProperty("Covariance", true, "", this); + property_sphere_scale_ = + new rviz_common::properties::FloatProperty("Scale", 1.0, "", property_sphere_view_); + property_sphere_alpha_ = + new rviz_common::properties::FloatProperty("Alpha", 0.5, "", property_sphere_view_); + property_sphere_alpha_->setMin(0.0); + property_sphere_alpha_->setMax(1.0); + property_sphere_color_ = new rviz_common::properties::ColorProperty( + "Color", QColor(204, 51, 204), "", property_sphere_view_); + + property_buffer_size_->setMin(0); + property_buffer_size_->setMax(10000); + property_line_width_->setMin(0.0); + property_sphere_scale_->setMin(0.0); + property_sphere_scale_->setMax(1000); + property_arrow_shaft_length_->setMin(0.0); + property_arrow_shaft_diameter_->setMin(0.0); + property_arrow_head_length_->setMin(0.0); + property_arrow_head_diameter_->setMin(0.0); +} + +PoseWithCovarianceHistory::~PoseWithCovarianceHistory() = default; // Properties are deleted by Qt + +void PoseWithCovarianceHistory::onInitialize() +{ + MFDClass::onInitialize(); + lines_ = std::make_unique(scene_manager_, scene_node_); +} + +void PoseWithCovarianceHistory::onEnable() +{ + subscribe(); +} + +void PoseWithCovarianceHistory::onDisable() +{ + unsubscribe(); +} + +void PoseWithCovarianceHistory::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + if (!history_.empty()) { + lines_->clear(); + arrows_.clear(); + spheres_.clear(); + updateShapeType(); + updateShapes(); + } +} + +void PoseWithCovarianceHistory::subscribe() +{ + MFDClass::subscribe(); +} + +void PoseWithCovarianceHistory::unsubscribe() +{ + MFDClass::unsubscribe(); + + history_.clear(); + lines_->clear(); + arrows_.clear(); + spheres_.clear(); +} + +void PoseWithCovarianceHistory::updateShapeType() +{ + bool is_line = property_shape_type_->getOptionInt() == 0; + bool is_arrow = property_shape_type_->getOptionInt() == 1; + + property_line_width_->setHidden(!is_line); + property_line_alpha_->setHidden(!is_line); + property_line_color_->setHidden(!is_line); + + property_arrow_shaft_length_->setHidden(!is_arrow); + property_arrow_shaft_diameter_->setHidden(!is_arrow); + property_arrow_head_length_->setHidden(!is_arrow); + property_arrow_head_diameter_->setHidden(!is_arrow); + property_arrow_alpha_->setHidden(!is_arrow); + property_arrow_color_->setHidden(!is_arrow); +} + +void PoseWithCovarianceHistory::processMessage( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr message) +{ + if ( + !rviz_common::validateFloats(message->pose.pose) || + !rviz_common::validateFloats(message->pose.covariance)) { + 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(); + spheres_.clear(); + target_frame_ = message->header.frame_id; + } + history_.emplace_back(message); + last_stamp_ = message->header.stamp; + updateHistory(); +} + +void PoseWithCovarianceHistory::updateHistory() +{ + const auto buffer_size = static_cast(property_buffer_size_->getInt()); + while (buffer_size < history_.size()) { + history_.pop_front(); + } +} + +void PoseWithCovarianceHistory::updateShapes() +{ + int shape_type = property_shape_type_->getOptionInt(); + Ogre::ColourValue color_line = + rviz_common::properties::qtToOgre(property_line_color_->getColor()); + color_line.a = property_line_alpha_->getFloat(); + Ogre::ColourValue color_sphere = + rviz_common::properties::qtToOgre(property_sphere_color_->getColor()); + color_sphere.a = property_sphere_alpha_->getFloat(); + Ogre::ColourValue color_arrow = + rviz_common::properties::qtToOgre(property_arrow_color_->getColor()); + color_arrow.a = property_arrow_alpha_->getFloat(); + + Ogre::Vector3 line_position; + Ogre::Quaternion line_orientation; + + auto frame_manager = context_->getFrameManager(); + if (!frame_manager->getTransform(target_frame_, last_stamp_, line_position, line_orientation)) { + setMissingTransformToFixedFrame(target_frame_); + return; + } + + setTransformOk(); + lines_->setMaxPointsPerLine(history_.size()); + lines_->setLineWidth(property_line_width_->getFloat()); + lines_->setPosition(line_position); + lines_->setOrientation(line_orientation); + lines_->setColor(color_line.r, color_line.g, color_line.b, color_line.a); + + while (spheres_.size() < history_.size()) { + spheres_.emplace_back(std::make_unique( + rviz_rendering::Shape::Sphere, scene_manager_, scene_node_)); + } + while (arrows_.size() < history_.size()) { + arrows_.emplace_back(std::make_unique(scene_manager_, scene_node_)); + } + + for (size_t i = 0; i < history_.size(); ++i) { + const auto & message = history_[i]; + + Ogre::Vector3 position; + position.x = message->pose.pose.position.x; + position.y = message->pose.pose.position.y; + position.z = message->pose.pose.position.z; + + Ogre::Quaternion orientation; + orientation.w = message->pose.pose.orientation.w; + orientation.x = message->pose.pose.orientation.x; + orientation.y = message->pose.pose.orientation.y; + orientation.z = message->pose.pose.orientation.z; + + Eigen::Matrix3d covariance_3d_map; + covariance_3d_map(0, 0) = message->pose.covariance[0]; + covariance_3d_map(0, 1) = message->pose.covariance[1 + 6 * 0]; + covariance_3d_map(0, 2) = message->pose.covariance[2 + 6 * 0]; + covariance_3d_map(1, 0) = message->pose.covariance[1 + 6 * 0]; + covariance_3d_map(1, 1) = message->pose.covariance[1 + 6 * 1]; + covariance_3d_map(1, 2) = message->pose.covariance[2 + 6 * 1]; + covariance_3d_map(2, 0) = message->pose.covariance[0 + 6 * 2]; + covariance_3d_map(2, 1) = message->pose.covariance[1 + 6 * 2]; + covariance_3d_map(2, 2) = message->pose.covariance[2 + 6 * 2]; + + if (property_sphere_view_->getBool()) { + Eigen::Matrix3d covariance_3d_base_link; + Eigen::Translation3f translation( + message->pose.pose.position.x, message->pose.pose.position.y, + message->pose.pose.position.z); + Eigen::Quaternionf rotation( + message->pose.pose.orientation.w, message->pose.pose.orientation.x, + message->pose.pose.orientation.y, message->pose.pose.orientation.z); + Eigen::Matrix4f pose_matrix4f = (translation * rotation).matrix(); + const Eigen::Matrix3d rot = pose_matrix4f.topLeftCorner<3, 3>().cast(); + covariance_3d_base_link = rot.transpose() * covariance_3d_map * rot; + + auto & sphere = spheres_[i]; + sphere->setPosition(position); + sphere->setOrientation(orientation); + sphere->setColor(color_sphere.r, color_sphere.g, color_sphere.b, color_sphere.a); + sphere->setScale(Ogre::Vector3( + property_sphere_scale_->getFloat() * 2 * std::sqrt(covariance_3d_base_link(0, 0)), + property_sphere_scale_->getFloat() * 2 * std::sqrt(covariance_3d_base_link(1, 1)), + property_sphere_scale_->getFloat() * 2 * std::sqrt(covariance_3d_base_link(2, 2)))); + } + + if (property_path_view_->getBool()) { + if (shape_type == 0) { + lines_->addPoint(position); + } + if (shape_type == 1) { + auto & arrow = arrows_[i]; + arrow->set( + property_arrow_shaft_length_->getFloat(), property_arrow_shaft_diameter_->getFloat(), + property_arrow_head_length_->getFloat(), property_arrow_head_diameter_->getFloat()); + arrow->setPosition(position); + Ogre::Quaternion y90(Ogre::Degree(-90), Ogre::Vector3::UNIT_Y); + arrow->setOrientation(orientation * y90); + arrow->setColor(color_arrow.r, color_arrow.g, color_arrow.b, color_arrow.a); + } + } + } +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::PoseWithCovarianceHistory, rviz_common::Display) diff --git a/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.hpp b/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.hpp new file mode 100644 index 0000000000000..172124ba176ee --- /dev/null +++ b/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.hpp @@ -0,0 +1,109 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_WITH_COVARIANCE_HISTORY__POSE_WITH_COVARIANCE_HISTORY_DISPLAY_HPP_ +#define POSE_WITH_COVARIANCE_HISTORY__POSE_WITH_COVARIANCE_HISTORY_DISPLAY_HPP_ + +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace rviz_rendering +{ +class Shape; +class BillboardLine; +class Arrow; +} // namespace rviz_rendering +namespace rviz_common::properties +{ +class ColorProperty; +class FloatProperty; +class IntProperty; +class BoolProperty; +class EnumProperty; +} // namespace rviz_common::properties + +namespace rviz_plugins +{ +class PoseWithCovarianceHistory +: public rviz_common::MessageFilterDisplay +{ + Q_OBJECT + +public: + PoseWithCovarianceHistory(); + ~PoseWithCovarianceHistory() override; + PoseWithCovarianceHistory(const PoseWithCovarianceHistory &) = delete; + PoseWithCovarianceHistory(const PoseWithCovarianceHistory &&) = delete; + PoseWithCovarianceHistory & operator=(const PoseWithCovarianceHistory &) = delete; + PoseWithCovarianceHistory & operator=(const PoseWithCovarianceHistory &&) = delete; + +protected: + void onInitialize() override; + void onEnable() override; + void onDisable() override; + void update(float wall_dt, float ros_dt) override; + +private Q_SLOTS: + void updateShapeType(); + +private: + void subscribe() override; + void unsubscribe() override; + void processMessage( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr message) override; + void updateHistory(); + void updateShapes(); + + std::string target_frame_; + std::deque history_; + std::unique_ptr lines_; + std::vector> spheres_; + std::vector> arrows_; + rclcpp::Time last_stamp_; + + 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_; + rviz_common::properties::IntProperty * property_buffer_size_; + + rviz_common::properties::BoolProperty * property_sphere_view_; + rviz_common::properties::FloatProperty * property_sphere_width_; + rviz_common::properties::FloatProperty * property_sphere_alpha_; + rviz_common::properties::ColorProperty * property_sphere_color_; + rviz_common::properties::FloatProperty * property_sphere_scale_; + + rviz_common::properties::BoolProperty * property_arrow_view_; + rviz_common::properties::FloatProperty * property_arrow_shaft_length_; + rviz_common::properties::FloatProperty * property_arrow_shaft_diameter_; + rviz_common::properties::FloatProperty * property_arrow_head_length_; + rviz_common::properties::FloatProperty * property_arrow_head_diameter_; + rviz_common::properties::FloatProperty * property_arrow_alpha_; + rviz_common::properties::ColorProperty * property_arrow_color_; + + rviz_common::properties::BoolProperty * property_path_view_; + rviz_common::properties::EnumProperty * property_shape_type_; +}; + +} // namespace rviz_plugins + +#endif // POSE_WITH_COVARIANCE_HISTORY__POSE_WITH_COVARIANCE_HISTORY_DISPLAY_HPP_