fix(tier4_planning_rviz_plugin): support backward driving in path_wit…

…h_lane_id/path/trajectory plugin Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
autowarefoundation · Jul 19, 2022 · c9638aa · c9638aa
1 parent 057da0f
commit c9638aa
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 8 deletions.
diff --git a/common/tier4_planning_rviz_plugin/package.xml b/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>

diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/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;

diff --git a/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp b/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;

diff --git a/common/tier4_planning_rviz_plugin/src/trajectory/display.cpp b/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;