From 90b86179137c495f8d9350fb7c8ee7dbc85646e2 Mon Sep 17 00:00:00 2001 From: shulanbushangshu <102840938+shulanbushangshu@users.noreply.github.com> Date: Mon, 23 Jan 2023 07:01:07 +0800 Subject: [PATCH 01/60] fix(behavior_path_planner): revise some content about avoidance in .md (#2261) * fix-behavior-path-doc Signed-off-by: jack.song * fix-behavior-path-doc Signed-off-by: jack.song * ci(pre-commit): autofix Signed-off-by: jack.song Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/behavior_path_planner/README.md | 1 + .../behavior_path_planner_avoidance-design.md | 39 ++++++++++++++++--- 2 files changed, 35 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index faa61a7449e3c..cf335aa3a5db1 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -135,6 +135,7 @@ Dynamic objects that satisfy the following conditions are considered to be avoid - low speed (default: < `1.0 m/s`) - Not being around center line (default: deviation from center > `0.5 m`) - Any footprint of the object in on the detection area (driving lane + `1 m` margin for lateral direction). +- Object is not behind ego(default: > -`2.0 m`) or too far(default: < `150.0 m`) and object is not behind the path goal. diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md index 4143c7dfdc986..1f3ff22093689 100644 --- a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md +++ b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md @@ -181,6 +181,7 @@ The avoidance target should be limited to stationary objects (you should not avo - User can limit avoidance targets (e.g. do not avoid unknown-class targets). - It is not being in the center of the route - This means that the vehicle is parked on the edge of the lane. This prevents the vehicle from avoiding a vehicle waiting at a traffic light in the middle of the lane. However, this is not an appropriate implementation for the purpose. Even if a vehicle is in the center of the lane, it should be avoided if it has its hazard lights on, and this is a point that should be improved in the future as the recognition performance improves. +- Object is not behind ego(default: > -`2.0 m`) or too far(default: < `150.0 m`) and object is not behind the path goal. ![fig1](./image/avoidance_design/target_vehicle_selection.drawio.svg) @@ -193,11 +194,20 @@ In order to prevent chattering of recognition results, once an obstacle is targe The lateral shift length is affected by 4 variables, namely `lateral_collision_safety_buffer`, `lateral_collision_margin`, `vehicle_width` and `overhang_distance`. The equation is as follows ```C++ -max_allowable_lateral_distance = lateral_collision_margin + lateral_collision_safety_buffer + 0.5 * vehicle_width -shift_length = max_allowable_lateral_distance - overhang_distance +avoid_margin = lateral_collision_margin + lateral_collision_safety_buffer + 0.5 * vehicle_width +max_allowable_lateral_distance = to_road_shoulder_distance - road_shoulder_safety_margin - 0.5 * vehicle_width +if(isOnRight(o)) +{ + shift_length = avoid_margin + overhang_distance +} +else +{ + shift_length = avoid_margin - overhang_distance +} ``` -The following figure illustrates these variables. +The following figure illustrates these variables(This figure just shows the max value of lateral shift length). + ![shift_point_and_its_constraints](./image/avoidance_design/avoidance_module-shift_point_and_its_constraints.drawio.png) ##### Rationale of having safety buffer and safety margin @@ -222,12 +232,29 @@ These elements are used to compute the distance from the object to the road's sh ![obstacle_to_road_shoulder_distance](./image/avoidance_design/obstacle_to_road_shoulder_distance.drawio.svg) -If the following condition is `false`, then the shift point will not be generated. +If one of the following conditions is `false`, then the shift point will not be generated. + +- The distance to shoulder of road is enough ```C++ -max_allowable_lateral_distance <= (to_road_shoulder_distance - 0.5 * vehicle_width - road_shoulder_safety_margin) +avoid_margin = lateral_collision_margin + lateral_collision_safety_buffer + 0.5 * vehicle_width +avoid_margin <= (to_road_shoulder_distance - 0.5 * vehicle_width - road_shoulder_safety_margin) ``` +- The obstacle intrudes into the current driving path. + + - when the object is on right of the path + + ```C++ + -overhang_dist<(lateral_collision_margin + lateral_collision_safety_buffer + 0.5 * vehicle_width) + ``` + + - when the object is on left of the path + + ```C++ + overhang_dist<(lateral_collision_margin + lateral_collision_safety_buffer + 0.5 * vehicle_width) + ``` + ##### Flow-chart of the process @@ -322,6 +349,8 @@ if(isOnRight(object)?) then (yes) else (\n No) :shift_length = std::max(object.overhang_dist - avoid_margin); endif +if(isSameDirectionShift(isOnRight(object),shift_length)?) then (no) +stop endif } stop From e64860a18054ab1e9657458cb9ae611306537bb3 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 23 Jan 2023 10:09:29 +0900 Subject: [PATCH 02/60] feat(obstacle_stop_planner): improve chattering prevention mechanism (#2647) * fix(obstacle_stop_planner): hunting prevention Signed-off-by: satoshi-ota * fix(obstacle_stop_planner): remove old hunting prevention mechanism Signed-off-by: satoshi-ota * fix(obstacle_stop_planner): fix logic Signed-off-by: satoshi-ota * fix(obstacle_stop_planner): use struct Signed-off-by: satoshi-ota * fix(obstacle_stop_planner): init target stop pose Signed-off-by: satoshi-ota * fix(obstacle_stop_planner): fix constructor Signed-off-by: satoshi-ota * fix(obstacle_stop_planner): hunting -> chattering Signed-off-by: satoshi-ota Signed-off-by: satoshi-ota --- planning/obstacle_stop_planner/README.md | 10 +- .../config/obstacle_stop_planner.param.yaml | 2 +- .../include/obstacle_stop_planner/node.hpp | 41 +++++- .../obstacle_stop_planner/planner_data.hpp | 2 +- .../src/debug_marker.cpp | 1 + planning/obstacle_stop_planner/src/node.cpp | 136 ++++++++++-------- 6 files changed, 120 insertions(+), 72 deletions(-) diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index a76b8a9b814f4..2ae4146ce09be 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -31,11 +31,11 @@ ### Common Parameter -| Parameter | Type | Description | -| ------------------- | ------ | -------------------------------------------------------------------------------------- | -| `enable_slow_down` | bool | enable slow down planner [-] | -| `max_velocity` | double | max velocity [m/s] | -| `hunting_threshold` | double | even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] | +| Parameter | Type | Description | +| ---------------------- | ------ | ----------------------------------------------------------------------------------------- | +| `enable_slow_down` | bool | enable slow down planner [-] | +| `max_velocity` | double | max velocity [m/s] | +| `chattering_threshold` | double | even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] | ## Obstacle Stop Planner diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml index de07e9e41b7c1..e9445f4e920e7 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] + chattering_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] max_velocity: 20.0 # max velocity [m/s] enable_slow_down: False # whether to use slow down planner [-] voxel_grid_x: 0.05 # voxel grid x parameter for filtering pointcloud [m] diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 236d7338bcdd7..c461e112e40e7 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -58,6 +58,7 @@ #include #include #include +#include #include namespace motion_planning @@ -91,6 +92,17 @@ using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; +struct ObstacleWithDetectionTime +{ + explicit ObstacleWithDetectionTime(const rclcpp::Time & t, pcl::PointXYZ & p) + : detection_time(t), point(p) + { + } + + rclcpp::Time detection_time; + pcl::PointXYZ point; +}; + class ObstacleStopPlannerNode : public rclcpp::Node { public: @@ -121,14 +133,12 @@ class ObstacleStopPlannerNode : public rclcpp::Node std::unique_ptr acc_controller_; std::shared_ptr debug_ptr_; - boost::optional latest_stop_point_{boost::none}; boost::optional latest_slow_down_section_{boost::none}; + std::vector obstacle_history_{}; tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; - rclcpp::Time last_detect_time_collision_point_; - rclcpp::Time last_detect_time_slowdown_point_; Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_{nullptr}; @@ -202,6 +212,31 @@ class ObstacleStopPlannerNode : public rclcpp::Node void onDynamicObjects(const PredictedObjects::ConstSharedPtr input_msg); void onExpandStopRange(const ExpandStopRange::ConstSharedPtr input_msg); + + void updateObstacleHistory(const rclcpp::Time & now) + { + for (auto itr = obstacle_history_.begin(); itr != obstacle_history_.end();) { + const auto expired = (now - itr->detection_time).seconds() > node_param_.chattering_threshold; + + if (expired) { + itr = obstacle_history_.erase(itr); + continue; + } + + itr++; + } + } + + PointCloud::Ptr getOldPointCloudPtr() const + { + PointCloud::Ptr ret(new PointCloud); + + for (const auto & p : obstacle_history_) { + ret->push_back(p.point); + } + + return ret; + } }; } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp index 58895a9d40c78..4f88972d7206e 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp @@ -63,7 +63,7 @@ struct NodeParam double max_velocity; // keep slow down or stop state if obstacle vanished [s] - double hunting_threshold; + double chattering_threshold; // dist threshold for ego's nearest index double ego_nearest_dist_threshold; diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp index 683bbdf9e6113..425577504867e 100644 --- a/planning/obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -168,6 +168,7 @@ void ObstacleStopPlannerDebugNode::publish() slow_down_range_polygons_.clear(); slow_down_polygons_.clear(); stop_pose_ptr_ = nullptr; + target_stop_pose_ptr_ = nullptr; slow_down_start_pose_ptr_ = nullptr; slow_down_end_pose_ptr_ = nullptr; stop_obstacle_point_ptr_ = nullptr; diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 0a29beec48840..5e2c031925786 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -65,7 +65,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod auto & p = node_param_; p.enable_slow_down = declare_parameter("enable_slow_down"); p.max_velocity = declare_parameter("max_velocity"); - p.hunting_threshold = declare_parameter("hunting_threshold"); + p.chattering_threshold = declare_parameter("chattering_threshold"); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); p.voxel_grid_x = declare_parameter("voxel_grid_x", 0.05); @@ -152,8 +152,6 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod acc_controller_ = std::make_unique( this, i.vehicle_width_m, i.vehicle_length_m, i.max_longitudinal_offset_m); debug_ptr_ = std::make_shared(this, i.max_longitudinal_offset_m); - last_detect_time_slowdown_point_ = this->now(); - last_detect_time_collision_point_ = this->now(); // Publishers pub_trajectory_ = this->create_publisher("~/output/trajectory", 1); @@ -307,10 +305,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m current_vel, stop_param); const auto no_slow_down_section = !planner_data.slow_down_require && !latest_slow_down_section_; - const auto no_hunting = - (rclcpp::Time(input_msg->header.stamp) - last_detect_time_slowdown_point_).seconds() > - node_param_.hunting_threshold; - if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_ && no_hunting) { + if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_) { resetExternalVelocityLimit(current_acc, current_vel); } @@ -335,6 +330,10 @@ void ObstacleStopPlannerNode::searchObstacle( return; } + const auto now = this->now(); + + updateObstacleHistory(now); + for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { // create one step circle center for vehicle const auto & p_front = decimate_trajectory.at(i).pose; @@ -375,8 +374,6 @@ void ObstacleStopPlannerNode::searchObstacle( debug_ptr_->pushObstaclePoint(planner_data.nearest_slow_down_point, PointType::SlowDown); debug_ptr_->pushPolygon( one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDown); - - last_detect_time_slowdown_point_ = trajectory_header.stamp; } } else { @@ -395,52 +392,88 @@ void ObstacleStopPlannerNode::searchObstacle( PointCloud::Ptr collision_pointcloud_ptr(new PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - planner_data.found_collision_points = withinPolygon( + const auto found_collision_points = withinPolygon( one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point, next_center_point, slow_down_pointcloud_ptr, collision_pointcloud_ptr); - if (planner_data.found_collision_points) { - planner_data.decimate_trajectory_collision_index = i; + if (found_collision_points) { + pcl::PointXYZ nearest_collision_point; + rclcpp::Time nearest_collision_point_time; + getNearestPoint( - *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point, - &planner_data.nearest_collision_point_time); + *collision_pointcloud_ptr, p_front, &nearest_collision_point, + &nearest_collision_point_time); - debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); + obstacle_history_.emplace_back(now, nearest_collision_point); - planner_data.stop_require = planner_data.found_collision_points; + break; + } + } + } - mutex_.lock(); - const auto object_ptr = object_ptr_; - const auto current_velocity_ptr = current_velocity_ptr_; - mutex_.unlock(); + for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { + // create one step circle center for vehicle + const auto & p_front = decimate_trajectory.at(i).pose; + const auto & p_back = decimate_trajectory.at(i + 1).pose; + const auto prev_center_pose = getVehicleCenterFromBase(p_front, vehicle_info); + const Point2d prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); + const auto next_center_pose = getVehicleCenterFromBase(p_back, vehicle_info); + const Point2d next_center_point(next_center_pose.position.x, next_center_pose.position.y); - acc_controller_->insertAdaptiveCruiseVelocity( - decimate_trajectory, planner_data.decimate_trajectory_collision_index, - planner_data.current_pose, planner_data.nearest_collision_point, - planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, - &planner_data.stop_require, &output, trajectory_header); + std::vector one_step_move_vehicle_polygon; + // create one step polygon for vehicle + createOneStepPolygon( + p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.lateral_margin); + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, + PolygonType::Vehicle); + + PointCloud::Ptr collision_pointcloud_ptr(new PointCloud); + collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; + + // check new collision points + planner_data.found_collision_points = withinPolygon( + one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point, + next_center_point, getOldPointCloudPtr(), collision_pointcloud_ptr); + + if (planner_data.found_collision_points) { + planner_data.decimate_trajectory_collision_index = i; + getNearestPoint( + *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point, + &planner_data.nearest_collision_point_time); + + debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); - if (planner_data.stop_require) { - last_detect_time_collision_point_ = trajectory_header.stamp; - } + planner_data.stop_require = planner_data.found_collision_points; - break; + mutex_.lock(); + const auto object_ptr = object_ptr_; + const auto current_velocity_ptr = current_velocity_ptr_; + mutex_.unlock(); + + acc_controller_->insertAdaptiveCruiseVelocity( + decimate_trajectory, planner_data.decimate_trajectory_collision_index, + planner_data.current_pose, planner_data.nearest_collision_point, + planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, + &planner_data.stop_require, &output, trajectory_header); + + if (!planner_data.stop_require) { + obstacle_history_.clear(); } + + break; } } } void ObstacleStopPlannerNode::insertVelocity( - TrajectoryPoints & output, PlannerData & planner_data, const Header & trajectory_header, - const VehicleInfo & vehicle_info, const double current_acc, const double current_vel, - const StopParam & stop_param) + TrajectoryPoints & output, PlannerData & planner_data, + [[maybe_unused]] const Header & trajectory_header, const VehicleInfo & vehicle_info, + const double current_acc, const double current_vel, const StopParam & stop_param) { const auto & base_link2front = vehicle_info.max_longitudinal_offset_m; - const auto no_hunting_collision_point = - (rclcpp::Time(trajectory_header.stamp) - last_detect_time_collision_point_).seconds() > - node_param_.hunting_threshold; if (planner_data.stop_require) { // insert stop point @@ -502,7 +535,6 @@ void ObstacleStopPlannerNode::insertVelocity( current_stop_pos.point.pose = ego_pos_on_path.get(); insertStopPoint(current_stop_pos, output, planner_data.stop_reason_diag); - latest_stop_point_ = current_stop_pos; debug_ptr_->pushPose(getPose(stop_point.point), PoseType::TargetStop); debug_ptr_->pushPose(getPose(current_stop_pos.point), PoseType::Stop); @@ -510,28 +542,13 @@ void ObstacleStopPlannerNode::insertVelocity( } else { insertStopPoint(stop_point, output, planner_data.stop_reason_diag); - latest_stop_point_ = stop_point; debug_ptr_->pushPose(getPose(stop_point.point), PoseType::TargetStop); debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop); } } - } else if (!no_hunting_collision_point) { - if (latest_stop_point_) { - // update stop point index with the current trajectory - latest_stop_point_.get().index = findFirstNearestSegmentIndexWithSoftConstraints( - output, getPose(latest_stop_point_.get().point), node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); - insertStopPoint(latest_stop_point_.get(), output, planner_data.stop_reason_diag); - debug_ptr_->pushPose(getPose(latest_stop_point_.get().point), PoseType::TargetStop); - debug_ptr_->pushPose(getPose(latest_stop_point_.get().point), PoseType::Stop); - } } - const auto no_hunting_slowdown_point = - (rclcpp::Time(trajectory_header.stamp) - last_detect_time_slowdown_point_).seconds() > - node_param_.hunting_threshold; - if (planner_data.slow_down_require) { // insert slow down point const auto traj_end_idx = output.size() - 1; @@ -556,19 +573,14 @@ void ObstacleStopPlannerNode::insertVelocity( current_vel); if ( - (!latest_slow_down_section_ && - dist_baselink_to_obstacle + index_with_dist_remain.get().second < - vehicle_info.max_longitudinal_offset_m) || - !no_hunting_slowdown_point) { + !latest_slow_down_section_ && + dist_baselink_to_obstacle + index_with_dist_remain.get().second < + vehicle_info.max_longitudinal_offset_m) { latest_slow_down_section_ = slow_down_section; } insertSlowDownSection(slow_down_section, output); } - } else if (!no_hunting_slowdown_point) { - if (latest_slow_down_section_) { - insertSlowDownSection(latest_slow_down_section_.get(), output); - } } if (node_param_.enable_slow_down && latest_slow_down_section_) { @@ -604,7 +616,7 @@ void ObstacleStopPlannerNode::insertVelocity( set_velocity_limit_ ? std::numeric_limits::max() : target_velocity; insertSlowDownSection(slow_down_section, output); - } else if (no_hunting_slowdown_point) { + } else { latest_slow_down_section_ = {}; } } From 38992f07a23b9ab6784de746b22091962fffeafd Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Mon, 23 Jan 2023 12:59:46 +0900 Subject: [PATCH 03/60] refactor(ekf_localizer): use tier4_autoware_utils to simplify syntax (#2708) Use tier4_autoware_utils to simplify syntax --- .../ekf_localizer/src/ekf_localizer.cpp | 22 +++++-------------- 1 file changed, 5 insertions(+), 17 deletions(-) diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index e12e6dc8d9bb9..539911939a08d 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -231,9 +231,7 @@ void EKFLocalizer::timerCallback() current_ekf_pose_.header.frame_id = params_.pose_frame_id; current_ekf_pose_.header.stamp = this->now(); - current_ekf_pose_.pose.position.x = x; - current_ekf_pose_.pose.position.y = y; - current_ekf_pose_.pose.position.z = z; + current_ekf_pose_.pose.position = tier4_autoware_utils::createPoint(x, y, z); current_ekf_pose_.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw); @@ -271,20 +269,10 @@ void EKFLocalizer::timerTFCallback() return; } - geometry_msgs::msg::TransformStamped transformStamped; - transformStamped.header.stamp = this->now(); - transformStamped.header.frame_id = current_ekf_pose_.header.frame_id; - transformStamped.child_frame_id = "base_link"; - transformStamped.transform.translation.x = current_ekf_pose_.pose.position.x; - transformStamped.transform.translation.y = current_ekf_pose_.pose.position.y; - transformStamped.transform.translation.z = current_ekf_pose_.pose.position.z; - - transformStamped.transform.rotation.x = current_ekf_pose_.pose.orientation.x; - transformStamped.transform.rotation.y = current_ekf_pose_.pose.orientation.y; - transformStamped.transform.rotation.z = current_ekf_pose_.pose.orientation.z; - transformStamped.transform.rotation.w = current_ekf_pose_.pose.orientation.w; - - tf_br_->sendTransform(transformStamped); + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tier4_autoware_utils::pose2transform(current_ekf_pose_, "base_link"); + transform_stamped.header.stamp = this->now(); + tf_br_->sendTransform(transform_stamped); } /* From 4e554921f884af23024cabfde6e105659b5b3295 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 23 Jan 2023 13:35:27 +0900 Subject: [PATCH 04/60] chore(route_handler, rtc_interface, rtc_auto_mode_manager): add maintainer (#2714) * chore(route_handler): add maintainer Signed-off-by: Fumiya Watanabe * chore(rtc_interface): add maintainer Signed-off-by: Fumiya Watanabe * chore(rtc_auto_mode_manager): add maintainer Signed-off-by: Fumiya Watanabe Signed-off-by: Fumiya Watanabe --- planning/route_handler/package.xml | 3 +++ planning/rtc_auto_mode_manager/package.xml | 1 + planning/rtc_interface/package.xml | 1 + 3 files changed, 5 insertions(+) diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index 53858ca7adf00..fb863d2e825b3 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -5,6 +5,9 @@ 0.1.0 The route_handling package Fumiya Watanabe + Zulfaqar Azmi + Yutaka Shimizu + Kosuke Takeuchi Apache License 2.0 Fumiya Watanabe diff --git a/planning/rtc_auto_mode_manager/package.xml b/planning/rtc_auto_mode_manager/package.xml index 5f2c9179c8006..dc6d452df0649 100644 --- a/planning/rtc_auto_mode_manager/package.xml +++ b/planning/rtc_auto_mode_manager/package.xml @@ -6,6 +6,7 @@ The rtc_auto_mode_manager package Fumiya Watanabe + Taiki Tanaka Apache License 2.0 diff --git a/planning/rtc_interface/package.xml b/planning/rtc_interface/package.xml index 83853332884c2..152ca8c572aa7 100644 --- a/planning/rtc_interface/package.xml +++ b/planning/rtc_interface/package.xml @@ -5,6 +5,7 @@ The rtc_interface package Fumiya Watanabe + Taiki Tanaka Apache License 2.0 From 2a5bdbb11e0b1df84594d3d612554c0cfe9cf298 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 23 Jan 2023 13:36:28 +0900 Subject: [PATCH 05/60] chore(operation_mode_transition_manager): add maintainer (#2713) Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe --- control/operation_mode_transition_manager/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/control/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml index 417f2e9c383d3..86dc23b6c0368 100644 --- a/control/operation_mode_transition_manager/package.xml +++ b/control/operation_mode_transition_manager/package.xml @@ -3,6 +3,7 @@ 0.1.0 The vehicle_cmd_gate package Takamasa Horibe + Tomoya Kimura Apache License 2.0 Takamasa Horibe From 97b67589fd4c22f07af2aec1b5aa3c2ca508e3e9 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 23 Jan 2023 14:06:27 +0900 Subject: [PATCH 06/60] chore(vehicle_cmd_gate): add maintainer (#2716) Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe Co-authored-by: Takamasa Horibe --- control/vehicle_cmd_gate/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index ca509f653fdb6..92d6911c8f69d 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -5,6 +5,7 @@ 0.1.0 The vehicle_cmd_gate package Takamasa Horibe + Tomoya Kimura Apache License 2.0 Hiroki OTA From 0f3bfb74f707b0054208bcfedcb52954fed025fe Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Mon, 23 Jan 2023 05:31:24 +0000 Subject: [PATCH 07/60] chore: update CODEOWNERS (#2600) Signed-off-by: GitHub Signed-off-by: GitHub Co-authored-by: TakaHoribe --- .github/CODEOWNERS | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index df2f0c379c265..0172b42878437 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -44,20 +44,20 @@ common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp @autowarefoundati common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners common/time_utils/** christopherj.ho@gmail.com @autowarefoundation/autoware-global-codeowners common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tvm_utility/** ambroise.vincent@arm.com @autowarefoundation/autoware-global-codeowners +common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp @autowarefoundation/autoware-global-codeowners control/control_performance_analysis/** ali.boyali@tier4.jp berkay@leodrive.ai @autowarefoundation/autoware-global-codeowners control/external_cmd_selector/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners control/joy_controller/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners control/lane_departure_checker/** kyoichi.sugahara@tier4.jp @autowarefoundation/autoware-global-codeowners control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners control/obstacle_collision_checker/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners -control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners +control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners control/pid_longitudinal_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners control/pure_pursuit/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners control/shift_decider/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners +control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai @autowarefoundation/autoware-global-codeowners evaluator/localization_evaluator/** dominik.jargot@robotec.ai @autowarefoundation/autoware-global-codeowners launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -128,12 +128,13 @@ planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp @autowarefoundatio planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners planning/planning_error_monitor/** yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/planning_evaluator/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/route_handler/** fumiya.watanabe@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/rtc_interface/** fumiya.watanabe@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/planning_validator/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/rtc_interface/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners planning/scenario_selector/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/static_centerline_optimizer/** takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners planning/surround_obstacle_checker/** satoshi.ota@tier4.jp @autowarefoundation/autoware-global-codeowners sensing/geo_pos_conv/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners sensing/gnss_poser/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -166,7 +167,7 @@ system/system_monitor/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp @autowar system/topic_state_monitor/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners system/velodyne_monitor/** fumihito.ito@tier4.jp @autowarefoundation/autoware-global-codeowners tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai @autowarefoundation/autoware-global-codeowners -vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners +vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners From 5657467477f470563f1ba386636f1da8878741c9 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 23 Jan 2023 14:49:42 +0900 Subject: [PATCH 08/60] docs(operation_mode_transition_manager): update readme (#2606) * add readme for operation_transition_manager Signed-off-by: Takamasa Horibe * precommit Signed-off-by: Takamasa Horibe * Update control/operation_mode_transition_manager/README.md Signed-off-by: Takamasa Horibe --- .../README.md | 99 +++++++++++++++++++ .../src/node.cpp | 5 +- .../src/node.hpp | 4 +- 3 files changed, 104 insertions(+), 4 deletions(-) diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md index e852b552130e8..3bbc3a006c73a 100644 --- a/control/operation_mode_transition_manager/README.md +++ b/control/operation_mode_transition_manager/README.md @@ -1 +1,100 @@ # operation_mode_transition_manager + +## Purpose / Use cases + +This module is responsible for managing the different modes of operation for the Autoware system. The possible modes are: + +- `Autonomous`: the vehicle is fully controlled by the autonomous driving system +- `Local`: the vehicle is controlled by a physically connected control system such as a joy stick +- `Remote`: the vehicle is controlled by a remote controller +- `Stop`: the vehicle is stopped and there is no active control system. + +There is also an `In Transition` state that occurs during each mode transitions. During this state, the transition to the new operator is not yet complete, and the previous operator is still responsible for controlling the system until the transition is complete. Some actions may be restricted during the `In Transition` state, such as sudden braking or steering. (This is restricted by the `vehicle_cmd_gate`). + +### Features + +- Transit mode between `Autonomous`, `Local`, `Remote` and `Stop` based on the indication command. +- Check whether the each transition is available (safe or not). +- Limit some sudden motion control in `In Transition` mode (this is done with `vehicle_cmd_gate` feature). +- Check whether the transition is completed. + +- Transition between the `Autonomous`, `Local`, `Remote`, and `Stop` modes based on the indicated command. +- Determine whether each transition is safe to execute. +- Restrict certain sudden motion controls during the `In Transition` mode (using the `vehicle_cmd_gate` feature). +- Verify that the transition is complete. + +## Design + + + +## Inputs / Outputs / API + +### Inputs + +For the mode transition: + +- /system/operation_mode/change_autoware_control [`tier4_system_msgs/srv/ChangeAutowareControl`]: change operation mode to Autonomous +- /system/operation_mode/change_operation_mode [`tier4_system_msgs/srv/ChangeOperationMode`]: change operation mode + +For the transition availability/completion check: + +- /control/command/control_cmd [`autoware_auto_control_msgs/msg/AckermannControlCommand`]: vehicle control signal +- /localization/kinematic_state [`nav_msgs/msg/Odometry`]: ego vehicle state +- /planning/scenario_planning/trajectory [`autoware_auto_planning_msgs/msg/Trajectory`]: planning trajectory +- /vehicle/status/control_mode [`autoware_auto_vehicle_msgs/msg/ControlModeReport`]: vehicle control mode (autonomous/manual) +- /control/vehicle_cmd_gate/operation_mode [`autoware_adapi_v1_msgs/msg/OperationModeState`]: the operation mode in the `vehicle_cmd_gate`. (To be removed) + +For the backward compatibility (to be removed): + +- /api/autoware/get/engage [`autoware_auto_vehicle_msgs/msg/Engage`] +- /control/current_gate_mode [`tier4_control_msgs/msg/GateMode`] +- /control/external_cmd_selector/current_selector_mode [`tier4_control_msgs/msg/ExternalCommandSelectorMode`] + +### Outputs + +- /system/operation_mode/state [`autoware_adapi_v1_msgs/msg/OperationModeState`]: to inform the current operation mode +- /control/operation_mode_transition_manager/debug_info [`operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug`]: detailed information about the operation mode transition + +- /control/gate_mode_cmd [`tier4_control_msgs/msg/GateMode`]: to change the `vehicle_cmd_gate` state to use its features (to be removed) +- /autoware/engage [`autoware_auto_vehicle_msgs/msg/Engage`]: + +- /control/control_mode_request [`autoware_auto_vehicle_msgs/srv/ControlModeCommand`]: to change the vehicle control mode (autonomous/manual) +- /control/external_cmd_selector/select_external_command [`tier4_control_msgs/srv/ExternalCommandSelect`]: + +## Parameters + +| Name | Type | Description | Default value | +| :--------------------------------- | :------- | :------------------------------------------------------------------------------------------------ | :------------ | +| `transition_timeout` | `double` | If the state transition is not completed within this time, it is considered a transition failure. | 10.0 | +| `frequency_hz` | `double` | running hz | 10.0 | +| `check_engage_condition` | `double` | If false, autonomous transition is always available | 0.1 | +| `nearest_dist_deviation_threshold` | `double` | distance threshold used to find nearest trajectory point | 3.0 | +| `nearest_yaw_deviation_threshold` | `double` | angle threshold used to find nearest trajectory point | 1.57 | + +For `engage_acceptable_limits` related parameters: + +| Name | Type | Description | Default value | +| :---------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `allow_autonomous_in_stopped` | `bool` | If true, autonomous transition is available when the vehicle is stopped even if other checks fail. | true | +| `dist_threshold` | `double` | the distance between the trajectory and ego vehicle must be within this distance for `Autonomous` transition. | 1.5 | +| `yaw_threshold` | `double` | the yaw angle between trajectory and ego vehicle must be within this threshold for `Autonomous` transition. | 0.524 | +| `speed_upper_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold for `Autonomous` transition. | 10.0 | +| `speed_lower_threshold` | `double` | the velocity deviation between the control command and ego vehicle must be within this threshold for `Autonomous` transition. | -10.0 | +| `acc_threshold` | `double` | the control command acceleration must be less than this threshold for `Autonomous` transition. | 1.5 | +| `lateral_acc_threshold` | `double` | the control command lateral acceleration must be less than this threshold for `Autonomous` transition. | 1.0 | +| `lateral_acc_diff_threshold` | `double` | the lateral acceleration deviation between the control command must be less than this threshold for `Autonomous` transition. | 0.5 | + +For `stable_check` related parameters: + +| Name | Type | Description | Default value | +| :---------------------- | :------- | :-------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `duration` | `double` | the stable condition must be satisfied for this duration to complete the transition. | 0.1 | +| `dist_threshold` | `double` | the distance between the trajectory and ego vehicle must be within this distance to complete `Autonomous` transition. | 1.5 | +| `yaw_threshold` | `double` | the yaw angle between trajectory and ego vehicle must be within this threshold to complete `Autonomous` transition. | 0.262 | +| `speed_upper_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 | +| `speed_lower_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 | + +## Future extensions / Unimplemented parts + +- Need to remove backward compatibility interfaces. +- This node should be merged to the `vehicle_cmd_gate` due to its strong connection. diff --git a/control/operation_mode_transition_manager/src/node.cpp b/control/operation_mode_transition_manager/src/node.cpp index ddd37b0206fad..d620e68ababe5 100644 --- a/control/operation_mode_transition_manager/src/node.cpp +++ b/control/operation_mode_transition_manager/src/node.cpp @@ -37,8 +37,9 @@ OperationModeTransitionManager::OperationModeTransitionManager(const rclcpp::Nod { const auto node = component_interface_utils::NodeAdaptor(this); node.init_srv( - srv_autoware_control, this, &OperationModeTransitionManager::onChangeAutowareControl); - node.init_srv(srv_operation_mode, this, &OperationModeTransitionManager::onChangeOperationMode); + srv_autoware_control_, this, &OperationModeTransitionManager::onChangeAutowareControl); + node.init_srv( + srv_operation_mode_, this, &OperationModeTransitionManager::onChangeOperationMode); node.init_pub(pub_operation_mode_); } diff --git a/control/operation_mode_transition_manager/src/node.hpp b/control/operation_mode_transition_manager/src/node.hpp index fafc68a50b9e4..c2d761fcf138c 100644 --- a/control/operation_mode_transition_manager/src/node.hpp +++ b/control/operation_mode_transition_manager/src/node.hpp @@ -38,8 +38,8 @@ class OperationModeTransitionManager : public rclcpp::Node using ChangeAutowareControlAPI = system_interface::ChangeAutowareControl; using ChangeOperationModeAPI = system_interface::ChangeOperationMode; using OperationModeStateAPI = system_interface::OperationModeState; - component_interface_utils::Service::SharedPtr srv_autoware_control; - component_interface_utils::Service::SharedPtr srv_operation_mode; + component_interface_utils::Service::SharedPtr srv_autoware_control_; + component_interface_utils::Service::SharedPtr srv_operation_mode_; component_interface_utils::Publisher::SharedPtr pub_operation_mode_; void onChangeAutowareControl( const ChangeAutowareControlAPI::Service::Request::SharedPtr request, From 7451eb162ebdf014f47ccaeb3daef4374b52d647 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 23 Jan 2023 15:26:13 +0900 Subject: [PATCH 09/60] refactor(vehicle_cmd_gate): using namespace, etc (#2627) * refactor(vehicle_cmd_gate): using namespace, etc Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe * fix test Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe --- .../src/vehicle_cmd_filter.cpp | 26 ++-- .../src/vehicle_cmd_filter.hpp | 35 ++--- .../vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 145 +++++++----------- .../vehicle_cmd_gate/src/vehicle_cmd_gate.hpp | 37 ++--- .../test/src/test_vehicle_cmd_filter.cpp | 5 +- 5 files changed, 96 insertions(+), 152 deletions(-) diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index 2a000d9902174..5696e0445e407 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -17,17 +17,19 @@ #include #include +namespace vehicle_cmd_gate +{ + VehicleCmdFilter::VehicleCmdFilter() {} -void VehicleCmdFilter::limitLongitudinalWithVel( - autoware_auto_control_msgs::msg::AckermannControlCommand & input) const +void VehicleCmdFilter::limitLongitudinalWithVel(AckermannControlCommand & input) const { input.longitudinal.speed = std::max( std::min(static_cast(input.longitudinal.speed), param_.vel_lim), -param_.vel_lim); } void VehicleCmdFilter::limitLongitudinalWithAcc( - const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const + const double dt, AckermannControlCommand & input) const { input.longitudinal.acceleration = std::max( std::min(static_cast(input.longitudinal.acceleration), param_.lon_acc_lim), @@ -37,15 +39,14 @@ void VehicleCmdFilter::limitLongitudinalWithAcc( } void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( - const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const + const double dt, AckermannControlCommand & input) const { input.longitudinal.acceleration = limitDiff( input.longitudinal.acceleration, prev_cmd_.longitudinal.acceleration, param_.lon_jerk_lim * dt); } void VehicleCmdFilter::limitLateralWithLatAcc( - [[maybe_unused]] const double dt, - autoware_auto_control_msgs::msg::AckermannControlCommand & input) const + [[maybe_unused]] const double dt, AckermannControlCommand & input) const { double latacc = calcLatAcc(input); if (std::fabs(latacc) > param_.lat_acc_lim) { @@ -57,7 +58,7 @@ void VehicleCmdFilter::limitLateralWithLatAcc( } void VehicleCmdFilter::limitLateralWithLatJerk( - const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const + const double dt, AckermannControlCommand & input) const { double curr_latacc = calcLatAcc(input); double prev_latacc = calcLatAcc(prev_cmd_); @@ -73,8 +74,7 @@ void VehicleCmdFilter::limitLateralWithLatJerk( } void VehicleCmdFilter::limitActualSteerDiff( - const double current_steer_angle, - autoware_auto_control_msgs::msg::AckermannControlCommand & input) const + const double current_steer_angle, AckermannControlCommand & input) const { auto ds = input.lateral.steering_tire_angle - current_steer_angle; ds = std::clamp(ds, -param_.actual_steer_diff_lim, param_.actual_steer_diff_lim); @@ -82,8 +82,7 @@ void VehicleCmdFilter::limitActualSteerDiff( } void VehicleCmdFilter::filterAll( - const double dt, const double current_steer_angle, - autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const + const double dt, const double current_steer_angle, AckermannControlCommand & cmd) const { limitLongitudinalWithJerk(dt, cmd); limitLongitudinalWithAcc(dt, cmd); @@ -100,8 +99,7 @@ double VehicleCmdFilter::calcSteerFromLatacc(const double v, const double latacc return std::atan(latacc * param_.wheel_base / v_sq); } -double VehicleCmdFilter::calcLatAcc( - const autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const +double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd) const { double v = cmd.longitudinal.speed; return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; @@ -113,3 +111,5 @@ double VehicleCmdFilter::limitDiff( double diff = std::max(std::min(curr - prev, diff_lim), -diff_lim); return prev + diff; } + +} // namespace vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index 4cb6b6a1cef8f..e6b60687f6b65 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -19,6 +19,10 @@ #include +namespace vehicle_cmd_gate +{ +using autoware_auto_control_msgs::msg::AckermannControlCommand; + struct VehicleCmdFilterParam { double wheel_base; @@ -43,35 +47,26 @@ class VehicleCmdFilter void setLatJerkLim(double v) { param_.lat_jerk_lim = v; } void setActualSteerDiffLim(double v) { param_.actual_steer_diff_lim = v; } void setParam(const VehicleCmdFilterParam & p) { param_ = p; } - void setPrevCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand & v) - { - prev_cmd_ = v; - } + void setPrevCmd(const AckermannControlCommand & v) { prev_cmd_ = v; } - void limitLongitudinalWithVel( - autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; - void limitLongitudinalWithAcc( - const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; - void limitLongitudinalWithJerk( - const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; - void limitLateralWithLatAcc( - const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; - void limitLateralWithLatJerk( - const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; + void limitLongitudinalWithVel(AckermannControlCommand & input) const; + void limitLongitudinalWithAcc(const double dt, AckermannControlCommand & input) const; + void limitLongitudinalWithJerk(const double dt, AckermannControlCommand & input) const; + void limitLateralWithLatAcc(const double dt, AckermannControlCommand & input) const; + void limitLateralWithLatJerk(const double dt, AckermannControlCommand & input) const; void limitActualSteerDiff( - const double current_steer_angle, - autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; + const double current_steer_angle, AckermannControlCommand & input) const; void filterAll( - const double dt, const double current_steer_angle, - autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; + const double dt, const double current_steer_angle, AckermannControlCommand & input) const; private: VehicleCmdFilterParam param_; - autoware_auto_control_msgs::msg::AckermannControlCommand prev_cmd_; + AckermannControlCommand prev_cmd_; - double calcLatAcc(const autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const; + double calcLatAcc(const AckermannControlCommand & cmd) const; double calcSteerFromLatacc(const double v, const double latacc) const; double limitDiff(const double curr, const double prev, const double diff_lim) const; }; +} // namespace vehicle_cmd_gate #endif // VEHICLE_CMD_FILTER_HPP_ diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index e4eadc1b69d8c..2792c308fde97 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -53,81 +53,82 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) // Publisher vehicle_cmd_emergency_pub_ = - this->create_publisher("output/vehicle_cmd_emergency", durable_qos); - control_cmd_pub_ = - this->create_publisher("output/control_cmd", durable_qos); - gear_cmd_pub_ = this->create_publisher("output/gear_cmd", durable_qos); + create_publisher("output/vehicle_cmd_emergency", durable_qos); + control_cmd_pub_ = create_publisher("output/control_cmd", durable_qos); + gear_cmd_pub_ = create_publisher("output/gear_cmd", durable_qos); turn_indicator_cmd_pub_ = - this->create_publisher("output/turn_indicators_cmd", durable_qos); + create_publisher("output/turn_indicators_cmd", durable_qos); hazard_light_cmd_pub_ = - this->create_publisher("output/hazard_lights_cmd", durable_qos); - gate_mode_pub_ = this->create_publisher("output/gate_mode", durable_qos); - engage_pub_ = this->create_publisher("output/engage", durable_qos); - pub_external_emergency_ = - this->create_publisher("output/external_emergency", durable_qos); - operation_mode_pub_ = - this->create_publisher("output/operation_mode", durable_qos); + create_publisher("output/hazard_lights_cmd", durable_qos); + + gate_mode_pub_ = create_publisher("output/gate_mode", durable_qos); + engage_pub_ = create_publisher("output/engage", durable_qos); + pub_external_emergency_ = create_publisher("output/external_emergency", durable_qos); + operation_mode_pub_ = create_publisher("output/operation_mode", durable_qos); // Subscriber - external_emergency_stop_heartbeat_sub_ = this->create_subscription( + external_emergency_stop_heartbeat_sub_ = create_subscription( "input/external_emergency_stop_heartbeat", 1, std::bind(&VehicleCmdGate::onExternalEmergencyStopHeartbeat, this, _1)); - gate_mode_sub_ = this->create_subscription( + gate_mode_sub_ = create_subscription( "input/gate_mode", 1, std::bind(&VehicleCmdGate::onGateMode, this, _1)); - engage_sub_ = this->create_subscription( + engage_sub_ = create_subscription( "input/engage", 1, std::bind(&VehicleCmdGate::onEngage, this, _1)); - steer_sub_ = this->create_subscription( + steer_sub_ = create_subscription( "input/steering", 1, std::bind(&VehicleCmdGate::onSteering, this, _1)); - operation_mode_sub_ = this->create_subscription( + operation_mode_sub_ = create_subscription( "input/operation_mode", rclcpp::QoS(1).transient_local(), [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; }); - mrm_state_sub_ = this->create_subscription( + mrm_state_sub_ = create_subscription( "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1)); - gear_status_sub_ = this->create_subscription( - "input/gear_status", 1, std::bind(&VehicleCmdGate::onGearStatus, this, _1)); + gear_status_sub_ = create_subscription( + "input/gear_status", 1, [this](GearReport::SharedPtr msg) { current_gear_ptr_ = msg; }); // Subscriber for auto - auto_control_cmd_sub_ = this->create_subscription( + auto_control_cmd_sub_ = create_subscription( "input/auto/control_cmd", 1, std::bind(&VehicleCmdGate::onAutoCtrlCmd, this, _1)); - auto_turn_indicator_cmd_sub_ = this->create_subscription( + auto_turn_indicator_cmd_sub_ = create_subscription( "input/auto/turn_indicators_cmd", 1, - std::bind(&VehicleCmdGate::onAutoTurnIndicatorsCmd, this, _1)); + [this](TurnIndicatorsCommand::ConstSharedPtr msg) { auto_commands_.turn_indicator = *msg; }); - auto_hazard_light_cmd_sub_ = this->create_subscription( - "input/auto/hazard_lights_cmd", 1, std::bind(&VehicleCmdGate::onAutoHazardLightsCmd, this, _1)); + auto_hazard_light_cmd_sub_ = create_subscription( + "input/auto/hazard_lights_cmd", 1, + [this](HazardLightsCommand::ConstSharedPtr msg) { auto_commands_.hazard_light = *msg; }); - auto_gear_cmd_sub_ = this->create_subscription( - "input/auto/gear_cmd", 1, std::bind(&VehicleCmdGate::onAutoShiftCmd, this, _1)); + auto_gear_cmd_sub_ = create_subscription( + "input/auto/gear_cmd", 1, + [this](GearCommand::ConstSharedPtr msg) { auto_commands_.gear = *msg; }); // Subscriber for external - remote_control_cmd_sub_ = this->create_subscription( + remote_control_cmd_sub_ = create_subscription( "input/external/control_cmd", 1, std::bind(&VehicleCmdGate::onRemoteCtrlCmd, this, _1)); - remote_turn_indicator_cmd_sub_ = this->create_subscription( + remote_turn_indicator_cmd_sub_ = create_subscription( "input/external/turn_indicators_cmd", 1, - std::bind(&VehicleCmdGate::onRemoteTurnIndicatorsCmd, this, _1)); + [this](TurnIndicatorsCommand::ConstSharedPtr msg) { remote_commands_.turn_indicator = *msg; }); - remote_hazard_light_cmd_sub_ = this->create_subscription( + remote_hazard_light_cmd_sub_ = create_subscription( "input/external/hazard_lights_cmd", 1, - std::bind(&VehicleCmdGate::onRemoteHazardLightsCmd, this, _1)); + [this](HazardLightsCommand::ConstSharedPtr msg) { remote_commands_.hazard_light = *msg; }); - remote_gear_cmd_sub_ = this->create_subscription( - "input/external/gear_cmd", 1, std::bind(&VehicleCmdGate::onRemoteShiftCmd, this, _1)); + remote_gear_cmd_sub_ = create_subscription( + "input/external/gear_cmd", 1, + [this](GearCommand::ConstSharedPtr msg) { remote_commands_.gear = *msg; }); // Subscriber for emergency - emergency_control_cmd_sub_ = this->create_subscription( + emergency_control_cmd_sub_ = create_subscription( "input/emergency/control_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyCtrlCmd, this, _1)); - emergency_hazard_light_cmd_sub_ = this->create_subscription( + emergency_hazard_light_cmd_sub_ = create_subscription( "input/emergency/hazard_lights_cmd", 1, - std::bind(&VehicleCmdGate::onEmergencyHazardLightsCmd, this, _1)); + [this](HazardLightsCommand::ConstSharedPtr msg) { emergency_commands_.hazard_light = *msg; }); - emergency_gear_cmd_sub_ = this->create_subscription( - "input/emergency/gear_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyShiftCmd, this, _1)); + emergency_gear_cmd_sub_ = create_subscription( + "input/emergency/gear_cmd", 1, + [this](GearCommand::ConstSharedPtr msg) { emergency_commands_.gear = *msg; }); // Parameter - update_period_ = 1.0 / declare_parameter("update_rate"); use_emergency_handling_ = declare_parameter("use_emergency_handling"); check_external_emergency_heartbeat_ = declare_parameter("check_external_emergency_heartbeat"); @@ -169,15 +170,15 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) current_operation_mode_.mode = OperationModeState::STOP; // Service - srv_engage_ = create_service( + srv_engage_ = create_service( "~/service/engage", std::bind(&VehicleCmdGate::onEngageService, this, _1, _2)); - srv_external_emergency_ = create_service( + srv_external_emergency_ = create_service( "~/service/external_emergency", std::bind(&VehicleCmdGate::onExternalEmergencyStopService, this, _1, _2, _3)); - srv_external_emergency_stop_ = this->create_service( + srv_external_emergency_stop_ = create_service( "~/service/external_emergency_stop", std::bind(&VehicleCmdGate::onSetExternalEmergencyStopService, this, _1, _2, _3)); - srv_clear_external_emergency_stop_ = this->create_service( + srv_clear_external_emergency_stop_ = create_service( "~/service/clear_external_emergency_stop", std::bind(&VehicleCmdGate::onClearExternalEmergencyStopService, this, _1, _2, _3)); @@ -192,8 +193,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) pause_ = std::make_unique(this); // Timer + const auto update_period = 1.0 / declare_parameter("update_rate"); const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(update_period_)); + std::chrono::duration(update_period)); timer_ = rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&VehicleCmdGate::onTimer, this)); timer_pub_status_ = rclcpp::create_timer( @@ -246,20 +248,6 @@ void VehicleCmdGate::onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) } } -void VehicleCmdGate::onAutoTurnIndicatorsCmd(TurnIndicatorsCommand::ConstSharedPtr msg) -{ - auto_commands_.turn_indicator = *msg; -} - -void VehicleCmdGate::onAutoHazardLightsCmd(HazardLightsCommand::ConstSharedPtr msg) -{ - auto_commands_.hazard_light = *msg; -} - -void VehicleCmdGate::onAutoShiftCmd(GearCommand::ConstSharedPtr msg) { auto_commands_.gear = *msg; } - -void VehicleCmdGate::onGearStatus(GearReport::ConstSharedPtr msg) { current_gear_ptr_ = msg; } - // for remote void VehicleCmdGate::onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) { @@ -270,21 +258,6 @@ void VehicleCmdGate::onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg } } -void VehicleCmdGate::onRemoteTurnIndicatorsCmd(TurnIndicatorsCommand::ConstSharedPtr msg) -{ - remote_commands_.turn_indicator = *msg; -} - -void VehicleCmdGate::onRemoteHazardLightsCmd(HazardLightsCommand::ConstSharedPtr msg) -{ - remote_commands_.hazard_light = *msg; -} - -void VehicleCmdGate::onRemoteShiftCmd(GearCommand::ConstSharedPtr msg) -{ - remote_commands_.gear = *msg; -} - // for emergency void VehicleCmdGate::onEmergencyCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) { @@ -294,14 +267,6 @@ void VehicleCmdGate::onEmergencyCtrlCmd(AckermannControlCommand::ConstSharedPtr publishControlCommands(emergency_commands_); } } -void VehicleCmdGate::onEmergencyHazardLightsCmd(HazardLightsCommand::ConstSharedPtr msg) -{ - emergency_commands_.hazard_light = *msg; -} -void VehicleCmdGate::onEmergencyShiftCmd(GearCommand::ConstSharedPtr msg) -{ - emergency_commands_.gear = *msg; -} void VehicleCmdGate::onTimer() { @@ -592,8 +557,7 @@ void VehicleCmdGate::onGateMode(GateMode::ConstSharedPtr msg) void VehicleCmdGate::onEngage(EngageMsg::ConstSharedPtr msg) { is_engaged_ = msg->engage; } void VehicleCmdGate::onEngageService( - const tier4_external_api_msgs::srv::Engage::Request::SharedPtr request, - const tier4_external_api_msgs::srv::Engage::Response::SharedPtr response) + const EngageSrv::Request::SharedPtr request, const EngageSrv::Response::SharedPtr response) { is_engaged_ = request->engage; response->status = tier4_api_utils::response_success(); @@ -629,11 +593,10 @@ double VehicleCmdGate::getDt() void VehicleCmdGate::onExternalEmergencyStopService( const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) + const SetEmergency::Request::SharedPtr request, const SetEmergency::Response::SharedPtr response) { - auto req = std::make_shared(); - auto res = std::make_shared(); + auto req = std::make_shared(); + auto res = std::make_shared(); if (request->emergency) { onSetExternalEmergencyStopService(request_header, req, res); } else { @@ -649,8 +612,7 @@ void VehicleCmdGate::onExternalEmergencyStopService( bool VehicleCmdGate::onSetExternalEmergencyStopService( [[maybe_unused]] const std::shared_ptr req_header, - [[maybe_unused]] const std::shared_ptr req, - const std::shared_ptr res) + [[maybe_unused]] const Trigger::Request::SharedPtr req, const Trigger::Response::SharedPtr res) { is_external_emergency_stop_ = true; res->success = true; @@ -661,8 +623,7 @@ bool VehicleCmdGate::onSetExternalEmergencyStopService( bool VehicleCmdGate::onClearExternalEmergencyStopService( [[maybe_unused]] const std::shared_ptr req_header, - [[maybe_unused]] const std::shared_ptr req, - const std::shared_ptr res) + [[maybe_unused]] const Trigger::Request::SharedPtr req, const Trigger::Response::SharedPtr res) { if (is_external_emergency_stop_) { if (!is_external_emergency_stop_heartbeat_timeout_) { diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 6358a039bbbfb..9b6f45353942f 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -55,6 +55,7 @@ using autoware_auto_vehicle_msgs::msg::GearReport; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::SteeringReport; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using std_srvs::srv::Trigger; using tier4_control_msgs::msg::GateMode; using tier4_external_api_msgs::msg::Emergency; using tier4_external_api_msgs::msg::Heartbeat; @@ -108,7 +109,6 @@ class VehicleCmdGate : public rclcpp::Node void onExternalEmergencyStopHeartbeat(Heartbeat::ConstSharedPtr msg); void onSteering(SteeringReport::ConstSharedPtr msg); void onMrmState(MrmState::ConstSharedPtr msg); - void onGearStatus(GearReport::ConstSharedPtr msg); bool is_engaged_; bool is_system_emergency_ = false; @@ -137,9 +137,6 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Subscription::SharedPtr auto_hazard_light_cmd_sub_; rclcpp::Subscription::SharedPtr auto_gear_cmd_sub_; void onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); - void onAutoTurnIndicatorsCmd(TurnIndicatorsCommand::ConstSharedPtr msg); - void onAutoHazardLightsCmd(HazardLightsCommand::ConstSharedPtr msg); - void onAutoShiftCmd(GearCommand::ConstSharedPtr msg); // Subscription for external Commands remote_commands_; @@ -148,9 +145,6 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Subscription::SharedPtr remote_hazard_light_cmd_sub_; rclcpp::Subscription::SharedPtr remote_gear_cmd_sub_; void onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); - void onRemoteTurnIndicatorsCmd(TurnIndicatorsCommand::ConstSharedPtr msg); - void onRemoteHazardLightsCmd(HazardLightsCommand::ConstSharedPtr msg); - void onRemoteShiftCmd(GearCommand::ConstSharedPtr msg); // Subscription for emergency Commands emergency_commands_; @@ -158,12 +152,8 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Subscription::SharedPtr emergency_hazard_light_cmd_sub_; rclcpp::Subscription::SharedPtr emergency_gear_cmd_sub_; void onEmergencyCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); - void onEmergencyTurnIndicatorsCmd(TurnIndicatorsCommand::ConstSharedPtr msg); - void onEmergencyHazardLightsCmd(HazardLightsCommand::ConstSharedPtr msg); - void onEmergencyShiftCmd(GearCommand::ConstSharedPtr msg); // Parameter - double update_period_; bool use_emergency_handling_; bool check_external_emergency_heartbeat_; double system_emergency_heartbeat_timeout_; @@ -172,30 +162,27 @@ class VehicleCmdGate : public rclcpp::Node double emergency_acceleration_; // Service - rclcpp::Service::SharedPtr srv_engage_; - rclcpp::Service::SharedPtr srv_external_emergency_; + rclcpp::Service::SharedPtr srv_engage_; + rclcpp::Service::SharedPtr srv_external_emergency_; rclcpp::Publisher::SharedPtr pub_external_emergency_; void onEngageService( - const tier4_external_api_msgs::srv::Engage::Request::SharedPtr request, - const tier4_external_api_msgs::srv::Engage::Response::SharedPtr response); + const EngageSrv::Request::SharedPtr request, const EngageSrv::Response::SharedPtr response); void onExternalEmergencyStopService( const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response); + const SetEmergency::Request::SharedPtr request, + const SetEmergency::Response::SharedPtr response); // TODO(Takagi, Isamu): deprecated rclcpp::Subscription::SharedPtr engage_sub_; - rclcpp::Service::SharedPtr srv_external_emergency_stop_; - rclcpp::Service::SharedPtr srv_clear_external_emergency_stop_; + rclcpp::Service::SharedPtr srv_external_emergency_stop_; + rclcpp::Service::SharedPtr srv_clear_external_emergency_stop_; void onEngage(EngageMsg::ConstSharedPtr msg); bool onSetExternalEmergencyStopService( - const std::shared_ptr req_header, - const std::shared_ptr req, - const std::shared_ptr res); + const std::shared_ptr req_header, const Trigger::Request::SharedPtr req, + const Trigger::Response::SharedPtr res); bool onClearExternalEmergencyStopService( - const std::shared_ptr req_header, - const std::shared_ptr req, - const std::shared_ptr res); + const std::shared_ptr req_header, const Trigger::Request::SharedPtr req, + const Trigger::Response::SharedPtr res); // Timer / Event rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp index 47c9bde90b8b2..5fbfec237b047 100644 --- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -29,7 +29,8 @@ using autoware_auto_control_msgs::msg::AckermannControlCommand; constexpr double NOMINAL_INTERVAL = 1.0; void setFilterParams( - VehicleCmdFilter & f, double v, double a, double j, double lat_a, double lat_j, double wheelbase) + vehicle_cmd_gate::VehicleCmdFilter & f, double v, double a, double j, double lat_a, double lat_j, + double wheelbase) { f.setVelLim(v); f.setLonAccLim(a); @@ -62,7 +63,7 @@ void test_all( const double WHEELBASE = 3.0; const double DT = 0.1; // [s] - VehicleCmdFilter filter; + vehicle_cmd_gate::VehicleCmdFilter filter; setFilterParams(filter, V_LIM, A_LIM, J_LIM, LAT_A_LIM, LAT_J_LIM, WHEELBASE); filter.setPrevCmd(prev_cmd); From a9c7dc2cec6a321d1d243b154db63577d33503b1 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 23 Jan 2023 15:55:11 +0900 Subject: [PATCH 10/60] feat(behavior_path_planner): external request lane change (#2442) * feature(behavior_path_planner): add external request lane change module Signed-off-by: Fumiya Watanabe feature(behavior_path_planner): fix for RTC Signed-off-by: Fumiya Watanabe feature(behavior_path_planner): fix decision logic Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): fix behavior_path_planner_tree.xml Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): fix for rebase Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): output multiple candidate paths Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): get path candidate in behavior tree manager Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): fix for multiple candidate path Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): separate external request lane change module Signed-off-by: Fumiya Watanabe feature(behavior_path_planner): add create publisher method Signed-off-by: Fumiya Watanabe feature(behavior_path_planner): move publishers to node Signed-off-by: Fumiya Watanabe feature(behavior_path_planner): remove unnecessary publisher Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): move reset path candidate function to behavior tree manager Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): add external request lane change path candidate publisher Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): apply abort lane change Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): remove unnecessary change Signed-off-by: Fumiya Watanabe * feat(behavior_path_planner): fix getLaneChangePaths() Signed-off-by: Fumiya Watanabe * feat(behavior_path_planner): disable external request lane change in default tree Signed-off-by: Fumiya Watanabe * Update rtc_auto_mode_manager.param.yaml * fix(route_handler): remove redundant code * fix(behavior_path_planner): fix for turn signal Signed-off-by: Fumiya Watanabe Signed-off-by: Fumiya Watanabe --- .../src/rtc_manager_panel.cpp | 12 +- .../rtc_auto_mode_manager.param.yaml | 31 + planning/behavior_path_planner/CMakeLists.txt | 1 + .../config/behavior_path_planner_tree.xml | 50 +- ..._planner_tree_with_external_request_LC.xml | 74 ++ .../behavior_path_planner_node.hpp | 1 + .../external_request_lane_change_module.hpp | 158 ++++ .../scene_module/lane_change/util.hpp | 10 +- .../scene_module/scene_module_visitor.hpp | 3 + .../src/behavior_path_planner_node.cpp | 16 + .../external_request_lane_change_module.cpp | 796 ++++++++++++++++++ .../lane_change/lane_change_module.cpp | 8 +- .../src/scene_module/lane_change/util.cpp | 39 +- .../include/route_handler/route_handler.hpp | 4 + planning/route_handler/src/route_handler.cpp | 40 + .../config/rtc_auto_mode_manager.param.yaml | 4 + .../src/rtc_auto_mode_manager_interface.cpp | 4 + planning/rtc_interface/src/rtc_interface.cpp | 4 + 18 files changed, 1193 insertions(+), 62 deletions(-) create mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml create mode 100644 planning/behavior_path_planner/config/behavior_path_planner_tree_with_external_request_LC.xml create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp create mode 100644 planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp index 398321bce7eb2..8647d87558cb0 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -39,6 +39,12 @@ std::string getModuleName(const uint8_t module_type) case Module::LANE_CHANGE_RIGHT: { return "lane_change_right"; } + case Module::EXT_REQUEST_LANE_CHANGE_LEFT: { + return "ext_request_lane_change_left"; + } + case Module::EXT_REQUEST_LANE_CHANGE_RIGHT: { + return "ext_request_lane_change_right"; + } case Module::AVOIDANCE_LEFT: { return "avoidance_left"; } @@ -80,8 +86,10 @@ bool isPathChangeModule(const uint8_t module_type) { if ( module_type == Module::LANE_CHANGE_LEFT || module_type == Module::LANE_CHANGE_RIGHT || - module_type == Module::AVOIDANCE_LEFT || module_type == Module::AVOIDANCE_RIGHT || - module_type == Module::PULL_OVER || module_type == Module::PULL_OUT) { + module_type == Module::EXT_REQUEST_LANE_CHANGE_LEFT || + module_type == Module::EXT_REQUEST_LANE_CHANGE_RIGHT || module_type == Module::AVOIDANCE_LEFT || + module_type == Module::AVOIDANCE_RIGHT || module_type == Module::PULL_OVER || + module_type == Module::PULL_OUT) { return true; } return false; diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml new file mode 100644 index 0000000000000..0e3391e69eb11 --- /dev/null +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml @@ -0,0 +1,31 @@ +/**: + ros__parameters: + module_list: + - "blind_spot" + - "crosswalk" + - "detection_area" + - "intersection" + - "no_stopping_area" + - "traffic_light" + - "ext_request_lane_change_left" + - "ext_request_lane_change_right" + - "lane_change_left" + - "lane_change_right" + - "avoidance_left" + - "avoidance_right" + - "pull_over" + - "pull_out" + + default_enable_list: + - "blind_spot" + - "crosswalk" + - "detection_area" + - "intersection" + - "no_stopping_area" + - "traffic_light" + - "lane_change_left" + - "lane_change_right" + - "avoidance_left" + - "avoidance_right" + - "pull_over" + - "pull_out" diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index e00f7ade345b5..0204581ff649c 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -21,6 +21,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/scene_module/avoidance/avoidance_utils.cpp src/scene_module/avoidance/debug.cpp src/scene_module/lane_following/lane_following_module.cpp + src/scene_module/lane_change/external_request_lane_change_module.cpp src/scene_module/lane_change/lane_change_module.cpp src/scene_module/lane_change/util.cpp src/scene_module/lane_change/debug.cpp diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree.xml index 748b70b33f181..017f052ba2d19 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree.xml @@ -15,6 +15,14 @@ + @@ -27,48 +35,27 @@ - - - - - - - - - - - - - - - - - - - desc - - desc - - - + + desc + + + desc - + + desc desc - - desc - @@ -78,17 +65,10 @@ desc - desc - - desc - - - - diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree_with_external_request_LC.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree_with_external_request_LC.xml new file mode 100644 index 0000000000000..e848f156a4e1a --- /dev/null +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree_with_external_request_LC.xml @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + desc + + + + + desc + + + + desc + + + + desc + + + + desc + + + + + + + + desc + + + + desc + + + + + diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index b1a951c7fbaff..d785fc0186b99 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/behavior_tree_manager.hpp" #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp" #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp new file mode 100644 index 0000000000000..e564d1c8a1e83 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp @@ -0,0 +1,158 @@ +// Copyright 2022 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. + +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_LANE_CHANGE_MODULE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_LANE_CHANGE_MODULE_HPP_ + +#include "behavior_path_planner/scene_module/lane_change/debug.hpp" +#include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp" +#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" +#include "behavior_path_planner/scene_module/scene_module_interface.hpp" +#include "behavior_path_planner/turn_signal_decider.hpp" + +#include + +#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" +#include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp" +#include "tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp" +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace behavior_path_planner +{ +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using marker_utils::CollisionCheckDebug; +using tier4_planning_msgs::msg::LaneChangeDebugMsg; +using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; + +class ExternalRequestLaneChangeModule : public SceneModuleInterface +{ +public: + enum class Direction { + RIGHT = 0, + LEFT, + }; + + ExternalRequestLaneChangeModule( + const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, + const Direction & direction); + + bool isExecutionRequested() const override; + bool isExecutionReady() const override; + BT::NodeStatus updateState() override; + BehaviorModuleOutput plan() override; + BehaviorModuleOutput planWaitingApproval() override; + CandidateOutput planCandidate() const override; + void onEntry() override; + void onExit() override; + + std::shared_ptr get_debug_msg_array() const; + void acceptVisitor( + [[maybe_unused]] const std::shared_ptr & visitor) const override; + +protected: + std::shared_ptr parameters_; + LaneChangeStatus status_; + PathShifter path_shifter_; + mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; + LaneChangeStates current_lane_change_state_; + std::shared_ptr abort_path_; + PathWithLaneId prev_approved_path_; + + Direction direction_; + + double lane_change_lane_length_{200.0}; + double check_distance_{100.0}; + bool is_abort_path_approved_{false}; + bool is_abort_approval_requested_{false}; + bool is_abort_condition_satisfied_{false}; + bool is_activated_ = false; + + void resetParameters(); + void resetPathIfAbort(); + + void waitApproval(const double start_distance, const double finish_distance) + { + updateRTCStatus(start_distance, finish_distance); + is_waiting_approval_ = true; + } + + lanelet::ConstLanelets get_original_lanes() const; + PathWithLaneId getReferencePath() const; + lanelet::ConstLanelets getLaneChangeLanes( + const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const; + std::pair getSafePath( + const lanelet::ConstLanelets & lane_change_lanes, const double check_distance, + LaneChangePath & safe_path) const; + + void updateLaneChangeStatus(); + void generateExtendedDrivableArea(PathWithLaneId & path); + void updateOutputTurnSignal(BehaviorModuleOutput & output); + void updateSteeringFactorPtr(const BehaviorModuleOutput & output); + void updateSteeringFactorPtr( + const CandidateOutput & output, const LaneChangePath & selected_path) const; + void calcTurnSignalInfo(); + + bool isSafe() const; + bool isValidPath(const PathWithLaneId & path) const; + bool isApprovedPathSafe(Pose & ego_pose_before_collision) const; + bool isNearEndOfLane() const; + bool isCurrentSpeedLow() const; + bool isAbortConditionSatisfied(); + bool hasFinishedLaneChange() const; + bool isAbortState() const; + + // getter + Pose getEgoPose() const; + Twist getEgoTwist() const; + std_msgs::msg::Header getRouteHeader() const; + + // debug + mutable std::unordered_map object_debug_; + mutable std::vector debug_valid_path_; + + void setObjectDebugVisualization() const; +}; + +class ExternalRequestLaneChangeRightModule : public ExternalRequestLaneChangeModule +{ +public: + ExternalRequestLaneChangeRightModule( + const std::string & name, rclcpp::Node & node, + std::shared_ptr parameters); +}; + +class ExternalRequestLaneChangeLeftModule : public ExternalRequestLaneChangeModule +{ +public: + ExternalRequestLaneChangeLeftModule( + const std::string & name, rclcpp::Node & node, + std::shared_ptr parameters); +}; + +} // namespace behavior_path_planner +// clang-format off +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_LANE_CHANGE_MODULE_HPP_ // NOLINT +// clang-format on diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index d91a9373bb492..dfc5708f10480 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -77,9 +77,8 @@ LaneChangePaths getLaneChangePaths( LaneChangePaths selectValidPaths( const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, - const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose); + const lanelet::ConstLanelets & target_lanes, const RouteHandler & route_handler, + const Pose & current_pose, const Pose & goal_pose, const double minimum_lane_change_length); bool selectSafePath( const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, @@ -103,9 +102,8 @@ bool isLaneChangePathSafe( bool hasEnoughDistance( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose, - const lanelet::routing::RoutingGraphContainer & overall_graphs); + const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const Pose & goal_pose, + const RouteHandler & route_handler, const double minimum_lane_change_length); ShiftLine getLaneChangeShiftLine( const PathWithLaneId & path1, const PathWithLaneId & path2, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp index 9d1d03e2fbc9d..ee7789c4040af 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp @@ -28,6 +28,7 @@ namespace behavior_path_planner // Forward Declaration class AvoidanceModule; class LaneChangeModule; +class ExternalRequestLaneChangeModule; class LaneFollowingModule; class PullOutModule; class PullOverModule; @@ -42,6 +43,7 @@ class SceneModuleVisitor { public: void visitLaneChangeModule(const LaneChangeModule * module) const; + void visitExternalRequestLaneChangeModule(const ExternalRequestLaneChangeModule * module) const; void visitAvoidanceModule(const AvoidanceModule * module) const; std::shared_ptr getAvoidanceModuleDebugMsg() const; @@ -49,6 +51,7 @@ class SceneModuleVisitor protected: mutable std::shared_ptr lane_change_visitor_; + mutable std::shared_ptr ext_request_lane_change_visitor_; mutable std::shared_ptr avoidance_visitor_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index b5e40f06d7ad6..2114750679d16 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -134,6 +134,22 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod std::make_shared("LaneFollowing", *this, getLaneFollowingParam()); bt_manager_->registerSceneModule(lane_following_module); + auto ext_request_lane_change_right_module = + std::make_shared( + "ExternalRequestLaneChangeRight", *this, lane_change_param_ptr); + path_candidate_publishers_.emplace( + "ExternalRequestLaneChangeRight", + create_publisher(path_candidate_name_space + "ext_request_lane_change_right", 1)); + bt_manager_->registerSceneModule(ext_request_lane_change_right_module); + + auto ext_request_lane_change_left_module = + std::make_shared( + "ExternalRequestLaneChangeLeft", *this, lane_change_param_ptr); + path_candidate_publishers_.emplace( + "ExternalRequestLaneChangeLeft", + create_publisher(path_candidate_name_space + "ext_request_lane_change_left", 1)); + bt_manager_->registerSceneModule(ext_request_lane_change_left_module); + auto lane_change_module = std::make_shared("LaneChange", *this, lane_change_param_ptr); path_candidate_publishers_.emplace( diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp new file mode 100644 index 0000000000000..ec000759aa002 --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -0,0 +1,796 @@ +// Copyright 2022 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 "behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp" + +#include "behavior_path_planner/path_utilities.hpp" +#include "behavior_path_planner/scene_module/lane_change/util.hpp" +#include "behavior_path_planner/scene_module/scene_module_interface.hpp" +#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" +#include "behavior_path_planner/turn_signal_decider.hpp" +#include "behavior_path_planner/utilities.hpp" + +#include +#include +#include + +#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" +#include +#include + +#include +#include +#include +#include +#include +#include +namespace behavior_path_planner +{ +std::string getTopicName(const ExternalRequestLaneChangeModule::Direction & direction) +{ + const std::string direction_name = + direction == ExternalRequestLaneChangeModule::Direction::RIGHT ? "right" : "left"; + return "ext_request_lane_change_" + direction_name; +} + +ExternalRequestLaneChangeModule::ExternalRequestLaneChangeModule( + const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, + const Direction & direction) +: SceneModuleInterface{name, node}, parameters_{std::move(parameters)}, direction_{direction} +{ + rtc_interface_ptr_ = std::make_shared(&node, getTopicName(direction)); + steering_factor_interface_ptr_ = + std::make_unique(&node, getTopicName(direction)); +} + +void ExternalRequestLaneChangeModule::onEntry() +{ + RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onEntry"); + current_state_ = BT::NodeStatus::SUCCESS; + current_lane_change_state_ = LaneChangeStates::Normal; + updateLaneChangeStatus(); +} + +void ExternalRequestLaneChangeModule::onExit() +{ + resetParameters(); + current_state_ = BT::NodeStatus::SUCCESS; + RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onExit"); +} + +bool ExternalRequestLaneChangeModule::isExecutionRequested() const +{ + if (current_state_ == BT::NodeStatus::RUNNING) { + return true; + } + + const auto current_lanes = util::getCurrentLanes(planner_data_); + const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); + + LaneChangePath selected_path; + const auto [found_valid_path, found_safe_path] = + getSafePath(lane_change_lanes, check_distance_, selected_path); + + return found_valid_path; +} + +bool ExternalRequestLaneChangeModule::isExecutionReady() const +{ + if (current_state_ == BT::NodeStatus::RUNNING) { + return true; + } + + const auto current_lanes = util::getCurrentLanes(planner_data_); + const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); + + LaneChangePath selected_path; + const auto [found_valid_path, found_safe_path] = + getSafePath(lane_change_lanes, check_distance_, selected_path); + + return found_safe_path; +} + +BT::NodeStatus ExternalRequestLaneChangeModule::updateState() +{ + RCLCPP_DEBUG(getLogger(), "LANE_CHANGE updateState"); + if (!isSafe()) { + current_state_ = BT::NodeStatus::SUCCESS; + return current_state_; + } + + const auto is_within_current_lane = lane_change_utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters); + if (isAbortState() && !is_within_current_lane) { + current_state_ = BT::NodeStatus::RUNNING; + return current_state_; + } + + if (isAbortConditionSatisfied()) { + if (isNearEndOfLane() && isCurrentSpeedLow()) { + current_state_ = BT::NodeStatus::RUNNING; + return current_state_; + } + + current_state_ = BT::NodeStatus::FAILURE; + return current_state_; + } + + if (hasFinishedLaneChange()) { + current_state_ = BT::NodeStatus::SUCCESS; + return current_state_; + } + current_state_ = BT::NodeStatus::RUNNING; + return current_state_; +} + +BehaviorModuleOutput ExternalRequestLaneChangeModule::plan() +{ + resetPathCandidate(); + is_activated_ = isActivated(); + + auto path = status_.lane_change_path.path; + + if (!isValidPath(path)) { + status_.is_safe = false; + return BehaviorModuleOutput{}; + } + + if ((is_abort_condition_satisfied_ && isNearEndOfLane() && isCurrentSpeedLow())) { + const auto stop_point = util::insertStopPoint(0.1, &path); + } + + if (isAbortState()) { + resetPathIfAbort(); + if (is_activated_) { + path = abort_path_->path; + } + } + + generateExtendedDrivableArea(path); + + prev_approved_path_ = path; + + BehaviorModuleOutput output; + output.path = std::make_shared(path); + updateOutputTurnSignal(output); + + updateSteeringFactorPtr(output); + + return output; +} + +void ExternalRequestLaneChangeModule::resetPathIfAbort() +{ + if (!abort_path_) { + RCLCPP_WARN(getLogger(), "[abort] No abort path found!"); + } + + if (!is_abort_approval_requested_) { + RCLCPP_DEBUG(getLogger(), "[abort] uuid is reset to request abort approval."); + uuid_ = generateUUID(); + is_abort_approval_requested_ = true; + is_abort_path_approved_ = false; + return; + } + + if (isActivated()) { + RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is true. set is_abort_path_approved to true."); + is_abort_path_approved_ = true; + clearWaitingApproval(); + } else { + RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is False."); + is_abort_path_approved_ = false; + const double start_distance = motion_utils::calcSignedArcLength( + abort_path_->path.points, getEgoPose().position, abort_path_->shift_line.start.position); + const double finish_distance = motion_utils::calcSignedArcLength( + abort_path_->path.points, getEgoPose().position, abort_path_->shift_line.end.position); + waitApproval(start_distance, finish_distance); + } +} + +CandidateOutput ExternalRequestLaneChangeModule::planCandidate() const +{ + CandidateOutput output; + + // Get lane change lanes + const auto current_lanes = util::getCurrentLanes(planner_data_); + const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); + + LaneChangePath selected_path; + [[maybe_unused]] const auto [found_valid_path, found_safe_path] = + getSafePath(lane_change_lanes, check_distance_, selected_path); + selected_path.path.header = planner_data_->route_handler->getRouteHeader(); + + if (isAbortState()) { + selected_path = *abort_path_; + } + + if (selected_path.path.points.empty()) { + return output; + } + + output.path_candidate = selected_path.path; + output.lateral_shift = lane_change_utils::getLateralShift(selected_path); + output.start_distance_to_path_change = motion_utils::calcSignedArcLength( + selected_path.path.points, getEgoPose().position, selected_path.shift_line.start.position); + output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( + selected_path.path.points, getEgoPose().position, selected_path.shift_line.end.position); + + updateSteeringFactorPtr(output, selected_path); + return output; +} + +BehaviorModuleOutput ExternalRequestLaneChangeModule::planWaitingApproval() +{ + const auto is_within_current_lane = lane_change_utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters); + + if (is_within_current_lane) { + prev_approved_path_ = getReferencePath(); + } + + BehaviorModuleOutput out; + out.path = std::make_shared(getReferencePath()); + + const auto candidate = planCandidate(); + path_candidate_ = std::make_shared(candidate.path_candidate); + waitApproval(candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + is_abort_path_approved_ = false; + return out; +} + +void ExternalRequestLaneChangeModule::updateLaneChangeStatus() +{ + status_.current_lanes = util::getCurrentLanes(planner_data_); + status_.lane_change_lanes = getLaneChangeLanes(status_.current_lanes, lane_change_lane_length_); + + // Find lane change path + LaneChangePath selected_path; + [[maybe_unused]] const auto [found_valid_path, found_safe_path] = + getSafePath(status_.lane_change_lanes, check_distance_, selected_path); + + // Update status + status_.is_safe = found_safe_path; + status_.lane_change_path = selected_path; + status_.lane_follow_lane_ids = util::getIds(status_.current_lanes); + status_.lane_change_lane_ids = util::getIds(status_.lane_change_lanes); + + const auto arclength_start = + lanelet::utils::getArcCoordinates(status_.lane_change_lanes, getEgoPose()); + status_.start_distance = arclength_start.length; + status_.lane_change_path.path.header = getRouteHeader(); +} + +PathWithLaneId ExternalRequestLaneChangeModule::getReferencePath() const +{ + PathWithLaneId reference_path; + + const auto & route_handler = planner_data_->route_handler; + const auto current_pose = getEgoPose(); + const auto & common_parameters = planner_data_->parameters; + + // Set header + reference_path.header = getRouteHeader(); + + const auto current_lanes = util::getCurrentLanes(planner_data_); + + if (current_lanes.empty()) { + return reference_path; + } + + if (reference_path.points.empty()) { + reference_path = util::getCenterLinePath( + *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, + common_parameters.forward_path_length, common_parameters); + } + + const int num_lane_change = + std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back())); + double optional_lengths{0.0}; + const auto isInIntersection = util::checkLaneIsInIntersection( + *route_handler, reference_path, current_lanes, common_parameters, num_lane_change, + optional_lengths); + if (isInIntersection) { + reference_path = util::getCenterLinePath( + *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, + common_parameters.forward_path_length, common_parameters, optional_lengths); + } + + const double lane_change_buffer = + util::calcLaneChangeBuffer(common_parameters, num_lane_change, optional_lengths); + + reference_path = util::setDecelerationVelocity( + *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration, + lane_change_buffer); + + const auto drivable_lanes = util::generateDrivableLanes(current_lanes); + const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_->drivable_area_left_bound_offset, + parameters_->drivable_area_right_bound_offset); + util::generateDrivableArea( + reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + + return reference_path; +} + +lanelet::ConstLanelets ExternalRequestLaneChangeModule::getLaneChangeLanes( + const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const +{ + lanelet::ConstLanelets lane_change_lanes; + const auto & route_handler = planner_data_->route_handler; + const auto minimum_lane_change_length = planner_data_->parameters.minimum_lane_change_length; + const auto lane_change_prepare_duration = parameters_->lane_change_prepare_duration; + const auto current_pose = getEgoPose(); + const auto current_twist = getEgoTwist(); + + if (current_lanes.empty()) { + return lane_change_lanes; + } + + // Get lane change lanes + lanelet::ConstLanelet current_lane; + lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, ¤t_lane); + const double lane_change_prepare_length = + std::max(current_twist.linear.x * lane_change_prepare_duration, minimum_lane_change_length); + lanelet::ConstLanelets current_check_lanes = + route_handler->getLaneletSequence(current_lane, current_pose, 0.0, lane_change_prepare_length); + lanelet::ConstLanelet lane_change_lane; + + auto getLaneChangeTargetExceptPreferredLane = [this, &route_handler]( + const lanelet::ConstLanelets & lanelets, + lanelet::ConstLanelet * target_lanelet) { + return direction_ == Direction::RIGHT + ? route_handler->getRightLaneChangeTargetExceptPreferredLane(lanelets, target_lanelet) + : route_handler->getLeftLaneChangeTargetExceptPreferredLane(lanelets, target_lanelet); + }; + + if (getLaneChangeTargetExceptPreferredLane(current_check_lanes, &lane_change_lane)) { + lane_change_lanes = route_handler->getLaneletSequence( + lane_change_lane, current_pose, lane_change_lane_length, lane_change_lane_length); + } else { + lane_change_lanes.clear(); + } + + return lane_change_lanes; +} + +std::pair ExternalRequestLaneChangeModule::getSafePath( + const lanelet::ConstLanelets & lane_change_lanes, const double check_distance, + LaneChangePath & safe_path) const +{ + const auto & route_handler = planner_data_->route_handler; + const auto current_pose = getEgoPose(); + const auto current_twist = getEgoTwist(); + const auto & common_parameters = planner_data_->parameters; + + const auto current_lanes = util::getCurrentLanes(planner_data_); + + if (!lane_change_lanes.empty()) { + // find candidate paths + const auto lane_change_paths = lane_change_utils::getLaneChangePaths( + *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist, + common_parameters, *parameters_); + + // get lanes used for detection + lanelet::ConstLanelets check_lanes; + if (!lane_change_paths.empty()) { + const auto & longest_path = lane_change_paths.front(); + // we want to see check_distance [m] behind vehicle so add lane changing length + const double check_distance_with_path = + check_distance + longest_path.preparation_length + longest_path.lane_change_length; + check_lanes = route_handler->getCheckTargetLanesFromPath( + longest_path.path, lane_change_lanes, check_distance_with_path); + } + + // select valid path + const LaneChangePaths valid_paths = lane_change_utils::selectValidPaths( + lane_change_paths, current_lanes, check_lanes, *route_handler, current_pose, + route_handler->getGoalPose(), + common_parameters.minimum_lane_change_length + + common_parameters.backward_length_buffer_for_end_of_lane + + parameters_->lane_change_finish_judge_buffer); + + if (valid_paths.empty()) { + return std::make_pair(false, false); + } + debug_valid_path_ = valid_paths; + + // select safe path + const bool found_safe_path = lane_change_utils::selectSafePath( + valid_paths, current_lanes, check_lanes, planner_data_->dynamic_object, current_pose, + current_twist, common_parameters, *parameters_, &safe_path, object_debug_); + + if (parameters_->publish_debug_marker) { + setObjectDebugVisualization(); + } else { + debug_marker_.markers.clear(); + } + + return std::make_pair(true, found_safe_path); + } + + return std::make_pair(false, false); +} + +bool ExternalRequestLaneChangeModule::isSafe() const { return status_.is_safe; } + +bool ExternalRequestLaneChangeModule::isValidPath(const PathWithLaneId & path) const +{ + const auto & route_handler = planner_data_->route_handler; + + // check lane departure + const auto drivable_lanes = lane_change_utils::generateDrivableLanes( + *route_handler, util::extendLanes(route_handler, status_.current_lanes), + util::extendLanes(route_handler, status_.lane_change_lanes)); + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_->drivable_area_left_bound_offset, + parameters_->drivable_area_right_bound_offset); + const auto lanelets = util::transformToLanelets(expanded_lanes); + + // check path points are in any lanelets + for (const auto & point : path.points) { + bool is_in_lanelet = false; + for (const auto & lanelet : lanelets) { + if (lanelet::utils::isInLanelet(point.point.pose, lanelet)) { + is_in_lanelet = true; + break; + } + } + if (!is_in_lanelet) { + RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes"); + return false; + } + } + + // check relative angle + if (!util::checkPathRelativeAngle(path, M_PI)) { + RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path relative angle is invalid"); + return false; + } + + return true; +} + +bool ExternalRequestLaneChangeModule::isNearEndOfLane() const +{ + const auto & current_pose = getEgoPose(); + const auto minimum_lane_change_length = planner_data_->parameters.minimum_lane_change_length; + const auto end_of_lane_buffer = planner_data_->parameters.backward_length_buffer_for_end_of_lane; + const double threshold = end_of_lane_buffer + minimum_lane_change_length; + + return std::max(0.0, util::getDistanceToEndOfLane(current_pose, status_.current_lanes)) < + threshold; +} + +bool ExternalRequestLaneChangeModule::isCurrentSpeedLow() const +{ + constexpr double threshold_ms = 10.0 * 1000 / 3600; + return util::l2Norm(getEgoTwist().linear) < threshold_ms; +} + +bool ExternalRequestLaneChangeModule::isAbortConditionSatisfied() +{ + is_abort_condition_satisfied_ = false; + current_lane_change_state_ = LaneChangeStates::Normal; + + if (!parameters_->enable_cancel_lane_change) { + return false; + } + + if (!is_activated_) { + return false; + } + + Pose ego_pose_before_collision; + const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); + + if (!is_path_safe) { + const auto & common_parameters = planner_data_->parameters; + const bool is_within_original_lane = lane_change_utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), common_parameters); + + if (is_within_original_lane) { + current_lane_change_state_ = LaneChangeStates::Cancel; + return true; + } + + // check abort enable flag + RCLCPP_WARN_STREAM_THROTTLE( + getLogger(), *clock_, 1000, + "DANGER!!! Path is not safe anymore, but it is too late to CANCEL! Please be cautious"); + + if (!parameters_->enable_abort_lane_change) { + current_lane_change_state_ = LaneChangeStates::Stop; + return false; + } + + const auto found_abort_path = lane_change_utils::getAbortPaths( + planner_data_, status_.lane_change_path, ego_pose_before_collision, common_parameters, + *parameters_); + + if (!found_abort_path && !is_abort_path_approved_) { + current_lane_change_state_ = LaneChangeStates::Stop; + return true; + } + + current_lane_change_state_ = LaneChangeStates::Abort; + + if (!is_abort_path_approved_) { + abort_path_ = std::make_shared(*found_abort_path); + } + + return true; + } + + return false; +} + +bool ExternalRequestLaneChangeModule::isAbortState() const +{ + if (!parameters_->enable_abort_lane_change) { + return false; + } + + if (current_lane_change_state_ != LaneChangeStates::Abort) { + return false; + } + + if (!abort_path_) { + return false; + } + + RCLCPP_WARN_STREAM_THROTTLE( + getLogger(), *clock_, 1000, + "DANGER!!! Lane change transition to ABORT state, return path will be computed!"); + return true; +} + +bool ExternalRequestLaneChangeModule::hasFinishedLaneChange() const +{ + const auto & current_pose = getEgoPose(); + const auto arclength_current = + lanelet::utils::getArcCoordinates(status_.lane_change_lanes, current_pose); + const double travel_distance = arclength_current.length - status_.start_distance; + const double finish_distance = status_.lane_change_path.preparation_length + + status_.lane_change_path.lane_change_length + + parameters_->lane_change_finish_judge_buffer; + return travel_distance > finish_distance; +} + +void ExternalRequestLaneChangeModule::setObjectDebugVisualization() const +{ + using marker_utils::lane_change_markers::showAllValidLaneChangePath; + using marker_utils::lane_change_markers::showLerpedPose; + using marker_utils::lane_change_markers::showObjectInfo; + using marker_utils::lane_change_markers::showPolygon; + using marker_utils::lane_change_markers::showPolygonPose; + + debug_marker_.markers.clear(); + const auto add = [this](const MarkerArray & added) { + tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); + }; + + add(showObjectInfo(object_debug_, "object_debug_info")); + add(showLerpedPose(object_debug_, "lerp_pose_before_true")); + add(showPolygonPose(object_debug_, "expected_pose")); + add(showPolygon(object_debug_, "lerped_polygon")); + add(showAllValidLaneChangePath(debug_valid_path_, "lane_change_valid_paths")); +} + +std::shared_ptr ExternalRequestLaneChangeModule::get_debug_msg_array() + const +{ + LaneChangeDebugMsgArray debug_msg_array; + debug_msg_array.lane_change_info.reserve(object_debug_.size()); + for (const auto & [uuid, debug_data] : object_debug_) { + LaneChangeDebugMsg debug_msg; + debug_msg.object_id = uuid; + debug_msg.allow_lane_change = debug_data.allow_lane_change; + debug_msg.is_front = debug_data.is_front; + debug_msg.relative_distance = debug_data.relative_to_ego; + debug_msg.failed_reason = debug_data.failed_reason; + debug_msg.velocity = util::l2Norm(debug_data.object_twist.linear); + debug_msg_array.lane_change_info.push_back(debug_msg); + } + lane_change_debug_msg_array_ = debug_msg_array; + + lane_change_debug_msg_array_.header.stamp = clock_->now(); + return std::make_shared(lane_change_debug_msg_array_); +} + +void ExternalRequestLaneChangeModule::updateSteeringFactorPtr(const BehaviorModuleOutput & output) +{ + const auto current_pose = getEgoPose(); + const double start_distance = motion_utils::calcSignedArcLength( + output.path->points, current_pose.position, status_.lane_change_path.shift_line.start.position); + const double finish_distance = motion_utils::calcSignedArcLength( + output.path->points, current_pose.position, status_.lane_change_path.shift_line.end.position); + + waitApproval(start_distance, finish_distance); + uint16_t steering_factor_direction; + if (direction_ == Direction::RIGHT) { + steering_factor_direction = SteeringFactor::RIGHT; + } else if (direction_ == Direction::LEFT) { + steering_factor_direction = SteeringFactor::LEFT; + } else { + steering_factor_direction = SteeringFactor::UNKNOWN; + } + + // TODO(tkhmy) add handle status TRYING + steering_factor_interface_ptr_->updateSteeringFactor( + {status_.lane_change_path.shift_line.start, status_.lane_change_path.shift_line.end}, + {start_distance, finish_distance}, SteeringFactor::LANE_CHANGE, steering_factor_direction, + SteeringFactor::TURNING, ""); +} + +void ExternalRequestLaneChangeModule::updateSteeringFactorPtr( + const CandidateOutput & output, const LaneChangePath & selected_path) const +{ + const uint16_t steering_factor_direction = std::invoke([&output]() { + if (output.lateral_shift > 0.0) { + return SteeringFactor::LEFT; + } + return SteeringFactor::RIGHT; + }); + + steering_factor_interface_ptr_->updateSteeringFactor( + {selected_path.shift_line.start, selected_path.shift_line.end}, + {output.start_distance_to_path_change, output.finish_distance_to_path_change}, + SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); +} +Pose ExternalRequestLaneChangeModule::getEgoPose() const { return planner_data_->self_pose->pose; } +Twist ExternalRequestLaneChangeModule::getEgoTwist() const +{ + return planner_data_->self_odometry->twist.twist; +} +std_msgs::msg::Header ExternalRequestLaneChangeModule::getRouteHeader() const +{ + return planner_data_->route_handler->getRouteHeader(); +} +void ExternalRequestLaneChangeModule::generateExtendedDrivableArea(PathWithLaneId & path) +{ + const auto & common_parameters = planner_data_->parameters; + const auto & route_handler = planner_data_->route_handler; + const auto drivable_lanes = lane_change_utils::generateDrivableLanes( + *route_handler, status_.current_lanes, status_.lane_change_lanes); + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_->drivable_area_left_bound_offset, + parameters_->drivable_area_right_bound_offset); + + util::generateDrivableArea(path, expanded_lanes, common_parameters.vehicle_length, planner_data_); +} + +bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) const +{ + const auto current_pose = getEgoPose(); + const auto current_twist = getEgoTwist(); + const auto & dynamic_objects = planner_data_->dynamic_object; + const auto & current_lanes = status_.current_lanes; + const auto & common_parameters = planner_data_->parameters; + const auto & route_handler = planner_data_->route_handler; + const auto path = status_.lane_change_path; + + constexpr double check_distance = 100.0; + // get lanes used for detection + const double check_distance_with_path = + check_distance + path.preparation_length + path.lane_change_length; + const auto check_lanes = route_handler->getCheckTargetLanesFromPath( + path.path, status_.lane_change_lanes, check_distance_with_path); + + std::unordered_map debug_data; + + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); + return lane_change_utils::isLaneChangePathSafe( + path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, + current_twist, common_parameters, *parameters_, + common_parameters.expected_front_deceleration_for_abort, + common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, + false, status_.lane_change_path.acceleration); +} + +void ExternalRequestLaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) +{ + calcTurnSignalInfo(); + const auto turn_signal_info = util::getPathTurnSignal( + status_.current_lanes, status_.lane_change_path.shifted_path, + status_.lane_change_path.shift_line, getEgoPose(), getEgoTwist().linear.x, + planner_data_->parameters); + output.turn_signal_info.turn_signal.command = turn_signal_info.first.command; + + lane_change_utils::get_turn_signal_info(status_.lane_change_path, &output.turn_signal_info); +} + +void ExternalRequestLaneChangeModule::calcTurnSignalInfo() +{ + const auto get_blinker_pose = + [this](const PathWithLaneId & path, const lanelet::ConstLanelets & lanes, const double length) { + const auto & points = path.points; + const auto arc_front = lanelet::utils::getArcCoordinates(lanes, points.front().point.pose); + for (const auto & point : points) { + const auto & pt = point.point.pose; + const auto arc_current = lanelet::utils::getArcCoordinates(lanes, pt); + const auto diff = arc_current.length - arc_front.length; + if (diff > length) { + return pt; + } + } + + RCLCPP_WARN(getLogger(), "unable to determine blinker pose..."); + return points.front().point.pose; + }; + + const auto & path = status_.lane_change_path; + TurnSignalInfo turn_signal_info{}; + + turn_signal_info.desired_start_point = std::invoke([&]() { + const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time; + const auto prepare_duration = parameters_->lane_change_prepare_duration; + const auto prepare_to_blinker_start_diff = prepare_duration - blinker_start_duration; + if (prepare_to_blinker_start_diff < 1e-5) { + return path.path.points.front().point.pose; + } + + return get_blinker_pose(path.path, path.reference_lanelets, prepare_to_blinker_start_diff); + }); + turn_signal_info.desired_end_point = path.shift_line.end; + + turn_signal_info.required_start_point = path.shift_line.start; + const auto mid_lane_change_length = path.lane_change_length / 2; + const auto & shifted_path = path.shifted_path.path; + turn_signal_info.required_end_point = + get_blinker_pose(shifted_path, path.target_lanelets, mid_lane_change_length); + + status_.lane_change_path.turn_signal_info = turn_signal_info; +} + +void ExternalRequestLaneChangeModule::resetParameters() +{ + clearWaitingApproval(); + removeRTCStatus(); + resetPathCandidate(); + steering_factor_interface_ptr_->clearSteeringFactors(); + object_debug_.clear(); + debug_marker_.markers.clear(); +} + +void ExternalRequestLaneChangeModule::acceptVisitor( + const std::shared_ptr & visitor) const +{ + if (visitor) { + visitor->visitExternalRequestLaneChangeModule(this); + } +} + +void SceneModuleVisitor::visitExternalRequestLaneChangeModule( + const ExternalRequestLaneChangeModule * module) const +{ + ext_request_lane_change_visitor_ = module->get_debug_msg_array(); +} + +ExternalRequestLaneChangeRightModule::ExternalRequestLaneChangeRightModule( + const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) +: ExternalRequestLaneChangeModule{name, node, parameters, Direction::RIGHT} +{ +} + +ExternalRequestLaneChangeLeftModule::ExternalRequestLaneChangeLeftModule( + const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) +: ExternalRequestLaneChangeModule{name, node, parameters, Direction::LEFT} +{ +} + +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 8fc29f554e5a4..cff04f6784be9 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -395,9 +395,11 @@ std::pair LaneChangeModule::getSafePath( // select valid path const LaneChangePaths valid_paths = lane_change_utils::selectValidPaths( - lane_change_paths, current_lanes, check_lanes, *route_handler->getOverallGraphPtr(), - current_pose, route_handler->isInGoalRouteSection(current_lanes.back()), - route_handler->getGoalPose()); + lane_change_paths, current_lanes, check_lanes, *route_handler, current_pose, + route_handler->getGoalPose(), + common_parameters.minimum_lane_change_length + + common_parameters.backward_length_buffer_for_end_of_lane + + parameters_->lane_change_finish_judge_buffer); if (valid_paths.empty()) { return std::make_pair(false, false); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 2535596d89801..fd54a7006c086 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -212,7 +212,7 @@ LaneChangePaths getLaneChangePaths( util::getArcLengthToTargetLanelet(original_lanelets, target_lanelets.front(), pose); const auto num_to_preferred_lane = - std::abs(route_handler.getNumLaneToPreferredLane(original_lanelets.back())); + std::abs(route_handler.getNumLaneToPreferredLane(target_lanelets.back())); const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanelets.back()); const auto end_of_lane_dist = std::invoke([&]() { @@ -224,9 +224,8 @@ LaneChangePaths getLaneChangePaths( return util::getDistanceToEndOfLane(pose, original_lanelets) - required_dist; }); - const auto num_lane_change = std::max(0, num_to_preferred_lane - 1); const auto required_total_min_distance = - util::calcLaneChangeBuffer(common_parameter, num_lane_change); + util::calcLaneChangeBuffer(common_parameter, num_to_preferred_lane); const auto arc_position_from_current = lanelet::utils::getArcCoordinates(original_lanelets, pose); const auto arc_position_from_target = lanelet::utils::getArcCoordinates(target_lanelets, pose); @@ -307,16 +306,15 @@ LaneChangePaths getLaneChangePaths( LaneChangePaths selectValidPaths( const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, - const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose) + const lanelet::ConstLanelets & target_lanes, const RouteHandler & route_handler, + const Pose & current_pose, const Pose & goal_pose, const double minimum_lane_change_length) { LaneChangePaths available_paths; for (const auto & path : paths) { if (hasEnoughDistance( - path, current_lanes, target_lanes, current_pose, isInGoalRouteSection, goal_pose, - overall_graphs)) { + path, current_lanes, target_lanes, current_pose, goal_pose, route_handler, + minimum_lane_change_length)) { available_paths.push_back(path); } } @@ -359,31 +357,40 @@ bool selectSafePath( bool hasEnoughDistance( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, [[maybe_unused]] const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose, - const lanelet::routing::RoutingGraphContainer & overall_graphs) + const Pose & goal_pose, const RouteHandler & route_handler, + const double minimum_lane_change_length) { const double & lane_change_prepare_distance = path.preparation_length; const double & lane_changing_distance = path.lane_change_length; const double lane_change_total_distance = lane_change_prepare_distance + lane_changing_distance; + const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back())); + const auto overall_graphs = route_handler.getOverallGraphPtr(); + + const double lane_change_required_distance = + static_cast(num) * minimum_lane_change_length; - if (lane_change_total_distance > util::getDistanceToEndOfLane(current_pose, current_lanes)) { + if ( + lane_change_total_distance + lane_change_required_distance > + util::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; } if ( - lane_change_total_distance > util::getDistanceToNextIntersection(current_pose, current_lanes)) { + lane_change_total_distance + lane_change_required_distance > + util::getDistanceToNextIntersection(current_pose, current_lanes)) { return false; } if ( - isInGoalRouteSection && - lane_change_total_distance > util::getSignedDistance(current_pose, goal_pose, current_lanes)) { + route_handler.isInGoalRouteSection(current_lanes.back()) && + lane_change_total_distance + lane_change_required_distance > + util::getSignedDistance(current_pose, goal_pose, current_lanes)) { return false; } if ( - lane_change_total_distance > - util::getDistanceToCrosswalk(current_pose, current_lanes, overall_graphs)) { + lane_change_total_distance + lane_change_required_distance > + util::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs)) { return false; } diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index a54536ecc82d3..c1e3fd561eaa0 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -265,6 +265,10 @@ class RouteHandler bool use_exact = true) const; bool getLaneChangeTarget( const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; + bool getRightLaneChangeTargetExceptPreferredLane( + const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; + bool getLeftLaneChangeTargetExceptPreferredLane( + const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; static bool getPullOverTarget( const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, lanelet::ConstLanelet * target_lanelet); diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 79464515545b1..c41f6c9be3729 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1112,6 +1112,46 @@ bool RouteHandler::getLaneChangeTarget( return false; } +bool RouteHandler::getRightLaneChangeTargetExceptPreferredLane( + const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const +{ + for (const auto & lanelet : lanelets) { + const int num = getNumLaneToPreferredLane(lanelet); + + // Get right lanelet if preferred lane is on the left + if (num >= 0) { + if (!!routing_graph_ptr_->right(lanelet)) { + const auto right_lanelet = routing_graph_ptr_->right(lanelet); + *target_lanelet = right_lanelet.get(); + return true; + } + } + } + + *target_lanelet = lanelets.front(); + return false; +} + +bool RouteHandler::getLeftLaneChangeTargetExceptPreferredLane( + const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const +{ + for (const auto & lanelet : lanelets) { + const int num = getNumLaneToPreferredLane(lanelet); + + // Get left lanelet if preferred lane is on the right + if (num <= 0) { + if (!!routing_graph_ptr_->left(lanelet)) { + const auto left_lanelet = routing_graph_ptr_->left(lanelet); + *target_lanelet = left_lanelet.get(); + return true; + } + } + } + + *target_lanelet = lanelets.front(); + return false; +} + bool RouteHandler::getPullOverTarget( const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, lanelet::ConstLanelet * target_lanelet) diff --git a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml b/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml index 8ba0a402ee861..0f8d970d7c3a4 100644 --- a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml +++ b/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml @@ -7,6 +7,8 @@ - "intersection" - "no_stopping_area" - "traffic_light" + - "ext_request_lane_change_left" + - "ext_request_lane_change_right" - "lane_change_left" - "lane_change_right" - "avoidance_left" @@ -21,6 +23,8 @@ - "intersection" - "no_stopping_area" - "traffic_light" + - "ext_request_lane_change_left" + - "ext_request_lane_change_right" - "lane_change_left" - "lane_change_right" - "avoidance_left" diff --git a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp index 9ac306534e62e..a63b74e91aae3 100644 --- a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp +++ b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp @@ -35,6 +35,10 @@ Module getModuleType(const std::string & module_name) module.type = Module::TRAFFIC_LIGHT; } else if (module_name == "virtual_traffic_light") { module.type = Module::TRAFFIC_LIGHT; + } else if (module_name == "ext_request_lane_change_left") { + module.type = Module::EXT_REQUEST_LANE_CHANGE_LEFT; + } else if (module_name == "ext_request_lane_change_right") { + module.type = Module::EXT_REQUEST_LANE_CHANGE_RIGHT; } else if (module_name == "lane_change_left") { module.type = Module::LANE_CHANGE_LEFT; } else if (module_name == "lane_change_right") { diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index 8115a347c4c21..be8da279ec4fa 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -52,6 +52,10 @@ Module getModuleType(const std::string & module_name) module.type = Module::LANE_CHANGE_LEFT; } else if (module_name == "lane_change_right") { module.type = Module::LANE_CHANGE_RIGHT; + } else if (module_name == "ext_request_lane_change_left") { + module.type = Module::EXT_REQUEST_LANE_CHANGE_LEFT; + } else if (module_name == "ext_request_lane_change_right") { + module.type = Module::EXT_REQUEST_LANE_CHANGE_RIGHT; } else if (module_name == "avoidance_left") { module.type = Module::AVOIDANCE_LEFT; } else if (module_name == "avoidance_right") { From c43e89d2a3fc1a1b1a9afb18a9f9739b29da6c18 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 23 Jan 2023 16:11:56 +0900 Subject: [PATCH 11/60] refactor(trajectory_follower_base): use shorter namespace expression (#2715) Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe --- .../include/trajectory_follower_base/input_data.hpp | 13 ++----------- .../lateral_controller_base.hpp | 13 ++----------- .../longitudinal_controller_base.hpp | 13 ++----------- .../include/trajectory_follower_base/sync_data.hpp | 13 ++----------- .../src/lateral_controller_base.cpp | 13 ++----------- .../src/longitudinal_controller_base.cpp | 13 ++----------- .../trajectory_follower_node/controller_node.hpp | 10 ++-------- .../src/controller_node.cpp | 13 ++----------- 8 files changed, 16 insertions(+), 85 deletions(-) diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp index b135f5034d103..705f4df9088ee 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp @@ -20,13 +20,7 @@ #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { struct InputData { @@ -35,9 +29,6 @@ struct InputData autoware_auto_vehicle_msgs::msg::SteeringReport current_steering; geometry_msgs::msg::AccelWithCovarianceStamped current_accel; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp index 28eddd175e38e..657c981414c32 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp @@ -23,13 +23,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { struct LateralOutput { @@ -48,9 +42,6 @@ class LateralControllerBase LongitudinalSyncData longitudinal_sync_data_; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp index 5da298fe32b12..2c2fe2199f007 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp @@ -23,13 +23,7 @@ #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { struct LongitudinalOutput { @@ -50,9 +44,6 @@ class LongitudinalControllerBase LateralSyncData lateral_sync_data_; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp index 4735fc4a3e842..60c91019d10c3 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp @@ -15,13 +15,7 @@ #ifndef TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ #define TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { struct LateralSyncData { @@ -34,9 +28,6 @@ struct LongitudinalSyncData // bool is_velocity_converged{false}; }; -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower #endif // TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ diff --git a/control/trajectory_follower_base/src/lateral_controller_base.cpp b/control/trajectory_follower_base/src/lateral_controller_base.cpp index 1fe2f77d8b074..6acdbc4a8f5eb 100644 --- a/control/trajectory_follower_base/src/lateral_controller_base.cpp +++ b/control/trajectory_follower_base/src/lateral_controller_base.cpp @@ -14,20 +14,11 @@ #include "trajectory_follower_base/lateral_controller_base.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { void LateralControllerBase::sync(LongitudinalSyncData const & longitudinal_sync_data) { longitudinal_sync_data_ = longitudinal_sync_data; } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower_base/src/longitudinal_controller_base.cpp b/control/trajectory_follower_base/src/longitudinal_controller_base.cpp index 70fe47ee9d54a..c358f732643e0 100644 --- a/control/trajectory_follower_base/src/longitudinal_controller_base.cpp +++ b/control/trajectory_follower_base/src/longitudinal_controller_base.cpp @@ -14,20 +14,11 @@ #include "trajectory_follower_base/longitudinal_controller_base.hpp" -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower +namespace autoware::motion::control::trajectory_follower { void LongitudinalControllerBase::sync(LateralSyncData const & lateral_sync_data) { lateral_sync_data_ = lateral_sync_data; } void LongitudinalControllerBase::reset() { lateral_sync_data_.is_steer_converged = false; } -} // namespace trajectory_follower -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index 8d46732679af8..0f0baccc4e29d 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -41,11 +41,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control +namespace autoware::motion::control { using trajectory_follower::LateralOutput; using trajectory_follower::LongitudinalOutput; @@ -111,8 +107,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node const trajectory_follower::LateralOutput & lat_out) const; }; } // namespace trajectory_follower_node -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control #endif // TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 7b98a69a7c3a2..69b68bd456977 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -25,13 +25,7 @@ #include #include -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower_node +namespace autoware::motion::control::trajectory_follower_node { Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("controller", node_options) { @@ -248,10 +242,7 @@ void Controller::publishDebugMarker( debug_marker_pub_->publish(debug_marker_array); } -} // namespace trajectory_follower_node -} // namespace control -} // namespace motion -} // namespace autoware +} // namespace autoware::motion::control::trajectory_follower_node #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::trajectory_follower_node::Controller) From bc370f268fd26ef13903183caf9f8d4122713d7c Mon Sep 17 00:00:00 2001 From: beyzanurkaya <32412808+beyzanurkaya@users.noreply.github.com> Date: Mon, 23 Jan 2023 21:24:55 +0300 Subject: [PATCH 12/60] feat(obstacle_avoidance_planner): check footprint with boost::geometry::intersection (#1999) * feat(obstacle_avoidance_planner): add second check with boost::geometry::intersection Signed-off-by: beyza * add is_considering_footprint_edges param Signed-off-by: beyza * ci(pre-commit): autofix * add description Signed-off-by: beyza * add false return Signed-off-by: beyza Signed-off-by: beyza Co-authored-by: beyza Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../obstacle_avoidance_planner.param.yaml | 1 + .../obstacle_avoidance_planner/node.hpp | 1 + .../utils/utils.hpp | 3 +- .../obstacle_avoidance_planner/src/node.cpp | 6 ++- .../src/utils/utils.cpp | 38 ++++++++++++++++++- 5 files changed, 46 insertions(+), 3 deletions(-) diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml index fbe6b5d756bb2..78e123ded70bd 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -18,6 +18,7 @@ enable_pre_smoothing: true # enable EB skip_optimization: false # skip MPT and EB reset_prev_optimization: false + is_considering_footprint_edges: false # consider ego footprint edges to decide whether footprint is outside drivable area common: # sampling diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 6b9683fb24400..e2a17a2cee0b1 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -178,6 +178,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node bool enable_pre_smoothing_; bool skip_optimization_; bool reset_prev_optimization_; + bool is_considering_footprint_edges_; // vehicle circles info for for mpt constraints std::string vehicle_circle_method_; diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp index fd8a8f88e054d..e6c6c2d4dc467 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp @@ -359,7 +359,8 @@ namespace drivable_area_utils bool isOutsideDrivableAreaFromRectangleFootprint( const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, const std::vector & left_bound, - const std::vector & right_bound, const VehicleParam & vehicle_param); + const std::vector & right_bound, const VehicleParam & vehicle_param, + const bool is_considering_footprint_edges); } #endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 8508a71013479..70e1f6b4db3c6 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -305,6 +305,8 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n enable_pre_smoothing_ = declare_parameter("option.enable_pre_smoothing"); skip_optimization_ = declare_parameter("option.skip_optimization"); reset_prev_optimization_ = declare_parameter("option.reset_prev_optimization"); + is_considering_footprint_edges_ = + declare_parameter("option.is_considering_footprint_edges"); } { // trajectory parameter @@ -600,6 +602,8 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( updateParam(parameters, "option.enable_pre_smoothing", enable_pre_smoothing_); updateParam(parameters, "option.skip_optimization", skip_optimization_); updateParam(parameters, "option.reset_prev_optimization", reset_prev_optimization_); + updateParam( + parameters, "option.is_considering_footprint_edges", is_considering_footprint_edges_); } { // trajectory parameter @@ -1293,7 +1297,7 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( // calculate the first point being outside drivable area const bool is_outside = drivable_area_utils::isOutsideDrivableAreaFromRectangleFootprint( - traj_point, left_bound, right_bound, vehicle_param_); + traj_point, left_bound, right_bound, vehicle_param_, is_considering_footprint_edges_); // only insert zero velocity to the first point outside drivable area if (is_outside) { diff --git a/planning/obstacle_avoidance_planner/src/utils/utils.cpp b/planning/obstacle_avoidance_planner/src/utils/utils.cpp index bb74a3e57677c..0a35b9d532ac3 100644 --- a/planning/obstacle_avoidance_planner/src/utils/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/utils.cpp @@ -38,6 +38,9 @@ typedef bg::model::polygon bg_polygon; namespace { +namespace bg = boost::geometry; +using tier4_autoware_utils::LinearRing2d; +using tier4_autoware_utils::LineString2d; std::vector convertEulerAngleToMonotonic(const std::vector & angle) { if (angle.empty()) { @@ -683,7 +686,8 @@ namespace drivable_area_utils bool isOutsideDrivableAreaFromRectangleFootprint( const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, const std::vector & left_bound, - const std::vector & right_bound, const VehicleParam & vehicle_param) + const std::vector & right_bound, const VehicleParam & vehicle_param, + const bool is_considering_footprint_edges) { if (left_bound.empty() || right_bound.empty()) { return false; @@ -708,6 +712,38 @@ bool isOutsideDrivableAreaFromRectangleFootprint( tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_left, 0.0) .position; + if (is_considering_footprint_edges) { + LinearRing2d footprint_polygon; + LineString2d left_bound_line; + LineString2d right_bound_line; + LineString2d back_bound_line; + + footprint_polygon.push_back({top_left_pos.x, top_left_pos.y}); + footprint_polygon.push_back({top_right_pos.x, top_right_pos.y}); + footprint_polygon.push_back({bottom_right_pos.x, bottom_right_pos.y}); + footprint_polygon.push_back({bottom_left_pos.x, bottom_left_pos.y}); + + bg::correct(footprint_polygon); + + for (const auto & p : left_bound) { + left_bound_line.push_back({p.x, p.y}); + } + + for (const auto & p : right_bound) { + right_bound_line.push_back({p.x, p.y}); + } + + back_bound_line = {left_bound_line.back(), right_bound_line.back()}; + + if ( + bg::intersects(footprint_polygon, left_bound_line) || + bg::intersects(footprint_polygon, right_bound_line) || + bg::intersects(footprint_polygon, back_bound_line)) { + return true; + } + return false; + } + const bool front_top_left = isFrontDrivableArea(top_left_pos, left_bound, right_bound); const bool front_top_right = isFrontDrivableArea(top_right_pos, left_bound, right_bound); const bool front_bottom_left = isFrontDrivableArea(bottom_left_pos, left_bound, right_bound); From b7b0188e4ed94740f954ef41f6976ebda6697989 Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Tue, 24 Jan 2023 15:44:46 +0900 Subject: [PATCH 13/60] refactor(ekf_localizer): add a function to convert quaternion to euler angles (#2696) * Use 'getRPY' to convert quaternion to euler Signed-off-by: IshitaTakeshi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ekf_localizer/src/ekf_localizer.cpp | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 539911939a08d..5f98a5adcfc21 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -636,11 +637,8 @@ void EKFLocalizer::publishEstimateResult() void EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { double z = pose.pose.pose.position.z; - double roll = 0.0, pitch = 0.0, yaw_tmp = 0.0; - tf2::Quaternion q_tf; - tf2::fromMsg(pose.pose.pose.orientation, q_tf); - tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw_tmp); + const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; double z_dev = pose.pose.covariance[COV_IDX::Z_Z]; @@ -648,18 +646,15 @@ void EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovar double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH]; z_filter_.update(z, z_dev, pose.header.stamp); - roll_filter_.update(roll, roll_dev, pose.header.stamp); - pitch_filter_.update(pitch, pitch_dev, pose.header.stamp); + roll_filter_.update(rpy.x, roll_dev, pose.header.stamp); + pitch_filter_.update(rpy.y, pitch_dev, pose.header.stamp); } void EKFLocalizer::initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { double z = pose.pose.pose.position.z; - double roll = 0.0, pitch = 0.0, yaw_tmp = 0.0; - tf2::Quaternion q_tf; - tf2::fromMsg(pose.pose.pose.orientation, q_tf); - tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw_tmp); + const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; double z_dev = pose.pose.covariance[COV_IDX::Z_Z]; @@ -667,8 +662,8 @@ void EKFLocalizer::initSimple1DFilters(const geometry_msgs::msg::PoseWithCovaria double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH]; z_filter_.init(z, z_dev, pose.header.stamp); - roll_filter_.init(roll, roll_dev, pose.header.stamp); - pitch_filter_.init(pitch, pitch_dev, pose.header.stamp); + roll_filter_.init(rpy.x, roll_dev, pose.header.stamp); + pitch_filter_.init(rpy.y, pitch_dev, pose.header.stamp); } /** From 11ce12b63c9067b1c8843a3a05cc8c902e438faa Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Tue, 24 Jan 2023 16:34:31 +0900 Subject: [PATCH 14/60] refactor(ekf_localizer): add a class to manage queue elements with their ages (#2681) * Add a class to manage queue elements with their ages * Add tests of queue.clear() * Implement queue.back() Signed-off-by: IshitaTakeshi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ekf_localizer/CMakeLists.txt | 1 + .../ekf_localizer/aged_object_queue.hpp | 66 ++++++++++++ .../include/ekf_localizer/ekf_localizer.hpp | 11 +- .../ekf_localizer/src/ekf_localizer.cpp | 61 ++++------- .../test/test_aged_object_queue.cpp | 101 ++++++++++++++++++ 5 files changed, 192 insertions(+), 48 deletions(-) create mode 100644 localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp create mode 100644 localization/ekf_localizer/test/test_aged_object_queue.cpp diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 93223ac99b4b9..b9cd687a78025 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -43,6 +43,7 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) set(TEST_FILES + test/test_aged_object_queue.cpp test/test_mahalanobis.cpp test/test_measurement.cpp test/test_numeric.cpp diff --git a/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp b/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp new file mode 100644 index 0000000000000..737c25f8ce024 --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp @@ -0,0 +1,66 @@ +// Copyright 2023 Autoware Foundation +// +// 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. + +#ifndef EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_ +#define EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_ + +#include +#include + +template +class AgedObjectQueue +{ +public: + explicit AgedObjectQueue(const int max_age) : max_age_(max_age) {} + + bool empty() const { return this->size() == 0; } + + size_t size() const { return objects_.size(); } + + Object back() const { return objects_.back(); } + + void push(const Object & object) + { + objects_.push(object); + ages_.push(0); + } + + Object pop_increment_age() + { + const Object object = objects_.front(); + const int age = ages_.front() + 1; + objects_.pop(); + ages_.pop(); + + if (age < max_age_) { + objects_.push(object); + ages_.push(age); + } + + return object; + } + + void clear() + { + objects_ = std::queue(); + ages_ = std::queue(); + } + +private: + const int max_age_; + std::queue objects_; + std::queue ages_; +}; + +#endif // EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 8ebcc8b7e379f..d78d08b3f01bf 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -15,6 +15,7 @@ #ifndef EKF_LOCALIZER__EKF_LOCALIZER_HPP_ #define EKF_LOCALIZER__EKF_LOCALIZER_HPP_ +#include "ekf_localizer/aged_object_queue.hpp" #include "ekf_localizer/hyper_parameters.hpp" #include "ekf_localizer/warning.hpp" @@ -166,14 +167,8 @@ class EKFLocalizer : public rclcpp::Node bool is_activated_; - /* for model prediction */ - std::queue - current_twist_queue_; //!< @brief current measured twist - std::queue current_twist_count_queue_; - - std::queue - current_pose_queue_; //!< @brief current measured pose - std::queue current_pose_count_queue_; + AgedObjectQueue pose_queue_; + AgedObjectQueue twist_queue_; geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose geometry_msgs::msg::PoseStamped diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 5f98a5adcfc21..8b2ed64a4f89e 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -54,7 +54,9 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti params_(this), ekf_rate_(params_.ekf_rate), ekf_dt_(params_.ekf_dt), - dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */) + dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */), + pose_queue_(params_.pose_smoothing_steps), + twist_queue_(params_.twist_smoothing_steps) { /* convert to continuous to discrete */ proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0); @@ -172,46 +174,30 @@ void EKFLocalizer::timerCallback() DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); /* pose measurement update */ - if (!current_pose_queue_.empty()) { + if (!pose_queue_.empty()) { DEBUG_INFO(get_logger(), "------------------------- start Pose -------------------------"); stop_watch_.tic(); - for (size_t i = 0; i < current_pose_queue_.size(); ++i) { - const auto pose = current_pose_queue_.front(); - current_pose_queue_.pop(); - - const int count = current_pose_count_queue_.front(); - current_pose_count_queue_.pop(); - + // save the initial size because the queue size can change in the loop + const size_t n = pose_queue_.size(); + for (size_t i = 0; i < n; ++i) { + const auto pose = pose_queue_.pop_increment_age(); measurementUpdatePose(*pose); - - if (count + 1 < params_.pose_smoothing_steps) { - current_pose_queue_.push(pose); - current_pose_count_queue_.push(count + 1); - } } DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n"); } /* twist measurement update */ - if (!current_twist_queue_.empty()) { + if (!twist_queue_.empty()) { DEBUG_INFO(get_logger(), "------------------------- start Twist -------------------------"); stop_watch_.tic(); - for (size_t i = 0; i < current_twist_queue_.size(); ++i) { - const auto twist = current_twist_queue_.front(); - current_twist_queue_.pop(); - - const int count = current_twist_count_queue_.front(); - current_twist_count_queue_.pop(); - + // save the initial size because the queue size can change in the loop + const size_t n = twist_queue_.size(); + for (size_t i = 0; i < n; ++i) { + const auto twist = twist_queue_.pop_increment_age(); measurementUpdateTwist(*twist); - - if (count + 1 < params_.twist_smoothing_steps) { - current_twist_queue_.push(twist); - current_twist_count_queue_.push(count + 1); - } } DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n"); @@ -359,8 +345,7 @@ void EKFLocalizer::callbackPoseWithCovariance( return; } - current_pose_queue_.push(msg); - current_pose_count_queue_.push(0); + pose_queue_.push(msg); updateSimple1DFilters(*msg); } @@ -371,8 +356,7 @@ void EKFLocalizer::callbackPoseWithCovariance( void EKFLocalizer::callbackTwistWithCovariance( geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - current_twist_queue_.push(msg); - current_twist_count_queue_.push(0); + twist_queue_.push(msg); } /* @@ -613,17 +597,17 @@ void EKFLocalizer::publishEstimateResult() pub_odom_->publish(odometry); /* debug measured pose */ - if (!current_pose_queue_.empty()) { + if (!pose_queue_.empty()) { geometry_msgs::msg::PoseStamped p; - p.pose = current_pose_queue_.back()->pose.pose; + p.pose = pose_queue_.back()->pose.pose; p.header.stamp = current_time; pub_measured_pose_->publish(p); } /* debug publish */ double pose_yaw = 0.0; - if (!current_pose_queue_.empty()) { - pose_yaw = tf2::getYaw(current_pose_queue_.back()->pose.pose.orientation); + if (!pose_queue_.empty()) { + pose_yaw = tf2::getYaw(pose_queue_.back()->pose.pose.orientation); } tier4_debug_msgs::msg::Float64MultiArrayStamped msg; @@ -674,11 +658,8 @@ void EKFLocalizer::serviceTriggerNode( std_srvs::srv::SetBool::Response::SharedPtr res) { if (req->data) { - while (!current_pose_queue_.empty()) current_pose_queue_.pop(); - while (!current_pose_count_queue_.empty()) current_pose_count_queue_.pop(); - - while (!current_twist_queue_.empty()) current_twist_queue_.pop(); - while (!current_twist_count_queue_.empty()) current_twist_count_queue_.pop(); + pose_queue_.clear(); + twist_queue_.clear(); is_activated_ = true; } else { is_activated_ = false; diff --git a/localization/ekf_localizer/test/test_aged_object_queue.cpp b/localization/ekf_localizer/test/test_aged_object_queue.cpp new file mode 100644 index 0000000000000..9be834bf0612e --- /dev/null +++ b/localization/ekf_localizer/test/test_aged_object_queue.cpp @@ -0,0 +1,101 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "ekf_localizer/aged_object_queue.hpp" + +#include + +TEST(AgedObjectQueue, DiscardsObjectWhenAgeReachesMaximum) +{ + AgedObjectQueue queue(3); + + queue.push("a"); + EXPECT_EQ(queue.size(), 1U); + + queue.pop_increment_age(); // age = 1 + EXPECT_EQ(queue.size(), 1U); + + queue.pop_increment_age(); // age = 2 + EXPECT_EQ(queue.size(), 1U); + + queue.pop_increment_age(); // age = 3 + EXPECT_EQ(queue.size(), 0U); +} + +TEST(AgedObjectQueue, MultipleObjects) +{ + AgedObjectQueue queue(3); + + queue.push("a"); + EXPECT_EQ(queue.size(), 1U); + EXPECT_EQ(queue.pop_increment_age(), std::string{"a"}); // age of a = 1 + EXPECT_EQ(queue.size(), 1U); + EXPECT_EQ(queue.pop_increment_age(), std::string{"a"}); // age of a = 2 + + queue.push("b"); + + EXPECT_EQ(queue.pop_increment_age(), std::string{"a"}); // age of a = 3 + EXPECT_EQ(queue.size(), 1U); + + EXPECT_EQ(queue.pop_increment_age(), std::string{"b"}); // age of b = 1 + EXPECT_EQ(queue.size(), 1U); + + EXPECT_EQ(queue.pop_increment_age(), std::string{"b"}); // age of b = 2 + EXPECT_EQ(queue.size(), 1U); + + EXPECT_EQ(queue.pop_increment_age(), std::string{"b"}); // age of b = 3 + EXPECT_EQ(queue.size(), 0U); +} + +TEST(AgedObjectQueue, Empty) +{ + AgedObjectQueue queue(2); + + EXPECT_TRUE(queue.empty()); + + queue.push("a"); + + EXPECT_FALSE(queue.empty()); + + queue.pop_increment_age(); + queue.pop_increment_age(); + + EXPECT_TRUE(queue.empty()); +} + +TEST(AgedObjectQueue, Clear) +{ + AgedObjectQueue queue(3); + + queue.push("a"); + queue.push("b"); + + EXPECT_EQ(queue.size(), 2U); + + queue.clear(); + + EXPECT_EQ(queue.size(), 0U); +} + +TEST(AgedObjectQueue, Back) +{ + AgedObjectQueue queue(3); + + queue.push("a"); + + EXPECT_EQ(queue.back(), std::string{"a"}); + queue.push("b"); + + EXPECT_EQ(queue.back(), std::string{"b"}); +} From 4c9d247ad4fbdf019b842be9a8db5ad9858ab818 Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Tue, 24 Jan 2023 17:49:36 +0900 Subject: [PATCH 15/60] refactor(ekf_localizer): replace the mahalanobisGate function with a simple if statement (#2698) --- localization/ekf_localizer/CMakeLists.txt | 4 ++- .../include/ekf_localizer/mahalanobis.hpp | 4 +-- .../include/ekf_localizer/warning_message.hpp | 22 ++++++++++++ .../ekf_localizer/src/ekf_localizer.cpp | 34 +++++++++---------- .../ekf_localizer/src/mahalanobis.cpp | 6 ++-- .../ekf_localizer/src/warning_message.cpp | 24 +++++++++++++ .../ekf_localizer/test/test_mahalanobis.cpp | 29 ++++++++++------ .../test/test_warning_message.cpp | 24 +++++++++++++ 8 files changed, 112 insertions(+), 35 deletions(-) create mode 100644 localization/ekf_localizer/include/ekf_localizer/warning_message.hpp create mode 100644 localization/ekf_localizer/src/warning_message.cpp create mode 100644 localization/ekf_localizer/test/test_warning_message.cpp diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index b9cd687a78025..75d2c88bad060 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -19,6 +19,7 @@ ament_auto_add_library(ekf_localizer_lib SHARED src/mahalanobis.cpp src/measurement.cpp src/state_transition.cpp + src/warning_message.cpp ) target_link_libraries(ekf_localizer_lib Eigen3::Eigen) @@ -47,7 +48,8 @@ if(BUILD_TESTING) test/test_mahalanobis.cpp test/test_measurement.cpp test/test_numeric.cpp - test/test_state_transition.cpp) + test/test_state_transition.cpp + test/test_warning_message.cpp) foreach(filepath ${TEST_FILES}) add_testcase(${filepath}) diff --git a/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp b/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp index ff7c9bcb29270..f2b9dc626e67a 100644 --- a/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp @@ -21,8 +21,6 @@ double squaredMahalanobis( const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C); -bool mahalanobisGate( - const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x, - const Eigen::MatrixXd & cov); +double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C); #endif // EKF_LOCALIZER__MAHALANOBIS_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp new file mode 100644 index 0000000000000..019fe0825013c --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -0,0 +1,22 @@ +// Copyright 2023 Autoware Foundation +// +// 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. + +#ifndef EKF_LOCALIZER__WARNING_MESSAGE_HPP_ +#define EKF_LOCALIZER__WARNING_MESSAGE_HPP_ + +#include + +std::string mahalanobisWarningMessage(const double distance, const double max_distance); + +#endif // EKF_LOCALIZER__WARNING_MESSAGE_HPP_ diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 8b2ed64a4f89e..580262f08a3eb 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -22,6 +22,7 @@ #include "ekf_localizer/state_index.hpp" #include "ekf_localizer/state_transition.hpp" #include "ekf_localizer/warning.hpp" +#include "ekf_localizer/warning_message.hpp" #include #include @@ -437,16 +438,16 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar } /* Gate */ - Eigen::MatrixXd y_ekf(dim_y, 1); - y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::X), - ekf_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw; + const Eigen::Vector3d y_ekf( + ekf_.getXelement(delay_step * dim_x_ + IDX::X), ekf_.getXelement(delay_step * dim_x_ + IDX::Y), + ekf_yaw); const Eigen::MatrixXd P_curr = ekf_.getLatestP(); const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y); - if (!mahalanobisGate(params_.pose_gate_dist, y_ekf, y, P_y)) { - warning_.warnThrottle( - "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore " - "measurement data.", - 2000); + + const double distance = mahalanobis(y_ekf, y, P_y); + if (distance > params_.pose_gate_dist) { + warning_.warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000); + warning_.warnThrottle("Ignore the measurement data.", 2000); return; } @@ -515,17 +516,16 @@ void EKFLocalizer::measurementUpdateTwist( return; } - /* Gate */ - Eigen::MatrixXd y_ekf(dim_y, 1); - y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::VX), - ekf_.getXelement(delay_step * dim_x_ + IDX::WZ); + const Eigen::Vector2d y_ekf( + ekf_.getXelement(delay_step * dim_x_ + IDX::VX), + ekf_.getXelement(delay_step * dim_x_ + IDX::WZ)); const Eigen::MatrixXd P_curr = ekf_.getLatestP(); const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y); - if (!mahalanobisGate(params_.twist_gate_dist, y_ekf, y, P_y)) { - warning_.warnThrottle( - "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore " - "measurement data.", - 2000); + + const double distance = mahalanobis(y_ekf, y, P_y); + if (distance > params_.twist_gate_dist) { + warning_.warnThrottle(mahalanobisWarningMessage(distance, params_.twist_gate_dist), 2000); + warning_.warnThrottle("Ignore the measurement data.", 2000); return; } diff --git a/localization/ekf_localizer/src/mahalanobis.cpp b/localization/ekf_localizer/src/mahalanobis.cpp index 0584165855bdb..ff5ef13892b73 100644 --- a/localization/ekf_localizer/src/mahalanobis.cpp +++ b/localization/ekf_localizer/src/mahalanobis.cpp @@ -21,9 +21,7 @@ double squaredMahalanobis( return d.dot(C.inverse() * d); } -bool mahalanobisGate( - const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x, - const Eigen::MatrixXd & cov) +double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C) { - return squaredMahalanobis(x, obj_x, cov) <= dist_max * dist_max; + return std::sqrt(squaredMahalanobis(x, y, C)); } diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp new file mode 100644 index 0000000000000..5dbeaa847509a --- /dev/null +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -0,0 +1,24 @@ +// Copyright 2023 Autoware Foundation +// +// 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 "ekf_localizer/warning_message.hpp" + +#include +#include + +std::string mahalanobisWarningMessage(const double distance, const double max_distance) +{ + const std::string s = "The Mahalanobis distance {:.4f} is over the limit {:.4f}."; + return fmt::format(s, distance, max_distance); +} diff --git a/localization/ekf_localizer/test/test_mahalanobis.cpp b/localization/ekf_localizer/test/test_mahalanobis.cpp index a9f6c1a1bc38b..d208c1e8da06b 100644 --- a/localization/ekf_localizer/test/test_mahalanobis.cpp +++ b/localization/ekf_localizer/test/test_mahalanobis.cpp @@ -18,7 +18,7 @@ constexpr double tolerance = 1e-8; -TEST(MahalanobisGate, SquaredMahalanobis) +TEST(SquaredMahalanobis, SmokeTest) { { Eigen::Vector2d x(0, 1); @@ -39,14 +39,23 @@ TEST(MahalanobisGate, SquaredMahalanobis) } } -TEST(MahalanobisGate, MahalanobisGate) +TEST(Mahalanobis, SmokeTest) { - Eigen::Vector2d x(0, 1); - Eigen::Vector2d y(3, 2); - Eigen::Matrix2d C; - C << 10, 0, 0, 10; - - EXPECT_FALSE(mahalanobisGate(0.99, x, y, C)); - EXPECT_FALSE(mahalanobisGate(1.00, x, y, C)); - EXPECT_TRUE(mahalanobisGate(1.01, x, y, C)); + { + Eigen::Vector2d x(0, 1); + Eigen::Vector2d y(3, 2); + Eigen::Matrix2d C; + C << 10, 0, 0, 10; + + EXPECT_NEAR(mahalanobis(x, y, C), 1.0, tolerance); + } + + { + Eigen::Vector2d x(4, 1); + Eigen::Vector2d y(1, 5); + Eigen::Matrix2d C; + C << 5, 0, 0, 5; + + EXPECT_NEAR(mahalanobis(x, y, C), std::sqrt(5.0), tolerance); + } } diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp new file mode 100644 index 0000000000000..d499a80f2acd5 --- /dev/null +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -0,0 +1,24 @@ +// Copyright 2023 Autoware Foundation +// +// 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 "ekf_localizer/warning_message.hpp" + +#include + +TEST(MahalanobisWarningMessage, SmokeTest) +{ + EXPECT_STREQ( + mahalanobisWarningMessage(1.0, 0.5).c_str(), + "The Mahalanobis distance 1.0000 is over the limit 0.5000."); +} From a84040466fd56f7cb63d185feacb264795bd9d69 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 24 Jan 2023 18:40:13 +0900 Subject: [PATCH 16/60] feat(behavior_path_planner): visualize pull_over planner type text (#2169) * feat(behavior_path_planner): visualize pull_over planner type text Signed-off-by: kosuke55 * update for latest pull over Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- planning/behavior_path_planner/CMakeLists.txt | 1 + planning/behavior_path_planner/package.xml | 1 + .../pull_over/pull_over_module.cpp | 19 ++++++++++++++++++- 3 files changed, 20 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 0204581ff649c..7a31aed99f160 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(autoware_cmake REQUIRED) autoware_package() find_package(OpenCV REQUIRED) +find_package(magic_enum CONFIG REQUIRED) ament_auto_add_library(behavior_path_planner_node SHARED src/behavior_path_planner_node.cpp diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index ab1ce592fd1eb..1b50220431362 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -53,6 +53,7 @@ lanelet2_extension libboost-dev libopencv-dev + magic_enum motion_utils rclcpp rclcpp_components diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index f3d65b790d2b8..e2ae97e2cacb5 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -845,7 +846,11 @@ void PullOverModule::setDebugData() using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; using motion_utils::createStopVirtualWallMarker; + using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; + using tier4_autoware_utils::createMarkerScale; + + const auto header = planner_data_->route_handler->getRouteHeader(); const auto add = [this](const MarkerArray & added) { tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); @@ -853,7 +858,6 @@ void PullOverModule::setDebugData() if (parameters_.enable_goal_research) { // Visualize pull over areas - const auto header = planner_data_->route_handler->getRouteHeader(); const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green const double z = refined_goal_pose_.position.z; @@ -873,6 +877,19 @@ void PullOverModule::setDebugData() add(createPathMarkerArray(status_.pull_over_path.getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); } + // Visualize planner type text + { + visualization_msgs::msg::MarkerArray planner_type_marker_array{}; + auto marker = createDefaultMarker( + header.frame_id, header.stamp, "planner_type", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), + createMarkerColor(1.0, 1.0, 1.0, 0.99)); + marker.pose = modified_goal_pose_->goal_pose; + marker.text = magic_enum::enum_name(status_.pull_over_path.type); + planner_type_marker_array.markers.push_back(marker); + add(planner_type_marker_array); + } + // Visualize debug poses const auto & debug_poses = status_.pull_over_path.debug_poses; for (size_t i = 0; i < debug_poses.size(); ++i) { From 60d76c15787067d86717efd56293558e05aa696c Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Tue, 24 Jan 2023 19:20:56 +0900 Subject: [PATCH 17/60] refactor: ego's position source change from tf to topic in planning/control module (#2530) * change in pure_pursuit Signed-off-by: kyoichi-sugahara * change in motion_velocity_smoother Signed-off-by: kyoichi-sugahara * replace pose from tf to topic in behavior_path_planner Signed-off-by: kyoichi-sugahara * ci(pre-commit): autofix * replace PoseStamped with Pose Signed-off-by: kyoichi-sugahara * replace self_pose with self_odometry Signed-off-by: kyoichi-sugahara * delete self_pose in planner_data Signed-off-by: kyoichi-sugahara * define current_odometry Signed-off-by: kyoichi-sugahara * replace transofrmed position with topic in behavior_velocity_planner Signed-off-by: kyoichi-sugahara * remove current_pose Signed-off-by: kyoichi-sugahara * ci(pre-commit): autofix * fix build error Signed-off-by: kyoichi-sugahara * ci(pre-commit): autofix * fix(system_monitor): change default param path (#2560) Signed-off-by: kminoda Signed-off-by: kminoda * revert(tier4_map_launch): move config back to autoware.universe (#2561) * revert(tier4_map_launch): move config back to autoware.universe Signed-off-by: kminoda * fix map.launch.xml Signed-off-by: kminoda Signed-off-by: kminoda * define self_odometry as ConstPointer Signed-off-by: kyoichi-sugahara * tf to topic in obstacle avoidance planner Signed-off-by: kyoichi-sugahara * tf to topic in obstacle velocity limiter node Signed-off-by: kyoichi-sugahara * tf to topic in obstacle velocity limiter node Signed-off-by: kyoichi-sugahara * fix build error Signed-off-by: kyoichi-sugahara * ci(pre-commit): autofix * replace TF to odom Signed-off-by: kyoichi-sugahara * delete unnecessary unlock Signed-off-by: kyoichi-sugahara * fix builf error * fix bug Signed-off-by: kyoichi-sugahara * self_pose to self_odometry in lanechange module Signed-off-by: kyoichi-sugahara * fix small bug Signed-off-by: kyoichi-sugahara * Update node.cpp Signed-off-by: kyoichi-sugahara Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> --- .../pure_pursuit_lateral_controller.cpp | 10 ++--- .../behavior_path_planner_node.hpp | 9 ++--- .../behavior_path_planner/data_manager.hpp | 1 - .../avoidance/avoidance_module.hpp | 6 +-- .../scene_module/scene_module_interface.hpp | 4 +- .../side_shift/side_shift_module.hpp | 6 +-- .../utils/geometric_parallel_parking.hpp | 1 - .../behavior_path_planner/utilities.hpp | 1 - .../src/behavior_path_planner_node.cpp | 12 +----- .../avoidance/avoidance_module.cpp | 2 +- .../external_request_lane_change_module.cpp | 5 ++- .../lane_change/lane_change_module.cpp | 2 +- .../src/scene_module/lane_change/util.cpp | 2 +- .../lane_following/lane_following_module.cpp | 2 +- .../scene_module/pull_out/pull_out_module.cpp | 35 +++++++++-------- .../src/scene_module/pull_out/util.cpp | 2 +- .../pull_over/pull_over_module.cpp | 26 ++++++------- .../pull_over/shift_pull_over.cpp | 2 +- .../side_shift/side_shift_module.cpp | 31 +++++++-------- .../utils/geometric_parallel_parking.cpp | 3 +- .../src/turn_signal_decider.cpp | 2 +- .../behavior_path_planner/src/utilities.cpp | 20 +++++----- .../behavior_velocity_planner/node.hpp | 2 +- .../planner_data.hpp | 3 +- .../scene_module/scene_module_interface.hpp | 12 ++++-- .../include/utilization/trajectory_utils.hpp | 2 +- .../behavior_velocity_planner/src/node.cpp | 39 +++++-------------- .../src/scene_module/blind_spot/manager.cpp | 4 +- .../src/scene_module/blind_spot/scene.cpp | 14 +++---- .../src/scene_module/crosswalk/manager.cpp | 8 ++-- .../crosswalk/scene_crosswalk.cpp | 24 ++++++------ .../scene_module/crosswalk/scene_walkway.cpp | 7 ++-- .../scene_module/detection_area/manager.cpp | 4 +- .../src/scene_module/detection_area/scene.cpp | 5 ++- .../src/scene_module/intersection/manager.cpp | 8 ++-- .../intersection/scene_intersection.cpp | 18 ++++----- .../scene_merge_from_private_road.cpp | 6 +-- .../scene_module/no_stopping_area/manager.cpp | 4 +- .../scene_no_stopping_area.cpp | 14 +++---- .../occlusion_spot/scene_occlusion_spot.cpp | 2 +- .../src/scene_module/run_out/scene.cpp | 8 ++-- .../src/scene_module/speed_bump/manager.cpp | 4 +- .../src/scene_module/speed_bump/scene.cpp | 2 +- .../src/scene_module/stop_line/manager.cpp | 2 +- .../src/scene_module/stop_line/scene.cpp | 11 +++--- .../scene_module/traffic_light/manager.cpp | 4 +- .../src/scene_module/traffic_light/scene.cpp | 8 ++-- .../virtual_traffic_light/manager.cpp | 4 +- .../virtual_traffic_light/scene.cpp | 20 +++++----- .../motion_velocity_smoother_node.hpp | 3 -- .../src/motion_velocity_smoother_node.cpp | 38 ++++++++---------- .../obstacle_avoidance_planner/node.hpp | 4 +- .../obstacle_avoidance_planner/src/node.cpp | 12 ++---- .../include/obstacle_stop_planner/node.hpp | 2 +- .../src/adaptive_cruise_control.cpp | 4 +- planning/obstacle_stop_planner/src/node.cpp | 14 +++---- .../obstacle_velocity_limiter_node.hpp | 3 +- .../src/obstacle_velocity_limiter_node.cpp | 20 ++++------ .../static_centerline_optimizer/src/utils.cpp | 11 +++--- 59 files changed, 243 insertions(+), 291 deletions(-) diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp index 5cf4ee07990b2..c288df3a8fb12 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp @@ -261,7 +261,7 @@ void PurePursuitLateralController::averageFilterTrajectory( boost::optional PurePursuitLateralController::generatePredictedTrajectory() { const auto closest_idx_result = - motion_utils::findNearestIndex(output_tp_array_, current_pose_, 3.0, M_PI_4); + motion_utils::findNearestIndex(output_tp_array_, current_odometry_.pose.pose, 3.0, M_PI_4); if (!closest_idx_result) { return boost::none; @@ -282,7 +282,7 @@ boost::optional PurePursuitLateralController::generatePredictedTraje // For first point, use the odometry for velocity, and use the current_pose for prediction. TrajectoryPoint p; - p.pose = current_pose_; + p.pose = current_odometry_.pose.pose; p.longitudinal_velocity_mps = current_odometry_.twist.twist.linear.x; predicted_trajectory.points.push_back(p); @@ -370,7 +370,7 @@ bool PurePursuitLateralController::calcIsSteerConverged(const AckermannLateralCo AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd() { // Generate the control command - const auto pp_output = calcTargetCurvature(true, current_pose_); + const auto pp_output = calcTargetCurvature(true, current_odometry_.pose.pose); AckermannLateralCommand output_cmd; if (pp_output) { @@ -410,9 +410,7 @@ void PurePursuitLateralController::publishDebugMarker() const marker_array.markers.push_back(createNextTargetMarker(debug_data_.next_target)); marker_array.markers.push_back( - createTrajectoryCircleMarker(debug_data_.next_target, current_pose_)); - - pub_debug_marker_->publish(marker_array); + createTrajectoryCircleMarker(debug_data_.next_target, current_odometry_.pose.pose)); } boost::optional PurePursuitLateralController::calcTargetCurvature( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index d785fc0186b99..07d5621ec7fe9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -27,8 +27,6 @@ #include "behavior_path_planner/steering_factor_interface.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" -#include - #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" #include #include @@ -113,7 +111,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::shared_ptr planner_data_; std::shared_ptr bt_manager_; std::unique_ptr steering_factor_interface_ptr_; - tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; Scenario::SharedPtr current_scenario_{nullptr}; HADMapBin::ConstSharedPtr map_ptr_{nullptr}; @@ -146,7 +143,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node PullOutParameters getPullOutParam(); // callback - void onVelocity(const Odometry::ConstSharedPtr msg); + void onOdometry(const Odometry::SharedPtr msg); void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg); void onPerception(const PredictedObjects::ConstSharedPtr msg); void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); @@ -226,7 +223,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node { const auto & p = planner_data_; return motion_utils::findFirstNearestIndexWithSoftConstraints( - points, p->self_pose->pose, p->parameters.ego_nearest_dist_threshold, + points, p->self_odometry->pose.pose, p->parameters.ego_nearest_dist_threshold, p->parameters.ego_nearest_yaw_threshold); } @@ -235,7 +232,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node { const auto & p = planner_data_; return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->self_pose->pose, p->parameters.ego_nearest_dist_threshold, + points, p->self_odometry->pose.pose, p->parameters.ego_nearest_dist_threshold, p->parameters.ego_nearest_yaw_threshold); } }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 0543c9319f47d..ccc4b8e183fd5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -69,7 +69,6 @@ struct DrivableLanes struct PlannerData { - PoseStamped::ConstSharedPtr self_pose{}; Odometry::ConstSharedPtr self_odometry{}; AccelWithCovarianceStamped::ConstSharedPtr self_acceleration{}; PredictedObjects::ConstSharedPtr dynamic_object{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index a3727740ee2e3..43c6467dc672d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -137,7 +137,7 @@ class AvoidanceModule : public SceneModuleInterface void updateRegisteredRTCStatus(const PathWithLaneId & path) { - const Point ego_position = planner_data_->self_pose->pose.position; + const Point ego_position = planner_data_->self_odometry->pose.pose.position; for (const auto & left_shift : left_shift_array_) { const double start_distance = @@ -451,9 +451,9 @@ class AvoidanceModule : public SceneModuleInterface double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); } - Point getEgoPosition() const { return planner_data_->self_pose->pose.position; } + Point getEgoPosition() const { return planner_data_->self_odometry->pose.pose.position; } - Pose getEgoPose() const { return planner_data_->self_pose->pose; } + Pose getEgoPose() const { return planner_data_->self_odometry->pose.pose; } Pose getUnshiftedEgoPose(const ShiftedPath & prev_path) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index ba03589cc769e..ab4aaa7ac72b7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -311,7 +311,7 @@ class SceneModuleInterface { const auto & p = planner_data_; return motion_utils::findFirstNearestIndexWithSoftConstraints( - points, p->self_pose->pose, p->parameters.ego_nearest_dist_threshold, + points, p->self_odometry->pose.pose, p->parameters.ego_nearest_dist_threshold, p->parameters.ego_nearest_yaw_threshold); } @@ -320,7 +320,7 @@ class SceneModuleInterface { const auto & p = planner_data_; return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->self_pose->pose, p->parameters.ego_nearest_dist_threshold, + points, p->self_odometry->pose.pose, p->parameters.ego_nearest_dist_threshold, p->parameters.ego_nearest_yaw_threshold); } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index e4e39443b2350..49404c4613984 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -128,10 +128,10 @@ class SideShiftModule : public SceneModuleInterface ShiftLine prev_shift_line_; // NOTE: this function is ported from avoidance. - PoseStamped getUnshiftedEgoPose(const ShiftedPath & prev_path) const; - inline PoseStamped getEgoPose() const { return *(planner_data_->self_pose); } + Pose getUnshiftedEgoPose(const ShiftedPath & prev_path) const; + inline Pose getEgoPose() const { return planner_data_->self_odometry->pose.pose; } PathWithLaneId calcCenterLinePath( - const std::shared_ptr & planner_data, const PoseStamped & pose) const; + const std::shared_ptr & planner_data, const Pose & pose) const; mutable rclcpp::Time last_requested_shift_change_time_{clock_->now()}; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp index 3f8e20fa59659..74d47615a3a79 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp @@ -44,7 +44,6 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; -using geometry_msgs::msg::PoseStamped; struct ParallelParkingParameters { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index ca4f924677b2d..3c9cf8c66ae57 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -64,7 +64,6 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; -using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; using route_handler::RouteHandler; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 2114750679d16..a319217ce6b3b 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -79,7 +79,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod // subscriber velocity_subscriber_ = create_subscription( - "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1), + "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onOdometry, this, _1), createSubscriptionOptions(this)); acceleration_subscriber_ = create_subscription( "~/input/accel", 1, std::bind(&BehaviorPathPlannerNode::onAcceleration, this, _1), @@ -734,11 +734,6 @@ bool BehaviorPathPlannerNode::isDataReady() return missing("self_acceleration"); } - planner_data_->self_pose = self_pose_listener_.getCurrentPose(); - if (!planner_data_->self_pose) { - return missing("self_pose"); - } - return true; } @@ -746,9 +741,6 @@ std::shared_ptr BehaviorPathPlannerNode::createLatestPlannerData() { const std::lock_guard lock(mutex_pd_); - // update self - planner_data_->self_pose = self_pose_listener_.getCurrentPose(); - // update map if (has_received_map_) { planner_data_->route_handler->setMap(*map_ptr_); @@ -1027,7 +1019,7 @@ bool BehaviorPathPlannerNode::keepInputPoints( return false; } -void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg) +void BehaviorPathPlannerNode::onOdometry(const Odometry::SharedPtr msg) { const std::lock_guard lock(mutex_pd_); planner_data_->self_odometry = msg; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 2d8cd0da7f0fb..3c8ffe389aca6 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -3413,7 +3413,7 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con } if (ego_front_to_shift_start > 0.0) { - turn_signal_info.desired_start_point = planner_data_->self_pose->pose; + turn_signal_info.desired_start_point = planner_data_->self_odometry->pose.pose; } else { turn_signal_info.desired_start_point = blinker_start_pose; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp index ec000759aa002..2d82be7a381e3 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -650,7 +650,10 @@ void ExternalRequestLaneChangeModule::updateSteeringFactorPtr( {output.start_distance_to_path_change, output.finish_distance_to_path_change}, SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); } -Pose ExternalRequestLaneChangeModule::getEgoPose() const { return planner_data_->self_pose->pose; } +Pose ExternalRequestLaneChangeModule::getEgoPose() const +{ + return planner_data_->self_odometry->pose.pose; +} Twist ExternalRequestLaneChangeModule::getEgoTwist() const { return planner_data_->self_odometry->twist.twist; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index cff04f6784be9..1797b5632b444 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -652,7 +652,7 @@ void LaneChangeModule::updateSteeringFactorPtr( {output.start_distance_to_path_change, output.finish_distance_to_path_change}, SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); } -Pose LaneChangeModule::getEgoPose() const { return planner_data_->self_pose->pose; } +Pose LaneChangeModule::getEgoPose() const { return planner_data_->self_odometry->pose.pose; } Twist LaneChangeModule::getEgoTwist() const { return planner_data_->self_odometry->twist.twist; } std_msgs::msg::Header LaneChangeModule::getRouteHeader() const { diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index fd54a7006c086..3e683f9b05e10 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -776,7 +776,7 @@ std::optional getAbortPaths( { const auto & route_handler = planner_data->route_handler; const auto current_speed = util::l2Norm(planner_data->self_odometry->twist.twist.linear); - const auto current_pose = planner_data->self_pose->pose; + const auto current_pose = planner_data->self_odometry->pose.pose; const auto reference_lanelets = selected_path.reference_lanelets; const auto ego_nearest_dist_threshold = planner_data->parameters.ego_nearest_dist_threshold; diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index 67fc8c3ef7a0e..46e96690ba5ee 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -102,7 +102,7 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const PathWithLaneId reference_path{}; const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; + const auto current_pose = planner_data_->self_odometry->pose.pose; const auto p = planner_data_->parameters; // Set header diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index c827d9a5d452a..12f23e7b7b507 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -121,7 +121,8 @@ bool PullOutModule::isExecutionRequested() const // Create vehicle footprint const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); const auto vehicle_footprint = transformVector( - local_vehicle_footprint, tier4_autoware_utils::pose2transform(planner_data_->self_pose->pose)); + local_vehicle_footprint, + tier4_autoware_utils::pose2transform(planner_data_->self_odometry->pose.pose)); // Check if ego is not out of lanes const auto current_lanes = util::getExtendedCurrentLanes(planner_data_); @@ -135,8 +136,8 @@ bool PullOutModule::isExecutionRequested() const // Check if any of the footprint points are in the shoulder lane lanelet::Lanelet closest_shoulder_lanelet; if (!lanelet::utils::query::getClosestLanelet( - planner_data_->route_handler->getShoulderLanelets(), planner_data_->self_pose->pose, - &closest_shoulder_lanelet)) { + planner_data_->route_handler->getShoulderLanelets(), + planner_data_->self_odometry->pose.pose, &closest_shoulder_lanelet)) { return false; } if (!isOverlappedWithLane(closest_shoulder_lanelet, vehicle_footprint)) { @@ -169,7 +170,7 @@ BehaviorModuleOutput PullOutModule::plan() clearWaitingApproval(); resetPathCandidate(); // save current_pose when approved for start_point of turn_signal for backward driving - last_approved_pose_ = std::make_unique(planner_data_->self_pose->pose); + last_approved_pose_ = std::make_unique(planner_data_->self_odometry->pose.pose); } BehaviorModuleOutput output; @@ -216,10 +217,10 @@ BehaviorModuleOutput PullOutModule::plan() if (status_.back_finished) { const double start_distance = motion_utils::calcSignedArcLength( - path.points, planner_data_->self_pose->pose.position, + path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); const double finish_distance = motion_utils::calcSignedArcLength( - path.points, planner_data_->self_pose->pose.position, + path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); // TODO(tkhmy) add handle status TRYING @@ -229,7 +230,7 @@ BehaviorModuleOutput PullOutModule::plan() SteeringFactor::TURNING, ""); } else { const double distance = motion_utils::calcSignedArcLength( - path.points, planner_data_->self_pose->pose.position, + path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); // TODO(tkhmy) add handle status TRYING @@ -326,10 +327,10 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() if (status_.back_finished) { const double start_distance = motion_utils::calcSignedArcLength( - stop_path.points, planner_data_->self_pose->pose.position, + stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); const double finish_distance = motion_utils::calcSignedArcLength( - stop_path.points, planner_data_->self_pose->pose.position, + stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); steering_factor_interface_ptr_->updateSteeringFactor( @@ -338,7 +339,7 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() SteeringFactor::APPROACHING, ""); } else { const double distance = motion_utils::calcSignedArcLength( - stop_path.points, planner_data_->self_pose->pose.position, + stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); steering_factor_interface_ptr_->updateSteeringFactor( @@ -483,7 +484,7 @@ void PullOutModule::planWithPriorityOnShortBackDistance( PathWithLaneId PullOutModule::generateStopPath() const { - const auto & current_pose = planner_data_->self_pose->pose; + const auto & current_pose = planner_data_->self_odometry->pose.pose; constexpr double dummy_path_distance = 1.0; const auto moved_pose = calcOffsetPose(current_pose, dummy_path_distance, 0, 0); @@ -517,7 +518,7 @@ PathWithLaneId PullOutModule::generateStopPath() const void PullOutModule::updatePullOutStatus() { const auto & route_handler = planner_data_->route_handler; - const auto & current_pose = planner_data_->self_pose->pose; + const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & goal_pose = planner_data_->route_handler->getGoalPose(); status_.current_lanes = util::getExtendedCurrentLanes(planner_data_); @@ -576,7 +577,7 @@ void PullOutModule::updatePullOutStatus() // make this class? std::vector PullOutModule::searchBackedPoses() { - const Pose & current_pose = planner_data_->self_pose->pose; + const Pose & current_pose = planner_data_->self_odometry->pose.pose; // get backward shoulder path const auto arc_position_pose = @@ -649,7 +650,7 @@ bool PullOutModule::hasFinishedPullOut() const } // check ego car is close enough to goal pose - const auto current_pose = planner_data_->self_pose->pose; + const auto current_pose = planner_data_->self_odometry->pose.pose; const auto arclength_current = lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose); const auto arclength_pull_out_end = @@ -662,7 +663,7 @@ bool PullOutModule::hasFinishedPullOut() const void PullOutModule::checkBackFinished() { // check ego car is close enough to pull out start pose - const auto current_pose = planner_data_->self_pose->pose; + const auto current_pose = planner_data_->self_odometry->pose.pose; const auto distance = tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); @@ -709,7 +710,7 @@ bool PullOutModule::hasFinishedCurrentPath() { const auto current_path = getCurrentPath(); const auto current_path_end = current_path.points.back(); - const auto self_pose = planner_data_->self_pose->pose; + const auto self_pose = planner_data_->self_odometry->pose.pose; const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < parameters_.th_arrived_distance; @@ -719,7 +720,7 @@ bool PullOutModule::hasFinishedCurrentPath() TurnSignalInfo PullOutModule::calcTurnSignalInfo() const { TurnSignalInfo turn_signal{}; // output - const auto & current_pose = planner_data_->self_pose->pose; + const auto & current_pose = planner_data_->self_odometry->pose.pose; // turn on hazard light when backward driving if (!status_.back_finished) { diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp index 9ec243d2484de..09304d1665cc2 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp @@ -99,7 +99,7 @@ lanelet::ConstLanelets getPullOutLanes(const std::shared_ptr const double pull_out_lane_length = 200.0; const double & vehicle_width = planner_data->parameters.vehicle_width; const auto & route_handler = planner_data->route_handler; - const auto current_pose = planner_data->self_pose->pose; + const auto current_pose = planner_data->self_odometry->pose.pose; lanelet::ConstLanelet current_shoulder_lane; lanelet::ConstLanelets shoulder_lanes; diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index e2ae97e2cacb5..1cbdae1f5731f 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -273,7 +273,7 @@ bool PullOverModule::isExecutionRequested() const return true; } const auto & current_lanes = util::getCurrentLanes(planner_data_); - const auto & current_pose = planner_data_->self_pose->pose; + const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & goal_pose = planner_data_->route_handler->getGoalPose(); // check if goal_pose is far @@ -311,7 +311,7 @@ bool PullOverModule::isExecutionRequested() const // check if self pose is NOT in shoulder lane bool self_is_in_shoulder_lane = false; - const auto self_pose = planner_data_->self_pose->pose; + const auto self_pose = planner_data_->self_odometry->pose.pose; if (lanelet::utils::query::getClosestLanelet( planner_data_->route_handler->getShoulderLanelets(), self_pose, &closest_shoulder_lanelet)) { @@ -379,7 +379,7 @@ BT::NodeStatus PullOverModule::updateState() BehaviorModuleOutput PullOverModule::plan() { - const auto & current_pose = planner_data_->self_pose->pose; + const auto & current_pose = planner_data_->self_odometry->pose.pose; resetPathCandidate(); @@ -609,10 +609,10 @@ std::pair PullOverModule::calcDistanceToPathChange() const { const auto & full_path = status_.pull_over_path.getFullPath(); const auto dist_to_parking_start_pose = calcSignedArcLength( - full_path.points, planner_data_->self_pose->pose, status_.pull_over_path.start_pose.position, - std::numeric_limits::max(), M_PI_2); + full_path.points, planner_data_->self_odometry->pose.pose, + status_.pull_over_path.start_pose.position, std::numeric_limits::max(), M_PI_2); const double dist_to_parking_finish_pose = calcSignedArcLength( - full_path.points, planner_data_->self_pose->pose.position, + full_path.points, planner_data_->self_odometry->pose.pose.position, modified_goal_pose_->goal_pose.position); const double start_distance_to_path_change = dist_to_parking_start_pose ? *dist_to_parking_start_pose : std::numeric_limits::max(); @@ -627,7 +627,7 @@ void PullOverModule::setParameters(const PullOverParameters & parameters) PathWithLaneId PullOverModule::generateStopPath() { const auto & route_handler = planner_data_->route_handler; - const auto & current_pose = planner_data_->self_pose->pose; + const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & common_parameters = planner_data_->parameters; if (status_.current_lanes.empty()) { @@ -699,7 +699,7 @@ PathWithLaneId PullOverModule::generateStopPath() PathWithLaneId PullOverModule::generateEmergencyStopPath() { const auto & route_handler = planner_data_->route_handler; - const auto & current_pose = planner_data_->self_pose->pose; + const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & common_parameters = planner_data_->parameters; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; constexpr double eps_vel = 0.01; @@ -790,7 +790,7 @@ bool PullOverModule::isStopped() bool PullOverModule::hasFinishedCurrentPath() { const auto & current_path_end = getCurrentPath().points.back(); - const auto & self_pose = planner_data_->self_pose->pose; + const auto & self_pose = planner_data_->self_odometry->pose.pose; const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < parameters_.th_arrived_distance; @@ -800,7 +800,7 @@ bool PullOverModule::hasFinishedCurrentPath() bool PullOverModule::hasFinishedPullOver() { // check ego car is close enough to goal pose - const auto current_pose = planner_data_->self_pose->pose; + const auto current_pose = planner_data_->self_odometry->pose.pose; const bool car_is_on_goal = calcDistance2d(current_pose, modified_goal_pose_->goal_pose) < parameters_.th_arrived_distance; @@ -811,7 +811,7 @@ TurnSignalInfo PullOverModule::calcTurnSignalInfo() const { TurnSignalInfo turn_signal{}; // output - const auto & current_pose = planner_data_->self_pose->pose; + const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & start_pose = status_.pull_over_path.start_pose; const auto & end_pose = status_.pull_over_path.end_pose; const auto full_path = status_.pull_over_path.getFullPath(); @@ -927,7 +927,7 @@ bool PullOverModule::checkCollision(const PathWithLaneId & path) const bool PullOverModule::hasEnoughDistance(const PullOverPath & pull_over_path) const { - const Pose & current_pose = planner_data_->self_pose->pose; + const Pose & current_pose = planner_data_->self_odometry->pose.pose; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; // once stopped, the vehicle cannot start again if start_pose is close. @@ -954,7 +954,7 @@ bool PullOverModule::hasEnoughDistance(const PullOverPath & pull_over_path) cons void PullOverModule::printParkingPositionError() const { - const auto current_pose = planner_data_->self_pose->pose; + const auto current_pose = planner_data_->self_odometry->pose.pose; const double real_shoulder_to_map_shoulder = 0.0; const Pose goal_to_ego = inverseTransformPose(current_pose, modified_goal_pose_->goal_pose); diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp index 731b818666347..cc079ec42e3da 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp @@ -64,7 +64,7 @@ PathWithLaneId ShiftPullOver::generateReferencePath( const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const { const auto & route_handler = planner_data_->route_handler; - const Pose & current_pose = planner_data_->self_pose->pose; + const Pose & current_pose = planner_data_->self_odometry->pose.pose; const double backward_path_length = planner_data_->parameters.backward_path_length; const double pull_over_velocity = parameters_.pull_over_velocity; const double deceleration_interval = parameters_.deceleration_interval; diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 9d6e2e76aca4c..a7897618e14e8 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -27,7 +27,6 @@ namespace behavior_path_planner { using geometry_msgs::msg::Point; -using geometry_msgs::msg::PoseStamped; SideShiftModule::SideShiftModule( const std::string & name, rclcpp::Node & node, const SideShiftParameters & parameters) @@ -149,7 +148,7 @@ BT::NodeStatus SideShiftModule::updateState() current_state_ = BT::NodeStatus::SUCCESS; } else { const auto & current_lanes = util::getCurrentLanes(planner_data_); - const auto & current_pose = planner_data_->self_pose->pose; + const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & inserted_shift_line_start_pose = inserted_shift_line_.start; const auto & inserted_shift_line_end_pose = inserted_shift_line_.end; const double self_to_shift_line_start_arc_length = @@ -172,8 +171,9 @@ BT::NodeStatus SideShiftModule::updateState() void SideShiftModule::updateData() { - const auto reference_pose = prev_output_.shift_length.empty() ? *planner_data_->self_pose - : getUnshiftedEgoPose(prev_output_); + const auto reference_pose = prev_output_.shift_length.empty() + ? planner_data_->self_odometry->pose.pose + : getUnshiftedEgoPose(prev_output_); const auto centerline_path = calcCenterLinePath(planner_data_, reference_pose); constexpr double resample_interval = 1.0; @@ -185,14 +185,14 @@ void SideShiftModule::updateData() const auto & p = planner_data_->parameters; lanelet::ConstLanelet current_lane; - if (!route_handler->getClosestLaneletWithinRoute(reference_pose.pose, ¤t_lane)) { + if (!route_handler->getClosestLaneletWithinRoute(reference_pose, ¤t_lane)) { RCLCPP_ERROR_THROTTLE( getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!"); } // For current_lanes with desired length current_lanelets_ = route_handler->getLaneletSequence( - current_lane, reference_pose.pose, p.backward_path_length, p.forward_path_length); + current_lane, reference_pose, p.backward_path_length, p.forward_path_length); const size_t nearest_idx = findEgoIndex(path_shifter_.getReferencePath().points); path_shifter_.removeBehindShiftLineAndSetBaseOffset(nearest_idx); @@ -354,7 +354,7 @@ double SideShiftModule::getClosestShiftLength() const return 0.0; } - const auto ego_point = planner_data_->self_pose->pose.position; + const auto ego_point = planner_data_->self_odometry->pose.pose.position; const auto closest = motion_utils::findNearestIndex(prev_output_.path.points, ego_point); return prev_output_.shift_length.at(closest); } @@ -384,7 +384,7 @@ void SideShiftModule::adjustDrivableArea(ShiftedPath * path) const } // NOTE: this function is ported from avoidance. -PoseStamped SideShiftModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) const +Pose SideShiftModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) const { const auto ego_pose = getEgoPose(); if (prev_path.path.points.empty()) { @@ -392,20 +392,19 @@ PoseStamped SideShiftModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) } // un-shifted fot current ideal pose - const auto closest = - motion_utils::findNearestIndex(prev_path.path.points, ego_pose.pose.position); + const auto closest = motion_utils::findNearestIndex(prev_path.path.points, ego_pose.position); - PoseStamped unshifted_pose = ego_pose; + Pose unshifted_pose = ego_pose; - util::shiftPose(&unshifted_pose.pose, -prev_path.shift_length.at(closest)); - unshifted_pose.pose.orientation = ego_pose.pose.orientation; + util::shiftPose(&unshifted_pose, -prev_path.shift_length.at(closest)); + unshifted_pose.orientation = ego_pose.orientation; return unshifted_pose; } // NOTE: this function is ported from avoidance. PathWithLaneId SideShiftModule::calcCenterLinePath( - const std::shared_ptr & planner_data, const PoseStamped & pose) const + const std::shared_ptr & planner_data, const Pose & pose) const { const auto & p = planner_data->parameters; const auto & route_handler = planner_data->route_handler; @@ -430,9 +429,9 @@ PathWithLaneId SideShiftModule::calcCenterLinePath( p.backward_path_length, longest_dist_to_shift_line, backward_length); const lanelet::ConstLanelets current_lanes = - util::calcLaneAroundPose(route_handler, pose.pose, p.forward_path_length, backward_length); + util::calcLaneAroundPose(route_handler, pose, p.forward_path_length, backward_length); centerline_path = util::getCenterLinePath( - *route_handler, current_lanes, pose.pose, backward_length, p.forward_path_length, p); + *route_handler, current_lanes, pose, backward_length, p.forward_path_length, p); centerline_path.header = route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index 38cecf8ee699d..8c3271b8e75cf 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -43,7 +43,6 @@ using behavior_path_planner::util::removeOverlappingPoints; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; -using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::Transform; using geometry_msgs::msg::TransformStamped; using lanelet::utils::getArcCoordinates; @@ -322,7 +321,7 @@ PathWithLaneId GeometricParallelParking::generateStraightPath(const Pose & start const auto current_lanes = util::getExtendedCurrentLanes(planner_data_); const auto start_arc_position = lanelet::utils::getArcCoordinates(current_lanes, start_pose); - const Pose current_pose = planner_data_->self_pose->pose; + const Pose current_pose = planner_data_->self_odometry->pose.pose; const auto current_arc_position = lanelet::utils::getArcCoordinates(current_lanes, current_pose); auto path = planner_data_->route_handler->getCenterLinePath( diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index 5a590f9ae473f..a6587f9f54325 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -40,7 +40,7 @@ TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( // Data const double nearest_dist_threshold = planner_data->parameters.ego_nearest_dist_threshold; const double nearest_yaw_threshold = planner_data->parameters.ego_nearest_yaw_threshold; - const auto & current_pose = planner_data->self_pose->pose; + const auto & current_pose = planner_data->self_odometry->pose.pose; const double & current_vel = planner_data->self_odometry->twist.twist.linear.x; const auto route_handler = *(planner_data->route_handler); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index defdf6b6d51b1..cec12b798720c 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1157,7 +1157,7 @@ void generateDrivableArea( // extract data const auto transformed_lanes = util::transformToLanelets(lanes); - const auto current_pose = planner_data->self_pose; + const auto current_pose = planner_data->self_odometry->pose.pose; const auto route_handler = planner_data->route_handler; constexpr double overlap_threshold = 0.01; @@ -1226,7 +1226,7 @@ void generateDrivableArea( // Get Closest segment for the start point constexpr double front_length = 0.5; - const auto front_pose = path.points.empty() ? current_pose->pose : path.points.front().point.pose; + const auto front_pose = path.points.empty() ? current_pose : path.points.front().point.pose; const size_t front_left_start_idx = findNearestSegmentIndexFromLateralDistance(left_bound, front_pose.position); const size_t front_right_start_idx = @@ -1241,7 +1241,7 @@ void generateDrivableArea( findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); // Get Closest segment for the goal point - const auto goal_pose = path.points.empty() ? current_pose->pose : path.points.back().point.pose; + const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; const size_t goal_left_start_idx = findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose.position); const size_t goal_right_start_idx = @@ -1709,10 +1709,10 @@ std::shared_ptr generateCenterLinePath( const auto & p = planner_data->parameters; const auto & route_handler = planner_data->route_handler; - const auto & pose = planner_data->self_pose; + const auto & pose = planner_data->self_odometry->pose.pose; lanelet::ConstLanelet current_lane; - if (!route_handler->getClosestLaneletWithinRoute(pose->pose, ¤t_lane)) { + if (!route_handler->getClosestLaneletWithinRoute(pose, ¤t_lane)) { RCLCPP_ERROR( rclcpp::get_logger("behavior_path_planner").get_child("utilities"), "failed to find closest lanelet within route!!!"); @@ -1721,7 +1721,7 @@ std::shared_ptr generateCenterLinePath( // For lanelet_sequence with desired length lanelet::ConstLanelets lanelet_sequence = route_handler->getLaneletSequence( - current_lane, pose->pose, p.backward_path_length, p.forward_path_length); + current_lane, pose, p.backward_path_length, p.forward_path_length); std::vector drivable_lanes(lanelet_sequence.size()); for (size_t i = 0; i < lanelet_sequence.size(); ++i) { @@ -1730,7 +1730,7 @@ std::shared_ptr generateCenterLinePath( } *centerline_path = getCenterLinePath( - *route_handler, lanelet_sequence, pose->pose, p.backward_path_length, p.forward_path_length, p); + *route_handler, lanelet_sequence, pose, p.backward_path_length, p.forward_path_length, p); centerline_path->header = route_handler->getRouteHeader(); @@ -1746,7 +1746,7 @@ lanelet::ConstLineStrings3d getMaximumDrivableArea( { const auto & p = planner_data->parameters; const auto & route_handler = planner_data->route_handler; - const auto & ego_pose = planner_data->self_pose->pose; + const auto & ego_pose = planner_data->self_odometry->pose.pose; lanelet::ConstLanelet current_lane; if (!route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { @@ -2073,7 +2073,7 @@ std::uint8_t getHighestProbLabel(const std::vector & class lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr & planner_data) { const auto & route_handler = planner_data->route_handler; - const auto current_pose = planner_data->self_pose->pose; + const auto current_pose = planner_data->self_odometry->pose.pose; const auto & common_parameters = planner_data->parameters; lanelet::ConstLanelet current_lane; @@ -2113,7 +2113,7 @@ lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data) { const auto & route_handler = planner_data->route_handler; - const auto current_pose = planner_data->self_pose->pose; + const auto current_pose = planner_data->self_odometry->pose.pose; const auto common_parameters = planner_data->parameters; const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp index 371c0cedfb174..d595a633b36b7 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp @@ -79,7 +79,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void onPredictedObjects( const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); void onNoGroundPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - void onVehicleVelocity(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void onAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); void onTrafficSignals( diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp index 9de2d422c1d2c..bdfa81a0199e3 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp +++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp @@ -66,10 +66,9 @@ struct PlannerData system_delay = node.declare_parameter("system_delay", 0.50); delay_response_time = node.declare_parameter("delay_response_time", 0.50); } - // tf - geometry_msgs::msg::PoseStamped current_pose; // msgs from callbacks that are used for data-ready + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry; geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; static constexpr double velocity_buffer_time_sec = 10.0; diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp index 77fafdf7ddeea..528726787887f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp @@ -126,7 +126,8 @@ class SceneModuleInterface { const auto & p = planner_data_; return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); + points, p->current_odometry->pose, p->ego_nearest_dist_threshold, + p->ego_nearest_yaw_threshold); } template @@ -134,7 +135,8 @@ class SceneModuleInterface { const auto & p = planner_data_; return motion_utils::findFirstNearestIndexWithSoftConstraints( - points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); + points, p->current_odometry->pose, p->ego_nearest_dist_threshold, + p->ego_nearest_yaw_threshold); } }; @@ -304,7 +306,8 @@ class SceneModuleManagerInterface { const auto & p = planner_data_; return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); + points, p->current_odometry->pose, p->ego_nearest_dist_threshold, + p->ego_nearest_yaw_threshold); } template @@ -313,7 +316,8 @@ class SceneModuleManagerInterface { const auto & p = planner_data_; return motion_utils::findFirstNearestIndexWithSoftConstraints( - points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); + points, p->current_odometry->pose, p->ego_nearest_dist_threshold, + p->ego_nearest_yaw_threshold); } std::set> scene_modules_; diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp index d049c94930ccc..a7fdc337d3dae 100644 --- a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp @@ -90,7 +90,7 @@ inline bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, const std::shared_ptr & planner_data) { - const geometry_msgs::msg::Pose current_pose = planner_data->current_pose.pose; + const geometry_msgs::msg::Pose current_pose = planner_data->current_odometry->pose; const double v0 = planner_data->current_velocity->twist.linear.x; const double a0 = planner_data->current_acceleration->accel.accel.linear.x; const auto & external_v_limit = planner_data->external_velocity_limit; diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index d01df2a0a3449..9bf21a9d8fbea 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -64,17 +64,6 @@ namespace behavior_velocity_planner { namespace { -geometry_msgs::msg::PoseStamped transform2pose( - const geometry_msgs::msg::TransformStamped & transform) -{ - geometry_msgs::msg::PoseStamped pose; - pose.header = transform.header; - pose.pose.position.x = transform.transform.translation.x; - pose.pose.position.y = transform.transform.translation.y; - pose.pose.position.z = transform.transform.translation.z; - pose.pose.orientation = transform.transform.rotation; - return pose; -} autoware_auto_planning_msgs::msg::Path to_path( const autoware_auto_planning_msgs::msg::PathWithLaneId & path_with_id) @@ -111,8 +100,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio std::bind(&BehaviorVelocityPlannerNode::onNoGroundPointCloud, this, _1), createSubscriptionOptions(this)); sub_vehicle_odometry_ = this->create_subscription( - "~/input/vehicle_odometry", 1, - std::bind(&BehaviorVelocityPlannerNode::onVehicleVelocity, this, _1), + "~/input/vehicle_odometry", 1, std::bind(&BehaviorVelocityPlannerNode::onOdometry, this, _1), createSubscriptionOptions(this)); sub_acceleration_ = this->create_subscription( "~/input/accel", 1, std::bind(&BehaviorVelocityPlannerNode::onAcceleration, this, _1), @@ -219,13 +207,12 @@ bool BehaviorVelocityPlannerNode::isDataReady( { const auto & d = planner_data; - // from tf - if (d.current_pose.header.frame_id == "") { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Frame id of current pose is missing"); + // from callbacks + if (!d.current_odometry) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current odometry"); return false; } - // from callbacks if (!d.current_velocity) { RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current velocity"); return false; @@ -295,11 +282,15 @@ void BehaviorVelocityPlannerNode::onNoGroundPointCloud( } } -void BehaviorVelocityPlannerNode::onVehicleVelocity( - const nav_msgs::msg::Odometry::ConstSharedPtr msg) +void BehaviorVelocityPlannerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) { std::lock_guard lock(mutex_); + auto current_odometry = std::make_shared(); + current_odometry->header = msg->header; + current_odometry->pose = msg->pose.pose; + planner_data_.current_odometry = current_odometry; + auto current_velocity = std::make_shared(); current_velocity->header = msg->header; current_velocity->twist = msg->twist.twist; @@ -403,16 +394,6 @@ void BehaviorVelocityPlannerNode::onTrigger( { mutex_.lock(); // for planner_data_ - // Check ready - try { - planner_data_.current_pose = - transform2pose(tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero)); - } catch (tf2::TransformException & e) { - RCLCPP_INFO(get_logger(), "waiting for transform from `map` to `base_link`"); - mutex_.unlock(); - return; - } - if (!isDataReady(planner_data_, *get_clock())) { mutex_.unlock(); return; diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp index 2d0ecee9cee30..4dfd6606856ea 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp @@ -45,7 +45,7 @@ void BlindSpotModuleManager::launchNewModules( { for (const auto & ll : planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), - planner_data_->current_pose.pose)) { + planner_data_->current_odometry->pose)) { const auto lane_id = ll.id(); const auto module_id = lane_id; @@ -73,7 +73,7 @@ BlindSpotModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { const auto lane_id_set = planning_utils::getLaneIdSetOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose); + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); return [lane_id_set](const std::shared_ptr & scene_module) { return lane_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index 4b81d11e4bcbb..c02f9877b4daa 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -90,7 +90,7 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto logger_, "lane_id = %ld, state = %s", lane_id_, StateMachine::toString(current_state).c_str()); /* get current pose */ - geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; + geometry_msgs::msg::Pose current_pose = planner_data_->current_odometry->pose; /* get lanelet map */ const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); @@ -117,7 +117,7 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto /* calc closest index */ const auto closest_idx_opt = - motion_utils::findNearestIndex(input_path.points, current_pose.pose, 3.0, M_PI_4); + motion_utils::findNearestIndex(input_path.points, current_pose, 3.0, M_PI_4); if (!closest_idx_opt) { RCLCPP_WARN_SKIPFIRST_THROTTLE( logger_, *clock_, 1000 /* ms */, "[Blind Spot] motion_utils::findNearestIndex fail"); @@ -133,8 +133,8 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const double pass_judge_line_dist = planning_utils::calcJudgeLineDistWithAccLimit(current_vel, max_acc, delay_response_time); const auto stop_point_pose = path->points.at(stop_line_idx).point.pose; - const auto distance_until_stop = motion_utils::calcSignedArcLength( - input_path.points, current_pose.pose, stop_point_pose.position); + const auto distance_until_stop = + motion_utils::calcSignedArcLength(input_path.points, current_pose, stop_point_pose.position); if (distance_until_stop == boost::none) return true; /* get debug info */ @@ -174,7 +174,7 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto /* set stop speed */ setSafe(state_machine_.getState() != StateMachine::State::STOP); setDistance(motion_utils::calcSignedArcLength( - path->points, current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position)); + path->points, current_pose.position, path->points.at(stop_line_idx).point.pose.position)); if (!isActivated()) { constexpr double stop_vel = 0.0; planning_utils::setVelocityFromIndex(stop_line_idx, stop_vel, path); @@ -185,7 +185,7 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( - path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN); + path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); } else { *path = input_path; // reset path } @@ -390,7 +390,7 @@ bool BlindSpotModule::checkObstacleInBlindSpot( to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), lanelet::utils::to2D(areas_opt.get().detection_area)); bool exist_in_conflict_area = isPredictedPathInArea( - object, areas_opt.get().conflict_area, planner_data_->current_pose.pose); + object, areas_opt.get().conflict_area, planner_data_->current_odometry->pose); if (exist_in_detection_area || exist_in_conflict_area) { obstacle_detected = true; debug_data_.conflicting_targets.objects.push_back(object); diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp index 6028932ec2a94..f19642ca9fe6b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp @@ -129,7 +129,7 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; for (const auto & crosswalk : getCrosswalksOnPath( - planner_data_->current_pose.pose, path, rh->getLaneletMapPtr(), + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr())) { const auto module_id = crosswalk.id(); if (!isModuleRegistered(module_id)) { @@ -148,7 +148,7 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; const auto crosswalk_id_set = getCrosswalkIdSetOnPath( - planner_data_->current_pose.pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); return [crosswalk_id_set](const std::shared_ptr & scene_module) { return crosswalk_id_set.count(scene_module->getModuleId()) == 0; @@ -172,7 +172,7 @@ void WalkwayModuleManager::launchNewModules( { const auto rh = planner_data_->route_handler_; for (const auto & crosswalk : getCrosswalksOnPath( - planner_data_->current_pose.pose, path, rh->getLaneletMapPtr(), + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr())) { const auto module_id = crosswalk.id(); if ( @@ -191,7 +191,7 @@ WalkwayModuleManager::getModuleExpiredFunction( { const auto rh = planner_data_->route_handler_; const auto walkway_id_set = getCrosswalkIdSetOnPath( - planner_data_->current_pose.pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); return [walkway_id_set](const std::shared_ptr & scene_module) { return walkway_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp index 6b71d8a5cf4a2..597ed619a3b0f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp @@ -153,7 +153,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto path_intersects_.clear(); - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto intersects = getPolygonIntersects(ego_path, crosswalk_.polygon2d().basicPolygon(), ego_pos, 2); @@ -213,13 +213,13 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto insertDecelPointWithDebugInfo(nearest_stop_point.get(), 0.0, *path); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( - path->points, planner_data_->current_pose.pose, stop_factor.stop_pose, + path->points, planner_data_->current_odometry->pose, stop_factor.stop_pose, VelocityFactor::UNKNOWN); } else if (rtc_stop_point) { insertDecelPointWithDebugInfo(rtc_stop_point.get(), 0.0, *path); planning_utils::appendStopReason(stop_factor_rtc, stop_reason); velocity_factor_.set( - path->points, planner_data_->current_pose.pose, stop_factor_rtc.stop_pose, + path->points, planner_data_->current_odometry->pose, stop_factor_rtc.stop_pose, VelocityFactor::UNKNOWN); } @@ -233,7 +233,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto boost::optional> CrosswalkModule::getStopLine( const PathWithLaneId & ego_path, bool & exist_stopline_in_map) const { - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto stop_line = getStopLineFromMap(module_id_, planner_data_, "crosswalk_id"); exist_stopline_in_map = static_cast(stop_line); @@ -298,7 +298,7 @@ boost::optional CrosswalkModule::findNearestStopPoint } const auto crosswalk_attention_range = getAttentionRange(sparse_resample_path); - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto & objects_ptr = planner_data_->predicted_objects; const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -461,7 +461,7 @@ std::pair CrosswalkModule::getAttentionRange(const PathWithLaneI { stop_watch_.tic(__func__); - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pos = planner_data_->current_odometry->pose.position; double near_attention_range = 0.0; double far_attention_range = 0.0; @@ -492,7 +492,7 @@ void CrosswalkModule::insertDecelPointWithDebugInfo( if (!stop_pose) { return; } - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pos = planner_data_->current_odometry->pose.position; setDistance(calcSignedArcLength(output.points, ego_pos, stop_pose->position)); @@ -510,7 +510,7 @@ float CrosswalkModule::calcTargetVelocity( { const auto & max_jerk = planner_param_.max_slow_down_jerk; const auto & max_accel = planner_param_.max_slow_down_accel; - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto & ego_vel = planner_data_->current_velocity->twist.linear.x; if (ego_vel < planner_param_.no_relax_velocity) { @@ -531,7 +531,7 @@ void CrosswalkModule::clampAttentionRangeByNeighborCrosswalks( { stop_watch_.tic(__func__); - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto p_near = calcLongitudinalOffsetPoint(ego_path.points, ego_pos, near_attention_range); const auto p_far = calcLongitudinalOffsetPoint(ego_path.points, ego_pos, far_attention_range); @@ -645,7 +645,7 @@ std::vector CrosswalkModule::getCollisionPoints( const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto & ego_vel = planner_data_->current_velocity->twist.linear; const auto obj_polygon = @@ -757,7 +757,7 @@ bool CrosswalkModule::applySafetySlowDownSpeed(PathWithLaneId & output) return false; } - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto ego_path = output; // Safety slow down speed in [m/s] @@ -813,7 +813,7 @@ bool CrosswalkModule::isStuckVehicle( return false; } - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pos = planner_data_->current_odometry->pose.position; double near_attention_range = 0.0; double far_attention_range = 0.0; diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp index c28c1ff461800..7b968552ce654 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp @@ -45,7 +45,7 @@ WalkwayModule::WalkwayModule( boost::optional> WalkwayModule::getStopLine( const PathWithLaneId & ego_path, bool & exist_stopline_in_map) const { - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto stop_line = getStopLineFromMap(module_id_, planner_data_, "crosswalk_id"); exist_stopline_in_map = static_cast(stop_line); @@ -84,7 +84,7 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_ path_intersects_.clear(); - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto intersects = getPolygonIntersects(input, walkway_.polygon2d().basicPolygon(), ego_pos, 2); @@ -123,7 +123,8 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_ stop_factor.stop_factor_points.push_back(path_intersects_.front()); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( - path->points, planner_data_->current_pose.pose, stop_pose.get(), VelocityFactor::UNKNOWN); + path->points, planner_data_->current_odometry->pose, stop_pose.get(), + VelocityFactor::UNKNOWN); // use arc length to identify if ego vehicle is in front of walkway stop or not. const double signed_arc_dist_to_stop_point = diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp index 39fa4a8438853..afaa3502c2b1b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp @@ -47,7 +47,7 @@ void DetectionAreaModuleManager::launchNewModules( for (const auto & detection_area_with_lane_id : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), - planner_data_->current_pose.pose)) { + planner_data_->current_odometry->pose)) { // Use lanelet_id to unregister module when the route is changed const auto lane_id = detection_area_with_lane_id.second.id(); const auto module_id = detection_area_with_lane_id.first->id(); @@ -67,7 +67,7 @@ DetectionAreaModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { const auto detection_area_id_set = planning_utils::getRegElemIdSetOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose); + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); return [detection_area_id_set](const std::shared_ptr & scene_module) { return detection_area_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index 8f9bc0a6e92a3..faa5bbcc06ea1 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -77,7 +77,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const auto stop_line = getStopLineGeometry2d(); // Get self pose - const auto & self_pose = planner_data_->current_pose.pose; + const auto & self_pose = planner_data_->current_odometry->pose; const size_t current_seg_idx = findEgoSegmentIndex(path->points); // Get stop point @@ -184,7 +184,8 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_factor.stop_factor_points = obstacle_points; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( - path->points, planner_data_->current_pose.pose, stop_point->second, VelocityFactor::UNKNOWN); + path->points, planner_data_->current_odometry->pose, stop_point->second, + VelocityFactor::UNKNOWN); } // Create legacy StopReason diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 08e39abedb6f3..c1159b1bd9eee 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -78,7 +78,7 @@ void IntersectionModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { const auto lanelets = planning_utils::getLaneletsOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose); + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); const auto lane_id = ll.id(); @@ -108,7 +108,7 @@ void MergeFromPrivateModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { const auto lanelets = planning_utils::getLaneletsOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose); + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); const auto lane_id = ll.id(); @@ -162,7 +162,7 @@ IntersectionModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { const auto lane_id_set = planning_utils::getLaneIdSetOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose); + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); return [lane_id_set](const std::shared_ptr & scene_module) { return lane_id_set.count(scene_module->getModuleId()) == 0; @@ -173,7 +173,7 @@ MergeFromPrivateModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { const auto lane_id_set = planning_utils::getLaneIdSetOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose); + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); return [lane_id_set](const std::shared_ptr & scene_module) { return lane_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 0b617373ff5ba..3e25a1bb73d76 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -85,7 +85,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * logger_, "lane_id = %ld, state = %s", lane_id_, StateMachine::toString(current_state).c_str()); /* get current pose */ - const geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; + const geometry_msgs::msg::Pose current_pose = planner_data_->current_odometry->pose; const double current_vel = planner_data_->current_velocity->twist.linear.x; /* get lanelet map */ @@ -139,7 +139,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * /* calc closest index */ const auto closest_idx_opt = - motion_utils::findNearestIndex(path->points, current_pose.pose, 3.0, M_PI_4); + motion_utils::findNearestIndex(path->points, current_pose, 3.0, M_PI_4); if (!closest_idx_opt) { RCLCPP_WARN_SKIPFIRST_THROTTLE( logger_, *clock_, 1000 /* ms */, "motion_utils::findNearestIndex fail"); @@ -156,10 +156,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const auto keep_detection_line_idx = stop_lines_idx_opt.value().keep_detection_line; const bool is_over_pass_judge_line = - util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx); + util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); const bool is_before_keep_detection_line = stop_lines_idx_opt.has_value() - ? util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx) + ? util::isBeforeTargetIndex(*path, closest_idx, current_pose, keep_detection_line_idx) : false; const bool keep_detection = is_before_keep_detection_line && std::fabs(current_vel) < planner_param_.keep_detection_vel_thr; @@ -172,7 +172,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * RCLCPP_DEBUG(logger_, "===== plan end ====="); setSafe(true); setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_pose.pose.position, + path->points, planner_data_->current_odometry->pose.position, path->points.at(stop_line_idx).point.pose.position)); return true; } @@ -213,7 +213,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * path->points.at(closest_idx).point.pose.position); const double eps = 1e-1; // NOTE: check if sufficiently over the stuck stopline const bool is_over_stuck_stopline = - util::isOverTargetIndex(*path, closest_idx, current_pose.pose, stuck_line_idx) && + util::isOverTargetIndex(*path, closest_idx, current_pose, stuck_line_idx) && dist_stuck_stopline > eps; if (is_stuck && !is_over_stuck_stopline) { stop_line_idx_final = stuck_line_idx; @@ -239,7 +239,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * setSafe(state_machine_.getState() == StateMachine::State::GO); setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_pose.pose.position, + path->points, planner_data_->current_odometry->pose.position, path->points.at(stop_line_idx_final).point.pose.position)); if (!isActivated()) { @@ -268,7 +268,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const auto & stop_pose = path->points.at(stop_line_idx_final).point.pose; velocity_factor_.set( - path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN); + path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); } RCLCPP_DEBUG(logger_, "not activated. stop at the line."); @@ -342,7 +342,7 @@ bool IntersectionModule::checkCollision( const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; const bool is_in_ego_lane = bg::within(to_bg2d(object_pose.position), ego_poly); if (is_in_ego_lane) { - if (!planning_utils::isAheadOf(object_pose, planner_data_->current_pose.pose)) { + if (!planning_utils::isAheadOf(object_pose, planner_data_->current_odometry->pose)) { continue; } if ( diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index d880a6c76dc1f..7eec56b4d0707 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -57,7 +57,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR logger_, "lane_id = %ld, state = %s", lane_id_, StateMachine::toString(current_state).c_str()); /* get current pose */ - geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; + geometry_msgs::msg::Pose current_pose = planner_data_->current_odometry->pose; /* get lanelet map */ const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); @@ -114,10 +114,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR planning_utils::appendStopReason(stop_factor, stop_reason); const auto & stop_pose = path->points.at(stop_line_idx).point.pose; velocity_factor_.set( - path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN); + path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength( - path->points, current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position); + path->points, current_pose.position, path->points.at(stop_line_idx).point.pose.position); constexpr double distance_threshold = 2.0; if ( diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp index 00bd22ddaab97..37763f245e84d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp @@ -50,7 +50,7 @@ void NoStoppingAreaModuleManager::launchNewModules( { for (const auto & m : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), - planner_data_->current_pose.pose)) { + planner_data_->current_odometry->pose)) { // Use lanelet_id to unregister module when the route is changed const int64_t module_id = m.first->id(); const int64_t lane_id = m.second.id(); @@ -72,7 +72,7 @@ NoStoppingAreaModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { const auto no_stopping_area_id_set = planning_utils::getRegElemIdSetOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose); + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); return [no_stopping_area_id_set](const std::shared_ptr & scene_module) { return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp index 1789b6dc6ff3c..a9c9e583bbfd3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp @@ -113,7 +113,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason // Store original path const auto original_path = *path; const auto & predicted_obj_arr_ptr = planner_data_->predicted_objects; - const auto & current_pose = planner_data_->current_pose; + const auto & current_pose = planner_data_->current_odometry; if (path->points.size() <= 2) { return true; } @@ -137,9 +137,9 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason } const auto & stop_pose = stop_point->second; setDistance(motion_utils::calcSignedArcLength( - original_path.points, current_pose.pose.position, stop_pose.position)); + original_path.points, current_pose->pose.position, stop_pose.position)); if (planning_utils::isOverLine( - original_path, current_pose.pose, stop_pose, planner_param_.dead_line_margin)) { + original_path, current_pose->pose, stop_pose, planner_param_.dead_line_margin)) { // ego can't stop in front of no stopping area -> GO or OR state_machine_.setState(StateMachine::State::GO); setSafe(true); @@ -150,12 +150,12 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason const double ego_space_in_front_of_stuck_vehicle = margin + vi.vehicle_length_m + planner_param_.stuck_vehicle_front_margin; const Polygon2d stuck_vehicle_detect_area = generateEgoNoStoppingAreaLanePolygon( - *path, current_pose.pose, ego_space_in_front_of_stuck_vehicle, + *path, current_pose->pose, ego_space_in_front_of_stuck_vehicle, planner_param_.detection_area_length); const double ego_space_in_front_of_stop_line = margin + planner_param_.stop_margin + vi.rear_overhang_m; const Polygon2d stop_line_detect_area = generateEgoNoStoppingAreaLanePolygon( - *path, current_pose.pose, ego_space_in_front_of_stop_line, + *path, current_pose->pose, ego_space_in_front_of_stop_line, planner_param_.detection_area_length); if (stuck_vehicle_detect_area.outer().empty() && stop_line_detect_area.outer().empty()) { setSafe(true); @@ -171,7 +171,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason checkStopLinesInNoStoppingArea(*path, stop_line_detect_area); const bool is_entry_prohibited = is_entry_prohibited_by_stuck_vehicle || is_entry_prohibited_by_stop_line; - if (!isStoppable(current_pose.pose, stop_point->second)) { + if (!isStoppable(current_pose->pose, stop_point->second)) { state_machine_.setState(StateMachine::State::GO); setSafe(true); return false; @@ -195,7 +195,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason stop_factor.stop_factor_points = debug_data_.stuck_points; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( - path->points, planner_data_->current_pose.pose, stop_point->second, + path->points, planner_data_->current_odometry->pose, stop_point->second, VelocityFactor::UNKNOWN); } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 1d09cac0bceeb..ffdf987b3be12 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -100,7 +100,7 @@ bool OcclusionSpotModule::modifyPathVelocity( planner_data_->delay_response_time) + param_.detection_area_offset; // To fill difference between planned and measured acc } - const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose; + const geometry_msgs::msg::Pose ego_pose = planner_data_->current_odometry->pose; PathWithLaneId clipped_path; utils::clipPathByLength(*path, clipped_path, param_.detection_area_length); PathWithLaneId path_interpolated; diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp index 50334e356556b..77c3eca4f8592 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -55,7 +55,7 @@ bool RunOutModule::modifyPathVelocity( // set planner data const auto current_vel = planner_data_->current_velocity->twist.linear.x; const auto current_acc = planner_data_->current_acceleration->accel.accel.linear.x; - const auto & current_pose = planner_data_->current_pose.pose; + const auto & current_pose = planner_data_->current_odometry->pose; // set height of debug data debug_ptr_->setHeight(current_pose.position.z); @@ -164,8 +164,8 @@ Polygons2d RunOutModule::createDetectionAreaPolygon(const PathWithLaneId & smoot Polygons2d detection_area_poly; const size_t ego_seg_idx = findEgoSegmentIndex(smoothed_path.points); planning_utils::createDetectionAreaPolygons( - detection_area_poly, smoothed_path, planner_data_->current_pose.pose, ego_seg_idx, da_range, - p.dynamic_obstacle.max_vel_kmph / 3.6); + detection_area_poly, smoothed_path, planner_data_->current_odometry->pose, ego_seg_idx, + da_range, p.dynamic_obstacle.max_vel_kmph / 3.6); for (const auto & poly : detection_area_poly) { debug_ptr_->pushDetectionAreaPolygons(poly); @@ -663,7 +663,7 @@ void RunOutModule::insertVelocityForState( { using State = run_out_utils::StateMachine::State; - const auto & current_pose = planner_data.current_pose.pose; + const auto & current_pose = planner_data.current_odometry->pose; const auto & current_vel = planner_data.current_velocity->twist.linear.x; const auto & current_acc = planner_data.current_acceleration->accel.accel.linear.x; diff --git a/planning/behavior_velocity_planner/src/scene_module/speed_bump/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/speed_bump/manager.cpp index 14882f0c2a17b..d696642e6b5b7 100644 --- a/planning/behavior_velocity_planner/src/scene_module/speed_bump/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/speed_bump/manager.cpp @@ -54,7 +54,7 @@ void SpeedBumpModuleManager::launchNewModules( { for (const auto & speed_bump_with_lane_id : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), - planner_data_->current_pose.pose)) { + planner_data_->current_odometry->pose)) { const auto lane_id = speed_bump_with_lane_id.second.id(); const auto module_id = speed_bump_with_lane_id.first->id(); if (!isModuleRegistered(module_id)) { @@ -70,7 +70,7 @@ SpeedBumpModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { const auto speed_bump_id_set = planning_utils::getRegElemIdSetOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose); + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); return [speed_bump_id_set](const std::shared_ptr & scene_module) { return speed_bump_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/speed_bump/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/speed_bump/scene.cpp index bfbe86a80c718..49506e5bcc8f2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/speed_bump/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/speed_bump/scene.cpp @@ -79,7 +79,7 @@ bool SpeedBumpModule::modifyPathVelocity( debug_data_ = DebugData(); debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto & speed_bump = speed_bump_reg_elem_.speedBump(); const auto & speed_bump_polygon = lanelet::utils::to2D(speed_bump).basicPolygon(); diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp index 4129494ef8703..b284f3c4d598e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp @@ -45,7 +45,7 @@ std::vector StopLineModuleManager::getStopLinesWithLaneIdOnP std::vector stop_lines_with_lane_id; for (const auto & m : planning_utils::getRegElemMapOnPath( - path, lanelet_map, planner_data_->current_pose.pose)) { + path, lanelet_map, planner_data_->current_odometry->pose)) { const auto & traffic_sign_reg_elem = m.first; const int64_t lane_id = m.second.id(); // Is stop sign? diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp index 0210c716ac514..96e306df62fc0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp @@ -73,8 +73,8 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop path->points, stop_pose.position, stop_point_idx); const size_t current_seg_idx = findEgoSegmentIndex(path->points); const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength( - path->points, planner_data_->current_pose.pose.position, current_seg_idx, stop_pose.position, - stop_line_seg_idx); + path->points, planner_data_->current_odometry->pose.position, current_seg_idx, + stop_pose.position, stop_line_seg_idx); switch (state_) { case State::APPROACH: { // Insert stop pose @@ -91,7 +91,8 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( - path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::APPROACHING); + path->points, planner_data_->current_odometry->pose, stop_pose, + VelocityFactor::APPROACHING); } // Move to stopped state if stopped @@ -115,7 +116,7 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop case State::STOPPED: { // Change state after vehicle departure const auto stopped_pose = motion_utils::calcLongitudinalOffsetPose( - path->points, planner_data_->current_pose.pose.position, 0.0); + path->points, planner_data_->current_odometry->pose.position, 0.0); if (!stopped_pose) { break; @@ -137,7 +138,7 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( - path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::STOPPED); + path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED); } const auto elapsed_time = (clock_->now() - *stopped_time_).seconds(); diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp index 523750c833889..183150954929a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp @@ -112,7 +112,7 @@ void TrafficLightModuleManager::launchNewModules( { for (const auto & traffic_light_reg_elem : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), - planner_data_->current_pose.pose)) { + planner_data_->current_odometry->pose)) { const auto stop_line = traffic_light_reg_elem.first->stopLine(); if (!stop_line) { @@ -141,7 +141,7 @@ TrafficLightModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose); + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); return [lanelet_id_set](const std::shared_ptr & scene_module) { return lanelet_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp index 4f09f89a458d3..45b44e705b6bb 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp @@ -214,7 +214,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const auto input_path = *path; - const auto & self_pose = planner_data_->current_pose; + const auto & self_pose = planner_data_->current_odometry; // Get lanelet2 traffic lights and stop lines. lanelet::ConstLineString3d lanelet_stop_lines = *(traffic_light_reg_elem_.stopLine()); @@ -238,7 +238,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_line_point_msg.x = stop_line_point.x(); stop_line_point_msg.y = stop_line_point.y(); const double signed_arc_length_to_stop_point = motion_utils::calcSignedArcLength( - input_path.points, self_pose.pose.position, stop_line_point_msg); + input_path.points, self_pose->pose.position, stop_line_point_msg); setDistance(signed_arc_length_to_stop_point); // Check state @@ -489,8 +489,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP debug_data_.highest_confidence_traffic_light_point.value()}; } velocity_factor_.set( - modified_path.points, planner_data_->current_pose.pose, target_point_with_lane_id.point.pose, - VelocityFactor::UNKNOWN); + modified_path.points, planner_data_->current_odometry->pose, + target_point_with_lane_id.point.pose, VelocityFactor::UNKNOWN); planning_utils::appendStopReason(stop_factor, stop_reason); return modified_path; diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp index f8ae83dc6f9a7..919c143b216e2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp @@ -48,7 +48,7 @@ void VirtualTrafficLightModuleManager::launchNewModules( { for (const auto & m : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), - planner_data_->current_pose.pose)) { + planner_data_->current_odometry->pose)) { // Use lanelet_id to unregister module when the route is changed const auto lane_id = m.second.id(); const auto module_id = lane_id; @@ -65,7 +65,7 @@ VirtualTrafficLightModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { const auto id_set = planning_utils::getLaneletIdSetOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose); + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); return [id_set](const std::shared_ptr & scene_module) { return id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp index 263daa9c169bc..affc5d7ef8259 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp @@ -256,7 +256,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopRe // Copy data module_data_.head_pose = calcHeadPose( - planner_data_->current_pose.pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); + planner_data_->current_odometry->pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); module_data_.path = *path; // Calculate path index of end line @@ -407,7 +407,7 @@ bool VirtualTrafficLightModule::isBeforeStartLine(const size_t end_line_idx) const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( module_data_.path.points, collision->point, collision->index); - const auto & ego_pose = planner_data_->current_pose.pose; + const auto & ego_pose = planner_data_->current_odometry->pose; const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); const auto signed_arc_length = motion_utils::calcSignedArcLength( module_data_.path.points, ego_pose.position, ego_seg_idx, @@ -430,7 +430,7 @@ bool VirtualTrafficLightModule::isBeforeStopLine(const size_t end_line_idx) const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( module_data_.path.points, collision->point, collision->index); - const auto & ego_pose = planner_data_->current_pose.pose; + const auto & ego_pose = planner_data_->current_odometry->pose; const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); const auto signed_arc_length = motion_utils::calcSignedArcLength( module_data_.path.points, ego_pose.position, ego_seg_idx, @@ -458,7 +458,7 @@ bool VirtualTrafficLightModule::isAfterAnyEndLine(const size_t end_line_idx) const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( module_data_.path.points, collision->point, collision->index); - const auto & ego_pose = planner_data_->current_pose.pose; + const auto & ego_pose = planner_data_->current_odometry->pose; const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); const auto signed_arc_length = motion_utils::calcSignedArcLength( module_data_.path.points, ego_pose.position, ego_seg_idx, @@ -479,7 +479,7 @@ bool VirtualTrafficLightModule::isNearAnyEndLine(const size_t end_line_idx) const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( module_data_.path.points, collision->point, collision->index); - const auto & ego_pose = planner_data_->current_pose.pose; + const auto & ego_pose = planner_data_->current_odometry->pose; const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); const auto signed_arc_length = motion_utils::calcSignedArcLength( module_data_.path.points, ego_pose.position, ego_seg_idx, @@ -535,9 +535,9 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( geometry_msgs::msg::Pose stop_pose{}; if (!collision) { insertStopVelocityFromStart(path); - stop_pose = planner_data_->current_pose.pose; + stop_pose = planner_data_->current_odometry->pose; } else { - const auto & ego_pose = planner_data_->current_pose.pose; + const auto & ego_pose = planner_data_->current_odometry->pose; const size_t ego_seg_idx = findEgoSegmentIndex(path->points); const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( path->points, collision->point, collision->index); @@ -579,7 +579,7 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( // Set StopReason setStopReason(stop_pose, stop_reason); velocity_factor_.set( - path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN, + path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN, command_.type); // Set data for visualization @@ -602,7 +602,7 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( } insertStopVelocityFromStart(path); - stop_pose = planner_data_->current_pose.pose; + stop_pose = planner_data_->current_odometry->pose; } else { const auto offset = -planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto insert_index = insertStopVelocityAtCollision(*collision, offset, path); @@ -614,7 +614,7 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( // Set StopReason setStopReason(stop_pose, stop_reason); velocity_factor_.set( - path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN); + path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); // Set data for visualization module_data_.stop_head_pose_at_end_line = diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index 9e6a2d1fc12ee..262473565daab 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -77,7 +77,6 @@ class MotionVelocitySmootherNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_current_trajectory_; rclcpp::Subscription::SharedPtr sub_external_velocity_limit_; - PoseStamped::ConstSharedPtr current_pose_ptr_; // current vehicle pose Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry VelocityLimit::ConstSharedPtr external_velocity_limit_ptr_{ nullptr}; // external velocity limit message @@ -92,8 +91,6 @@ class MotionVelocitySmootherNode : public rclcpp::Node boost::optional prev_closest_point_{}; boost::optional current_closest_point_from_prev_output_{}; - tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; - bool is_reverse_; enum class AlgorithmType { diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 6bd407b230b48..fdba04a625661 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -116,9 +116,6 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions create_publisher("~/debug/trajectory_steering_rate_limited", 1); pub_trajectory_resampled_ = create_publisher("~/debug/trajectory_time_resampled", 1); - // Wait for first self pose - self_pose_listener_.waitForFirstPose(); - external_velocity_limit_.velocity = node_param_.max_velocity; max_velocity_with_deceleration_ = node_param_.max_velocity; @@ -374,10 +371,10 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit() bool MotionVelocitySmootherNode::checkData() const { - if (!current_pose_ptr_ || !current_odometry_ptr_ || !base_traj_raw_ptr_) { + if (!current_odometry_ptr_ || !base_traj_raw_ptr_) { RCLCPP_DEBUG( - get_logger(), "wait topics : current_pose = %d, current_vel = %d, base_traj = %d", - (bool)current_pose_ptr_, (bool)current_odometry_ptr_, (bool)base_traj_raw_ptr_); + get_logger(), "wait topics : current_vel = %d, base_traj = %d", (bool)current_odometry_ptr_, + (bool)base_traj_raw_ptr_); return false; } if (base_traj_raw_ptr_->points.size() < 2) { @@ -395,8 +392,6 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar base_traj_raw_ptr_ = msg; - current_pose_ptr_ = self_pose_listener_.getCurrentPose(); - // guard if (!checkData()) { return; @@ -405,10 +400,10 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar // calculate prev closest point if (!prev_output_.empty()) { const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_output_, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + prev_output_, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( - prev_output_, current_pose_ptr_->pose, current_seg_idx); + prev_output_, current_odometry_ptr_->pose.pose, current_seg_idx); current_closest_point_from_prev_output_ = closest_point; } @@ -437,7 +432,7 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar // Note that output velocity is resampled by linear interpolation auto output_resampled = resampling::resampleTrajectory( - output, current_odometry_ptr_->twist.twist.linear.x, current_pose_ptr_->pose, + output, current_odometry_ptr_->twist.twist.linear.x, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold, node_param_.post_resample_param, false); @@ -557,7 +552,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( // Resample trajectory with ego-velocity based interval distance auto traj_resampled = smoother_->resampleTrajectory( *traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x, - current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); const size_t traj_resampled_closest = findNearestIndexFromEgo(traj_resampled); @@ -568,7 +563,8 @@ bool MotionVelocitySmootherNode::smoothVelocity( } // Publish Closest Resample Trajectory Velocity - publishClosestVelocity(traj_resampled, current_pose_ptr_->pose, debug_closest_max_velocity_); + publishClosestVelocity( + traj_resampled, current_odometry_ptr_->pose.pose, debug_closest_max_velocity_); // Clip trajectory from closest point TrajectoryPoints clipped; @@ -880,7 +876,7 @@ void MotionVelocitySmootherNode::publishDebugTrajectories( pub_backward_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories_tmp.at(1))); pub_merged_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories_tmp.at(2))); publishClosestVelocity( - debug_trajectories_tmp.at(2), current_pose_ptr_->pose, pub_closest_merged_velocity_); + debug_trajectories_tmp.at(2), current_odometry_ptr_->pose.pose, pub_closest_merged_velocity_); } } @@ -903,10 +899,10 @@ void MotionVelocitySmootherNode::publishClosestVelocity( void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & trajectory) { const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - trajectory, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + trajectory, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( - trajectory, current_pose_ptr_->pose, current_seg_idx); + trajectory, current_odometry_ptr_->pose.pose, current_seg_idx); auto publishFloat = [=](const double data, const auto pub) { Float32Stamped msg{}; @@ -943,10 +939,10 @@ void MotionVelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final { prev_output_ = final_result; const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - final_result, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + final_result, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( - final_result, current_pose_ptr_->pose, current_seg_idx); + final_result, current_odometry_ptr_->pose.pose, current_seg_idx); prev_closest_point_ = closest_point; } @@ -973,10 +969,10 @@ MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorit double MotionVelocitySmootherNode::calcTravelDistance() const { const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_output_, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + prev_output_, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( - prev_output_, current_pose_ptr_->pose, current_seg_idx); + prev_output_, current_odometry_ptr_->pose.pose, current_seg_idx); if (prev_closest_point_) { const double travel_dist = @@ -1005,7 +1001,7 @@ Trajectory MotionVelocitySmootherNode::toTrajectoryMsg( size_t MotionVelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const { return motion_utils::findFirstNearestIndexWithSoftConstraints( - points, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + points, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); } diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index e2a17a2cee0b1..ad0fefe2037ef 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -208,7 +208,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node stop_watch_; // variables for subscribers - std::unique_ptr current_twist_ptr_; + std::unique_ptr current_odometry_ptr_; std::unique_ptr objects_ptr_; // variables for previous information @@ -217,8 +217,6 @@ class ObstacleAvoidancePlanner : public rclcpp::Node std::unique_ptr> prev_path_points_ptr_; std::unique_ptr prev_replanned_time_ptr_; - tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; - // ROS rclcpp::Publisher::SharedPtr traj_pub_; rclcpp::Publisher::SharedPtr debug_extended_fixed_traj_pub_; diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 70e1f6b4db3c6..7ee938c0b98bd 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -571,8 +571,6 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n std::bind(&ObstacleAvoidancePlanner::onParam, this, std::placeholders::_1)); resetPlanning(); - - self_pose_listener_.waitForFirstPose(); } rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( @@ -865,9 +863,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( void ObstacleAvoidancePlanner::onOdometry(const Odometry::SharedPtr msg) { - current_twist_ptr_ = std::make_unique(); - current_twist_ptr_->header = msg->header; - current_twist_ptr_->twist = msg->twist.twist; + current_odometry_ptr_ = std::make_unique(*msg); } void ObstacleAvoidancePlanner::onObjects(const PredictedObjects::SharedPtr msg) @@ -905,7 +901,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::SharedPtr path_ptr) { stop_watch_.tic(__func__); - if (path_ptr->points.empty() || !current_twist_ptr_ || !objects_ptr_) { + if (path_ptr->points.empty() || !current_odometry_ptr_ || !objects_ptr_) { return; } @@ -916,8 +912,8 @@ void ObstacleAvoidancePlanner::onPath(const Path::SharedPtr path_ptr) // create planner data PlannerData planner_data; planner_data.path = *path_ptr; - planner_data.ego_pose = self_pose_listener_.getCurrentPose()->pose; - planner_data.ego_vel = current_twist_ptr_->twist.linear.x; + planner_data.ego_pose = current_odometry_ptr_->pose.pose; + planner_data.ego_vel = current_odometry_ptr_->twist.twist.linear.x; planner_data.objects = objects_ptr_->objects; debug_data_ = DebugData(); diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index c461e112e40e7..d678b77f73043 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -140,7 +140,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; - Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; + Odometry::ConstSharedPtr current_odometry_ptr_{nullptr}; AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_{nullptr}; bool is_driving_forward_{true}; diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index 08a412cce97d9..8c91ec9cbedff 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -194,13 +194,13 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, - const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, + const nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr, bool * need_to_stop, TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header) { debug_values_.data.clear(); debug_values_.data.resize(num_debug_values_, 0.0); - const double current_velocity = current_velocity_ptr->twist.twist.linear.x; + const double current_velocity = current_odometry_ptr->twist.twist.linear.x; double col_point_distance; double point_velocity; bool success_estimate_vel = false; diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 5e2c031925786..2e7a022835663 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -228,7 +228,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m const auto stop_param = stop_param_; const auto obstacle_ros_pointcloud_ptr = obstacle_ros_pointcloud_ptr_; const auto object_ptr = object_ptr_; - const auto current_velocity_ptr = current_velocity_ptr_; + const auto current_odometry_ptr = current_odometry_ptr_; const auto current_acceleration_ptr = current_acceleration_ptr_; mutex_.unlock(); @@ -249,7 +249,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m return; } - if (!current_velocity_ptr) { + if (!current_odometry_ptr) { waiting("current velocity"); return; } @@ -264,7 +264,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m } } - const auto current_vel = current_velocity_ptr->twist.twist.linear.x; + const auto current_vel = current_odometry_ptr->twist.twist.linear.x; const auto current_acc = current_acceleration_ptr->accel.accel.linear.x; // TODO(someone): support backward path @@ -279,7 +279,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m PlannerData planner_data{}; - getSelfPose(input_msg->header, tf_buffer_, planner_data.current_pose); + planner_data.current_pose = current_odometry_ptr_->pose.pose; Trajectory output_trajectory = *input_msg; TrajectoryPoints output_trajectory_points = @@ -450,13 +450,13 @@ void ObstacleStopPlannerNode::searchObstacle( mutex_.lock(); const auto object_ptr = object_ptr_; - const auto current_velocity_ptr = current_velocity_ptr_; + const auto current_odometry_ptr = current_odometry_ptr_; mutex_.unlock(); acc_controller_->insertAdaptiveCruiseVelocity( decimate_trajectory, planner_data.decimate_trajectory_collision_index, planner_data.current_pose, planner_data.nearest_collision_point, - planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, + planner_data.nearest_collision_point_time, object_ptr, current_odometry_ptr, &planner_data.stop_require, &output, trajectory_header); if (!planner_data.stop_require) { @@ -851,7 +851,7 @@ void ObstacleStopPlannerNode::onOdometry(const Odometry::ConstSharedPtr input_ms { // mutex for current_acc_, lpf_acc_ std::lock_guard lock(mutex_); - current_velocity_ptr_ = input_msg; + current_odometry_ptr_ = input_msg; } void ObstacleStopPlannerNode::onAcceleration( diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index 3b3cff45f5c21..1575fbcd13423 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -51,7 +51,6 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node private: tier4_autoware_utils::TransformListener transform_listener_{this}; - tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; rclcpp::Publisher::SharedPtr pub_trajectory_; //!< @brief publisher for output trajectory rclcpp::Publisher::SharedPtr @@ -76,7 +75,7 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node PointCloud::ConstSharedPtr pointcloud_ptr_; lanelet::LaneletMapPtr lanelet_map_ptr_{new lanelet::LaneletMap}; multilinestring_t static_map_obstacles_; - std::optional current_ego_velocity_; + nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr_; // parameters PreprocessingParameters preprocessing_params_; diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index 8b208320dd3c1..15dd0fbadb5bb 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -55,9 +55,8 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio "~/input/dynamic_obstacles", 1, [this](const PredictedObjects::ConstSharedPtr msg) { dynamic_obstacles_ptr_ = msg; }); sub_odom_ = create_subscription( - "~/input/odometry", rclcpp::QoS{1}, [this](const nav_msgs::msg::Odometry::ConstSharedPtr msg) { - current_ego_velocity_ = static_cast(msg->twist.twist.linear.x); - }); + "~/input/odometry", rclcpp::QoS{1}, + [this](const nav_msgs::msg::Odometry::ConstSharedPtr msg) { current_odometry_ptr_ = msg; }); map_sub_ = create_subscription( "~/input/map", rclcpp::QoS{1}.transient_local(), [this](const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) { @@ -171,18 +170,13 @@ rcl_interfaces::msg::SetParametersResult ObstacleVelocityLimiterNode::onParamete void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { const auto t_start = std::chrono::system_clock::now(); - const auto current_pose_ptr = self_pose_listener_.getCurrentPose(); - if (!current_pose_ptr) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), rcutils_duration_value_t(1000), "Waiting for current pose"); - return; - } - const auto ego_idx = motion_utils::findNearestIndex(msg->points, current_pose_ptr->pose); + const auto ego_idx = + motion_utils::findNearestIndex(msg->points, current_odometry_ptr_->pose.pose); if (!validInputs(ego_idx)) return; auto original_traj = *msg; if (preprocessing_params_.calculate_steering_angles) calculateSteeringAngles(original_traj, projection_params_.wheel_base); - velocity_params_.current_ego_velocity = *current_ego_velocity_; + velocity_params_.current_ego_velocity = current_odometry_ptr_->twist.twist.linear.x; const auto start_idx = calculateStartIndex(original_traj, *ego_idx, preprocessing_params_.start_distance); const auto end_idx = calculateEndIndex( @@ -244,11 +238,11 @@ bool ObstacleVelocityLimiterNode::validInputs(const boost::optional & eg if (!ego_idx) RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), one_sec, "Cannot calculate ego index on the trajectory"); - if (!current_ego_velocity_) + if (!current_odometry_ptr_) RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), one_sec, "Current ego velocity not yet received"); - return occupancy_grid_ptr_ && dynamic_obstacles_ptr_ && ego_idx && current_ego_velocity_; + return occupancy_grid_ptr_ && dynamic_obstacles_ptr_ && ego_idx && current_odometry_ptr_; } } // namespace obstacle_velocity_limiter diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index 3999c5cbce50a..2441945567657 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -22,12 +22,11 @@ namespace static_centerline_optimizer { namespace { -geometry_msgs::msg::PoseStamped::ConstSharedPtr convert_to_pose_stamped( - const geometry_msgs::msg::Pose & pose) +nav_msgs::msg::Odometry::ConstSharedPtr convert_to_odometry(const geometry_msgs::msg::Pose & pose) { - auto pose_stamped_ptr = std::make_shared(); - pose_stamped_ptr->pose = pose; - return pose_stamped_ptr; + auto odometry_ptr = std::make_shared(); + odometry_ptr->pose.pose = pose; + return odometry_ptr; } lanelet::Point3d createPoint3d(const double x, const double y, const double z = 19.0) @@ -84,7 +83,7 @@ PathWithLaneId get_path_with_lane_id( // create planner data auto planner_data = std::make_shared(); planner_data->route_handler = std::make_shared(route_handler); - planner_data->self_pose = convert_to_pose_stamped(start_pose); + planner_data->self_odometry = convert_to_odometry(start_pose); planner_data->parameters.ego_nearest_dist_threshold = ego_nearest_dist_threshold; planner_data->parameters.ego_nearest_yaw_threshold = ego_nearest_yaw_threshold; From 5088e8e5c10115ae2cc057d3c7900b60755050f9 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 24 Jan 2023 19:35:22 +0900 Subject: [PATCH 18/60] fix(motion_utils/path_shifter): modify the sampling method (#2658) Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../motion_utils/trajectory/trajectory.hpp | 33 +++++++++++++++++++ .../src/path_utilities.cpp | 6 ++-- 2 files changed, 37 insertions(+), 2 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index f93e758c47332..b6656393a8582 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -437,6 +437,39 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t return dist_sum; } +/** + * @brief Computes the partial sums of the elements in the sub-ranges of + * the range [src_idx, dst_idx) and return these sum as vector + */ +template +std::vector calcSignedArcLengthPartialSum( + const T & points, const size_t src_idx, const size_t dst_idx) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } + + if (src_idx + 1 > dst_idx) { + auto copied = points; + std::reverse(copied.begin(), copied.end()); + return calcSignedArcLengthPartialSum(points, dst_idx, src_idx); + } + + std::vector partial_dist; + partial_dist.reserve(dst_idx - src_idx); + + double dist_sum = 0.0; + partial_dist.push_back(dist_sum); + for (size_t i = src_idx; i < dst_idx - 1; ++i) { + dist_sum += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + partial_dist.push_back(dist_sum); + } + return partial_dist; +} + /** * @brief calcSignedArcLength from point to index */ diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 659006752f45e..a2a3343986576 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -78,8 +78,10 @@ PathWithLaneId resamplePathWithSpline( // Get lane ids that are not duplicated std::vector s_in; std::vector unique_lane_ids; + const auto s_vec = + motion_utils::calcSignedArcLengthPartialSum(transformed_path, 0, transformed_path.size()); for (size_t i = 0; i < path.points.size(); ++i) { - const double s = motion_utils::calcSignedArcLength(transformed_path, 0, i); + const double s = s_vec.at(i); for (const auto & lane_id : path.points.at(i).lane_ids) { if (keep_input_points && !has_almost_same_value(s_in, s)) { s_in.push_back(s); @@ -98,7 +100,7 @@ PathWithLaneId resamplePathWithSpline( std::vector s_out = s_in; const auto start_s = std::max(target_section.first, 0.0); - const auto end_s = std::min(target_section.second, motion_utils::calcArcLength(transformed_path)); + const auto end_s = std::min(target_section.second, s_vec.back()); for (double s = start_s; s < end_s; s += interval) { if (!has_almost_same_value(s_out, s)) { s_out.push_back(s); From 0443b68d33ace7d931f2d7442d22bdfa8ed48f89 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 24 Jan 2023 20:04:23 +0900 Subject: [PATCH 19/60] fix(behavior_path_planner): lane change interpolated obj orientation (#2727) Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_planner/utilities.hpp | 39 ++++++++++++++++++- .../behavior_path_planner/src/utilities.cpp | 30 ++------------ 2 files changed, 41 insertions(+), 28 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 3c9cf8c66ae57..d3baceeb7b415 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -154,8 +154,21 @@ double getArcLengthToTargetLanelet( const Pose & pose); // object collision check - -Pose lerpByPose(const Pose & p1, const Pose & p2, const double t); +inline Pose lerpByPose(const Pose & p1, const Pose & p2, const double t) +{ + tf2::Transform tf_transform1; + tf2::Transform tf_transform2; + tf2::fromMsg(p1, tf_transform1); + tf2::fromMsg(p2, tf_transform2); + const auto & tf_point = tf2::lerp(tf_transform1.getOrigin(), tf_transform2.getOrigin(), t); + const auto & tf_quaternion = + tf2::slerp(tf_transform1.getRotation(), tf_transform2.getRotation(), t); + + Pose pose{}; + pose.position = tf2::toMsg(tf_point, pose.position); + pose.orientation = tf2::toMsg(tf_quaternion); + return pose; +} inline Point lerpByPoint(const Point & p1, const Point & p2, const double t) { @@ -194,6 +207,28 @@ Point lerpByLength(const std::vector & point_array, const double length) return tier4_autoware_utils::getPoint(point_array.back()); } +template +Pose lerpPoseByLength(const std::vector & point_array, const double length) +{ + Pose lerped_pose; + if (point_array.empty()) { + return lerped_pose; + } + Pose prev_geom_pose = tier4_autoware_utils::getPose(point_array.front()); + double accumulated_length = 0; + for (const auto & pt : point_array) { + const auto & geom_pose = tier4_autoware_utils::getPose(pt); + const double distance = tier4_autoware_utils::calcDistance3d(prev_geom_pose, geom_pose); + if (accumulated_length + distance > length) { + return lerpByPose(prev_geom_pose, geom_pose, (length - accumulated_length) / distance); + } + accumulated_length += distance; + prev_geom_pose = geom_pose; + } + + return tier4_autoware_utils::getPose(point_array.back()); +} + bool lerpByTimeStamp(const PredictedPath & path, const double t, Pose * lerped_pt); bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index cec12b798720c..6754a5368ec5d 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -197,10 +197,7 @@ PredictedPath convertToPredictedPath( double prev_vehicle_speed = vehicle_speed; // first point - const auto pt = lerpByLength(path.points, vehicle_pose_frenet.length); - Pose predicted_pose; - predicted_pose.position = pt; - predicted_path.path.push_back(predicted_pose); + predicted_path.path.push_back(lerpPoseByLength(path.points, vehicle_pose_frenet.length)); for (double t = resolution; t < duration; t += resolution) { double accelerated_velocity = prev_vehicle_speed + acceleration * t; @@ -213,10 +210,8 @@ PredictedPath convertToPredictedPath( } length += travel_distance; - const auto pt = lerpByLength(path.points, vehicle_pose_frenet.length + length); - Pose predicted_pose; - predicted_pose.position = pt; - predicted_path.path.push_back(predicted_pose); + predicted_path.path.push_back( + lerpPoseByLength(path.points, vehicle_pose_frenet.length + length)); prev_vehicle_speed = accelerated_velocity; } return predicted_path; @@ -238,21 +233,6 @@ PredictedPath resamplePredictedPath( return resampled_path; } -Pose lerpByPose(const Pose & p1, const Pose & p2, const double t) -{ - tf2::Transform tf_transform1, tf_transform2; - tf2::fromMsg(p1, tf_transform1); - tf2::fromMsg(p2, tf_transform2); - const auto & tf_point = tf2::lerp(tf_transform1.getOrigin(), tf_transform2.getOrigin(), t); - const auto & tf_quaternion = - tf2::slerp(tf_transform1.getRotation(), tf_transform2.getRotation(), t); - - Pose pose{}; - pose.position = tf2::toMsg(tf_point, pose.position); - pose.orientation = tf2::toMsg(tf_quaternion); - return pose; -} - bool lerpByTimeStamp(const PredictedPath & path, const double t_query, Pose * lerped_pt) { const rclcpp::Duration time_step(path.time_step); @@ -2311,13 +2291,12 @@ void getProjectedDistancePointFromPolygons( } bool getEgoExpectedPoseAndConvertToPolygon( - const Pose & current_pose, const PredictedPath & pred_path, + [[maybe_unused]] const Pose & current_pose, const PredictedPath & pred_path, tier4_autoware_utils::Polygon2d & ego_polygon, const double & check_current_time, const VehicleInfo & ego_info, Pose & expected_pose, std::string & failed_reason) { bool is_lerped = util::lerpByTimeStamp(pred_path, check_current_time, &expected_pose, failed_reason); - expected_pose.orientation = current_pose.orientation; const auto & i = ego_info; const auto & front_m = i.max_longitudinal_offset_m; @@ -2336,7 +2315,6 @@ bool getObjectExpectedPoseAndConvertToPolygon( { bool is_lerped = util::lerpByTimeStamp(pred_path, check_current_time, &expected_pose, failed_reason); - expected_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; is_lerped = util::calcObjectPolygon(object.shape, expected_pose, &obj_polygon); return is_lerped; From 43a80d5b9668d582d36bcb69c8623a95943839d6 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 24 Jan 2023 20:06:38 +0900 Subject: [PATCH 20/60] refactor(behavior_path_planner): set occupancy grid map topic name from launch (#2725) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../behavior_planning/behavior_planning.launch.py | 1 + .../behavior_planning/behavior_planning.launch.xml | 1 + .../behavior_path_planner/src/behavior_path_planner_node.cpp | 4 +--- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 755db3597f4c8..aa2d31ff15745 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -67,6 +67,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/route", LaunchConfiguration("input_route_topic_name")), ("~/input/vector_map", LaunchConfiguration("map_topic_name")), ("~/input/perception", "/perception/object_recognition/objects"), + ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"), ("~/input/odometry", "/localization/kinematic_state"), ("~/input/accel", "/localization/acceleration"), ("~/input/scenario", "/planning/scenario_planning/scenario"), diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index e0c51546217c8..8d51ffe89a07c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -7,6 +7,7 @@ + diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index a319217ce6b3b..9847287793725 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -87,10 +87,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod perception_subscriber_ = create_subscription( "~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1), createSubscriptionOptions(this)); - // todo: change to ~/input occupancy_grid_subscriber_ = create_subscription( - "/perception/occupancy_grid_map/map", 1, - std::bind(&BehaviorPathPlannerNode::onOccupancyGrid, this, _1), + "~/input/occupancy_grid_map", 1, std::bind(&BehaviorPathPlannerNode::onOccupancyGrid, this, _1), createSubscriptionOptions(this)); scenario_subscriber_ = create_subscription( "~/input/scenario", 1, From 7d262323d2992d90c9488a3b25fff82b0c812bfb Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 24 Jan 2023 20:14:29 +0900 Subject: [PATCH 21/60] fix(behavior_path_planner): reduce obj indices call in lane change (#2726) * fix(behavior_path_planner): reduce obj indices call in lane change Signed-off-by: Muhammad Zulfaqar Azmi * remove unused functions Signed-off-by: Muhammad Zulfaqar Azmi * add continue for the intersect in current * replace target and current lanes Signed-off-by: Muhammad Zulfaqar Azmi * change isLaneChangePathSafe arguments Signed-off-by: Muhammad Zulfaqar Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar --- .../scene_module/lane_change/util.hpp | 2 +- .../behavior_path_planner/utilities.hpp | 12 -- .../external_request_lane_change_module.cpp | 5 +- .../lane_change/lane_change_module.cpp | 5 +- .../src/scene_module/lane_change/util.cpp | 115 +++++++++++++++--- .../behavior_path_planner/src/utilities.cpp | 66 ---------- 6 files changed, 101 insertions(+), 104 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index dfc5708f10480..d0b83171771ba 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -90,7 +90,7 @@ bool selectSafePath( std::unordered_map & debug_data); bool isLaneChangePathSafe( - const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, + const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const size_t current_seg_idx, const Twist & current_twist, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index d3baceeb7b415..4f9b8300ab87b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -289,14 +289,6 @@ double calcLongitudinalDistanceFromEgoToObjects( const Pose & ego_pose, double base_link2front, double base_link2rear, const PredictedObjects & dynamic_objects); -/** - * @brief Get index of the obstacles inside the lanelets with start and end length - * @return Indices corresponding to the obstacle inside the lanelets - */ -std::vector filterObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & lanelets, - const double start_arc_length, const double end_arc_length); - /** * @brief Separate index of the obstacles into two part based on whether the object is within * lanelet. @@ -313,10 +305,6 @@ std::pair, std::vector> separateObjectIndicesByLanel std::pair separateObjectsByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); -std::vector filterObjectsIndicesByPath( - const PredictedObjects & objects, const std::vector & object_indices, - const PathWithLaneId & ego_path, const double vehicle_width); - PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v); PredictedObjects filterObjectsByVelocity( diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp index 2d82be7a381e3..cb3bafdb65965 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -698,9 +698,8 @@ bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_ path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); return lane_change_utils::isLaneChangePathSafe( - path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, - current_twist, common_parameters, *parameters_, - common_parameters.expected_front_deceleration_for_abort, + path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, + common_parameters, *parameters_, common_parameters.expected_front_deceleration_for_abort, common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, false, status_.lane_change_path.acceleration); } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 1797b5632b444..40d4231036209 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -694,9 +694,8 @@ bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) cons path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); return lane_change_utils::isLaneChangePathSafe( - path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, - current_twist, common_parameters, *parameters_, - common_parameters.expected_front_deceleration_for_abort, + path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, + common_parameters, *parameters_, common_parameters.expected_front_deceleration_for_abort, common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, false, status_.lane_change_path.acceleration); } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 3e683f9b05e10..23e93f803258a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp" +#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" #include "behavior_path_planner/utilities.hpp" @@ -34,6 +35,85 @@ #include #include +namespace +{ + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::util::calcObjectPolygon; +using behavior_path_planner::util::getHighestProbLabel; +using geometry_msgs::msg::Pose; +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +void filterObjectIndices( + const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & ego_path, + const Pose & current_pose, const double forward_path_length, const double filter_width, + std::vector & current_lane_obj_indices, std::vector & target_lane_obj_indices, + std::vector & others_obj_indices, const bool ignore_unknown_obj = false) +{ + // Reserve maximum amount possible + current_lane_obj_indices.reserve(objects.objects.size()); + target_lane_obj_indices.reserve(objects.objects.size()); + others_obj_indices.reserve(objects.objects.size()); + + const auto get_basic_polygon = + [](const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist) { + const auto polygon_3d = lanelet::utils::getPolygonFromArcLength(lanes, start_dist, end_dist); + return lanelet::utils::to2D(polygon_3d).basicPolygon(); + }; + const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose); + const auto current_polygon = + get_basic_polygon(current_lanes, arc.length, arc.length + forward_path_length); + const auto target_polygon = + get_basic_polygon(target_lanes, 0.0, std::numeric_limits::max()); + LineString2d ego_path_linestring; + ego_path_linestring.reserve(ego_path.points.size()); + for (const auto & pt : ego_path.points) { + const auto & position = pt.point.pose.position; + boost::geometry::append(ego_path_linestring, Point2d(position.x, position.y)); + } + + for (size_t i = 0; i < objects.objects.size(); ++i) { + const auto & obj = objects.objects.at(i); + + if (ignore_unknown_obj) { + const auto t = getHighestProbLabel(obj.classification); + if (t == ObjectClassification::UNKNOWN) { + continue; + } + } + + Polygon2d obj_polygon; + if (!calcObjectPolygon(obj, &obj_polygon)) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change"), + "Failed to calcObjectPolygon...!!!"); + continue; + } + + if (boost::geometry::intersects(current_polygon, obj_polygon)) { + const double distance = boost::geometry::distance(obj_polygon, ego_path_linestring); + + if (distance < filter_width) { + current_lane_obj_indices.push_back(i); + continue; + } + } + + const bool is_intersect_with_target = boost::geometry::intersects(target_polygon, obj_polygon); + if (is_intersect_with_target) { + target_lane_obj_indices.push_back(i); + } else { + others_obj_indices.push_back(i); + } + } +} +} // namespace + namespace behavior_path_planner::lane_change_utils { using autoware_auto_planning_msgs::msg::PathPointWithLaneId; @@ -337,7 +417,7 @@ bool selectSafePath( common_parameters.ego_nearest_yaw_threshold); Pose ego_pose_before_collision; if (isLaneChangePathSafe( - path.path, current_lanes, target_lanes, dynamic_objects, current_pose, current_seg_idx, + path, current_lanes, target_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, common_parameters, ros_parameters, common_parameters.expected_front_deceleration, common_parameters.expected_rear_deceleration, ego_pose_before_collision, debug_data, true, @@ -403,7 +483,7 @@ bool hasEnoughDistance( } bool isLaneChangePathSafe( - const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, + const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const size_t current_seg_idx, const Twist & current_twist, @@ -417,6 +497,7 @@ bool isLaneChangePathSafe( return true; } + const auto & path = lane_change_path.path; if (path.points.empty() || target_lanes.empty() || current_lanes.empty()) { return false; } @@ -435,24 +516,20 @@ bool isLaneChangePathSafe( const auto prepare_phase_ignore_target_speed_thresh = lane_change_parameters.prepare_phase_ignore_target_speed_thresh; - const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose); - - // find obstacle in lane change target lanes - // retrieve lanes that are merging target lanes as well - const auto [target_lane_object_indices, others] = - util::separateObjectIndicesByLanelets(*dynamic_objects, target_lanes); - - // find objects in current lane - const double check_distance = common_parameters.forward_path_length; - const auto current_lane_object_indices_lanelet = util::filterObjectIndicesByLanelets( - *dynamic_objects, current_lanes, arc.length, arc.length + check_distance); - - const double lateral_buffer = (use_buffer) ? 0.5 : 0.0; const auto & vehicle_info = common_parameters.vehicle_info; - const auto & vehicle_width = common_parameters.vehicle_width; - const auto current_lane_object_indices = util::filterObjectsIndicesByPath( - *dynamic_objects, current_lane_object_indices_lanelet, path, - vehicle_width / 2 + lateral_buffer); + + std::vector current_lane_object_indices{}; + std::vector target_lane_object_indices{}; + std::vector other_lane_object_indices{}; + { + const auto lateral_buffer = (use_buffer) ? 0.5 : 0.0; + const auto current_obj_filtering_buffer = lateral_buffer + common_parameters.vehicle_width / 2; + + filterObjectIndices( + *dynamic_objects, lane_change_path.reference_lanelets, lane_change_path.target_lanelets, path, + current_pose, common_parameters.forward_path_length, current_obj_filtering_buffer, + current_lane_object_indices, target_lane_object_indices, other_lane_object_indices, true); + } const auto assignDebugData = [](const PredictedObject & obj) { CollisionCheckDebug debug; diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 6754a5368ec5d..156bca488cc58 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -490,48 +490,6 @@ double calcLongitudinalDistanceFromEgoToObjects( return min_distance; } -// only works with consecutive lanes -std::vector filterObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, - const double start_arc_length, const double end_arc_length) -{ - std::vector indices; - if (target_lanelets.empty()) { - return {}; - } - const auto polygon = - lanelet::utils::getPolygonFromArcLength(target_lanelets, start_arc_length, end_arc_length); - const auto polygon2d = lanelet::utils::to2D(polygon).basicPolygon(); - if (polygon2d.empty()) { - // no lanelet polygon - return {}; - } - - for (size_t i = 0; i < objects.objects.size(); i++) { - const auto & obj = objects.objects.at(i); - // create object polygon - Polygon2d obj_polygon; - if (!calcObjectPolygon(obj, &obj_polygon)) { - continue; - } - // create lanelet polygon - Polygon2d lanelet_polygon; - lanelet_polygon.outer().reserve(polygon2d.size() + 1); - for (const auto & lanelet_point : polygon2d) { - lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); - } - - lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); - - // check the object does not intersect the lanelet - if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { - indices.push_back(i); - continue; - } - } - return indices; -} - std::pair, std::vector> separateObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) { @@ -674,30 +632,6 @@ std::vector calcObjectsDistanceToPath( return distance_array; } -std::vector filterObjectsIndicesByPath( - const PredictedObjects & objects, const std::vector & object_indices, - const PathWithLaneId & ego_path, const double vehicle_width) -{ - std::vector indices; - const auto ego_path_point_array = convertToGeometryPointArray(ego_path); - for (const auto & i : object_indices) { - Polygon2d obj_polygon; - if (!calcObjectPolygon(objects.objects.at(i), &obj_polygon)) { - continue; - } - LineString2d ego_path_line; - ego_path_line.reserve(ego_path_point_array.size()); - for (const auto & ego_path_point : ego_path_point_array) { - boost::geometry::append(ego_path_line, Point2d(ego_path_point.x, ego_path_point.y)); - } - const double distance = boost::geometry::distance(obj_polygon, ego_path_line); - if (distance < vehicle_width) { - indices.push_back(i); - } - } - return indices; -} - PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) { PathWithLaneId filtered_path; From 663cc03aaee49bb59c29542289063d3f889eafac Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 24 Jan 2023 20:35:08 +0900 Subject: [PATCH 22/60] feat(behavior_path_planner): enable lane change in intersection (#2720) fix(behavior_path_planner): enable lane change in intersection Signed-off-by: Fumiya Watanabe Signed-off-by: Fumiya Watanabe --- .../external_request_lane_change_module.cpp | 14 +-- .../lane_change/lane_change_module.cpp | 15 +-- .../src/scene_module/lane_change/util.cpp | 29 +----- .../scene_module/intersection/manager.hpp | 4 + .../scene_module/traffic_light/manager.hpp | 6 ++ .../src/scene_module/intersection/manager.cpp | 93 +++++++++++++++++-- .../scene_module/traffic_light/manager.cpp | 44 ++++++++- .../include/route_handler/route_handler.hpp | 1 + planning/route_handler/src/route_handler.cpp | 6 ++ 9 files changed, 155 insertions(+), 57 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp index cb3bafdb65965..1f489583c4708 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -297,18 +297,12 @@ PathWithLaneId ExternalRequestLaneChangeModule::getReferencePath() const const int num_lane_change = std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back())); - double optional_lengths{0.0}; - const auto isInIntersection = util::checkLaneIsInIntersection( - *route_handler, reference_path, current_lanes, common_parameters, num_lane_change, - optional_lengths); - if (isInIntersection) { - reference_path = util::getCenterLinePath( - *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters, optional_lengths); - } + reference_path = util::getCenterLinePath( + *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, + common_parameters.forward_path_length, common_parameters, 0.0); const double lane_change_buffer = - util::calcLaneChangeBuffer(common_parameters, num_lane_change, optional_lengths); + util::calcLaneChangeBuffer(common_parameters, num_lane_change, 0.0); reference_path = util::setDecelerationVelocity( *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 40d4231036209..7c98dca621bf7 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -305,18 +305,13 @@ PathWithLaneId LaneChangeModule::getReferencePath() const const int num_lane_change = std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back())); - double optional_lengths{0.0}; - const auto isInIntersection = util::checkLaneIsInIntersection( - *route_handler, reference_path, current_lanes, common_parameters, num_lane_change, - optional_lengths); - if (isInIntersection) { - reference_path = util::getCenterLinePath( - *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters, optional_lengths); - } + + reference_path = util::getCenterLinePath( + *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, + common_parameters.forward_path_length, common_parameters, 0.0); const double lane_change_buffer = - util::calcLaneChangeBuffer(common_parameters, num_lane_change, optional_lengths); + util::calcLaneChangeBuffer(common_parameters, num_lane_change, 0.0); reference_path = util::setDecelerationVelocity( *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 23e93f803258a..2dceb4f16aec1 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -455,12 +455,6 @@ bool hasEnoughDistance( return false; } - if ( - lane_change_total_distance + lane_change_required_distance > - util::getDistanceToNextIntersection(current_pose, current_lanes)) { - return false; - } - if ( route_handler.isInGoalRouteSection(current_lanes.back()) && lane_change_total_distance + lane_change_required_distance > @@ -468,17 +462,15 @@ bool hasEnoughDistance( return false; } - if ( - lane_change_total_distance + lane_change_required_distance > - util::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs)) { - return false; - } - // return is there is no target lanes. Else continue checking if (target_lanes.empty()) { return true; } + if (lane_change_total_distance > util::getDistanceToEndOfLane(current_pose, target_lanes)) { + return false; + } + return true; } @@ -1002,12 +994,6 @@ bool hasEnoughDistanceToLaneChangeAfterAbort( return false; } - if ( - abort_plus_lane_change_distance > - util::getDistanceToNextIntersection(current_pose, current_lanes)) { - return false; - } - if ( route_handler.isInGoalRouteSection(current_lanes.back()) && abort_plus_lane_change_distance > @@ -1015,13 +1001,6 @@ bool hasEnoughDistanceToLaneChangeAfterAbort( return false; } - if ( - abort_plus_lane_change_distance > - util::getDistanceToCrosswalk( - current_pose, current_lanes, *route_handler.getOverallGraphPtr())) { - return false; - } - return true; } } // namespace behavior_path_planner::lane_change_utils diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp index bcbe4a4bba40a..c42a311ffa623 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp @@ -42,6 +42,8 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + bool hasSameParentLanelet(const lanelet::ConstLanelet & lane) const; }; class MergeFromPrivateModuleManager : public SceneModuleManagerInterface @@ -58,6 +60,8 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + bool hasSameParentLanelet(const lanelet::ConstLanelet & lane) const; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp index 6bdc554fff50c..a27d8b8d9e3c7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp @@ -43,6 +43,12 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override; + bool isModuleRegisteredFromRegElement(const lanelet::Id & id) const; + + bool hasSameTrafficLight( + const lanelet::TrafficLightConstPtr element, + const lanelet::TrafficLightConstPtr registered_element) const; + // Debug rclcpp::Publisher::SharedPtr pub_tl_state_; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index c1159b1bd9eee..5a35fb4c888de 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -26,6 +26,18 @@ namespace behavior_velocity_planner { +bool hasSameLanelet(const lanelet::ConstLanelets & lanes1, const lanelet::ConstLanelets & lanes2) +{ + for (const auto & lane1 : lanes1) { + for (const auto & lane2 : lanes2) { + if (lane1.id() == lane2.id()) { + return true; + } + } + } + return false; +} + IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC(node, getModuleName()) { @@ -84,7 +96,7 @@ void IntersectionModuleManager::launchNewModules( const auto lane_id = ll.id(); const auto module_id = lane_id; - if (isModuleRegistered(module_id)) { + if (hasSameParentLanelet(ll)) { continue; } @@ -161,23 +173,86 @@ std::function &)> IntersectionModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto lane_id_set = planning_utils::getLaneIdSetOnPath( + const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [lane_id_set](const std::shared_ptr & scene_module) { - return lane_id_set.count(scene_module->getModuleId()) == 0; - }; + return + [this, lane_set]([[maybe_unused]] const std::shared_ptr & scene_module) { + for (const auto & lane : lane_set) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + if (hasSameParentLanelet(lane)) { + return false; + } + } + return true; + }; } + std::function &)> MergeFromPrivateModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto lane_id_set = planning_utils::getLaneIdSetOnPath( + const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [lane_id_set](const std::shared_ptr & scene_module) { - return lane_id_set.count(scene_module->getModuleId()) == 0; - }; + return + [this, lane_set]([[maybe_unused]] const std::shared_ptr & scene_module) { + for (const auto & lane : lane_set) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + if (hasSameParentLanelet(lane)) { + return false; + } + } + return true; + }; +} + +bool IntersectionModuleManager::hasSameParentLanelet(const lanelet::ConstLanelet & lane) const +{ + lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); + + for (const auto & id : registered_module_id_set_) { + const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(id); + lanelet::ConstLanelets registered_parents = + planner_data_->route_handler_->getPreviousLanelets(registered_lane); + for (const auto & ll : registered_parents) { + auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); + neighbor_lanes.push_back(ll); + if (hasSameLanelet(parents, neighbor_lanes)) { + return true; + } + } + } + return false; +} + +bool MergeFromPrivateModuleManager::hasSameParentLanelet(const lanelet::ConstLanelet & lane) const +{ + lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); + + for (const auto & id : registered_module_id_set_) { + const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(id); + lanelet::ConstLanelets registered_parents = + planner_data_->route_handler_->getPreviousLanelets(registered_lane); + for (const auto & ll : registered_parents) { + auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); + neighbor_lanes.push_back(ll); + if (hasSameLanelet(parents, neighbor_lanes)) { + return true; + } + } + } + return false; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp index 183150954929a..363cb191d3405 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp @@ -125,7 +125,7 @@ void TrafficLightModuleManager::launchNewModules( // Use lanelet_id to unregister module when the route is changed const auto lane_id = traffic_light_reg_elem.second.id(); const auto module_id = lane_id; - if (!isModuleRegistered(module_id)) { + if (!isModuleRegisteredFromRegElement(module_id)) { registerModule(std::make_shared( module_id, lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, logger_.get_child("traffic_light_module"), clock_)); @@ -143,9 +143,47 @@ TrafficLightModuleManager::getModuleExpiredFunction( const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [lanelet_id_set](const std::shared_ptr & scene_module) { - return lanelet_id_set.count(scene_module->getModuleId()) == 0; + return [this, lanelet_id_set]( + [[maybe_unused]] const std::shared_ptr & scene_module) { + for (const auto & id : lanelet_id_set) { + if (isModuleRegisteredFromRegElement(id)) { + return false; + } + } + return true; }; } +bool TrafficLightModuleManager::isModuleRegisteredFromRegElement(const lanelet::Id & id) const +{ + const auto lane = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(id); + + for (const auto & registered_id : registered_module_id_set_) { + const auto registered_lane = + planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(registered_id); + for (const auto & registered_element : registered_lane.regulatoryElementsAs()) { + for (const auto & element : lane.regulatoryElementsAs()) { + if (hasSameTrafficLight(element, registered_element)) { + return true; + } + } + } + } + return false; +} + +bool TrafficLightModuleManager::hasSameTrafficLight( + const lanelet::TrafficLightConstPtr element, + const lanelet::TrafficLightConstPtr registered_element) const +{ + for (const auto & traffic_light : element->trafficLights()) { + for (const auto & registered_traffic_light : registered_element->trafficLights()) { + if (traffic_light.id() == registered_traffic_light.id()) { + return true; + } + } + } + return false; +} + } // namespace behavior_velocity_planner diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index c1e3fd561eaa0..1a340c3d031c0 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -105,6 +105,7 @@ class RouteHandler const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const; bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getLaneletsFromPoint(const lanelet::ConstPoint3d & point) const; + lanelet::ConstLanelets getLaneChangeableNeighbors(const lanelet::ConstLanelet & lanelet) const; /** * @brief Check if same-direction lane is available at the right side of the lanelet diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index c41f6c9be3729..b1fc3c33a572b 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -428,6 +428,12 @@ bool RouteHandler::isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const return !getNextLaneletWithinRoute(lanelet, &next_lanelet); } +lanelet::ConstLanelets RouteHandler::getLaneChangeableNeighbors( + const lanelet::ConstLanelet & lanelet) const +{ + return lanelet::utils::query::getLaneChangeableNeighbors(routing_graph_ptr_, lanelet); +} + lanelet::ConstLanelets RouteHandler::getLaneletSequenceAfter( const lanelet::ConstLanelet & lanelet, const double min_length) const { From 36c4b10d8d648f76420d73c8b2e06ea6bea1153e Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Tue, 24 Jan 2023 20:52:43 +0900 Subject: [PATCH 23/60] refactor(ekf_localizer): add a function to generate a delay time warning message (#2694) * Add a function to generate a delay time warning message * Add a warning message function for each twist and pose Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../include/ekf_localizer/warning_message.hpp | 2 ++ .../ekf_localizer/src/ekf_localizer.cpp | 22 ++++++------------- .../ekf_localizer/src/warning_message.cpp | 15 ++++++++++++- .../test/test_warning_message.cpp | 20 +++++++++++++++++ 4 files changed, 43 insertions(+), 16 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp index 019fe0825013c..dbe44df702af0 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -17,6 +17,8 @@ #include +std::string poseDelayTimeWarningMessage(const double delay_time); +std::string twistDelayTimeWarningMessage(const double delay_time); std::string mahalanobisWarningMessage(const double distance, const double max_distance); #endif // EKF_LOCALIZER__WARNING_MESSAGE_HPP_ diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 580262f08a3eb..5243015a64bae 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -377,15 +377,6 @@ void EKFLocalizer::initEKF() ekf_.init(X, P, params_.extend_state_step); } -void warnIfPoseDelayTimeLessThanZero(const Warning & warning, const double delay_time) -{ - if (delay_time < 0.0) { - const auto s = - fmt::format("Pose time stamp is inappropriate, set delay to 0[s]. delay = {}", delay_time); - warning.warnThrottle(s, 1000); - } -} - /* * measurementUpdatePose */ @@ -406,7 +397,10 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar /* Calculate delay step */ double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay; - warnIfPoseDelayTimeLessThanZero(warning_, delay_time); + if (delay_time < 0.0) { + warning_.warnThrottle(poseDelayTimeWarningMessage(delay_time), 1000); + } + delay_time = std::max(delay_time, 0.0); int delay_step = std::roundf(delay_time / ekf_dt_); @@ -488,12 +482,10 @@ void EKFLocalizer::measurementUpdateTwist( /* Calculate delay step */ double delay_time = (t_curr - twist.header.stamp).seconds() + params_.twist_additional_delay; if (delay_time < 0.0) { - warning_.warnThrottle( - fmt::format( - "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].", delay_time), - 1000); - delay_time = 0.0; + warning_.warnThrottle(twistDelayTimeWarningMessage(delay_time), 1000); } + delay_time = std::max(delay_time, 0.0); + int delay_step = std::roundf(delay_time / ekf_dt_); if (delay_step > params_.extend_state_step - 1) { warning_.warnThrottle( diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp index 5dbeaa847509a..1088d5c061375 100644 --- a/localization/ekf_localizer/src/warning_message.cpp +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -15,7 +15,20 @@ #include "ekf_localizer/warning_message.hpp" #include -#include + +#include + +std::string poseDelayTimeWarningMessage(const double delay_time) +{ + const std::string s = "Pose time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}"; + return fmt::format(s, delay_time); +} + +std::string twistDelayTimeWarningMessage(const double delay_time) +{ + const std::string s = "Twist time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}"; + return fmt::format(s, delay_time); +} std::string mahalanobisWarningMessage(const double distance, const double max_distance) { diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp index d499a80f2acd5..fc86df0d6cd80 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -16,6 +16,26 @@ #include +TEST(PoseDelayTimeWarningMessage, SmokeTest) +{ + EXPECT_STREQ( + poseDelayTimeWarningMessage(-1.0).c_str(), + "Pose time stamp is inappropriate, set delay to 0[s]. delay = -1.000"); + EXPECT_STREQ( + poseDelayTimeWarningMessage(-0.4).c_str(), + "Pose time stamp is inappropriate, set delay to 0[s]. delay = -0.400"); +} + +TEST(TwistDelayTimeWarningMessage, SmokeTest) +{ + EXPECT_STREQ( + twistDelayTimeWarningMessage(-1.0).c_str(), + "Twist time stamp is inappropriate, set delay to 0[s]. delay = -1.000"); + EXPECT_STREQ( + twistDelayTimeWarningMessage(-0.4).c_str(), + "Twist time stamp is inappropriate, set delay to 0[s]. delay = -0.400"); +} + TEST(MahalanobisWarningMessage, SmokeTest) { EXPECT_STREQ( From 403713d249b9500f0ff470d5240883a741996e0e Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 24 Jan 2023 21:08:31 +0900 Subject: [PATCH 24/60] fix(behavior_path_planner): make lane change safety check adaptive (#2704) * fix(behavior_path_planner): make lane change safety check adaptive Signed-off-by: Muhammad Zulfaqar Azmi * Temporarily hard code use all predicted path Signed-off-by: Muhammad Zulfaqar Azmi * Revert "Temporarily hard code use all predicted path" This reverts commit 8f92e4511d7f91ce4492847ea87fda1da696d6d8. * fix external lane change request Signed-off-by: Muhammad Zulfaqar * use prediction resolution as rounding multiplier Signed-off-by: Muhammad Zulfaqar Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar --- .../config/lane_change/lane_change.param.yaml | 1 - .../lane_change/lane_change_module_data.hpp | 14 ------- .../lane_change/lane_change_path.hpp | 18 ++++++++- .../external_request_lane_change_module.cpp | 13 +++---- .../lane_change/lane_change_module.cpp | 15 +++----- .../src/scene_module/lane_change/util.cpp | 38 ++++++++++++++----- 6 files changed, 56 insertions(+), 43 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 41de868418491..2a025be8765b4 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -2,7 +2,6 @@ ros__parameters: lane_change: lane_change_prepare_duration: 4.0 # [s] - lane_changing_safety_check_duration: 8.0 # [s] minimum_lane_change_prepare_distance: 2.0 # [m] minimum_lane_change_length: 16.5 # [m] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp index 77a7f6925a01f..9acb136a86f55 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp @@ -14,7 +14,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ -#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "lanelet2_core/geometry/Lanelet.h" #include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" @@ -59,19 +58,6 @@ struct LaneChangeParameters bool publish_debug_marker{false}; }; -struct LaneChangeStatus -{ - PathWithLaneId lane_follow_path; - LaneChangePath lane_change_path; - lanelet::ConstLanelets current_lanes; - lanelet::ConstLanelets lane_change_lanes; - std::vector lane_follow_lane_ids; - std::vector lane_change_lane_ids; - bool is_safe; - bool is_valid_path = true; - double start_distance; -}; - enum class LaneChangeStates { Normal = 0, Cancel, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp index 83de5e24d5dba..caefa3d357ca4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ +#include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" @@ -34,12 +35,25 @@ struct LaneChangePath ShiftedPath shifted_path; ShiftLine shift_line; double acceleration{0.0}; - double preparation_length{0.0}; - double lane_change_length{0.0}; + LaneChangePhaseInfo length{}; + LaneChangePhaseInfo duration{}; TurnSignalInfo turn_signal_info; PathWithLaneId prev_path; }; using LaneChangePaths = std::vector; +struct LaneChangeStatus +{ + PathWithLaneId lane_follow_path; + LaneChangePath lane_change_path; + lanelet::ConstLanelets current_lanes; + lanelet::ConstLanelets lane_change_lanes; + std::vector lane_follow_lane_ids; + std::vector lane_change_lane_ids; + bool is_safe; + bool is_valid_path = true; + double start_distance; +}; + } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp index 1f489583c4708..0a3fe546212b5 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -382,8 +382,7 @@ std::pair ExternalRequestLaneChangeModule::getSafePath( if (!lane_change_paths.empty()) { const auto & longest_path = lane_change_paths.front(); // we want to see check_distance [m] behind vehicle so add lane changing length - const double check_distance_with_path = - check_distance + longest_path.preparation_length + longest_path.lane_change_length; + const double check_distance_with_path = check_distance + longest_path.length.sum(); check_lanes = route_handler->getCheckTargetLanesFromPath( longest_path.path, lane_change_lanes, check_distance_with_path); } @@ -557,9 +556,8 @@ bool ExternalRequestLaneChangeModule::hasFinishedLaneChange() const const auto arclength_current = lanelet::utils::getArcCoordinates(status_.lane_change_lanes, current_pose); const double travel_distance = arclength_current.length - status_.start_distance; - const double finish_distance = status_.lane_change_path.preparation_length + - status_.lane_change_path.lane_change_length + - parameters_->lane_change_finish_judge_buffer; + const double finish_distance = + status_.lane_change_path.length.sum() + parameters_->lane_change_finish_judge_buffer; return travel_distance > finish_distance; } @@ -681,8 +679,7 @@ bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_ constexpr double check_distance = 100.0; // get lanes used for detection - const double check_distance_with_path = - check_distance + path.preparation_length + path.lane_change_length; + const double check_distance_with_path = check_distance + path.length.sum(); const auto check_lanes = route_handler->getCheckTargetLanesFromPath( path.path, status_.lane_change_lanes, check_distance_with_path); @@ -745,7 +742,7 @@ void ExternalRequestLaneChangeModule::calcTurnSignalInfo() turn_signal_info.desired_end_point = path.shift_line.end; turn_signal_info.required_start_point = path.shift_line.start; - const auto mid_lane_change_length = path.lane_change_length / 2; + const auto mid_lane_change_length = path.length.lane_changing / 2; const auto & shifted_path = path.shifted_path.path; turn_signal_info.required_end_point = get_blinker_pose(shifted_path, path.target_lanelets, mid_lane_change_length); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 7c98dca621bf7..1f0d4cdbbc755 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -382,8 +382,7 @@ std::pair LaneChangeModule::getSafePath( if (!lane_change_paths.empty()) { const auto & longest_path = lane_change_paths.front(); // we want to see check_distance [m] behind vehicle so add lane changing length - const double check_distance_with_path = - check_distance + longest_path.preparation_length + longest_path.lane_change_length; + const double check_distance_with_path = check_distance + longest_path.length.sum(); check_lanes = route_handler->getCheckTargetLanesFromPath( longest_path.path, lane_change_lanes, check_distance_with_path); } @@ -557,9 +556,8 @@ bool LaneChangeModule::hasFinishedLaneChange() const const auto arclength_current = lanelet::utils::getArcCoordinates(status_.lane_change_lanes, current_pose); const double travel_distance = arclength_current.length - status_.start_distance; - const double finish_distance = status_.lane_change_path.preparation_length + - status_.lane_change_path.lane_change_length + - parameters_->lane_change_finish_judge_buffer; + const double finish_distance = + status_.lane_change_path.length.sum() + parameters_->lane_change_finish_judge_buffer; return travel_distance > finish_distance; } @@ -674,12 +672,11 @@ bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) cons const auto & current_lanes = status_.current_lanes; const auto & common_parameters = planner_data_->parameters; const auto & route_handler = planner_data_->route_handler; - const auto path = status_.lane_change_path; + const auto & path = status_.lane_change_path; constexpr double check_distance = 100.0; // get lanes used for detection - const double check_distance_with_path = - check_distance + path.preparation_length + path.lane_change_length; + const double check_distance_with_path = check_distance + path.length.sum(); const auto check_lanes = route_handler->getCheckTargetLanesFromPath( path.path, status_.lane_change_lanes, check_distance_with_path); @@ -742,7 +739,7 @@ void LaneChangeModule::calcTurnSignalInfo() turn_signal_info.desired_end_point = path.shift_line.end; turn_signal_info.required_start_point = path.shift_line.start; - const auto mid_lane_change_length = path.lane_change_length / 2; + const auto mid_lane_change_length = path.length.prepare / 2; const auto & shifted_path = path.shifted_path.path; turn_signal_info.required_end_point = get_blinker_pose(shifted_path, path.target_lanelets, mid_lane_change_length); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 2dceb4f16aec1..4212b368cf33a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -208,8 +208,18 @@ std::optional constructCandidatePath( LaneChangePath candidate_path; candidate_path.acceleration = acceleration; - candidate_path.preparation_length = prepare_distance; - candidate_path.lane_change_length = lane_change_distance; + candidate_path.length.prepare = prepare_distance; + candidate_path.length.lane_changing = lane_change_distance; + candidate_path.duration.prepare = std::invoke([&]() { + const auto duration = + prepare_distance / std::max(lane_change_param.minimum_lane_change_velocity, speed.prepare); + return std::min(duration, lane_change_param.lane_change_prepare_duration); + }); + candidate_path.duration.lane_changing = std::invoke([&]() { + const auto rounding_multiplier = 1.0 / lane_change_param.prediction_time_resolution; + return std::ceil((lane_change_distance / lane_changing_speed) * rounding_multiplier) / + rounding_multiplier; + }); candidate_path.shift_line = shift_line; candidate_path.reference_lanelets = original_lanelets; candidate_path.target_lanelets = target_lanelets; @@ -440,9 +450,7 @@ bool hasEnoughDistance( const Pose & goal_pose, const RouteHandler & route_handler, const double minimum_lane_change_length) { - const double & lane_change_prepare_distance = path.preparation_length; - const double & lane_changing_distance = path.lane_change_length; - const double lane_change_total_distance = lane_change_prepare_distance + lane_changing_distance; + const double lane_change_total_distance = path.length.sum(); const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back())); const auto overall_graphs = route_handler.getOverallGraphPtr(); @@ -495,15 +503,27 @@ bool isLaneChangePathSafe( } const double time_resolution = lane_change_parameters.prediction_time_resolution; - const auto & lane_change_prepare_duration = lane_change_parameters.lane_change_prepare_duration; + const auto & lane_change_prepare_duration = lane_change_path.duration.prepare; const auto & enable_collision_check_at_prepare_phase = lane_change_parameters.enable_collision_check_at_prepare_phase; - const auto & lane_changing_safety_check_duration = - lane_change_parameters.lane_changing_safety_check_duration; + const auto & lane_changing_safety_check_duration = lane_change_path.duration.lane_changing; const double check_end_time = lane_change_prepare_duration + lane_changing_safety_check_duration; const double min_lc_speed{lane_change_parameters.minimum_lane_change_velocity}; + + const auto get_pose = std::invoke([&]() { + Pose p; + double dist{0.0}; + for (size_t i = 1; i < path.points.size(); ++i) { + dist += motion_utils::calcSignedArcLength(path.points, i - 1, i); + if (dist >= common_parameters.backward_path_length) { + return path.points.at(i).point.pose; + } + } + return path.points.front().point.pose; + }); + const auto vehicle_predicted_path = util::convertToPredictedPath( - path, current_twist, current_pose, static_cast(current_seg_idx), check_end_time, + path, current_twist, get_pose, static_cast(current_seg_idx), check_end_time, time_resolution, acceleration, min_lc_speed); const auto prepare_phase_ignore_target_speed_thresh = lane_change_parameters.prepare_phase_ignore_target_speed_thresh; From dd022c4cd545bbf18fa04afc92d513408bed579f Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Tue, 24 Jan 2023 17:04:21 +0300 Subject: [PATCH 25/60] feat(lane_departure_checker): add braking distance offset to avoid unwanted move after stop (#2712) Signed-off-by: Berkay Karaman --- .../config/lane_departure_checker.param.yaml | 1 + .../lane_departure_checker/lane_departure_checker.hpp | 1 + .../lane_departure_checker_node/lane_departure_checker.cpp | 5 +++-- .../lane_departure_checker_node.cpp | 2 ++ 4 files changed, 7 insertions(+), 2 deletions(-) diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/lane_departure_checker/config/lane_departure_checker.param.yaml index 092a6765aa948..9512ed388daaf 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/lane_departure_checker/config/lane_departure_checker.param.yaml @@ -13,3 +13,4 @@ max_longitudinal_deviation: 2.0 max_yaw_deviation_deg: 60.0 delta_yaw_threshold_for_closest_point: 1.570 #M_PI/2.0, delta yaw thres for closest point + min_braking_distance: 0.0 diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index fd21d18b98a64..9605f53cde1d3 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -58,6 +58,7 @@ struct Param double max_lateral_deviation; double max_longitudinal_deviation; double max_yaw_deviation_deg; + double min_braking_distance; // nearest search to ego double ego_nearest_dist_threshold; double ego_nearest_yaw_threshold; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index e3e753d9e4023..1d594180a638b 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -109,8 +109,9 @@ Output LaneDepartureChecker::update(const Input & input) const auto & raw_abs_velocity = std::abs(input.current_odom->twist.twist.linear.x); const auto abs_velocity = raw_abs_velocity < min_velocity ? 0.0 : raw_abs_velocity; - const auto braking_distance = - calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time); + const auto braking_distance = std::max( + param_.min_braking_distance, + calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time)); output.resampled_trajectory = cutTrajectory( resampleTrajectory(*input.predicted_trajectory, param_.resample_interval), braking_distance); diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index f0274e5c4712f..661a189ef20bb 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -144,6 +144,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o param_.max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg", 30.0); param_.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); param_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + param_.min_braking_distance = declare_parameter("min_braking_distance"); // Parameter Callback set_param_res_ = @@ -367,6 +368,7 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( update_param(parameters, "resample_interval", param_.resample_interval); update_param(parameters, "max_deceleration", param_.max_deceleration); update_param(parameters, "delay_time", param_.delay_time); + update_param(parameters, "min_braking_distance", param_.min_braking_distance); if (lane_departure_checker_) { lane_departure_checker_->setParam(param_); From 66f8342e697abbff907e78daf09cdfab3f18800e Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 24 Jan 2023 23:05:53 +0900 Subject: [PATCH 26/60] feat(intersection): consider state transit margin time in collision detection (#2719) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 9 ++- .../intersection/scene_intersection.hpp | 16 ++--- .../scene_module/intersection/util.hpp | 4 +- .../src/scene_module/intersection/manager.cpp | 4 +- .../intersection/scene_intersection.cpp | 40 +++++------ .../scene_merge_from_private_road.cpp | 3 +- .../src/scene_module/intersection/util.cpp | 68 ++++++------------- 7 files changed, 50 insertions(+), 94 deletions(-) diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 220c087d18de0..a769848497680 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -1,10 +1,8 @@ /**: ros__parameters: intersection: - state_transit_margin_time: 0.5 + state_transit_margin_time: 1.0 stop_line_margin: 3.0 - keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr - keep_detection_vel_thr: 0.833 # == 3.0km/h stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h @@ -16,8 +14,9 @@ detection_area_length: 200.0 # [m] detection_area_angle_threshold: 0.785 # [rad] min_predicted_path_confidence: 0.05 - collision_start_margin_time: 5.0 # [s] - collision_end_margin_time: 2.0 # [s] + minimum_ego_predicted_velocity: 1.388 # [m/s] + collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object + collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning enable_front_car_decel_prediction: false # By default this feature is disabled diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index c1097f39bbf01..f8e0fea2f23de 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -69,10 +69,6 @@ class IntersectionModule : public SceneModuleInterface { double state_transit_margin_time; double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary - double keep_detection_line_margin; //! distance (toward path end) from generated stop line. - //! keep detection if ego is before this line and ego.vel < - //! keep_detection_vel_thr - double keep_detection_vel_thr; double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check double stuck_vehicle_ignore_dist; //! distance from intersection start to start stuck vehicle check @@ -88,9 +84,10 @@ class IntersectionModule : public SceneModuleInterface double detection_area_angle_thr; //! threshold in checking the angle of detecting objects double min_predicted_path_confidence; //! minimum confidence value of predicted path to use for collision detection - double external_input_timeout; //! used to disable external input - double collision_start_margin_time; //! start margin time to check collision - double collision_end_margin_time; //! end margin time to check collision + double external_input_timeout; //! used to disable external input + double minimum_ego_predicted_velocity; //! used to calclate ego's future velocity profile + double collision_start_margin_time; //! start margin time to check collision + double collision_end_margin_time; //! end margin time to check collision bool use_stuck_stopline; //! stopline generate before the intersection lanelet when is_stuck. double assumed_front_car_decel; //! the expected deceleration of front car when front car as well @@ -116,7 +113,6 @@ class IntersectionModule : public SceneModuleInterface std::string turn_direction_; bool has_traffic_light_; bool is_go_out_; - // Parameter PlannerParam planner_param_; @@ -139,7 +135,7 @@ class IntersectionModule : public SceneModuleInterface const lanelet::ConstLanelets & adjacent_lanelets, const std::optional & intersection_area, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const Polygon2d & stuck_vehicle_detect_area); + const int closest_idx, const Polygon2d & stuck_vehicle_detect_area, const double time_delay); /** * @brief Check if there is a stopped vehicle on the ego-lane. @@ -186,7 +182,7 @@ class IntersectionModule : public SceneModuleInterface */ TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const int objective_lane_id) const; + const int objective_lane_id, const double time_delay) const; /** * @brief check if the object has a target type for collision check diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 23c8c76d134f4..085de0c834ab7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -61,7 +61,6 @@ struct StopLineIdx size_t first_inside_lane = 0; size_t pass_judge_line = 0; size_t stop_line = 0; - size_t keep_detection_line = 0; }; /** @@ -78,8 +77,7 @@ std::pair, std::optional> generateStopLine( const int lane_id, const std::vector & detection_areas, const std::vector & conflicting_areas, const std::shared_ptr & planner_data, const double stop_line_margin, - const double keep_detection_line_margin, const bool use_stuck_stopline, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 5a35fb4c888de..a8af4938b1bf0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -46,8 +46,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ip.state_transit_margin_time = node.declare_parameter(ns + ".state_transit_margin_time", 2.0); ip.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0); - ip.keep_detection_line_margin = node.declare_parameter(ns + ".keep_detection_line_margin", 1.0); - ip.keep_detection_vel_thr = node.declare_parameter(ns + ".keep_detection_vel_thr", 0.833); ip.stuck_vehicle_detect_dist = node.declare_parameter(ns + ".stuck_vehicle_detect_dist", 3.0); ip.stuck_vehicle_ignore_dist = node.declare_parameter(ns + ".stuck_vehicle_ignore_dist", 5.0) + vehicle_info.max_longitudinal_offset_m; @@ -63,6 +61,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.min_predicted_path_confidence = node.declare_parameter(ns + ".min_predicted_path_confidence", 0.05); ip.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0); + ip.minimum_ego_predicted_velocity = + node.declare_parameter(ns + ".minimum_ego_predicted_velocity", 1.388); ip.collision_start_margin_time = node.declare_parameter(ns + ".collision_start_margin_time", 5.0); ip.collision_end_margin_time = node.declare_parameter(ns + ".collision_end_margin_time", 2.0); ip.use_stuck_stopline = node.declare_parameter(ns + ".use_stuck_stopline", true); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 3e25a1bb73d76..bbe6955c2a601 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -86,7 +86,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * /* get current pose */ const geometry_msgs::msg::Pose current_pose = planner_data_->current_odometry->pose; - const double current_vel = planner_data_->current_velocity->twist.linear.x; /* get lanelet map */ const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); @@ -124,8 +123,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * /* set stop lines for base_link */ const auto [stuck_line_idx_opt, stop_lines_idx_opt] = util::generateStopLine( lane_id_, detection_area, conflicting_area, planner_data_, planner_param_.stop_line_margin, - planner_param_.keep_detection_line_margin, planner_param_.use_stuck_stopline, path, *path, - logger_.get_child("util"), clock_); + planner_param_.use_stuck_stopline, path, *path, logger_.get_child("util"), clock_); if (!stuck_line_idx_opt.has_value()) { // returns here if path is not intersecting with conflicting areas RCLCPP_DEBUG_SKIPFIRST_THROTTLE( @@ -153,22 +151,13 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if (stop_lines_idx_opt.has_value()) { const auto stop_line_idx = stop_lines_idx_opt.value().stop_line; const auto pass_judge_line_idx = stop_lines_idx_opt.value().pass_judge_line; - const auto keep_detection_line_idx = stop_lines_idx_opt.value().keep_detection_line; const bool is_over_pass_judge_line = util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); - const bool is_before_keep_detection_line = - stop_lines_idx_opt.has_value() - ? util::isBeforeTargetIndex(*path, closest_idx, current_pose, keep_detection_line_idx) - : false; - const bool keep_detection = is_before_keep_detection_line && - std::fabs(current_vel) < planner_param_.keep_detection_vel_thr; - - if (is_over_pass_judge_line && keep_detection) { - // in case ego could not stop exactly before the stop line, but with some overshoot, - // keep detection within some margin under low velocity threshold - } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) { - RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed."); + + /* if ego is over the pass judge line before collision is detected, keep going */ + if (is_over_pass_judge_line && is_go_out_ && !external_stop) { + RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); RCLCPP_DEBUG(logger_, "===== plan end ====="); setSafe(true); setDistance(motion_utils::calcSignedArcLength( @@ -193,9 +182,11 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); /* calculate dynamic collision around detection area */ + const double time_delay = + is_go_out_ ? 0.0 : (planner_param_.state_transit_margin_time - state_machine_.getDuration()); const bool has_collision = checkCollision( lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr, - closest_idx, stuck_vehicle_detect_area); + closest_idx, stuck_vehicle_detect_area, time_delay); /* calculate final stop lines */ int stop_line_idx_final = @@ -310,7 +301,7 @@ bool IntersectionModule::checkCollision( const lanelet::ConstLanelets & adjacent_lanelets, const std::optional & intersection_area, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const Polygon2d & stuck_vehicle_detect_area) + const int closest_idx, const Polygon2d & stuck_vehicle_detect_area, const double time_delay) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -378,7 +369,8 @@ bool IntersectionModule::checkCollision( /* check collision between target_objects predicted path and ego lane */ // cut the predicted path at passing_time - const auto time_distance_array = calcIntersectionPassingTime(path, closest_idx, lane_id_); + const auto time_distance_array = + calcIntersectionPassingTime(path, closest_idx, lane_id_, time_delay); const double passing_time = time_distance_array.back().first; cutPredictPathWithDuration(&target_objects, passing_time); @@ -522,10 +514,8 @@ Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const int objective_lane_id) const + const int objective_lane_id, const double time_delay) const { - static constexpr double k_minimum_velocity = 1e-01; - double dist_sum = 0.0; int assigned_lane_found = false; @@ -555,7 +545,7 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( // calculate when ego is going to reach each (interpolated) points on the path TimeDistanceArray time_distance_array{}; dist_sum = 0.0; - double passing_time = 0.0; + double passing_time = time_delay; time_distance_array.emplace_back(passing_time, dist_sum); for (size_t i = 1; i < smoothed_reference_path.points.size(); ++i) { const auto & p1 = smoothed_reference_path.points.at(i - 1); @@ -568,7 +558,9 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( const double average_velocity = (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; passing_time += - (dist / std::max(k_minimum_velocity, average_velocity)); // to avoid zero-division + (dist / std::max( + planner_param_.minimum_ego_predicted_velocity, + average_velocity)); // to avoid zero-division time_distance_array.emplace_back(passing_time, dist_sum); } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index 7eec56b4d0707..d6084a5d8ce92 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -82,8 +82,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m); const auto [stuck_line_idx_opt, stop_lines_idx_opt] = util::generateStopLine( lane_id_, detection_area, conflicting_area, planner_data_, planner_param_.stop_line_margin, - 0.0 /* unnecessary in merge_from_private */, false /* same */, path, *path, - logger_.get_child("util"), clock_); + false /* same */, path, *path, logger_.get_child("util"), clock_); if (!stop_lines_idx_opt.has_value()) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); return false; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index 1f4cba618fe28..c047c7814e018 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -145,8 +145,7 @@ std::pair, std::optional> generateStopLine( const int lane_id, const std::vector & detection_areas, const std::vector & conflicting_areas, const std::shared_ptr & planner_data, const double stop_line_margin, - const double keep_detection_line_margin, const bool use_stuck_stopline, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger, [[maybe_unused]] const rclcpp::Clock::SharedPtr clock) { @@ -179,7 +178,6 @@ std::pair, std::optional> generateStopLine( } const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / interval); - const int keep_detection_line_margin_idx_dist = std::ceil(keep_detection_line_margin / interval); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval); @@ -288,53 +286,28 @@ std::pair, std::optional> generateStopLine( original_path->points.begin(), original_path->points.begin() + idxs.stop_line, [](const auto & p) { return std::fabs(p.point.longitudinal_velocity_mps) < 0.1; }); - { - /* insert judge point */ - const size_t pass_judge_idx_ip = static_cast(std::min( - static_cast(path_ip.points.size()) - 1, - std::max(static_cast(stop_idx_ip) - pass_judge_idx_dist, 0))); - /* if another stop point exist before intersection stop_line, disable judge_line. */ - if (has_prior_stopline || pass_judge_idx_ip == stop_idx_ip) { - idxs.pass_judge_line = idxs.stop_line; - } else { - const auto & insert_point = path_ip.points.at(pass_judge_idx_ip).point.pose; - const auto duplicate_idx_opt = - util::getDuplicatedPointIdx(*original_path, insert_point.position); - if (duplicate_idx_opt.has_value()) { - idxs.pass_judge_line = duplicate_idx_opt.value(); - } else { - const auto insert_idx_opt = util::insertPoint(insert_point, original_path); - if (!insert_idx_opt.has_value()) { - RCLCPP_WARN(logger, "insertPoint failed to pass judge line"); - return {stuck_stop_line_idx, std::nullopt}; - } - idxs.pass_judge_line = insert_idx_opt.value(); - idxs.stop_line = std::min(idxs.stop_line + 1, original_path->points.size() - 1); - if (stuck_stop_line_idx >= idxs.pass_judge_line) { - stuck_stop_line_idx = - std::min(stuck_stop_line_idx + 1, original_path->points.size() - 1); - } - } - } - } - - { - /* insert keep_detection_line */ - const int keep_detection_idx_ip = std::min( - stop_idx_ip + keep_detection_line_margin_idx_dist, path_ip.points.size() - 1); - const auto & insert_point = path_ip.points.at(keep_detection_idx_ip).point.pose; - const auto insert_idx_opt = util::getDuplicatedPointIdx(*original_path, insert_point.position); - if (insert_idx_opt.has_value()) { - idxs.keep_detection_line = insert_idx_opt.value(); + /* insert judge point */ + const size_t pass_judge_idx_ip = static_cast(std::min( + static_cast(path_ip.points.size()) - 1, + std::max(static_cast(stop_idx_ip) - pass_judge_idx_dist, 0))); + /* if another stop point exist before intersection stop_line, disable judge_line. */ + if (has_prior_stopline || pass_judge_idx_ip == stop_idx_ip) { + idxs.pass_judge_line = idxs.stop_line; + } else { + const auto & insert_point = path_ip.points.at(pass_judge_idx_ip).point.pose; + const auto duplicate_idx_opt = + util::getDuplicatedPointIdx(*original_path, insert_point.position); + if (duplicate_idx_opt.has_value()) { + idxs.pass_judge_line = duplicate_idx_opt.value(); } else { const auto insert_idx_opt = util::insertPoint(insert_point, original_path); if (!insert_idx_opt.has_value()) { - RCLCPP_WARN(logger, "insertPoint failed for keep detection line"); + RCLCPP_WARN(logger, "insertPoint failed to pass judge line"); return {stuck_stop_line_idx, std::nullopt}; } - idxs.keep_detection_line = insert_idx_opt.value(); - // keep_detection_line is after stop_line and pass_judge_line - if (stuck_stop_line_idx >= idxs.keep_detection_line) { + idxs.pass_judge_line = insert_idx_opt.value(); + idxs.stop_line = std::min(idxs.stop_line + 1, original_path->points.size() - 1); + if (stuck_stop_line_idx >= idxs.pass_judge_line) { stuck_stop_line_idx = std::min(stuck_stop_line_idx + 1, original_path->points.size() - 1); } @@ -343,10 +316,9 @@ std::pair, std::optional> generateStopLine( RCLCPP_DEBUG( logger, - "generateStopLine() : keep_detection_idx = %ld, stop_idx = %ld, pass_judge_idx = %ld" + "generateStopLine() : stop_idx = %ld, pass_judge_idx = %ld" ", stuck_stop_idx = %ld, has_prior_stopline = %d", - idxs.keep_detection_line, idxs.stop_line, idxs.pass_judge_line, stuck_stop_line_idx, - has_prior_stopline); + idxs.stop_line, idxs.pass_judge_line, stuck_stop_line_idx, has_prior_stopline); return {stuck_stop_line_idx, std::make_optional(idxs)}; } From 95b9ff67fb7340bc1ff65668d1e938d25bbec0b7 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 24 Jan 2023 16:39:21 +0100 Subject: [PATCH 27/60] build(kalman_filter): add build dependency (#2734) --- common/kalman_filter/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/common/kalman_filter/package.xml b/common/kalman_filter/package.xml index 950319c9ea282..f75755c172605 100644 --- a/common/kalman_filter/package.xml +++ b/common/kalman_filter/package.xml @@ -14,6 +14,8 @@ ament_cmake_auto autoware_cmake + eigen + eigen3_cmake_module ament_cmake_cppcheck ament_lint_auto From 8f025014177881d9b42d5397440029cba0f23ec6 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 24 Jan 2023 16:39:40 +0100 Subject: [PATCH 28/60] build(dummy_infrastructure): add build dependency (#2739) --- system/dummy_infrastructure/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/system/dummy_infrastructure/package.xml b/system/dummy_infrastructure/package.xml index bc1fad3a8e339..a361656545306 100644 --- a/system/dummy_infrastructure/package.xml +++ b/system/dummy_infrastructure/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_cmake + libboost-dev rclcpp rclcpp_components From ea2b32bac4004753653a0ec47ed8aabda73473a3 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 25 Jan 2023 09:39:38 +0900 Subject: [PATCH 29/60] fix(lane_change): chattering issue when performing check (#2741) Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/util.cpp | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 4212b368cf33a..718c1ef88a4b0 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -510,20 +510,8 @@ bool isLaneChangePathSafe( const double check_end_time = lane_change_prepare_duration + lane_changing_safety_check_duration; const double min_lc_speed{lane_change_parameters.minimum_lane_change_velocity}; - const auto get_pose = std::invoke([&]() { - Pose p; - double dist{0.0}; - for (size_t i = 1; i < path.points.size(); ++i) { - dist += motion_utils::calcSignedArcLength(path.points, i - 1, i); - if (dist >= common_parameters.backward_path_length) { - return path.points.at(i).point.pose; - } - } - return path.points.front().point.pose; - }); - const auto vehicle_predicted_path = util::convertToPredictedPath( - path, current_twist, get_pose, static_cast(current_seg_idx), check_end_time, + path, current_twist, current_pose, static_cast(current_seg_idx), check_end_time, time_resolution, acceleration, min_lc_speed); const auto prepare_phase_ignore_target_speed_thresh = lane_change_parameters.prepare_phase_ignore_target_speed_thresh; From c1a98f7f8822d7766a65c9fedf4ef805861d320e Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 25 Jan 2023 14:23:31 +0900 Subject: [PATCH 30/60] refactor(motion_utils): delete wall marker explicitly (#2506) * delete wall marker in intersection module Signed-off-by: kyoichi-sugahara * delete wall marker in stop_line module Signed-off-by: kyoichi-sugahara * delete wall marker in blind_spot module Signed-off-by: kyoichi-sugahara * delete wall marker in no_stopping_area Signed-off-by: kyoichi-sugahara * delete wall marker in run_out module Signed-off-by: kyoichi-sugahara * make motion util function to delete wall marker Signed-off-by: kyoichi-sugahara * add common function * temp * fix(default_ad_api): fix autoware state to add wait time (#2407) * fix(default_ad_api): fix autoware state to add wait time Signed-off-by: Takagi, Isamu * Update system/default_ad_api/src/compatibility/autoware_state.cpp Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Signed-off-by: Takagi, Isamu Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * chore(behaviror_velocity_planner): changed logging level for intersection (#2459) changed logging level Signed-off-by: Mamoru Sobue * fix(tvm_utility): copy test result to CPU (#2414) Also remove dependency to autoware_auto_common. Issue-Id: SCM-5401 Signed-off-by: Ambroise Vincent Change-Id: I83b859742df2f2ff7df1d0bd2d287bfe0aa04c3d Signed-off-by: Ambroise Vincent Co-authored-by: Xinyu Wang <93699235+angry-crab@users.noreply.github.com> * fix(raw_vehicle_cmd_converter): fix column index for map validation (#2450) Signed-off-by: Makoto Kurihara Signed-off-by: Makoto Kurihara * revert(default_ad_api): fix autoware state to add wait time (#2407) (#2460) Revert "fix(default_ad_api): fix autoware state to add wait time (#2407)" This reverts commit c4224854a7e57a9526dde998f742741fe383471c. * feat(behavior_path_planner): cut overlapped path (#2451) * feat(behavior_path_planner): cut overlapped path Signed-off-by: yutaka * clean code Signed-off-by: yutaka Signed-off-by: yutaka * perf(behavior_velocity_planner): remove unnecessary debug data (#2462) Signed-off-by: veqcc * docs: update training data for traffic light (#2464) * update traffic light cnn classifier README.md * correct to upper case Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * feat(motion_utils): add points resample function (#2465) Signed-off-by: yutaka Signed-off-by: yutaka * feat: remove web controller (#2405) Signed-off-by: Yukihiro Saito Signed-off-by: Yukihiro Saito * fix(motion_utils): rename sampling function (#2469) Signed-off-by: yutaka Signed-off-by: yutaka * fix: rename `use_external_emergency_stop` to `check_external_emergency_heartbeat` (#2455) * fix: rename use_external_emergency_stop to check_external_emergency_heartbeat * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(tier4_autoware_api_launch): add rosbridge_server dependency (#2470) Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka * fix: remove unnecessary DEBUG_INFO declarations (#2457) Signed-off-by: kminoda Signed-off-by: kminoda * feat(gyro_odometer): publish twist when both data arrives (#2423) * feat(gyro_odometer): publish when both data arrive Signed-off-by: kminoda * remove unnecessary commentouts Signed-off-by: kminoda * ci(pre-commit): autofix * use latest timestamp Signed-off-by: kminoda * small fix Signed-off-by: kminoda * debugged Signed-off-by: kminoda * update gyro_odometer Signed-off-by: kminoda * ci(pre-commit): autofix * add comments Signed-off-by: kminoda * add comments Signed-off-by: kminoda * ci(pre-commit): autofix * fix timestamp validation flow Signed-off-by: kminoda * ci(pre-commit): autofix * remove unnecessary commentouts Signed-off-by: kminoda * pre-commit Signed-off-by: kminoda * ci(pre-commit): autofix Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(freespace_planning_algorithms): comment out failing tests (#2440) Signed-off-by: kosuke55 Signed-off-by: kosuke55 * feat(behavior_velocity_planner): add velocity factors (#1985) * (editting) add intersection_coordination to stop reason Signed-off-by: TakumiKozaka-T4 * (editting) add intersection coordination to stop reasons Signed-off-by: TakumiKozaka-T4 * (Editting) add v2x to stop reason Signed-off-by: TakumiKozaka-T4 * (editting) add stop reason2 publisher Signed-off-by: TakumiKozaka-T4 * (editting) add stop reason2 to scene modules Signed-off-by: TakumiKozaka-T4 * add stop reason2 to obstacle stop planner and surround obstacle checker Signed-off-by: TakumiKozaka-T4 * Modify files including unintended change by rebase Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * Modification 1: not to publsh vacant stop reason, 2: change default status in obstacle stop and surround obstacle checker Signed-off-by: TakumiKozaka-T4 * fix error Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * modification for renaming stop_reason2 to motion_factor Signed-off-by: TakumiKozaka-T4 * (Editting) rename variables Signed-off-by: TakumiKozaka-T4 * bug fix Signed-off-by: TakumiKozaka-T4 * (WIP) Add motion factor message. Modify scene modules due to new motion factor. Moving motion factor aggregator. Signed-off-by: TakumiKozaka-T4 * (WIP) Save current work. Modify aggregator, CMakeList. Add launcher Signed-off-by: TakumiKozaka-T4 * (WIP) Solved build error, but not launched Signed-off-by: TakumiKozaka-T4 * (WIP) fixing error in launch Signed-off-by: TakumiKozaka-T4 * (WIP) fixing error in launch Signed-off-by: TakumiKozaka-T4 * (WIP) fixing launch error Signed-off-by: TakumiKozaka-T4 * Fix error in launching motion factor aggregator Signed-off-by: TakumiKozaka-T4 * Delete unnecessary comment-out in CMakelists. Change remapping in launcher. Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * pull the latest foundation/main Signed-off-by: TakumiKozaka-T4 * (fix for pre-commit.ci) Add to motion_factor_aggregator.hpp Signed-off-by: TakumiKozaka-T4 * ci(pre-commit): autofix * feat: add velocity factor interface Signed-off-by: Takagi, Isamu * fix: fix build error Signed-off-by: Takagi, Isamu * feat: stop sign Signed-off-by: Takagi, Isamu * WIP Signed-off-by: Takagi, Isamu * feat: update visualizer Signed-off-by: Takagi, Isamu * feat: modify traffic light manager Signed-off-by: Takagi, Isamu * feat: update velocity factors Signed-off-by: Takagi, Isamu * feat: update api Signed-off-by: Takagi, Isamu * feat: move adapi msgs Signed-off-by: Takagi, Isamu * feat: remove old aggregator Signed-off-by: Takagi, Isamu * feat: move api Signed-off-by: Takagi, Isamu * feat: rename message Signed-off-by: Takagi, Isamu * feat: add using Signed-off-by: Takagi, Isamu * feat: add distance Signed-off-by: Takagi, Isamu * feat: fix build error Signed-off-by: Takagi, Isamu * feat: use nan as default distance Signed-off-by: Takagi, Isamu * fix: set virtual traffic light detail Signed-off-by: Takagi, Isamu * fix: remove debug code Signed-off-by: Takagi, Isamu * fix: copyright Signed-off-by: Takagi, Isamu Signed-off-by: TakumiKozaka-T4 Signed-off-by: Takagi, Isamu Co-authored-by: TakumiKozaka-T4 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(emergency_handler): fix mrm handling when mrm behavior is none (#2476) Signed-off-by: Makoto Kurihara Signed-off-by: Makoto Kurihara * feat(behavior_velocity_planner): clean walkway module (#2480) Signed-off-by: yutaka Signed-off-by: yutaka * fix(trajectory_follower): fix mpc trajectory z pos (#2482) Signed-off-by: takahoribe Signed-off-by: takahoribe * fix(ground-segmentation): recheck gnd cluster pointcloud (#2448) * fix: reclassify ground cluster pcl Signed-off-by: badai-nguyen * fix: add lowest-based recheck Signed-off-by: badai-nguyen * chore: refactoring Signed-off-by: badai-nguyen * chore: refactoring Signed-off-by: badai-nguyen Signed-off-by: badai-nguyen Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> * fix(freespace_planning_algorithms): fix rrtstar can't arrive goal error (#2350) Signed-off-by: NorahXiong Signed-off-by: NorahXiong Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> * revert(behavior_path): revert removal of refineGoalFunction (#2340)" (#2485) This reverts commit 8e13ced6dfb6edfea77a589ef4cb93d82683bf51. Signed-off-by: mitsudome-r Signed-off-by: mitsudome-r * fix(behavior_path_planner): minimum distance for lane change (#2413) Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi * fix(behavior_path_planner): lane change candidate resolution (#2426) * fix(behavior_path_planner): lane change candidate resolution Signed-off-by: Muhammad Zulfaqar Azmi * rework sampling based on current speed Signed-off-by: Muhammad Zulfaqar Azmi * refactor code Signed-off-by: Muhammad Zulfaqar Azmi * use util's resampler Signed-off-by: Muhammad Zulfaqar Azmi * consider min_resampling_points and resampling dt Signed-off-by: Muhammad Zulfaqar * simplify code Signed-off-by: Muhammad Zulfaqar Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar * chore(simulator_compatibility_test): suppress setuptools warnings (#2483) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu * docs(tier4_state_rviz_plugin): update readme (#2475) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu * feat(trajectory_follower): pub steer converged marker (#2441) * feat(trajectory_follower): pub steer converged marker Signed-off-by: kosuke55 * Revert "feat(trajectory_follower): pub steer converged marker" This reverts commit a6f6917bc542d5b533150f6abba086121e800974. Signed-off-by: kosuke55 * add steer converged debug marker in contoller_node Signed-off-by: kosuke55 Signed-off-by: kosuke55 * feat: replace python launch with xml launch for system monitor (#2430) * feat: replace python launch with xml launch for system monitor Signed-off-by: Daisuke Nishimatsu * ci(pre-commit): autofix * update figure Signed-off-by: Daisuke Nishimatsu Signed-off-by: Daisuke Nishimatsu Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * refactor(vehicle_cmd_gate): remove old emergency topics (#2403) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu * feat(behavior_path_planner, obstacle_avoidance_planner): add new drivable area (#2472) * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update obstacle avoidance planner Signed-off-by: yutaka * update Signed-off-by: yutaka * clean code Signed-off-by: yutaka * uddate Signed-off-by: yutaka * clean code Signed-off-by: yutaka * remove resample Signed-off-by: yutaka * update Signed-off-by: yutaka * add orientation Signed-off-by: yutaka * change color Signed-off-by: yutaka * update Signed-off-by: yutaka * remove drivable area Signed-off-by: yutaka * add flag Signed-off-by: yutaka * update Signed-off-by: yutaka * update color Signed-off-by: yutaka * fix some codes Signed-off-by: yutaka * change to makerker array Signed-off-by: yutaka * change avoidance utils Signed-off-by: yutaka Signed-off-by: yutaka * feat(default_ad_api): split parameters into file (#2488) * feat(default_ad_api): split parameters into file Signed-off-by: Takagi, Isamu * feat: remove old parameter Signed-off-by: Takagi, Isamu * fix: test Signed-off-by: Takagi, Isamu * feat: add default config Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu * docs(default_ad_api): add readme (#2491) * docs(default_ad_api): add readme Signed-off-by: Takagi, Isamu * feat: update table Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu * feat(map_loader): add differential map loading interface (#2417) * first commit Signed-off-by: kminoda * ci(pre-commit): autofix * added module load in _node.cpp Signed-off-by: kminoda * ci(pre-commit): autofix * create pcd metadata dict when either of the flag is true Signed-off-by: kminoda * ci(pre-commit): autofix * fix readme * ci(pre-commit): autofix Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * feat(behavior_path_planner): change turn signal output timing (#2493) Signed-off-by: yutaka Signed-off-by: yutaka * feat(behavior_path_planner): remove unnecessary code and clean turn signal decider (#2494) * feat(behavior_path_planner): clean drivable area code Signed-off-by: yutaka * make a function for turn signal decider Signed-off-by: yutaka Signed-off-by: yutaka * feat(trajectory_visualizer): update for steer limit, remove tf for pose source (#2267) Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe * feat(mission_planner): check goal footprint (#2088) Signed-off-by: ismet atabay * feat(behavior_path_planner): fix overlap checker (#2498) * feat(behavior_path_planner): fix overlap checker Signed-off-by: yutaka * remove reserve Signed-off-by: yutaka Signed-off-by: yutaka * fix(behavior_path_planner): fix find nearest function from lateral distance (#2499) * feat(behavior_path_planner): fix find nearest function from lateral distance * empty commit * fix(behavior_path_planner): fix planner data copy (#2501) Signed-off-by: yutaka Signed-off-by: yutaka * refactor(simple_planning_simulator): make function for duplicated code (#2502) Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe * fix build error Signed-off-by: kyoichi-sugahara * ci(pre-commit): autofix * revert intersection module change Signed-off-by: kyoichi-sugahara * delete wallmarker in intersection module Signed-off-by: kyoichi-sugahara * compare pose with resample_uitils Signed-off-by: kyoichi-sugahara * delete wallmarker in merge_from_private module Signed-off-by: kyoichi-sugahara * delete wallmarker in no_stopping_area Signed-off-by: kyoichi-sugahara * ci(pre-commit): autofix * add dead line creator Signed-off-by: kyoichi-sugahara * fix base function Signed-off-by: kyoichi-sugahara * delete wall marker in each module Signed-off-by: kyoichi-sugahara * ci(pre-commit): autofix * empty gurad Signed-off-by: kyoichi-sugahara * ci(pre-commit): autofix * instantiation wall maeker creator Signed-off-by: kyoichi-sugahara * initialize pointer Signed-off-by: kyoichi-sugahara * ci(pre-commit): autofix * define create_wall_function and delete_wall_function in VirtualWallMarkerCreator Signed-off-by: kyoichi-sugahara * change instance name Signed-off-by: kyoichi-sugahara Signed-off-by: kyoichi-sugahara Signed-off-by: Takagi, Isamu Signed-off-by: Mamoru Sobue Signed-off-by: Ambroise Vincent Signed-off-by: Makoto Kurihara Signed-off-by: yutaka Signed-off-by: veqcc Signed-off-by: Yukihiro Saito Signed-off-by: Takayuki Murooka Signed-off-by: kminoda Signed-off-by: kosuke55 Signed-off-by: TakumiKozaka-T4 Signed-off-by: takahoribe Signed-off-by: badai-nguyen Signed-off-by: NorahXiong Signed-off-by: mitsudome-r Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Signed-off-by: Daisuke Nishimatsu Signed-off-by: Takamasa Horibe Signed-off-by: ismet atabay Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Mamoru Sobue Co-authored-by: Ambroise Vincent Co-authored-by: Xinyu Wang <93699235+angry-crab@users.noreply.github.com> Co-authored-by: Makoto Kurihara Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Co-authored-by: Ryuta Kambe Co-authored-by: Mingyu1991 <115005477+Mingyu1991@users.noreply.github.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Yukihiro Saito Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takayuki Murooka Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: Kosuke Takeuchi Co-authored-by: TakumiKozaka-T4 Co-authored-by: Takamasa Horibe Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Co-authored-by: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: Ismet Atabay <56237550+ismetatabay@users.noreply.github.com> --- .../motion_utils/marker/marker_helper.hpp | 57 ++++++++++++-- .../motion_utils/src/marker/marker_helper.cpp | 78 +++++++++++++++++++ .../include/scene_module/blind_spot/scene.hpp | 3 + .../crosswalk/scene_crosswalk.hpp | 4 + .../scene_module/crosswalk/scene_walkway.hpp | 4 + .../scene_module/detection_area/scene.hpp | 3 + .../intersection/scene_intersection.hpp | 4 + .../scene_merge_from_private_road.hpp | 3 + .../scene_no_stopping_area.hpp | 3 + .../occlusion_spot/scene_occlusion_spot.hpp | 3 + .../include/scene_module/run_out/debug.hpp | 3 + .../include/scene_module/stop_line/scene.hpp | 3 + .../virtual_traffic_light/scene.hpp | 2 + .../src/scene_module/blind_spot/debug.cpp | 4 +- .../src/scene_module/crosswalk/debug.cpp | 22 +++++- .../src/scene_module/detection_area/debug.cpp | 20 +++-- .../src/scene_module/intersection/debug.cpp | 13 ++-- .../scene_module/no_stopping_area/debug.cpp | 11 ++- .../src/scene_module/occlusion_spot/debug.cpp | 6 +- .../src/scene_module/run_out/debug.cpp | 8 +- .../src/scene_module/stop_line/debug.cpp | 7 +- .../virtual_traffic_light/debug.cpp | 16 ++-- 22 files changed, 235 insertions(+), 42 deletions(-) diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp index 714220d5dedc4..166d3a6661689 100644 --- a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp +++ b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp @@ -15,23 +15,28 @@ #ifndef MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ #define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ +#include "motion_utils/resample/resample_utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include #include +#include namespace motion_utils { +using geometry_msgs::msg::Pose; + visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( - const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset = 0.0); + const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, + const double longitudinal_offset = 0.0); visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( - const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset = 0.0); + const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, + const double longitudinal_offset = 0.0); visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( - const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset = 0.0); + const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, + const double longitudinal_offset = 0.0); visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id); @@ -41,6 +46,46 @@ visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker( visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker( const rclcpp::Time & now, const int32_t id); + +visualization_msgs::msg::MarkerArray createVirtualWallMarkerFromPreviousPoses( + const std::vector & stop_poses, const std::vector & previous_poses, + const rclcpp::Time & now, int32_t id); + +class VirtualWallMarkerCreator +{ +public: + virtual ~VirtualWallMarkerCreator() = default; + + using create_wall_function = std::function; + + using delete_wall_function = + std::function; + + visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( + const std::vector & stop_poses, const std::string & module_name, const rclcpp::Time & now, + int32_t id, const double longitudinal_offset = 0.0); + + visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( + const std::vector & slow_down_poses, const std::string & module_name, + const rclcpp::Time & now, int32_t id, const double longitudinal_offset = 0.0); + + visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( + const std::vector & dead_line_poses, const std::string & module_name, + const rclcpp::Time & now, int32_t id, const double longitudinal_offset = 0.0); + +private: + visualization_msgs::msg::MarkerArray handleVirtualWallMarker( + const std::vector & poses, const std::string & module_name, const rclcpp::Time & now, + int32_t id, create_wall_function function_create_wall_marker, + delete_wall_function function_delete_wall_marker, + std::vector & previous_poses, const double longitudinal_offset = 0.0); + + std::vector previous_stop_poses_; + std::vector previous_slow_down_poses_; + std::vector previous_dead_line_poses_; +}; } // namespace motion_utils #endif // MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index d88bcada25193..60e5f09bbea89 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -16,13 +16,16 @@ #include +using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createDeletedDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; +using visualization_msgs::msg::MarkerArray; namespace { + inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name, const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id, @@ -128,4 +131,79 @@ visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker( { return createDeletedVirtualWallMarkerArray("dead_line_", now, id); } + +visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWallMarker( + const std::vector & poses, const std::string & module_name, const rclcpp::Time & now, + int32_t id, create_wall_function function_create_wall_marker, + delete_wall_function function_delete_wall_marker, + std::vector & previous_virtual_wall_poses, + const double longitudinal_offset) +{ + size_t id_to_create = id; + size_t id_to_delete = id; + visualization_msgs::msg::MarkerArray wall_marker; + + if (poses.size() == 0 || previous_virtual_wall_poses.empty()) { + return wall_marker; + } + + for (const auto & p : previous_virtual_wall_poses) { + const bool previous_stop_pose_is_in_stop_pose = + std::any_of(poses.begin(), poses.end(), [&](const geometry_msgs::msg::Pose & elem) { + std::vector poses; + poses.push_back(p); + poses.push_back(elem); + return resample_utils::validate_points_duplication(poses); + }); + + if (!previous_stop_pose_is_in_stop_pose) { + appendMarkerArray(function_delete_wall_marker(now, id_to_delete), &wall_marker, now); + } + id_to_delete++; + } + + for (const auto & p : poses) { + appendMarkerArray( + function_create_wall_marker(p, module_name, now, id_to_create++, longitudinal_offset), + &wall_marker); + } + previous_virtual_wall_poses = poses; + return wall_marker; +} + +visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createStopVirtualWallMarker( + const std::vector & stop_poses, const std::string & module_name, const rclcpp::Time & now, + int32_t id, const double longitudinal_offset) +{ + create_wall_function creator = motion_utils::createStopVirtualWallMarker; + delete_wall_function deleter = motion_utils::createDeletedStopVirtualWallMarker; + + return handleVirtualWallMarker( + stop_poses, module_name, now, id, creator, deleter, previous_stop_poses_, longitudinal_offset); +} + +visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createSlowDownVirtualWallMarker( + const std::vector & slow_down_poses, const std::string & module_name, + const rclcpp::Time & now, int32_t id, const double longitudinal_offset) +{ + create_wall_function creator = motion_utils::createSlowDownVirtualWallMarker; + delete_wall_function deleter = motion_utils::createDeletedSlowDownVirtualWallMarker; + + return handleVirtualWallMarker( + slow_down_poses, module_name, now, id, creator, deleter, previous_slow_down_poses_, + longitudinal_offset); +} + +visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createDeadLineVirtualWallMarker( + const std::vector & dead_line_poses, const std::string & module_name, + const rclcpp::Time & now, int32_t id, const double longitudinal_offset) +{ + create_wall_function creator = motion_utils::createDeadLineVirtualWallMarker; + delete_wall_function deleter = motion_utils::createDeletedDeadLineVirtualWallMarker; + + return handleVirtualWallMarker( + dead_line_poses, module_name, now, id, creator, deleter, previous_dead_line_poses_, + longitudinal_offset); +} + } // namespace motion_utils diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp index 4b1c672ff2b08..d85bc002297df 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp @@ -217,6 +217,9 @@ class BlindSpotModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; + + std::shared_ptr virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp index f3930291d5b9f..acd0c45755b71 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp @@ -38,6 +38,7 @@ #include #include +#include #include #include #include @@ -174,6 +175,9 @@ class CrosswalkModule : public SceneModuleInterface // whether ego passed safety_slow_point bool passed_safety_slow_point_; + + std::shared_ptr virtual_wall_marker_creator_crosswalk_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp index 8d05fc4f52d39..42be22f1386b6 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -72,6 +73,9 @@ class WalkwayModule : public SceneModuleInterface // Debug DebugData debug_data_; + + std::shared_ptr virtual_wall_marker_creator_walkway_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp index 3d2ede0c970ab..b6896f4b68eb9 100644 --- a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp @@ -99,6 +99,9 @@ class DetectionAreaModule : public SceneModuleInterface // Debug DebugData debug_data_; + + std::shared_ptr virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index f8e0fea2f23de..98589555bd7aa 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -15,6 +15,7 @@ #ifndef SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_ #define SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_ +#include #include #include #include @@ -256,6 +257,9 @@ class IntersectionModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; + + std::shared_ptr virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp index 137a3c6286705..a2d884240747d 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp @@ -100,6 +100,9 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; + + std::shared_ptr virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp index 181293144bd8b..e387b1b39cf2d 100644 --- a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp @@ -174,6 +174,9 @@ class NoStoppingAreaModule : public SceneModuleInterface // Debug DebugData debug_data_; + + std::shared_ptr virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp index 05eab13f79e63..c7ee714b03a7b 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp @@ -68,6 +68,9 @@ class OcclusionSpotModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; + + std::shared_ptr virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp index f833e4e0e46b2..414ce63ed8d7f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp @@ -16,6 +16,7 @@ #include "scene_module/run_out/dynamic_obstacle.hpp" +#include #include #include namespace behavior_velocity_planner @@ -130,6 +131,8 @@ class RunOutDebug DebugValues debug_values_; AccelReason accel_reason_; double height_{0}; + std::shared_ptr virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp index b22c61b19f549..f9846f970beb5 100644 --- a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp @@ -105,6 +105,9 @@ class StopLineModule : public SceneModuleInterface // Debug DebugData debug_data_; + + std::shared_ptr virtual_wall_marker_creator_ = + std::make_shared(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp index 130e4a1715fde..2bd7078d93aeb 100644 --- a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp @@ -94,6 +94,8 @@ class VirtualTrafficLightModule : public SceneModuleInterface tier4_v2x_msgs::msg::InfrastructureCommand command_; MapData map_data_; ModuleData module_data_; + std::shared_ptr virtual_wall_marker_creator_ = + std::make_shared(); void updateInfrastructureCommand(); diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp index fbd525294a50e..67b364033b4fd 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp @@ -78,8 +78,8 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createVirtualWallMarkerArr if (!isActivated() && !is_over_pass_judge_line_) { appendMarkerArray( - motion_utils::createStopVirtualWallMarker( - debug_data_.virtual_wall_pose, "blind_spot", now, module_id_), + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {debug_data_.virtual_wall_pose}, "blind_spot", now, module_id_), &wall_marker, now); } return wall_marker; diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp index 6aab24b9a24fa..1b27e2b06cf55 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp @@ -223,17 +223,26 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createVirtualWallMarkerArr { const auto now = this->clock_->now(); auto id = module_id_; + std::vector stop_poses; + std::vector slow_down_poses; visualization_msgs::msg::MarkerArray wall_marker; for (const auto & p : debug_data_.stop_poses) { const auto p_front = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); - appendMarkerArray(createStopVirtualWallMarker(p_front, "crosswalk", now, id++), &wall_marker); + stop_poses.push_back(p_front); } + appendMarkerArray( + virtual_wall_marker_creator_crosswalk_->createStopVirtualWallMarker( + stop_poses, "crosswalk", now, id), + &wall_marker); for (const auto & p : debug_data_.slow_poses) { const auto p_front = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); - appendMarkerArray( - createSlowDownVirtualWallMarker(p_front, "crosswalk", now, id++), &wall_marker); + slow_down_poses.push_back(p_front); } + appendMarkerArray( + virtual_wall_marker_creator_crosswalk_->createSlowDownVirtualWallMarker( + slow_down_poses, "crosswalk", now, id), + &wall_marker); return wall_marker; } @@ -242,12 +251,17 @@ visualization_msgs::msg::MarkerArray WalkwayModule::createVirtualWallMarkerArray { const auto now = this->clock_->now(); auto id = module_id_; + std::vector stop_poses; visualization_msgs::msg::MarkerArray wall_marker; for (const auto & p : debug_data_.stop_poses) { const auto p_front = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); - appendMarkerArray(createStopVirtualWallMarker(p_front, "walkway", now, id++), &wall_marker); + stop_poses.push_back(p_front); } + appendMarkerArray( + virtual_wall_marker_creator_walkway_->createStopVirtualWallMarker( + stop_poses, "walkway", now, id), + &wall_marker); return wall_marker; } diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp index 5d62dfbc35d4e..9bd56c79cf70d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp @@ -175,21 +175,29 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createVirtualWallMarke const rclcpp::Time now = clock_->now(); auto id = getModuleId(); + + std::vector stop_poses; + std::vector dead_line_poses; + for (const auto & p : debug_data_.stop_poses) { const auto p_front = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); - appendMarkerArray( - motion_utils::createStopVirtualWallMarker(p_front, "detection_area", now, id++), &wall_marker, - now); + stop_poses.push_back(p_front); } + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker( + stop_poses, "detection_area", now, id), + &wall_marker, now); for (const auto & p : debug_data_.dead_line_poses) { const auto p_front = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); - appendMarkerArray( - motion_utils::createDeadLineVirtualWallMarker(p_front, "detection_area", now, id++), - &wall_marker, now); + dead_line_poses.push_back(p_front); } + appendMarkerArray( + virtual_wall_marker_creator_->createDeadLineVirtualWallMarker( + dead_line_poses, "detection_area", now, id), + &wall_marker, now); return wall_marker; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 8697be34d0222..23ebdcef0f09a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -193,13 +193,13 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker if (debug_data_.stop_required) { appendMarkerArray( - motion_utils::createStopVirtualWallMarker( - debug_data_.stop_wall_pose, "intersection", now, module_id_), + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {debug_data_.stop_wall_pose}, "intersection", now, module_id_), &wall_marker, now); } else if (state == StateMachine::State::STOP) { appendMarkerArray( - motion_utils::createStopVirtualWallMarker( - debug_data_.slow_wall_pose, "intersection", now, module_id_), + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {debug_data_.slow_wall_pose}, "intersection", now, module_id_), &wall_marker, now); } return wall_marker; @@ -230,9 +230,10 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createVirtualWa const auto now = this->clock_->now(); if (state == StateMachine::State::STOP) { + const std::vector & pose = {debug_data_.virtual_wall_pose}; appendMarkerArray( - motion_utils::createStopVirtualWallMarker( - debug_data_.virtual_wall_pose, "merge_from_private_road", now, module_id_), + virtual_wall_marker_creator_->createStopVirtualWallMarker( + pose, "merge_from_private_road", now, module_id_), &wall_marker, now); } diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp index b50fa38e48d6d..205d9314c2505 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp @@ -168,13 +168,18 @@ visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createVirtualWallMark const auto now = clock_->now(); auto id = module_id_; + std::vector stop_poses; + for (const auto & p : debug_data_.stop_poses) { const auto p_front = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); - appendMarkerArray( - motion_utils::createStopVirtualWallMarker(p_front, "no_stopping_area", now, id++), - &wall_marker, now); + stop_poses.push_back(p_front); } + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker( + stop_poses, "no_stopping_area", now, id), + &wall_marker, now); + return wall_marker; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index 73c8b12b79162..bf82bd74552df 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -219,11 +219,15 @@ MarkerArray OcclusionSpotModule::createVirtualWallMarkerArray() MarkerArray wall_marker; std::string module_name = "occlusion_spot"; + std::vector slow_down_poses; + size_t module_id = 0; for (size_t id = 0; id < debug_data_.debug_poses.size(); id++) { const auto p_front = calcOffsetPose(debug_data_.debug_poses.at(id), debug_data_.baselink_to_front, 0.0, 0.0); + slow_down_poses.push_back(p_front); appendMarkerArray( - motion_utils::createSlowDownVirtualWallMarker(p_front, module_name, current_time, id), + virtual_wall_marker_creator_->createSlowDownVirtualWallMarker( + slow_down_poses, module_name, current_time, module_id), &wall_marker, current_time); } return wall_marker; diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp index 61577891224c3..cdd94bfb9b001 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp @@ -165,12 +165,12 @@ visualization_msgs::msg::MarkerArray RunOutDebug::createVirtualWallMarkerArray() { visualization_msgs::msg::MarkerArray wall_marker; rclcpp::Time now = node_.now(); + size_t id = 0; - for (const auto & p : stop_pose_) { - appendMarkerArray( - motion_utils::createStopVirtualWallMarker(p, "run_out", now, id++), &wall_marker); - } + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker(stop_pose_, "run_out", now, id), + &wall_marker, now); stop_pose_.clear(); diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp index 72daa9012a8b7..d86c6f72b1649 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp @@ -98,6 +98,7 @@ visualization_msgs::msg::MarkerArray StopLineModule::createVirtualWallMarkerArra { const auto now = this->clock_->now(); visualization_msgs::msg::MarkerArray wall_marker; + if (!debug_data_.stop_pose) { return wall_marker; } @@ -105,9 +106,11 @@ visualization_msgs::msg::MarkerArray StopLineModule::createVirtualWallMarkerArra *debug_data_.stop_pose, debug_data_.base_link2front, 0.0, 0.0); if (state_ == State::APPROACH || state_ == State::STOPPED) { appendMarkerArray( - motion_utils::createStopVirtualWallMarker(p_front, "stopline", now, module_id_), &wall_marker, - now); + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {p_front}, "stopline", now, module_id_), + &wall_marker, now); } + return wall_marker; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp index e001e581232eb..f60a317de996e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp @@ -58,18 +58,18 @@ visualization_msgs::msg::MarkerArray VirtualTrafficLightModule::createVirtualWal // virtual_wall_stop_line if (d.stop_head_pose_at_stop_line) { - const auto markers = createStopVirtualWallMarker( - *d.stop_head_pose_at_stop_line, "virtual_traffic_light", now, module_id_); - - appendMarkerArray(markers, &wall_marker); + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {*d.stop_head_pose_at_stop_line}, "virtual_traffic_light", now, module_id_), + &wall_marker, now); } // virtual_wall_end_line if (d.stop_head_pose_at_end_line) { - const auto markers = createStopVirtualWallMarker( - *d.stop_head_pose_at_end_line, "virtual_traffic_light", now, module_id_); - - appendMarkerArray(markers, &wall_marker); + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {*d.stop_head_pose_at_end_line}, "virtual_traffic_light", now, module_id_), + &wall_marker, now); } return wall_marker; From 74594a68303be1efd966357cdd0cae7db6c78dbb Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Date: Wed, 25 Jan 2023 11:25:50 +0300 Subject: [PATCH 31/60] fix(obstacle_avoidance_planner): fix offset signs for footprint corners (#2687) Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> --- planning/obstacle_avoidance_planner/src/utils/utils.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/obstacle_avoidance_planner/src/utils/utils.cpp b/planning/obstacle_avoidance_planner/src/utils/utils.cpp index 0a35b9d532ac3..44548979c1758 100644 --- a/planning/obstacle_avoidance_planner/src/utils/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/utils.cpp @@ -700,16 +700,16 @@ bool isOutsideDrivableAreaFromRectangleFootprint( const double base_to_rear = vehicle_param.rear_overhang; const auto top_left_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_left, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) .position; const auto top_right_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_right, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) .position; const auto bottom_right_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_right, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) .position; const auto bottom_left_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_left, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) .position; if (is_considering_footprint_edges) { From 2d98ee84c9b422529a543922732e6f12046c0e20 Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Date: Wed, 25 Jan 2023 13:04:26 +0300 Subject: [PATCH 32/60] feat(obstacle_avoidance_planner): add a separate marker for the first footprint which goes out of drivable area (#2689) feat(obstacle_avoidance_planner): add a separate distinguishing footprint marker Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> --- .../src/utils/debug_utils.cpp | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp index 7cab7cb64d202..2f1ba9f3a58ef 100644 --- a/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp @@ -675,6 +675,37 @@ visualization_msgs::msg::MarkerArray getVehicleCirclesMarkerArray( return msg; } +visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray( + const geometry_msgs::msg::Pose & stop_pose, const VehicleParam & vehicle_param, + const std::string & ns, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, 1, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 1.0)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; + const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; + const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; + const double base_to_rear = vehicle_param.rear_overhang; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(stop_pose, base_to_front, base_to_left, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(stop_pose, base_to_front, -base_to_right, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(stop_pose, -base_to_rear, -base_to_right, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(stop_pose, -base_to_rear, base_to_left, 0.0).position); + marker.points.push_back(marker.points.front()); + + msg.markers.push_back(marker); + + return msg; +} + visualization_msgs::msg::MarkerArray getVirtualWallMarkerArray( const geometry_msgs::msg::Pose & pose, const std::string & ns, const double r, const double g, const double b) @@ -794,6 +825,15 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( 1.0, 0.3, 0.3), &vis_marker_array); + // footprint by drivable area + if (debug_data.stop_pose_by_drivable_area) { + appendMarkerArray( + getFootprintByDrivableAreaMarkerArray( + *debug_data.stop_pose_by_drivable_area, vehicle_param, "footprint_by_drivable_area", 1.0, + 0.0, 0.0), + &vis_marker_array); + } + return vis_marker_array; } From 2edacb6533332a9db1d3bb72ca1b4136ab498f85 Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Date: Wed, 25 Jan 2023 13:07:01 +0300 Subject: [PATCH 33/60] fix(obstacle_avoidance_planner): fix drivable area polygon point order direction (#2691) Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> --- .../src/utils/utils.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/planning/obstacle_avoidance_planner/src/utils/utils.cpp b/planning/obstacle_avoidance_planner/src/utils/utils.cpp index 44548979c1758..377cb8150d60c 100644 --- a/planning/obstacle_avoidance_planner/src/utils/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/utils.cpp @@ -664,18 +664,18 @@ bg_polygon createDrivablePolygon( { bg_polygon drivable_area_poly; - // right bound - for (const auto rp : right_bound) { - drivable_area_poly.outer().push_back(bg_point(rp.x, rp.y)); - } - // left bound - auto reversed_left_bound = left_bound; - std::reverse(reversed_left_bound.begin(), reversed_left_bound.end()); - for (const auto lp : reversed_left_bound) { + for (const auto lp : left_bound) { drivable_area_poly.outer().push_back(bg_point(lp.x, lp.y)); } + // right bound + auto reversed_right_bound = right_bound; + std::reverse(reversed_right_bound.begin(), reversed_right_bound.end()); + for (const auto rp : reversed_right_bound) { + drivable_area_poly.outer().push_back(bg_point(rp.x, rp.y)); + } + drivable_area_poly.outer().push_back(drivable_area_poly.outer().front()); return drivable_area_poly; } From 854bff76adfbe982cc7378d639be117bd14cf343 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 25 Jan 2023 23:05:57 +0100 Subject: [PATCH 34/60] build(bluetooth_monitor): add build dependency (#2738) --- system/bluetooth_monitor/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/system/bluetooth_monitor/package.xml b/system/bluetooth_monitor/package.xml index 1b96451044631..12f3e8088d7d3 100644 --- a/system/bluetooth_monitor/package.xml +++ b/system/bluetooth_monitor/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_cmake + libboost-serialization-dev diagnostic_msgs diagnostic_updater From 5a2704aac21e022f8b6af9585684c911c59960b3 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 25 Jan 2023 23:09:23 +0100 Subject: [PATCH 35/60] build(system_monitor): add build dependency (#2740) --- system/system_monitor/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/system/system_monitor/package.xml b/system/system_monitor/package.xml index 30a4af4af4926..eade23a610eea 100644 --- a/system/system_monitor/package.xml +++ b/system/system_monitor/package.xml @@ -11,6 +11,8 @@ ament_cmake_auto autoware_cmake + libboost-serialization-dev + libboost-thread-dev diagnostic_msgs diagnostic_updater From b81fc590329534f5aca81fd71f114f03021dac05 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 26 Jan 2023 09:00:20 +0900 Subject: [PATCH 36/60] test(gyro_odometer): add test (#2743) * added test, now build failes due to the lack of tf? Signed-off-by: kminoda * now test works Signed-off-by: kminoda * added NG test Signed-off-by: kminoda * update license Signed-off-by: kminoda * ci(pre-commit): autofix * added explicit Signed-off-by: kminoda * update isAllOk Signed-off-by: kminoda * remove main function from library Signed-off-by: kminoda * refactor test Signed-off-by: kminoda * ci(pre-commit): autofix * refactor test Signed-off-by: kminoda * ci(pre-commit): autofix * update comment Signed-off-by: kminoda * update comment Signed-off-by: kminoda * ci(pre-commit): autofix * update comment Signed-off-by: kminoda * ci(pre-commit): autofix * update comment Signed-off-by: kminoda * remove output Signed-off-by: kminoda Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/gyro_odometer/CMakeLists.txt | 19 +++ .../gyro_odometer/gyro_odometer_core.hpp | 2 +- localization/gyro_odometer/package.xml | 1 + .../gyro_odometer/src/gyro_odometer_core.cpp | 4 +- .../gyro_odometer/src/gyro_odometer_node.cpp | 3 +- .../test/test_gyro_odometer_helper.cpp | 46 ++++++ .../test/test_gyro_odometer_helper.hpp | 30 ++++ .../test/test_gyro_odometer_pubsub.cpp | 151 ++++++++++++++++++ localization/gyro_odometer/test/test_main.cpp | 26 +++ 9 files changed, 278 insertions(+), 4 deletions(-) create mode 100644 localization/gyro_odometer/test/test_gyro_odometer_helper.cpp create mode 100644 localization/gyro_odometer/test/test_gyro_odometer_helper.hpp create mode 100644 localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp create mode 100644 localization/gyro_odometer/test/test_main.cpp diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index 0d53b64f16e36..59a5e1121b22f 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -11,6 +11,25 @@ ament_auto_add_executable(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} fmt) +ament_auto_add_library(gyro_odometer_node SHARED + src/gyro_odometer_core.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_gyro_odometer + test/test_main.cpp + test/test_gyro_odometer_pubsub.cpp + test/test_gyro_odometer_helper.cpp + ) + ament_target_dependencies(test_gyro_odometer + rclcpp + ) + target_link_libraries(test_gyro_odometer + gyro_odometer_node + ) +endif() + + ament_auto_package(INSTALL_TO_SHARE launch ) diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index c83447369855a..b0c7a5c4471c6 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -41,7 +41,7 @@ class GyroOdometer : public rclcpp::Node using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; public: - GyroOdometer(); + explicit GyroOdometer(const rclcpp::NodeOptions & options); ~GyroOdometer(); private: diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index 43717dbd49352..5aa85a8c4afe1 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -24,6 +24,7 @@ rclcpp rclcpp_components + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index b413779705af9..764f6c63812d1 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -100,8 +100,8 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( return twist_with_cov; } -GyroOdometer::GyroOdometer() -: Node("gyro_odometer"), +GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) +: Node("gyro_odometer", options), output_frame_(declare_parameter("base_link", "base_link")), message_timeout_sec_(declare_parameter("message_timeout_sec", 0.2)), vehicle_twist_arrived_(false), diff --git a/localization/gyro_odometer/src/gyro_odometer_node.cpp b/localization/gyro_odometer/src/gyro_odometer_node.cpp index 06fb094d208e9..5bb6241506fbe 100644 --- a/localization/gyro_odometer/src/gyro_odometer_node.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_node.cpp @@ -21,7 +21,8 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + rclcpp::NodeOptions options; + auto node = std::make_shared(options); rclcpp::spin(node); rclcpp::shutdown(); diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp b/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp new file mode 100644 index 0000000000000..54e1d320319d3 --- /dev/null +++ b/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 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 "test_gyro_odometer_helper.hpp" + +using geometry_msgs::msg::TwistWithCovarianceStamped; +using sensor_msgs::msg::Imu; + +Imu generateSampleImu() +{ + Imu imu; + imu.header.frame_id = "base_link"; + imu.angular_velocity.x = 0.1; + imu.angular_velocity.y = 0.2; + imu.angular_velocity.z = 0.3; + return imu; +} + +TwistWithCovarianceStamped generateSampleVelocity() +{ + TwistWithCovarianceStamped twist; + twist.header.frame_id = "base_link"; + twist.twist.twist.linear.x = 1.0; + return twist; +} + +rclcpp::NodeOptions getNodeOptionsWithDefaultParams() +{ + rclcpp::NodeOptions node_options; + + // for gyro_odometer + node_options.append_parameter_override("output_frame", "base_link"); + node_options.append_parameter_override("message_timeout_sec", 1e12); + return node_options; +} diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp b/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp new file mode 100644 index 0000000000000..6e3aff0b841a9 --- /dev/null +++ b/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp @@ -0,0 +1,30 @@ +// Copyright 2023 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. + +#ifndef TEST_GYRO_ODOMETER_HELPER_HPP_ +#define TEST_GYRO_ODOMETER_HELPER_HPP_ + +#include + +#include +#include + +using geometry_msgs::msg::TwistWithCovarianceStamped; +using sensor_msgs::msg::Imu; + +Imu generateSampleImu(); +TwistWithCovarianceStamped generateSampleVelocity(); +rclcpp::NodeOptions getNodeOptionsWithDefaultParams(); + +#endif // TEST_GYRO_ODOMETER_HELPER_HPP_ diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp new file mode 100644 index 0000000000000..7f6416fbdda16 --- /dev/null +++ b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -0,0 +1,151 @@ +// Copyright 2023 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 "gyro_odometer/gyro_odometer_core.hpp" +#include "test_gyro_odometer_helper.hpp" + +#include + +#include + +#include +#include + +/* + * This test checks if twist is published from gyro_odometer + */ +using geometry_msgs::msg::TwistWithCovarianceStamped; +using sensor_msgs::msg::Imu; + +class ImuGenerator : public rclcpp::Node +{ +public: + ImuGenerator() : Node("imu_generator"), imu_pub(create_publisher("/imu", 1)) {} + rclcpp::Publisher::SharedPtr imu_pub; +}; + +class VelocityGenerator : public rclcpp::Node +{ +public: + VelocityGenerator() + : Node("velocity_generator"), + vehicle_velocity_pub( + create_publisher("/vehicle/twist_with_covariance", 1)) + { + } + rclcpp::Publisher::SharedPtr vehicle_velocity_pub; +}; + +class GyroOdometerValidator : public rclcpp::Node +{ +public: + GyroOdometerValidator() : Node("gyro_odometer_validator"), received_latest_twist_ptr(nullptr) + { + twist_sub = create_subscription( + "/twist_with_covariance", 1, [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { + received_latest_twist_ptr = msg; + }); + } + + rclcpp::Subscription::SharedPtr twist_sub; + TwistWithCovarianceStamped::ConstSharedPtr received_latest_twist_ptr; +}; + +void spinSome(rclcpp::Node::SharedPtr node_ptr) +{ + for (int i = 0; i < 50; ++i) { + rclcpp::spin_some(node_ptr); + rclcpp::WallRate(100).sleep(); + } +} + +bool isTwistValid( + const TwistWithCovarianceStamped & twist, const TwistWithCovarianceStamped & twist_ground_truth) +{ + if (twist.twist.twist.linear.x != twist_ground_truth.twist.twist.linear.x) { + return false; + } + if (twist.twist.twist.linear.y != twist_ground_truth.twist.twist.linear.y) { + return false; + } + if (twist.twist.twist.linear.z != twist_ground_truth.twist.twist.linear.z) { + return false; + } + if (twist.twist.twist.angular.x != twist_ground_truth.twist.twist.angular.x) { + return false; + } + if (twist.twist.twist.angular.y != twist_ground_truth.twist.twist.angular.y) { + return false; + } + if (twist.twist.twist.angular.z != twist_ground_truth.twist.twist.angular.z) { + return false; + } + return true; +} + +// IMU & Velocity test +// Verify that the gyro_odometer successfully publishes the fused twist message when both IMU and +// velocity data are provided +TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) +{ + Imu input_imu = generateSampleImu(); + TwistWithCovarianceStamped input_velocity = generateSampleVelocity(); + + TwistWithCovarianceStamped expected_output_twist; + expected_output_twist.twist.twist.linear.x = input_velocity.twist.twist.linear.x; + expected_output_twist.twist.twist.angular.x = input_imu.angular_velocity.x; + expected_output_twist.twist.twist.angular.y = input_imu.angular_velocity.y; + expected_output_twist.twist.twist.angular.z = input_imu.angular_velocity.z; + + auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto imu_generator = std::make_shared(); + auto velocity_generator = std::make_shared(); + auto gyro_odometer_validator_node = std::make_shared(); + + velocity_generator->vehicle_velocity_pub->publish( + input_velocity); // need this for now, which should eventually be removed + imu_generator->imu_pub->publish(input_imu); + velocity_generator->vehicle_velocity_pub->publish(input_velocity); + + // gyro_odometer receives IMU and velocity, and publishes the fused twist data. + spinSome(gyro_odometer_node); + + // validator node receives the fused twist data and store in "received_latest_twist_ptr". + spinSome(gyro_odometer_validator_node); + + EXPECT_FALSE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr); + EXPECT_TRUE(isTwistValid( + *(gyro_odometer_validator_node->received_latest_twist_ptr), expected_output_twist)); +} + +// IMU-only test +// Verify that the gyro_odometer does NOT publish any outputs when only IMU is provided +TEST(GyroOdometer, TestGyroOdometerImuOnly) +{ + Imu input_imu = generateSampleImu(); + + auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto imu_generator = std::make_shared(); + auto gyro_odometer_validator_node = std::make_shared(); + + imu_generator->imu_pub->publish(input_imu); + + // gyro_odometer receives IMU + spinSome(gyro_odometer_node); + + // validator node waits for the output fused twist from gyro_odometer + spinSome(gyro_odometer_validator_node); + + EXPECT_TRUE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr); +} diff --git a/localization/gyro_odometer/test/test_main.cpp b/localization/gyro_odometer/test/test_main.cpp new file mode 100644 index 0000000000000..fa27d8ae5a938 --- /dev/null +++ b/localization/gyro_odometer/test/test_main.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 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 + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} From 7ac518fbf8c49e7e79ecbff10e1fb95c72190738 Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Thu, 26 Jan 2023 11:34:55 +0900 Subject: [PATCH 37/60] fix(costmap_generator): fix invalid argument order (#2751) function `object_map::FillPolygonAreas` is declared with arguments `..., in_layer_background_value, in_fill_color, in_layer_min_value, in_layer_max_value,...`, but implementation has different order: `..., in_layer_background_value, in_layer_min_value, in_fill_color, in_layer_max_value,...` The function is only used once in `costmap_generator_node.cpp`, for which `in_layer_min_value == in_fill_color`, so fixing the argument order has no impact on the code behavior. --- .../nodes/costmap_generator/object_map_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp b/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp index 0dbf1feb5561b..6955798be3e8a 100644 --- a/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp @@ -61,7 +61,7 @@ void FillPolygonAreas( grid_map::GridMap & out_grid_map, const std::vector> & in_points, const std::string & in_grid_layer_name, const int in_layer_background_value, - const int in_layer_min_value, const int in_fill_color, const int in_layer_max_value, + const int in_fill_color, const int in_layer_min_value, const int in_layer_max_value, const std::string & in_tf_target_frame, const std::string & in_tf_source_frame, const tf2_ros::Buffer & in_tf_buffer) { From 23ad8eba44b3c192f407a2725b8343474944a3b2 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 26 Jan 2023 14:27:32 +0900 Subject: [PATCH 38/60] fix(lane_change): fix bug when target reference path is empty (#2752) * fix(lane_change): fix bug when target reference path is empty Signed-off-by: tomoya.kimura * fix Signed-off-by: tomoya.kimura Signed-off-by: tomoya.kimura --- .../src/scene_module/lane_change/util.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 718c1ef88a4b0..85454cc449e3e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -374,6 +374,9 @@ LaneChangePaths getLaneChangePaths( const auto target_lane_reference_path = getReferencePathFromTargetLane( route_handler, target_lanelets, lane_changing_start_pose, target_lane_length, lc_dist, required_total_min_distance, forward_path_length, resample_interval, is_goal_in_route); + if (target_lane_reference_path.points.empty()) { + continue; + } const ShiftLine shift_line = getLaneChangeShiftLine( prepare_segment_reference, lane_changing_segment_reference, target_lanelets, From deb4c41fc41902677b4b930626d24d158c4c1547 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 26 Jan 2023 15:10:13 +0900 Subject: [PATCH 39/60] refactor(tier4_map_launch): remove unused config (#2722) * refactor(tier4_map_launch): remove unused config Signed-off-by: kminoda * load lanelet2 parameter from upper level Signed-off-by: kminoda * revert the addition of lanelet2 param Signed-off-by: kminoda Signed-off-by: kminoda --- launch/tier4_map_launch/CMakeLists.txt | 1 - .../config/pointcloud_map_loader.param.yaml | 9 --------- launch/tier4_map_launch/launch/map.launch.py | 6 +----- launch/tier4_map_launch/launch/map.launch.xml | 2 +- 4 files changed, 2 insertions(+), 16 deletions(-) delete mode 100644 launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml diff --git a/launch/tier4_map_launch/CMakeLists.txt b/launch/tier4_map_launch/CMakeLists.txt index b0e770d7c7619..98f12b64f3ca4 100644 --- a/launch/tier4_map_launch/CMakeLists.txt +++ b/launch/tier4_map_launch/CMakeLists.txt @@ -7,5 +7,4 @@ autoware_package() ament_auto_package( INSTALL_TO_SHARE launch - config ) diff --git a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml deleted file mode 100644 index 8f3ccbff00360..0000000000000 --- a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml +++ /dev/null @@ -1,9 +0,0 @@ -/**: - ros__parameters: - enable_whole_load: true - enable_downsampled_whole_load: false - enable_partial_load: false - enable_differential_load: false - - # only used when downsample_whole_load enabled - leaf_size: 3.0 # downsample leaf size [m] diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index 3be3082e58040..867eb99b8f8b3 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -161,11 +161,7 @@ def add_launch_arg(name: str, default_value=None, description=None): ), add_launch_arg( "pointcloud_map_loader_param_path", - [ - FindPackageShare("tier4_map_launch"), - "/config/pointcloud_map_loader.param.yaml", - # ToDo(kminoda): This file should eventually be removed as well as the other components - ], + None, "path to pointcloud_map_loader param file", ), add_launch_arg("use_intra_process", "false", "use ROS2 component container communication"), diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 39a398e7c1d77..7ecc61605eeac 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -1,6 +1,6 @@ - + From 379456ba48ed07ef878dd75923c89fc9a9cb2189 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 26 Jan 2023 09:39:48 +0100 Subject: [PATCH 40/60] build(gnss_poser): add build dependency (#2737) --- sensing/gnss_poser/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index c551f8001001a..250122b929e7f 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -11,6 +11,7 @@ ament_cmake_auto autoware_cmake + libboost-dev autoware_sensing_msgs geo_pos_conv From 3523270f6903317aed6ceb51f34c565f32598ef4 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 27 Jan 2023 09:55:03 +0900 Subject: [PATCH 41/60] fix(lane_change): set constructCandidatePath logger as debug (#2731) fix(lane change): Set constructCandidatePath logger as debug Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_planner/src/scene_module/lane_change/util.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 85454cc449e3e..1059a1187684c 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -198,7 +198,7 @@ std::optional constructCandidatePath( path_shifter.setLateralAccelerationLimit(std::abs(lane_change_param.lane_changing_lateral_acc)); if (!path_shifter.generate(&shifted_path, offset_back)) { - RCLCPP_ERROR_STREAM( + RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), "failed to generate shifted path."); } From 7b16fb81f2834419dac74415e00415f0fd3ec7cb Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 27 Jan 2023 13:15:48 +0900 Subject: [PATCH 42/60] chore(control_launch): add maintainer (#2758) Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe --- launch/tier4_control_launch/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml index bcb705d6b885e..108e43915b26f 100644 --- a/launch/tier4_control_launch/package.xml +++ b/launch/tier4_control_launch/package.xml @@ -5,6 +5,7 @@ 0.1.0 The tier4_control_launch package Takamasa Horibe + Takayuki Murooka Apache License 2.0 ament_cmake_auto From 091160047d77f5cfd140aef8b8a58681590b9b80 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 27 Jan 2023 14:04:43 +0900 Subject: [PATCH 43/60] feat(intersection): add param for stuck stopline overshoot margin (#2756) Signed-off-by: Mamoru Sobue --- .../behavior_velocity_planner/config/intersection.param.yaml | 1 + .../include/scene_module/intersection/scene_intersection.hpp | 1 + .../src/scene_module/intersection/manager.cpp | 1 + .../src/scene_module/intersection/scene_intersection.cpp | 3 +-- 4 files changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index a769848497680..9ed153bb1c3d4 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -20,6 +20,7 @@ use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning enable_front_car_decel_prediction: false # By default this feature is disabled + stop_overshoot_margin: 0.5 # [m] overshoot margin for stuck, collsion detection merge_from_private_road: stop_duration_sec: 1.0 diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 98589555bd7aa..7ac5c25ce38e1 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -93,6 +93,7 @@ class IntersectionModule : public SceneModuleInterface double assumed_front_car_decel; //! the expected deceleration of front car when front car as well bool enable_front_car_decel_prediction; //! flag for using above feature + double stop_overshoot_margin; //! overshoot margin for stuck, collsion detection }; IntersectionModule( diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index a8af4938b1bf0..8f4ee1c91cee2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -69,6 +69,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.assumed_front_car_decel = node.declare_parameter(ns + ".assumed_front_car_decel", 1.0); ip.enable_front_car_decel_prediction = node.declare_parameter(ns + ".enable_front_car_decel_prediction", false); + ip.stop_overshoot_margin = node.declare_parameter(ns + ".stop_overshoot_margin", 0.5); } MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node) diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index bbe6955c2a601..ed44be0508ee5 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -202,10 +202,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const double dist_stuck_stopline = motion_utils::calcSignedArcLength( path->points, path->points.at(stuck_line_idx).point.pose.position, path->points.at(closest_idx).point.pose.position); - const double eps = 1e-1; // NOTE: check if sufficiently over the stuck stopline const bool is_over_stuck_stopline = util::isOverTargetIndex(*path, closest_idx, current_pose, stuck_line_idx) && - dist_stuck_stopline > eps; + dist_stuck_stopline > planner_param_.stop_overshoot_margin; if (is_stuck && !is_over_stuck_stopline) { stop_line_idx_final = stuck_line_idx; pass_judge_line_idx_final = stuck_line_idx; From 0717cd4e4e975fed172f88c7dc74075a888ba5f9 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 27 Jan 2023 14:10:32 +0900 Subject: [PATCH 44/60] fix(motion_utils): fixed virtual wall marker to appear (#2757) Signed-off-by: Mamoru Sobue --- common/motion_utils/src/marker/marker_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index 60e5f09bbea89..ab4dc623c3125 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -143,7 +143,7 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWall size_t id_to_delete = id; visualization_msgs::msg::MarkerArray wall_marker; - if (poses.size() == 0 || previous_virtual_wall_poses.empty()) { + if (poses.size() == 0) { return wall_marker; } From 1f62b911fc845da9789f52337d7d1076ee5de2cc Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri, 27 Jan 2023 14:42:18 +0900 Subject: [PATCH 45/60] fix(euclidean_cluster): keep pointcloud resolution in long-range for Euclidean cluster (#2749) * fix: change downnsample split pipeline Signed-off-by: badai-nguyen * fix: add downsample range params Signed-off-by: badai-nguyen * fix: revise disuse map_filter case Signed-off-by: badai-nguyen Signed-off-by: badai-nguyen --- ...el_grid_based_euclidean_cluster.param.yaml | 6 + ...xel_grid_based_euclidean_cluster.launch.py | 118 +++++++++++++++--- 2 files changed, 110 insertions(+), 14 deletions(-) diff --git a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml index 27ba5a8178fcf..1ff1e7c8ab55e 100644 --- a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml +++ b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml @@ -6,3 +6,9 @@ min_cluster_size: 10 max_cluster_size: 3000 use_height: false + max_x: 70.0 + min_x: -70.0 + max_y: 70.0 + min_y: -70.0 + max_z: 4.5 + min_z: -4.5 diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 2cfc5142f6fd8..f523245092564 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -49,23 +49,85 @@ def load_composable_node_param(param_path): parameters=[load_composable_node_param("compare_map_param_path")], ) - # set voxel grid filter as a component - use_map_voxel_grid_filter_component = ComposableNode( + # separate range of poincloud when map_filter used + use_map_short_range_crop_box_filter_component = ComposableNode( package="pointcloud_preprocessor", namespace=ns, - plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", - name=AnonName("voxel_grid_filter"), - remappings=[("input", "map_filter/pointcloud"), ("output", "downsampled/pointcloud")], - parameters=[load_composable_node_param("voxel_grid_param_path")], + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="short_distance_crop_box_range", + remappings=[ + ("input", "map_filter/pointcloud"), + ("output", "short_range/pointcloud"), + ], + parameters=[ + load_composable_node_param("voxel_grid_based_euclidean_param_path"), + { + "negative": False, + }, + ], ) - disuse_map_voxel_grid_filter_component = ComposableNode( + + use_map_long_range_crop_box_filter_component = ComposableNode( + package="pointcloud_preprocessor", + namespace=ns, + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="long_distance_crop_box_range", + remappings=[ + ("input", LaunchConfiguration("input_pointcloud")), + ("output", "long_range/pointcloud"), + ], + parameters=[ + load_composable_node_param("voxel_grid_based_euclidean_param_path"), + { + "negative": True, + }, + ], + ) + + # disuse_map_voxel_grid_filter + disuse_map_short_range_crop_box_filter_component = ComposableNode( + package="pointcloud_preprocessor", + namespace=ns, + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="short_distance_crop_box_range", + remappings=[ + ("input", LaunchConfiguration("input_pointcloud")), + ("output", "short_range/pointcloud"), + ], + parameters=[ + load_composable_node_param("voxel_grid_based_euclidean_param_path"), + { + "negative": False, + }, + ], + ) + + disuse_map_long_range_crop_box_filter_component = ComposableNode( + package="pointcloud_preprocessor", + namespace=ns, + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="long_distance_crop_box_range", + remappings=[ + ("input", LaunchConfiguration("input_pointcloud")), + ("output", "long_range/pointcloud"), + ], + parameters=[ + load_composable_node_param("voxel_grid_based_euclidean_param_path"), + { + "negative": True, + }, + ], + ) + + # set voxel grid filter as a component + voxel_grid_filter_component = ComposableNode( package="pointcloud_preprocessor", namespace=ns, plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", name=AnonName("voxel_grid_filter"), remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "downsampled/pointcloud"), + ("input", "short_range/pointcloud"), + ("output", "downsampled/short_range/pointcloud"), ], parameters=[load_composable_node_param("voxel_grid_param_path")], ) @@ -76,10 +138,29 @@ def load_composable_node_param(param_path): namespace=ns, plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent", name="outlier_filter", - remappings=[("input", "downsampled/pointcloud"), ("output", "outlier_filter/pointcloud")], + remappings=[ + ("input", "downsampled/short_range/pointcloud"), + ("output", "outlier_filter/pointcloud"), + ], parameters=[load_composable_node_param("outlier_param_path")], ) + # concat with-outlier pointcloud and without-outlier pcl + downsample_concat_component = ComposableNode( + package="pointcloud_preprocessor", + namespace=ns, + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="concat_downsampled_pcl", + remappings=[("output", "downsampled/concatenated/pointcloud")], + parameters=[ + { + "input_topics": ["long_range/pointcloud", "outlier_filter/pointcloud"], + "output_frame": "base_link", + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + # set euclidean cluster as a component euclidean_cluster_component = ComposableNode( package=pkg, @@ -87,7 +168,7 @@ def load_composable_node_param(param_path): plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ - ("input", "outlier_filter/pointcloud"), + ("input", "downsampled/concatenated/pointcloud"), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], @@ -98,21 +179,30 @@ def load_composable_node_param(param_path): package="rclcpp_components", namespace=ns, executable="component_container", - composable_node_descriptions=[outlier_filter_component, euclidean_cluster_component], + composable_node_descriptions=[ + voxel_grid_filter_component, + outlier_filter_component, + downsample_concat_component, + euclidean_cluster_component, + ], output="screen", ) use_map_loader = LoadComposableNodes( composable_node_descriptions=[ compare_map_filter_component, - use_map_voxel_grid_filter_component, + use_map_short_range_crop_box_filter_component, + use_map_long_range_crop_box_filter_component, ], target_container=container, condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), ) disuse_map_loader = LoadComposableNodes( - composable_node_descriptions=[disuse_map_voxel_grid_filter_component], + composable_node_descriptions=[ + disuse_map_short_range_crop_box_filter_component, + disuse_map_long_range_crop_box_filter_component, + ], target_container=container, condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), ) From 642f1722c99ac6908044d37ef5fb9b33f063154f Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Fri, 27 Jan 2023 14:46:20 +0900 Subject: [PATCH 46/60] fix(raw_vehicle_cmd_converter): remove external command converter validation (#2711) * fix(raw_vehicle_cmd_converter): remove external command converter validation Signed-off-by: taikitanaka3 * chore: fix Signed-off-by: taikitanaka3 --- .../raw_vehicle_cmd_converter/accel_map.hpp | 2 +- .../raw_vehicle_cmd_converter/brake_map.hpp | 2 +- .../raw_vehicle_cmd_converter/steer_map.hpp | 2 +- .../src/accel_map.cpp | 4 ++-- .../src/brake_map.cpp | 4 ++-- vehicle/raw_vehicle_cmd_converter/src/node.cpp | 6 +++--- .../src/steer_map.cpp | 4 ++-- .../test/test_raw_vehicle_cmd_converter.cpp | 18 +++++++++--------- 8 files changed, 21 insertions(+), 21 deletions(-) diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp index fba69efdbc921..3bdb71a11f744 100644 --- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp @@ -29,7 +29,7 @@ namespace raw_vehicle_cmd_converter class AccelMap { public: - bool readAccelMapFromCSV(const std::string & csv_path); + bool readAccelMapFromCSV(const std::string & csv_path, const bool validation = false); bool getThrottle(const double acc, const double vel, double & throttle) const; bool getAcceleration(const double throttle, const double vel, double & acc) const; std::vector getVelIdx() const { return vel_index_; } diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp index 8d4d35545254c..6dd5ab94c5d06 100644 --- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp @@ -29,7 +29,7 @@ namespace raw_vehicle_cmd_converter class BrakeMap { public: - bool readBrakeMapFromCSV(const std::string & csv_path); + bool readBrakeMapFromCSV(const std::string & csv_path, const bool validation = false); bool getBrake(const double acc, const double vel, double & brake); bool getAcceleration(const double brake, const double vel, double & acc) const; std::vector getVelIdx() const { return vel_index_; } diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_map.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_map.hpp index 5bdfdef639d20..7e4a3084d0223 100644 --- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_map.hpp +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_map.hpp @@ -28,7 +28,7 @@ namespace raw_vehicle_cmd_converter class SteerMap { public: - bool readSteerMapFromCSV(const std::string & csv_path); + bool readSteerMapFromCSV(const std::string & csv_path, const bool validation = false); void getSteer(const double steer_rate, const double steer, double & output) const; private: diff --git a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp index 9ebb4bdbb5df7..42c63b152e4f4 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp @@ -25,7 +25,7 @@ using namespace std::literals::chrono_literals; namespace raw_vehicle_cmd_converter { -bool AccelMap::readAccelMapFromCSV(const std::string & csv_path) +bool AccelMap::readAccelMapFromCSV(const std::string & csv_path, const bool validation) { CSVLoader csv(csv_path); std::vector> table; @@ -38,7 +38,7 @@ bool AccelMap::readAccelMapFromCSV(const std::string & csv_path) vel_index_ = CSVLoader::getRowIndex(table); throttle_index_ = CSVLoader::getColumnIndex(table); accel_map_ = CSVLoader::getMap(table); - if (!CSVLoader::validateMap(accel_map_, true)) { + if (validation && !CSVLoader::validateMap(accel_map_, true)) { return false; } return true; diff --git a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp index 154a0fc455a0a..68d89474f3ca6 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp @@ -22,7 +22,7 @@ namespace raw_vehicle_cmd_converter { -bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path) +bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path, const bool validation) { CSVLoader csv(csv_path); std::vector> table; @@ -36,7 +36,7 @@ bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path) brake_index_ = CSVLoader::getColumnIndex(table); brake_map_ = CSVLoader::getMap(table); brake_index_rev_ = brake_index_; - if (!CSVLoader::validateMap(brake_map_, false)) { + if (validation && !CSVLoader::validateMap(brake_map_, false)) { return false; } std::reverse(std::begin(brake_index_rev_), std::end(brake_index_rev_)); diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp index 007cce0b66cff..fb0bd00794c52 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -42,17 +42,17 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( use_steer_ff_ = declare_parameter("use_steer_ff", true); use_steer_fb_ = declare_parameter("use_steer_fb", true); if (convert_accel_cmd_) { - if (!accel_map_.readAccelMapFromCSV(csv_path_accel_map)) { + if (!accel_map_.readAccelMapFromCSV(csv_path_accel_map, true)) { throw std::invalid_argument("Accel map is invalid."); } } if (convert_brake_cmd_) { - if (!brake_map_.readBrakeMapFromCSV(csv_path_brake_map)) { + if (!brake_map_.readBrakeMapFromCSV(csv_path_brake_map, true)) { throw std::invalid_argument("Brake map is invalid."); } } if (convert_steer_cmd_) { - if (!steer_map_.readSteerMapFromCSV(csv_path_steer_map)) { + if (!steer_map_.readSteerMapFromCSV(csv_path_steer_map, true)) { throw std::invalid_argument("Steer map is invalid."); } const auto kp_steer{declare_parameter("steer_pid.kp", 150.0)}; diff --git a/vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp index 73a6641dd424d..e050a1beba3ae 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp @@ -22,7 +22,7 @@ namespace raw_vehicle_cmd_converter { -bool SteerMap::readSteerMapFromCSV(const std::string & csv_path) +bool SteerMap::readSteerMapFromCSV(const std::string & csv_path, const bool validation) { CSVLoader csv(csv_path); std::vector> table; @@ -35,7 +35,7 @@ bool SteerMap::readSteerMapFromCSV(const std::string & csv_path) steer_index_ = CSVLoader::getRowIndex(table); output_index_ = CSVLoader::getColumnIndex(table); steer_map_ = CSVLoader::getMap(table); - if (!CSVLoader::validateMap(steer_map_, true)) { + if (validation && !CSVLoader::validateMap(steer_map_, true)) { return false; } return true; diff --git a/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp b/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp index 3c8eba3072126..9f035b303e1a9 100644 --- a/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp +++ b/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp @@ -95,9 +95,9 @@ TEST(ConverterTests, LoadExampleMap) const auto data_path = ament_index_cpp::get_package_share_directory("raw_vehicle_cmd_converter") + "/data/default/"; // for invalid path - EXPECT_TRUE(accel_map.readAccelMapFromCSV(data_path + "accel_map.csv")); - EXPECT_TRUE(brake_map.readBrakeMapFromCSV(data_path + "brake_map.csv")); - EXPECT_TRUE(steer_map.readSteerMapFromCSV(data_path + "steer_map.csv")); + EXPECT_TRUE(accel_map.readAccelMapFromCSV(data_path + "accel_map.csv", true)); + EXPECT_TRUE(brake_map.readBrakeMapFromCSV(data_path + "brake_map.csv", true)); + EXPECT_TRUE(steer_map.readSteerMapFromCSV(data_path + "steer_map.csv", true)); } TEST(ConverterTests, LoadValidPath) @@ -112,14 +112,14 @@ TEST(ConverterTests, LoadValidPath) EXPECT_TRUE(loadSteerMapData(steer_map)); // for invalid path - EXPECT_FALSE(accel_map.readAccelMapFromCSV("invalid.csv")); - EXPECT_FALSE(brake_map.readBrakeMapFromCSV("invalid.csv")); - EXPECT_FALSE(steer_map.readSteerMapFromCSV("invalid.csv")); + EXPECT_FALSE(accel_map.readAccelMapFromCSV("invalid.csv", true)); + EXPECT_FALSE(brake_map.readBrakeMapFromCSV("invalid.csv", true)); + EXPECT_FALSE(steer_map.readSteerMapFromCSV("invalid.csv", true)); // for invalid maps - EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_1col_map.csv")); - EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_inconsistent_rows_map.csv")); - EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_not_interpolatable.csv")); + EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_1col_map.csv", true)); + EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_inconsistent_rows_map.csv", true)); + EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_not_interpolatable.csv", true)); } TEST(ConverterTests, AccelMapCalculation) From d5a627e3bbec07a6e1aedfaafcee0c2bf8f65a17 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 27 Jan 2023 15:41:53 +0900 Subject: [PATCH 47/60] chore(intersection): remove unnecessary marker (#2760) Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.hpp | 3 -- .../src/scene_module/intersection/debug.cpp | 35 +++---------------- .../intersection/scene_intersection.cpp | 10 +----- 3 files changed, 6 insertions(+), 42 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 7ac5c25ce38e1..90bdbea51297a 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -50,9 +50,6 @@ class IntersectionModule : public SceneModuleInterface geometry_msgs::msg::Pose slow_wall_pose; geometry_msgs::msg::Pose stop_wall_pose; - geometry_msgs::msg::Pose stop_point_pose; - geometry_msgs::msg::Pose judge_point_pose; - geometry_msgs::msg::Polygon ego_lane_polygon; geometry_msgs::msg::Polygon stuck_vehicle_detect_area; geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon; std::vector candidate_collision_object_polygons; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 23ebdcef0f09a..ccaca32f8245c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -109,7 +109,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( { visualization_msgs::msg::MarkerArray debug_marker_array; - const auto state = state_machine_.getState(); const auto now = this->clock_->now(); appendMarkerArray( @@ -128,17 +127,13 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( 0.0), &debug_marker_array); - appendMarkerArray( - debug::createPolygonMarkerArray( - debug_data_.ego_lane_polygon, "ego_lane", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 0.3, 0.7), - &debug_marker_array); - appendMarkerArray( debug::createPolygonMarkerArray( debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 0.5, 0.5), &debug_marker_array, now); + // TODO(Mamoru Sobue): should be changed path polygon for LC appendMarkerArray( debug::createPolygonMarkerArray( debug_data_.candidate_collision_ego_lane_polygon, "candidate_collision_ego_lane_polygon", @@ -169,18 +164,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.predicted_obj_pose, "predicted_obj_pose", module_id_, 0.7, 0.85, 0.9), &debug_marker_array, now); - if (state == StateMachine::State::STOP) { - appendMarkerArray( - createPoseMarkerArray( - debug_data_.stop_point_pose, "stop_point_pose", module_id_, 1.0, 0.0, 0.0), - &debug_marker_array, now); - - appendMarkerArray( - createPoseMarkerArray( - debug_data_.judge_point_pose, "judge_point_pose", module_id_, 1.0, 1.0, 0.5), - &debug_marker_array, now); - } - return debug_marker_array; } @@ -189,19 +172,11 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker visualization_msgs::msg::MarkerArray wall_marker; const auto now = this->clock_->now(); - const auto state = state_machine_.getState(); - if (debug_data_.stop_required) { - appendMarkerArray( - virtual_wall_marker_creator_->createStopVirtualWallMarker( - {debug_data_.stop_wall_pose}, "intersection", now, module_id_), - &wall_marker, now); - } else if (state == StateMachine::State::STOP) { - appendMarkerArray( - virtual_wall_marker_creator_->createStopVirtualWallMarker( - {debug_data_.slow_wall_pose}, "intersection", now, module_id_), - &wall_marker, now); - } + appendMarkerArray( + virtual_wall_marker_creator_->createStopVirtualWallMarker( + {debug_data_.stop_wall_pose}, "intersection", now, module_id_), + &wall_marker, now); return wall_marker; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index ed44be0508ee5..0096b922ea75f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -191,8 +191,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * /* calculate final stop lines */ int stop_line_idx_final = stop_lines_idx_opt.has_value() ? stop_lines_idx_opt.value().stop_line : -1; - int pass_judge_line_idx_final = - stop_lines_idx_opt.has_value() ? stop_lines_idx_opt.value().pass_judge_line : -1; if (external_go) { is_entry_prohibited = false; } else if (external_stop) { @@ -207,11 +205,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * dist_stuck_stopline > planner_param_.stop_overshoot_margin; if (is_stuck && !is_over_stuck_stopline) { stop_line_idx_final = stuck_line_idx; - pass_judge_line_idx_final = stuck_line_idx; } else if ( ((is_stuck && is_over_stuck_stopline) || has_collision) && stop_lines_idx_opt.has_value()) { stop_line_idx_final = stop_lines_idx_opt.value().stop_line; - pass_judge_line_idx_final = stop_lines_idx_opt.value().pass_judge_line; } } @@ -242,13 +238,11 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.stop_wall_pose = planning_utils::getAheadPose(stop_line_idx_final, base_link2front, *path); - debug_data_.stop_point_pose = path->points.at(stop_line_idx_final).point.pose; - debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx_final).point.pose; // Get stop point and stop factor { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = debug_data_.stop_point_pose; + stop_factor.stop_pose = path->points.at(stop_line_idx_final).point.pose; const auto stop_factor_conflict = planning_utils::toRosPoints(debug_data_.conflicting_targets); const auto stop_factor_stuck = planning_utils::toRosPoints(debug_data_.stuck_targets); @@ -317,8 +311,6 @@ bool IntersectionModule::checkCollision( ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point), &closest_lanelet); - debug_data_.ego_lane_polygon = toGeomPoly(ego_poly); - /* extract target objects */ autoware_auto_perception_msgs::msg::PredictedObjects target_objects; target_objects.header = objects_ptr->header; From 2d8c4f5ce8a692fbd1e5cb0bf3099d5b3ecb4176 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 27 Jan 2023 20:02:03 +0900 Subject: [PATCH 48/60] feat(longitudinal_controller): add feedforward conversion to arc coordinate (#2729) * feat(longitudinal_controller): add feedforward conversion to arc coordinate Signed-off-by: Takamasa Horibe * fix build brake Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe --- .../pid_longitudinal_controller/debug_values.hpp | 1 + .../src/pid_longitudinal_controller.cpp | 15 ++++++++++++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp index f1d7dab6c5b09..51f420470ff34 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp @@ -55,6 +55,7 @@ class DebugValues PITCH_RAW_TRAJ_RAD = 26, PITCH_RAW_TRAJ_DEG = 27, STOP_DIST = 28, + FF_SCALE = 29, SIZE // this is the number of enum elements }; diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 01641244cc14e..dd73d6413c65e 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -914,13 +914,26 @@ double PidLongitudinalController::applyVelocityFeedback( std::vector pid_contributions(3); const double pid_acc = m_pid_vel.calculate(error_vel_filtered, dt, enable_integration, pid_contributions); - const double feedback_acc = target_motion.acc + pid_acc; + + // Feedforward scaling: + // This is for the coordinate convertion where feedforward is applied, from Time to Arclength. + // Details: For accurate control, the feedforward should be calculated in the arclength coordinate + // system, not in the time coordinate system. Otherwise, even if FF is applied, the vehicle speed + // deviation will be bigger. + constexpr double ff_scale_max = 2.0; // for safety + constexpr double ff_scale_min = 0.5; // for safety + const double ff_scale = + std::clamp(current_vel_abs / std::max(target_vel_abs, 0.1), ff_scale_min, ff_scale_max); + const double ff_acc = target_motion.acc * ff_scale; + + const double feedback_acc = ff_acc + pid_acc; m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PID_APPLIED, feedback_acc); m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL_FILTERED, error_vel_filtered); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_FB_P_CONTRIBUTION, pid_contributions.at(0)); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_FB_I_CONTRIBUTION, pid_contributions.at(1)); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_FB_D_CONTRIBUTION, pid_contributions.at(2)); + m_debug_values.setValues(DebugValues::TYPE::FF_SCALE, ff_acc); return feedback_acc; } From d3e571465f442621acb95ced9384bd3db84d50b0 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Fri, 27 Jan 2023 11:14:49 +0000 Subject: [PATCH 49/60] chore: update CODEOWNERS (#2763) Signed-off-by: GitHub Signed-off-by: GitHub Co-authored-by: TakaHoribe --- .github/CODEOWNERS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 0172b42878437..e6d88c3df9934 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -61,7 +61,7 @@ control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @aut evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai @autowarefoundation/autoware-global-codeowners evaluator/localization_evaluator/** dominik.jargot@robotec.ai @autowarefoundation/autoware-global-codeowners launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/tier4_control_launch/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners +launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_localization_launch/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_map_launch/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp @autowarefoundation/autoware-global-codeowners launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners From 594b248796048e3da3f78cb827e1efe9b9f37b32 Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Fri, 27 Jan 2023 20:38:58 +0900 Subject: [PATCH 50/60] fix(mpc_lateral_controller): fix inconsistent argument order (#2750) `DynamicsBicycleModel::calculateDiscreteMatrix` is declared with arguments `a_d, b_d, w_d, c_d, dt`, but implementation uses `a_d, b_d, c_d, w_d, dt` order. Usage in mpc.cpp follows implementation. Signed-off-by: Vincent Richard Signed-off-by: Vincent Richard --- .../vehicle_model/vehicle_model_bicycle_dynamics.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp index c6f27ab3219d8..b167efe28d810 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -89,7 +89,7 @@ class DynamicsBicycleModel : public VehicleModelInterface * @param [in] dt Discretization time [s] */ void calculateDiscreteMatrix( - Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & w_d, Eigen::MatrixXd & c_d, + Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, const double dt) override; /** From 75ae084658ecfea090bc5386568ed7b919f75d3c Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 27 Jan 2023 20:48:52 +0900 Subject: [PATCH 51/60] feat(longitudinal_controller): skip integral in manual mode (#2619) * feat(longitudinal_controller): skip integral in manual mode Signed-off-by: Takamasa Horibe * change control_mode to operation_mode Signed-off-by: Takamasa Horibe * fix test Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe --- .../pid_longitudinal_controller.hpp | 10 ++++++- .../pid_longitudinal_controller/package.xml | 1 + .../src/pid_longitudinal_controller.cpp | 10 ++++++- .../trajectory_follower_base/input_data.hpp | 2 ++ control/trajectory_follower_base/package.xml | 1 + .../controller_node.hpp | 4 +++ control/trajectory_follower_node/package.xml | 1 + .../src/controller_node.cpp | 9 ++++++ .../test/test_controller_node.cpp | 28 +++++++++++++++++++ .../launch/control.launch.py | 1 + 10 files changed, 65 insertions(+), 2 deletions(-) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 505fede84647c..4112171633af9 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -30,6 +30,7 @@ #include "trajectory_follower_base/longitudinal_controller_base.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" +#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" @@ -46,7 +47,7 @@ namespace autoware::motion::control::pid_longitudinal_controller { - +using autoware_adapi_v1_msgs::msg::OperationModeState; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; /// \class PidLongitudinalController @@ -88,6 +89,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro nav_msgs::msg::Odometry m_current_kinematic_state; geometry_msgs::msg::AccelWithCovarianceStamped m_current_accel; autoware_auto_planning_msgs::msg::Trajectory m_trajectory; + OperationModeState m_current_operation_mode; // vehicle info double m_wheel_base; @@ -225,6 +227,12 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro */ void setCurrentAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped & msg); + /** + * @brief set current operation mode with received message + * @param [in] msg operation mode report message + */ + void setCurrentOperationMode(const OperationModeState & msg); + /** * @brief set reference trajectory with received message * @param [in] msg trajectory message diff --git a/control/pid_longitudinal_controller/package.xml b/control/pid_longitudinal_controller/package.xml index e3ff39b187d14..8473ead09cce7 100644 --- a/control/pid_longitudinal_controller/package.xml +++ b/control/pid_longitudinal_controller/package.xml @@ -18,6 +18,7 @@ autoware_cmake + autoware_adapi_v1_msgs autoware_auto_control_msgs autoware_auto_geometry autoware_auto_planning_msgs diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index dd73d6413c65e..16b477387a598 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -203,6 +203,11 @@ void PidLongitudinalController::setCurrentAcceleration( m_current_accel = msg; } +void PidLongitudinalController::setCurrentOperationMode(const OperationModeState & msg) +{ + m_current_operation_mode = msg; +} + void PidLongitudinalController::setTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & msg) { @@ -357,6 +362,7 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run( setTrajectory(input_data.current_trajectory); setKinematicState(input_data.current_odometry); setCurrentAcceleration(input_data.current_accel); + setCurrentOperationMode(input_data.current_operation_mode); // calculate current pose and control data geometry_msgs::msg::Pose current_pose = m_current_kinematic_state.pose.pose; @@ -908,7 +914,9 @@ double PidLongitudinalController::applyVelocityFeedback( { const double current_vel_abs = std::fabs(current_vel); const double target_vel_abs = std::fabs(target_motion.vel); - const bool enable_integration = (current_vel_abs > m_current_vel_threshold_pid_integrate); + const bool is_under_control = m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; + const bool enable_integration = + (current_vel_abs > m_current_vel_threshold_pid_integrate) && is_under_control; const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs); std::vector pid_contributions(3); diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp index 705f4df9088ee..52a82526a9548 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp @@ -15,6 +15,7 @@ #ifndef TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ #define TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ +#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" @@ -28,6 +29,7 @@ struct InputData nav_msgs::msg::Odometry current_odometry; autoware_auto_vehicle_msgs::msg::SteeringReport current_steering; geometry_msgs::msg::AccelWithCovarianceStamped current_accel; + autoware_adapi_v1_msgs::msg::OperationModeState current_operation_mode; }; } // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower_base/package.xml b/control/trajectory_follower_base/package.xml index 7482cbf190be3..1a231bec3cc7a 100644 --- a/control/trajectory_follower_base/package.xml +++ b/control/trajectory_follower_base/package.xml @@ -20,6 +20,7 @@ autoware_cmake + autoware_adapi_v1_msgs autoware_auto_control_msgs autoware_auto_geometry autoware_auto_planning_msgs diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index 0f0baccc4e29d..6fc7a0cbedc59 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -48,6 +48,8 @@ using trajectory_follower::LongitudinalOutput; namespace trajectory_follower_node { +using autoware_adapi_v1_msgs::msg::OperationModeState; + namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; /// \classController @@ -70,6 +72,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr sub_steering_; rclcpp::Subscription::SharedPtr sub_accel_; + rclcpp::Subscription::SharedPtr sub_operation_mode_; rclcpp::Publisher::SharedPtr control_cmd_pub_; rclcpp::Publisher::SharedPtr debug_marker_pub_; @@ -78,6 +81,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr_; autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr_; geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr_; + OperationModeState::SharedPtr current_operation_mode_ptr_; enum class LateralControllerMode { INVALID = 0, diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml index 7a32ac96d1555..c524d07fcff79 100644 --- a/control/trajectory_follower_node/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -20,6 +20,7 @@ autoware_cmake + autoware_adapi_v1_msgs autoware_auto_control_msgs autoware_auto_planning_msgs autoware_auto_system_msgs diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 69b68bd456977..b9966263a5504 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -69,6 +69,9 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control "~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1)); sub_accel_ = create_subscription( "~/input/current_accel", rclcpp::QoS{1}, std::bind(&Controller::onAccel, this, _1)); + sub_operation_mode_ = create_subscription( + "~/input/current_operation_mode", rclcpp::QoS{1}, + [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ptr_ = msg; }); control_cmd_pub_ = create_publisher( "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); debug_marker_pub_ = @@ -163,11 +166,17 @@ boost::optional Controller::createInputData( return {}; } + if (!current_operation_mode_ptr_) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current operation mode."); + return {}; + } + trajectory_follower::InputData input_data; input_data.current_trajectory = *current_trajectory_ptr_; input_data.current_odometry = *current_odometry_ptr_; input_data.current_steering = *current_steering_ptr_; input_data.current_accel = *current_accel_ptr_; + input_data.current_operation_mode = *current_operation_mode_ptr_; return input_data; } diff --git a/control/trajectory_follower_node/test/test_controller_node.cpp b/control/trajectory_follower_node/test/test_controller_node.cpp index d3b39a018652c..29c4add348766 100644 --- a/control/trajectory_follower_node/test/test_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_controller_node.cpp @@ -20,6 +20,7 @@ #include "trajectory_follower_node/controller_node.hpp" #include "trajectory_follower_test_utils.hpp" +#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" @@ -39,6 +40,7 @@ using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; using VehicleOdometry = nav_msgs::msg::Odometry; using SteeringReport = autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_adapi_v1_msgs::msg::OperationModeState; using geometry_msgs::msg::AccelWithCovarianceStamped; using FakeNodeFixture = autoware::tools::testing::FakeTestNode; @@ -136,6 +138,14 @@ class ControllerTester accel_pub->publish(acc_msg); }; + void publish_autonomous_operation_mode() + { + OperationModeState msg; + msg.stamp = node->now(); + msg.mode = OperationModeState::AUTONOMOUS; + operation_mode_pub->publish(msg); + }; + void publish_default_traj() { Trajectory traj_msg; @@ -167,6 +177,9 @@ class ControllerTester rclcpp::Publisher::SharedPtr accel_pub = fnf->create_publisher("controller/input/current_accel"); + rclcpp::Publisher::SharedPtr operation_mode_pub = + fnf->create_publisher("controller/input/current_operation_mode"); + rclcpp::Subscription::SharedPtr cmd_sub = fnf->create_subscription( "controller/output/control_cmd", *fnf->get_fake_node(), @@ -209,6 +222,7 @@ TEST_F(FakeNodeFixture, empty_trajectory) // Empty trajectory: expect a stopped command tester.publish_default_traj(); tester.publish_default_odom(); + tester.publish_autonomous_operation_mode(); tester.publish_default_acc(); tester.publish_default_steer(); @@ -225,6 +239,7 @@ TEST_F(FakeNodeFixture, straight_trajectory) tester.send_default_transform(); tester.publish_odom_vx(1.0); + tester.publish_autonomous_operation_mode(); tester.publish_default_steer(); tester.publish_default_acc(); @@ -251,6 +266,7 @@ TEST_F(FakeNodeFixture, right_turn) tester.send_default_transform(); tester.publish_odom_vx(1.0); + tester.publish_autonomous_operation_mode(); tester.publish_default_steer(); tester.publish_default_acc(); @@ -278,6 +294,7 @@ TEST_F(FakeNodeFixture, left_turn) tester.send_default_transform(); tester.publish_odom_vx(1.0); + tester.publish_autonomous_operation_mode(); tester.publish_default_steer(); tester.publish_default_acc(); @@ -305,6 +322,7 @@ TEST_F(FakeNodeFixture, stopped) tester.send_default_transform(); tester.publish_default_odom(); + tester.publish_autonomous_operation_mode(); tester.publish_default_acc(); const double steering_tire_angle = -0.5; @@ -335,6 +353,7 @@ TEST_F(FakeNodeFixture, longitudinal_keep_velocity) tester.send_default_transform(); tester.publish_odom_vx(1.0); + tester.publish_autonomous_operation_mode(); tester.publish_default_steer(); tester.publish_default_acc(); @@ -373,6 +392,8 @@ TEST_F(FakeNodeFixture, longitudinal_slow_down) const double odom_vx = 1.0; tester.publish_odom_vx(odom_vx); + tester.publish_autonomous_operation_mode(); + // Publish non stopping trajectory Trajectory traj; traj.header.stamp = tester.node->now(); @@ -408,6 +429,8 @@ TEST_F(FakeNodeFixture, longitudinal_accelerate) const double odom_vx = 0.5; tester.publish_odom_vx(odom_vx); + tester.publish_autonomous_operation_mode(); + // Publish non stopping trajectory Trajectory traj; traj.header.stamp = tester.node->now(); @@ -438,6 +461,7 @@ TEST_F(FakeNodeFixture, longitudinal_stopped) tester.send_default_transform(); tester.publish_default_odom(); + tester.publish_autonomous_operation_mode(); tester.publish_default_steer(); tester.publish_default_acc(); @@ -467,6 +491,7 @@ TEST_F(FakeNodeFixture, longitudinal_reverse) tester.send_default_transform(); tester.publish_default_odom(); + tester.publish_autonomous_operation_mode(); tester.publish_default_steer(); tester.publish_default_acc(); @@ -493,6 +518,7 @@ TEST_F(FakeNodeFixture, longitudinal_emergency) tester.send_default_transform(); tester.publish_default_odom(); + tester.publish_autonomous_operation_mode(); tester.publish_default_steer(); tester.publish_default_acc(); @@ -520,6 +546,7 @@ TEST_F(FakeNodeFixture, longitudinal_not_check_steer_converged) tester.send_default_transform(); tester.publish_default_odom(); + tester.publish_autonomous_operation_mode(); tester.publish_default_acc(); // steering_tire_angle has to be larger than the threshold to check convergence. @@ -550,6 +577,7 @@ TEST_F(FakeNodeFixture, longitudinal_check_steer_converged) tester.send_default_transform(); tester.publish_default_odom(); + tester.publish_autonomous_operation_mode(); tester.publish_default_acc(); // steering_tire_angle has to be larger than the threshold to check convergence. diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 7abb41d8af294..086ad0ad698e2 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -70,6 +70,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/current_odometry", "/localization/kinematic_state"), ("~/input/current_steering", "/vehicle/status/steering_status"), ("~/input/current_accel", "/localization/acceleration"), + ("~/input/current_operation_mode", "/system/operation_mode/state"), ("~/output/predicted_trajectory", "lateral/predicted_trajectory"), ("~/output/lateral_diagnostic", "lateral/diagnostic"), ("~/output/slope_angle", "longitudinal/slope_angle"), From 7da9114244bc0c3f6bc645f6af7bb4520bb1ccb6 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 27 Jan 2023 21:10:08 +0900 Subject: [PATCH 52/60] feat(mpc_lateral_controller): add control input bias removal (#2730) * feat(mpc_lateral_controller): add steering bias removal Signed-off-by: Takamasa Horibe * fix logic Signed-off-by: Takamasa Horibe * add steering bias in simulation Signed-off-by: Takamasa Horibe * Revert "add steering bias in simulation" This reverts commit 1638695072821d6b87f725fe1b83adb3a1f808b4. * Update control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp Co-authored-by: Yukihiro Saito * Update control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp * change param name Signed-off-by: Takamasa Horibe * update for publisher Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe Co-authored-by: Yukihiro Saito --- control/mpc_lateral_controller/CMakeLists.txt | 5 ++ .../mpc_lateral_controller.hpp | 6 ++ .../steering_offset/steering_offset.hpp | 48 ++++++++++++++++ .../lateral_controller_defaults.param.yaml | 8 +++ .../src/mpc_lateral_controller.cpp | 30 ++++++++++ .../src/steering_offset/steering_offset.cpp | 56 +++++++++++++++++++ 6 files changed, 153 insertions(+) create mode 100644 control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp create mode 100644 control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp diff --git a/control/mpc_lateral_controller/CMakeLists.txt b/control/mpc_lateral_controller/CMakeLists.txt index 2167805d99c9b..8acf55be4630e 100644 --- a/control/mpc_lateral_controller/CMakeLists.txt +++ b/control/mpc_lateral_controller/CMakeLists.txt @@ -4,6 +4,10 @@ project(mpc_lateral_controller) find_package(autoware_cmake REQUIRED) autoware_package() +ament_auto_add_library(steering_offset_lib SHARED + src/steering_offset/steering_offset.cpp +) + set(MPC_LAT_CON_LIB ${PROJECT_NAME}_lib) ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED src/mpc_lateral_controller.cpp @@ -19,6 +23,7 @@ ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED src/vehicle_model/vehicle_model_bicycle_kinematics.cpp src/vehicle_model/vehicle_model_interface.cpp ) +target_link_libraries(${MPC_LAT_CON_LIB} steering_offset_lib) if(BUILD_TESTING) set(TEST_LAT_SOURCES diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index d2dc63bb074ec..9ab555635a8a2 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -22,6 +22,7 @@ #include "mpc_lateral_controller/mpc_utils.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/steering_offset/steering_offset.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" @@ -73,6 +74,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase rclcpp::Publisher::SharedPtr m_pub_predicted_traj; //!< @brief topic publisher for control debug values rclcpp::Publisher::SharedPtr m_pub_debug_values; + rclcpp::Publisher::SharedPtr m_pub_steer_offset; //!< @brief subscription for transform messages rclcpp::Subscription::SharedPtr m_tf_sub; rclcpp::Subscription::SharedPtr m_tf_static_sub; @@ -129,6 +131,10 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase double m_ego_nearest_dist_threshold; double m_ego_nearest_yaw_threshold; + // for steering offset compensation + bool enable_auto_steering_offset_removal_; + std::shared_ptr steering_offset_; + //!< initialize timer to work in real, simulation, and replay void initTimer(double period_s); diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp new file mode 100644 index 0000000000000..494961ef679d2 --- /dev/null +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp @@ -0,0 +1,48 @@ +// Copyright 2018-2023 The Autoware Foundation +// +// 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. + +#ifndef MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ +#define MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ + +#include + +#include +#include +#include + +class SteeringOffsetEstimator +{ +public: + SteeringOffsetEstimator( + double wheelbase, double average_num, double vel_thres, double steer_thres, + double offset_limit); + ~SteeringOffsetEstimator() = default; + + double getOffset() const; + void updateOffset(const geometry_msgs::msg::Twist & twist, const double steering); + +private: + // parameters + double wheelbase_ = 3.0; + size_t average_num_ = 1000; + double update_vel_threshold_ = 8.0; + double update_steer_threshold_ = 0.05; + double offset_limit_ = 0.02; + + // results + std::deque steering_offset_storage_; + double steering_offset_ = 0.0; +}; + +#endif // MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml index 92d15da9e49e6..30f74be8c2326 100644 --- a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml +++ b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml @@ -63,6 +63,14 @@ new_traj_duration_time: 1.0 new_traj_end_dist: 0.3 + # steer offset + steering_offset: + enable_auto_steering_offset_removal: true + update_vel_threshold: 5.56 + update_steer_threshold: 0.035 + average_num: 1000 + steering_offset_limit: 0.02 + # vehicle parameters mass_kg: 2400.0 mass_fl: 600.0 diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 80f05255079e9..a884b7f8b32e3 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -123,6 +123,19 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); } + /* steering offset compensation */ + { + const std::string ns = "steering_offset."; + enable_auto_steering_offset_removal_ = + node_->declare_parameter(ns + "enable_auto_steering_offset_removal"); + const auto vel_thres = node_->declare_parameter(ns + "update_vel_threshold"); + const auto steer_thres = node_->declare_parameter(ns + "update_steer_threshold"); + const auto limit = node_->declare_parameter(ns + "steering_offset_limit"); + const auto num = node_->declare_parameter(ns + "average_num"); + steering_offset_ = + std::make_shared(wheelbase, num, vel_thres, steer_thres, limit); + } + /* initialize lowpass filter */ { const double steering_lpf_cutoff_hz = @@ -148,6 +161,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} "~/output/predicted_trajectory", 1); m_pub_debug_values = node_->create_publisher( "~/output/lateral_diagnostic", 1); + m_pub_steer_offset = node_->create_publisher( + "~/output/estimated_steer_offset", 1); // TODO(Frederik.Beaujean) ctor is too long, should factor out parameter declarations declareMPCparameters(); @@ -172,6 +187,9 @@ trajectory_follower::LateralOutput MpcLateralController::run( setTrajectory(input_data.current_trajectory); m_current_kinematic_state = input_data.current_odometry; m_current_steering = input_data.current_steering; + if (enable_auto_steering_offset_removal_) { + m_current_steering.steering_tire_angle -= steering_offset_->getOffset(); + } autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd; autoware_auto_planning_msgs::msg::Trajectory predicted_traj; @@ -186,6 +204,13 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_current_steering, m_current_kinematic_state.twist.twist.linear.x, m_current_kinematic_state.pose.pose, ctrl_cmd, predicted_traj, debug_values); + if (enable_auto_steering_offset_removal_) { + steering_offset_->updateOffset( + m_current_kinematic_state.twist.twist, + input_data.current_steering.steering_tire_angle); // use unbiased steering + ctrl_cmd.steering_tire_angle += steering_offset_->getOffset(); + } + publishPredictedTraj(predicted_traj); publishDebugValues(debug_values); @@ -367,6 +392,11 @@ void MpcLateralController::publishDebugValues( { debug_values.stamp = node_->now(); m_pub_debug_values->publish(debug_values); + + tier4_debug_msgs::msg::Float32Stamped offset; + offset.stamp = node_->now(); + offset.data = steering_offset_->getOffset(); + m_pub_steer_offset->publish(offset); } void MpcLateralController::declareMPCparameters() diff --git a/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp b/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp new file mode 100644 index 0000000000000..60d4dd7901394 --- /dev/null +++ b/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp @@ -0,0 +1,56 @@ +// Copyright 2018-2023 The Autoware Foundation +// +// 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 "mpc_lateral_controller/steering_offset/steering_offset.hpp" + +#include +#include +#include + +SteeringOffsetEstimator::SteeringOffsetEstimator( + double wheelbase, double average_num, double vel_thres, double steer_thres, double offset_limit) +: wheelbase_(wheelbase), + average_num_(average_num), + update_vel_threshold_(vel_thres), + update_steer_threshold_(steer_thres), + offset_limit_(offset_limit), + steering_offset_storage_(average_num, 0.0) +{ +} + +void SteeringOffsetEstimator::updateOffset( + const geometry_msgs::msg::Twist & twist, const double steering) +{ + const bool update_offset = + (std::abs(twist.linear.x) > update_vel_threshold_ && + std::abs(steering) < update_steer_threshold_); + + if (!update_offset) return; + + const auto steer_angvel = std::atan2(twist.angular.z * wheelbase_, twist.linear.x); + const auto steer_offset = steering - steer_angvel; + steering_offset_storage_.push_back(steer_offset); + if (steering_offset_storage_.size() > average_num_) { + steering_offset_storage_.pop_front(); + } + + steering_offset_ = + std::accumulate(std::begin(steering_offset_storage_), std::end(steering_offset_storage_), 0.0) / + std::size(steering_offset_storage_); +} + +double SteeringOffsetEstimator::getOffset() const +{ + return std::clamp(steering_offset_, -offset_limit_, offset_limit_); +} From 28f6c1f5a0de5e41b6be7dca2e8960f7a0f5826a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 27 Jan 2023 21:51:22 +0900 Subject: [PATCH 53/60] fix(behavior_path_planner): support the goal behind ego in pull out (#2759) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../utils/geometric_parallel_parking.hpp | 3 +- .../pull_out/geometric_pull_out.cpp | 11 +++-- .../scene_module/pull_out/pull_out_module.cpp | 5 +-- .../scene_module/pull_out/shift_pull_out.cpp | 35 ++++++++-------- .../utils/geometric_parallel_parking.cpp | 41 ++++++++++++------- 5 files changed, 55 insertions(+), 40 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp index 74d47615a3a79..b9df9623b2b25 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp @@ -118,7 +118,8 @@ class GeometricParallelParking const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const double end_pose_offset, const double velocity); PathWithLaneId generateStraightPath(const Pose & start_pose); - void setVelocityToArcPaths(std::vector & arc_paths, const double velocity); + void setVelocityToArcPaths( + std::vector & arc_paths, const double velocity, const bool set_stop_end); // debug Pose Cr_; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp index 2316ac35c2ef5..af2b2e82a3437 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp @@ -62,12 +62,11 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p if (parameters_.divide_pull_out_path) { output.partial_paths = planner_.getPaths(); } else { - const auto partial_paths = planner_.getPaths(); - auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); - // set same velocity to all points not to stop - for (auto & point : combined_path.points) { - point.point.longitudinal_velocity_mps = parameters_.geometric_pull_out_velocity; - } + auto partial_paths = planner_.getPaths(); + // remove stop velocity of first arc path + partial_paths.front().points.back().point.longitudinal_velocity_mps = + parameters_.geometric_pull_out_velocity; + const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); output.partial_paths.push_back(combined_path); } output.start_pose = planner_.getArcPaths().at(0).points.back().point.pose; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 12f23e7b7b507..9e96a502561c2 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -136,8 +136,7 @@ bool PullOutModule::isExecutionRequested() const // Check if any of the footprint points are in the shoulder lane lanelet::Lanelet closest_shoulder_lanelet; if (!lanelet::utils::query::getClosestLanelet( - planner_data_->route_handler->getShoulderLanelets(), - planner_data_->self_odometry->pose.pose, &closest_shoulder_lanelet)) { + pull_out_lanes, planner_data_->self_odometry->pose.pose, &closest_shoulder_lanelet)) { return false; } if (!isOverlappedWithLane(closest_shoulder_lanelet, vehicle_footprint)) { @@ -495,7 +494,7 @@ PathWithLaneId PullOutModule::generateStopPath() const p.point.longitudinal_velocity_mps = 0.0; lanelet::Lanelet closest_shoulder_lanelet; lanelet::utils::query::getClosestLanelet( - planner_data_->route_handler->getShoulderLanelets(), pose, &closest_shoulder_lanelet); + status_.pull_out_lanes, pose, &closest_shoulder_lanelet); p.lane_ids.push_back(closest_shoulder_lanelet.id()); return p; }; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp index 385bbe6486edd..d5fb98aa8e980 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp @@ -140,22 +140,26 @@ std::vector ShiftPullOut::calcPullOutPaths( for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk; lateral_jerk += jerk_resolution) { - // lateral distance from road center to start pose - const double shift_length = getArcCoordinates(road_lanes, start_pose).distance; + // generate road lane reference path + const auto arc_position_start = getArcCoordinates(road_lanes, start_pose); + const double s_start = std::max(arc_position_start.length - backward_path_length, 0.0); + const auto arc_position_goal = getArcCoordinates(road_lanes, goal_pose); + const double road_lanes_length = std::accumulate( + road_lanes.begin(), road_lanes.end(), 0.0, [](const double sum, const auto & lane) { + return sum + lanelet::utils::getLaneletLength2d(lane); + }); + // if goal is behind start pose, + const bool goal_is_behind = arc_position_goal.length < s_start; + const double s_end = goal_is_behind ? road_lanes_length : arc_position_goal.length; + PathWithLaneId road_lane_reference_path = util::resamplePathWithSpline( + route_handler.getCenterLinePath(road_lanes, s_start, s_end), 1.0); - PathWithLaneId road_lane_reference_path{}; - { - const auto arc_position = getArcCoordinates(road_lanes, start_pose); - const double s_start = std::max(arc_position.length - backward_path_length, 0.0); - const auto arc_position_goal = getArcCoordinates(road_lanes, goal_pose); - double s_end = arc_position_goal.length; - road_lane_reference_path = util::resamplePathWithSpline( - route_handler.getCenterLinePath(road_lanes, s_start, s_end), 1.0); - } PathShifter path_shifter{}; path_shifter.setPath(road_lane_reference_path); // calculate after/before shifted pull out distance + // lateral distance from road center to start pose + const double shift_length = getArcCoordinates(road_lanes, start_pose).distance; const double pull_out_distance = std::max( PathShifter::calcLongitudinalDistFromJerk( abs(shift_length), lateral_jerk, shift_pull_out_velocity), @@ -205,18 +209,17 @@ std::vector ShiftPullOut::calcPullOutPaths( // set velocity const size_t pull_out_end_idx = findNearestIndex(shifted_path.path.points, shift_end_pose.position); - const size_t goal_idx = findNearestIndex(shifted_path.path.points, goal_pose.position); for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { auto & point = shifted_path.path.points.at(i); if (i < pull_out_end_idx) { point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, static_cast(shift_pull_out_velocity)); - continue; - } else if (i >= goal_idx) { - point.point.longitudinal_velocity_mps = 0.0; - continue; } } + // if the end point is the goal, set the velocity to 0 + if (!goal_is_behind) { + shifted_path.path.points.back().point.longitudinal_velocity_mps = 0.0; + } // add shifted path to candidates PullOutPath candidate_path; diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index 8c3271b8e75cf..15fa153221820 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -98,11 +98,11 @@ PathWithLaneId GeometricParallelParking::getArcPath() const bool GeometricParallelParking::isParking() const { return current_path_idx_ > 0; } void GeometricParallelParking::setVelocityToArcPaths( - std::vector & arc_paths, const double velocity) + std::vector & arc_paths, const double velocity, const bool set_stop_end) { for (auto & path : arc_paths) { for (size_t i = 0; i < path.points.size(); i++) { - if (i == path.points.size() - 1) { + if (i == path.points.size() - 1 && set_stop_end) { // stop point at the end of the path path.points.at(i).point.longitudinal_velocity_mps = 0.0; } else { @@ -129,7 +129,8 @@ std::vector GeometricParallelParking::generatePullOverPaths( arc_paths_ = arc_paths; // set parking velocity and stop velocity at the end of the path - setVelocityToArcPaths(arc_paths, velocity); + constexpr bool set_stop_end = true; + setVelocityToArcPaths(arc_paths, velocity, set_stop_end); // straight path from current to parking start const auto straight_path = generateStraightPath(start_pose); @@ -265,24 +266,36 @@ bool GeometricParallelParking::planPullOut( } } - arc_paths_ = arc_paths; - // get road center line path from departing end to goal, and combine after the second arc path - PathWithLaneId road_center_line_path; - { - const double s_start = getArcCoordinates(road_lanes, *end_pose).length + 1.0; // need buffer? - const double s_end = getArcCoordinates(road_lanes, goal_pose).length; - road_center_line_path = - planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true); - } + const double s_start = getArcCoordinates(road_lanes, *end_pose).length + 1.0; // need buffer? + const double s_goal = getArcCoordinates(road_lanes, goal_pose).length; + const double road_lanes_length = std::accumulate( + road_lanes.begin(), road_lanes.end(), 0.0, [](const double sum, const auto & lane) { + return sum + lanelet::utils::getLaneletLength2d(lane); + }); + const bool goal_is_behind = s_goal < s_start; + const double s_end = goal_is_behind ? road_lanes_length : s_goal; + PathWithLaneId road_center_line_path = + planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true); + + // set departing velocity to arc paths and 0 velocity to end point + constexpr bool set_stop_end = false; + setVelocityToArcPaths(arc_paths, parameters_.departing_velocity, set_stop_end); + arc_paths.back().points.front().point.longitudinal_velocity_mps = 0.0; + + // combine the road center line path with the second arc path auto paths = arc_paths; paths.back().points.insert( paths.back().points.end(), road_center_line_path.points.begin(), road_center_line_path.points.end()); removeOverlappingPoints(paths.back()); - // set departing velocity and stop velocity at the end of the path - setVelocityToArcPaths(paths, parameters_.departing_velocity); + // if the end point is the goal, set the velocity to 0 + if (!goal_is_behind) { + paths.back().points.back().point.longitudinal_velocity_mps = 0.0; + } + + arc_paths_ = arc_paths; paths_ = paths; return true; From 45079b54125e256ff6154360a2d9ec64a2c747b8 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 27 Jan 2023 23:59:57 +0900 Subject: [PATCH 54/60] fix(costmap_generator): wait for lanelet map (#2765) Signed-off-by: kosuke55 --- .../nodes/costmap_generator/costmap_generator_node.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp index 6999cd54d57c2..473e0875609ea 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp @@ -334,6 +334,10 @@ void CostmapGenerator::onTimer() bool CostmapGenerator::isActive() { + if (!lanelet_map_) { + return false; + } + if (activate_by_scenario_) { if (scenario_) { const auto & s = scenario_->activating_scenarios; From 743bcf8f6865f0814155b89dfbccae06d302879c Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sat, 28 Jan 2023 00:32:06 +0900 Subject: [PATCH 55/60] chore(costmap_generator): add maintainers (#2766) * chore(costmap_generator): add maintainers Signed-off-by: kosuke55 * Update package.xml --------- Signed-off-by: kosuke55 Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --- planning/costmap_generator/package.xml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/planning/costmap_generator/package.xml b/planning/costmap_generator/package.xml index a65a5bba21000..f2741df943c28 100644 --- a/planning/costmap_generator/package.xml +++ b/planning/costmap_generator/package.xml @@ -4,14 +4,15 @@ costmap_generator 0.1.0 The costmap_generator package - Kenji Miyake + Kosuke Takeuchi + Takamasa Horibe + Takayuki Murooka + Yutaka Shimizu Apache License 2.0 BSD-3-Clause - Kenji Miyake - ament_cmake_auto autoware_cmake From 8c7b2c1be28f3b6bb48d628f6bc1a95c50560261 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 27 Jan 2023 17:06:55 +0100 Subject: [PATCH 56/60] build(detected_object_validation): add build dependency (#2735) --- perception/detected_object_validation/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index b1317d9dcb90d..f13f05c614620 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -11,6 +11,7 @@ eigen3_cmake_module autoware_cmake + libopencv-dev autoware_auto_mapping_msgs autoware_auto_perception_msgs From 3d6961173ac0a014bbf08c0a1fcb03d8c1d6d26c Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 27 Jan 2023 17:07:07 +0100 Subject: [PATCH 57/60] build(autoware_auto_perception_rviz_plugin): add build dependency (#2733) --- common/autoware_auto_perception_rviz_plugin/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index a1f184a15e7a8..aec99ef805f5f 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -12,6 +12,7 @@ ament_cmake autoware_cmake + libboost-dev qtbase5-dev autoware_auto_perception_msgs From 097228f50b7314b4374228f1d71924e31e87fc16 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Sat, 28 Jan 2023 00:17:30 +0000 Subject: [PATCH 58/60] chore: update CODEOWNERS (#2768) Signed-off-by: GitHub Co-authored-by: kenji-miyake --- .github/CODEOWNERS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index e6d88c3df9934..091110b5f5148 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -115,7 +115,7 @@ perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp @autow perception/traffic_light_visualization/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-global-codeowners planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/costmap_generator/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners planning/freespace_planner/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners planning/freespace_planning_algorithms/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners From bac20ee65aca0bf8f247b8aa6fa052e393090244 Mon Sep 17 00:00:00 2001 From: Taichi Hirano <85976747+td12321@users.noreply.github.com> Date: Sat, 28 Jan 2023 17:09:01 +0900 Subject: [PATCH 59/60] feat(accel_map_calibrator): publish estimated covariance (#2551) * add calcuration of data covariance for 4cell-calibration. * add publisher for offset_covariance. * fix publisher of offset_covariance. * refactor code Signed-off-by: takahoribe * change publisher variance to standard deviation. * fix error Signed-off-by: Takamasa Horibe --------- Signed-off-by: takahoribe Signed-off-by: Takamasa Horibe Co-authored-by: takahoribe --- .../accel_brake_map_calibrator_node.hpp | 17 +++ .../src/accel_brake_map_calibrator_node.cpp | 125 +++++++++++++++++- 2 files changed, 137 insertions(+), 5 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index bfd2b40cce4c2..0e34e9cb24e6d 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -79,6 +79,7 @@ class AccelBrakeMapCalibrator : public rclcpp::Node rclcpp::Publisher::SharedPtr update_map_occ_pub_; rclcpp::Publisher::SharedPtr original_map_raw_pub_; rclcpp::Publisher::SharedPtr update_map_raw_pub_; + rclcpp::Publisher::SharedPtr offset_covariance_pub_; rclcpp::Publisher::SharedPtr debug_pub_; rclcpp::Publisher::SharedPtr data_count_pub_; rclcpp::Publisher::SharedPtr data_count_with_self_pose_pub_; @@ -184,6 +185,8 @@ class AccelBrakeMapCalibrator : public rclcpp::Node std::vector> brake_map_value_; std::vector> update_accel_map_value_; std::vector> update_brake_map_value_; + std::vector> accel_offset_covariance_value_; + std::vector> brake_offset_covariance_value_; std::vector>> map_value_data_; std::vector accel_vel_index_; std::vector brake_vel_index_; @@ -285,6 +288,17 @@ class AccelBrakeMapCalibrator : public rclcpp::Node bool isTimeout(const builtin_interfaces::msg::Time & stamp, const double timeout_sec); bool isTimeout(const DataStampedPtr & data_stamped, const double timeout_sec); + /* for covariance calculation */ + // mean value on each cell (counting methoud depends on the update algorithm) + Eigen::MatrixXd accel_data_mean_mat_; + Eigen::MatrixXd brake_data_mean_mat_; + // calculated vairiance on each cell + Eigen::MatrixXd accel_data_covariance_mat_; + Eigen::MatrixXd brake_data_covariance_mat_; + // number of data on each cell (counting methoud depends on the update algorithm) + Eigen::MatrixXd accel_data_num_; + Eigen::MatrixXd brake_data_num_; + nav_msgs::msg::OccupancyGrid getOccMsg( const std::string frame_id, const double height, const double width, const double resolution, const std::vector & map_value); @@ -296,6 +310,9 @@ class AccelBrakeMapCalibrator : public rclcpp::Node void publishMap( const std::vector> accel_map_value, const std::vector> brake_map_value, const std::string publish_type); + void publishOffsetCovMap( + const std::vector> accel_map_value, + const std::vector> brake_map_value); void publishCountMap(); void publishIndex(); bool writeMapToCSV( diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index b664352f70fab..cce02a8bef26e 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -144,6 +144,14 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod for (auto & m : update_brake_map_value_) { m.resize(brake_map_value_.at(0).size()); } + accel_offset_covariance_value_.resize((accel_map_value_.size())); + brake_offset_covariance_value_.resize((brake_map_value_.size())); + for (auto & m : accel_offset_covariance_value_) { + m.resize(accel_map_value_.at(0).size(), covariance_); + } + for (auto & m : brake_offset_covariance_value_) { + m.resize(brake_map_value_.at(0).size(), covariance_); + } map_value_data_.resize(accel_map_value_.size() + brake_map_value_.size() - 1); for (auto & m : map_value_data_) { m.resize(accel_map_value_.at(0).size()); @@ -152,6 +160,19 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod std::copy(accel_map_value_.begin(), accel_map_value_.end(), update_accel_map_value_.begin()); std::copy(brake_map_value_.begin(), brake_map_value_.end(), update_brake_map_value_.begin()); + // inialize matrix for covariance calculation + { + const auto genConstMat = [](const raw_vehicle_cmd_converter::Map & map, const auto val) { + return Eigen::MatrixXd::Constant(map.size(), map.at(0).size(), val); + }; + accel_data_mean_mat_ = genConstMat(accel_map_value_, map_offset_); + brake_data_mean_mat_ = genConstMat(brake_map_value_, map_offset_); + accel_data_covariance_mat_ = genConstMat(accel_map_value_, covariance_); + brake_data_covariance_mat_ = genConstMat(brake_map_value_, covariance_); + accel_data_num_ = genConstMat(accel_map_value_, 1); + brake_data_num_ = genConstMat(brake_map_value_, 1); + } + // publisher update_suggest_pub_ = create_publisher("~/output/update_suggest", durable_qos); @@ -181,6 +202,8 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod "/accel_brake_map_calibrator/output/updated_map_error", durable_qos); map_error_ratio_pub_ = create_publisher( "/accel_brake_map_calibrator/output/map_error_ratio", durable_qos); + offset_covariance_pub_ = create_publisher( + "/accel_brake_map_calibrator/debug/offset_covariance", durable_qos); // subscriber using std::placeholders::_1; @@ -320,6 +343,7 @@ void AccelBrakeMapCalibrator::timerCallback() /* publish map & debug_values*/ publishMap(accel_map_value_, brake_map_value_, "original"); publishMap(update_accel_map_value_, update_brake_map_value_, "update"); + publishOffsetCovMap(accel_offset_covariance_value_, brake_offset_covariance_value_); publishCountMap(); publishIndex(); publishUpdateSuggestFlag(); @@ -846,6 +870,8 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset( accel_map_value_.at(0).size() - 1, Eigen::MatrixXd::Identity(4, 4) * covariance_)); auto & update_map_value = accel_mode ? update_accel_map_value_ : update_brake_map_value_; + auto & offset_covariance_value = + accel_mode ? accel_offset_covariance_value_ : brake_offset_covariance_value_; const auto & map_value = accel_mode ? accel_map_value_ : brake_map_value_; const auto & pedal_index = accel_mode ? accel_pedal_index : brake_pedal_index; const auto & pedal_index_ = accel_mode ? accel_pedal_index_ : brake_pedal_index_; @@ -855,6 +881,9 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset( accel_mode ? delayed_accel_pedal_ptr_->data : delayed_brake_pedal_ptr_->data; auto & map_offset_vec = accel_mode ? accel_map_offset_vec_ : brake_map_offset_vec_; auto & covariance_mat = accel_mode ? accel_covariance_mat_ : brake_covariance_mat_; + auto & data_mean_mat = accel_mode ? accel_data_mean_mat_ : brake_data_mean_mat_; + auto & data_covariance_mat = accel_mode ? accel_data_covariance_mat_ : brake_data_covariance_mat_; + auto & data_weighted_num = accel_mode ? accel_data_num_ : brake_data_num_; const double zll = update_map_value.at(pedal_index + 0).at(vel_index + 0); const double zhl = update_map_value.at(pedal_index + 1).at(vel_index + 0); @@ -874,6 +903,23 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset( Eigen::Vector4d theta(4); theta << zll, zhl, zlh, zhh; + Eigen::Vector4d weighted_sum(4); + weighted_sum << data_weighted_num(pedal_index + 0, vel_index + 0), + data_weighted_num(pedal_index + 1, vel_index + 0), + data_weighted_num(pedal_index + 0, vel_index + 1), + data_weighted_num(pedal_index + 1, vel_index + 1); + + Eigen::Vector4d sigma(4); + sigma << data_covariance_mat(pedal_index + 0, vel_index + 0), + data_covariance_mat(pedal_index + 1, vel_index + 0), + data_covariance_mat(pedal_index + 0, vel_index + 1), + data_covariance_mat(pedal_index + 1, vel_index + 1); + + Eigen::Vector4d mean(4); + mean << data_mean_mat(pedal_index + 0, vel_index + 0), + data_mean_mat(pedal_index + 1, vel_index + 0), data_mean_mat(pedal_index + 0, vel_index + 1), + data_mean_mat(pedal_index + 1, vel_index + 1); + const int vel_idx_l = vel_index + 0; const int vel_idx_h = vel_index + 1; const int ped_idx_l = pedal_index + 0; @@ -885,6 +931,8 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset( map_offset(2) = map_offset_vec.at(ped_idx_l).at(vel_idx_h); map_offset(3) = map_offset_vec.at(ped_idx_h).at(vel_idx_h); + Eigen::VectorXd updated_map_offset(4); + Eigen::MatrixXd covariance = covariance_mat.at(ped_idx_l).at(vel_idx_l); /* calculate adaptive map offset */ @@ -899,13 +947,38 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset( double eta_hat = phiT * theta; const double error_map_offset = measured_acc - eta_hat; - map_offset = map_offset + G * error_map_offset; + updated_map_offset = map_offset + G * error_map_offset; + + for (int i = 0; i < 4; i++) { + const double pre_mean = mean(i); + mean(i) = (weighted_sum(i) * pre_mean + error_map_offset) / (weighted_sum(i) + 1); + sigma(i) = + (weighted_sum(i) * (sigma(i) + pre_mean * pre_mean) + error_map_offset * error_map_offset) / + (weighted_sum(i) + 1) - + mean(i) * mean(i); + weighted_sum(i) = weighted_sum(i) + 1; + } /* input calculated result and update map */ - map_offset_vec.at(ped_idx_l).at(vel_idx_l) = map_offset(0); - map_offset_vec.at(ped_idx_h).at(vel_idx_l) = map_offset(1); - map_offset_vec.at(ped_idx_l).at(vel_idx_h) = map_offset(2); - map_offset_vec.at(ped_idx_h).at(vel_idx_h) = map_offset(3); + map_offset_vec.at(ped_idx_l).at(vel_idx_l) = updated_map_offset(0); + map_offset_vec.at(ped_idx_h).at(vel_idx_l) = updated_map_offset(1); + map_offset_vec.at(ped_idx_l).at(vel_idx_h) = updated_map_offset(2); + map_offset_vec.at(ped_idx_h).at(vel_idx_h) = updated_map_offset(3); + + data_covariance_mat(ped_idx_l, vel_idx_l) = sigma(0); + data_covariance_mat(ped_idx_h, vel_idx_l) = sigma(1); + data_covariance_mat(ped_idx_l, vel_idx_h) = sigma(2); + data_covariance_mat(ped_idx_h, vel_idx_h) = sigma(3); + + data_weighted_num(ped_idx_l, vel_idx_l) = weighted_sum(0); + data_weighted_num(ped_idx_h, vel_idx_l) = weighted_sum(1); + data_weighted_num(ped_idx_l, vel_idx_h) = weighted_sum(2); + data_weighted_num(ped_idx_h, vel_idx_h) = weighted_sum(3); + + data_mean_mat(ped_idx_l, vel_idx_l) = mean(0); + data_mean_mat(ped_idx_h, vel_idx_l) = mean(1); + data_mean_mat(ped_idx_l, vel_idx_h) = mean(2); + data_mean_mat(ped_idx_h, vel_idx_h) = mean(3); covariance_mat.at(ped_idx_l).at(vel_idx_l) = covariance; @@ -918,6 +991,11 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset( update_map_value.at(pedal_index + 1).at(vel_index + 1) = map_value.at(pedal_index + 1).at(vel_index + 1) + map_offset(3); + offset_covariance_value.at(pedal_index + 0).at(vel_index + 0) = std::sqrt(sigma(0)); + offset_covariance_value.at(pedal_index + 1).at(vel_index + 1) = std::sqrt(sigma(1)); + offset_covariance_value.at(pedal_index + 0).at(vel_index + 0) = std::sqrt(sigma(2)); + offset_covariance_value.at(pedal_index + 1).at(vel_index + 1) = std::sqrt(sigma(3)); + return true; } @@ -1342,6 +1420,43 @@ void AccelBrakeMapCalibrator::publishMap( } } +void AccelBrakeMapCalibrator::publishOffsetCovMap( + const std::vector> accel_map_value, + const std::vector> brake_map_value) +{ + if (accel_map_value.at(0).size() != brake_map_value.at(0).size()) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("accel_brake_map_calibrator"), + "Invalid map. The number of velocity index of accel map and brake map is different."); + return; + } + const double h = accel_map_value.size() + brake_map_value.size() - + 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) + const double w = accel_map_value.at(0).size(); // velocity + + // publish raw map + std_msgs::msg::Float32MultiArray float_map; + + float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); + float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); + float_map.layout.dim[0].label = "height"; + float_map.layout.dim[1].label = "width"; + float_map.layout.dim[0].size = h; + float_map.layout.dim[1].size = w; + float_map.layout.dim[0].stride = h * w; + float_map.layout.dim[1].stride = w; + float_map.layout.data_offset = 0; + std::vector vec(h * w, 0); + for (int i = 0; i < h; i++) { + for (int j = 0; j < w; j++) { + vec[i * w + j] = + static_cast(getMapColumnFromUnifiedIndex(accel_map_value, brake_map_value, i).at(j)); + } + } + float_map.data = vec; + offset_covariance_pub_->publish(float_map); +} + void AccelBrakeMapCalibrator::publishFloat32(const std::string publish_type, const double val) { tier4_debug_msgs::msg::Float32Stamped msg; From 245242cee866de2d113e89c562353c5fc17f1f98 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sun, 29 Jan 2023 12:04:22 +0900 Subject: [PATCH 60/60] refactor(accel_brake_map_calibrator): minor refactors (#2771) Signed-off-by: Takamasa Horibe --- .../accel_brake_map_calibrator_node.hpp | 124 +++++---- .../src/accel_brake_map_calibrator_node.cpp | 256 ++++++++---------- .../accel_brake_map_calibrator/src/main.cpp | 2 +- 3 files changed, 172 insertions(+), 210 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 0e34e9cb24e6d..a00f780379d41 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -22,13 +22,6 @@ #include "raw_vehicle_cmd_converter/brake_map.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" - -#ifdef ROS_DISTRO_GALACTIC -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#else -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#endif - #include "tier4_autoware_utils/ros/transform_listener.hpp" #include @@ -42,6 +35,7 @@ #include "std_msgs/msg/float32_multi_array.hpp" #include "std_msgs/msg/multi_array_dimension.hpp" #include "std_msgs/msg/string.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" #include "tier4_external_api_msgs/msg/calibration_status.hpp" @@ -57,8 +51,26 @@ #include #include +namespace accel_brake_map_calibrator +{ + +using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_auto_vehicle_msgs::msg::VelocityReport; +using geometry_msgs::msg::TwistStamped; +using nav_msgs::msg::OccupancyGrid; using raw_vehicle_cmd_converter::AccelMap; using raw_vehicle_cmd_converter::BrakeMap; +using std_msgs::msg::Float32MultiArray; +using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using tier4_debug_msgs::msg::Float32Stamped; +using tier4_external_api_msgs::msg::CalibrationStatus; +using tier4_vehicle_msgs::msg::ActuationStatusStamped; +using visualization_msgs::msg::MarkerArray; + +using tier4_vehicle_msgs::srv::UpdateAccelBrakeMap; + +using Map = std::vector>; + struct DataStamped { DataStamped(const double _data, const rclcpp::Time & _data_time) @@ -75,42 +87,40 @@ class AccelBrakeMapCalibrator : public rclcpp::Node private: std::shared_ptr transform_listener_; std::string csv_default_map_dir_; - rclcpp::Publisher::SharedPtr original_map_occ_pub_; - rclcpp::Publisher::SharedPtr update_map_occ_pub_; - rclcpp::Publisher::SharedPtr original_map_raw_pub_; - rclcpp::Publisher::SharedPtr update_map_raw_pub_; - rclcpp::Publisher::SharedPtr offset_covariance_pub_; - rclcpp::Publisher::SharedPtr debug_pub_; - rclcpp::Publisher::SharedPtr data_count_pub_; - rclcpp::Publisher::SharedPtr data_count_with_self_pose_pub_; - rclcpp::Publisher::SharedPtr data_ave_pub_; - rclcpp::Publisher::SharedPtr data_std_pub_; - rclcpp::Publisher::SharedPtr index_pub_; + rclcpp::Publisher::SharedPtr original_map_occ_pub_; + rclcpp::Publisher::SharedPtr update_map_occ_pub_; + rclcpp::Publisher::SharedPtr original_map_raw_pub_; + rclcpp::Publisher::SharedPtr update_map_raw_pub_; + rclcpp::Publisher::SharedPtr offset_covariance_pub_; + rclcpp::Publisher::SharedPtr debug_pub_; + rclcpp::Publisher::SharedPtr data_count_pub_; + rclcpp::Publisher::SharedPtr data_count_with_self_pose_pub_; + rclcpp::Publisher::SharedPtr data_ave_pub_; + rclcpp::Publisher::SharedPtr data_std_pub_; + rclcpp::Publisher::SharedPtr index_pub_; rclcpp::Publisher::SharedPtr update_suggest_pub_; - rclcpp::Publisher::SharedPtr current_map_error_pub_; - rclcpp::Publisher::SharedPtr updated_map_error_pub_; - rclcpp::Publisher::SharedPtr map_error_ratio_pub_; - rclcpp::Publisher::SharedPtr - calibration_status_pub_; + rclcpp::Publisher::SharedPtr current_map_error_pub_; + rclcpp::Publisher::SharedPtr updated_map_error_pub_; + rclcpp::Publisher::SharedPtr map_error_ratio_pub_; + rclcpp::Publisher::SharedPtr calibration_status_pub_; - rclcpp::Subscription::SharedPtr velocity_sub_; - rclcpp::Subscription::SharedPtr steer_sub_; - rclcpp::Subscription::SharedPtr - actuation_status_sub_; + rclcpp::Subscription::SharedPtr velocity_sub_; + rclcpp::Subscription::SharedPtr steer_sub_; + rclcpp::Subscription::SharedPtr actuation_status_sub_; // Service - rclcpp::Service::SharedPtr update_map_dir_server_; + rclcpp::Service::SharedPtr update_map_dir_server_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_output_csv_; void initTimer(double period_s); void initOutputCSVTimer(double period_s); - geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_ptr_; - std::vector> twist_vec_; + TwistStamped::ConstSharedPtr twist_ptr_; + std::vector> twist_vec_; std::vector accel_pedal_vec_; // for delayed pedal std::vector brake_pedal_vec_; // for delayed pedal - autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr steer_ptr_; + SteeringReport::ConstSharedPtr steer_ptr_; DataStampedPtr accel_pedal_ptr_; DataStampedPtr brake_pedal_ptr_; DataStampedPtr delayed_accel_pedal_ptr_; @@ -181,13 +191,13 @@ class AccelBrakeMapCalibrator : public rclcpp::Node double update_suggest_thresh_; // Accel / Brake Map update - std::vector> accel_map_value_; - std::vector> brake_map_value_; - std::vector> update_accel_map_value_; - std::vector> update_brake_map_value_; - std::vector> accel_offset_covariance_value_; - std::vector> brake_offset_covariance_value_; - std::vector>> map_value_data_; + Map accel_map_value_; + Map brake_map_value_; + Map update_accel_map_value_; + Map update_brake_map_value_; + Map accel_offset_covariance_value_; + Map brake_offset_covariance_value_; + std::vector map_value_data_; std::vector accel_vel_index_; std::vector brake_vel_index_; std::vector accel_pedal_index_; @@ -229,22 +239,20 @@ class AccelBrakeMapCalibrator : public rclcpp::Node const int brake_pedal_index, const int brake_vel_index, const double measured_acc, const double map_acc); void updateTotalMapOffset(const double measured_acc, const double map_acc); - void callbackActuationStatus( - const tier4_vehicle_msgs::msg::ActuationStatusStamped::ConstSharedPtr msg); - void callbackVelocity(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg); - void callbackSteer(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg); + void callbackActuationStatus(const ActuationStatusStamped::ConstSharedPtr msg); + void callbackVelocity(const VelocityReport::ConstSharedPtr msg); + void callbackSteer(const SteeringReport::ConstSharedPtr msg); bool callbackUpdateMapService( const std::shared_ptr request_header, - tier4_vehicle_msgs::srv::UpdateAccelBrakeMap::Request::SharedPtr req, - tier4_vehicle_msgs::srv::UpdateAccelBrakeMap::Response::SharedPtr res); + UpdateAccelBrakeMap::Request::SharedPtr req, UpdateAccelBrakeMap::Response::SharedPtr res); bool getAccFromMap(const double velocity, const double pedal); double lowpass(const double original, const double current, const double gain = 0.8); double getPedalSpeed( const DataStampedPtr & prev_pedal, const DataStampedPtr & current_pedal, const double prev_pedal_speed); double getAccel( - const geometry_msgs::msg::TwistStamped::ConstSharedPtr & prev_twist, - const geometry_msgs::msg::TwistStamped::ConstSharedPtr & current_twist); + const TwistStamped::ConstSharedPtr & prev_twist, + const TwistStamped::ConstSharedPtr & current_twist); double getJerk(); bool indexValueSearch( const std::vector value_index, const double value, const double value_thresh, @@ -269,13 +277,12 @@ class AccelBrakeMapCalibrator : public rclcpp::Node const double throttle, const double brake, const double vel, AccelMap & accel_map, BrakeMap & brake_map); std::vector getMapColumnFromUnifiedIndex( - const std::vector> & accel_map_value, - const std::vector> & brake_map_value, const std::size_t index); + const Map & accel_map_value, const Map & brake_map_value, const std::size_t index); double getPedalValueFromUnifiedIndex(const std::size_t index); int getUnifiedIndexFromAccelBrakeIndex(const bool accel_map, const std::size_t index); void pushDataToQue( - const geometry_msgs::msg::TwistStamped::ConstSharedPtr & data, const std::size_t max_size, - std::queue * que); + const TwistStamped::ConstSharedPtr & data, const std::size_t max_size, + std::queue * que); template void pushDataToVec(const T data, const std::size_t max_size, std::vector * vec); template @@ -299,7 +306,7 @@ class AccelBrakeMapCalibrator : public rclcpp::Node Eigen::MatrixXd accel_data_num_; Eigen::MatrixXd brake_data_num_; - nav_msgs::msg::OccupancyGrid getOccMsg( + OccupancyGrid getOccMsg( const std::string frame_id, const double height, const double width, const double resolution, const std::vector & map_value); @@ -308,16 +315,13 @@ class AccelBrakeMapCalibrator : public rclcpp::Node /* Debug */ void publishMap( - const std::vector> accel_map_value, - const std::vector> brake_map_value, const std::string publish_type); - void publishOffsetCovMap( - const std::vector> accel_map_value, - const std::vector> brake_map_value); + const Map accel_map_value, const Map brake_map_value, const std::string publish_type); + void publishOffsetCovMap(const Map accel_map_value, const Map brake_map_value); void publishCountMap(); void publishIndex(); bool writeMapToCSV( - std::vector vel_index, std::vector pedal_index, - std::vector> value_map, std::string filename); + std::vector vel_index, std::vector pedal_index, Map value_map, + std::string filename); void addIndexToCSV(std::ofstream * csv_file); void addLogToCSV( std::ofstream * csv_file, const double & timestamp, const double velocity, const double accel, @@ -326,7 +330,7 @@ class AccelBrakeMapCalibrator : public rclcpp::Node const double steer, const double jerk, const double full_original_accel_mse, const double part_original_accel_mse, const double new_accel_mse); - mutable tier4_debug_msgs::msg::Float32MultiArrayStamped debug_values_; + mutable Float32MultiArrayStamped debug_values_; enum DBGVAL { CURRENT_SPEED = 0, CURRENT_ACCEL_PEDAL = 1, @@ -357,4 +361,6 @@ class AccelBrakeMapCalibrator : public rclcpp::Node explicit AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options); }; +} // namespace accel_brake_map_calibrator + #endif // ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index cce02a8bef26e..0b24f6c4d41c0 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -26,47 +26,45 @@ #include #include +namespace accel_brake_map_calibrator +{ + AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options) : Node("accel_brake_map_calibrator", node_options) { transform_listener_ = std::make_shared(this); // get parameter - update_hz_ = this->declare_parameter("update_hz", 10.0); - covariance_ = this->declare_parameter("initial_covariance", 0.05); - velocity_min_threshold_ = this->declare_parameter("velocity_min_threshold", 0.1); - velocity_diff_threshold_ = this->declare_parameter("velocity_diff_threshold", 0.556); - pedal_diff_threshold_ = this->declare_parameter("pedal_diff_threshold", 0.03); - max_steer_threshold_ = this->declare_parameter("max_steer_threshold", 0.2); - max_pitch_threshold_ = this->declare_parameter("max_pitch_threshold", 0.02); - max_jerk_threshold_ = this->declare_parameter("max_jerk_threshold", 0.7); - pedal_velocity_thresh_ = this->declare_parameter("pedal_velocity_thresh", 0.15); - max_accel_ = this->declare_parameter("max_accel", 5.0); - min_accel_ = this->declare_parameter("min_accel", -5.0); - pedal_to_accel_delay_ = this->declare_parameter("pedal_to_accel_delay", 0.3); - max_data_count_ = this->declare_parameter("max_data_count", 200); - pedal_accel_graph_output_ = this->declare_parameter("pedal_accel_graph_output", false); - progress_file_output_ = this->declare_parameter("progress_file_output", false); - precision_ = this->declare_parameter("precision", 3); - const auto get_pitch_method_str = - this->declare_parameter("get_pitch_method", std::string("tf")); + update_hz_ = declare_parameter("update_hz", 10.0); + covariance_ = declare_parameter("initial_covariance", 0.05); + velocity_min_threshold_ = declare_parameter("velocity_min_threshold", 0.1); + velocity_diff_threshold_ = declare_parameter("velocity_diff_threshold", 0.556); + pedal_diff_threshold_ = declare_parameter("pedal_diff_threshold", 0.03); + max_steer_threshold_ = declare_parameter("max_steer_threshold", 0.2); + max_pitch_threshold_ = declare_parameter("max_pitch_threshold", 0.02); + max_jerk_threshold_ = declare_parameter("max_jerk_threshold", 0.7); + pedal_velocity_thresh_ = declare_parameter("pedal_velocity_thresh", 0.15); + max_accel_ = declare_parameter("max_accel", 5.0); + min_accel_ = declare_parameter("min_accel", -5.0); + pedal_to_accel_delay_ = declare_parameter("pedal_to_accel_delay", 0.3); + max_data_count_ = declare_parameter("max_data_count", 200); + pedal_accel_graph_output_ = declare_parameter("pedal_accel_graph_output", false); + progress_file_output_ = declare_parameter("progress_file_output", false); + precision_ = declare_parameter("precision", 3); + const auto get_pitch_method_str = declare_parameter("get_pitch_method", std::string("tf")); if (get_pitch_method_str == std::string("tf")) { get_pitch_method_ = GET_PITCH_METHOD::TF; } else if (get_pitch_method_str == std::string("none")) { get_pitch_method_ = GET_PITCH_METHOD::NONE; } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("accel_brake_map_calibrator"), - "update_method_" - << " is wrong. (available method: tf, file, none"); + RCLCPP_ERROR_STREAM(get_logger(), "update_method_ is wrong. (available method: tf, file, none"); return; } - update_suggest_thresh_ = this->declare_parameter("update_suggest_thresh", 0.7); - csv_calibrated_map_dir_ = - this->declare_parameter("csv_calibrated_map_dir", std::string("")); + update_suggest_thresh_ = declare_parameter("update_suggest_thresh", 0.7); + csv_calibrated_map_dir_ = declare_parameter("csv_calibrated_map_dir", std::string("")); output_accel_file_ = csv_calibrated_map_dir_ + "/accel_map.csv"; output_brake_file_ = csv_calibrated_map_dir_ + "/brake_map.csv"; const std::string update_method_str = - this->declare_parameter("update_method", std::string("update_offset_each_cell")); + declare_parameter("update_method", std::string("update_offset_each_cell")); if (update_method_str == std::string("update_offset_each_cell")) { update_method_ = UPDATE_METHOD::UPDATE_OFFSET_EACH_CELL; } else if (update_method_str == std::string("update_offset_total")) { @@ -75,9 +73,8 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod update_method_ = UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND; } else { RCLCPP_ERROR_STREAM( - rclcpp::get_logger("accel_brake_map_calibrator"), - "update_method" - << " is wrong. (available method: update_offset_each_cell, update_offset_total"); + get_logger(), + "update_method is wrong. (available method: update_offset_each_cell, update_offset_total"); return; } // initializer @@ -87,7 +84,7 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod rclcpp::QoS durable_qos(queue_size); // Publisher for checkUpdateSuggest - calibration_status_pub_ = create_publisher( + calibration_status_pub_ = create_publisher( "/accel_brake_map_calibrator/output/calibration_status", durable_qos); /* Diagnostic Updater */ @@ -97,8 +94,7 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod "accel_brake_map_calibrator", this, &AccelBrakeMapCalibrator::checkUpdateSuggest); { - csv_default_map_dir_ = - this->declare_parameter("csv_default_map_dir", std::string("")); + csv_default_map_dir_ = declare_parameter("csv_default_map_dir", std::string("")); std::string csv_path_accel_map = csv_default_map_dir_ + "/accel_map.csv"; std::string csv_path_brake_map = csv_default_map_dir_ + "/brake_map.csv"; @@ -107,7 +103,7 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod !new_accel_map_.readAccelMapFromCSV(csv_path_accel_map)) { is_default_map_ = false; RCLCPP_ERROR_STREAM( - rclcpp::get_logger("accel_brake_map_calibrator"), + get_logger(), "Cannot read accelmap. csv path = " << csv_path_accel_map.c_str() << ". stop calculation."); return; } @@ -116,14 +112,13 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod !new_brake_map_.readBrakeMapFromCSV(csv_path_brake_map)) { is_default_map_ = false; RCLCPP_ERROR_STREAM( - rclcpp::get_logger("accel_brake_map_calibrator"), + get_logger(), "Cannot read brakemap. csv path = " << csv_path_brake_map.c_str() << ". stop calculation."); return; } } - std::string output_log_file = - this->declare_parameter("output_log_file", std::string("")); + std::string output_log_file = declare_parameter("output_log_file", std::string("")); output_log_.open(output_log_file); addIndexToCSV(&output_log_); @@ -162,7 +157,7 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod // inialize matrix for covariance calculation { - const auto genConstMat = [](const raw_vehicle_cmd_converter::Map & map, const auto val) { + const auto genConstMat = [](const Map & map, const auto val) { return Eigen::MatrixXd::Constant(map.size(), map.at(0).size(), val); }; accel_data_mean_mat_ = genConstMat(accel_map_value_, map_offset_); @@ -176,51 +171,42 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod // publisher update_suggest_pub_ = create_publisher("~/output/update_suggest", durable_qos); - original_map_occ_pub_ = create_publisher( - "/accel_brake_map_calibrator/debug/original_occ_map", durable_qos); - update_map_occ_pub_ = create_publisher( - "/accel_brake_map_calibrator/debug/update_occ_map", durable_qos); - data_ave_pub_ = create_publisher( - "/accel_brake_map_calibrator/debug/data_average_occ_map", durable_qos); - data_std_pub_ = create_publisher( - "/accel_brake_map_calibrator/debug/data_std_dev_occ_map", durable_qos); - data_count_pub_ = create_publisher( - "/accel_brake_map_calibrator/debug/data_count_occ_map", durable_qos); - data_count_with_self_pose_pub_ = create_publisher( - "/accel_brake_map_calibrator/debug/data_count_self_pose_occ_map", durable_qos); - index_pub_ = create_publisher( - "/accel_brake_map_calibrator/debug/occ_index", durable_qos); - original_map_raw_pub_ = create_publisher( - "/accel_brake_map_calibrator/debug/original_raw_map", durable_qos); - update_map_raw_pub_ = create_publisher( - "/accel_brake_map_calibrator/output/update_raw_map", durable_qos); - debug_pub_ = create_publisher( - "/accel_brake_map_calibrator/output/debug_values", durable_qos); - current_map_error_pub_ = create_publisher( - "/accel_brake_map_calibrator/output/current_map_error", durable_qos); - updated_map_error_pub_ = create_publisher( - "/accel_brake_map_calibrator/output/updated_map_error", durable_qos); - map_error_ratio_pub_ = create_publisher( - "/accel_brake_map_calibrator/output/map_error_ratio", durable_qos); - offset_covariance_pub_ = create_publisher( - "/accel_brake_map_calibrator/debug/offset_covariance", durable_qos); + original_map_occ_pub_ = create_publisher("~/debug/original_occ_map", durable_qos); + update_map_occ_pub_ = create_publisher("~/debug/update_occ_map", durable_qos); + data_ave_pub_ = create_publisher("~/debug/data_average_occ_map", durable_qos); + data_std_pub_ = create_publisher("~/debug/data_std_dev_occ_map", durable_qos); + data_count_pub_ = create_publisher("~/debug/data_count_occ_map", durable_qos); + data_count_with_self_pose_pub_ = + create_publisher("~/debug/data_count_self_pose_occ_map", durable_qos); + index_pub_ = create_publisher("~/debug/occ_index", durable_qos); + original_map_raw_pub_ = + create_publisher("~/debug/original_raw_map", durable_qos); + update_map_raw_pub_ = create_publisher("~/output/update_raw_map", durable_qos); + debug_pub_ = create_publisher("~/output/debug_values", durable_qos); + current_map_error_pub_ = + create_publisher("~/output/current_map_error", durable_qos); + updated_map_error_pub_ = + create_publisher("~/output/updated_map_error", durable_qos); + map_error_ratio_pub_ = create_publisher("~/output/map_error_ratio", durable_qos); + offset_covariance_pub_ = + create_publisher("~/debug/offset_covariance", durable_qos); // subscriber using std::placeholders::_1; using std::placeholders::_2; using std::placeholders::_3; - velocity_sub_ = create_subscription( + velocity_sub_ = create_subscription( "~/input/velocity", queue_size, std::bind(&AccelBrakeMapCalibrator::callbackVelocity, this, _1)); - steer_sub_ = create_subscription( + steer_sub_ = create_subscription( "~/input/steer", queue_size, std::bind(&AccelBrakeMapCalibrator::callbackSteer, this, _1)); - actuation_status_sub_ = create_subscription( + actuation_status_sub_ = create_subscription( "~/input/actuation_status", queue_size, std::bind(&AccelBrakeMapCalibrator::callbackActuationStatus, this, _1)); // Service - update_map_dir_server_ = create_service( + update_map_dir_server_ = create_service( "~/input/update_map_dir", std::bind(&AccelBrakeMapCalibrator::callbackUpdateMapService, this, _1, _2, _3)); @@ -258,10 +244,8 @@ bool AccelBrakeMapCalibrator::getCurrentPitchFromTF(double * pitch) const auto transform = transform_listener_->getTransform( "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); if (!transform) { - auto & clk = *this->get_clock(); RCLCPP_WARN_STREAM_THROTTLE( - rclcpp::get_logger("accel_brake_map_calibrator"), clk, 5000, - "cannot get map to base_link transform. "); + get_logger(), *get_clock(), 5000, "cannot get map to base_link transform. "); return false; } double roll, raw_pitch, yaw; @@ -275,10 +259,9 @@ bool AccelBrakeMapCalibrator::getCurrentPitchFromTF(double * pitch) void AccelBrakeMapCalibrator::timerCallback() { update_count_++; - auto & clk = *this->get_clock(); RCLCPP_DEBUG_STREAM_THROTTLE( - rclcpp::get_logger("accel_brake_map_calibrator"), clk, 5000, + get_logger(), *get_clock(), 5000, "map updating... count: " << update_success_count_ << " / " << update_count_ << "\n\t" << "lack_of_data_count: " << lack_of_data_count_ << "\n\t" << " failed_to_get_pitch_count: " << failed_to_get_pitch_count_ @@ -299,10 +282,8 @@ void AccelBrakeMapCalibrator::timerCallback() !twist_ptr_ || !steer_ptr_ || !accel_pedal_ptr_ || !brake_pedal_ptr_ || !delayed_accel_pedal_ptr_ || !delayed_brake_pedal_ptr_) { // lack of data - auto & clk = *this->get_clock(); RCLCPP_WARN_STREAM_THROTTLE( - rclcpp::get_logger("accel_brake_map_calibrator"), clk, 5000, - "lack of topics (twist, steer, accel, brake)"); + get_logger(), *get_clock(), 5000, "lack of topics (twist, steer, accel, brake)"); lack_of_data_count_++; return; } @@ -312,10 +293,8 @@ void AccelBrakeMapCalibrator::timerCallback() isTimeout(twist_ptr_->header.stamp, timeout_sec_) || isTimeout(steer_ptr_->stamp, timeout_sec_) || isTimeout(accel_pedal_ptr_, timeout_sec_) || isTimeout(brake_pedal_ptr_, timeout_sec_)) { - auto & clk = *this->get_clock(); RCLCPP_WARN_STREAM_THROTTLE( - rclcpp::get_logger("accel_brake_map_calibrator"), clk, 5000, - "timeout of topics (twist, steer, accel, brake)"); + get_logger(), *get_clock(), 5000, "timeout of topics (twist, steer, accel, brake)"); lack_of_data_count_++; return; } @@ -323,9 +302,7 @@ void AccelBrakeMapCalibrator::timerCallback() // data check 3 if (!getCurrentPitchFromTF(&pitch_)) { // cannot get pitch - auto & clk = *this->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE( - rclcpp::get_logger("accel_brake_map_calibrator"), clk, 5000, "cannot get pitch"); + RCLCPP_WARN_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, "cannot get pitch"); failed_to_get_pitch_count_++; return; } @@ -396,9 +373,8 @@ void AccelBrakeMapCalibrator::timerCallback() delayed_accel_pedal_ptr_->data > std::numeric_limits::epsilon() && delayed_brake_pedal_ptr_->data > std::numeric_limits::epsilon()) { // both (accel/brake) output - auto & clk = *this->get_clock(); RCLCPP_DEBUG_STREAM_THROTTLE( - rclcpp::get_logger("accel_brake_map_calibrator"), clk, 5000, + get_logger(), *get_clock(), 5000, "invalid pedal value (Both of accel output and brake output area not zero. )"); invalid_acc_brake_count_++; return; @@ -447,30 +423,24 @@ void AccelBrakeMapCalibrator::timerCallbackOutputCSV() std::ifstream af(output_accel_file_); std::ifstream bf(output_brake_file_); if (!af.is_open() || !bf.is_open()) { - RCLCPP_WARN( - rclcpp::get_logger("accel_brake_map_calibrator"), "Accel map or brake map does not exist"); + RCLCPP_WARN(get_logger(), "Accel map or brake map does not exist"); return; } new_accel_map_ = AccelMap(); if (!new_accel_map_.readAccelMapFromCSV(output_accel_file_)) { - RCLCPP_WARN( - rclcpp::get_logger("accel_brake_map_calibrator"), "Cannot read accelmap. csv path = %s. ", - output_accel_file_.c_str()); + RCLCPP_WARN(get_logger(), "Cannot read accelmap. csv path = %s. ", output_accel_file_.c_str()); } new_brake_map_ = BrakeMap(); if (!new_brake_map_.readBrakeMapFromCSV(output_brake_file_)) { - RCLCPP_WARN( - rclcpp::get_logger("accel_brake_map_calibrator"), "Cannot read brakemap. csv path = %s. ", - output_brake_file_.c_str()); + RCLCPP_WARN(get_logger(), "Cannot read brakemap. csv path = %s. ", output_brake_file_.c_str()); } } -void AccelBrakeMapCalibrator::callbackVelocity( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) +void AccelBrakeMapCalibrator::callbackVelocity(const VelocityReport::ConstSharedPtr msg) { // convert velocity-report to twist-stamped - auto twist_msg = std::make_shared(); + auto twist_msg = std::make_shared(); twist_msg->header = msg->header; twist_msg->twist.linear.x = msg->longitudinal_velocity; twist_msg->twist.linear.y = msg->lateral_velocity; @@ -488,9 +458,7 @@ void AccelBrakeMapCalibrator::callbackVelocity( if ( this->now().seconds() - pre_acceleration_time_ > timeout_sec_ || (acceleration_time_ - pre_acceleration_time_) <= std::numeric_limits::epsilon()) { - auto & clk = *this->get_clock(); - RCLCPP_DEBUG_STREAM_THROTTLE( - rclcpp::get_logger("accel_brake_map_calibrator"), clk, 5000, "cannot calculate jerk"); + RCLCPP_DEBUG_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, "cannot calculate jerk"); // does not update jerk } else { const double raw_jerk = getJerk(); @@ -507,15 +475,14 @@ void AccelBrakeMapCalibrator::callbackVelocity( pushDataToVec(twist_msg, twist_vec_max_size_, &twist_vec_); } -void AccelBrakeMapCalibrator::callbackSteer( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) +void AccelBrakeMapCalibrator::callbackSteer(const SteeringReport::ConstSharedPtr msg) { debug_values_.data.at(CURRENT_STEER) = msg->steering_tire_angle; steer_ptr_ = msg; } void AccelBrakeMapCalibrator::callbackActuationStatus( - const tier4_vehicle_msgs::msg::ActuationStatusStamped::ConstSharedPtr msg) + const ActuationStatusStamped::ConstSharedPtr msg) { // get accel data accel_pedal_ptr_ = @@ -554,15 +521,12 @@ void AccelBrakeMapCalibrator::callbackActuationStatus( bool AccelBrakeMapCalibrator::callbackUpdateMapService( [[maybe_unused]] const std::shared_ptr request_header, - tier4_vehicle_msgs::srv::UpdateAccelBrakeMap::Request::SharedPtr req, - tier4_vehicle_msgs::srv::UpdateAccelBrakeMap::Response::SharedPtr res) + UpdateAccelBrakeMap::Request::SharedPtr req, UpdateAccelBrakeMap::Response::SharedPtr res) { // if req.path is input, use this as the directory to set updated maps std::string update_map_dir = req->path.empty() ? csv_default_map_dir_ : req->path; - RCLCPP_INFO_STREAM( - rclcpp::get_logger("accel_brake_map_calibrator"), - "update accel/brake map. directory: " << update_map_dir); + RCLCPP_INFO_STREAM(get_logger(), "update accel/brake map. directory: " << update_map_dir); const auto accel_map_file = update_map_dir + "/accel_map.csv"; const auto brake_map_file = update_map_dir + "/brake_map.csv"; if ( @@ -599,8 +563,8 @@ double AccelBrakeMapCalibrator::getPedalSpeed( } double AccelBrakeMapCalibrator::getAccel( - const geometry_msgs::msg::TwistStamped::ConstSharedPtr & prev_twist, - const geometry_msgs::msg::TwistStamped::ConstSharedPtr & current_twist) + const TwistStamped::ConstSharedPtr & prev_twist, + const TwistStamped::ConstSharedPtr & current_twist) { const double dt = (rclcpp::Time(current_twist->header.stamp) - rclcpp::Time(prev_twist->header.stamp)).seconds(); @@ -826,9 +790,7 @@ void AccelBrakeMapCalibrator::executeUpdate( const double map_acc = accel_mode ? update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index) : update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index); - RCLCPP_DEBUG_STREAM( - rclcpp::get_logger("accel_brake_map_calibrator"), - "measured_acc: " << measured_acc << ", map_acc: " << map_acc); + RCLCPP_DEBUG_STREAM(get_logger(), "measured_acc: " << measured_acc << ", map_acc: " << map_acc); if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_EACH_CELL) { updateEachValOffset( @@ -856,9 +818,9 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset( const int brake_pedal_index, const int brake_vel_index, const double measured_acc) { // pre-defined - static std::vector> accel_map_offset_vec_( + static Map accel_map_offset_vec_( accel_map_value_.size(), std::vector(accel_map_value_.at(0).size(), map_offset_)); - static std::vector> brake_map_offset_vec_( + static Map brake_map_offset_vec_( brake_map_value_.size(), std::vector(accel_map_value_.at(0).size(), map_offset_)); static std::vector> accel_covariance_mat_( accel_map_value_.size() - 1, @@ -1005,10 +967,10 @@ bool AccelBrakeMapCalibrator::updateEachValOffset( const double map_acc) { // pre-defined - static std::vector> map_offset_vec_( + static Map map_offset_vec_( accel_map_value_.size() + brake_map_value_.size() - 1, std::vector(accel_map_value_.at(0).size(), map_offset_)); - static std::vector> covariance_vec_( + static Map covariance_vec_( accel_map_value_.size() + brake_map_value_.size() - 1, std::vector(accel_map_value_.at(0).size(), covariance_)); @@ -1030,10 +992,10 @@ bool AccelBrakeMapCalibrator::updateEachValOffset( map_offset = map_offset + coef * error_map_offset; RCLCPP_DEBUG_STREAM( - rclcpp::get_logger("accel_brake_map_calibrator"), - "index: " << ped_idx << ", " << vel_idx << ": map_offset_ = " - << map_offset_vec_.at(ped_idx).at(vel_idx) << " -> " << map_offset << "\t" - << " covariance = " << covariance); + get_logger(), "index: " << ped_idx << ", " << vel_idx + << ": map_offset_ = " << map_offset_vec_.at(ped_idx).at(vel_idx) + << " -> " << map_offset << "\t" + << " covariance = " << covariance); /* input calculated result and update map */ map_offset_vec_.at(ped_idx).at(vel_idx) = map_offset; @@ -1061,9 +1023,8 @@ void AccelBrakeMapCalibrator::updateTotalMapOffset(const double measured_acc, co map_offset_ = map_offset_ + coef * error_map_offset; RCLCPP_DEBUG_STREAM( - rclcpp::get_logger("accel_brake_map_calibrator"), - "map_offset_ = " << map_offset_ << "\t" - << "covariance = " << covariance_); + get_logger(), "map_offset_ = " << map_offset_ << "\t" + << "covariance = " << covariance_); /* update map */ for (std::size_t ped_idx = 0; ped_idx < update_accel_map_value_.size() - 1; ped_idx++) { @@ -1081,8 +1042,7 @@ void AccelBrakeMapCalibrator::updateTotalMapOffset(const double measured_acc, co } std::vector AccelBrakeMapCalibrator::getMapColumnFromUnifiedIndex( - const std::vector> & accel_map_value, - const std::vector> & brake_map_value, const std::size_t index) + const Map & accel_map_value, const Map & brake_map_value, const std::size_t index) { if (index < brake_map_value.size()) { // input brake map value @@ -1198,8 +1158,8 @@ double AccelBrakeMapCalibrator::calculateAccelErrorL1Norm( } void AccelBrakeMapCalibrator::pushDataToQue( - const geometry_msgs::msg::TwistStamped::ConstSharedPtr & data, const std::size_t max_size, - std::queue * que) + const TwistStamped::ConstSharedPtr & data, const std::size_t max_size, + std::queue * que) { que->push(data); while (que->size() > max_size) { @@ -1294,11 +1254,11 @@ bool AccelBrakeMapCalibrator::isTimeout( return dt > timeout_sec; } -nav_msgs::msg::OccupancyGrid AccelBrakeMapCalibrator::getOccMsg( +OccupancyGrid AccelBrakeMapCalibrator::getOccMsg( const std::string frame_id, const double height, const double width, const double resolution, const std::vector & map_value) { - nav_msgs::msg::OccupancyGrid occ; + OccupancyGrid occ; occ.header.frame_id = frame_id; occ.header.stamp = this->now(); occ.info.height = height; @@ -1320,7 +1280,7 @@ nav_msgs::msg::OccupancyGrid AccelBrakeMapCalibrator::getOccMsg( void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticStatusWrapper & stat) { using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; - using CalibrationStatus = tier4_external_api_msgs::msg::CalibrationStatus; + using CalibrationStatus = CalibrationStatus; CalibrationStatus accel_brake_map_status; int8_t level; std::string msg; @@ -1359,12 +1319,11 @@ void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticS // function for debug void AccelBrakeMapCalibrator::publishMap( - const std::vector> accel_map_value, - const std::vector> brake_map_value, const std::string publish_type) + const Map accel_map_value, const Map brake_map_value, const std::string publish_type) { if (accel_map_value.at(0).size() != brake_map_value.at(0).size()) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger("accel_brake_map_calibrator"), + get_logger(), "Invalid map. The number of velocity index of accel map and brake map is different."); return; } @@ -1394,7 +1353,7 @@ void AccelBrakeMapCalibrator::publishMap( } // publish raw map - std_msgs::msg::Float32MultiArray float_map; + Float32MultiArray float_map; float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); @@ -1421,12 +1380,11 @@ void AccelBrakeMapCalibrator::publishMap( } void AccelBrakeMapCalibrator::publishOffsetCovMap( - const std::vector> accel_map_value, - const std::vector> brake_map_value) + const Map accel_map_value, const Map brake_map_value) { if (accel_map_value.at(0).size() != brake_map_value.at(0).size()) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger("accel_brake_map_calibrator"), + get_logger(), "Invalid map. The number of velocity index of accel map and brake map is different."); return; } @@ -1435,7 +1393,7 @@ void AccelBrakeMapCalibrator::publishOffsetCovMap( const double w = accel_map_value.at(0).size(); // velocity // publish raw map - std_msgs::msg::Float32MultiArray float_map; + Float32MultiArray float_map; float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); @@ -1459,7 +1417,7 @@ void AccelBrakeMapCalibrator::publishOffsetCovMap( void AccelBrakeMapCalibrator::publishFloat32(const std::string publish_type, const double val) { - tier4_debug_msgs::msg::Float32Stamped msg; + Float32Stamped msg; msg.stamp = this->now(); msg.data = val; if (publish_type == "current_map_error") { @@ -1475,7 +1433,7 @@ void AccelBrakeMapCalibrator::publishCountMap() { if (accel_map_value_.at(0).size() != brake_map_value_.at(0).size()) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger("accel_brake_map_calibrator"), + get_logger(), "Invalid map. The number of velocity index of accel map and brake map is different."); return; } @@ -1539,7 +1497,7 @@ void AccelBrakeMapCalibrator::publishCountMap() void AccelBrakeMapCalibrator::publishIndex() { - visualization_msgs::msg::MarkerArray markers; + MarkerArray markers; const double h = accel_map_value_.size() + brake_map_value_.size() - 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) const double w = accel_map_value_.at(0).size(); // velocity @@ -1622,9 +1580,8 @@ void AccelBrakeMapCalibrator::publishUpdateSuggestFlag() const double rmse_rate = new_accel_rmse_ / part_original_accel_rmse_; update_suggest.data = (rmse_rate < update_suggest_thresh_); if (update_suggest.data) { - auto & clk = *this->get_clock(); RCLCPP_WARN_STREAM_THROTTLE( - rclcpp::get_logger("accel_brake_map_calibrator"), clk, 5000, + get_logger(), *get_clock(), 5000, "suggest to update accel/brake map. evaluation score = " << rmse_rate); } } @@ -1633,8 +1590,8 @@ void AccelBrakeMapCalibrator::publishUpdateSuggestFlag() } bool AccelBrakeMapCalibrator::writeMapToCSV( - std::vector vel_index, std::vector pedal_index, - std::vector> value_map, std::string filename) + std::vector vel_index, std::vector pedal_index, Map value_map, + std::string filename) { if (update_success_count_ == 0) { return false; @@ -1643,9 +1600,7 @@ bool AccelBrakeMapCalibrator::writeMapToCSV( std::ofstream csv_file(filename); if (!csv_file.is_open()) { - RCLCPP_WARN( - rclcpp::get_logger("accel_brake_map_calibrator"), "Failed to open csv file : %s", - filename.c_str()); + RCLCPP_WARN(get_logger(), "Failed to open csv file : %s", filename.c_str()); return false; } @@ -1669,8 +1624,7 @@ bool AccelBrakeMapCalibrator::writeMapToCSV( csv_file << "\n"; } csv_file.close(); - RCLCPP_DEBUG_STREAM( - rclcpp::get_logger("accel_brake_map_calibrator"), "output map to " << filename); + RCLCPP_DEBUG_STREAM(get_logger(), "output map to " << filename); return true; } @@ -1697,3 +1651,5 @@ void AccelBrakeMapCalibrator::addLogToCSV( << ", " << jerk << ", " << full_original_accel_mse << ", " << part_original_accel_mse << ", " << new_accel_mse << "," << rmse_rate << std::endl; } + +} // namespace accel_brake_map_calibrator diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/main.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/main.cpp index 14bac723fd777..01dfe20307f80 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/main.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/main.cpp @@ -22,7 +22,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; - rclcpp::spin(std::make_shared(node_options)); + rclcpp::spin(std::make_shared(node_options)); rclcpp::shutdown(); return 0; }