From a2ec9039cbb696f328fe49cc16ff80156aecc994 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Mon, 19 Aug 2024 17:32:19 +0900 Subject: [PATCH 1/6] fix(autoware_tensorrt_yolox): fix path to json schema files in README (#8497) Signed-off-by: Ryohsuke Mitsudome Co-authored-by: Yi-Hsiang Fang (Vivid) <146902905+vividf@users.noreply.github.com> From 776f5d82e3d9a8cd86c50397c2e4f7b757631e8b Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Mon, 19 Aug 2024 17:51:44 +0900 Subject: [PATCH 2/6] feat(psim)!: preapre settings to launch localization modules on psim (#8212) Signed-off-by: Yuki Takagi --- .../launch/simulator.launch.xml | 57 +++++++++++++++- .../simple_planning_simulator_core.hpp | 8 ++- .../simple_planning_simulator.launch.py | 68 ++++++++++++------- .../simple_planning_simulator_core.cpp | 23 ++++++- 4 files changed, 129 insertions(+), 27 deletions(-) diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 5f6632e76cdfa..5dec24c66f6ca 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -5,13 +5,18 @@ + + + + + @@ -130,7 +135,7 @@ - + @@ -148,6 +153,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -163,11 +212,15 @@ + + + + diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 0ed603960a6c5..547689dc847a0 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -137,7 +137,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_turn_indicators_report_; rclcpp::Publisher::SharedPtr pub_hazard_lights_report_; rclcpp::Publisher::SharedPtr pub_tf_; - rclcpp::Publisher::SharedPtr pub_current_pose_; + rclcpp::Publisher::SharedPtr pub_current_pose_; rclcpp::Subscription::SharedPtr sub_gear_cmd_; rclcpp::Subscription::SharedPtr sub_manual_gear_cmd_; @@ -346,6 +346,12 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ void publish_odometry(const Odometry & odometry); + /** + * @brief publish pose + * @param [in] odometry The odometry to publish its pose + */ + void publish_pose(const Odometry & odometry); + /** * @brief publish steering * @param [in] steer The steering to publish diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index afdbeb120e2d3..e6a35bd420bf3 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -41,6 +41,50 @@ def launch_setup(context, *args, **kwargs): param_file=simulator_model_param_path, allow_substs=True ) + # Base remappings + remappings = [ + ("input/vector_map", "/map/vector_map"), + ("input/initialpose", "/initialpose3d"), + ("input/ackermann_control_command", "/control/command/control_cmd"), + ("input/actuation_command", "/control/command/actuation_cmd"), + ("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"), + ("input/gear_command", "/control/command/gear_cmd"), + ("input/manual_gear_command", "/vehicle/command/manual_gear_command"), + ("input/turn_indicators_command", "/control/command/turn_indicators_cmd"), + ("input/hazard_lights_command", "/control/command/hazard_lights_cmd"), + ("input/trajectory", "/planning/scenario_planning/trajectory"), + ("input/engage", "/vehicle/engage"), + ("input/control_mode_request", "/control/control_mode_request"), + ("output/twist", "/vehicle/status/velocity_status"), + ("output/imu", "/sensing/imu/imu_data"), + ("output/steering", "/vehicle/status/steering_status"), + ("output/gear_report", "/vehicle/status/gear_status"), + ("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"), + ("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"), + ("output/control_mode_report", "/vehicle/status/control_mode"), + ] + + # Additional remappings + if LaunchConfiguration("motion_publish_mode").perform(context) == "pose_only": + remappings.extend( + [ + ("output/odometry", "/simulation/debug/localization/kinematic_state"), + ("output/acceleration", "/simulation/debug/localization/acceleration"), + ("output/pose", "/localization/pose_estimator/pose_with_covariance"), + ] + ) + elif LaunchConfiguration("motion_publish_mode").perform(context) == "full_motion": + remappings.extend( + [ + ("output/odometry", "/localization/kinematic_state"), + ("output/acceleration", "/localization/acceleration"), + ( + "output/pose", + "/simulation/debug/localization/pose_estimator/pose_with_covariance", + ), + ] + ) + simple_planning_simulator_node = Node( package="simple_planning_simulator", executable="simple_planning_simulator_exe", @@ -55,29 +99,7 @@ def launch_setup(context, *args, **kwargs): "initial_engage_state": LaunchConfiguration("initial_engage_state"), }, ], - remappings=[ - ("input/vector_map", "/map/vector_map"), - ("input/initialpose", "/initialpose3d"), - ("input/ackermann_control_command", "/control/command/control_cmd"), - ("input/actuation_command", "/control/command/actuation_cmd"), - ("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"), - ("input/gear_command", "/control/command/gear_cmd"), - ("input/manual_gear_command", "/vehicle/command/manual_gear_command"), - ("input/turn_indicators_command", "/control/command/turn_indicators_cmd"), - ("input/hazard_lights_command", "/control/command/hazard_lights_cmd"), - ("input/trajectory", "/planning/scenario_planning/trajectory"), - ("input/engage", "/vehicle/engage"), - ("input/control_mode_request", "/control/control_mode_request"), - ("output/twist", "/vehicle/status/velocity_status"), - ("output/odometry", "/localization/kinematic_state"), - ("output/acceleration", "/localization/acceleration"), - ("output/imu", "/sensing/imu/imu_data"), - ("output/steering", "/vehicle/status/steering_status"), - ("output/gear_report", "/vehicle/status/gear_status"), - ("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"), - ("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"), - ("output/control_mode_report", "/vehicle/status/control_mode"), - ], + remappings=remappings, ) # Determine if we should launch raw_vehicle_cmd_converter based on the vehicle_model_type diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 3095e060a71bf..4252ace6c1920 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -165,7 +165,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt create_publisher("output/turn_indicators_report", QoS{1}); pub_hazard_lights_report_ = create_publisher("output/hazard_lights_report", QoS{1}); - pub_current_pose_ = create_publisher("output/debug/pose", QoS{1}); + pub_current_pose_ = create_publisher("output/pose", QoS{1}); pub_velocity_ = create_publisher("output/twist", QoS{1}); pub_odom_ = create_publisher("output/odometry", QoS{1}); pub_steer_ = create_publisher("output/steering", QoS{1}); @@ -444,6 +444,7 @@ void SimplePlanningSimulator::on_timer() // publish vehicle state publish_odometry(current_odometry_); + publish_pose(current_odometry_); publish_velocity(current_velocity_); publish_steering(current_steer_); publish_acceleration(); @@ -749,6 +750,26 @@ void SimplePlanningSimulator::publish_odometry(const Odometry & odometry) pub_odom_->publish(msg); } +void SimplePlanningSimulator::publish_pose(const Odometry & odometry) +{ + geometry_msgs::msg::PoseWithCovarianceStamped msg; + + msg.pose = odometry.pose; + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + constexpr auto COV_POS = 0.0225; // same value as current ndt output + constexpr auto COV_ANGLE = 0.000625; // same value as current ndt output + msg.pose.covariance.at(COV_IDX::X_X) = COV_POS; + msg.pose.covariance.at(COV_IDX::Y_Y) = COV_POS; + msg.pose.covariance.at(COV_IDX::Z_Z) = COV_POS; + msg.pose.covariance.at(COV_IDX::ROLL_ROLL) = COV_ANGLE; + msg.pose.covariance.at(COV_IDX::PITCH_PITCH) = COV_ANGLE; + msg.pose.covariance.at(COV_IDX::YAW_YAW) = COV_ANGLE; + + msg.header.frame_id = origin_frame_id_; + msg.header.stamp = get_clock()->now(); + pub_current_pose_->publish(msg); +} + void SimplePlanningSimulator::publish_steering(const SteeringReport & steer) { SteeringReport msg = steer; From 5dd947a64221726f78a6c855f9a16665ffecb31a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 19 Aug 2024 17:54:55 +0900 Subject: [PATCH 3/6] chore(autoware_multi_object_tracker): fix typo in input_channels.schema.json (#8515) * fix(schema): fix typo in input_channels.schema.json Fixed a typo in the "lidar_pointpainting" key in the input_channels.schema.json file. Signed-off-by: Taekjin LEE * fix: fix typo in lidar_pointpainting key Signed-off-by: Taekjin LEE * chore: fix typo of lidar_pointpainitng channel Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE Co-authored-by: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> --- .../config/input_channels.param.yaml | 4 ++-- .../schema/input_channels.schema.json | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/autoware_multi_object_tracker/config/input_channels.param.yaml b/perception/autoware_multi_object_tracker/config/input_channels.param.yaml index b57f37675d4f1..6f9bfcf6210dd 100644 --- a/perception/autoware_multi_object_tracker/config/input_channels.param.yaml +++ b/perception/autoware_multi_object_tracker/config/input_channels.param.yaml @@ -40,8 +40,8 @@ name: "apollo" short_name: "Lap" # LIDAR-CAMERA - DNN - # cspell:ignore lidar_pointpainitng pointpainting - lidar_pointpainitng: + # cspell:ignore lidar_pointpainting pointpainting + lidar_pointpainting: topic: "/perception/object_recognition/detection/pointpainting/objects" can_spawn_new_tracker: true optional: diff --git a/perception/autoware_multi_object_tracker/schema/input_channels.schema.json b/perception/autoware_multi_object_tracker/schema/input_channels.schema.json index 3d7bba9daae87..6a35ca4f0e3bc 100644 --- a/perception/autoware_multi_object_tracker/schema/input_channels.schema.json +++ b/perception/autoware_multi_object_tracker/schema/input_channels.schema.json @@ -111,7 +111,7 @@ } } }, - "lidar_pointpainitng": { + "lidar_pointpainting": { "$ref": "#/definitions/input_channel", "default": { "topic": "/perception/object_recognition/detection/pointpainting/objects", @@ -185,7 +185,7 @@ "lidar_centerpoint_validated", "lidar_apollo", "lidar_apollo_validated", - "lidar_pointpainitng", + "lidar_pointpainting", "lidar_pointpainting_validated", "camera_lidar_fusion", "detection_by_tracker", From 7424c48764b07b0e89759fc70b7550d544193d40 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 19 Aug 2024 21:31:03 +0900 Subject: [PATCH 4/6] refactor(behavior_path_planner): apply clang-tidy check (#7549) * goal_planner Signed-off-by: Mamoru Sobue * lane_change Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> --- .../src/goal_searcher.cpp | 4 +++- .../src/utils/utils.cpp | 5 +++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index 9c501b7b2ae82..9137e71accf87 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -207,7 +207,7 @@ void GoalSearcher::countObjectsToAvoid( const double backward_length = parameters_.backward_goal_search_length; // calculate search start/end pose in pull over lanes - const auto [search_start_pose, search_end_pose] = std::invoke([&]() -> std::pair { + const auto search_start_end_poses = std::invoke([&]() -> std::pair { const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); @@ -221,6 +221,8 @@ void GoalSearcher::countObjectsToAvoid( return std::make_pair( center_line_path.points.front().point.pose, center_line_path.points.back().point.pose); }); + const auto search_start_pose = std::get<0>(search_start_end_poses); + const auto search_end_pose = std::get<1>(search_start_end_poses); // generate current lane center line path to check collision with objects const auto current_lanes = utils::getExtendedCurrentLanes( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 928506fa448c3..45cbb122d1e93 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -92,8 +92,9 @@ double calcMinimumLaneChangeLength( } const auto min_vel = lane_change_parameters.minimum_lane_changing_velocity; - const auto [min_lat_acc, max_lat_acc] = - lane_change_parameters.lane_change_lat_acc_map.find(min_vel); + const auto min_max_lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(min_vel); + // const auto min_lat_acc = std::get<0>(min_max_lat_acc); + const auto max_lat_acc = std::get<1>(min_max_lat_acc); const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk; const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; From 56ba7f90dc52db163bcfb9c0c4cf626c002e5134 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 20 Aug 2024 00:10:26 +0900 Subject: [PATCH 5/6] refactor(autowre_accel_brake_map_calibrator): fix for flake-ros v0.9.0 (#8529) Signed-off-by: kosuke55 --- .../autoware_accel_brake_map_calibrator/scripts/calc_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vehicle/autoware_accel_brake_map_calibrator/scripts/calc_utils.py b/vehicle/autoware_accel_brake_map_calibrator/scripts/calc_utils.py index f4581699369a1..dbb70a92d3b09 100755 --- a/vehicle/autoware_accel_brake_map_calibrator/scripts/calc_utils.py +++ b/vehicle/autoware_accel_brake_map_calibrator/scripts/calc_utils.py @@ -37,7 +37,7 @@ def get_map_list(y_num, x_num): class CalcUtils: @staticmethod def average_filter(data, average_num): - if type(average_num) != int: + if not isinstance(average_num, int): print( "Error in average_filter(data, average_num):\ Type of average_num must be int" From 5e1188fd02788a41bfe813acb78daf65bc53fa62 Mon Sep 17 00:00:00 2001 From: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Date: Tue, 20 Aug 2024 08:15:15 +0900 Subject: [PATCH 6/6] feat(pointcloud_preprocessor)!: revert "fix: added temporary retrocompatibility to old perception data (#7929)" (#8397) * feat!(pointcloud_preprocessor): Revert "fix: added temporary retrocompatibility to old perception data (#7929)" This reverts commit 6b9f164b123e2f6a6fedf7330e507d4b68e45a09. Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * feat(pointcloud_preprocessor): minor grammar fix Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> --------- Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Co-authored-by: Kenzo Lobos Tsunekawa Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> --- .../autoware_pointcloud_preprocessor/src/filter.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp index 854e93ed52ebc..de6549463e7ff 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp @@ -359,19 +359,12 @@ bool autoware::pointcloud_preprocessor::Filter::convert_output_costly( void autoware::pointcloud_preprocessor::Filter::faster_input_indices_callback( const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices) { - // TODO(knzo25): This first branch is to give temporary compatibility with old perception data. - // Should be deleted soon - if (utils::is_data_layout_compatible_with_point_xyzi(*cloud)) { - RCLCPP_ERROR_THROTTLE( - get_logger(), *get_clock(), 10000, - "The pointcloud layout is compatible with PointXYZI. You may be using legacy " - "code/data. Continue at your own risk"); - } else if ( + if ( !utils::is_data_layout_compatible_with_point_xyzircaedt(*cloud) && !utils::is_data_layout_compatible_with_point_xyzirc(*cloud)) { RCLCPP_ERROR( get_logger(), - "The pointcloud layout is not compatible with PointXYZIRCAEDT/PointXYZIRC. Aborting"); + "The pointcloud layout is not compatible with PointXYZIRCAEDT or PointXYZIRC. Aborting"); if (utils::is_data_layout_compatible_with_point_xyziradrt(*cloud)) { RCLCPP_ERROR(