From d31cad4948f5c6267672823a5e6c60b6465119a2 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 18 Jun 2024 16:17:34 +0900 Subject: [PATCH] fix launch file and broken code Signed-off-by: Maxime CLEMENT --- .../motion_planning/motion_planning.launch.py | 57 ++++++- .../src/out_of_lane_module.cpp | 6 +- .../CMakeLists.txt | 11 -- .../src/node.cpp | 3 - .../src/node.hpp | 3 - .../test/src/test_node_interface.cpp | 139 ------------------ 6 files changed, 57 insertions(+), 162 deletions(-) delete mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index d8c5d7825f19d..d695d0081a91e 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/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, ], ) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index e8d03be962ddf..4c6ba34b8a48e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/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)"); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt index 0af4da6fd4426..5d87d758422d3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt +++ b/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 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index c0373678270dd..522d359d47667 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/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(this); - published_time_publisher_ = std::make_unique(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( diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index ecf128bf9a012..2f20669ded687 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -132,8 +131,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points); std::unique_ptr logger_configure_; - - std::unique_ptr published_time_publisher_; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp deleted file mode 100644 index 3ad5bab8e070b..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp +++ /dev/null @@ -1,139 +0,0 @@ -// Copyright 2024 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. - -#include "node.hpp" - -#include -#include -#include - -#include - -#include - -using autoware::motion_velocity_planner::MotionVelocityPlannerNode; -using planning_test_utils::PlanningInterfaceTestManager; - -std::shared_ptr generateTestManager() -{ - auto test_manager = std::make_shared(); - - // set subscriber with topic name: motion_velocity_planner → test_node_ - test_manager->setTrajectorySubscriber("motion_velocity_planner_node/output/trajectory"); - - // set motion_velocity_planner node's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("motion_velocity_planner_node/input/trajectory"); - - test_manager->setInitialPoseTopicName("motion_velocity_planner_node/input/vehicle_odometry"); - test_manager->setOdometryTopicName("motion_velocity_planner_node/input/vehicle_odometry"); - - return test_manager; -} - -std::shared_ptr generateNode() -{ - auto node_options = rclcpp::NodeOptions{}; - - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto motion_velocity_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_motion_velocity_planner_node"); - const auto motion_velocity_smoother_dir = - ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); - - const auto get_motion_velocity_module_config = [](const std::string & module) { - const auto package_name = "autoware_motion_velocity_" + module + "_module"; - const auto package_path = ament_index_cpp::get_package_share_directory(package_name); - return package_path + "/config/" + module + ".param.yaml"; - }; - - std::vector module_names; - module_names.emplace_back("autoware::motion_velocity_planner::OutOfLaneModule"); - - std::vector params; - params.emplace_back("launch_modules", module_names); - params.emplace_back("is_simulation", false); - node_options.parameter_overrides(params); - - test_utils::updateNodeOptions( - node_options, - {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", - motion_velocity_smoother_dir + "/config/Analytical.param.yaml", - motion_velocity_planner_dir + "/config/motion_velocity_planner.param.yaml", - get_motion_velocity_module_config("out_of_lane")}); - - return std::make_shared(node_options); -} - -void publishMandatoryTopics( - std::shared_ptr test_manager, - std::shared_ptr test_target_node) -{ - // publish necessary topics from test_manager - test_manager->publishTF(test_target_node, "/tf"); - test_manager->publishAcceleration(test_target_node, "motion_velocity_planner_node/input/accel"); - test_manager->publishPredictedObjects( - test_target_node, "motion_velocity_planner_node/input/dynamic_objects"); - test_manager->publishPointCloud( - test_target_node, "motion_velocity_planner_node/input/no_ground_pointcloud"); - test_manager->publishOdometry( - test_target_node, "motion_velocity_planner_node/input/vehicle_odometry"); - test_manager->publishAcceleration(test_target_node, "motion_velocity_planner_node/input/accel"); - test_manager->publishMap(test_target_node, "motion_velocity_planner_node/input/vector_map"); - test_manager->publishTrafficSignals( - test_target_node, "motion_velocity_planner_node/input/traffic_signals"); - test_manager->publishVirtualTrafficLightState( - test_target_node, "motion_velocity_planner_node/input/virtual_traffic_light_states"); - test_manager->publishOccupancyGrid( - test_target_node, "motion_velocity_planner_node/input/occupancy_grid"); -} - -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) -{ - rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); - - publishMandatoryTopics(test_manager, test_target_node); - - // test with nominal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); - EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - - // test with empty trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalTrajectory(test_target_node)); - rclcpp::shutdown(); -} - -TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) -{ - rclcpp::init(0, nullptr); - - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); - publishMandatoryTopics(test_manager, test_target_node); - - // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); - - // make sure motion_velocity_planner is running - EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromTrajectory(test_target_node)); - - rclcpp::shutdown(); -}