chore: sync upstream

common/tier4_planning_rviz_plugin/CMakeLists.txt

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

common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp

@@ -0,0 +1,84 @@
+// Copyright 2023 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_UUID_STAMPED__DISPLAY_HPP_
+#define POSE_WITH_UUID_STAMPED__DISPLAY_HPP_
+
+#include <rviz_common/message_filter_display.hpp>
+
+#include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
+
+#include <deque>
+#include <memory>
+#include <string>
+
+namespace rviz_rendering
+{
+class Axes;
+class MovableText;
+}  // namespace rviz_rendering
+namespace rviz_common::properties
+{
+class FloatProperty;
+class TfFrameProperty;
+}  // namespace rviz_common::properties
+
+namespace rviz_plugins
+{
+class AutowarePoseWithUuidStampedDisplay
+: public rviz_common::MessageFilterDisplay<autoware_planning_msgs::msg::PoseWithUuidStamped>
+{
+  Q_OBJECT
+
+public:
+  AutowarePoseWithUuidStampedDisplay();
+  ~AutowarePoseWithUuidStampedDisplay() override;
+  AutowarePoseWithUuidStampedDisplay(const AutowarePoseWithUuidStampedDisplay &) = delete;
+  AutowarePoseWithUuidStampedDisplay(const AutowarePoseWithUuidStampedDisplay &&) = delete;
+  AutowarePoseWithUuidStampedDisplay & operator=(const AutowarePoseWithUuidStampedDisplay &) =
+    delete;
+  AutowarePoseWithUuidStampedDisplay & operator=(const AutowarePoseWithUuidStampedDisplay &&) =
+    delete;
+
+protected:
+  void onInitialize() override;
+  void onEnable() override;
+  void onDisable() override;
+
+private Q_SLOTS:
+  void updateVisualization();
+
+private:
+  void subscribe() override;
+  void unsubscribe() override;
+  void processMessage(
+    const autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr meg_ptr) override;
+
+  std::unique_ptr<rviz_rendering::Axes> axes_;
+  std::unique_ptr<Ogre::SceneNode> uuid_node_;
+  rviz_rendering::MovableText * uuid_;
+
+  rviz_common::properties::FloatProperty * length_property_;
+  rviz_common::properties::FloatProperty * radius_property_;
+  rviz_common::properties::TfFrameProperty * frame_property_;
+
+  rviz_common::properties::BoolProperty * uuid_text_view_property_;
+  rviz_common::properties::FloatProperty * uuid_text_scale_property_;
+
+  autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr last_msg_ptr_;
+};
+
+}  // namespace rviz_plugins
+
+#endif  // POSE_WITH_UUID_STAMPED__DISPLAY_HPP_

common/tier4_planning_rviz_plugin/package.xml

@@ -13,6 +13,7 @@
   <build_depend>autoware_cmake</build_depend>
 
   <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
   <depend>libqt5-core</depend>
   <depend>libqt5-gui</depend>
   <depend>libqt5-widgets</depend>

common/tier4_planning_rviz_plugin/plugins/plugin_description.xml

@@ -24,6 +24,11 @@
     base_class_type="rviz_common::Display">
     <description>Display footprint of autoware_auto_planning_msg::PathWithLaneId</description>
   </class>
+  <class name="rviz_plugins/PoseWithUuidStamped"
+    type="rviz_plugins::AutowarePoseWithUuidStampedDisplay"
+    base_class_type="rviz_common::Display">
+    <description>Display pose_with_uuid_stamped of autoware_planning_msg::PoseWithUuidStamped</description>
+  </class>
   <class name="rviz_plugins/Trajectory"
     type="rviz_plugins::AutowareTrajectoryDisplay"
     base_class_type="rviz_common::Display">

common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp

@@ -0,0 +1,146 @@
+// Copyright 2023 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 "rviz_common/properties/tf_frame_property.hpp"
+
+#include <pose_with_uuid_stamped/display.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include <rviz_common/validate_floats.hpp>
+#include <rviz_rendering/objects/axes.hpp>
+#include <rviz_rendering/objects/movable_text.hpp>
+
+namespace
+{
+std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u)
+{
+  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();
+}
+}  // namespace
+
+namespace rviz_plugins
+{
+AutowarePoseWithUuidStampedDisplay::AutowarePoseWithUuidStampedDisplay()
+{
+  length_property_ = new rviz_common::properties::FloatProperty(
+    "Length", 1.5f, "Length of each axis, in meters.", this, SLOT(updateVisualization()));
+  length_property_->setMin(0.0001f);
+
+  radius_property_ = new rviz_common::properties::FloatProperty(
+    "Radius", 0.5f, "Radius of each axis, in meters.", this, SLOT(updateVisualization()));
+  radius_property_->setMin(0.0001f);
+
+  uuid_text_view_property_ = new rviz_common::properties::BoolProperty(
+    "UUID", false, "flag of visualizing uuid text", this, SLOT(updateVisualization()), this);
+  uuid_text_scale_property_ = new rviz_common::properties::FloatProperty(
+    "Scale", 0.3f, "Scale of uuid text", uuid_text_view_property_, SLOT(updateVisualization()),
+    this);
+}
+
+AutowarePoseWithUuidStampedDisplay::~AutowarePoseWithUuidStampedDisplay() = default;
+
+void AutowarePoseWithUuidStampedDisplay::onInitialize()
+{
+  MFDClass::onInitialize();
+  axes_ = std::make_unique<rviz_rendering::Axes>(
+    scene_manager_, scene_node_, length_property_->getFloat(), radius_property_->getFloat());
+  axes_->getSceneNode()->setVisible(isEnabled());
+
+  uuid_node_.reset(scene_node_->createChildSceneNode());
+  uuid_ = new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1);
+}
+
+void AutowarePoseWithUuidStampedDisplay::onEnable()
+{
+  subscribe();
+  axes_->getSceneNode()->setVisible(true);
+}
+
+void AutowarePoseWithUuidStampedDisplay::onDisable()
+{
+  unsubscribe();
+  axes_->getSceneNode()->setVisible(false);
+}
+
+void AutowarePoseWithUuidStampedDisplay::subscribe() { MFDClass::subscribe(); }
+
+void AutowarePoseWithUuidStampedDisplay::unsubscribe() { MFDClass::unsubscribe(); }
+
+void AutowarePoseWithUuidStampedDisplay::updateVisualization()
+{
+  if (last_msg_ptr_ != nullptr) {
+    processMessage(last_msg_ptr_);
+  }
+}
+
+void AutowarePoseWithUuidStampedDisplay::processMessage(
+  const autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr msg_ptr)
+{
+  uuid_node_->detachAllObjects();
+
+  {
+    Ogre::Vector3 position;
+    Ogre::Quaternion orientation;
+
+    if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) {
+      const auto frame = msg_ptr->header.frame_id.c_str();
+      RCLCPP_DEBUG(
+        rclcpp::get_logger("AutowarePoseWithUuidStampedDisplay"),
+        "Error transforming from frame '%s' to frame '%s'", frame, qPrintable(fixed_frame_));
+      axes_->getSceneNode()->setVisible(false);
+      uuid_->setVisible(false);
+      setMissingTransformToFixedFrame(frame);
+    } else {
+      setTransformOk();
+      axes_->getSceneNode()->setVisible(true);
+      uuid_->setVisible(true);
+      scene_node_->setPosition(position);
+      scene_node_->setOrientation(orientation);
+    }
+  }
+
+  {
+    Ogre::Vector3 position;
+    position.x = msg_ptr->pose.position.x;
+    position.y = msg_ptr->pose.position.y;
+    position.z = msg_ptr->pose.position.z;
+
+    Ogre::Quaternion orientation;
+    orientation.x = msg_ptr->pose.orientation.x;
+    orientation.y = msg_ptr->pose.orientation.y;
+    orientation.z = msg_ptr->pose.orientation.z;
+    orientation.w = msg_ptr->pose.orientation.w;
+    axes_->setPosition(position);
+    axes_->setOrientation(orientation);
+    axes_->set(length_property_->getFloat(), radius_property_->getFloat());
+
+    if (uuid_text_view_property_->getBool()) {
+      uuid_node_->setPosition(position);
+      uuid_->setTextAlignment(
+        rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE);
+      uuid_->setCaption(uuid_to_string(msg_ptr->uuid));
+      uuid_->setCharacterHeight(uuid_text_scale_property_->getFloat());
+      uuid_->setVisible(true);
+      uuid_node_->attachObject(uuid_);
+    }
+  }
+  last_msg_ptr_ = msg_ptr;
+}
+}  // namespace rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePoseWithUuidStampedDisplay, rviz_common::Display)

launch/tier4_planning_launch/launch/planning.launch.xml

@@ -8,7 +8,8 @@
   <arg name="vehicle_param_file"/>
   <arg name="use_pointcloud_container"/>
   <arg name="pointcloud_container_name"/>
-  <arg name="smoother_type"/>
+  <arg name="use_surround_obstacle_check"/>
+  <arg name="use_experimental_lane_change_function"/>
 
   <!-- common -->
   <arg name="common_param_path"/>
@@ -49,6 +50,8 @@
   <!-- motion velocity smoother -->
   <arg name="motion_velocity_smoother_param_path"/>
   <arg name="smoother_type_param_path"/>
+  <!-- planning validator -->
+  <arg name="planning_validator_param_path"/>
 
   <group>
     <push-ros-namespace namespace="planning"/>
@@ -65,13 +68,19 @@
         <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
         <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
+        <arg name="use_surround_obstacle_check" value="$(var use_surround_obstacle_check)"/>
+        <arg name="cruise_planner_type" value="$(var cruise_planner_type)"/>
+        <arg name="use_experimental_lane_change_function" value="$(var use_experimental_lane_change_function)"/>
       </include>
     </group>
 
-    <!-- planning error monitor -->
+    <!-- planning validator -->
     <group>
-      <push-ros-namespace namespace="planning_diagnostics"/>
-      <include file="$(find-pkg-share tier4_planning_launch)/launch/planning_diagnostics/planning_error_monitor.launch.xml"/>
+      <include file="$(find-pkg-share planning_validator)/launch/planning_validator.launch.xml">
+        <arg name="input_trajectory" value="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
+        <arg name="output_trajectory" value="/planning/scenario_planning/trajectory"/>
+        <arg name="planning_validator_param_path" value="$(var planning_validator_param_path)"/>
+      </include>
     </group>
   </group>
 </launch>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml

@@ -11,6 +11,7 @@
           <arg name="use_multithread" value="true"/>
           <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
           <arg name="container_name" value="$(var pointcloud_container_name)"/>
+          <arg name="use_experimental_lane_change_function" value="$(var use_experimental_lane_change_function)"/>
         </include>
       </group>
       <group>
@@ -26,6 +27,8 @@
       <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py">
         <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
         <arg name="use_multithread" value="true"/>
+        <arg name="use_surround_obstacle_check" value="$(var use_surround_obstacle_check)"/>
+        <arg name="cruise_planner_type" value="$(var cruise_planner_type)"/>
       </include>
     </group>
   </group>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -73,6 +73,7 @@ def launch_setup(context, *args, **kwargs):
             ("~/output/path", "path_with_lane_id"),
             ("~/output/turn_indicators_cmd", "/planning/turn_indicators_cmd"),
             ("~/output/hazard_lights_cmd", "/planning/hazard_lights_cmd"),
+            ("~/output/modified_goal", "/planning/scenario_planning/modified_goal"),
         ],
         parameters=[
             nearest_search_param,
@@ -86,11 +87,19 @@ def launch_setup(context, *args, **kwargs):
             behavior_path_planner_param,
             vehicle_param,
             {
-                "bt_tree_config_path": [
-                    FindPackageShare("behavior_path_planner"),
-                    "/config/behavior_path_planner_tree.xml",
-                ],
-                "planning_hz": 10.0,
+                "lane_change.enable_abort_lane_change": LaunchConfiguration(
+                    "use_experimental_lane_change_function"
+                ),
+                "lane_change.enable_collision_check_at_prepare_phase": LaunchConfiguration(
+                    "use_experimental_lane_change_function"
+                ),
+                "lane_change.use_predicted_path_outside_lanelet": LaunchConfiguration(
+                    "use_experimental_lane_change_function"
+                ),
+                "lane_change.use_all_predicted_path": LaunchConfiguration(
+                    "use_experimental_lane_change_function"
+                ),
+                "bt_tree_config_path": LaunchConfiguration("behavior_path_planner_tree_param_path"),
             },
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
@@ -280,20 +289,18 @@ def add_launch_arg(name: str, default_value=None, description=None):
             DeclareLaunchArgument(name, default_value=default_value, description=description)
         )
 
-    add_launch_arg(
-        "vehicle_param_file",
-        [
-            FindPackageShare("vehicle_info_util"),
-            "/config/vehicle_info.param.yaml",
-        ],
-        "path to the parameter file of vehicle information",
-    )
+    # vehicle parameter
+    add_launch_arg("vehicle_param_file")
 
+    # interface parameter
     add_launch_arg(
         "input_route_topic_name", "/planning/mission_planning/route", "input topic of route"
     )
     add_launch_arg("map_topic_name", "/map/vector_map", "input topic of map")
 
+    # package parameter
+    add_launch_arg("use_experimental_lane_change_function")
+
     # component
     add_launch_arg("use_intra_process", "false", "use ROS2 component container communication")
     add_launch_arg("use_multithread", "false", "use multithread")

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

@@ -12,6 +12,7 @@
     <remap from="~/output/path" to="path_with_lane_id"/>
     <remap from="~/output/turn_indicators_cmd" to="/planning/turn_signal_cmd"/>
     <remap from="~/output/hazard_lights_cmd" to="/planning/hazard_signal_cmd"/>
+    <remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal"/>
     <param from="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml"/>
     <param from="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml"/>
     <param from="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml"/>