diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt index 454e0e7ef044f..bf48e8fcaca3f 100644 --- a/common/autoware_auto_geometry/CMakeLists.txt +++ b/common/autoware_auto_geometry/CMakeLists.txt @@ -12,6 +12,12 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/bounding_box.cpp ) +if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) + target_compile_definitions(${PROJECT_NAME} PRIVATE + DEFINE_LEGACY_FUNCTION + ) +endif() + if(BUILD_TESTING) set(GEOMETRY_GTEST geometry_gtest) set(GEOMETRY_SRC test/src/test_geometry.cpp diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt index a8ae9ec2d962e..fe4cec1c0fc89 100755 --- a/common/autoware_auto_tf2/CMakeLists.txt +++ b/common/autoware_auto_tf2/CMakeLists.txt @@ -17,6 +17,11 @@ if(BUILD_TESTING) "tf2_geometry_msgs" "tf2_ros" ) + if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) + target_compile_definitions(test_tf2_autoware_auto_msgs PRIVATE + DEFINE_LEGACY_FUNCTION + ) + endif() endif() ament_auto_package() diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp index 9b20953a7b3c5..ee8d52fa402c0 100644 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp @@ -45,7 +45,7 @@ using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; namespace tf2 { - +#ifdef DEFINE_LEGACY_FUNCTION /*************/ /** Point32 **/ /*************/ @@ -94,6 +94,7 @@ inline void doTransform( t_out.points[i].z = static_cast(v_out[2]); } } +#endif /******************/ /** Quaternion32 **/ diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 7ed5860612601..12190fb659235 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -122,12 +122,16 @@ def create_ransac_pipeline(self): plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_data", namespace="plane_fitting", - remappings=[("output", "concatenated/pointcloud")], + remappings=[ + ("~/input/odom", "/localization/kinematic_state"), + ("output", "concatenated/pointcloud"), + ], parameters=[ { "input_topics": self.ground_segmentation_param["ransac_input_topics"], "output_frame": LaunchConfiguration("base_frame"), "timeout_sec": 1.0, + "input_twist_topic_type": "odom", } ], extra_arguments=[ @@ -432,11 +436,15 @@ def get_additional_lidars_concatenated_component(input_topics, output_topic): package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_data", - remappings=[("output", output_topic)], + remappings=[ + ("~/input/odom", "/localization/kinematic_state"), + ("output", output_topic), + ], parameters=[ { "input_topics": input_topics, "output_frame": LaunchConfiguration("base_frame"), + "input_twist_topic_type": "odom", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -448,11 +456,15 @@ def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_no_ground_data", - remappings=[("output", output_topic)], + remappings=[ + ("~/input/odom", "/localization/kinematic_state"), + ("output", output_topic), + ], parameters=[ { "input_topics": input_topics, "output_frame": LaunchConfiguration("base_frame"), + "input_twist_topic_type": "odom", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md index 8c51febc6f60c..b212711f38a57 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md @@ -25,7 +25,8 @@ The positions and orientations of the AR-Tags are assumed to be written in the L | :------------------------------ | :---------------------------------------------- | :---------------------------------------------------------------------------------------- | | `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated Pose | | `~/debug/result` | `sensor_msgs::msg::Image` | [debug topic] Image in which marker detection results are superimposed on the input image | -| `tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | +| `/tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics outputs | ## How to launch diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp index 6af9fefae23f7..37725dd06c34e 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp @@ -48,6 +48,7 @@ #include #include +#include #include #include @@ -98,6 +99,7 @@ class ArTagBasedLocalizer : public rclcpp::Node // Publishers image_transport::Publisher image_pub_; rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; // Others aruco::MarkerDetector detector_; diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index e68fd21ed340a..2c35be181d2f0 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -13,6 +13,7 @@ aruco cv_bridge + diagnostic_msgs geometry_msgs image_transport rclcpp diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp index f664ef6378a69..afa82ab3e0677 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp @@ -129,6 +129,8 @@ bool ArTagBasedLocalizer::setup() image_pub_ = it_->advertise("~/debug/result", 1); pose_pub_ = this->create_publisher( "~/output/pose_with_covariance", qos_pub); + diag_pub_ = + this->create_publisher("/diagnostics", qos_pub); RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); return true; @@ -199,6 +201,32 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha out_msg.image = in_image; image_pub_.publish(out_msg.toImageMsg()); } + + const int detected_tags = markers.size(); + + diagnostic_msgs::msg::DiagnosticStatus diag_status; + + if (detected_tags > 0) { + diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diag_status.message = "AR tags detected. The number of tags: " + std::to_string(detected_tags); + } else { + diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status.message = "No AR tags detected."; + } + + diag_status.name = "localization: " + std::string(this->get_name()); + diag_status.hardware_id = this->get_name(); + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "Number of Detected AR Tags"; + key_value.value = std::to_string(detected_tags); + diag_status.values.push_back(key_value); + + diagnostic_msgs::msg::DiagnosticArray diag_msg; + diag_msg.header.stamp = this->now(); + diag_msg.status.push_back(diag_status); + + diag_pub_->publish(diag_msg); } // wait for one camera info, then shut down that subscriber diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index f62e418371401..542d4daa0c676 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -58,6 +58,12 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/marker_utils/lane_change/debug.cpp ) +if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) + target_compile_definitions(behavior_path_planner_node PRIVATE + DEFINE_LEGACY_FUNCTION + ) +endif() + target_include_directories(behavior_path_planner_node SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR} ) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 589ccebf5b0cb..ee5771c98217e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -19,10 +19,12 @@ #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" +#include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include +#include "tier4_debug_msgs/msg/float64_stamped.hpp" #include #include @@ -44,6 +46,8 @@ using tier4_autoware_utils::StopWatch; using tier4_planning_msgs::msg::StopReasonArray; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; +using DebugPublisher = tier4_autoware_utils::DebugPublisher; +using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped; enum Action { ADD = 0, @@ -421,6 +425,8 @@ class PlannerManager std::vector candidate_module_ptrs_; + std::unique_ptr debug_publisher_ptr_; + mutable rclcpp::Logger logger_; mutable rclcpp::Clock clock_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 6fd2045b4f097..d4013fd97b316 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -168,11 +168,12 @@ class StartPlannerModule : public SceneModuleInterface lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; std::vector generateDrivableLanes(const PathWithLaneId & path) const; void updatePullOutStatus(); + void updateStatusAfterBackwardDriving(); static bool isOverlappedWithLane( const lanelet::ConstLanelet & candidate_lanelet, const tier4_autoware_utils::LinearRing2d & vehicle_footprint); bool hasFinishedPullOut() const; - void checkBackFinished(); + bool isBackwardDrivingComplete() const; bool isStopped(); bool isStuck(); bool hasFinishedCurrentPath(); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 7a7ee046ca069..5963ddb778874 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include @@ -34,6 +35,7 @@ PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose) verbose_{verbose} { processing_time_.emplace("total_time", 0.0); + debug_publisher_ptr_ = std::make_unique(&node, "behavior_planner_manager/debug"); } BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & data) @@ -884,6 +886,8 @@ void PlannerManager::print() const string_stream << std::right << "[" << std::setw(max_string_num + 1) << std::left << t.first << ":" << std::setw(4) << std::right << t.second << "ms]\n" << std::setw(21); + std::string name = std::string("processing_time/") + t.first; + debug_publisher_ptr_->publish(name, t.second); } RCLCPP_INFO_STREAM(logger_, string_stream.str()); diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 5247a24579212..c2df11b4bede6 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -192,7 +192,10 @@ ModuleStatus StartPlannerModule::updateState() return ModuleStatus::SUCCESS; } - checkBackFinished(); + if (isBackwardDrivingComplete()) { + updateStatusAfterBackwardDriving(); + return ModuleStatus::SUCCESS; // for breaking loop + } return current_state_; } @@ -658,14 +661,28 @@ void StartPlannerModule::updatePullOutStatus() planWithPriority( start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); - checkBackFinished(); - if (!status_.back_finished) { + if (isBackwardDrivingComplete()) { + updateStatusAfterBackwardDriving(); + current_state_ = ModuleStatus::SUCCESS; // for breaking loop + } else { status_.backward_path = start_planner_utils::getBackwardPath( *route_handler, status_.pull_out_lanes, current_pose, status_.pull_out_start_pose, parameters_->backward_velocity); } } +void StartPlannerModule::updateStatusAfterBackwardDriving() +{ + status_.back_finished = true; + // request start_planner approval + waitApproval(); + // To enable approval of the forward path, the RTC status is removed. + removeRTCStatus(); + for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { + itr->second = generateUUID(); + } +} + PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const { const Pose & current_pose = planner_data_->self_odometry->pose.pose; @@ -787,9 +804,9 @@ bool StartPlannerModule::hasFinishedPullOut() const return has_finished; } -void StartPlannerModule::checkBackFinished() +bool StartPlannerModule::isBackwardDrivingComplete() const { - // check ego car is close enough to pull out start pose + // check ego car is close enough to pull out start pose and stopped const auto current_pose = planner_data_->self_odometry->pose.pose; const auto distance = tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); @@ -798,18 +815,12 @@ void StartPlannerModule::checkBackFinished() const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear); const bool is_stopped = ego_vel < parameters_->th_stopped_velocity; - if (!status_.back_finished && is_near && is_stopped) { + const bool back_finished = !status_.back_finished && is_near && is_stopped; + if (back_finished) { RCLCPP_INFO(getLogger(), "back finished"); - status_.back_finished = true; - - // request start_planner approval - waitApproval(); - removeRTCStatus(); - for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { - itr->second = generateUUID(); - } - current_state_ = ModuleStatus::SUCCESS; // for breaking loop } + + return back_finished; } bool StartPlannerModule::isStopped() diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index b5dd0e91a541e..f911fa87bedeb 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -443,7 +443,24 @@ void ObstacleAvoidancePlanner::applyInputVelocity( ego_nearest_param_.yaw_threshold); // calculate and insert stop pose on output trajectory - if (stop_seg_idx) { + const bool is_stop_point_inside_trajectory = [&]() { + if (!stop_seg_idx) { + return false; + } + if (*stop_seg_idx == output_traj_points.size() - 2) { + const double signed_projected_length_to_segment = + motion_utils::calcLongitudinalOffsetToSegment( + output_traj_points, *stop_seg_idx, input_stop_pose.position); + const double segment_length = + motion_utils::calcSignedArcLength(output_traj_points, *stop_seg_idx, *stop_seg_idx + 1); + if (segment_length < signed_projected_length_to_segment) { + // NOTE: input_stop_pose is outside output_traj_points. + return false; + } + } + return true; + }(); + if (is_stop_point_inside_trajectory) { trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, *stop_seg_idx); } } @@ -573,25 +590,25 @@ std::vector ObstacleAvoidancePlanner::extendTrajectory( const auto joint_end_traj_point_idx = trajectory_utils::getPointIndexAfter( traj_points, joint_start_pose.position, joint_start_traj_seg_idx, joint_traj_max_length_for_smoothing, joint_traj_min_length_for_smoothing); + if (!joint_end_traj_point_idx) { + return trajectory_utils::resampleTrajectoryPoints( + optimized_traj_points, traj_param_.output_delta_arc_length); + } // calculate full trajectory points const auto full_traj_points = [&]() { - if (!joint_end_traj_point_idx) { - return optimized_traj_points; - } - - const auto extended_traj_points = std::vector{ + auto extended_traj_points = std::vector{ traj_points.begin() + *joint_end_traj_point_idx, traj_points.end()}; - // NOTE: if optimized_traj_points's back is non zero velocity and extended_traj_points' front is - // zero velocity, the zero velocity will be inserted in the whole joint trajectory. - auto modified_optimized_traj_points = optimized_traj_points; - if (!extended_traj_points.empty() && !modified_optimized_traj_points.empty()) { - modified_optimized_traj_points.back().longitudinal_velocity_mps = - extended_traj_points.front().longitudinal_velocity_mps; + if (!extended_traj_points.empty() && !optimized_traj_points.empty()) { + // NOTE: Without this code, if optimized_traj_points's back is non zero velocity and + // extended_traj_points' front + // is zero velocity, the zero velocity will be inserted in the whole joint trajectory. + // The input stop point will be inserted explicitly in the latter part. + extended_traj_points.front().longitudinal_velocity_mps = + optimized_traj_points.back().longitudinal_velocity_mps; } - - return concatVectors(modified_optimized_traj_points, extended_traj_points); + return concatVectors(optimized_traj_points, extended_traj_points); }(); // resample trajectory points @@ -599,7 +616,7 @@ std::vector ObstacleAvoidancePlanner::extendTrajectory( full_traj_points, traj_param_.output_delta_arc_length); // update stop velocity on joint - for (size_t i = joint_start_traj_seg_idx + 1; i <= joint_end_traj_point_idx; ++i) { + for (size_t i = joint_start_traj_seg_idx + 1; i <= *joint_end_traj_point_idx; ++i) { if (hasZeroVelocity(traj_points.at(i))) { if (i != 0 && !hasZeroVelocity(traj_points.at(i - 1))) { // Here is when current point is 0 velocity, but previous point is not 0 velocity. diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index dbd0f2de92f29..31fe23a47b0ca 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -270,12 +270,36 @@ void ElasticBandSmoother::applyInputVelocity( // insert stop point explicitly const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); if (stop_idx) { - const auto input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; - const size_t stop_seg_idx = trajectory_utils::findEgoSegmentIndex( - output_traj_points, input_stop_pose, ego_nearest_param_); + const auto & input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; + // NOTE: motion_utils::findNearestSegmentIndex is used instead of + // trajectory_utils::findEgoSegmentIndex + // for the case where input_traj_points is much longer than output_traj_points, and the + // former has a stop point but the latter will not have. + const auto stop_seg_idx = motion_utils::findNearestSegmentIndex( + output_traj_points, input_stop_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); // calculate and insert stop pose on output trajectory - trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, stop_seg_idx); + const bool is_stop_point_inside_trajectory = [&]() { + if (!stop_seg_idx) { + return false; + } + if (*stop_seg_idx == output_traj_points.size() - 2) { + const double signed_projected_length_to_segment = + motion_utils::calcLongitudinalOffsetToSegment( + output_traj_points, *stop_seg_idx, input_stop_pose.position); + const double segment_length = + motion_utils::calcSignedArcLength(output_traj_points, *stop_seg_idx, *stop_seg_idx + 1); + if (segment_length < signed_projected_length_to_segment) { + // NOTE: input_stop_pose is outside output_traj_points. + return false; + } + } + return true; + }(); + if (is_stop_point_inside_trajectory) { + trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, *stop_seg_idx); + } } time_keeper_ptr_->toc(__func__, " "); @@ -299,25 +323,25 @@ std::vector ElasticBandSmoother::extendTrajectory( const auto joint_end_traj_point_idx = trajectory_utils::getPointIndexAfter( traj_points, joint_start_pose.position, joint_start_traj_seg_idx, joint_traj_max_length_for_smoothing, joint_traj_min_length_for_smoothing); + if (!joint_end_traj_point_idx) { + return trajectory_utils::resampleTrajectoryPoints( + optimized_traj_points, common_param_.output_delta_arc_length); + } // calculate full trajectory points const auto full_traj_points = [&]() { - if (!joint_end_traj_point_idx) { - return optimized_traj_points; - } - - const auto extended_traj_points = std::vector{ + auto extended_traj_points = std::vector{ traj_points.begin() + *joint_end_traj_point_idx, traj_points.end()}; - // NOTE: if optimized_traj_points's back is non zero velocity and extended_traj_points' front is - // zero velocity, the zero velocity will be inserted in the whole joint trajectory. - auto modified_optimized_traj_points = optimized_traj_points; - if (!extended_traj_points.empty() && !modified_optimized_traj_points.empty()) { - modified_optimized_traj_points.back().longitudinal_velocity_mps = - extended_traj_points.front().longitudinal_velocity_mps; + if (!extended_traj_points.empty() && !optimized_traj_points.empty()) { + // NOTE: Without this code, if optimized_traj_points's back is non zero velocity and + // extended_traj_points' front + // is zero velocity, the zero velocity will be inserted in the whole joint trajectory. + // The input stop point will be inserted explicitly in the latter part. + extended_traj_points.front().longitudinal_velocity_mps = + optimized_traj_points.back().longitudinal_velocity_mps; } - - return concatVectors(modified_optimized_traj_points, extended_traj_points); + return concatVectors(optimized_traj_points, extended_traj_points); }(); // resample trajectory points @@ -325,7 +349,7 @@ std::vector ElasticBandSmoother::extendTrajectory( full_traj_points, common_param_.output_delta_arc_length); // update stop velocity on joint - for (size_t i = joint_start_traj_seg_idx + 1; i <= joint_end_traj_point_idx; ++i) { + for (size_t i = joint_start_traj_seg_idx + 1; i <= *joint_end_traj_point_idx; ++i) { if (hasZeroVelocity(traj_points.at(i))) { if (i != 0 && !hasZeroVelocity(traj_points.at(i - 1))) { // Here is when current point is 0 velocity, but previous point is not 0 velocity. diff --git a/planning/surround_obstacle_checker/CMakeLists.txt b/planning/surround_obstacle_checker/CMakeLists.txt index 0d7b636646783..694bddf421486 100644 --- a/planning/surround_obstacle_checker/CMakeLists.txt +++ b/planning/surround_obstacle_checker/CMakeLists.txt @@ -18,6 +18,12 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/node.cpp ) +if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) + target_compile_definitions(${PROJECT_NAME} PRIVATE + DEFINE_LEGACY_FUNCTION + ) +endif() + target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} ) diff --git a/sensing/pointcloud_preprocessor/docs/concatenate-data.md b/sensing/pointcloud_preprocessor/docs/concatenate-data.md index a3b5268815f24..74b4f3df17615 100644 --- a/sensing/pointcloud_preprocessor/docs/concatenate-data.md +++ b/sensing/pointcloud_preprocessor/docs/concatenate-data.md @@ -42,6 +42,7 @@ The figure below represents the reception time of each sensor data and how it is | `timeout_sec` | double | 0.1 | tolerance of time to publish next pointcloud [s]
When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. | | `input_offset` | vector of double | [] | This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers.
For its tuning, please see [actual usage page](#how-to-tuning-timeout_sec-and-input_offset). | | `publish_synchronized_pointcloud` | bool | false | If true, publish the time synchronized pointclouds. All input pointclouds are transformed and then re-published as message named `_synchronized`. | +| `input_twist_topic_type` | std::string | twist | Topic type for twist. Currently support `twist` or `odom`. | ## Actual Usage diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index f5d55a2ebcbf7..be96aa94382dd 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -71,6 +71,7 @@ #include #include #include +#include #include #include #include @@ -88,6 +89,7 @@ namespace pointcloud_preprocessor { using autoware_point_types::PointXYZI; using point_cloud_msg_wrapper::PointCloud2Modifier; + /** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data * synchronizer: it listens for a set of input PointCloud messages on the same topic, * checks their timestamps, and concatenates their fields together into a single @@ -131,10 +133,13 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node std::vector::SharedPtr> filters_; rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::TimerBase::SharedPtr timer_; diagnostic_updater::Updater updater_{this}; + const std::string input_twist_topic_type_; + /** \brief Output TF frame the concatenated points should be transformed to. */ std::string output_frame_; @@ -170,6 +175,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name); void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input); + void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input); void timer_callback(); void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index 6b1fc0f5bd976..0d65b45b1b3de 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -38,13 +38,17 @@ def launch_setup(context, *args, **kwargs): package=pkg, plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="sync_and_concatenate_filter", - remappings=[("output", "points_raw/concatenated")], + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("output", "points_raw/concatenated"), + ], parameters=[ { "input_topics": LaunchConfiguration("input_points_raw_list"), "output_frame": LaunchConfiguration("tf_output_frame"), "approximate_sync": True, "publish_synchronized_pointcloud": False, + "input_twist_topic_type": "twist", } ], ) diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index 8ae0bdacb983a..ff72e433e9602 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -69,7 +69,8 @@ namespace pointcloud_preprocessor { PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchronizerComponent( const rclcpp::NodeOptions & node_options) -: Node("point_cloud_concatenator_component", node_options) +: Node("point_cloud_concatenator_component", node_options), + input_twist_topic_type_(declare_parameter("input_twist_topic_type", "twist")) { // initialize debug tool { @@ -164,10 +165,24 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro filters_[d] = this->create_subscription( input_topics_[d], rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), cb); } - auto twist_cb = std::bind( - &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); - sub_twist_ = this->create_subscription( - "~/input/twist", rclcpp::QoS{100}, twist_cb); + + if (input_twist_topic_type_ == "twist") { + auto twist_cb = std::bind( + &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, + std::placeholders::_1); + sub_twist_ = this->create_subscription( + "~/input/twist", rclcpp::QoS{100}, twist_cb); + } else if (input_twist_topic_type_ == "odom") { + auto odom_cb = std::bind( + &PointCloudConcatenateDataSynchronizerComponent::odom_callback, this, + std::placeholders::_1); + sub_odom_ = this->create_subscription( + "~/input/odom", rclcpp::QoS{100}, odom_cb); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), "input_twist_topic_type is invalid: " << input_twist_topic_type_); + throw std::runtime_error("input_twist_topic_type is invalid: " + input_twist_topic_type_); + } } // Transformed Raw PointCloud2 Publisher @@ -227,8 +242,19 @@ Eigen::Matrix4f PointCloudConcatenateDataSynchronizerComponent::computeTransformToAdjustForOldTimestamp( const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp) { - // return identity if no twist is available or old_stamp is newer than new_stamp - if (twist_ptr_queue_.empty() || old_stamp > new_stamp) { + // return identity if no twist is available + if (twist_ptr_queue_.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), + "No twist is available. Please confirm twist topic and timestamp"); + return Eigen::Matrix4f::Identity(); + } + + // return identity if old_stamp is newer than new_stamp + if (old_stamp > new_stamp) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), + "old_stamp is newer than new_stamp,"); return Eigen::Matrix4f::Identity(); } @@ -558,6 +584,32 @@ void PointCloudConcatenateDataSynchronizerComponent::twist_callback( twist_ptr_queue_.push_back(twist_ptr); } +void PointCloudConcatenateDataSynchronizerComponent::odom_callback( + const nav_msgs::msg::Odometry::ConstSharedPtr input) +{ + // if rosbag restart, clear buffer + if (!twist_ptr_queue_.empty()) { + if (rclcpp::Time(twist_ptr_queue_.front()->header.stamp) > rclcpp::Time(input->header.stamp)) { + twist_ptr_queue_.clear(); + } + } + + // pop old data + while (!twist_ptr_queue_.empty()) { + if ( + rclcpp::Time(twist_ptr_queue_.front()->header.stamp) + rclcpp::Duration::from_seconds(1.0) > + rclcpp::Time(input->header.stamp)) { + break; + } + twist_ptr_queue_.pop_front(); + } + + auto twist_ptr = std::make_shared(); + twist_ptr->header = input->header; + twist_ptr->twist = input->twist.twist; + twist_ptr_queue_.push_back(twist_ptr); +} + void PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus( diagnostic_updater::DiagnosticStatusWrapper & stat) {