feat(tier4_planning_rviz_plugin): add PathWithLaneIdFootprint rviz pl…

…ugin (#594)

* feat(tier4_planning_rviz_plugin): add PathWithLaneIdFootprint rviz plugin

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

common/tier4_planning_rviz_plugin/CMakeLists.txt

@@ -29,6 +29,8 @@ ament_auto_add_library(tier4_planning_rviz_plugin SHARED
   src/path_footprint/display.cpp
   include/path_with_lane_id/display.hpp
   src/path_with_lane_id/display.cpp
+  include/path_with_lane_id_footprint/display.hpp
+  src/path_with_lane_id_footprint/display.cpp
   include/trajectory/display.hpp
   src/trajectory/display.cpp
   include/trajectory_footprint/display.hpp

common/tier4_planning_rviz_plugin/include/path_with_lane_id_footprint/display.hpp

@@ -0,0 +1,85 @@
+// 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_WITH_LANE_ID_FOOTPRINT__DISPLAY_HPP_
+#define PATH_WITH_LANE_ID_FOOTPRINT__DISPLAY_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <rviz_common/display_context.hpp>
+#include <rviz_common/frame_manager_iface.hpp>
+#include <rviz_common/message_filter_display.hpp>
+#include <rviz_common/properties/bool_property.hpp>
+#include <rviz_common/properties/color_property.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include <rviz_common/properties/parse_color.hpp>
+#include <rviz_common/validate_floats.hpp>
+
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+
+#include <OgreBillboardSet.h>
+#include <OgreManualObject.h>
+#include <OgreMaterialManager.h>
+#include <OgreSceneManager.h>
+#include <OgreSceneNode.h>
+
+#include <memory>
+
+namespace rviz_plugins
+{
+class AutowarePathWithLaneIdFootprintDisplay
+: public rviz_common::MessageFilterDisplay<autoware_auto_planning_msgs::msg::PathWithLaneId>
+{
+  Q_OBJECT
+
+public:
+  AutowarePathWithLaneIdFootprintDisplay();
+  virtual ~AutowarePathWithLaneIdFootprintDisplay();
+
+  void onInitialize() override;
+  void reset() override;
+
+private Q_SLOTS:
+  void updateVisualization();
+  void updateVehicleInfo();
+
+protected:
+  void processMessage(
+    const autoware_auto_planning_msgs::msg::PathWithLaneId::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<VehicleFootprintInfo> vehicle_footprint_info_;
+
+private:
+  autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr last_msg_ptr_;
+  bool validateFloats(
+    const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr & msg_ptr);
+};
+
+}  // namespace rviz_plugins
+
+#endif  // PATH_WITH_LANE_ID_FOOTPRINT__DISPLAY_HPP_

common/tier4_planning_rviz_plugin/plugins/plugin_description.xml

@@ -19,6 +19,11 @@
     base_class_type="rviz_common::Display">
     <description>Display path_with_lane_id of autoware_auto_planning_msg::PathWithLaneId</description>
   </class>
+  <class name="rviz_plugins/PathWithLaneIdFootprint"
+    type="rviz_plugins::AutowarePathWithLaneIdFootprintDisplay"
+    base_class_type="rviz_common::Display">
+    <description>Display footprint of autoware_auto_planning_msg::PathWithLaneId</description>
+  </class>
   <class name="rviz_plugins/Trajectory"
     type="rviz_plugins::AutowareTrajectoryDisplay"
     base_class_type="rviz_common::Display">

common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp

@@ -0,0 +1,187 @@
+// 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 <memory>
+
+#define EIGEN_MPL2_ONLY
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Geometry>
+#include <path_with_lane_id_footprint/display.hpp>
+
+namespace rviz_plugins
+{
+AutowarePathWithLaneIdFootprintDisplay::AutowarePathWithLaneIdFootprintDisplay()
+{
+  property_path_footprint_view_ = new rviz_common::properties::BoolProperty(
+    "View PathWithLaneId 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();
+}
+
+AutowarePathWithLaneIdFootprintDisplay::~AutowarePathWithLaneIdFootprintDisplay()
+{
+  if (initialized()) {
+    scene_manager_->destroyManualObject(path_footprint_manual_object_);
+  }
+}
+
+void AutowarePathWithLaneIdFootprintDisplay::onInitialize()
+{
+  MFDClass::onInitialize();
+
+  path_footprint_manual_object_ = scene_manager_->createManualObject();
+  path_footprint_manual_object_->setDynamic(true);
+  scene_node_->attachObject(path_footprint_manual_object_);
+}
+
+void AutowarePathWithLaneIdFootprintDisplay::reset()
+{
+  MFDClass::reset();
+  path_footprint_manual_object_->clear();
+}
+
+bool AutowarePathWithLaneIdFootprintDisplay::validateFloats(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr & msg_ptr)
+{
+  for (auto && path_point : msg_ptr->points) {
+    if (!rviz_common::validateFloats(path_point.point.pose)) {
+      return false;
+    }
+  }
+  return true;
+}
+
+void AutowarePathWithLaneIdFootprintDisplay::processMessage(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId::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<float, 4> lon_offset_vec{top, top, bottom, bottom};
+        const std::array<float, 4> lat_offset_vec{left, right, right, left};
+
+        for (int f_idx = 0; f_idx < 4; ++f_idx) {
+          const Eigen::Quaternionf quat(
+            path_point.point.pose.orientation.w, path_point.point.pose.orientation.x,
+            path_point.point.pose.orientation.y, path_point.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.point.pose.position.x + offset_to_edge.x(),
+              path_point.point.pose.position.y + offset_to_edge.y(),
+              path_point.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.point.pose.position.x + offset_to_edge.x(),
+              path_point.point.pose.position.y + offset_to_edge.y(),
+              path_point.point.pose.position.z);
+            path_footprint_manual_object_->colour(color);
+          }
+        }
+      }
+    }
+
+    path_footprint_manual_object_->end();
+  }
+  last_msg_ptr_ = msg_ptr;
+}
+
+void AutowarePathWithLaneIdFootprintDisplay::updateVisualization()
+{
+  if (last_msg_ptr_ != nullptr) {
+    processMessage(last_msg_ptr_);
+  }
+}
+
+void AutowarePathWithLaneIdFootprintDisplay::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<VehicleFootprintInfo>(length, width, rear_overhang);
+}
+
+}  // namespace rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathWithLaneIdFootprintDisplay, rviz_common::Display)