Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

test(no_stopping_area): refactor and add tests #9009

Draft
wants to merge 13 commits into
base: main
Choose a base branch
from
Draft

test(no_stopping_area): refactor and add tests #9009

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
e5f2f4e
refactoring to make testing easier
maxime-clem Oct 2, 2024
5b7c166
more refactoring
maxime-clem Oct 2, 2024
49246f4
[DEBUG] run bvp in gdb+konsole
maxime-clem Oct 2, 2024
2147877
more refactoring
maxime-clem Oct 3, 2024
9baf711
add first test (is_vehicle_type)
maxime-clem Oct 3, 2024
5eced7c
add test for insert_stop_point
maxime-clem Oct 3, 2024
9ba045e
add test for generate_stop_line
maxime-clem Oct 3, 2024
9d03cf3
refactor and add test for is_stoppable()
maxime-clem Oct 3, 2024
2f4c759
cleanup dependencies in package.xml and add Maxime as maintainer
maxime-clem Oct 4, 2024
e89546e
refactor and add test for generate_ego_no_stopping_area_lane_polygons
maxime-clem Oct 4, 2024
e32a6fb
refactor and add test for check_stop_lines_in_no_stopping_area
maxime-clem Oct 4, 2024
43cd5fb
update docstrings and add test for get_stop_line_geometry_2d
maxime-clem Oct 4, 2024
32aa3ed
disable failing checks for checkStopLinesInNoStoppingArea (need fix)
maxime-clem Oct 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
90 changes: 44 additions & 46 deletions ...unch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -208,55 +208,53 @@
<extra_arg name="use_intra_process_comms" value="false"/>
</composable_node>

<composable_node pkg="autoware_behavior_velocity_planner" plugin="autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode" name="behavior_velocity_planner" namespace="">
<!-- topic remap -->
<remap from="~/input/path_with_lane_id" to="path_with_lane_id"/>
<remap from="~/input/vector_map" to="$(var input_vector_map_topic_name)"/>
<remap from="~/input/vehicle_odometry" to="/localization/kinematic_state"/>
<remap from="~/input/accel" to="/localization/acceleration"/>
<remap from="~/input/dynamic_objects" to="/perception/object_recognition/objects"/>
<remap from="~/input/no_ground_pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
<remap from="~/input/compare_map_filtered_pointcloud" to="compare_map_filtered/pointcloud"/>
<remap from="~/input/vector_map_inside_area_filtered_pointcloud" to="vector_map_inside_area_filtered/pointcloud"/>
<remap from="~/input/external_velocity_limit_mps" to="/planning/scenario_planning/max_velocity_default"/>
<remap from="~/input/traffic_signals" to="$(var input_traffic_light_topic_name)"/>
<remap from="~/input/virtual_traffic_light_states" to="$(var input_virtual_traffic_light_topic_name)"/>
<remap from="~/input/occupancy_grid" to="/perception/occupancy_grid_map/map"/>
<remap from="~/output/path" to="path"/>
<remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
<remap from="~/output/infrastructure_commands" to="/planning/scenario_planning/status/infrastructure_commands"/>
<remap from="~/output/traffic_signal" to="debug/traffic_signal"/>
<!-- params -->
<param from="$(var common_param_path)"/>
<param from="$(var vehicle_param_file)"/>
<param from="$(var nearest_search_param_path)"/>
<param from="$(var velocity_smoother_param_path)"/>
<param from="$(var behavior_velocity_smoother_type_param_path)"/>
<param from="$(var behavior_velocity_planner_common_param_path)"/>
<param name="launch_modules" value="$(var behavior_velocity_planner_launch_modules)"/>
<param name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
<param name="is_simulation" value="$(var is_simulation)"/>
<!-- <param from="$(var template_param_path)"/> -->
<param from="$(var behavior_velocity_planner_blind_spot_module_param_path)"/>
<param from="$(var behavior_velocity_planner_crosswalk_module_param_path)"/>
<param from="$(var behavior_velocity_planner_walkway_module_param_path)"/>
<param from="$(var behavior_velocity_planner_detection_area_module_param_path)"/>
<param from="$(var behavior_velocity_planner_intersection_module_param_path)"/>
<param from="$(var behavior_velocity_planner_stop_line_module_param_path)"/>
<param from="$(var behavior_velocity_planner_traffic_light_module_param_path)"/>
<param from="$(var behavior_velocity_planner_virtual_traffic_light_module_param_path)"/>
<param from="$(var behavior_velocity_planner_occlusion_spot_module_param_path)"/>
<param from="$(var behavior_velocity_planner_no_stopping_area_module_param_path)"/>
<param from="$(var behavior_velocity_planner_run_out_module_param_path)"/>
<param from="$(var behavior_velocity_planner_speed_bump_module_param_path)"/>
<param from="$(var behavior_velocity_planner_no_drivable_lane_module_param_path)"/>
<!-- composable node config -->
<extra_arg name="use_intra_process_comms" value="false"/>
</composable_node>

<composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
</node_container>

<node pkg="autoware_behavior_velocity_planner" exec="autoware_behavior_velocity_planner_node" name="behavior_velocity_planner" namespace="" launch-prefix="konsole -e gdb -ex run --args">

Check warning on line 214 in launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (konsole)
<!-- topic remap -->
<remap from="~/input/path_with_lane_id" to="path_with_lane_id"/>
<remap from="~/input/vector_map" to="$(var input_vector_map_topic_name)"/>
<remap from="~/input/vehicle_odometry" to="/localization/kinematic_state"/>
<remap from="~/input/accel" to="/localization/acceleration"/>
<remap from="~/input/dynamic_objects" to="/perception/object_recognition/objects"/>
<remap from="~/input/no_ground_pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
<remap from="~/input/compare_map_filtered_pointcloud" to="compare_map_filtered/pointcloud"/>
<remap from="~/input/vector_map_inside_area_filtered_pointcloud" to="vector_map_inside_area_filtered/pointcloud"/>
<remap from="~/input/external_velocity_limit_mps" to="/planning/scenario_planning/max_velocity_default"/>
<remap from="~/input/traffic_signals" to="$(var input_traffic_light_topic_name)"/>
<remap from="~/input/virtual_traffic_light_states" to="$(var input_virtual_traffic_light_topic_name)"/>
<remap from="~/input/occupancy_grid" to="/perception/occupancy_grid_map/map"/>
<remap from="~/output/path" to="path"/>
<remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
<remap from="~/output/infrastructure_commands" to="/planning/scenario_planning/status/infrastructure_commands"/>
<remap from="~/output/traffic_signal" to="debug/traffic_signal"/>
<!-- params -->
<param from="$(var common_param_path)"/>
<param from="$(var vehicle_param_file)"/>
<param from="$(var nearest_search_param_path)"/>
<param from="$(var velocity_smoother_param_path)"/>
<param from="$(var behavior_velocity_smoother_type_param_path)"/>
<param from="$(var behavior_velocity_planner_common_param_path)"/>
<param name="launch_modules" value="$(var behavior_velocity_planner_launch_modules)"/>
<param name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
<param name="is_simulation" value="$(var is_simulation)"/>
<!-- <param from="$(var template_param_path)"/> -->
<param from="$(var behavior_velocity_planner_blind_spot_module_param_path)"/>
<param from="$(var behavior_velocity_planner_crosswalk_module_param_path)"/>
<param from="$(var behavior_velocity_planner_walkway_module_param_path)"/>
<param from="$(var behavior_velocity_planner_detection_area_module_param_path)"/>
<param from="$(var behavior_velocity_planner_intersection_module_param_path)"/>
<param from="$(var behavior_velocity_planner_stop_line_module_param_path)"/>
<param from="$(var behavior_velocity_planner_traffic_light_module_param_path)"/>
<param from="$(var behavior_velocity_planner_virtual_traffic_light_module_param_path)"/>
<param from="$(var behavior_velocity_planner_occlusion_spot_module_param_path)"/>
<param from="$(var behavior_velocity_planner_no_stopping_area_module_param_path)"/>
<param from="$(var behavior_velocity_planner_run_out_module_param_path)"/>
<param from="$(var behavior_velocity_planner_speed_bump_module_param_path)"/>
<param from="$(var behavior_velocity_planner_no_drivable_lane_module_param_path)"/>
</node>

<group if="$(var launch_compare_map_pipeline)">
<!-- use pointcloud container -->
<load_composable_node target="$(var pointcloud_container_name)">
Expand Down
13 changes: 10 additions & 3 deletions ...havior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,16 @@ autoware_package()
pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml)

ament_auto_add_library(${PROJECT_NAME} SHARED
src/debug.cpp
src/manager.cpp
src/scene_no_stopping_area.cpp
DIRECTORY src
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
test/test_utils.cpp
)
target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
endif()

ament_auto_package(INSTALL_TO_SHARE config)
8 changes: 2 additions & 6 deletions .../behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,35 +8,31 @@
<maintainer email="[email protected]">Kosuke Takeuchi</maintainer>
<maintainer email="[email protected]">Tomoya Kimura</maintainer>
<maintainer email="[email protected]">Shumpei Wakabayashi</maintainer>
<maintainer email="[email protected]">Shumpei Wakabayashi</maintainer>

<license>Apache License 2.0</license>

<author email="[email protected]">Kosuke Takeuchi</author>

<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>
<buildtool_depend>eigen3_cmake_module</buildtool_depend>

<depend>autoware_behavior_velocity_planner_common</depend>
<depend>autoware_interpolation</depend>
<depend>autoware_lanelet2_extension</depend>
<depend>autoware_motion_utils</depend>
<depend>autoware_perception_msgs</depend>
<depend>autoware_planning_msgs</depend>
<depend>autoware_route_handler</depend>
<depend>autoware_universe_utils</depend>
<depend>autoware_vehicle_info_utils</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
<depend>libboost-dev</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tf2_eigen</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tier4_planning_msgs</depend>
<depend>visualization_msgs</depend>

<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

Expand Down
35 changes: 16 additions & 19 deletions ...ehavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,17 +13,13 @@
// limitations under the License.

#include "scene_no_stopping_area.hpp"
#include "utils.hpp"

#include <autoware/behavior_velocity_planner_common/utilization/debug.hpp>
#include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
#include <autoware/motion_utils/marker/virtual_wall_marker_creator.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>
#include <autoware/universe_utils/ros/marker_helper.hpp>
#ifdef ROS_DISTRO_GALACTIC
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#endif

#include <string>
#include <vector>
Expand All @@ -33,13 +29,13 @@
namespace
{
const double marker_lifetime = 0.2;
using DebugData = NoStoppingAreaModule::DebugData;
using DebugData = no_stopping_area::DebugData;
using autoware::universe_utils::appendMarkerArray;
using autoware::universe_utils::createDefaultMarker;
using autoware::universe_utils::createMarkerColor;
using autoware::universe_utils::createMarkerScale;

lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly)
lanelet::BasicPoint3d get_centroid_point(const lanelet::BasicPolygon3d & poly)
{
lanelet::BasicPoint3d p_sum{0.0, 0.0, 0.0};
for (const auto & p : poly) {
Expand All @@ -48,7 +44,7 @@
return p_sum / poly.size();
}

geometry_msgs::msg::Point toMsg(const lanelet::BasicPoint3d & point)
geometry_msgs::msg::Point to_msg(const lanelet::BasicPoint3d & point)
{
geometry_msgs::msg::Point msg;
msg.x = point.x();
Expand All @@ -57,71 +53,72 @@
return msg;
}

visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray(
visualization_msgs::msg::MarkerArray create_lanelet_info_marker_array(
const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const rclcpp::Time & now)
{
visualization_msgs::msg::MarkerArray msg;

// ID
{
auto marker = createDefaultMarker(
"map", now, "no_stopping_area_id", no_stopping_area_reg_elem.id(),
"map", now, "no_stopping_area_id", static_cast<int32_t>(no_stopping_area_reg_elem.id()),
visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0),
createMarkerColor(1.0, 1.0, 1.0, 0.999));
marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime);

for (const auto & detection_area : no_stopping_area_reg_elem.noStoppingAreas()) {
const auto poly = detection_area.basicPolygon();

marker.pose.position = toMsg(poly.front());
marker.pose.position = to_msg(poly.front());
marker.pose.position.z += 2.0;
marker.text = std::to_string(no_stopping_area_reg_elem.id());

msg.markers.push_back(marker);
}
}

// Polygon
{
auto marker = createDefaultMarker(
"map", now, "no_stopping_area_polygon", no_stopping_area_reg_elem.id(),
"map", now, "no_stopping_area_polygon", static_cast<int32_t>(no_stopping_area_reg_elem.id()),
visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.1, 0.0, 0.0),
createMarkerColor(0.1, 0.1, 1.0, 0.500));
marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime);

for (const auto & no_stopping_area : no_stopping_area_reg_elem.noStoppingAreas()) {
const auto poly = no_stopping_area.basicPolygon();

for (size_t i = 0; i < poly.size(); ++i) {
const auto idx_front = i;
const auto idx_back = (i == poly.size() - 1) ? 0 : i + 1;

const auto & p_front = poly.at(idx_front);
const auto & p_back = poly.at(idx_back);

marker.points.push_back(toMsg(p_front));
marker.points.push_back(toMsg(p_back));
marker.points.push_back(to_msg(p_front));
marker.points.push_back(to_msg(p_back));
}
}
msg.markers.push_back(marker);
}

const auto & stop_line = no_stopping_area_reg_elem.stopLine();
// Polygon to StopLine
if (stop_line) {
const auto stop_line_center_point =
(stop_line.value().front().basicPoint() + stop_line.value().back().basicPoint()) / 2;
auto marker = createDefaultMarker(
"map", now, "no_stopping_area_correspondence", no_stopping_area_reg_elem.id(),
"map", now, "no_stopping_area_correspondence",
static_cast<int32_t>(no_stopping_area_reg_elem.id()),
visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0),
createMarkerColor(0.1, 0.1, 1.0, 0.500));
marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime);
for (const auto & detection_area : no_stopping_area_reg_elem.noStoppingAreas()) {
const auto poly = detection_area.basicPolygon();
const auto centroid_point = getCentroidPoint(poly);
const auto centroid_point = get_centroid_point(poly);
for (size_t i = 0; i < poly.size(); ++i) {
marker.points.push_back(toMsg(centroid_point));
marker.points.push_back(toMsg(stop_line_center_point));
marker.points.push_back(to_msg(centroid_point));
marker.points.push_back(to_msg(stop_line_center_point));

Check notice on line 121 in planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Bumpy Road Ahead 

createLaneletInfoMarkerArray is no longer above the threshold for logical blocks with deeply nested code. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

Check notice on line 121 in planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ New issue: Bumpy Road Ahead 

create_lanelet_info_marker_array has 2 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.
}
}
msg.markers.push_back(marker);
Expand All @@ -137,7 +134,7 @@
const rclcpp::Time now = clock_->now();

appendMarkerArray(
createLaneletInfoMarkerArray(no_stopping_area_reg_elem_, now), &debug_marker_array, now);
create_lanelet_info_marker_array(no_stopping_area_reg_elem_, now), &debug_marker_array, now);

if (!debug_data_.stuck_points.empty()) {
appendMarkerArray(
Expand Down
5 changes: 0 additions & 5 deletions ...avior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,17 +16,12 @@

#include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
#include <autoware/universe_utils/ros/parameter.hpp>
#include <autoware_lanelet2_extension/utility/query.hpp>

#include <tf2/utils.h>

#include <limits>
#include <memory>
#include <set>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

namespace autoware::behavior_velocity_planner
{
Expand Down
2 changes: 1 addition & 1 deletion ...avior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC
const char * getModuleName() override { return "no_stopping_area"; }

private:
NoStoppingAreaModule::PlannerParam planner_param_;
NoStoppingAreaModule::PlannerParam planner_param_{};

void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override;

Expand Down
Loading
Loading