fix(tier4_planning_rviz_plugin): support backward driving in path/tra…

…j plugin (tier4#1335)

* fix(tier4_planning_rviz_plugin): support backward driving in path_with_lane_id/path/trajectory plugin

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* add utils.hpp

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

common/tier4_planning_rviz_plugin/include/utils.hpp

@@ -0,0 +1,65 @@
+// 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 UTILS_HPP_
+#define UTILS_HPP_
+
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp>
+#include <tier4_autoware_utils/math/normalization.hpp>
+
+#include <tf2/utils.h>
+
+namespace rviz_plugins
+{
+template <class T>
+bool isDrivingForward(const T points_with_twist, size_t target_idx)
+{
+  constexpr double epsilon = 1e-6;
+
+  // 1. check velocity
+  const double target_velocity =
+    tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.at(target_idx));
+  if (epsilon < target_velocity) {
+    return true;
+  } else if (target_velocity < -epsilon) {
+    return false;
+  }
+
+  // 2. check points size is enough
+  if (points_with_twist.size() < 2) {
+    return true;
+  }
+
+  // 3. check points direction
+  const bool is_last_point = target_idx == points_with_twist.size() - 1;
+  const size_t first_idx = is_last_point ? target_idx - 1 : target_idx;
+  const size_t second_idx = is_last_point ? target_idx : target_idx + 1;
+
+  const auto first_pose = tier4_autoware_utils::getPose(points_with_twist.at(first_idx));
+  const auto second_pose = tier4_autoware_utils::getPose(points_with_twist.at(second_idx));
+  const double first_traj_yaw = tf2::getYaw(first_pose.orientation);
+  const double driving_direction_yaw =
+    tier4_autoware_utils::calcAzimuthAngle(first_pose.position, second_pose.position);
+  if (
+    std::abs(tier4_autoware_utils::normalizeRadian(first_traj_yaw - driving_direction_yaw)) <
+    M_PI_2) {
+    return true;
+  }
+
+  return false;
+}
+}  // namespace rviz_plugins
+
+#endif  // UTILS_HPP_

common/tier4_planning_rviz_plugin/package.xml

@@ -15,6 +15,7 @@
   <depend>libqt5-core</depend>
   <depend>libqt5-gui</depend>
   <depend>libqt5-widgets</depend>
+  <depend>motion_utils</depend>
   <depend>qtbase5-dev</depend>
   <depend>rclcpp</depend>
   <depend>rviz_common</depend>

common/tier4_planning_rviz_plugin/src/path/display.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <path/display.hpp>
+#include <utils.hpp>
 
 #include <memory>
 #define EIGEN_MPL2_ONLY
@@ -166,7 +167,8 @@ void AutowarePathDisplay::processMessage(
     // 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) {
+    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
        */
@@ -188,7 +190,7 @@ void AutowarePathDisplay::processMessage(
           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) {
+          if (!isDrivingForward(msg_ptr->points, point_idx)) {
             quat *= quat_yaw_reverse;
           }
           vec_out = quat * vec_in;
@@ -202,7 +204,7 @@ void AutowarePathDisplay::processMessage(
           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) {
+          if (!isDrivingForward(msg_ptr->points, point_idx)) {
             quat *= quat_yaw_reverse;
           }
           vec_out = quat * vec_in;

common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <path_with_lane_id/display.hpp>
+#include <utils.hpp>
 
 #include <memory>
 #define EIGEN_MPL2_ONLY
@@ -167,7 +168,8 @@ void AutowarePathWithLaneIdDisplay::processMessage(
     // path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP);
     velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP);
 
-    for (auto && e : msg_ptr->points) {
+    for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) {
+      const auto & e = msg_ptr->points.at(point_idx);
       /*
        * Path
        */
@@ -189,7 +191,7 @@ void AutowarePathWithLaneIdDisplay::processMessage(
           Eigen::Quaternionf quat(
             e.point.pose.orientation.w, e.point.pose.orientation.x, e.point.pose.orientation.y,
             e.point.pose.orientation.z);
-          if (e.point.longitudinal_velocity_mps < 0) {
+          if (!isDrivingForward(msg_ptr->points, point_idx)) {
             quat *= quat_yaw_reverse;
           }
           vec_out = quat * vec_in;
@@ -203,7 +205,7 @@ void AutowarePathWithLaneIdDisplay::processMessage(
           Eigen::Quaternionf quat(
             e.point.pose.orientation.w, e.point.pose.orientation.x, e.point.pose.orientation.y,
             e.point.pose.orientation.z);
-          if (e.point.longitudinal_velocity_mps < 0) {
+          if (!isDrivingForward(msg_ptr->points, point_idx)) {
             quat *= quat_yaw_reverse;
           }
           vec_out = quat * vec_in;

common/tier4_planning_rviz_plugin/src/trajectory/display.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <trajectory/display.hpp>
+#include <utils.hpp>
 
 #include <memory>
 #define EIGEN_MPL2_ONLY
@@ -229,7 +230,7 @@ void AutowareTrajectoryDisplay::processMessage(
           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) {
+          if (!isDrivingForward(msg_ptr->points, point_idx)) {
             quat *= quat_yaw_reverse;
           }
           vec_out = quat * vec_in;
@@ -243,7 +244,7 @@ void AutowareTrajectoryDisplay::processMessage(
           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) {
+          if (!isDrivingForward(msg_ptr->points, point_idx)) {
             quat *= quat_yaw_reverse;
           }
           vec_out = quat * vec_in;