fix launch file and broken code

Signed-off-by: Maxime CLEMENT <[email protected]>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py

@@ -146,6 +146,56 @@ def launch_setup(context, *args, **kwargs):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    # motion velocity planner
+    with open(LaunchConfiguration("motion_velocity_planner_param_path").perform(context), "r") as f:
+        motion_velocity_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    with open(
+        LaunchConfiguration("motion_velocity_smoother_param_path").perform(context), "r"
+    ) as f:
+        motion_velocity_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    with open(
+        LaunchConfiguration("behavior_velocity_smoother_type_param_path").perform(context), "r"
+    ) as f:
+        behavior_velocity_smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    with open(
+        LaunchConfiguration("motion_velocity_planner_out_of_lane_module_param_path").perform(
+            context
+        ),
+        "r",
+    ) as f:
+        out_of_lane_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    motion_velocity_planner_component = ComposableNode(
+        package="autoware_motion_velocity_planner_node",
+        plugin="autoware::motion_velocity_planner::MotionVelocityPlannerNode",
+        name="motion_velocity_planner",
+        namespace="",
+        remappings=[
+            ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
+            ("~/input/dynamic_objects", "/perception/object_recognition/objects"),
+            ("~/input/occupancy_grid", "/perception/occupancy_grid_map/map"),
+            ("~/input/vector_map", "/map/vector_map"),
+            ("~/output/trajectory", "motion_velocity_planner/trajectory"),
+            ("~/input/vehicle_odometry", "/localization/kinematic_state"),
+            ("~/input/accel", "/localization/acceleration"),
+            ("~/input/no_ground_pointcloud", "/perception/obstacle_segmentation/pointcloud"),
+            ("~/input/traffic_signals", "/perception/traffic_light_recognition/traffic_signals"),
+            ("~/input/virtual_traffic_light_states", "/perception/virtual_traffic_light_states"),
+            ("~/input/occupancy_grid", "/perception/occupancy_grid_map/map"),
+            ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"),
+            ("~/output/velocity_factors", "/planning/velocity_factors/motion_velocity_planner"),
+        ],
+        parameters=[
+            motion_velocity_planner_param,
+            vehicle_info_param,
+            nearest_search_param,
+            common_param,
+            motion_velocity_smoother_param,
+            behavior_velocity_smoother_type_param,
+            out_of_lane_param,
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
     # surround obstacle checker
     with open(
         LaunchConfiguration("surround_obstacle_checker_param_path").perform(context), "r"
@@ -206,7 +256,7 @@ def launch_setup(context, *args, **kwargs):
             ),
             ("~/input/objects", "/perception/object_recognition/objects"),
             ("~/input/odometry", "/localization/kinematic_state"),
-            ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"),
+            ("~/input/trajectory", "motion_velocity_planner/trajectory"),
         ],
         parameters=[
             nearest_search_param,
@@ -227,7 +277,7 @@ def launch_setup(context, *args, **kwargs):
         name="obstacle_cruise_planner",
         namespace="",
         remappings=[
-            ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"),
+            ("~/input/trajectory", "motion_velocity_planner/trajectory"),
             ("~/input/odometry", "/localization/kinematic_state"),
             ("~/input/acceleration", "/localization/acceleration"),
             ("~/input/objects", "/perception/object_recognition/objects"),
@@ -250,7 +300,7 @@ def launch_setup(context, *args, **kwargs):
         name="obstacle_cruise_planner_relay",
         namespace="",
         parameters=[
-            {"input_topic": "obstacle_velocity_limiter/trajectory"},
+            {"input_topic": "motion_velocity_planner/trajectory"},
             {"output_topic": "/planning/scenario_planning/lane_driving/trajectory"},
             {"type": "autoware_auto_planning_msgs/msg/Trajectory"},
         ],
@@ -263,6 +313,7 @@ def launch_setup(context, *args, **kwargs):
         package="rclcpp_components",
         executable=LaunchConfiguration("container_executable"),
         composable_node_descriptions=[
+            motion_velocity_planner_component,
             obstacle_velocity_limiter_component,
         ],
     )

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -267,9 +267,9 @@ VelocityPlanningResult OutOfLaneModule::plan(
                                 ego_data.velocity > 0.1;
     const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING
                                        : motion_utils::VelocityFactor::STOPPED;
-    velocity_factor_interface_.set(
-      ego_trajectory_points, ego_data.pose, point_to_insert->point.pose, status, "out_of_lane");
-    result.velocity_factor = velocity_factor_interface_.get();
+    // velocity_factor_interface_.set(
+    //   ego_trajectory_points, ego_data.pose, point_to_insert->point.pose, status, "out_of_lane");
+    // result.velocity_factor = velocity_factor_interface_.get();
   } else if (!decisions.empty()) {
     RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)");
   }

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt

@@ -31,17 +31,6 @@ else()
     target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}")
 endif()
 
-if(BUILD_TESTING)
-  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
-    test/src/test_node_interface.cpp
-  )
-  target_link_libraries(test_${PROJECT_NAME}
-    gtest_main
-    ${PROJECT_NAME}_lib
-  )
-  target_include_directories(test_${PROJECT_NAME} PRIVATE src)
-endif()
-
 ament_auto_package(INSTALL_TO_SHARE
   launch
   config

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp

@@ -131,7 +131,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions &
     std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1));
 
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
-  published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
 void MotionVelocityPlannerNode::on_load_plugin(
@@ -323,8 +322,6 @@ void MotionVelocityPlannerNode::on_trajectory(
   lk.unlock();
 
   trajectory_pub_->publish(output_trajectory_msg);
-  published_time_publisher_->publish_if_subscribed(
-    trajectory_pub_, output_trajectory_msg.header.stamp);
 }
 
 void MotionVelocityPlannerNode::insert_stop(

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp

@@ -22,7 +22,6 @@
 #include <autoware_motion_velocity_planner_node/srv/unload_plugin.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/logger_level_configure.hpp>
-#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
@@ -132,8 +131,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node
     autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points);
 
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
-
-  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 };
 }  // namespace autoware::motion_velocity_planner