From 0b14693b121d0d859dc24e089e7289af8735551f Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Fri, 9 Dec 2022 18:09:46 +0900 Subject: [PATCH] feat(planning_launch): add behavior_velocity_limiter to motion_planning launch file (#596) (#597) Signed-off-by: Maxime CLEMENT Signed-off-by: Maxime CLEMENT Signed-off-by: Maxime CLEMENT Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- .../obstacle_velocity_limiter.param.yaml | 33 +++++++++++++++ .../motion_planning/motion_planning.launch.py | 41 +++++++++++++++++-- 2 files changed, 71 insertions(+), 3 deletions(-) create mode 100644 planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml b/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml new file mode 100644 index 0000000000..c6a0c275db --- /dev/null +++ b/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml @@ -0,0 +1,33 @@ +/**: + ros__parameters: + min_ttc: 1.0 # [s] minimum time without collision when doing projection at each trajectory point + distance_buffer: 0.0 # [m] extra distance to add to a projection (in addition to the vehicle overhang) + min_adjusted_velocity: 2.5 # [m/s] minimum velocity that the module can set + max_deceleration: 2.0 # [m/s²] maximum deceleration caused by the adjusted velocity + + trajectory_preprocessing: + start_distance: 0.0 # [m] distance ahead of ego from which to start modifying the trajectory + max_length: 100.0 # [m] maximum distance (from the start point) where the velocity is adjusted + max_duration: 10.0 # [s] maximum duration (from the start point) where the velocity is adjusted + downsample_factor: 10 # factor by which to downsample the input trajectory for calculation + calculate_steering_angles: false # if true, the steering angle at each trajectory point is calculated from the change in heading + + simulation: + model: particle # model to use for forward projection at each trajectory point. Either "particle" or "bicycle" + distance_method: exact # distance calculation method. Either "exact" or "approximation". + # parameters used only with the bicycle model + steering_offset: 0.01 # [rad] steering angle offset used to model uncertainty in the forward projection + nb_points: 5 # number of points representing the curved projections + + obstacles: + dynamic_source: static_only # source of dynamic obstacles. Must be 'pointcloud', 'occupancy_grid', or 'static_only'. + ignore_obstacles_on_path: false # if true, obstacles on the ego paths are ignored + ignore_extra_distance: 1.0 # [m] extra lateral distance where obstacles along the path are ignored + occupancy_grid_threshold: 60 # occupancy grid values higher than this are considered to be obstacles + dynamic_obstacles_buffer: 1.5 # [m] extra distance around dynamic obstacles used to mask the occupancy grid and avoid false obstacle detection + dynamic_obstacles_min_vel: 0.5 # [m/s] velocity above which a dynamic obstacle is ignored by the module + static_map_tags: # linestring tags in the lanelet maps that will be used as static obstacles + - guard_rail + filter_envelope : false # whether to calculate the apparent safety envelope and use it to filter obstacles + rtree_min_points: 500 # from this number of obstacle points, a rtree is used for collision detection + rtree_min_segments: 1600 # from this number of obstacle segments, a rtree is used for collision detection diff --git a/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index e1f27d2b44..56a2468124 100644 --- a/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -94,6 +94,40 @@ def add_launch_arg(name: str, default_value=None, description=None): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # obstacle velocity limiter + obstacle_velocity_limiter_param_path = os.path.join( + get_package_share_directory("planning_launch"), + "config", + "scenario_planning", + "lane_driving", + "motion_planning", + "obstacle_velocity_limiter", + "obstacle_velocity_limiter.param.yaml", + ) + with open(obstacle_velocity_limiter_param_path, "r") as f: + obstacle_velocity_limiter_param = yaml.safe_load(f)["/**"]["ros__parameters"] + obstacle_velocity_limiter_component = ComposableNode( + package="obstacle_velocity_limiter", + plugin="obstacle_velocity_limiter::ObstacleVelocityLimiterNode", + name="obstacle_velocity_limiter", + namespace="", + remappings=[ + ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), + ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/dynamic_obstacles", "/perception/object_recognition/objects"), + ("~/input/occupancy_grid", "/perception/occupancy_grid_map/map"), + ("~/input/obstacle_pointcloud", "/perception/obstacle_segmentation/pointcloud"), + ("~/input/map", "/map/vector_map"), + ("~/output/debug_markers", "debug_markers"), + ("~/output/trajectory", "obstacle_velocity_limiter/trajectory"), + ], + parameters=[ + obstacle_velocity_limiter_param, + {"obstacles.dynamic_source": "static_only"}, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + # surround obstacle checker surround_obstacle_checker_param_path = os.path.join( get_package_share_directory("planning_launch"), @@ -151,7 +185,7 @@ def add_launch_arg(name: str, default_value=None, description=None): name="obstacle_cruise_planner", namespace="", remappings=[ - ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), + ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"), ("~/input/odometry", "/localization/kinematic_state"), ("~/input/acceleration", "/localization/acceleration"), ("~/input/objects", "/perception/object_recognition/objects"), @@ -212,7 +246,7 @@ def add_launch_arg(name: str, default_value=None, description=None): ), ("~/input/objects", "/perception/object_recognition/objects"), ("~/input/odometry", "/localization/kinematic_state"), - ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), + ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"), ], parameters=[ common_param, @@ -230,7 +264,7 @@ def add_launch_arg(name: str, default_value=None, description=None): name="obstacle_cruise_planner_relay", namespace="", parameters=[ - {"input_topic": "obstacle_avoidance_planner/trajectory"}, + {"input_topic": "obstacle_velocity_limiter/trajectory"}, {"output_topic": "/planning/scenario_planning/lane_driving/trajectory"}, {"type": "autoware_auto_planning_msgs/msg/Trajectory"}, ], @@ -244,6 +278,7 @@ def add_launch_arg(name: str, default_value=None, description=None): executable=LaunchConfiguration("container_executable"), composable_node_descriptions=[ obstacle_avoidance_planner_component, + obstacle_velocity_limiter_component, ], )