diff --git a/approaching_emergency_vehicle_plugin/src/approaching_emergency_vehicle_plugin_node.cpp b/approaching_emergency_vehicle_plugin/src/approaching_emergency_vehicle_plugin_node.cpp index 4c351f693d..31fd8e0568 100644 --- a/approaching_emergency_vehicle_plugin/src/approaching_emergency_vehicle_plugin_node.cpp +++ b/approaching_emergency_vehicle_plugin/src/approaching_emergency_vehicle_plugin_node.cpp @@ -21,10 +21,10 @@ namespace approaching_emergency_vehicle_plugin namespace { /** - * \brief Anonymous function to extract maneuver end speed which cannot be obtained with GET_MANEUVER_PROPERY calls + * \brief Anonymous function to extract maneuver end speed which cannot be obtained with GET_MANEUVER_PROPERY calls * due to it missing in stop and wait plugin - */ - double getManeuverEndSpeed(const carma_planning_msgs::msg::Maneuver& mvr) { + */ + double getManeuverEndSpeed(const carma_planning_msgs::msg::Maneuver& mvr) { switch(mvr.type) { case carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING: return mvr.lane_following_maneuver.end_speed; @@ -185,7 +185,7 @@ namespace approaching_emergency_vehicle_plugin // Return success if everything initialized successfully return CallbackReturn::SUCCESS; } - + carma_ros2_utils::CallbackReturn ApproachingEmergencyVehiclePlugin::on_activate_plugin() { // Timer setup for generating a BSM @@ -276,7 +276,7 @@ namespace approaching_emergency_vehicle_plugin carma_msgs::msg::UIInstructions status_msg = generateApproachingErvStatusMessage(); approaching_erv_status_pub_->publish(status_msg); } - + carma_msgs::msg::UIInstructions ApproachingEmergencyVehiclePlugin::generateApproachingErvStatusMessage(){ // Initialize the message that will be returned by this function carma_msgs::msg::UIInstructions status_msg; @@ -288,11 +288,11 @@ namespace approaching_emergency_vehicle_plugin * |--------0----------------------1----------------------2--------------| * Index 0: (Boolean) Value indicating whether the ego vehicle is tracking an approaching ERV * Index 1: (Double; rounded to first decimal place) If an approaching ERV exists, this indicates the estimated seconds until the ERV passes the ego vehicle. - * Index 2: (String) If an approaching ERV exists, this describes the current action of the ego vehicle in response to the approaching ERV. + * Index 2: (String) If an approaching ERV exists, this describes the current action of the ego vehicle in response to the approaching ERV. * NOTE: The values of indexes 1 and 2 can be ignored if index 0 indicates that no approaching ERV is being tracked. */ boost::format fmter(APPROACHING_ERV_STATUS_PARAMS); - + // Index 0 of formatted string; indicates whether an ERV that is approaching the ego vehicle is being tracked if(has_tracked_erv_ && tracked_erv_.seconds_until_passing <= config_.approaching_threshold){ fmter %true; @@ -373,7 +373,7 @@ namespace approaching_emergency_vehicle_plugin if (!map_projector_) { throw std::invalid_argument("Attempting to get ERV's current position in map before map projection was set"); } - + // Build map projector lanelet::projection::LocalFrameProjector projector(map_projector_.get().c_str()); @@ -480,7 +480,7 @@ namespace approaching_emergency_vehicle_plugin // Get vehicle's current position in the map frame from the BSM boost::optional erv_position_in_map = getErvPositionInMap(erv_information.current_latitude, erv_information.current_longitude); - + if(erv_position_in_map){ erv_information.current_position_in_map = *erv_position_in_map; } @@ -499,10 +499,10 @@ namespace approaching_emergency_vehicle_plugin } } } - + if(erv_destination_points.empty()){ RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "ERV's BSM does not contain any future destination points"); - + // BSM is not a valid ERV BSM since it does not include destination points; return an empty object return boost::optional(); } @@ -519,15 +519,15 @@ namespace approaching_emergency_vehicle_plugin return boost::optional(); } - + // Determine the ERV's current lane index // Note: For 'lane index', 0 is rightmost lane, 1 is second rightmost, etc.; Only the current travel direction is considered if(!erv_future_route.get().shortestPath().empty()){ lanelet::ConstLanelet erv_current_lanelet = erv_future_route.get().shortestPath()[0]; - // NOTE: this logic checks if the ERV and CMV are on a same direction or not. + // NOTE: this logic checks if the ERV and CMV are on a same direction or not. // Currently this check is sufficient to happen only once due to the use case scenarios - if (is_same_direction_.find(erv_information.vehicle_id) == is_same_direction_.end()) + if (is_same_direction_.find(erv_information.vehicle_id) == is_same_direction_.end()) { is_same_direction_[erv_information.vehicle_id] = false; for (auto llt: erv_future_route.get().shortestPath()) // checks if ERV is on the same path assuming CMV got all of its planned route when detected @@ -569,7 +569,7 @@ namespace approaching_emergency_vehicle_plugin erv_information.previous_lane_index = lane_index; } - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "ERV's lane index is " << erv_information.lane_index); } @@ -578,9 +578,9 @@ namespace approaching_emergency_vehicle_plugin } // Get intersecting lanelet between ERV's future route and ego vehicle's future shortest path - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Calling getRouteIntersectingLanelet"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Calling getRouteIntersectingLanelet"); boost::optional intersecting_lanelet = getRouteIntersectingLanelet(erv_future_route.get()); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Done calling getRouteIntersectingLanelet"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Done calling getRouteIntersectingLanelet"); if(intersecting_lanelet){ erv_information.intersecting_lanelet = *intersecting_lanelet; @@ -593,9 +593,9 @@ namespace approaching_emergency_vehicle_plugin } // Get the time (seconds) until the ERV passes the ego vehicle - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Calling getSecondsUntilPassing()"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Calling getSecondsUntilPassing()"); boost::optional seconds_until_passing = getSecondsUntilPassing(erv_future_route, erv_information.current_position_in_map, erv_information.current_speed, erv_information.intersecting_lanelet); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Done calling getSecondsUntilPassing()"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Done calling getSecondsUntilPassing()"); if(seconds_until_passing){ erv_information.seconds_until_passing = seconds_until_passing.get(); @@ -611,13 +611,13 @@ namespace approaching_emergency_vehicle_plugin return erv_information; } - void ApproachingEmergencyVehiclePlugin::georeferenceCallback(const std_msgs::msg::String::UniquePtr msg) + void ApproachingEmergencyVehiclePlugin::georeferenceCallback(const std_msgs::msg::String::UniquePtr msg) { // Build projector from proj string map_projector_ = msg->data; } - void ApproachingEmergencyVehiclePlugin::incomingEmergencyVehicleAckCallback(const carma_v2x_msgs::msg::EmergencyVehicleAck::UniquePtr msg) + void ApproachingEmergencyVehiclePlugin::incomingEmergencyVehicleAckCallback(const carma_v2x_msgs::msg::EmergencyVehicleAck::UniquePtr msg) { // Only process message if it is from the currently-tracked ERV and it is intended for the ego vehicle if(has_tracked_erv_ && (msg->m_header.sender_id == tracked_erv_.vehicle_id) && (msg->m_header.recipient_id == config_.vehicle_id)){ @@ -630,13 +630,13 @@ namespace approaching_emergency_vehicle_plugin } - lanelet::Optional ApproachingEmergencyVehiclePlugin::generateErvRoute(double current_latitude, double current_longitude, + lanelet::Optional ApproachingEmergencyVehiclePlugin::generateErvRoute(double current_latitude, double current_longitude, std::vector erv_destination_points){ // Check if the map projection is available if (!map_projector_) { throw std::invalid_argument("Attempting to generate an ERV's route before map projection was set"); } - + // Build map projector lanelet::projection::LocalFrameProjector projector(map_projector_.get().c_str()); @@ -649,13 +649,13 @@ namespace approaching_emergency_vehicle_plugin current_erv_location.lat = current_latitude; current_erv_location.lon = current_longitude; - + // Add ERV's future destination points to erv_destination_points if(erv_destination_points.size() > 0){ for(size_t i = 0; i < erv_destination_points.size(); ++i){ carma_v2x_msgs::msg::Position3D position_3d_point = erv_destination_points[i]; - + lanelet::GPSPoint erv_destination_point; erv_destination_point.lon = position_3d_point.longitude; erv_destination_point.lat = position_3d_point.latitude; @@ -688,7 +688,7 @@ namespace approaching_emergency_vehicle_plugin // Return empty route if a destination point is not contained within the map if((wm_->getLaneletsFromPoint(shortened_erv_destination_points_in_map[i])).empty()){ - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "ERV destination point " << i + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "ERV destination point " << i << " is not contained in a lanelet map; x: " << pt.x() << " y: " << pt.y()); return lanelet::Optional(); @@ -740,7 +740,7 @@ namespace approaching_emergency_vehicle_plugin { return original_points; } - + // extend the list by extrapolating last two points auto extended_points = original_points; double last_dx = (original_points.end() - 1 ) ->x() - (original_points.end() - 2 ) ->x(); @@ -775,12 +775,12 @@ namespace approaching_emergency_vehicle_plugin double angleRad = acos(dotProduct / (v1Mag * v2Mag)); // Angle between the vectors above 90 degrees means the point i is ahead - if (angleRad >= M_PI / 2) + if (angleRad >= M_PI / 2) { - double dx = reference_point.x() - extended_points[i].x(); + double dx = reference_point.x() - extended_points[i].x(); double dy = reference_point.y() - extended_points[i].y(); double distance = sqrt (dx * dx + dy * dy); - if (distance < closest_dist) // get closest point to the reference point from the multiple possible points + if (distance < closest_dist) // get closest point to the reference point from the multiple possible points { closest_dist = distance; closest_idx = i; @@ -791,16 +791,16 @@ namespace approaching_emergency_vehicle_plugin i ++; } - double dx = reference_point.x() - original_points.back().x(); + double dx = reference_point.x() - original_points.back().x(); double dy = reference_point.y() - original_points.back().y(); double distance = sqrt (dx * dx + dy * dy); // last point is still closer to the reference point, all points have passed // note that if the optimal point is the last one, this check fails as intended - if (distance < closest_dist) + if (distance < closest_dist) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name), "Returning empty here"); - + return {}; } @@ -880,7 +880,7 @@ namespace approaching_emergency_vehicle_plugin return; } - boost::optional ApproachingEmergencyVehiclePlugin::getSecondsUntilPassing(lanelet::Optional& erv_future_route, const lanelet::BasicPoint2d& erv_position_in_map, + boost::optional ApproachingEmergencyVehiclePlugin::getSecondsUntilPassing(lanelet::Optional& erv_future_route, const lanelet::BasicPoint2d& erv_position_in_map, const double& erv_current_speed, lanelet::ConstLanelet& intersecting_lanelet){ // Obtain ego vehicle and ERV distances to the end of the intersecting lanelet so neither vehicle will currently be past that point @@ -944,7 +944,7 @@ namespace approaching_emergency_vehicle_plugin boost::optional ApproachingEmergencyVehiclePlugin::getRouteIntersectingLanelet(const lanelet::routing::Route& erv_future_route){ if(future_route_lanelet_ids_.empty()){ - RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "Remaining route lanelets for the ego vehicle not found; plugin cannot compute the intersecting lanelet."); + RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "Remaining route lanelets for the ego vehicle not found; plugin cannot compute the intersecting lanelet."); return boost::optional(); } @@ -965,13 +965,13 @@ namespace approaching_emergency_vehicle_plugin if(current_downtrack > ego_route_lanelet_centerline_end_downtrack){ // If current downtrack is greater than lanelet ending downtrack, remove lanelet from future route lanelet and continue // NOTE: Iterator is not updated since an element is being erased - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Removing passed lanelet " << ego_route_lanelet_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Removing passed lanelet " << ego_route_lanelet_id); future_route_lanelet_ids_.erase(it); } else{ // If lanelet exists in both, return it as the intersecting lanelet if(erv_future_route.contains(ego_route_lanelet)){ - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Found intersecting lanelet " << ego_route_lanelet_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Found intersecting lanelet " << ego_route_lanelet_id); return boost::optional(ego_route_lanelet); } else{ @@ -1001,14 +1001,14 @@ namespace approaching_emergency_vehicle_plugin } void ApproachingEmergencyVehiclePlugin::routeCallback(carma_planning_msgs::msg::Route::UniquePtr msg){ - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Received route callback"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Received route callback"); std::vector new_future_route_lanelet_ids; for(size_t i = 0; i < msg->route_path_lanelet_ids.size(); ++i){ - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "New route lanelet: " << msg->route_path_lanelet_ids[i]); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "New route lanelet: " << msg->route_path_lanelet_ids[i]); new_future_route_lanelet_ids.push_back(msg->route_path_lanelet_ids[i]); } - + future_route_lanelet_ids_ = new_future_route_lanelet_ids; } @@ -1025,7 +1025,7 @@ namespace approaching_emergency_vehicle_plugin { throw std::invalid_argument("No speed limit could be found since valid traffic rules object could not be built"); } - + return speed_limit; } @@ -1045,12 +1045,12 @@ namespace approaching_emergency_vehicle_plugin RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name),"maneuver_end_dist: " << maneuver_end_dist << ", maneuver_start_dist: " << maneuver_start_dist << ", sum_start_and_end_speed: " << sum_start_and_end_speed); - maneuver_duration = rclcpp::Duration((maneuver_end_dist - maneuver_start_dist) / (0.5 * sum_start_and_end_speed) * 1e9); + maneuver_duration = rclcpp::Duration::from_nanoseconds((maneuver_end_dist - maneuver_start_dist) / (0.5 * sum_start_and_end_speed) * 1e9); return maneuver_duration; } - carma_planning_msgs::msg::Maneuver ApproachingEmergencyVehiclePlugin::composeLaneFollowingManeuverMessage(double start_dist, double end_dist, + carma_planning_msgs::msg::Maneuver ApproachingEmergencyVehiclePlugin::composeLaneFollowingManeuverMessage(double start_dist, double end_dist, double start_speed, double target_speed, int lanelet_id, rclcpp::Time& start_time) const { carma_planning_msgs::msg::Maneuver maneuver_msg; @@ -1077,7 +1077,7 @@ namespace approaching_emergency_vehicle_plugin return maneuver_msg; } - carma_planning_msgs::msg::Maneuver ApproachingEmergencyVehiclePlugin::composeLaneChangeManeuverMessage(double start_dist, double end_dist, + carma_planning_msgs::msg::Maneuver ApproachingEmergencyVehiclePlugin::composeLaneChangeManeuverMessage(double start_dist, double end_dist, double start_speed, double target_speed, lanelet::Id starting_lane_id, lanelet::Id ending_lane_id, rclcpp::Time& start_time) const { carma_planning_msgs::msg::Maneuver maneuver_msg; @@ -1113,7 +1113,7 @@ namespace approaching_emergency_vehicle_plugin return maneuver_msg; } - carma_planning_msgs::msg::Maneuver ApproachingEmergencyVehiclePlugin::composeStopAndWaitManeuverMessage(double start_dist, double end_dist, double start_speed, + carma_planning_msgs::msg::Maneuver ApproachingEmergencyVehiclePlugin::composeStopAndWaitManeuverMessage(double start_dist, double end_dist, double start_speed, lanelet::Id starting_lane_id, lanelet::Id ending_lane_id, double stopping_deceleration, rclcpp::Time& start_time) const { carma_planning_msgs::msg::Maneuver maneuver_msg; @@ -1147,8 +1147,8 @@ namespace approaching_emergency_vehicle_plugin return maneuver_msg; } - void ApproachingEmergencyVehiclePlugin::addStopAndWaitToEndOfPlan(carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, - double downtrack_progress, double stop_maneuver_beginning_downtrack, double end_of_route_downtrack, + void ApproachingEmergencyVehiclePlugin::addStopAndWaitToEndOfPlan(carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp, + double downtrack_progress, double stop_maneuver_beginning_downtrack, double end_of_route_downtrack, double stopping_entry_speed, double stopping_deceleration, double current_lanelet_ending_downtrack, lanelet::ConstLanelet current_lanelet, rclcpp::Time time_progress) { @@ -1157,9 +1157,9 @@ namespace approaching_emergency_vehicle_plugin // Add a lane follow maneuver before the stop and wait maneuver if the distance before stop_maneuver_beginning_downtrack is too large if((stop_maneuver_beginning_downtrack - downtrack_progress) >= config_.buffer_distance_before_stopping){ // Compose lane follow maneuver and update downtrack_progress - resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, stop_maneuver_beginning_downtrack, + resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, stop_maneuver_beginning_downtrack, stopping_entry_speed, stopping_entry_speed, current_lanelet.id(), time_progress)); - + downtrack_progress = stop_maneuver_beginning_downtrack; time_progress += getManeuverDuration(resp->new_plan.maneuvers.back(), epsilon_); } @@ -1177,7 +1177,7 @@ namespace approaching_emergency_vehicle_plugin RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "A following lanelet in the current lane could not be found; returning empty plan"); resp->new_plan.maneuvers = {}; return; - } + } // Check whether the next lanelet is on the route if(!wm_->getRoute()->contains(current_lanelet)){ @@ -1186,7 +1186,7 @@ namespace approaching_emergency_vehicle_plugin return; } - // Break from loop if lanelet's ending downtrack is after the route end downtrack; this is the maneuver's ending lanelet + // Break from loop if lanelet's ending downtrack is after the route end downtrack; this is the maneuver's ending lanelet current_lanelet_ending_downtrack = wm_->routeTrackPos(current_lanelet.centerline2d().back()).downtrack; if(current_lanelet_ending_downtrack >= end_of_route_downtrack){ break; @@ -1230,7 +1230,7 @@ namespace approaching_emergency_vehicle_plugin double stopping_deceleration = config_.vehicle_acceleration_limit * config_.stopping_accel_limit_multiplier; // Compute stopping distance where v_f = 0 - // (v_f^2 - v_i^2) / (2*a) = d + // (v_f^2 - v_i^2) / (2*a) = d double stopping_distance = (target_speed * target_speed) / (2.0 * stopping_deceleration); double begin_stopping_downtrack = wm_->getRouteEndTrackPos().downtrack - stopping_distance; RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Stop and wait maneuver must begin by downtrack " << begin_stopping_downtrack); @@ -1240,13 +1240,13 @@ namespace approaching_emergency_vehicle_plugin if(begin_stopping_downtrack <= current_lanelet_ending_downtrack){ // Complete maneuver plan with a stop and wait maneuver if the current lanelet intercepts the stopping downtrack - addStopAndWaitToEndOfPlan(resp, downtrack_progress, begin_stopping_downtrack, wm_->getRouteEndTrackPos().downtrack, + addStopAndWaitToEndOfPlan(resp, downtrack_progress, begin_stopping_downtrack, wm_->getRouteEndTrackPos().downtrack, speed_progress, stopping_deceleration, current_lanelet_ending_downtrack, current_lanelet, time_progress); return; } else{ // Compose lane following maneuver and add it to the response's maneuver plan - resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack, + resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack, speed_progress, target_speed, current_lanelet.id(), time_progress)); // Get the next lanelet in the current lane if it exists @@ -1310,7 +1310,7 @@ namespace approaching_emergency_vehicle_plugin double stopping_deceleration = config_.vehicle_acceleration_limit * config_.stopping_accel_limit_multiplier; // Compute stopping distance where v_f = 0 - // (v_f^2 - v_i^2) / (2*a) = d + // (v_f^2 - v_i^2) / (2*a) = d double stopping_distance = (target_speed * target_speed) / (2.0 * stopping_deceleration); double begin_stopping_downtrack = wm_->getRouteEndTrackPos().downtrack - stopping_distance; RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Stop and wait maneuver must begin by downtrack " << begin_stopping_downtrack); @@ -1320,23 +1320,23 @@ namespace approaching_emergency_vehicle_plugin while(downtrack_progress < maneuver_plan_ending_downtrack){ if(begin_stopping_downtrack <= current_lanelet_ending_downtrack){ // Complete maneuver plan with a stop and wait maneuver if the current lanelet intercepts the stopping downtrack - addStopAndWaitToEndOfPlan(resp, downtrack_progress, begin_stopping_downtrack, wm_->getRouteEndTrackPos().downtrack, + addStopAndWaitToEndOfPlan(resp, downtrack_progress, begin_stopping_downtrack, wm_->getRouteEndTrackPos().downtrack, speed_progress, stopping_deceleration, current_lanelet_ending_downtrack, current_lanelet, time_progress); return; } else{ // Plan lane change maneuver if current downtrack progress is between the starting and ending downtracks of the planned upcoming lane change, otherwise plan lane follow maneuver if(((upcoming_lc_params_.start_dist - epsilon_) <= downtrack_progress) && (downtrack_progress < (upcoming_lc_params_.end_dist - epsilon_))){ - resp->new_plan.maneuvers.push_back(composeLaneChangeManeuverMessage(upcoming_lc_params_.start_dist, upcoming_lc_params_.end_dist, + resp->new_plan.maneuvers.push_back(composeLaneChangeManeuverMessage(upcoming_lc_params_.start_dist, upcoming_lc_params_.end_dist, upcoming_lc_params_.start_speed, upcoming_lc_params_.end_speed, upcoming_lc_params_.starting_lanelet.id(), upcoming_lc_params_.ending_lanelet.id(), time_progress)); - + current_lanelet = upcoming_lc_params_.ending_lanelet; } else{ - resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack, + resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack, speed_progress, target_speed, current_lanelet.id(), time_progress)); - } - + } + // Get the next lanelet in the current lane if it exists if(!wm_->getMapRoutingGraph()->following(current_lanelet, false).empty()) { @@ -1375,14 +1375,14 @@ namespace approaching_emergency_vehicle_plugin if(begin_stopping_downtrack <= current_lanelet_ending_downtrack){ // Complete maneuver plan with a stop and wait maneuver if the current lanelet intercepts the stopping downtrack - addStopAndWaitToEndOfPlan(resp, downtrack_progress, begin_stopping_downtrack, wm_->getRouteEndTrackPos().downtrack, + addStopAndWaitToEndOfPlan(resp, downtrack_progress, begin_stopping_downtrack, wm_->getRouteEndTrackPos().downtrack, speed_progress, stopping_deceleration, current_lanelet_ending_downtrack, current_lanelet, time_progress); return; } else{ // First maneuver(s) are lane-following to enable sufficient time to activate turn signals before conducting lane change if(!completed_initial_lane_following){ - resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack, + resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack, speed_progress, target_speed, current_lanelet.id(), time_progress)); initial_lane_following_duration += getManeuverDuration(resp->new_plan.maneuvers.back(), epsilon_).seconds(); @@ -1404,12 +1404,12 @@ namespace approaching_emergency_vehicle_plugin if(ego_lane_index != erv_lane_index){ RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "Planning a move-over maneuver plan when the ego vehicle is not in the ERV's path! Returning an empty maneuver plan"); resp->new_plan.maneuvers = {}; - return; + return; } else if((ego_lane_index == 0) && (erv_lane_index == 0)){ // Ego vehicle and ERV are both in the rightmost lane so a left lane change is required new_lc_params.is_right_lane_change = false; - target_lanelet = wm_->getMapRoutingGraph()->left(current_lanelet); + target_lanelet = wm_->getMapRoutingGraph()->left(current_lanelet); RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Composing a left lane change maneuver from lanelet " << current_lanelet.id()); } else{ @@ -1421,11 +1421,11 @@ namespace approaching_emergency_vehicle_plugin // Only compose lane change maneuver if the target lanelet exists, otherwise compose lane follow maneuver if(target_lanelet){ - resp->new_plan.maneuvers.push_back(composeLaneChangeManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack, + resp->new_plan.maneuvers.push_back(composeLaneChangeManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack, speed_progress, target_speed, current_lanelet.id(), target_lanelet.get().id(), time_progress)); - + has_planned_upcoming_lc_ = true; - + new_lc_params.starting_lanelet = current_lanelet; new_lc_params.ending_lanelet = target_lanelet.get(); new_lc_params.start_dist = downtrack_progress; @@ -1439,12 +1439,12 @@ namespace approaching_emergency_vehicle_plugin } else{ RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "A lane change maneuver from " << current_lanelet.id() << " could not be composed since no target lanelet was found!"); - resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack, + resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack, speed_progress, target_speed, current_lanelet.id(), time_progress)); } } else{ - resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack, + resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(downtrack_progress, current_lanelet_ending_downtrack, speed_progress, target_speed, current_lanelet.id(), time_progress)); } } @@ -1477,7 +1477,7 @@ namespace approaching_emergency_vehicle_plugin RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Next maneuver starting downtrack is " << downtrack_progress << ", end of plan is at " << maneuver_plan_ending_downtrack); } - } + } } } @@ -1486,7 +1486,7 @@ namespace approaching_emergency_vehicle_plugin // Get lanelet(s) that the provided map coordinates are contained within std::vector ego_route_lanelets = wm_->getLaneletsFromPoint({x_position, y_position}); boost::optional ego_route_lanelet; - + if (ego_route_lanelets.empty()) { RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name), "Given vehicle position is not on the road."); @@ -1506,8 +1506,8 @@ namespace approaching_emergency_vehicle_plugin } void ApproachingEmergencyVehiclePlugin::plan_maneuvers_callback( - std::shared_ptr, - carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, + std::shared_ptr, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Received request to plan maneuvers"); @@ -1553,7 +1553,7 @@ namespace approaching_emergency_vehicle_plugin } else if((0.0 <= tracked_erv_.seconds_until_passing) && (tracked_erv_.seconds_until_passing < config_.passing_threshold)){ transition_table_.event(ApproachingEmergencyVehicleEvent::ERV_PASSING_IN_PATH); - + if(tracked_erv_.seconds_until_passing >= MAINTAIN_SPEED_THRESHOLD){ is_maintaining_non_reduced_speed_ = false; } @@ -1600,7 +1600,7 @@ namespace approaching_emergency_vehicle_plugin double downtrack_progress = req->veh_downtrack; double current_lanelet_ending_downtrack = wm_->routeTrackPos(ego_current_lanelet_optional.get().centerline2d().back()).downtrack; double speed_progress = req->veh_logitudinal_velocity; - double target_speed = getLaneletSpeedLimit(ego_current_lanelet_optional.get()); + double target_speed = getLaneletSpeedLimit(ego_current_lanelet_optional.get()); rclcpp::Time time_progress = rclcpp::Time(req->header.stamp); // Generate maneuver plan based on the current state diff --git a/arbitrator/include/arbitrator.hpp b/arbitrator/include/arbitrator.hpp index 74a1cbf51c..69860efd25 100644 --- a/arbitrator/include/arbitrator.hpp +++ b/arbitrator/include/arbitrator.hpp @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include "vehicle_state.hpp" #include "arbitrator_state_machine.hpp" diff --git a/arbitrator/src/arbitrator_node.cpp b/arbitrator/src/arbitrator_node.cpp index 7b9894cfc0..f5c5a5cbe2 100644 --- a/arbitrator/src/arbitrator_node.cpp +++ b/arbitrator/src/arbitrator_node.cpp @@ -67,7 +67,7 @@ namespace arbitrator auto bss = std::make_shared(config_.beam_width); auto png = std::make_shared>(ci); - arbitrator::TreePlanner tp(cf, png, bss, rclcpp::Duration(config_.target_plan_duration* 1e9)); + arbitrator::TreePlanner tp(cf, png, bss, rclcpp::Duration::from_nanoseconds(config_.target_plan_duration* 1e9)); wm_listener_ = std::make_shared( this->get_node_base_interface(), this->get_node_logging_interface(), @@ -81,7 +81,7 @@ namespace arbitrator std::make_shared(sm), ci, std::make_shared(tp), - rclcpp::Duration(config_.min_plan_duration* 1e9), + rclcpp::Duration::from_nanoseconds(config_.min_plan_duration* 1e9), 1/config_.planning_frequency, wm_ ); diff --git a/arbitrator/src/tree_planner.cpp b/arbitrator/src/tree_planner.cpp index 2598d8d3c5..66fda96c6b 100644 --- a/arbitrator/src/tree_planner.cpp +++ b/arbitrator/src/tree_planner.cpp @@ -32,7 +32,7 @@ namespace arbitrator open_list_to_evaluate.push_back(std::make_pair(root, INF)); carma_planning_msgs::msg::ManeuverPlan longest_plan = root; // Track longest plan in case target length is never reached - rclcpp::Duration longest_plan_duration = rclcpp::Duration(0); + rclcpp::Duration longest_plan_duration = rclcpp::Duration::from_nanoseconds(0); while (!open_list_to_evaluate.empty()) diff --git a/basic_autonomy/src/basic_autonomy.cpp b/basic_autonomy/src/basic_autonomy.cpp index b720deb51b..fb160185bb 100644 --- a/basic_autonomy/src/basic_autonomy.cpp +++ b/basic_autonomy/src/basic_autonomy.cpp @@ -1283,7 +1283,7 @@ namespace basic_autonomy autoware_point.heading.real = std::cos(yaw/2); autoware_point.heading.imag = std::sin(yaw/2); - autoware_point.time_from_start = rclcpp::Duration(times[i] * 1e9); + autoware_point.time_from_start = rclcpp::Duration::from_nanoseconds(times[i] * 1e9); RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Setting waypoint idx: " << i <<", with planner: << " << trajectory_points[i].planner_plugin_name << ", x: " << trajectory_points[i].x << ", y: " << trajectory_points[i].y << ", speed: " << lag_speeds[i]* 2.23694 << "mph"); diff --git a/carma_cooperative_perception/src/msg_conversion.cpp b/carma_cooperative_perception/src/msg_conversion.cpp index 475a18a634..905782302c 100644 --- a/carma_cooperative_perception/src/msg_conversion.cpp +++ b/carma_cooperative_perception/src/msg_conversion.cpp @@ -14,7 +14,7 @@ #include "carma_cooperative_perception/msg_conversion.hpp" -#include +#include #include #include #include diff --git a/carma_cooperative_perception/src/multiple_object_tracker_component.cpp b/carma_cooperative_perception/src/multiple_object_tracker_component.cpp index fe49e86941..ccccb999bf 100644 --- a/carma_cooperative_perception/src/multiple_object_tracker_component.cpp +++ b/carma_cooperative_perception/src/multiple_object_tracker_component.cpp @@ -14,7 +14,7 @@ #include "carma_cooperative_perception/multiple_object_tracker_component.hpp" -#include +#include #include #include #include diff --git a/carma_wm_ctrl/src/GeofenceScheduler.cpp b/carma_wm_ctrl/src/GeofenceScheduler.cpp index d754dd10ef..33bac32d75 100644 --- a/carma_wm_ctrl/src/GeofenceScheduler.cpp +++ b/carma_wm_ctrl/src/GeofenceScheduler.cpp @@ -25,7 +25,7 @@ GeofenceScheduler::GeofenceScheduler(std::shared_ptr timerFactory) { // Create repeating loop to clear geofence timers which are no longer needed deletion_timer_ = - timerFactory_->buildTimer(nextId(), rclcpp::Duration(1), std::bind(&GeofenceScheduler::clearTimers, this)); + timerFactory_->buildTimer(nextId(), rclcpp::Duration::from_nanoseconds(1), std::bind(&GeofenceScheduler::clearTimers, this)); clock_type_ = timerFactory_->now().get_clock_type(); } @@ -68,7 +68,7 @@ void GeofenceScheduler::clearTimers() } void GeofenceScheduler::addGeofence(std::shared_ptr gf_ptr) -{ +{ std::lock_guard guard(mutex_); RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Attempting to add Geofence with Id: " << gf_ptr->id_); @@ -82,7 +82,7 @@ void GeofenceScheduler::addGeofence(std::shared_ptr gf_ptr) if (!interval_info.first && startTime == rclcpp::Time(0, 0, clock_type_)) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to add geofence as its schedule did not contain an active or upcoming control period. GF Id: " << gf_ptr->id_); return; @@ -95,7 +95,7 @@ void GeofenceScheduler::addGeofence(std::shared_ptr gf_ptr) int32_t timer_id = nextId(); - rclcpp::Duration control_duration = rclcpp::Duration(std::max((startTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value + rclcpp::Duration control_duration = rclcpp::Duration::from_nanoseconds(std::max((startTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value // Build timer to trigger when this geofence becomes active TimerPtr timer = timerFactory_->buildTimer( @@ -119,7 +119,7 @@ void GeofenceScheduler::startGeofenceCallback(std::shared_ptr gf_ptr, // Build timer to trigger when this geofence becomes inactive int32_t ending_timer_id = nextId(); - rclcpp::Duration control_duration = rclcpp::Duration(std::max((endTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value + rclcpp::Duration control_duration = rclcpp::Duration::from_nanoseconds(std::max((endTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value TimerPtr timer = timerFactory_->buildTimer( ending_timer_id, control_duration, @@ -157,7 +157,7 @@ void GeofenceScheduler::endGeofenceCallback(std::shared_ptr gf_ptr, co // Build timer to trigger when this geofence becomes active int32_t start_timer_id = nextId(); - rclcpp::Duration control_duration = rclcpp::Duration(std::max((startTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value + rclcpp::Duration control_duration = rclcpp::Duration::from_nanoseconds(std::max((startTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value TimerPtr timer = timerFactory_->buildTimer( start_timer_id, control_duration, @@ -177,4 +177,4 @@ void GeofenceScheduler::onGeofenceInactive(std::function guard(mutex_); inactive_callback_ = inactive_callback; } -} // namespace carma_wm_ctrl \ No newline at end of file +} // namespace carma_wm_ctrl diff --git a/carma_wm_ctrl/src/WMBroadcaster.cpp b/carma_wm_ctrl/src/WMBroadcaster.cpp index d3c767a3bd..b4b7b3dcd9 100644 --- a/carma_wm_ctrl/src/WMBroadcaster.cpp +++ b/carma_wm_ctrl/src/WMBroadcaster.cpp @@ -91,7 +91,7 @@ void WMBroadcaster::baseMapCallback(autoware_lanelet2_msgs::msg::MapBin::UniqueP // Publish map current_map_version_ += 1; // Increment the map version. It should always start from 1 for the first map - + autoware_lanelet2_msgs::msg::MapBin compliant_map_msg; // Populate the routing graph message @@ -120,14 +120,14 @@ void WMBroadcaster::addScheduleFromMsg(std::shared_ptr gf_ptr, const c carma_v2x_msgs::msg::TrafficControlSchedule msg_schedule = msg_v01.params.schedule; double ns_to_sec = 1.0e9; auto clock_type = scheduler_.getClockType(); - + rclcpp::Time end_time = {msg_schedule.end, clock_type}; if (!msg_schedule.end_exists) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "No end time for geofence, using rclcpp::Time::max()"); end_time = {rclcpp::Time::max(), clock_type}; // If there is no end time use the max time } - // If the days of the week are specified then convert them to the boost format. + // If the days of the week are specified then convert them to the boost format. GeofenceSchedule::DayOfTheWeekSet week_day_set = { 0, 1, 2, 3, 4, 5, 6 }; // Default to all days 0==Sun to 6==Sat if (msg_schedule.dow_exists) { // sun (6), mon (5), tue (4), wed (3), thu (2), fri (1), sat (0) @@ -168,48 +168,48 @@ void WMBroadcaster::addScheduleFromMsg(std::shared_ptr gf_ptr, const c for (auto daily_schedule : msg_schedule.between) { if (msg_schedule.repeat_exists) { - gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type}, + gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type}, end_time, - rclcpp::Duration(daily_schedule.begin), + rclcpp::Duration(daily_schedule.begin), rclcpp::Duration(daily_schedule.duration), rclcpp::Duration(msg_schedule.repeat.offset), - rclcpp::Duration(msg_schedule.repeat.span), + rclcpp::Duration(msg_schedule.repeat.span), rclcpp::Duration(msg_schedule.repeat.period), week_day_set)); } else { - gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type}, + gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type}, end_time, - rclcpp::Duration(daily_schedule.begin), + rclcpp::Duration(daily_schedule.begin), rclcpp::Duration(daily_schedule.duration), rclcpp::Duration(0.0), // No offset rclcpp::Duration(daily_schedule.duration) - rclcpp::Duration(daily_schedule.begin), // Compute schedule portion end time rclcpp::Duration(daily_schedule.duration) - rclcpp::Duration(daily_schedule.begin), // No repetition so same as portion end time - week_day_set)); + week_day_set)); } } } else { if (msg_schedule.repeat_exists) { - gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type}, + gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type}, end_time, - rclcpp::Duration(0.0), - rclcpp::Duration(86400.0e9), // 24 hr daily application + rclcpp::Duration::from_nanoseconds(0.0), + rclcpp::Duration::from_nanoseconds(86400.0e9), // 24 hr daily application rclcpp::Duration(msg_schedule.repeat.offset), - rclcpp::Duration(msg_schedule.repeat.span), + rclcpp::Duration(msg_schedule.repeat.span), rclcpp::Duration(msg_schedule.repeat.period), - week_day_set)); + week_day_set)); } else { - gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type}, + gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type}, end_time, - rclcpp::Duration(0.0), - rclcpp::Duration(86400.0e9), // 24 hr daily application - rclcpp::Duration(0.0), // No offset - rclcpp::Duration(86400.0e9), // Applied for full lenth of 24 hrs - rclcpp::Duration(86400.0e9), // No repetition - week_day_set)); + rclcpp::Duration::from_nanoseconds(0.0), + rclcpp::Duration::from_nanoseconds(86400.0e9), // 24 hr daily application + rclcpp::Duration::from_nanoseconds(0.0), // No offset + rclcpp::Duration::from_nanoseconds(86400.0e9), // Applied for full lenth of 24 hrs + rclcpp::Duration::from_nanoseconds(86400.0e9), // No repetition + week_day_set)); } - + } } @@ -255,7 +255,7 @@ std::vector> WMBroadcaster::geofenceFromMapMsg(std::sh } updates_to_send.push_back(update); } - + return updates_to_send; } @@ -266,7 +266,7 @@ void WMBroadcaster::geofenceFromMsg(std::shared_ptr gf_ptr, const carm // Get ID std::copy(msg_v01.id.id.begin(), msg_v01.id.id.end(), gf_ptr->id_.begin()); - + gf_ptr->gf_pts = getPointsInLocalFrame(msg_v01); gf_ptr->affected_parts_ = getAffectedLaneletOrAreas(gf_ptr->gf_pts); @@ -291,12 +291,12 @@ void WMBroadcaster::geofenceFromMsg(std::shared_ptr gf_ptr, const carm // currently only converting portion of control message that is relevant to: // - digital speed limit, passing control line, digital minimum gap, region access rule, and series of workzone related messages lanelet::Velocity sL; - - - if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE) - { + + + if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE) + { //Acquire speed limit information from TafficControlDetail msg - sL = lanelet::Velocity(msg_detail.maxspeed * lanelet::units::MPS()); + sL = lanelet::Velocity(msg_detail.maxspeed * lanelet::units::MPS()); std::string reason = ""; if (msg_v01.package.label_exists) reason = msg_v01.package.label; @@ -316,11 +316,11 @@ void WMBroadcaster::geofenceFromMsg(std::shared_ptr gf_ptr, const carm RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital speed limit is invalid. Value set to 0mph."); sL = 0_mph; }// @SONAR_START@ - - gf_ptr->regulatory_element_ = std::make_shared(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(), + + gf_ptr->regulatory_element_ = std::make_shared(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(), sL, affected_llts, affected_areas, participantsChecker(msg_v01), reason)); } - if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINSPEED_CHOICE) + if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINSPEED_CHOICE) { //Acquire speed limit information from TafficControlDetail msg sL = lanelet::Velocity(msg_detail.minspeed * lanelet::units::MPS()); @@ -331,7 +331,7 @@ void WMBroadcaster::geofenceFromMsg(std::shared_ptr gf_ptr, const carm if (msg_v01.package.label_exists) reason = msg_v01.package.label; - //Ensure Geofences do not provide invalid speed limit data + //Ensure Geofences do not provide invalid speed limit data // @SONAR_STOP@ if(sL > 80_mph ) { @@ -343,14 +343,14 @@ void WMBroadcaster::geofenceFromMsg(std::shared_ptr gf_ptr, const carm RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital speed limit is invalid. Value set to 0mph."); sL = 0_mph; }// @SONAR_START@ - gf_ptr->regulatory_element_ = std::make_shared(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(), + gf_ptr->regulatory_element_ = std::make_shared(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(), sL, affected_llts, affected_areas, participantsChecker(msg_v01), reason)); } if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::LATPERM_CHOICE || msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::LATAFFINITY_CHOICE) { addPassingControlLineFromMsg(gf_ptr, msg_v01, affected_llts); } - if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINHDWY_CHOICE) + if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINHDWY_CHOICE) { double min_gap = (double)msg_detail.minhdwy; @@ -394,7 +394,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeofence(std::unordered_m { std::shared_ptr> parallel_llts = std::make_shared>(std::vector()); std::shared_ptr> middle_opposite_lanelets = std::make_shared>(std::vector()); - + // Split existing lanelets and filter into parallel_llts and middle_opposite_lanelets preprocessWorkzoneGeometry(work_zone_geofence_cache, parallel_llts, middle_opposite_lanelets); @@ -403,10 +403,10 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeofence(std::unordered_m // copy static info from the existing workzone gf_ptr->id_ = work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->id_; //using taperright's id as the whole geofence's id - + // schedule gf_ptr->schedules = work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->schedules; //using taperright's schedule as the whole geofence's schedule - + // erase cache now that it is processed for (auto pair : work_zone_geofence_cache) { @@ -417,7 +417,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeofence(std::unordered_m return gf_ptr; } -std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_map> work_zone_geofence_cache, lanelet::Lanelet parallel_llt_front, lanelet::Lanelet parallel_llt_back, +std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_map> work_zone_geofence_cache, lanelet::Lanelet parallel_llt_front, lanelet::Lanelet parallel_llt_back, std::shared_ptr> middle_opposite_lanelets) { auto gf_ptr = std::make_shared(); @@ -426,7 +426,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m //FRONT DIAGONAL LANELET ////////////////////////////// - lanelet::Lanelet front_llt_diag = createLinearInterpolatingLanelet(parallel_llt_front.leftBound3d().back(), parallel_llt_front.rightBound3d().back(), + lanelet::Lanelet front_llt_diag = createLinearInterpolatingLanelet(parallel_llt_front.leftBound3d().back(), parallel_llt_front.rightBound3d().back(), middle_opposite_lanelets->back().rightBound3d().back(), middle_opposite_lanelets->back().leftBound3d().back()); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created diag front_llt_diag id:" << front_llt_diag.id()); for (auto regem : middle_opposite_lanelets->back().regulatoryElements()) //copy existing regem into the new llts @@ -438,7 +438,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m //BACK DIAGONAL LANELET ////////////////////////////// - lanelet::Lanelet back_llt_diag = createLinearInterpolatingLanelet(middle_opposite_lanelets->front().rightBound3d().front(), middle_opposite_lanelets->front().leftBound3d().front(), + lanelet::Lanelet back_llt_diag = createLinearInterpolatingLanelet(middle_opposite_lanelets->front().rightBound3d().front(), middle_opposite_lanelets->front().leftBound3d().front(), parallel_llt_back.leftBound3d().front(), parallel_llt_back.rightBound3d().front()); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created back_llt_diag diag id:" << back_llt_diag.id()); for (auto regem : parallel_llt_back.regulatoryElements()) //copy existing regem into the new llts @@ -477,7 +477,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m controlled_taper_right.push_back(back_llt_diag); - lanelet::CarmaTrafficSignalPtr tfl_parallel = std::make_shared(lanelet::CarmaTrafficSignal::buildData(lanelet::utils::getId(), {parallel_stop_ls}, {controlled_taper_right.front()}, {controlled_taper_right.back()})); + lanelet::CarmaTrafficSignalPtr tfl_parallel = std::make_shared(lanelet::CarmaTrafficSignal::buildData(lanelet::utils::getId(), {parallel_stop_ls}, {controlled_taper_right.front()}, {controlled_taper_right.back()})); gf_ptr->traffic_light_id_lookup_.push_back({generate32BitId(work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->label_),tfl_parallel->id()}); @@ -488,30 +488,30 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m //ADD TF_LIGHT TO OPPOSITE LANELET ////////////////////////////// std::shared_ptr> opposite_llts_with_stop_line = std::make_shared>(std::vector()); - - auto old_opposite_llts = splitOppositeLaneletWithPoint(opposite_llts_with_stop_line, work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->gf_pts.back().basicPoint2d(), + + auto old_opposite_llts = splitOppositeLaneletWithPoint(opposite_llts_with_stop_line, work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->gf_pts.back().basicPoint2d(), current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get().id()), error_distance_); - + lanelet::LineString3d opposite_stop_ls(lanelet::utils::getId(), {opposite_llts_with_stop_line->front().leftBound3d().back(), opposite_llts_with_stop_line->front().rightBound3d().back()}); - + std::vector controlled_open_right; - + controlled_open_right.insert(controlled_open_right.end(), opposite_llts_with_stop_line->begin(), opposite_llts_with_stop_line->end());; //split lanelet one of which has light - + for (auto llt : work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_) { controlled_open_right.push_back(current_map_->laneletLayer.get(llt.lanelet().get().id())); } lanelet::CarmaTrafficSignalPtr tfl_opposite = std::make_shared(lanelet::CarmaTrafficSignal::buildData(lanelet::utils::getId(), {opposite_stop_ls}, {controlled_open_right.front()}, {controlled_open_right.back()})); - + gf_ptr->traffic_light_id_lookup_.push_back({generate32BitId(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->label_), tfl_opposite->id()}); - + opposite_llts_with_stop_line->front().addRegulatoryElement(tfl_opposite); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created TF_LIGHT of Id: " << tfl_opposite->id() << ", to opposite_llts_with_stop_line->front() id:" << opposite_llts_with_stop_line->front().id()); ////////////////////////////// - //ADD ALL NEWLY CREATED LANELETS INTO GEOFENCE + //ADD ALL NEWLY CREATED LANELETS INTO GEOFENCE //OBJECTS TO BE PROCESSED LATER BY SCHEDULER ////////////////////////////// RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Added parallel_llt_front id:" << parallel_llt_front.id()); @@ -528,15 +528,15 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m gf_ptr->lanelet_additions_.insert(gf_ptr->lanelet_additions_.end(), opposite_llts_with_stop_line->begin(), opposite_llts_with_stop_line->end());; ////////////////////////////// - // ADD REGION_ACCESS_RULE REGEM TO THE OUTDATED LANELETS + // ADD REGION_ACCESS_RULE REGEM TO THE OUTDATED LANELETS // AS WELL AS LANELETS BEING BLOCKED BY WORKZONE GEOFENCE ////////////////////////////// - + // fill information for paricipants and reason for blocking carma_v2x_msgs::msg::TrafficControlMessageV01 participants_and_reason_only; j2735_v2x_msgs::msg::TrafficControlVehClass participant; // sending all possible VEHICLE will be processed as they are not accessuble by regionAccessRule - + participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::MICROMOBILE; participants_and_reason_only.params.vclasses.push_back(participant); @@ -556,7 +556,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m participants_and_reason_only.package.label = "SIG_WZ"; std::vector old_or_blocked_llts; // this is needed to addRegionAccessRule input signatures - + // from all affected parts, remove duplicate entries for (auto llt : work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_) { @@ -582,13 +582,13 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m old_or_blocked_llts.push_back(current_map_->laneletLayer.get(llt.lanelet()->id())); } } - + gf_ptr->affected_parts_.push_back(old_opposite_llts[0]); // block old lanelet in the opposing lanelet that will be replaced with split lanelets that have trafficlight old_or_blocked_llts.push_back(old_opposite_llts[0]); // actual regulatory element adder - addRegionAccessRule(gf_ptr, participants_and_reason_only, old_or_blocked_llts); - + addRegionAccessRule(gf_ptr, participants_and_reason_only, old_or_blocked_llts); + return gf_ptr; } @@ -647,7 +647,7 @@ void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_mapfollowing(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get()); @@ -657,7 +657,7 @@ void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_maplaneletLayer.get(next_lanelets.front().id()); @@ -679,10 +679,10 @@ void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_map temp_llts; - temp_llts = splitLaneletWithPoint({work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d(), + temp_llts = splitLaneletWithPoint({work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d(), work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d()}, current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.front().lanelet().get().id()), error_distance_); - + if (temp_llts.size() < 2) // if there is only 1 lanelet { // we found what we want, so return @@ -690,7 +690,7 @@ void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_mapsize() << ", and parallel_llts.size()" << parallel_llts->size()); return; } - else if (temp_llts.size() == 2) // determine which + else if (temp_llts.size() == 2) // determine which { // back gap is bigger than front's, we should lose front lanelet from the two if (lanelet::geometry::distance2d(work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d(), reverse_front_llt.centerline2d().back().basicPoint2d()) > @@ -711,7 +711,7 @@ void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_mapgf_pts.front().basicPoint2d(); auto reverse_first_llt = current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.front().lanelet().get().id()); std::vector temp_opposite_front_llts; @@ -733,7 +733,7 @@ void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_mapgf_pts.back().basicPoint2d(); auto reverse_last_llt = current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.back().lanelet().get().id()); std::vector temp_opposite_back_llts; @@ -765,7 +765,7 @@ std::vector WMBroadcaster::splitLaneletWithPoint(const std::ve double point_downtrack_ratio = point_downtrack / llt_downtrack; ratios.push_back(point_downtrack_ratio); } - + auto new_parallel_llts = splitLaneletWithRatio(ratios, input_llt, error_distance); parallel_llts.insert(parallel_llts.end(),new_parallel_llts.begin(), new_parallel_llts.end()); @@ -778,10 +778,10 @@ lanelet::Lanelets WMBroadcaster::splitOppositeLaneletWithPoint(std::shared_ptr WMBroadcaster::splitLaneletWithRatio(std::vector WMBroadcaster::splitLaneletWithRatio(std::vector left_pts; left_pts.insert(left_pts.end(), current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().begin() + left_prev_pt_idx, current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().begin() + left_next_pt_idx + 1); - lanelet::LineString3d left_ls(lanelet::utils::getId(), left_pts, current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().attributes()); + lanelet::LineString3d left_ls(lanelet::utils::getId(), left_pts, current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().attributes()); std::vector right_pts; right_pts.insert(right_pts.end(), current_map_->laneletLayer.get(input_lanelet.id()).rightBound3d().begin() + right_prev_pt_idx, current_map_->laneletLayer.get(input_lanelet.id()).rightBound3d().begin() + right_next_pt_idx + 1); - - lanelet::LineString3d right_ls(lanelet::utils::getId(), right_pts); + + lanelet::LineString3d right_ls(lanelet::utils::getId(), right_pts); lanelet::Lanelet new_llt (lanelet::utils::getId(), left_ls, right_ls, current_map_->laneletLayer.get(input_lanelet.id()).rightBound3d().attributes()); - + for (auto regem : current_map_->laneletLayer.get(input_lanelet.id()).regulatoryElements()) //copy existing regem into new llts { new_llt.addRegulatoryElement(current_map_->regulatoryElementLayer.get(regem->id())); } - + left_prev_pt_idx = left_next_pt_idx; right_prev_pt_idx = right_next_pt_idx; - + created_llts.push_back(new_llt); - + // Detected the end already. Exiting now if (left_prev_pt_idx == left_ls_size - 1 || right_prev_pt_idx == right_ls_size - 1) { break; } - + } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitLaneletWithRatio returning lanelets size: " << created_llts.size()); @@ -903,7 +903,7 @@ void WMBroadcaster::addPassingControlLineFromMsg(std::shared_ptr gf_pt if (msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED || msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::PASSINGONLY) { - left_participants = participants; + left_participants = participants; } else if (msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY) { @@ -912,7 +912,7 @@ void WMBroadcaster::addPassingControlLineFromMsg(std::shared_ptr gf_pt if (msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED || msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::PASSINGONLY) { - right_participants = participants; + right_participants = participants; } else if (msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY) { @@ -929,7 +929,7 @@ void WMBroadcaster::addRegionAccessRule(std::shared_ptr gf_ptr, const gf_ptr->label_ = msg_v01.package.label; auto regulatory_element = std::make_shared(lanelet::RegionAccessRule::buildData(lanelet::utils::getId(),affected_llts,{},invertParticipants(participantsChecker(msg_v01)), reason)); - if(!regulatory_element->accessable(lanelet::Participants::VehicleCar) || !regulatory_element->accessable(lanelet::Participants::VehicleTruck )) + if(!regulatory_element->accessable(lanelet::Participants::VehicleCar) || !regulatory_element->accessable(lanelet::Participants::VehicleTruck )) { gf_ptr->invalidate_route_=true; } @@ -942,9 +942,9 @@ void WMBroadcaster::addRegionAccessRule(std::shared_ptr gf_ptr, const void WMBroadcaster::addRegionMinimumGap(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01, double min_gap, const std::vector& affected_llts, const std::vector& affected_areas) const { - auto regulatory_element = std::make_shared(lanelet::DigitalMinimumGap::buildData(lanelet::utils::getId(), + auto regulatory_element = std::make_shared(lanelet::DigitalMinimumGap::buildData(lanelet::utils::getId(), min_gap, affected_llts, affected_areas,participantsChecker(msg_v01) )); - + gf_ptr->regulatory_element_ = regulatory_element; } @@ -1057,26 +1057,26 @@ void WMBroadcaster::externalMapMsgCallback(carma_v2x_msgs::msg::MapData::UniqueP { return; } - + gf_ptr->map_msg_ = *map_msg; gf_ptr->msg_.package.label_exists = true; gf_ptr->msg_.package.label = "MAP_MSG"; - gf_ptr->id_ = boost::uuids::random_generator()(); + gf_ptr->id_ = boost::uuids::random_generator()(); // create dummy traffic Control message to add instant activation schedule carma_v2x_msgs::msg::TrafficControlMessageV01 traffic_control_msg; // process schedule from message addScheduleFromMsg(gf_ptr, traffic_control_msg); - + scheduleGeofence(gf_ptr); } -// currently only supports geofence message version 1: TrafficControlMessageV01 +// currently only supports geofence message version 1: TrafficControlMessageV01 void WMBroadcaster::geofenceCallback(carma_v2x_msgs::msg::TrafficControlMessage::UniquePtr geofence_msg) { - + std::lock_guard guard(map_mutex_); std::stringstream reason_ss; // quickly check if the id has been added @@ -1089,7 +1089,7 @@ void WMBroadcaster::geofenceCallback(carma_v2x_msgs::msg::TrafficControlMessage: boost::uuids::uuid id; std::copy(geofence_msg->tcm_v01.id.id.begin(), geofence_msg->tcm_v01.id.id.end(), id.begin()); - if (checked_geofence_ids_.find(boost::uuids::to_string(id)) != checked_geofence_ids_.end()) { + if (checked_geofence_ids_.find(boost::uuids::to_string(id)) != checked_geofence_ids_.end()) { reason_ss.str(""); reason_ss << "Dropping received TrafficControl message with already handled id: " << boost::uuids::to_string(id); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), reason_ss.str()); @@ -1109,7 +1109,7 @@ void WMBroadcaster::geofenceCallback(carma_v2x_msgs::msg::TrafficControlMessage: RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "CARMA_WM_CTRL received a TrafficControlMessage with unknown TrafficControlRequest ID (reqid): " << reqid); return; } - + checked_geofence_ids_.insert(boost::uuids::to_string(id)); auto gf_ptr = std::make_shared(); @@ -1119,7 +1119,7 @@ void WMBroadcaster::geofenceCallback(carma_v2x_msgs::msg::TrafficControlMessage: try { // process schedule from message - addScheduleFromMsg(gf_ptr, geofence_msg->tcm_v01); + addScheduleFromMsg(gf_ptr, geofence_msg->tcm_v01); scheduleGeofence(gf_ptr); reason_ss.str(""); reason_ss << "Successfully processed TCM."; @@ -1137,11 +1137,11 @@ void WMBroadcaster::geofenceCallback(carma_v2x_msgs::msg::TrafficControlMessage: void WMBroadcaster::scheduleGeofence(std::shared_ptr gf_ptr) { RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Scheduling new geofence message received by WMBroadcaster with id: " << gf_ptr->id_); - + bool detected_workzone_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("SIG_WZ") != std::string::npos; - + carma_v2x_msgs::msg::TrafficControlDetail msg_detail = gf_ptr->msg_.params.detail; - + // create workzone specific extra speed geofence if (detected_workzone_signal && msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE) { @@ -1167,7 +1167,7 @@ void WMBroadcaster::scheduleGeofence(std::shared_ptr gf } duplicate_msg.geometry.nodes[0].x = first_x; duplicate_msg.geometry.nodes[0].y = first_y; - + gf_ptr_speed->msg_ = duplicate_msg; scheduler_.addGeofence(gf_ptr_speed); } @@ -1205,7 +1205,7 @@ void WMBroadcaster::geoReferenceCallback(std_msgs::msg::String::UniquePtr geo_re void WMBroadcaster::setMaxLaneWidth(double max_lane_width) { sim_ = std::make_shared(); - + max_lane_width_ = max_lane_width; sim_->setMaxLaneWidth(max_lane_width_); } @@ -1253,14 +1253,14 @@ uint32_t WMBroadcaster::generate32BitId(const std::string& label) { auto pos1 = label.find("INT_ID:") + 7; auto pos2 = label.find("SG_ID:") + 6; - + uint16_t intersection_id = std::stoi(label.substr(pos1 , 4)); uint8_t signal_id = std::stoi(label.substr(pos2 , 3)); return carma_wm::utils::get32BitId(intersection_id, signal_id); } -// currently only supports geofence message version 1: TrafficControlMessageV01 +// currently only supports geofence message version 1: TrafficControlMessageV01 lanelet::Points3d WMBroadcaster::getPointsInLocalFrame(const carma_v2x_msgs::msg::TrafficControlMessageV01& tcm_v01) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Getting affected lanelets"); @@ -1273,7 +1273,7 @@ lanelet::Points3d WMBroadcaster::getPointsInLocalFrame(const carma_v2x_msgs::msg std::string("get transformation between the geofence and the map")); // This next section handles the geofence projection conversion - // The datum field is used to identify the frame for the provided referance lat/lon. + // The datum field is used to identify the frame for the provided referance lat/lon. // This reference is then converted to the provided projection as a reference origin point // From the reference the message projection to map projection transformation is used to convert the nodes in the TrafficControlMessage std::string projection = tcm_v01.geometry.proj; @@ -1285,33 +1285,33 @@ lanelet::Points3d WMBroadcaster::getPointsInLocalFrame(const carma_v2x_msgs::msg RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Projection field: " << projection); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Datum field: " << datum); - + std::string universal_frame = datum; //lat/long included in TCM is in this datum RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Traffic Control heading provided: " << tcm_v01.geometry.heading << " System understanding is that this value will not affect the projection and is only provided for supporting derivative calculations."); - + // Create the resulting projection transformation PJ* universal_to_target = proj_create_crs_to_crs(PJ_DEFAULT_CTX, universal_frame.c_str(), projection.c_str(), nullptr); if (universal_to_target == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection - - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between geofence and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX) + + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between geofence and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX) << " universal_frame: " << universal_frame << " projection: " << projection); return {}; // Ignore geofence if it could not be projected from universal to TCM frame } - + PJ* target_to_map = proj_create_crs_to_crs(PJ_DEFAULT_CTX, projection.c_str(), base_map_georef_.c_str(), nullptr); if (target_to_map == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection - - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between geofence and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX) + + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between geofence and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX) << " target_to_map: " << target_to_map << " base_map_georef_: " << base_map_georef_); return {}; // Ignore geofence if it could not be projected into the map frame - + } - + // convert all geofence points into our map's frame std::vector gf_pts; carma_v2x_msgs::msg::PathNode prev_pt; @@ -1323,7 +1323,7 @@ lanelet::Points3d WMBroadcaster::getPointsInLocalFrame(const carma_v2x_msgs::msg RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "In TCM's frame, initial Point X "<< prev_pt.x<<" Before conversion: Point Y "<< prev_pt.y ); for (auto pt : tcm_v01.geometry.nodes) - { + { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion in TCM frame: Point X "<< pt.x <<" Before conversion: Point Y "<< pt.y); PJ_COORD c {{prev_pt.x + pt.x, prev_pt.y + pt.y, 0, 0}}; // z is not currently used @@ -1336,7 +1336,7 @@ lanelet::Points3d WMBroadcaster::getPointsInLocalFrame(const carma_v2x_msgs::msg RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "After conversion in Map frame: Point X "<< gf_pts.back().x() <<" After conversion: Point Y "<< gf_pts.back().y()); } - + // save the points converted to local map frame return gf_pts; } @@ -1361,7 +1361,7 @@ bool WMBroadcaster::shouldChangeControlLine(const lanelet::ConstLaneletOrArea& e { return true; } - + lanelet::PassingControlLinePtr pcl = std::dynamic_pointer_cast(current_map_->regulatoryElementLayer.get(regem->id())); // if this geofence's pcl doesn't match with the lanelets current bound side, return false as we shouldn't change bool should_change_pcl = false; @@ -1392,24 +1392,24 @@ bool WMBroadcaster::shouldChangeTrafficSignal(const lanelet::ConstLaneletOrArea& { return true; } - + lanelet::CarmaTrafficSignalPtr traffic_signal = std::dynamic_pointer_cast(current_map_->regulatoryElementLayer.get(regem->id())); uint8_t signal_id = 0; - - for (auto it = sim->signal_group_to_traffic_light_id_.begin(); it != sim->signal_group_to_traffic_light_id_.end(); ++it) + + for (auto it = sim->signal_group_to_traffic_light_id_.begin(); it != sim->signal_group_to_traffic_light_id_.end(); ++it) { if (regem->id() == it->second) { signal_id = it->first; - } - } + } + } if (signal_id == 0) // doesn't exist in the record return true; - + if (sim->signal_group_to_entry_lanelet_ids_[signal_id].find(el.id()) != sim->signal_group_to_entry_lanelet_ids_[signal_id].end()) return false; // signal group's entry lane is still part of the intersection, so don't change - + return true; } @@ -1436,7 +1436,7 @@ void WMBroadcaster::addRegulatoryComponent(std::shared_ptr gf_ptr) con } - // this loop is also kept separately because previously we assumed + // this loop is also kept separately because previously we assumed // there was existing regem, but this handles changes to all of the elements for (auto el: gf_ptr->affected_parts_) { @@ -1449,7 +1449,7 @@ void WMBroadcaster::addRegulatoryComponent(std::shared_ptr gf_ptr) con RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Regulatory element with invalid id in geofence cannot be added to the map"); } } - + } @@ -1457,7 +1457,7 @@ void WMBroadcaster::addRegulatoryComponent(std::shared_ptr gf_ptr) con void WMBroadcaster::addBackRegulatoryComponent(std::shared_ptr gf_ptr) const { // First loop is to remove the relation between element and regulatory element that this geofence added initially - + for (auto el: gf_ptr->affected_parts_) { for (auto regem : el.regulatoryElements()) @@ -1472,7 +1472,7 @@ void WMBroadcaster::addBackRegulatoryComponent(std::shared_ptr gf_ptr) } } } - + // As this gf received is the first gf that was sent in through addGeofence, // we have prev speed limit information inside it to put them back for (auto pair : gf_ptr->prev_regems_) @@ -1481,17 +1481,17 @@ void WMBroadcaster::addBackRegulatoryComponent(std::shared_ptr gf_ptr) { current_map_->update(current_map_->laneletLayer.get(pair.first), pair.second); gf_ptr->update_list_.push_back(pair); - } + } } } void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) { - + std::lock_guard guard(map_mutex_); RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Adding active geofence to the map with geofence id: " << gf_ptr->id_); - - // if applying workzone geometry geofence, utilize workzone chache to create one + + // if applying workzone geometry geofence, utilize workzone chache to create one // also multiple map updates can be sent from one geofence object std::vector> updates_to_send; @@ -1516,7 +1516,7 @@ void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) } for (auto update : updates_to_send) - { + { // add marker to rviz tcm_marker_array_.markers.push_back(composeTCMMarkerVisualizer(update->gf_pts)); // create visualizer in rviz @@ -1525,14 +1525,14 @@ void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) // Process the geofence object to populate update remove lists addGeofenceHelper(update); - + if (!detected_map_msg_signal) { for (auto pair : update->update_list_) active_geofence_llt_ids_.insert(pair.first); } autoware_lanelet2_msgs::msg::MapBin gf_msg; - + // If the geofence invalidates the route graph then recompute the routing graph now that the map has been updated if (update->invalidate_route_) { @@ -1554,7 +1554,7 @@ void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done creating routing graph message"); } - + // Publish auto send_data = std::make_shared(carma_wm::TrafficControl(update->id_, update->update_list_, update->remove_list_, update->lanelet_additions_)); @@ -1568,18 +1568,18 @@ void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) carma_wm::toBinMsg(send_data, &gf_msg); update_count_++; // Update the sequence count for the geofence messages gf_msg.seq_id = update_count_; - gf_msg.invalidates_route=update->invalidate_route_; + gf_msg.invalidates_route=update->invalidate_route_; gf_msg.map_version = current_map_version_; map_update_pub_(gf_msg); } - + } void WMBroadcaster::removeGeofence(std::shared_ptr gf_ptr) { std::lock_guard guard(map_mutex_); RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Removing inactive geofence from the map with geofence id: " << gf_ptr->id_); - + // Process the geofence object to populate update remove lists if (gf_ptr->affected_parts_.empty()) return; @@ -1613,7 +1613,7 @@ void WMBroadcaster::removeGeofence(std::shared_ptr gf_ptr) RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done creating routing graph message for geofence removal"); } - + carma_wm::toBinMsg(send_data, &gf_msg_revert); update_count_++; // Update the sequence count for geofence messages gf_msg_revert.seq_id = update_count_; @@ -1623,7 +1623,7 @@ void WMBroadcaster::removeGeofence(std::shared_ptr gf_ptr) } - + carma_planning_msgs::msg::Route WMBroadcaster::getRoute() { return current_route; @@ -1632,7 +1632,7 @@ carma_planning_msgs::msg::Route WMBroadcaster::getRoute() void WMBroadcaster::routeCallbackMessage(carma_planning_msgs::msg::Route::UniquePtr route_msg) { current_route = *route_msg; - carma_v2x_msgs::msg::TrafficControlRequest cR; + carma_v2x_msgs::msg::TrafficControlRequest cR; cR = controlRequestFromRoute(*route_msg); control_msg_pub_(cR); @@ -1640,7 +1640,7 @@ void WMBroadcaster::routeCallbackMessage(carma_planning_msgs::msg::Route::Uniqu carma_v2x_msgs::msg::TrafficControlRequest WMBroadcaster::controlRequestFromRoute(const carma_planning_msgs::msg::Route& route_msg, std::shared_ptr req_id_for_testing) { - lanelet::ConstLanelets path; + lanelet::ConstLanelets path; if (!current_map_ || current_map_->laneletLayer.size() == 0) { @@ -1650,7 +1650,7 @@ carma_v2x_msgs::msg::TrafficControlRequest WMBroadcaster::controlRequestFromRout } - for(auto id : route_msg.route_path_lanelet_ids) + for(auto id : route_msg.route_path_lanelet_ids) { auto laneLayer = current_map_->laneletLayer.get(id); path.push_back(laneLayer); @@ -1658,12 +1658,12 @@ carma_v2x_msgs::msg::TrafficControlRequest WMBroadcaster::controlRequestFromRout // update local copy route_path_ = path; - + if(path.size() == 0) throw lanelet::InvalidObjectStateError(std::string("No lanelets available in path.")); /*logic to determine route bounds*/ - std::vector llt; - std::vector pathBox; + std::vector llt; + std::vector pathBox; double minX = std::numeric_limits::max(); double minY = std::numeric_limits::max(); double maxX = std::numeric_limits::lowest(); @@ -1672,7 +1672,7 @@ carma_v2x_msgs::msg::TrafficControlRequest WMBroadcaster::controlRequestFromRout while (path.size() != 0) //Continue until there are no more lanelet elements in path { llt.push_back(path.back()); //Add a lanelet to the vector - + pathBox.push_back(lanelet::geometry::boundingBox2d(llt.back())); //Create a bounding box of the added lanelet and add it to the vector @@ -1696,17 +1696,17 @@ carma_v2x_msgs::msg::TrafficControlRequest WMBroadcaster::controlRequestFromRout path.pop_back(); //remove the added lanelet from path an reduce pack.size() by 1 } //end of while loop - + std::string target_frame = base_map_georef_; - if (target_frame.empty()) + if (target_frame.empty()) { // Return / log warning etc. RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Value 'target_frame' is empty."); throw lanelet::InvalidObjectStateError(std::string("Base georeference map may not be loaded to the WMBroadcaster")); } - + // Convert the minimum point to latlon lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str()); lanelet::BasicPoint3d localPoint; @@ -1716,19 +1716,19 @@ carma_v2x_msgs::msg::TrafficControlRequest WMBroadcaster::controlRequestFromRout lanelet::GPSPoint gpsRoute = local_projector.reverse(localPoint); //If the appropriate library is included, the reverse() function can be used to convert from local xyz to lat/lon - // Create a local transverse mercator frame at the minimum point to allow us to get east,north oriented bounds + // Create a local transverse mercator frame at the minimum point to allow us to get east,north oriented bounds std::string local_tmerc_enu_proj = "+proj=tmerc +datum=WGS84 +h_0=0 +lat_0=" + std::to_string(gpsRoute.lat) + " +lon_0=" + std::to_string(gpsRoute.lon); // Create transform from map frame to local transform mercator frame at bounds min point PJ* tmerc_proj = proj_create_crs_to_crs(PJ_DEFAULT_CTX, target_frame.c_str(), local_tmerc_enu_proj.c_str(), nullptr); - + if (tmerc_proj == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection - - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between request bounds frame and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX) + + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between request bounds frame and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX) << " MapProjection: " << target_frame << " Message Projection: " << local_tmerc_enu_proj); return {}; // Ignore geofence if it could not be projected into the map frame - + } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion: Top Left: ("<< minX <<", "< guard(map_mutex_); - if (!current_map_ || current_map_->laneletLayer.size() == 0) + if (!current_map_ || current_map_->laneletLayer.size() == 0) { throw lanelet::InvalidObjectStateError(std::string("Lanelet map (current_map_) is not loaded to the WMBroadcaster")); } @@ -1878,7 +1878,7 @@ double WMBroadcaster::distToNearestActiveGeofence(const lanelet::BasicPoint2d& c std::vector active_geofence_on_route; for (auto llt : route_path_) { - if (active_geofence_llt_ids_.find(llt.id()) != active_geofence_llt_ids_.end()) + if (active_geofence_llt_ids_.find(llt.id()) != active_geofence_llt_ids_.end()) active_geofence_on_route.push_back(llt.id()); } // Get the lanelet of this point @@ -1894,7 +1894,7 @@ double WMBroadcaster::distToNearestActiveGeofence(const lanelet::BasicPoint2d& c for (auto id: active_geofence_on_route) { carma_wm::TrackPos tp = carma_wm::geometry::trackPos(current_map_->laneletLayer.get(id), curr_pos); - // downtrack needs to be negative for lanelet to be in front of the point, + // downtrack needs to be negative for lanelet to be in front of the point, // also we don't account for the lanelet that the vehicle is on if (tp.downtrack < 0 && id != curr_lanelet.id()) { @@ -1974,14 +1974,14 @@ void WMBroadcaster::publishLightId() { return; } - for(auto id : current_route.route_path_lanelet_ids) + for(auto id : current_route.route_path_lanelet_ids) { bool convert_success = false; unsigned intersection_id = 0; unsigned group_id = 0; auto route_lanelet= current_map_->laneletLayer.get(id); auto traffic_lights = route_lanelet.regulatoryElementsAs(); - + if (!traffic_lights.empty()) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Found Traffic Light Regulatory Element id: " << traffic_lights.front()->id()); @@ -1989,7 +1989,7 @@ void WMBroadcaster::publishLightId() } if (!convert_success) - continue; + continue; RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Found Traffic Light with Intersection id: " << intersection_id << " Group id:" << group_id); bool id_exists = false; @@ -2012,7 +2012,7 @@ void WMBroadcaster::publishLightId() carma_perception_msgs::msg::CheckActiveGeofence WMBroadcaster::checkActiveGeofenceLogic(const geometry_msgs::msg::PoseStamped& current_pos) { - if (!current_map_ || current_map_->laneletLayer.size() == 0) + if (!current_map_ || current_map_->laneletLayer.size() == 0) { throw lanelet::InvalidObjectStateError(std::string("Lanelet map 'current_map_' is not loaded to the WMBroadcaster")); } @@ -2021,8 +2021,8 @@ carma_perception_msgs::msg::CheckActiveGeofence WMBroadcaster::checkActiveGeofen double current_pos_x = current_pos.pose.position.x; double current_pos_y = current_pos.pose.position.y; - - + + lanelet::BasicPoint2d curr_pos; curr_pos.x() = current_pos_x; @@ -2031,11 +2031,11 @@ carma_perception_msgs::msg::CheckActiveGeofence WMBroadcaster::checkActiveGeofen carma_perception_msgs::msg::CheckActiveGeofence outgoing_geof; //message to publish double next_distance = 0 ; //Distance to next geofence - if (active_geofence_llt_ids_.size() <= 0 ) + if (active_geofence_llt_ids_.size() <= 0 ) { return outgoing_geof; } - + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence llt ids are loaded to the WMBroadcaster"); // Obtain the closest lanelet to the vehicle's current position @@ -2044,14 +2044,14 @@ carma_perception_msgs::msg::CheckActiveGeofence WMBroadcaster::checkActiveGeofen /* determine whether or not the vehicle's current position is within an active geofence */ if (boost::geometry::within(curr_pos, current_llt.polygon2d().basicPolygon())) - { + { next_distance = distToNearestActiveGeofence(curr_pos); outgoing_geof.distance_to_next_geofence = next_distance; - for(auto id : active_geofence_llt_ids_) + for(auto id : active_geofence_llt_ids_) { if (id == current_llt.id()) - { + { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Vehicle is on Lanelet " << current_llt.id() << ", which has an active geofence"); outgoing_geof.is_on_active_geofence = true; for (auto regem: current_llt.regulatoryElements()) @@ -2063,10 +2063,10 @@ carma_perception_msgs::msg::CheckActiveGeofence WMBroadcaster::checkActiveGeofen (current_map_->regulatoryElementLayer.get(regem->id())); outgoing_geof.value = speed->speed_limit_.value(); outgoing_geof.advisory_speed = speed->speed_limit_.value(); - outgoing_geof.reason = speed->getReason(); + outgoing_geof.reason = speed->getReason(); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence has a speed limit of " << speed->speed_limit_.value()); - + // Cannot overrule outgoing_geof.type if it is already set to LANE_CLOSED if(outgoing_geof.type != carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED) { @@ -2082,7 +2082,7 @@ carma_perception_msgs::msg::CheckActiveGeofence WMBroadcaster::checkActiveGeofen outgoing_geof.minimum_gap = min_gap->getMinimumGap(); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence has a minimum gap of " << min_gap->getMinimumGap()); } - + // Assign active geofence fields based on whether the current lane is closed or is immediately adjacent to a closed lane if(regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0) { @@ -2090,7 +2090,7 @@ carma_perception_msgs::msg::CheckActiveGeofence WMBroadcaster::checkActiveGeofen (current_map_->regulatoryElementLayer.get(regem->id())); // Update the 'type' and 'reason' for this active geofence if the vehicle is in a closed lane - if(!accessRuleReg->accessable(lanelet::Participants::VehicleCar) || !accessRuleReg->accessable(lanelet::Participants::VehicleTruck)) + if(!accessRuleReg->accessable(lanelet::Participants::VehicleCar) || !accessRuleReg->accessable(lanelet::Participants::VehicleTruck)) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence is a closed lane."); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Closed lane reason: " << accessRuleReg->getReason()); @@ -2098,7 +2098,7 @@ carma_perception_msgs::msg::CheckActiveGeofence WMBroadcaster::checkActiveGeofen outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED; } // Otherwise, update the 'type' and 'reason' for this active geofence if the vehicle is in a lane immediately adjacent to a closed lane with the same travel direction - else + else { // Obtain all same-direction lanes sharing the right lane boundary (will include the current lanelet) auto right_boundary_lanelets = current_map_->laneletLayer.findUsages(current_llt.rightBound()); @@ -2202,9 +2202,9 @@ void WMBroadcaster::updateUpcomingSGIntersectionIds() lanelet::Lanelet route_lanelet; lanelet::Ids cur_route_lanelet_ids = current_route.route_path_lanelet_ids; bool isLightFound = false; - - for(auto id : cur_route_lanelet_ids) - { + + for(auto id : cur_route_lanelet_ids) + { route_lanelet= current_map_->laneletLayer.get(id); traffic_lights = route_lanelet.regulatoryElementsAs(); if(!traffic_lights.empty()) @@ -2217,7 +2217,7 @@ void WMBroadcaster::updateUpcomingSGIntersectionIds() if(isLightFound && sim_) { for(auto itr = sim_->signal_group_to_traffic_light_id_.begin(); itr != sim_->signal_group_to_traffic_light_id_.end(); itr++) - { + { if(itr->second == traffic_lights.front()->id()) { cur_signal_group_id = itr->first; @@ -2237,7 +2237,7 @@ void WMBroadcaster::updateUpcomingSGIntersectionIds() else { //Currently, each lanelet has only one intersection - lanelet::Id intersection_id = intersections.front()->id(); + lanelet::Id intersection_id = intersections.front()->id(); if(intersection_id != lanelet::InvalId) { for(auto itr = sim_->intersection_id_to_regem_id_.begin(); itr != sim_->intersection_id_to_regem_id_.end(); itr++) @@ -2252,7 +2252,7 @@ void WMBroadcaster::updateUpcomingSGIntersectionIds() RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "MAP msg: Intersection ID = " << map_msg_intersection_id << ", Signal Group ID =" << cur_signal_group_id ); if(map_msg_intersection_id != 0 && cur_signal_group_id != 0) - { + { upcoming_intersection_ids_.data.clear(); upcoming_intersection_ids_.data.push_back(static_cast(map_msg_intersection_id)); upcoming_intersection_ids_.data.push_back(static_cast(cur_signal_group_id)); @@ -2270,7 +2270,7 @@ void WMBroadcaster::pubTCMACK(j2735_v2x_msgs::msg::Id64b tcm_req_id, uint16_t ms { ss << std::setfill('0') << std::setw(2) << std::hex << (unsigned) tcm_req_id.id.at(i); } - std::string tcmv01_req_id_hex = ss.str(); + std::string tcmv01_req_id_hex = ss.str(); ss.str(""); ss << "traffic_control_id:" << tcmv01_req_id_hex << ", msgnum:"<< msgnum << ", acknowledgement:" << ack_status << ", reason:" << ack_reason; mom_msg.strategy_params = ss.str(); @@ -2290,4 +2290,3 @@ const uint8_t WorkZoneSection::REVERSE = 6; } // namespace carma_wm_ctrl - diff --git a/carma_wm_ctrl/test/GeofenceScheduleTest.cpp b/carma_wm_ctrl/test/GeofenceScheduleTest.cpp index cf0aa598b7..dcb202f5f4 100644 --- a/carma_wm_ctrl/test/GeofenceScheduleTest.cpp +++ b/carma_wm_ctrl/test/GeofenceScheduleTest.cpp @@ -24,11 +24,11 @@ TEST(GeofenceSchedule, scheduleStarted) GeofenceSchedule sch; sch.schedule_start_ = rclcpp::Time(0); sch.schedule_end_ = rclcpp::Time(1); - sch.control_start_ = rclcpp::Duration(0); - sch.control_duration_ = rclcpp::Duration(1); - sch.control_offset_ = rclcpp::Duration(0); - sch.control_span_ = rclcpp::Duration(1); - sch.control_period_ = rclcpp::Duration(2); + sch.control_start_ = rclcpp::Duration::from_nanoseconds(0); + sch.control_duration_ = rclcpp::Duration::from_nanoseconds(1); + sch.control_offset_ = rclcpp::Duration::from_nanoseconds(0); + sch.control_span_ = rclcpp::Duration::from_nanoseconds(1); + sch.control_period_ = rclcpp::Duration::from_nanoseconds(2); ASSERT_TRUE(sch.scheduleStarted(rclcpp::Time(0))); ASSERT_TRUE(sch.scheduleStarted(rclcpp::Time(0.9))); @@ -49,8 +49,8 @@ TEST(GeofenceSchedule, getNextInterval) { // Test before start - GeofenceSchedule sch(rclcpp::Time(1e9 * 1), rclcpp::Time(1e9 * 6), rclcpp::Duration(1e9 * 2), rclcpp::Duration(1e9 * 1), rclcpp::Duration(1e9 * 0), rclcpp::Duration(1e9 * 1), - rclcpp::Duration(1e9 * 2) // This means the next schedule is a 4 (2+2) + GeofenceSchedule sch(rclcpp::Time(1e9 * 1), rclcpp::Time(1e9 * 6), rclcpp::Duration::from_nanoseconds(1e9 * 2), rclcpp::Duration::from_nanoseconds(1e9 * 1), rclcpp::Duration::from_nanoseconds(1e9 * 0), rclcpp::Duration::from_nanoseconds(1e9 * 1), + rclcpp::Duration::from_nanoseconds(1e9 * 2) // This means the next schedule is a 4 (2+2) ); // Test before control start @@ -66,8 +66,8 @@ TEST(GeofenceSchedule, getNextInterval) ASSERT_NEAR(0.0, sch.getNextInterval(rclcpp::Time(1e9 * 3.5)).second.seconds(), 0.00001); ASSERT_FALSE(sch.getNextInterval(rclcpp::Time(1e9 * 3.5)).first); - sch = GeofenceSchedule(rclcpp::Time(1e9 * 1), rclcpp::Time(1e9 * 6), rclcpp::Duration(1e9 * 2), rclcpp::Duration(1e9 * 3), rclcpp::Duration(1e9 * 0), rclcpp::Duration(1e9 * 1), - rclcpp::Duration(1e9 * 2) // This means the next schedule is a 4 (2+2) + sch = GeofenceSchedule(rclcpp::Time(1e9 * 1), rclcpp::Time(1e9 * 6), rclcpp::Duration::from_nanoseconds(1e9 * 2), rclcpp::Duration::from_nanoseconds(1e9 * 3), rclcpp::Duration::from_nanoseconds(1e9 * 0), rclcpp::Duration::from_nanoseconds(1e9 * 1), + rclcpp::Duration::from_nanoseconds(1e9 * 2) // This means the next schedule is a 4 (2+2) ); // Test between end of first control and start of second ASSERT_NEAR(4, sch.getNextInterval(rclcpp::Time(1e9 * 3.5)).second.seconds(), 0.00001); @@ -85,4 +85,4 @@ TEST(GeofenceSchedule, getNextInterval) ASSERT_NEAR(0.0, sch.getNextInterval(rclcpp::Time(1e9 * 7.0)).second.seconds(), 0.00001); ASSERT_FALSE(sch.getNextInterval(rclcpp::Time(1e9 * 7.0)).first); } -} // namespace carma_wm_ctrl \ No newline at end of file +} // namespace carma_wm_ctrl diff --git a/carma_wm_ctrl/test/GeofenceSchedulerTest.cpp b/carma_wm_ctrl/test/GeofenceSchedulerTest.cpp index 1afc2db1ce..ec33a6751a 100644 --- a/carma_wm_ctrl/test/GeofenceSchedulerTest.cpp +++ b/carma_wm_ctrl/test/GeofenceSchedulerTest.cpp @@ -45,7 +45,7 @@ TEST(GeofenceScheduler, Constructor) GeofenceScheduler scheduler(timer); // Create scheduler with test timers. Having this // check helps verify that the timers do not crash // on destruction - + } TEST(GeofenceScheduler, addGeofence) @@ -54,23 +54,23 @@ TEST(GeofenceScheduler, addGeofence) // Finally test cleaing the timers auto gf_ptr = std::make_shared(); - boost::uuids::uuid first_id = boost::uuids::random_generator()(); + boost::uuids::uuid first_id = boost::uuids::random_generator()(); std::size_t first_id_hashed = boost::hash()(first_id); gf_ptr->id_ = first_id; auto timer = std::make_shared(); GeofenceScheduler scheduler(timer); // Create scheduler - + gf_ptr->schedules.push_back( GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 rclcpp::Time(8e9), - rclcpp::Duration(2e9), // Starts at 2 - rclcpp::Duration(3.5e9), // Ends at by 5.5 - rclcpp::Duration(0), // repetition start 0 offset, so still start at 2 - rclcpp::Duration(1e9), // Duration of 1 and interval of 2 so active durations are (2-3 and 4-5) - rclcpp::Duration(2e9))); - + rclcpp::Duration::from_nanoseconds(2e9), // Starts at 2 + rclcpp::Duration::from_nanoseconds(3.5e9), // Ends at by 5.5 + rclcpp::Duration::from_nanoseconds(0), // repetition start 0 offset, so still start at 2 + rclcpp::Duration::from_nanoseconds(1e9), // Duration of 1 and interval of 2 so active durations are (2-3 and 4-5) + rclcpp::Duration::from_nanoseconds(2e9))); + std::atomic active_call_count(0); std::atomic inactive_call_count(0); std::atomic last_active_gf(0); @@ -96,7 +96,7 @@ TEST(GeofenceScheduler, addGeofence) scheduler.addGeofence(gf_ptr); - + timer->setNow(rclcpp::Time(1.0e9)); // Set current time @@ -107,7 +107,7 @@ TEST(GeofenceScheduler, addGeofence) timer->setNow(rclcpp::Time(2.1e9)); // Set current time - + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10, first_id_hashed, last_active_gf)); ASSERT_EQ(1, active_call_count.load()); ASSERT_EQ(0, inactive_call_count.load()); @@ -164,4 +164,4 @@ TEST(GeofenceScheduler, addGeofence) } -} // namespace carma_wm_ctrl \ No newline at end of file +} // namespace carma_wm_ctrl diff --git a/carma_wm_ctrl/test/WMBroadcasterTest.cpp b/carma_wm_ctrl/test/WMBroadcasterTest.cpp index 8b7d39713b..1837c3df83 100644 --- a/carma_wm_ctrl/test/WMBroadcasterTest.cpp +++ b/carma_wm_ctrl/test/WMBroadcasterTest.cpp @@ -281,11 +281,11 @@ TEST(WMBroadcaster, geofenceCallback) gf->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 rclcpp::Time(8e9), - rclcpp::Duration(2e9), // Starts at 2 - rclcpp::Duration(1.1e9), // Ends at by 3.1 - rclcpp::Duration(0), // 0 offset for repetition start, so still starts at 2 - rclcpp::Duration(1e9), // Duration of 1 and interval of 2 so active durations are (2-3) - rclcpp::Duration(2e9))); + rclcpp::Duration::from_nanoseconds(2e9), // Starts at 2 + rclcpp::Duration::from_nanoseconds(1.1e9), // Ends at by 3.1 + rclcpp::Duration::from_nanoseconds(0), // 0 offset for repetition start, so still starts at 2 + rclcpp::Duration::from_nanoseconds(1e9), // Duration of 1 and interval of 2 so active durations are (2-3) + rclcpp::Duration::from_nanoseconds(2e9))); // convert to ros msg carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; std::copy(gf->id_.begin(), gf->id_.end(), msg_v01.id.id.begin()); @@ -737,11 +737,11 @@ TEST(WMBroadcaster, RegulatoryPCLTest) gf_ptr->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 rclcpp::Time(8e9), - rclcpp::Duration(2e9), // Starts at 2 - rclcpp::Duration(1.1e9), // Ends at by 3.1 - rclcpp::Duration(0), // 0 offset for repetition start, so still starts at 2 - rclcpp::Duration(1e9), // Duration of 1 and interval of two so active durations are (2-3) - rclcpp::Duration(2e9))); + rclcpp::Duration::from_nanoseconds(2e9), // Starts at 2 + rclcpp::Duration::from_nanoseconds(1.1e9), // Ends at by 3.1 + rclcpp::Duration::from_nanoseconds(0), // 0 offset for repetition start, so still starts at 2 + rclcpp::Duration::from_nanoseconds(1e9), // Duration of 1 and interval of two so active durations are (2-3) + rclcpp::Duration::from_nanoseconds(2e9))); // convert to ros msg carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; std::copy(gf_ptr->id_.begin(), gf_ptr->id_.end(), msg_v01.id.id.begin()); @@ -915,12 +915,12 @@ TEST(WMBroadcaster, geofenceFromMsgTest) msg_v01.params.schedule.start = rclcpp::Time(1); // Schedule between 1 ... msg_v01.params.schedule.end = rclcpp::Time(8); // and 8 carma_v2x_msgs::msg::DailySchedule daily_schedule; - daily_schedule.begin = rclcpp::Duration(2); // Starts at 2 - daily_schedule.duration = rclcpp::Duration(1.1); // Ends at by 3.1 + daily_schedule.begin = rclcpp::Duration::from_nanoseconds(2); // Starts at 2 + daily_schedule.duration = rclcpp::Duration::from_nanoseconds(1.1); // Ends at by 3.1 msg_v01.params.schedule.between.push_back(daily_schedule); - msg_v01.params.schedule.repeat.offset = rclcpp::Duration(0); // 0 offset for repetition start, so still starts at 2 - msg_v01.params.schedule.repeat.span = rclcpp::Duration(1); // Duration of 1 and interval of two so active durations are (2-3) - msg_v01.params.schedule.repeat.period = rclcpp::Duration(2); + msg_v01.params.schedule.repeat.offset = rclcpp::Duration::from_nanoseconds(0); // 0 offset for repetition start, so still starts at 2 + msg_v01.params.schedule.repeat.span = rclcpp::Duration::from_nanoseconds(1); // Duration of 1 and interval of two so active durations are (2-3) + msg_v01.params.schedule.repeat.period = rclcpp::Duration::from_nanoseconds(2); msg_v01.params.schedule.end_exists = true; msg_v01.params.schedule.between_exists = true; msg_v01.params.schedule.repeat_exists = true; @@ -1154,11 +1154,11 @@ TEST(WMBroadcaster, distToNearestActiveGeofence) gf->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 rclcpp::Time(8e9), - rclcpp::Duration(2e9), // Starts at 2 - rclcpp::Duration(1.1e9), // Ends at by 3.1 - rclcpp::Duration(0), // 0 offset for repetition start, so still starts at 2 - rclcpp::Duration(1e9), // Duration of 1 and interval of two so active durations are (2-3) - rclcpp::Duration(2e9))); + rclcpp::Duration::from_nanoseconds(2e9), // Starts at 2 + rclcpp::Duration::from_nanoseconds(1.1e9), // Ends at by 3.1 + rclcpp::Duration::from_nanoseconds(0), // 0 offset for repetition start, so still starts at 2 + rclcpp::Duration::from_nanoseconds(1e9), // Duration of 1 and interval of two so active durations are (2-3) + rclcpp::Duration::from_nanoseconds(2e9))); // convert to ros msg carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; std::copy(gf->id_.begin(), gf->id_.end(), msg_v01.id.id.begin()); @@ -1390,11 +1390,11 @@ TEST(WMBroadcaster, currentLocationCallback) gf->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 rclcpp::Time(8e9), - rclcpp::Duration(2e9), // Starts at 2 - rclcpp::Duration(1.1e9), // Ends at by 3.1 - rclcpp::Duration(0), // 0 offset for repetition start, so still starts at 2 - rclcpp::Duration(1e9), // Duration of 1 and interval of two so active durations are (2-3) - rclcpp::Duration(2e9))); + rclcpp::Duration::from_nanoseconds(2e9), // Starts at 2 + rclcpp::Duration::from_nanoseconds(1.1e9), // Ends at by 3.1 + rclcpp::Duration::from_nanoseconds(0), // 0 offset for repetition start, so still starts at 2 + rclcpp::Duration::from_nanoseconds(1e9), // Duration of 1 and interval of two so active durations are (2-3) + rclcpp::Duration::from_nanoseconds(2e9))); // convert to ros msg carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; std::copy(gf->id_.begin(), gf->id_.end(), msg_v01.id.id.begin()); @@ -1531,11 +1531,11 @@ TEST(WMBroadcaster, checkActiveGeofenceLogicTest) auto gf = std::make_shared(); gf->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 rclcpp::Time(8e9), - rclcpp::Duration(2e9), // Starts at 2 - rclcpp::Duration(1.1e9), // Ends at by 3.1 - rclcpp::Duration(0), // 0 offset for repetition start, so still starts at 2 - rclcpp::Duration(1e9), // Duration of 1 and interval of two so active durations are (2-3) - rclcpp::Duration(2e9))); + rclcpp::Duration::from_nanoseconds(2e9), // Starts at 2 + rclcpp::Duration::from_nanoseconds(1.1e9), // Ends at by 3.1 + rclcpp::Duration::from_nanoseconds(0), // 0 offset for repetition start, so still starts at 2 + rclcpp::Duration::from_nanoseconds(1e9), // Duration of 1 and interval of two so active durations are (2-3) + rclcpp::Duration::from_nanoseconds(2e9))); // Convert the geofence pointer into a TrafficControlMessageV01 message carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; @@ -1783,11 +1783,11 @@ TEST(WMBroadcaster, RegionAccessRuleTest) gf_ptr->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 rclcpp::Time(8e9), - rclcpp::Duration(2e9), // Starts at 2 - rclcpp::Duration(1.1e9), // Ends at by 3.1 - rclcpp::Duration(0), // 0 offset for repetition start, so still starts at 2 - rclcpp::Duration(1e9), // Duration of 1 and interval of two so active durations are (2-3) - rclcpp::Duration(2e9))); + rclcpp::Duration::from_nanoseconds(2e9), // Starts at 2 + rclcpp::Duration::from_nanoseconds(1.1e9), // Ends at by 3.1 + rclcpp::Duration::from_nanoseconds(0), // 0 offset for repetition start, so still starts at 2 + rclcpp::Duration::from_nanoseconds(1e9), // Duration of 1 and interval of two so active durations are (2-3) + rclcpp::Duration::from_nanoseconds(2e9))); // convert to ros msg carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; std::copy(gf_ptr->id_.begin(), gf_ptr->id_.end(), msg_v01.id.id.begin()); diff --git a/cooperative_lanechange/test/test_cooperative_plugin.cpp b/cooperative_lanechange/test/test_cooperative_plugin.cpp index 14a5cb9899..e9bd57940d 100644 --- a/cooperative_lanechange/test/test_cooperative_plugin.cpp +++ b/cooperative_lanechange/test/test_cooperative_plugin.cpp @@ -140,7 +140,7 @@ namespace cooperative_lanechange double acc = pow(maneuver.lane_change_maneuver.start_speed, 2) / (2 * (ending_downtrack - starting_downtrack)); double end_time = maneuver.lane_change_maneuver.start_speed / acc; maneuver.lane_change_maneuver.end_speed = 25.0; - maneuver.lane_change_maneuver.end_time = rclcpp::Time(end_time) + rclcpp::Duration(10 * 1e9); + maneuver.lane_change_maneuver.end_time = rclcpp::Time(end_time) + rclcpp::Duration::from_nanoseconds(10 * 1e9); maneuver.lane_change_maneuver.starting_lane_id = std::to_string(lane_change_start_id); maneuver.lane_change_maneuver.ending_lane_id = std::to_string(end_id); @@ -156,7 +156,7 @@ namespace cooperative_lanechange carma_planning_msgs::srv::PlanTrajectory::Response resp; req.maneuver_plan.planning_start_time = worker->now(); - req.maneuver_plan.planning_completion_time = rclcpp::Time(req.maneuver_plan.planning_start_time) + rclcpp::Duration(10 * 1e9); + req.maneuver_plan.planning_completion_time = rclcpp::Time(req.maneuver_plan.planning_start_time) + rclcpp::Duration::from_nanoseconds(10 * 1e9); req.vehicle_state.x_pos_global = veh_pos.x(); req.vehicle_state.y_pos_global = veh_pos.y(); req.vehicle_state.longitudinal_vel = maneuver.lane_change_maneuver.start_speed; @@ -189,7 +189,7 @@ namespace cooperative_lanechange req.maneuver_index_to_plan = 0; req.maneuver_plan.planning_start_time = worker->now(); - req.maneuver_plan.planning_completion_time = rclcpp::Time(req.maneuver_plan.planning_start_time) + rclcpp::Duration(10 * 1e9); + req.maneuver_plan.planning_completion_time = rclcpp::Time(req.maneuver_plan.planning_start_time) + rclcpp::Duration::from_nanoseconds(10 * 1e9); lanelet::BasicPoint2d veh_pos(1.0,1.0); req.vehicle_state.x_pos_global = veh_pos.x(); @@ -269,7 +269,7 @@ namespace cooperative_lanechange double acc = pow(maneuver.lane_change_maneuver.start_speed,2)/(2*(ending_downtrack - starting_downtrack)); double end_time = maneuver.lane_change_maneuver.start_speed/acc; maneuver.lane_change_maneuver.end_speed = 25.0; - maneuver.lane_change_maneuver.end_time = rclcpp::Time(end_time) + rclcpp::Duration(10 * 1e9); + maneuver.lane_change_maneuver.end_time = rclcpp::Time(end_time) + rclcpp::Duration::from_nanoseconds(10 * 1e9); maneuver.lane_change_maneuver.starting_lane_id = std::to_string(lane_change_start_id); maneuver.lane_change_maneuver.ending_lane_id = std::to_string(end_id); @@ -284,7 +284,7 @@ namespace cooperative_lanechange carma_planning_msgs::srv::PlanTrajectory::Response resp; req.maneuver_plan.planning_start_time = worker->now(); - req.maneuver_plan.planning_completion_time = rclcpp::Time(req.maneuver_plan.planning_start_time) + rclcpp::Duration(10 * 1e9); + req.maneuver_plan.planning_completion_time = rclcpp::Time(req.maneuver_plan.planning_start_time) + rclcpp::Duration::from_nanoseconds(10 * 1e9); req.vehicle_state.x_pos_global = veh_pos.x(); req.vehicle_state.y_pos_global = veh_pos.y(); req.vehicle_state.longitudinal_vel = maneuver.lane_change_maneuver.start_speed; @@ -398,7 +398,7 @@ namespace cooperative_lanechange double acc = pow(maneuver.lane_change_maneuver.start_speed,2)/(2*(ending_downtrack - starting_downtrack)); double end_time = maneuver.lane_change_maneuver.start_speed/acc; maneuver.lane_change_maneuver.end_speed = 25.0; - maneuver.lane_change_maneuver.end_time = rclcpp::Time(end_time) + rclcpp::Duration(10 * 1e9); + maneuver.lane_change_maneuver.end_time = rclcpp::Time(end_time) + rclcpp::Duration::from_nanoseconds(10 * 1e9); maneuver.lane_change_maneuver.starting_lane_id = std::to_string(lane_change_start_id); maneuver.lane_change_maneuver.ending_lane_id = std::to_string(end_id); @@ -413,7 +413,7 @@ namespace cooperative_lanechange carma_planning_msgs::srv::PlanTrajectory::Response resp; req.maneuver_plan.planning_start_time = worker->now(); - req.maneuver_plan.planning_completion_time = rclcpp::Time(req.maneuver_plan.planning_start_time) + rclcpp::Duration(10 * 1e9); + req.maneuver_plan.planning_completion_time = rclcpp::Time(req.maneuver_plan.planning_start_time) + rclcpp::Duration::from_nanoseconds(10 * 1e9); req.vehicle_state.x_pos_global = veh_pos.x(); req.vehicle_state.y_pos_global = veh_pos.y(); req.vehicle_state.longitudinal_vel = maneuver.lane_change_maneuver.start_speed; diff --git a/frame_transformer/include/frame_transformer/frame_transformer.hpp b/frame_transformer/include/frame_transformer/frame_transformer.hpp index 811b7b4814..316a298ac7 100644 --- a/frame_transformer/include/frame_transformer/frame_transformer.hpp +++ b/frame_transformer/include/frame_transformer/frame_transformer.hpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/gnss_to_map_convertor/include/gnss_to_map_convertor/gnss_to_map_convertor_node.hpp b/gnss_to_map_convertor/include/gnss_to_map_convertor/gnss_to_map_convertor_node.hpp index 603f833db3..d4ecbe3b76 100644 --- a/gnss_to_map_convertor/include/gnss_to_map_convertor/gnss_to_map_convertor_node.hpp +++ b/gnss_to_map_convertor/include/gnss_to_map_convertor/gnss_to_map_convertor_node.hpp @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include @@ -51,7 +51,7 @@ namespace gnss_to_map_convertor // Buffer which holds the tree of transforms tf2_ros::Buffer tfBuffer_; - + // tf2 listeners. Subscribes to the /tf and /tf_static topics tf2_ros::TransformListener tfListener_ {tfBuffer_}; @@ -60,7 +60,7 @@ namespace gnss_to_map_convertor public: /** - * \brief Node constructor + * \brief Node constructor */ explicit Node(const rclcpp::NodeOptions &); diff --git a/gnss_to_map_convertor/src/GNSSToMapConvertor.cpp b/gnss_to_map_convertor/src/GNSSToMapConvertor.cpp index 5ef4028e71..25bbdf3810 100644 --- a/gnss_to_map_convertor/src/GNSSToMapConvertor.cpp +++ b/gnss_to_map_convertor/src/GNSSToMapConvertor.cpp @@ -17,7 +17,7 @@ #include "gnss_to_map_convertor/GNSSToMapConvertor.hpp" #include #include -#include +#include namespace gnss_to_map_convertor { diff --git a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.hpp b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.hpp index e9912d37f6..a9c4a2a3df 100755 --- a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.hpp +++ b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.hpp @@ -36,7 +36,7 @@ #include #include -#include +#include #include #include #include @@ -858,4 +858,4 @@ class LCIStrategicPlugin : public carma_guidance_plugins::StrategicPlugin }; -} // namespace lci_strategic_plugin \ No newline at end of file +} // namespace lci_strategic_plugin diff --git a/lci_strategic_plugin/src/lci_strategic_plugin.cpp b/lci_strategic_plugin/src/lci_strategic_plugin.cpp index 464ded65f3..f43f3a738e 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin.cpp @@ -222,7 +222,7 @@ void LCIStrategicPlugin::lookupFrontBumperTransform() tf2_buffer_.setUsingDedicatedThread(true); try { - geometry_msgs::msg::TransformStamped tf2 = tf2_buffer_.lookupTransform("base_link", "vehicle_front", rclcpp::Time(0), rclcpp::Duration(20.0 * 1e9)); //save to local copy of transform 20 sec timeout + geometry_msgs::msg::TransformStamped tf2 = tf2_buffer_.lookupTransform("base_link", "vehicle_front", rclcpp::Time(0), rclcpp::Duration::from_nanoseconds(20.0 * 1e9)); //save to local copy of transform 20 sec timeout length_to_front_bumper_ = tf2.transform.translation.x; RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "length_to_front_bumper_: " << length_to_front_bumper_); @@ -298,10 +298,10 @@ bool LCIStrategicPlugin::validLightState(const boost::optional LCIStrategicPlugin::canArriveAtGreenWithCertainty(const rclcpp::Time& light_arrival_time_by_algo, const lanelet::CarmaTrafficSignalPtr& traffic_light, bool check_late = true, bool check_early = true) const { rclcpp::Time early_arrival_time_by_algo = - light_arrival_time_by_algo - rclcpp::Duration(config_.green_light_time_buffer * 1e9); + light_arrival_time_by_algo - rclcpp::Duration::from_nanoseconds(config_.green_light_time_buffer * 1e9); rclcpp::Time late_arrival_time_by_algo = - light_arrival_time_by_algo + rclcpp::Duration(config_.green_light_time_buffer * 1e9); + light_arrival_time_by_algo + rclcpp::Duration::from_nanoseconds(config_.green_light_time_buffer * 1e9); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "light_arrival_time_by_algo: " << std::to_string(light_arrival_time_by_algo.seconds())); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "early_arrival_time_by_algo: " << std::to_string(early_arrival_time_by_algo.seconds())); @@ -392,7 +392,7 @@ void LCIStrategicPlugin::handleStopping(carma_planning_msgs::srv::PlanManeuvers: resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage( current_state.downtrack, traffic_light_down_track, current_state.speed, crossed_lanelets.front().id(), crossed_lanelets.back().id(), current_state.stamp, - current_state.stamp + rclcpp::Duration(config_.min_maneuver_planning_period * 1e9), decel_rate)); + current_state.stamp + rclcpp::Duration::from_nanoseconds(config_.min_maneuver_planning_period * 1e9), decel_rate)); } @@ -421,16 +421,16 @@ void LCIStrategicPlugin::handleFailureCase(carma_planning_msgs::srv::PlanManeuve } resp->new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, crossed_lanelets, - current_state_speed, incomplete_traj_params.modified_departure_speed, current_state.stamp, current_state.stamp + rclcpp::Duration(incomplete_traj_params.modified_remaining_time * 1e9), incomplete_traj_params)); + current_state_speed, incomplete_traj_params.modified_departure_speed, current_state.stamp, current_state.stamp + rclcpp::Duration::from_nanoseconds(incomplete_traj_params.modified_remaining_time * 1e9), incomplete_traj_params)); double intersection_length = intersection_end_downtrack_.get() - traffic_light_down_track; rclcpp::Time intersection_exit_time = - current_state.stamp + rclcpp::Duration(incomplete_traj_params.modified_remaining_time * 1e9) + rclcpp::Duration(intersection_length / incomplete_traj_params.modified_departure_speed * 1e9); + current_state.stamp + rclcpp::Duration::from_nanoseconds(incomplete_traj_params.modified_remaining_time * 1e9) + rclcpp::Duration::from_nanoseconds(intersection_length / incomplete_traj_params.modified_departure_speed * 1e9); resp->new_plan.maneuvers.push_back(composeIntersectionTransitMessage( traffic_light_down_track, intersection_end_downtrack_.get(), intersection_speed_.get(), - incomplete_traj_params.modified_departure_speed, current_state.stamp + rclcpp::Duration(incomplete_traj_params.modified_remaining_time * 1e9), intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id())); + incomplete_traj_params.modified_departure_speed, current_state.stamp + rclcpp::Duration::from_nanoseconds(incomplete_traj_params.modified_remaining_time * 1e9), intersection_exit_time, crossed_lanelets.back().id(), crossed_lanelets.back().id())); last_case_num_ = TSCase::DEGRADED_TSCASE; } @@ -473,7 +473,7 @@ void LCIStrategicPlugin::handleCruisingUntilStop(carma_planning_msgs::srv::PlanM resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage( new_ts_params.x2_, traffic_light_down_track, new_ts_params.v2_, case_8_crossed_lanelets.front().id(), case_8_crossed_lanelets.back().id(), rclcpp::Time(new_ts_params.t2_ * 1e9), - rclcpp::Time(new_ts_params.t2_ * 1e9) + rclcpp::Duration(config_.min_maneuver_planning_period * 1e9), decel_rate)); + rclcpp::Time(new_ts_params.t2_ * 1e9) + rclcpp::Duration::from_nanoseconds(config_.min_maneuver_planning_period * 1e9), decel_rate)); return; } @@ -514,7 +514,7 @@ void LCIStrategicPlugin::handleGreenSignalScenario(carma_planning_msgs::srv::Pla double intersection_length = intersection_end_downtrack_.get() - traffic_light_down_track; rclcpp::Time intersection_exit_time = - light_arrival_time_by_algo + rclcpp::Duration(intersection_length / intersection_speed_.get() * 1e9); + light_arrival_time_by_algo + rclcpp::Duration::from_nanoseconds(intersection_length / intersection_speed_.get() * 1e9); resp->new_plan.maneuvers.push_back(composeIntersectionTransitMessage( traffic_light_down_track, intersection_end_downtrack_.get(), intersection_speed_.get(), @@ -964,7 +964,7 @@ void LCIStrategicPlugin::planWhenAPPROACHING(carma_planning_msgs::srv::PlanManeu double stopping_time = current_state.speed / 1.5 / max_comfort_decel_norm_; //one half the acceleration (twice the acceleration to stop) to account for emergency case, see emergency_decel_norm_ rclcpp::Time stopping_arrival_time = - current_state.stamp + rclcpp::Duration(stopping_time * 1e9); + current_state.stamp + rclcpp::Duration::from_nanoseconds(stopping_time * 1e9); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "stopping_arrival_time: " << std::to_string(stopping_arrival_time.seconds())); @@ -1120,7 +1120,7 @@ void LCIStrategicPlugin::planWhenWAITING(carma_planning_msgs::srv::PlanManeuvers resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage( current_state.downtrack - stop_maneuver_buffer, traffic_light_down_track, current_state.speed, current_state.lane_id, current_state.lane_id, rclcpp::Time(entering_time * 1e9), - rclcpp::Time(entering_time * 1e9) + rclcpp::Duration(config_.min_maneuver_planning_period * 1e9), stopping_accel)); + rclcpp::Time(entering_time * 1e9) + rclcpp::Duration::from_nanoseconds(config_.min_maneuver_planning_period * 1e9), stopping_accel)); } void LCIStrategicPlugin::planWhenDEPARTING(carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, @@ -1138,7 +1138,7 @@ void LCIStrategicPlugin::planWhenDEPARTING(carma_planning_msgs::srv::PlanManeuve // Calculate exit time assuming constant acceleration rclcpp::Time intersection_exit_time = current_state.stamp + - rclcpp::Duration(2.0 * (intersection_end_downtrack - current_state.downtrack) / (current_state.speed + intersection_speed_limit) * 1e9); + rclcpp::Duration::from_nanoseconds(2.0 * (intersection_end_downtrack - current_state.downtrack) / (current_state.speed + intersection_speed_limit) * 1e9); // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver std::vector crossed_lanelets = @@ -1331,7 +1331,7 @@ void LCIStrategicPlugin::plan_maneuvers_callback( current_lanelet = llt_on_route_optional.value(); } else{ - RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "When identifying the corresponding lanelet for requested maneuever's state, x: " + RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "When identifying the corresponding lanelet for requested maneuever's state, x: " << req->veh_x << ", y: " << req->veh_y << ", no possible lanelet was found to be on the shortest path." << "Picking arbitrary lanelet: " << current_lanelets[0].id() << ", instead"); current_lanelet = current_lanelets[0]; diff --git a/lci_strategic_plugin/src/lci_strategic_plugin_algo.cpp b/lci_strategic_plugin/src/lci_strategic_plugin_algo.cpp index 27fc3eeeee..f92c0cfbce 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin_algo.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin_algo.cpp @@ -212,38 +212,38 @@ rclcpp::Duration LCIStrategicPlugin::get_earliest_entry_time(double remaining_di rclcpp::Duration t_accel(0,0); if ( x < x2 && current_speed > departure_speed) { - t_accel = rclcpp::Duration(0.0); + t_accel = rclcpp::Duration::from_nanoseconds(0.0); } else { - t_accel = rclcpp::Duration(std::max((v_hat - current_speed) / max_accel, 0.0) * 1e9); + t_accel = rclcpp::Duration::from_nanoseconds(std::max((v_hat - current_speed) / max_accel, 0.0) * 1e9); } rclcpp::Duration t_decel(0,0); if ( x < x2 && current_speed < departure_speed) { - t_decel = rclcpp::Duration(0.0); + t_decel = rclcpp::Duration::from_nanoseconds(0.0); } else { if (x < x2) { - t_decel = rclcpp::Duration(std::max((v_hat - current_speed) / max_decel, 0.0) * 1e9); + t_decel = rclcpp::Duration::from_nanoseconds(std::max((v_hat - current_speed) / max_decel, 0.0) * 1e9); } else { - t_decel = rclcpp::Duration(std::max((departure_speed - v_hat) / max_decel, 0.0) * 1e9); + t_decel = rclcpp::Duration::from_nanoseconds(std::max((departure_speed - v_hat) / max_decel, 0.0) * 1e9); } } rclcpp::Duration t_cruise(0,0); if (x1 <= x) { - t_cruise = rclcpp::Duration(std::max((x - x1)/v_hat, 0.0) * 1e9); + t_cruise = rclcpp::Duration::from_nanoseconds(std::max((x - x1)/v_hat, 0.0) * 1e9); } else { - t_cruise = rclcpp::Duration(0.0); + t_cruise = rclcpp::Duration::from_nanoseconds(0.0); } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "t_accel: " << t_accel.seconds() << ", t_cruise: " << t_cruise.seconds() << ", t_decel: " << t_decel.seconds()); return t_accel + t_cruise + t_decel; @@ -279,12 +279,12 @@ std::tuple LCIStrategicPlugin::get_final_entry_time_an else { in_tbd = false; - nearest_green_entry_time = nearest_green_entry_time_optional.value() + rclcpp::Duration(EPSILON * 1e9); //0.01sec more buffer since green_light algorithm's timestamp picks the previous signal - Vehicle Estimation + nearest_green_entry_time = nearest_green_entry_time_optional.value() + rclcpp::Duration::from_nanoseconds(EPSILON * 1e9); //0.01sec more buffer since green_light algorithm's timestamp picks the previous signal - Vehicle Estimation } } else if(config_.enable_carma_streets_connection ==true && scheduled_enter_time_ != 0 ) // UC3 { - nearest_green_entry_time = rclcpp::Time(std::max(earliest_entry_time.seconds(), (scheduled_enter_time_)/1000.0) * 1e9) + rclcpp::Duration(EPSILON * 1e9); //Carma Street + nearest_green_entry_time = rclcpp::Time(std::max(earliest_entry_time.seconds(), (scheduled_enter_time_)/1000.0) * 1e9) + rclcpp::Duration::from_nanoseconds(EPSILON * 1e9); //Carma Street // check if scheduled_enter_time_ is inside the available states interval size_t i = 0; @@ -339,7 +339,7 @@ std::tuple LCIStrategicPlugin::get_final_entry_time_an // check if it needs buffer below: rclcpp::Time early_arrival_time_green_et = - nearest_green_entry_time - rclcpp::Duration(config_.green_light_time_buffer * 1e9); + nearest_green_entry_time - rclcpp::Duration::from_nanoseconds(config_.green_light_time_buffer * 1e9); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "early_arrival_time_green_et: " << std::to_string(early_arrival_time_green_et.seconds())); @@ -403,7 +403,7 @@ std::tuple LCIStrategicPlugin::get_final_entry_time_an // If ET is within green or TBD, it should always aim for at least minimum of "start_time of green or tdb + green_buffer" for safety - nearest_green_entry_time_cached_ = nearest_green_signal_start_time + rclcpp::Duration((config_.green_light_time_buffer + EPSILON) * 1e9); + nearest_green_entry_time_cached_ = nearest_green_signal_start_time + rclcpp::Duration::from_nanoseconds((config_.green_light_time_buffer + EPSILON) * 1e9); // EPSILON=0.01 is there because if predictState's input exactly falls on ending_time it picks the previous state. //For example, if 0 - 10s is GREEN, and 10 - 12s is YELLOW, checking exactly 10.0s will return GREEN, @@ -1380,4 +1380,4 @@ TrajectoryParams LCIStrategicPlugin::boundary_decel_cruise_minspeed_decel(double //// NEW EQUATIONS END -} // namespace lci_strategic_plugin \ No newline at end of file +} // namespace lci_strategic_plugin diff --git a/lci_strategic_plugin/test/test_strategic_plugin.cpp b/lci_strategic_plugin/test/test_strategic_plugin.cpp index 0aae6a2db3..165bfa9ca8 100755 --- a/lci_strategic_plugin/test/test_strategic_plugin.cpp +++ b/lci_strategic_plugin/test/test_strategic_plugin.cpp @@ -149,7 +149,7 @@ TEST_F(LCIStrategicTestFixture, planManeuverCb) EXPECT_EQ(289.0, resp->new_plan.maneuvers[0].stop_and_wait_maneuver.start_dist ); EXPECT_NEAR(6.0, rclcpp::Time(resp->new_plan.maneuvers[0].stop_and_wait_maneuver.start_time).seconds(), 0.02); EXPECT_NEAR(0.0, resp->new_plan.maneuvers[0].stop_and_wait_maneuver.start_speed, 0.02); - EXPECT_NEAR((rclcpp::Time(1e9 * 6.0) + rclcpp::Duration(config.min_maneuver_planning_period * 1e9)).seconds(), rclcpp::Time(resp->new_plan.maneuvers[0].stop_and_wait_maneuver.end_time).seconds(), 0.001); + EXPECT_NEAR((rclcpp::Time(1e9 * 6.0) + rclcpp::Duration::from_nanoseconds(config.min_maneuver_planning_period * 1e9)).seconds(), rclcpp::Time(resp->new_plan.maneuvers[0].stop_and_wait_maneuver.end_time).seconds(), 0.001); EXPECT_NEAR(300.0, resp->new_plan.maneuvers[0].stop_and_wait_maneuver.end_dist, 0.0001); EXPECT_TRUE(resp->new_plan.maneuvers[0].stop_and_wait_maneuver.starting_lane_id.compare("1201") == 0); EXPECT_TRUE(resp->new_plan.maneuvers[0].stop_and_wait_maneuver.ending_lane_id.compare("1201") == 0); diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp index 7dbbe68dab..0ead45bc04 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp @@ -77,7 +77,7 @@ namespace light_controlled_intersection_tactical_plugin current_lanelet = llt_on_route_optional.value(); } else{ - RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "When identifying the corresponding lanelet for requested trajectory plan's state, x: " + RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "When identifying the corresponding lanelet for requested trajectory plan's state, x: " << req->vehicle_state.x_pos_global << ", y: " << req->vehicle_state.y_pos_global << ", no possible lanelet was found to be on the shortest path." << "Picking arbitrary lanelet: " << current_lanelets[0].id() << ", instead"); current_lanelet = current_lanelets[0]; @@ -146,7 +146,7 @@ namespace light_controlled_intersection_tactical_plugin && last_case_.get() == new_case && is_new_case_successful == true && last_trajectory_.trajectory_points.size() >= 2 - && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time) > rclcpp::Time(req->header.stamp) + rclcpp::Duration(1 * 1e9)) // Duration is in nanoseconds + && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time) > rclcpp::Time(req->header.stamp) + rclcpp::Duration::from_nanoseconds(1 * 1e9)) // Duration is in nanoseconds { resp->trajectory_plan = last_trajectory_; RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Last TRAJ's target time: " << std::to_string(rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds()) << ", and stamp:" << std::to_string(rclcpp::Time(req->header.stamp).seconds())); diff --git a/localization_manager/src/LocalizationManager.cpp b/localization_manager/src/LocalizationManager.cpp index 6b554e583e..10eddd2b44 100644 --- a/localization_manager/src/LocalizationManager.cpp +++ b/localization_manager/src/LocalizationManager.cpp @@ -14,7 +14,7 @@ * the License. */ -#include +#include #include #include "localization_manager/LocalizationManager.hpp" @@ -242,14 +242,14 @@ namespace localization_manager prev_ndt_stamp_ = boost::none; current_timer_id_ = nextId(); - current_timer_ = timer_factory_->buildTimer(current_timer_id_, rclcpp::Duration(config_.auto_initialization_timeout * 1e6), + current_timer_ = timer_factory_->buildTimer(current_timer_id_, rclcpp::Duration::from_nanoseconds(config_.auto_initialization_timeout * 1e6), std::bind(&LocalizationManager::timerCallback, this, new_state), true, true); timers_[current_timer_id_] = std::make_pair(std::move(current_timer_), false); // Add start timer to map by Id break; case LocalizationState::DEGRADED_NO_LIDAR_FIX: current_timer_id_ = nextId(); - current_timer_ = timer_factory_->buildTimer(current_timer_id_, rclcpp::Duration(config_.gnss_only_operation_timeout * 1e6), + current_timer_ = timer_factory_->buildTimer(current_timer_id_, rclcpp::Duration::from_nanoseconds(config_.gnss_only_operation_timeout * 1e6), std::bind(&LocalizationManager::timerCallback, this, new_state), true, true); timers_[current_timer_id_] = std::make_pair(std::move(current_timer_), false); // Add start timer to map by Id @@ -281,7 +281,7 @@ namespace localization_manager } // check if last gnss time stamp is older than threshold and send the corresponding signal - if (last_raw_gnss_value_ && timer_factory_->now() - rclcpp::Time(last_raw_gnss_value_->header.stamp, timer_clock_type_) > rclcpp::Duration(config_.gnss_data_timeout * 1e6)) + if (last_raw_gnss_value_ && timer_factory_->now() - rclcpp::Time(last_raw_gnss_value_->header.stamp, timer_clock_type_) > rclcpp::Duration::from_nanoseconds(config_.gnss_data_timeout * 1e6)) { transition_table_.signal(LocalizationSignal::GNSS_DATA_TIMEOUT); } @@ -318,4 +318,4 @@ namespace localization_manager return transition_table_.getState(); } -} \ No newline at end of file +} diff --git a/mobilitypath_visualizer/src/mobilitypath_visualizer.cpp b/mobilitypath_visualizer/src/mobilitypath_visualizer.cpp index 8585503677..09f6586ac0 100644 --- a/mobilitypath_visualizer/src/mobilitypath_visualizer.cpp +++ b/mobilitypath_visualizer/src/mobilitypath_visualizer.cpp @@ -229,7 +229,7 @@ namespace mobilitypath_visualizer { marker.id = i; rclcpp::Time marker_cur_time(marker.header.stamp); //Update time by 0.1s - rclcpp::Time updated_time = marker_cur_time + rclcpp::Duration(0.1 * 1e9); + rclcpp::Time updated_time = marker_cur_time + rclcpp::Duration::from_nanoseconds(0.1 * 1e9); marker.header.stamp = builtin_interfaces::msg::Time(updated_time); if (i >= msg.trajectory.offsets.size()) { // If we need to delete previous points @@ -294,7 +294,7 @@ namespace mobilitypath_visualizer { marker.color.b = 1.0; marker.color.a = 1.0; marker.frame_locked = true; - marker.lifetime = builtin_interfaces::msg::Duration((rclcpp::Duration(config_.t * 1e9))); + marker.lifetime = builtin_interfaces::msg::Duration((rclcpp::Duration::from_nanoseconds(config_.t * 1e9))); for (auto const& cav_marker: cav_markers) { @@ -367,7 +367,7 @@ namespace mobilitypath_visualizer { rclcpp::Time host_marker_stamp(host_marker.markers[0].header.stamp); - if (curr_cav_marker_stamp + rclcpp::Duration(time_step * 1e9) < host_marker_stamp){ + if (curr_cav_marker_stamp + rclcpp::Duration::from_nanoseconds(time_step * 1e9) < host_marker_stamp){ continue; } diff --git a/mobilitypath_visualizer/test/test_mobility_path_visualizer.cpp b/mobilitypath_visualizer/test/test_mobility_path_visualizer.cpp index 77a8211bd4..c7ce53adb2 100644 --- a/mobilitypath_visualizer/test/test_mobility_path_visualizer.cpp +++ b/mobilitypath_visualizer/test/test_mobility_path_visualizer.cpp @@ -28,27 +28,27 @@ TEST(MobilityPathVisualizerTest, TestComposeVisualizationMarker) auto viz_node = std::make_shared(options); viz_node->configure(); viz_node->activate(); - + // 1 to 1 transform std::string base_proj = lanelet::projection::LocalFrameProjector::ECEF_PROJ_STR; std_msgs::msg::String msg; msg.data = base_proj; std::unique_ptr msg_ptr (new std_msgs::msg::String(msg)); viz_node->georeferenceCallback(std::move(msg_ptr)); // Set projection - + // INPUT MSG carma_v2x_msgs::msg::MobilityPath input_msg; input_msg.m_header.plan_id = ""; input_msg.m_header.sender_id = ""; //host input_msg.m_header.timestamp = 10000; // 10sec - + input_msg.trajectory.location.ecef_x = 0; input_msg.trajectory.location.ecef_y = 0; input_msg.trajectory.location.ecef_z = 0; carma_v2x_msgs::msg::LocationOffsetECEF offset; offset.offset_x = 100; - input_msg.trajectory.offsets.push_back(offset); + input_msg.trajectory.offsets.push_back(offset); input_msg.trajectory.offsets.push_back(offset); // actual positions now 0, 1, 2 -> 2 markers mobilitypath_visualizer::MarkerColor expected_color_blue; @@ -72,17 +72,17 @@ TEST(MobilityPathVisualizerTest, TestComposeVisualizationMarker) marker.color.g = expected_color_blue.green; marker.color.b = expected_color_blue.blue; marker.color.a = 1.0f; - + // EXPECTED MSG DYNAMIC marker.id = 0; geometry_msgs::msg::Point point; - point.x = 0; + point.x = 0; marker.points.push_back(point); point.x = 1; //in meters marker.points.push_back(point); expected_msg.markers.push_back(marker); - marker.header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(10.1*1e9)); + marker.header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(10.1*1e9)); marker.id = 1; marker.points = {}; point.x = 1; @@ -99,23 +99,23 @@ TEST(MobilityPathVisualizerTest, TestComposeVisualizationMarker) EXPECT_EQ(expected_msg.markers[0].type, result.markers[0].type); EXPECT_EQ(expected_msg.markers[0].action, result.markers[0].action); EXPECT_EQ(expected_msg.markers[0].ns, result.markers[0].ns); - + EXPECT_EQ(expected_msg.markers[0].color.a, result.markers[0].color.a); EXPECT_EQ(expected_msg.markers[0].color.b, result.markers[0].color.b); EXPECT_EQ(expected_msg.markers[0].color.g, result.markers[0].color.g); EXPECT_EQ(expected_msg.markers[0].color.r, result.markers[0].color.r); - + EXPECT_EQ(expected_msg.markers[0].points[0].x, result.markers[0].points[0].x); EXPECT_EQ(expected_msg.markers[0].points[0].y, result.markers[0].points[0].y); EXPECT_EQ(expected_msg.markers[0].points[0].z, result.markers[0].points[0].z); - + EXPECT_EQ(expected_msg.markers[0].points[1].x, result.markers[0].points[1].x); EXPECT_EQ(expected_msg.markers[0].points[1].y, result.markers[0].points[1].y); EXPECT_EQ(expected_msg.markers[0].points[1].z, result.markers[0].points[1].z); EXPECT_EQ(expected_msg.markers[1].points[0].x, result.markers[1].points[0].x); EXPECT_EQ(expected_msg.markers[1].points[1].x, result.markers[1].points[1].x); - + } TEST(MobilityPathVisualizerTest, TestECEFToMapPoint) @@ -161,26 +161,26 @@ TEST(MobilityPathVisualizerTest, TestMatchTrajectoryTimestamps) msg.data = base_proj; std::unique_ptr msg_ptr(new std_msgs::msg::String(msg)); viz_node->georeferenceCallback(std::move(msg_ptr)); // Set projection - + // INPUT RESULT STATIC INFO visualization_msgs::msg::MarkerArray host_msg; visualization_msgs::msg::Marker marker; - marker.header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(10*1e9)); //10sec + marker.header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(10*1e9)); //10sec marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; marker.ns = "mobilitypath_visualizer"; - + // INPUT MSG DYNAMIC marker.id = 0; geometry_msgs::msg::Point point; - point.x = 0; + point.x = 0; marker.points.push_back(point); point.x = 1; //in meters marker.points.push_back(point); host_msg.markers.push_back(marker); - marker.header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(10.1*1e9)); - + marker.header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(10.1*1e9)); + marker.id = 1; marker.points = {}; point.x = 1; @@ -192,23 +192,23 @@ TEST(MobilityPathVisualizerTest, TestMatchTrajectoryTimestamps) // Base test // Give CAV Exactly same as Host Marker itself - + auto result = viz_node->matchTrajectoryTimestamps(host_msg, {host_msg}); - + EXPECT_EQ(result[0].markers[0].header.stamp.sec, host_msg.markers[0].header.stamp.sec); EXPECT_EQ(result[0].markers[0].id, host_msg.markers[0].id); EXPECT_EQ(result[0].markers[0].points[0].x, host_msg.markers[0].points[0].x); EXPECT_EQ(result[0].markers[0].points[0].y, host_msg.markers[0].points[0].y); EXPECT_EQ(result[0].markers[0].points[1].x, host_msg.markers[0].points[1].x); EXPECT_EQ(result[0].markers[0].points[1].y, host_msg.markers[0].points[1].y); - + EXPECT_EQ(result[0].markers[1].header.stamp.sec, host_msg.markers[1].header.stamp.sec); EXPECT_EQ(result[0].markers[1].id, host_msg.markers[1].id); EXPECT_EQ(result[0].markers[1].points[0].x, host_msg.markers[1].points[0].x); EXPECT_EQ(result[0].markers[1].points[0].y, host_msg.markers[1].points[0].y); EXPECT_EQ(result[0].markers[1].points[1].x, host_msg.markers[1].points[1].x); EXPECT_EQ(result[0].markers[1].points[1].y, host_msg.markers[1].points[1].y); - + visualization_msgs::msg::MarkerArray cav_msg; cav_msg = host_msg; @@ -217,56 +217,56 @@ TEST(MobilityPathVisualizerTest, TestMatchTrajectoryTimestamps) cav_msg.markers[1].header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(10.06 * 1e9)); result = viz_node->matchTrajectoryTimestamps(host_msg, {cav_msg}); - + visualization_msgs::msg::MarkerArray expected_msg; expected_msg = host_msg; - + expected_msg.markers[0].points[0].x = 0.4; expected_msg.markers[0].points[1].x = 1.4; expected_msg.markers[1].points[0].x = 1.4; expected_msg.markers[1].points[1].x = 2.4; - + EXPECT_EQ(expected_msg.markers[0].header.stamp.sec, result[0].markers[0].header.stamp.sec); EXPECT_EQ(expected_msg.markers[0].id, result[0].markers[0].id); EXPECT_EQ(expected_msg.markers[0].points[0].x, result[0].markers[0].points[0].x); EXPECT_EQ(expected_msg.markers[0].points[1].x, result[0].markers[0].points[1].x); - + EXPECT_EQ(expected_msg.markers[1].header.stamp.sec, result[0].markers[1].header.stamp.sec); EXPECT_EQ(expected_msg.markers[1].id, result[0].markers[1].id); EXPECT_EQ(expected_msg.markers[1].points[0].x, result[0].markers[1].points[0].x); EXPECT_EQ(expected_msg.markers[1].points[1].x, result[0].markers[1].points[1].x); - + // 1 OUT OF 2 POINTS DROPPED cav_msg.markers[0].header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(9.86 * 1e9)); cav_msg.markers[1].header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(9.96 * 1e9)); - + result = viz_node->matchTrajectoryTimestamps(host_msg, {cav_msg}); - + expected_msg.markers[0].id = 1; expected_msg.markers[0].header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(10.0* 1e9)); expected_msg.markers[0].points[0].x = 1.4; expected_msg.markers[0].points[1].x = 2.4; - + EXPECT_EQ(expected_msg.markers[0].header.stamp, result[0].markers[0].header.stamp); EXPECT_EQ(expected_msg.markers[0].id, result[0].markers[0].id); EXPECT_EQ(expected_msg.markers[0].points[0].x, result[0].markers[0].points[0].x); EXPECT_EQ(expected_msg.markers[0].points[1].x, result[0].markers[0].points[1].x); - + // ALL POINTS DROPPED OUTDATED cav_msg.markers[0].header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(9.66* 1e9)); cav_msg.markers[1].header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(9.76* 1e9)); - + result = viz_node->matchTrajectoryTimestamps(host_msg, {cav_msg}); - - EXPECT_EQ(0, result.size()); + + EXPECT_EQ(0, result.size()); // ALL POINTS DROPPED TOO FUTURE cav_msg.markers[0].header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(11.66 * 1e9)); cav_msg.markers[1].header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(12.76 * 1e9)); - + result = viz_node->matchTrajectoryTimestamps(host_msg, {cav_msg}); - - EXPECT_EQ(0, result.size()); + + EXPECT_EQ(0, result.size()); } TEST(MobilityPathVisualizerTest, TestComposeLabelMarker) @@ -275,25 +275,25 @@ TEST(MobilityPathVisualizerTest, TestComposeLabelMarker) auto viz_node = std::make_shared(options); viz_node->configure(); viz_node->activate(); - + // INPUT RESULT STATIC INFO visualization_msgs::msg::MarkerArray host_msg; visualization_msgs::msg::Marker marker; - marker.header.stamp = builtin_interfaces::msg::Time(viz_node->now() + rclcpp::Duration(10.0 * 10e9)); //10sec + marker.header.stamp = builtin_interfaces::msg::Time(viz_node->now() + rclcpp::Duration::from_nanoseconds(10.0 * 10e9)); //10sec marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; marker.ns = "mobilitypath_visualizer"; - + // INPUT MSG DYNAMIC marker.id = 0; geometry_msgs::msg::Point point; - point.x = 0; + point.x = 0; marker.points.push_back(point); point.x = 1; //in meters marker.points.push_back(point); host_msg.markers.push_back(marker); - marker.header.stamp.sec += rclcpp::Duration(0.1,0.0).seconds(); + marker.header.stamp.sec += rclcpp::Duration(0.1,0.0).seconds(); marker.id = 1; marker.points = {}; point.x = 1; @@ -306,9 +306,9 @@ TEST(MobilityPathVisualizerTest, TestComposeLabelMarker) // Base test // Give CAV Exactly same as Host Marker itself // Collision in every seconds - + auto result = viz_node->composeLabelMarker(host_msg, {host_msg}); - + //TEST STATIC EXPECT_EQ(result.markers[0].header.frame_id, "map"); EXPECT_EQ(result.markers[0].type, visualization_msgs::msg::Marker::TEXT_VIEW_FACING); @@ -321,7 +321,7 @@ TEST(MobilityPathVisualizerTest, TestComposeLabelMarker) EXPECT_EQ(result.markers.size(), host_msg.markers.size() + 1); EXPECT_NEAR(result.markers[0].pose.position.x, host_msg.markers[0].points[0].x,0.001); EXPECT_NEAR(result.markers[0].pose.position.y, host_msg.markers[0].points[0].y,0.001); - + EXPECT_NEAR(result.markers[1].pose.position.x, host_msg.markers[1].points[0].x,0.001); EXPECT_NEAR(result.markers[1].pose.position.y, host_msg.markers[1].points[0].y,0.001); @@ -329,7 +329,7 @@ TEST(MobilityPathVisualizerTest, TestComposeLabelMarker) EXPECT_NEAR(result.markers[2].pose.position.y, host_msg.markers[1].points[1].y,0.001); EXPECT_NEAR(result.markers[2].header.stamp.sec, host_msg.markers[1].header.stamp.sec + 0.1,0.1); - + } // Run all the tests @@ -346,4 +346,4 @@ int main(int argc, char **argv) rclcpp::shutdown(); return success; -} \ No newline at end of file +} diff --git a/motion_computation/include/motion_computation/motion_computation_node.hpp b/motion_computation/include/motion_computation/motion_computation_node.hpp index 1f6ee2fe68..8be096c836 100644 --- a/motion_computation/include/motion_computation/motion_computation_node.hpp +++ b/motion_computation/include/motion_computation/motion_computation_node.hpp @@ -16,7 +16,7 @@ #define MOTION_COMPUTATION__MOTION_COMPUTATION_NODE_HPP_ #include -#include +#include #include #include diff --git a/motion_computation/include/motion_computation/motion_computation_worker.hpp b/motion_computation/include/motion_computation/motion_computation_worker.hpp index 23d44be437..35304655b6 100644 --- a/motion_computation/include/motion_computation/motion_computation_worker.hpp +++ b/motion_computation/include/motion_computation/motion_computation_worker.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include diff --git a/motion_computation/src/bsm_to_external_object_convertor.cpp b/motion_computation/src/bsm_to_external_object_convertor.cpp index 6a68d44136..1a414cacc1 100644 --- a/motion_computation/src/bsm_to_external_object_convertor.cpp +++ b/motion_computation/src/bsm_to_external_object_convertor.cpp @@ -201,7 +201,7 @@ void convert( out_msg.predictions = impl::predicted_poses_to_predicted_state( predicted_poses, out_msg.velocity.twist.linear.x, rclcpp::Time(out_msg.header.stamp), - rclcpp::Duration(pred_step_size * 1e9), map_frame_id, out_msg.confidence, out_msg.confidence); + rclcpp::Duration::from_nanoseconds(pred_step_size * 1e9), map_frame_id, out_msg.confidence, out_msg.confidence); out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::PREDICTION_PRESENCE_VECTOR; } diff --git a/motion_computation/src/psm_to_external_object_convertor.cpp b/motion_computation/src/psm_to_external_object_convertor.cpp index ed29359f09..a95474c806 100644 --- a/motion_computation/src/psm_to_external_object_convertor.cpp +++ b/motion_computation/src/psm_to_external_object_convertor.cpp @@ -14,7 +14,7 @@ #include #include -#include +#include #include #include @@ -227,7 +227,7 @@ void convert( out_msg.predictions = impl::predicted_poses_to_predicted_state( predicted_poses, out_msg.velocity.twist.linear.x, rclcpp::Time(out_msg.header.stamp), - rclcpp::Duration(pred_step_size * 1e9), map_frame_id, out_msg.confidence, out_msg.confidence); + rclcpp::Duration::from_nanoseconds(pred_step_size * 1e9), map_frame_id, out_msg.confidence, out_msg.confidence); out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::PREDICTION_PRESENCE_VECTOR; } diff --git a/motion_computation/test/MotionComputationTest.cpp b/motion_computation/test/MotionComputationTest.cpp index 6304c44892..da0dbe2d5c 100644 --- a/motion_computation/test/MotionComputationTest.cpp +++ b/motion_computation/test/MotionComputationTest.cpp @@ -187,7 +187,7 @@ TEST(MotionComputationWorker, ComposePredictedState) tf2::Vector3 prev = {4, 0, 0}; auto res = conversion::impl::composePredictedState( - curr, prev, time_stamp, time_stamp + rclcpp::Duration(100000000), 0.0); + curr, prev, time_stamp, time_stamp + rclcpp::Duration::from_nanoseconds(100000000), 0.0); auto test_result = std::get<0>(res); ASSERT_NEAR(test_result.predicted_position.position.x, 4.0, 0.0001); @@ -202,7 +202,7 @@ TEST(MotionComputationWorker, ComposePredictedState) prev = {0, 0, 0}; res = conversion::impl::composePredictedState( - curr, prev, time_stamp, time_stamp + rclcpp::Duration(100000000), std::get<1>(res)); + curr, prev, time_stamp, time_stamp + rclcpp::Duration::from_nanoseconds(100000000), std::get<1>(res)); test_result = std::get<0>(res); ASSERT_NEAR(test_result.predicted_position.position.x, 0.0, 0.0001); diff --git a/object_detection_tracking/include/object_detection_tracking_worker.h b/object_detection_tracking/include/object_detection_tracking_worker.h index b743cf783f..62e698815c 100644 --- a/object_detection_tracking/include/object_detection_tracking_worker.h +++ b/object_detection_tracking/include/object_detection_tracking_worker.h @@ -26,7 +26,7 @@ #include #include #include -#include +#include #include #include #include @@ -55,7 +55,7 @@ class ObjectDetectionTrackingWorker */ ObjectDetectionTrackingWorker(PublishObjectCallback obj_pub, TransformLookupCallback tf_lookup, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger); - + /*! \fn detectedObjectCallback(const autoware_auto_msgs::autoware_msgs &msg) \brief detectedObjectCallback populates detected object along with its velocity,yaw, yaw_rate and static/dynamic class to DetectedObject message. \param msg array of detected objects. @@ -66,7 +66,7 @@ class ObjectDetectionTrackingWorker // Setters for the parameters void setMapFrame(std::string map_frame); - + private: // local copy of external object publihsers @@ -82,16 +82,16 @@ class ObjectDetectionTrackingWorker // Logger interface rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_; - + /** * \brief Helper method to check if the provided tracked object has the provided class type - * + * * \param obj The object to check * \param class_id The class type to check - * + * * \return true if the object has the class type - */ + */ bool isClass(const autoware_auto_msgs::msg::TrackedObject& obj, uint8_t class_id); }; diff --git a/object_detection_tracking/src/covariance_helper.h b/object_detection_tracking/src/covariance_helper.h index 35b09485c7..bafd55d992 100644 --- a/object_detection_tracking/src/covariance_helper.h +++ b/object_detection_tracking/src/covariance_helper.h @@ -29,25 +29,25 @@ // POSSIBILITY OF SUCH DAMAGE. /** - * + * * Modifications (c) Leidos 2021 * - Moved to new namespace. - * - */ + * + */ -#include +#include namespace covariance_helper { -/** - * NOTE: The function is a direct copy from +/** + * NOTE: The function is a direct copy from * https://github.com/ros2/geometry2/blob/16562cee8694c11f1f82b2bdbde2814fca1c7954/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp#L422 - * + * * This bit of logic is normally included in the tf2_geometry_msgs packages, but it has not been backported to ROS2 Foxy * This means the current doTransform operation for PoseWithCovarianceStamped does not result in the correct covariance values * This function should be called after a call to doTransform to get the correct transformed covariance - * + * * \brief Transform the covariance matrix of a PoseWithCovariance message to a new frame. * \param cov_in The covariance matrix to transform. * \param transform The transform to apply, as a tf2::Transform structure. @@ -146,4 +146,4 @@ geometry_msgs::msg::PoseWithCovariance::_covariance_type transformCovariance( return cov_out; } -} // covariance_helper \ No newline at end of file +} // covariance_helper diff --git a/object_detection_tracking/src/object_detection_tracking_worker.cpp b/object_detection_tracking/src/object_detection_tracking_worker.cpp index 6cba3cacfe..b1890212da 100644 --- a/object_detection_tracking/src/object_detection_tracking_worker.cpp +++ b/object_detection_tracking/src/object_detection_tracking_worker.cpp @@ -15,7 +15,7 @@ */ #include "object_detection_tracking_worker.h" #include -#include +#include #include #include #include @@ -25,12 +25,12 @@ namespace object { -ObjectDetectionTrackingWorker::ObjectDetectionTrackingWorker(PublishObjectCallback obj_pub, TransformLookupCallback tf_lookup, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger) +ObjectDetectionTrackingWorker::ObjectDetectionTrackingWorker(PublishObjectCallback obj_pub, TransformLookupCallback tf_lookup, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger) : obj_pub_(obj_pub), tf_lookup_(tf_lookup), logger_(logger) {} bool ObjectDetectionTrackingWorker::isClass(const autoware_auto_msgs::msg::TrackedObject& obj, uint8_t class_id) { - return obj.classification.end() != std::find_if(obj.classification.begin(), obj.classification.end(), + return obj.classification.end() != std::find_if(obj.classification.begin(), obj.classification.end(), [&class_id](auto o){ return o.classification == class_id; } ); @@ -51,7 +51,7 @@ void ObjectDetectionTrackingWorker::detectedObjectCallback(autoware_auto_msgs::m return; } - geometry_msgs::msg::TransformStamped object_frame_tf = transform.get(); + geometry_msgs::msg::TransformStamped object_frame_tf = transform.get(); for (size_t i = 0; i < obj_array->objects.size(); i++) { @@ -70,7 +70,7 @@ void ObjectDetectionTrackingWorker::detectedObjectCallback(autoware_auto_msgs::m obj.presence_vector = obj.presence_vector | obj.OBJECT_TYPE_PRESENCE_VECTOR; obj.presence_vector = obj.presence_vector | obj.DYNAMIC_OBJ_PRESENCE; obj.presence_vector = obj.presence_vector | obj.CONFIDENCE_PRESENCE_VECTOR; - + // Object id. Matching ids on a topic should refer to the same object within some time period, expanded obj.id = obj_array->objects[i].object_id; @@ -100,7 +100,7 @@ void ObjectDetectionTrackingWorker::detectedObjectCallback(autoware_auto_msgs::m auto zz = obj_array->objects[i].kinematics.position_covariance[8]; // This matrix represents the covariance of the object before transformation - std::array input_covariance = { + std::array input_covariance = { xx, xy, xz, 0, 0, 0, yx, yy, yz, 0, 0, 0, zx, zy, zz, 0, 0, 0, @@ -133,7 +133,7 @@ void ObjectDetectionTrackingWorker::detectedObjectCallback(autoware_auto_msgs::m for(auto shape : obj_array->objects[i].shape) { for (auto point : shape.polygon.points) { - + if (point.x > maxX) maxX = point.x; @@ -157,7 +157,7 @@ void ObjectDetectionTrackingWorker::detectedObjectCallback(autoware_auto_msgs::m obj.size.y = dY / 2.0; // Height provided by autoware is overall height divide by 2 for delta from centroid - obj.size.z = maxHeight / 2.0; + obj.size.z = maxHeight / 2.0; // Update the object type and generate predictions using CV or CTRV vehicle models. // If the object is a bicycle or motor vehicle use CTRV otherwise use CV. diff --git a/plan_delegator/include/plan_delegator.hpp b/plan_delegator/include/plan_delegator.hpp index 1803612700..9ed7c8bcfa 100644 --- a/plan_delegator/include/plan_delegator.hpp +++ b/plan_delegator/include/plan_delegator.hpp @@ -29,7 +29,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/platooning_control/test/test_pure.cpp b/platooning_control/test/test_pure.cpp index d2e03cd0c7..e976505046 100644 --- a/platooning_control/test/test_pure.cpp +++ b/platooning_control/test/test_pure.cpp @@ -58,7 +58,7 @@ TEST(PurePursuitTest, sanity_check) state_tf.state.y = 0; state_tf.state.longitudinal_velocity_mps = 4.0; //arbitrary speed for first point plan.header.frame_id = state_tf.header.frame_id; - plan.header.stamp = rclcpp::Time(converted_time_now*1e9) + rclcpp::Duration(1.0*1e9); + plan.header.stamp = rclcpp::Time(converted_time_now*1e9) + rclcpp::Duration::from_nanoseconds(1.0*1e9); plan.trajectory_points = { tpp, tpp2, tpp3 }; @@ -79,4 +79,4 @@ TEST(PurePursuitTest, sanity_check) ASSERT_NEAR(converted_cmd.cmd.linear_velocity, speed_cmd, 0.01); ASSERT_NEAR(converted_cmd.cmd.steering_angle, cmd.front_wheel_angle_rad, 0.001); -} \ No newline at end of file +} diff --git a/platooning_strategic_ihp/include/platoon_strategic_ihp/platoon_strategic_ihp.h b/platooning_strategic_ihp/include/platoon_strategic_ihp/platoon_strategic_ihp.h index ae9b880446..02e6ed5a31 100644 --- a/platooning_strategic_ihp/include/platoon_strategic_ihp/platoon_strategic_ihp.h +++ b/platooning_strategic_ihp/include/platoon_strategic_ihp/platoon_strategic_ihp.h @@ -51,7 +51,7 @@ #include "platoon_manager_ihp.h" #include #include -#include +#include #include #include #include diff --git a/platooning_strategic_ihp/src/platoon_strategic_ihp.cpp b/platooning_strategic_ihp/src/platoon_strategic_ihp.cpp index 6d5c7ae000..b5bf640c3f 100644 --- a/platooning_strategic_ihp/src/platoon_strategic_ihp.cpp +++ b/platooning_strategic_ihp/src/platoon_strategic_ihp.cpp @@ -3137,7 +3137,7 @@ namespace platooning_strategic_ihp maneuver_msg.lane_following_maneuver.end_speed = target_speed; // because it is a rough plan, assume vehicle can always reach to the target speed in a lanelet - maneuver_msg.lane_following_maneuver.end_time = current_time + rclcpp::Duration(config_.time_step*1e9); + maneuver_msg.lane_following_maneuver.end_time = current_time + rclcpp::Duration::from_nanoseconds(config_.time_step*1e9); maneuver_msg.lane_following_maneuver.lane_ids = { std::to_string(lane_id) }; RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_strategic_ihp"), "in compose maneuver lane id:"<< lane_id); @@ -3183,12 +3183,12 @@ namespace platooning_strategic_ihp double cur_plus_target = current_speed + target_speed; if (cur_plus_target < 0.00001) { - maneuver_msg.lane_change_maneuver.end_time = current_time + rclcpp::Duration(config_.time_step*1e9); + maneuver_msg.lane_change_maneuver.end_time = current_time + rclcpp::Duration::from_nanoseconds(config_.time_step*1e9); } else { - // maneuver_msg.lane_change_maneuver.end_time = current_time + rclcpp::Duration((end_dist - current_dist) / (0.5 * cur_plus_target)); - maneuver_msg.lane_change_maneuver.end_time = current_time + rclcpp::Duration(20.0*1e9); + // maneuver_msg.lane_change_maneuver.end_time = current_time + rclcpp::Duration::from_nanoseconds((end_dist - current_dist) / (0.5 * cur_plus_target)); + maneuver_msg.lane_change_maneuver.end_time = current_time + rclcpp::Duration::from_nanoseconds(20.0*1e9); } diff --git a/route/include/route/route_generator_worker.hpp b/route/include/route/route_generator_worker.hpp index 375f01bb7a..089a5f738d 100644 --- a/route/include/route/route_generator_worker.hpp +++ b/route/include/route/route_generator_worker.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include "route/route_state_worker.hpp" @@ -72,7 +72,7 @@ namespace route { * \brief setReroutingChecker function to set the rerouting flag */ void setReroutingChecker(const std::function inputFunction); - + /** * \brief Dependency injection for world model pointer. * \param wm CARMA world model object providing lanelet vector map and traffic regulations @@ -113,10 +113,10 @@ namespace route { bool getAvailableRouteCb(const std::shared_ptr, const std::shared_ptr, std::shared_ptr resp); - + /** * \brief Set_active_route service callback. User can select a route to start following by calling this service - * \param req A carma_planning_msgs::srv::SetActiveRoute::Request msg which contains either a route name a user wants to select or + * \param req A carma_planning_msgs::srv::SetActiveRoute::Request msg which contains either a route name a user wants to select or * an array of carma_v2x_msgs::msg::Position3D destination points to generate a route from. * \param resp A carma_planning_msgs::srv::SetActiveRoute::Response msg contains error status indicating whether the routing succeeded */ @@ -147,7 +147,7 @@ namespace route { /** * \brief Callback for the georeference subscriber used to set the map projection * \param msg The latest georeference - */ + */ void georeferenceCb(std_msgs::msg::String::UniquePtr msg); /** @@ -173,7 +173,7 @@ namespace route { const carma_ros2_utils::PubPtr& route_state_pub, const carma_ros2_utils::PubPtr& route_pub, const carma_ros2_utils::PubPtr& route_marker_pub); - + /** * \brief Helper function to check whether a route's shortest path contains any duplicate Lanelet IDs. * 'true' indicates that the route's shortest path contains duplicate Lanelet IDs. @@ -213,7 +213,7 @@ namespace route { /** * \brief crosstrackErrorCheck is a function that determines when the vehicle has left the route and reports when a crosstrack error has * taken place - * + * * \param msg Msg that contains the vehicle's current position * \param current_llt The lanelet that the vehicle is currently in * */ @@ -221,22 +221,22 @@ namespace route { /** * \brief set the crosstrack error counter maximum limit - * + * * \param cte_max the maximum amount of acceptable crosstrack error instances */ void setCrosstrackErrorCountMax(int cte_max); /** * \brief set the maximum crosstrack error distance - * + * * \param cte_dist maximum distance value (specified in the this package's parameters.yaml configuration file) */ void setCrosstrackErrorDistance(double cte_dist); /** - * \brief "Get the closest lanelet on the route relative to the vehicle's current position. + * \brief "Get the closest lanelet on the route relative to the vehicle's current position. * If the input list does not contain lanelets on the route, still closest lanelet from the route will be returned - * + * * \param position the current position of the vehicle */ lanelet::ConstLanelet getClosestLaneletFromRouteLanelets(lanelet::BasicPoint2d position) const; @@ -262,7 +262,7 @@ namespace route { private: const double DEG_TO_RAD = 0.0174533; - + // route state worker RouteStateWorker rs_worker_; @@ -278,11 +278,11 @@ namespace route { carma_planning_msgs::msg::RouteState route_state_msg_; visualization_msgs::msg::Marker route_marker_msg_; - std::vector points_; - + std::vector points_; + //List of lanelets in the route lanelet::ConstLanelets route_llts; - + // minimum down track error which can trigger route complete event double down_track_target_range_; @@ -324,7 +324,7 @@ namespace route { std::queue route_event_queue; // private helper function to add a new route event into event queue - void publishRouteEvent(uint8_t event_type); + void publishRouteEvent(uint8_t event_type); // maximum cross track error which can trigger left route event double cross_track_dist_; @@ -353,4 +353,4 @@ namespace route { rclcpp::Clock::SharedPtr clock_; }; -} // namespace route \ No newline at end of file +} // namespace route diff --git a/route/src/route_generator_worker.cpp b/route/src/route_generator_worker.cpp index fc001ebf35..633ee3f294 100644 --- a/route/src/route_generator_worker.cpp +++ b/route/src/route_generator_worker.cpp @@ -20,7 +20,7 @@ namespace route { RouteGeneratorWorker::RouteGeneratorWorker(tf2_ros::Buffer& tf2_buffer) : tf2_buffer_(tf2_buffer) {} - + void RouteGeneratorWorker::setWorldModelPtr(carma_wm::WorldModelConstPtr wm) { this->world_model_ = wm; @@ -76,14 +76,14 @@ namespace route { bool RouteGeneratorWorker::getAvailableRouteCb(const std::shared_ptr, const std::shared_ptr, std::shared_ptr resp) - { + { // Return if the the directory specified by route_file_path_ does not exist if(!boost::filesystem::exists(boost::filesystem::path(this->route_file_path_))) { RCLCPP_ERROR_STREAM(logger_->get_logger(), "No directory exists at " << route_file_path_); return true; } - + // Read all route files in the given directory boost::filesystem::directory_iterator end_point; for(boost::filesystem::directory_iterator itr(boost::filesystem::path(this->route_file_path_)); itr != end_point; ++itr) @@ -99,9 +99,9 @@ namespace route { // Skip if '.csv' is not found in the file name if(full_file_name.find(".csv") == std::string::npos) - { + { continue; - } + } // Assume route files ending with ".csv", before that is the actual route name route_msg.route_id = full_file_name.substr(0, full_file_name.find(".csv")); @@ -116,7 +116,7 @@ namespace route { if(temp != "") dest_name = temp; } fin.close(); - } + } else { RCLCPP_ERROR_STREAM(logger_->get_logger(), "File open failed..."); @@ -128,9 +128,9 @@ namespace route { resp->available_routes.push_back(move(route_msg)); } } - + //after route path object is available to select, worker will able to transit state and provide route selection service - if(this->rs_worker_.getRouteState() == RouteStateWorker::RouteState::LOADING) + if(this->rs_worker_.getRouteState() == RouteStateWorker::RouteState::LOADING) { this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_LOADED); publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_LOADED); @@ -150,7 +150,7 @@ namespace route { bool RouteGeneratorWorker::setActiveRouteCb(const std::shared_ptr, const std::shared_ptr req, std::shared_ptr resp) - { + { // only allow a new route to be activated in route selection state if(this->rs_worker_.getRouteState() != RouteStateWorker::RouteState::SELECTION) { @@ -161,7 +161,7 @@ namespace route { } RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Valid state proceeding with selection"); - + // entering to routing state once destinations are picked this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_SELECTED); publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_SELECTED); @@ -186,7 +186,7 @@ namespace route { // load destination points in map frame std::vector destination_points; if(req->choice == carma_planning_msgs::srv::SetActiveRoute::Request::ROUTE_ID) - { + { RCLCPP_INFO_STREAM(logger_->get_logger(), "set_active_route_cb: Selected Route ID: " << req->route_id); std::vector gps_destination_points = loadRouteDestinationGpsPointsFromRouteId(req->route_id); destination_points = loadRouteDestinationsInMapFrame(gps_destination_points); @@ -220,7 +220,7 @@ namespace route { // Add vehicle as first destination point auto destination_points_in_map_with_vehicle = destination_points_in_map_; - + lanelet::BasicPoint2d vehicle_position(vehicle_pose_->pose.position.x, vehicle_pose_->pose.position.y); destination_points_in_map_with_vehicle.insert(destination_points_in_map_with_vehicle.begin(), vehicle_position); @@ -230,7 +230,7 @@ namespace route { { if ((world_model_->getLaneletsFromPoint(pt, 1)).empty()) { - RCLCPP_ERROR_STREAM(logger_->get_logger(), "Route Generator: " << idx + RCLCPP_ERROR_STREAM(logger_->get_logger(), "Route Generator: " << idx << "th destination point is not in the map, x: " << pt.x() << " y: " << pt.y()); resp->error_status = carma_planning_msgs::srv::SetActiveRoute::Response::ROUTE_FILE_ERROR; this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_GEN_FAILED); @@ -239,7 +239,7 @@ namespace route { } idx ++; } - + // get route graph from world model object auto p = world_model_->getMapRoutingGraph(); // generate a route @@ -306,7 +306,7 @@ namespace route { // Verify that there are no duplicate lanelet IDs in the shortest path std::sort(shortest_path_lanelet_ids.begin(), shortest_path_lanelet_ids.end()); - + if (std::adjacent_find(shortest_path_lanelet_ids.begin(), shortest_path_lanelet_ids.end()) != shortest_path_lanelet_ids.end()) { // Route's shortest path contains duplicate lanelet IDs @@ -322,7 +322,7 @@ namespace route { if (!map_proj_) { throw std::invalid_argument("loadRouteDestinationsInMapFrame (using destination points array) before map projection was set"); } - + lanelet::projection::LocalFrameProjector projector(map_proj_.get().c_str()); // Build map projector // Process each point in 'destinations' @@ -332,7 +332,7 @@ namespace route { lanelet::GPSPoint coordinate; coordinate.lon = destination.longitude; coordinate.lat = destination.latitude; - + if(destination.elevation_exists) { coordinate.ele = destination.elevation; @@ -354,7 +354,7 @@ namespace route { std::string route_file_name = route_file_path_ + route_id + ".csv"; std::ifstream fs(route_file_name); std::string line; - + // read each line in route file (if any) std::vector destination_points; while(std::getline(fs, line)) @@ -390,7 +390,7 @@ namespace route { double lanelet_downtrack = carma_wm::geometry::trackPos(last_ll, last_ll.centerline().back().basicPoint2d()).downtrack; // get number of points to display using ratio of the downtracks auto points_until_end_point = int (last_ll.centerline().size() * (end_point_downtrack / lanelet_downtrack)); - + for(const auto& ll : route.get().shortestPath()) { if (ll.id() == last_ll.id()) @@ -433,14 +433,14 @@ namespace route { RCLCPP_WARN_STREAM(logger_->get_logger(), "No central line points! Returning"); return route_marker_msg_; } - + for (int i = 0; i < points.size(); i=i+5) { geometry_msgs::msg::Point temp_point; temp_point.x = points[i].x(); temp_point.y = points[i].y(); temp_point.z = 1; //to show up on top of the lanelet lines - + marker.points.push_back(temp_point); } new_route_marker_generated_ = true; @@ -485,7 +485,7 @@ namespace route { return true; } - void RouteGeneratorWorker::initializeBumperTransformLookup() + void RouteGeneratorWorker::initializeBumperTransformLookup() { tf2_listener_.reset(new tf2_ros::TransformListener(tf2_buffer_)); tf2_buffer_.setUsingDedicatedThread(true); @@ -495,20 +495,20 @@ namespace route { { try { - tf_ = tf2_buffer_.lookupTransform("map", "vehicle_front", rclcpp::Time(0,0), rclcpp::Duration(1.0*1e9)); //save to local copy of transform 0.1 sec timeout + tf_ = tf2_buffer_.lookupTransform("map", "vehicle_front", rclcpp::Time(0,0), rclcpp::Duration::from_nanoseconds(1.0*1e9)); //save to local copy of transform 0.1 sec timeout tf2::fromMsg(tf_, frontbumper_transform_); } catch (const tf2::TransformException &ex) { RCLCPP_WARN_STREAM(logger_->get_logger(), ex.what()); } - + geometry_msgs::msg::PoseStamped updated_vehicle_pose; updated_vehicle_pose.pose.position.x = frontbumper_transform_.getOrigin().getX(); updated_vehicle_pose.pose.position.y = frontbumper_transform_.getOrigin().getY(); updated_vehicle_pose.pose.position.z = frontbumper_transform_.getOrigin().getZ(); - vehicle_pose_ = updated_vehicle_pose; - + vehicle_pose_ = updated_vehicle_pose; + if(this->rs_worker_.getRouteState() == RouteStateWorker::RouteState::FOLLOWING) { // convert from pose stamp into lanelet basic 2D point current_loc_ = lanelet::BasicPoint2d(vehicle_pose_->pose.position.x, vehicle_pose_->pose.position.y); @@ -536,27 +536,27 @@ namespace route { current_downtrack_distance_ = track.downtrack; // Determine speed limit lanelet::Optional traffic_rules = world_model_->getTrafficRules(); - - if (traffic_rules) + + if (traffic_rules) { auto laneletIterator = world_model_->getMap()->laneletLayer.find(ll_id_); if (laneletIterator != world_model_->getMap()->laneletLayer.end()) { speed_limit_ = (*traffic_rules)->speedLimit(*laneletIterator).speedLimit.value(); - } - else + } + else { RCLCPP_ERROR_STREAM(logger_->get_logger(), "Failed to set the current speed limit. The lanelet_id: " << ll_id_ << " could not be matched with a lanelet in the map. The previous speed limit of " << speed_limit_ << " will be used."); } - - } - else + + } + else { RCLCPP_ERROR_STREAM(logger_->get_logger(), "Failed to set the current speed limit. Valid traffic rules object could not be built."); } std::shared_ptr pose_ptr(new geometry_msgs::msg::PoseStamped(*vehicle_pose_)); - + // check if we left the seleted route by cross track error if (crosstrackErrorCheck(pose_ptr, current_lanelet)) { @@ -564,7 +564,7 @@ namespace route { publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_DEPARTED); } - // check if we reached our destination be remaining down track distance + // check if we reached our destination be remaining down track distance double route_length_2d = world_model_->getRouteEndTrackPos().downtrack; if((current_downtrack_distance_ > route_length_2d - down_track_target_range_ && current_speed_ < epsilon_) || (current_downtrack_distance_ > route_length_2d)) { @@ -604,25 +604,25 @@ namespace route { { route_event_queue.push(event_type); } - + lanelet::Optional RouteGeneratorWorker::rerouteAfterRouteInvalidation(const std::vector& destination_points_in_map) { std::vector destination_points_in_map_temp; - + for(const auto &i:destination_points_in_map) // Identify all route points that we have not yet passed { double destination_down_track=world_model_->routeTrackPos(i).downtrack; - + if( current_downtrack_distance_< destination_down_track) { destination_points_in_map_temp.push_back(i); RCLCPP_DEBUG_STREAM(logger_->get_logger(), "current_downtrack_distance_:" << current_downtrack_distance_); RCLCPP_DEBUG_STREAM(logger_->get_logger(), "destination_down_track:" << destination_down_track); } - } - + } + destination_points_in_map_ = destination_points_in_map_temp; // Update our route point list - + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "New destination_points_in_map.size:" << destination_points_in_map_.size()); auto route=routing(current_loc_, // Route from current location through future destinations @@ -663,8 +663,8 @@ namespace route { else { this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_STARTED); - publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_STARTED); - } + publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_STARTED); + } std::string original_route_name = route_msg_.route_name; route_msg_ = composeRouteMsg(route); route_msg_.route_name = original_route_name; @@ -674,7 +674,7 @@ namespace route { new_route_msg_generated_ = true; new_route_marker_generated_ = true; } - + // publish new route and set new route flag back to false if(new_route_msg_generated_ && new_route_marker_generated_) { @@ -692,7 +692,7 @@ namespace route { state_msg.route_id = route_msg_.route_name; state_msg.cross_track = current_crosstrack_distance_; state_msg.down_track = current_downtrack_distance_; - state_msg.lanelet_downtrack = ll_downtrack_distance_; + state_msg.lanelet_downtrack = ll_downtrack_distance_; state_msg.state = this->rs_worker_.getRouteState(); state_msg.lanelet_id = ll_id_; state_msg.speed_limit = speed_limit_; @@ -706,7 +706,7 @@ namespace route { route_event_pub_->publish(route_event_msg_); route_event_queue.pop(); } - return true; + return true; } bool RouteGeneratorWorker::crosstrackErrorCheck(const std::shared_ptr& msg, lanelet::ConstLanelet current) @@ -725,7 +725,7 @@ namespace route { RCLCPP_DEBUG_STREAM(logger_->get_logger(), "LLt Polygon Dimensions1: " << current.polygon2d().front().x()<< ", "<< current.polygon2d().front().y()); RCLCPP_DEBUG_STREAM(logger_->get_logger(), "LLt Polygon Dimensions2: " << current.polygon2d().back().x()<< ", "<< current.polygon2d().back().y()); RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Distance1: " << boost::geometry::distance(position, current.polygon2d()) << " Max allowed Crosstrack: " << cross_track_dist_ ); - + if (boost::geometry::distance(position, current.polygon2d()) > cross_track_dist_) //Evaluate lanelet crosstrack distance from vehicle { cte_count_++; @@ -735,7 +735,7 @@ namespace route { cte_count_ = 0; return true; } - else + else return false; } else @@ -743,7 +743,7 @@ namespace route { cte_count_ = 0; return false; } - + } @@ -780,4 +780,4 @@ namespace route { route_llts.push_back(llt); } -} // route \ No newline at end of file +} // route diff --git a/route_following_plugin/include/route_following_plugin.hpp b/route_following_plugin/include/route_following_plugin.hpp index ad7e4daba7..cbfdd1b96b 100644 --- a/route_following_plugin/include/route_following_plugin.hpp +++ b/route_following_plugin/include/route_following_plugin.hpp @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include #include "carma_guidance_plugins/strategic_plugin.hpp" #include "route_following_plugin_config.hpp" @@ -56,7 +56,7 @@ ((mvr).type == carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property = (value) :\ ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property = (value) :\ throw std::invalid_argument("SET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type))))))))) - + namespace route_following_plugin { class RouteFollowingPlugin : public carma_guidance_plugins::StrategicPlugin @@ -67,7 +67,7 @@ namespace route_following_plugin * \brief Default constructor for RouteFollowingPlugin class */ explicit RouteFollowingPlugin(const rclcpp::NodeOptions &); - + ////////// OVERRIDES /////////// carma_ros2_utils::CallbackReturn on_configure_plugin(); carma_ros2_utils::CallbackReturn on_activate_plugin(); @@ -78,7 +78,7 @@ namespace route_following_plugin bool get_availability(); std::string get_version_id(); - + private: /** @@ -87,13 +87,13 @@ namespace route_following_plugin * \param end_dist End downtrack distance of the current maneuver * \param start_speed Start speed of the current maneuver * \param target_speed Target speed pf the current maneuver, usually it is the lanelet speed limit - * \param lane_ids List of lanelet IDs that the current maneuver traverses. Message expects these to be contiguous and end to end + * \param lane_ids List of lanelet IDs that the current maneuver traverses. Message expects these to be contiguous and end to end * \return A lane keeping maneuver message which is ready to be published */ carma_planning_msgs::msg::Maneuver composeLaneFollowingManeuverMessage(double start_dist, double end_dist, double start_speed, double target_speed, const std::vector& lane_ids) const; /** - * \brief Compose a lane change maneuver message based on input params + * \brief Compose a lane change maneuver message based on input params * NOTE: The start and stop time are not set. This is because this is recomputed based on requests from the arbitrator * \param start_dist Start downtrack distance of the current maneuver * \param end_dist End downtrack distance of the current maneuver @@ -104,9 +104,9 @@ namespace route_following_plugin * \return A lane keeping maneuver message which is ready to be published */ carma_planning_msgs::msg::Maneuver composeLaneChangeManeuverMessage(double start_dist, double end_dist, double start_speed, double target_speed, lanelet::Id starting_lane_id,lanelet::Id ending_lane_id) const; - + /** - * \brief Compose a stop and wait maneuver message based on input params. + * \brief Compose a stop and wait maneuver message based on input params. * NOTE: The start and stop time are not set. This is because this is recomputed based on requests from the arbitrator * \param start_dist Start downtrack distance of the current maneuver * \param end_dist End downtrack distance of the current maneuver @@ -125,7 +125,7 @@ namespace route_following_plugin * \return Whether we need a lanechange to reach to the next lanelet in the shortest path. */ bool isLaneChangeNeeded(lanelet::routing::LaneletRelations relations, lanelet::Id target_id) const; - + /** * \brief Set the start distance of a maneuver based on the progress along the route * \param maneuver A maneuver (non-specific to type) to be performed @@ -135,7 +135,7 @@ namespace route_following_plugin /** * \brief Given an array of maneuvers update the starting time for each - * \param maneuvers An array of maneuvers (non-specific to type) + * \param maneuvers An array of maneuvers (non-specific to type) * \param start_time The starting time for the first maneuver in the sequence, each consequent maneuver is pushed ahead by same amount */ void updateTimeProgress(std::vector& maneuvers, rclcpp::Time start_time) const; @@ -152,10 +152,10 @@ namespace route_following_plugin * \param resp Plan maneuver response with a list of maneuver plan * \return If service call successed */ - + void plan_maneuvers_callback( - std::shared_ptr srv_header, - carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp); /** @@ -174,69 +174,69 @@ namespace route_following_plugin /** * \brief Adds a StopAndWait maneuver to the end of a maneuver set stopping at the provided downtrack value * NOTE: The priority of this method is to plan the stopping maneuver therefore earlier maneuvers will be modified or removed if required to allow the stopping behavior to be executed - * + * * \param input_maneuvers The set of maneuvers to modify to support the StopAndWait maneuver. * \param route_end_downtrack The target stopping point (normally the end of the route) which the vehicle should stop before. Units meters * \param stopping_entry_speed The expected entry speed for stopping. This is used to compute the stopping distance. Units m/s * \param stopping_logitudinal_accel The target deceleration (unsigned) for the stopping operation. Units m/s/s * \param lateral_accel_limit The lateral acceleration limit allowed for lane changes. Units m/s/s * \param min_maneuver_length The absolute minimum allowable maneuver length for any existing maneuvers in meters - * + * * NOTE: Only min_maneuver_length can be a zero-valued input. All other parameters must be positive values greater than zero. - * + * * \throw std::invalid_argument If existing maneuvers cannot be modified to allow stopping maneuver creation, or if the generated maneuvers do not overlap any lanelets in the map. - * - * \return A list of maneuvers which mirrors the input list but with the modifications required to include a stopping maneuver at the end - * + * + * \return A list of maneuvers which mirrors the input list but with the modifications required to include a stopping maneuver at the end + * * ASSUMPTION: At the moment the stopping entry speed is not updated because the assumption is - * that any previous maneuvers which were slower need not be accounted for as planning for a higher speed will always be capable of handling that case - * and any which were faster would already have their speed reduced by the maneuver which this speed was derived from. - */ + * that any previous maneuvers which were slower need not be accounted for as planning for a higher speed will always be capable of handling that case + * and any which were faster would already have their speed reduced by the maneuver which this speed was derived from. + */ std::vector addStopAndWaitAtRouteEnd ( - const std::vector& input_maneuvers, + const std::vector& input_maneuvers, double route_end_downtrack, double stopping_entry_speed, double stopping_logitudinal_accel, double lateral_accel_limit, double min_maneuver_length ) const; /** * \brief Identifies if a maneuver starts after the provided downtrack with compensation for a dynamic buffer size based on the maneuver type - * + * * \param maneuver The maneuver to compare * \param downtrack The downtrack value to evaluate in meters * \param lateral_accel The max lateral acceleration allowed for lane changes in m/s/s * \param min_maneuver_length The absolute minimum allowable for any maneuver in meters - * + * * \return true if the provided maneuver plus the computed dynamic buffer starts after the provided downtrack value - */ + */ bool maneuverWithBufferStartsAfterDowntrack(const carma_planning_msgs::msg::Maneuver& maneuver, double downtrack, double lateral_accel, double min_maneuver_length) const; /** * \brief This method returns a new UUID as a string for assignment to a Maneuver message - * + * * \return A new UUID as a string - */ + */ std::string getNewManeuverId() const; /** * \brief This method re-routes the vehicle back to the shortest path, if the vehicle has left the shortest path, but is still on the route * Re-routing is performed by generating a new shortest path via the closest lanelet on the original shortest path - * + * * \param current_lanelet curretn lanelet where the vehicle is at - */ + */ void returnToShortestPath(const lanelet::ConstLanelet ¤t_lanelet); //Subscribers carma_ros2_utils::SubPtr twist_sub_; carma_ros2_utils::SubPtr current_maneuver_plan_sub_; - + // unordered set of all the lanelet ids in shortest path std::unordered_set shortest_path_set_; - + static constexpr double MAX_LANE_WIDTH = 3.70; // Maximum lane width of a US highway // Node configuration Config config_; - + // Current vehicle forward speed double current_speed_ = 0.0; @@ -246,7 +246,7 @@ namespace route_following_plugin // Current vehicle pose in map geometry_msgs::msg::PoseStamped pose_msg_; lanelet::BasicPoint2d current_loc_; - + // Currently executing maneuver plan from Arbitrator carma_planning_msgs::msg::ManeuverPlan::UniquePtr current_maneuver_plan_; @@ -258,11 +258,11 @@ namespace route_following_plugin std::string stop_and_wait_plugin_ = "stop_and_wait_plugin"; std::string planning_strategic_plugin_ = "route_following_plugin"; - std::string lanefollow_planning_tactical_plugin_ = "inlanecruising_plugin"; - + std::string lanefollow_planning_tactical_plugin_ = "inlanecruising_plugin"; + // Timer used to update the front bumper pose rclcpp::TimerBase::SharedPtr bumper_pose_timer_; - + /** * \brief Callback for the front bumper pose transform */ @@ -296,10 +296,10 @@ namespace route_following_plugin void initializeBumperTransformLookup(); geometry_msgs::msg::TransformStamped tf_; - + // front bumper transform tf2::Stamped frontbumper_transform_; - + // TF listenser tf2_ros::Buffer tf2_buffer_; std::unique_ptr tf2_listener_; @@ -323,4 +323,4 @@ namespace route_following_plugin FRIEND_TEST(StopAndWaitTestFixture, CaseTen); }; -} \ No newline at end of file +} diff --git a/route_following_plugin/src/route_following_plugin.cpp b/route_following_plugin/src/route_following_plugin.cpp index ebd8edbf0c..ff7ca8746b 100644 --- a/route_following_plugin/src/route_following_plugin.cpp +++ b/route_following_plugin/src/route_following_plugin.cpp @@ -32,8 +32,8 @@ namespace route_following_plugin namespace { /** * \brief Anonymous function to extract maneuver end speed which can not be optained with GET_MANEUVER_PROPERY calls due to it missing in stop and wait plugin - */ -double getManeuverEndSpeed(const carma_planning_msgs::msg::Maneuver& mvr) { + */ +double getManeuverEndSpeed(const carma_planning_msgs::msg::Maneuver& mvr) { switch(mvr.type) { case carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING: return mvr.lane_following_maneuver.end_speed; @@ -54,7 +54,7 @@ double getManeuverEndSpeed(const carma_planning_msgs::msg::Maneuver& mvr) { /** * \brief Anonymous function to set the lanelet ids for all maneuver types except lane following - */ + */ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id start_id, lanelet::Id end_id) { switch(mvr.type) { @@ -114,7 +114,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id get_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit_); get_parameter("stopping_accel_limit_multiplier", config_.stopping_accel_limit_multiplier_); get_parameter("min_maneuver_length", config_.min_maneuver_length_); - + RCLCPP_INFO_STREAM(rclcpp::get_logger("route_following_plugin"), "RouteFollowingPlugin Config: " << config_); // Setup subscribers @@ -122,7 +122,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id std::bind(&RouteFollowingPlugin::twist_cb,this,std_ph::_1)); current_maneuver_plan_sub_ = create_subscription("maneuver_plan", 50, std::bind(&RouteFollowingPlugin::current_maneuver_plan_cb,this,std_ph::_1)); - + // set world model point form wm listener wml_ = get_world_model_listener(); @@ -142,7 +142,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id }); initializeBumperTransformLookup(); - + // Return success if everthing initialized successfully return CallbackReturn::SUCCESS; } @@ -156,20 +156,20 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id // Return success if everthing initialized successfully return CallbackReturn::SUCCESS; } - + void RouteFollowingPlugin::twist_cb(geometry_msgs::msg::TwistStamped::UniquePtr msg) { current_speed_ = msg->twist.linear.x; } - + void RouteFollowingPlugin::current_maneuver_plan_cb(carma_planning_msgs::msg::ManeuverPlan::UniquePtr msg) { current_maneuver_plan_ = std::move(msg); } std::vector RouteFollowingPlugin::routeCb(const lanelet::routing::LaneletPath &route_shortest_path) { - - + + for (const auto& ll:route_shortest_path) { shortest_path_set_.insert(ll.id()); @@ -227,9 +227,9 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id if (isLaneChangeNeeded(following_lanelets, route_shortest_path[shortest_path_index + 1].id())) { RCLCPP_DEBUG_STREAM(get_logger(),"LaneChangeNeeded"); - + // Determine the Lane Change Status - RCLCPP_DEBUG_STREAM(get_logger(),"Recording lanechange start_dist <<" << start_dist << ", from llt id:" << route_shortest_path[shortest_path_index].id() << " to llt id: " << + RCLCPP_DEBUG_STREAM(get_logger(),"Recording lanechange start_dist <<" << start_dist << ", from llt id:" << route_shortest_path[shortest_path_index].id() << " to llt id: " << route_shortest_path[shortest_path_index+ 1].id()); maneuvers.push_back(composeLaneChangeManeuverMessage(start_dist, end_dist, start_speed, target_speed_in_lanelet, route_shortest_path[shortest_path_index].id(), route_shortest_path[shortest_path_index + 1].id())); @@ -255,7 +255,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id // Add stop and wait maneuver based on deceleration target and entry speed maneuvers = addStopAndWaitAtRouteEnd( maneuvers, route_length, stopping_entry_speed, stopping_accel_limit, config_.lateral_accel_limit_, config_.min_maneuver_length_ ); - + } ////------------------ RCLCPP_DEBUG_STREAM(get_logger(),"Maneuver plan along route successfully generated"); @@ -263,7 +263,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id } std::vector RouteFollowingPlugin::addStopAndWaitAtRouteEnd ( - const std::vector& input_maneuvers, + const std::vector& input_maneuvers, double route_end_downtrack, double stopping_entry_speed, double stopping_logitudinal_accel, double lateral_accel_limit, double min_maneuver_length ) const @@ -276,20 +276,20 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id * 3. If end downtrack - stop distance is > last maneuver end distance then fill delta with lane follow followed by stop and wait * 4. If end downtrack - stop distance is < last maneuver end distance then reduce the existing maneuver to allow for stopping maneuver to be planned * 5. Add maneuver to list - */ + */ std::vector maneuvers = input_maneuvers; // Output maneuvers which will be modified // Compute stopping distance where v_f = 0 - // (v_f^2 - v_i^2) / (2*a) = d + // (v_f^2 - v_i^2) / (2*a) = d double stopping_distance = 0.5 * (stopping_entry_speed * stopping_entry_speed) / stopping_logitudinal_accel; // Compute required starting downtrack for maneuver double required_start_downtrack = std::max(0.0, route_end_downtrack - stopping_distance); // Loop to drop any maneuvers which fully overlap our stopping maneuver while accounting for minimum length maneuver buffers - while ( !maneuvers.empty() && maneuverWithBufferStartsAfterDowntrack(maneuvers.back(), required_start_downtrack, lateral_accel_limit, config_.min_maneuver_length_) ) { - + while ( !maneuvers.empty() && maneuverWithBufferStartsAfterDowntrack(maneuvers.back(), required_start_downtrack, lateral_accel_limit, config_.min_maneuver_length_) ) { + RCLCPP_WARN_STREAM(get_logger(),"Dropping maneuver with id: " << GET_MANEUVER_PROPERTY(maneuvers.back(), parameters.maneuver_id) ); if (maneuvers.back().type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE) { @@ -302,21 +302,21 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id } - double last_maneuver_end_downtrack = required_start_downtrack; // Set default starting location for stop and wait maneuver + double last_maneuver_end_downtrack = required_start_downtrack; // Set default starting location for stop and wait maneuver if ( !maneuvers.empty() ) { // If there are existing maneuvers we need to make sure stop and wait does not overwrite them - + last_maneuver_end_downtrack = GET_MANEUVER_PROPERTY(maneuvers.back(), end_dist); - + // If our stopping maneuver does not intersect with existing maneuvers - if ( required_start_downtrack >= last_maneuver_end_downtrack ) { - + if ( required_start_downtrack >= last_maneuver_end_downtrack ) { + // If the delta is under minimum_maneuver_length we can just extend the stopping maneuver // Otherwise add a new lane follow maneuver if (required_start_downtrack - last_maneuver_end_downtrack > min_maneuver_length) { - + // Identify the lanelets which will be crossed by this lane follow maneuver - std::vector crossed_lanelets = wm_->getLaneletsBetween(last_maneuver_end_downtrack, required_start_downtrack, true, false); + std::vector crossed_lanelets = wm_->getLaneletsBetween(last_maneuver_end_downtrack, required_start_downtrack, true, false); if (crossed_lanelets.empty()) { throw std::invalid_argument("The new lane follow maneuver does not cross any lanelets going from: " + std::to_string(last_maneuver_end_downtrack) + " to: " + std::to_string(required_start_downtrack)); @@ -324,16 +324,16 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id // Create the lane follow maneuver maneuvers.push_back(composeLaneFollowingManeuverMessage(last_maneuver_end_downtrack, required_start_downtrack, stopping_entry_speed, stopping_entry_speed, lanelet::utils::transform(crossed_lanelets, [](auto ll) { return ll.id(); }))); - + // Update last maneuver end downtrack so the stop and wait maneuver can be properly formulated - last_maneuver_end_downtrack = required_start_downtrack; + last_maneuver_end_downtrack = required_start_downtrack; } else { RCLCPP_DEBUG_STREAM(get_logger(),"Stop and wait maneuver being extended to nearest maneuver which is closer than the minimum maneuver length"); } } else { // If our stopping maneuver intersects with existing maneuvers - + SET_MANEUVER_PROPERTY(maneuvers.back(), end_dist, required_start_downtrack); // Identify the lanelets which will be crossed by this updated maneuver @@ -356,9 +356,9 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id last_maneuver_end_downtrack = required_start_downtrack; - } + } } - + // Identify the lanelets which will be crossed by this stop and wait maneuver std::vector crossed_lanelets = wm_->getLaneletsBetween(last_maneuver_end_downtrack, route_end_downtrack, true, false); @@ -379,24 +379,24 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id } bool RouteFollowingPlugin::maneuverWithBufferStartsAfterDowntrack(const carma_planning_msgs::msg::Maneuver& maneuver, double downtrack, double lateral_accel, double min_maneuver_length) const { - + if (maneuver.type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE) { - + // Compute the time it takes to move laterally to the next lane double lane_change_time = sqrt(0.5 * MAX_LANE_WIDTH / lateral_accel); - + // Compute logitudinal distance covered in lane change time double min_lane_change_distance = std::max( - min_maneuver_length, + min_maneuver_length, lane_change_time * (GET_MANEUVER_PROPERTY(maneuver, start_speed) + getManeuverEndSpeed(maneuver)) / 2.0 // dist = v_avg * t ); return GET_MANEUVER_PROPERTY(maneuver, start_dist) + min_lane_change_distance > downtrack; - } else { - - return GET_MANEUVER_PROPERTY(maneuver, start_dist) + min_maneuver_length > downtrack; - + } else { + + return GET_MANEUVER_PROPERTY(maneuver, start_dist) + min_maneuver_length > downtrack; + } } @@ -404,15 +404,15 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id { return true; } - + std::string RouteFollowingPlugin::get_version_id() { return "v1.0"; } void RouteFollowingPlugin::plan_maneuvers_callback( - std::shared_ptr srv_header, - carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp) { @@ -423,18 +423,18 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id } double current_downtrack; - + if (!req->prior_plan.maneuvers.empty()) { current_downtrack = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_dist); - RCLCPP_DEBUG_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end_dist:"<< current_downtrack); + RCLCPP_DEBUG_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end_dist:"<< current_downtrack); } else { current_downtrack = req->veh_downtrack; RCLCPP_DEBUG_STREAM(get_logger(),"Detected NO prior plan! Using req.veh_downtrack: "<< current_downtrack); } - + //Return the set of maneuvers which intersect with min_plan_duration size_t i = 0; double planned_time = 0.0; @@ -472,13 +472,13 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id if (!req->prior_plan.maneuvers.empty()) { updateTimeProgress(new_maneuvers, GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_time)); - RCLCPP_DEBUG_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end time:"<< std::to_string(rclcpp::Time(GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_time)).seconds())); - RCLCPP_DEBUG_STREAM(get_logger(),"Where plan_completion_time was:"<< std::to_string(rclcpp::Time(req->prior_plan.planning_completion_time).seconds())); + RCLCPP_DEBUG_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end time:"<< std::to_string(rclcpp::Time(GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_time)).seconds())); + RCLCPP_DEBUG_STREAM(get_logger(),"Where plan_completion_time was:"<< std::to_string(rclcpp::Time(req->prior_plan.planning_completion_time).seconds())); } else { updateTimeProgress(new_maneuvers, rclcpp::Time(req->header.stamp)); - RCLCPP_DEBUG_STREAM(get_logger(),"Detected NO prior plan! Using this->now():"<< std::to_string(this->now().seconds())); + RCLCPP_DEBUG_STREAM(get_logger(),"Detected NO prior plan! Using this->now():"<< std::to_string(this->now().seconds())); } //update starting speed of first maneuver @@ -505,14 +505,14 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id default: throw std::invalid_argument("Invalid maneuver type, cannot update starting speed for maneuver"); } - + updateStartingSpeed(new_maneuvers.front(), start_speed); - RCLCPP_DEBUG_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end speed:"<< start_speed); + RCLCPP_DEBUG_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end speed:"<< start_speed); } - else + else { updateStartingSpeed(new_maneuvers.front(), req->veh_logitudinal_velocity); - RCLCPP_DEBUG_STREAM(get_logger(),"Detected NO prior plan! Using req->veh_logitudinal_velocity:"<< req->veh_logitudinal_velocity); + RCLCPP_DEBUG_STREAM(get_logger(),"Detected NO prior plan! Using req->veh_logitudinal_velocity:"<< req->veh_logitudinal_velocity); } //update plan resp->new_plan = req->prior_plan; @@ -533,7 +533,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id RCLCPP_DEBUG_STREAM(get_logger(),"Entering pose_cb"); RCLCPP_DEBUG_STREAM(get_logger(),"Looking up front bumper pose..."); - + try { tf_ = tf2_buffer_.lookupTransform("map", "vehicle_front", rclcpp::Time(0), rclcpp::Duration(1, 0)); //save to local copy of transform 1 sec timeout @@ -547,23 +547,23 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id geometry_msgs::msg::Pose front_bumper_pose; front_bumper_pose.position.x = frontbumper_transform_.getOrigin().getX(); front_bumper_pose.position.y = frontbumper_transform_.getOrigin().getY(); - + if (!wm_->getRoute()) return; lanelet::BasicPoint2d current_loc(front_bumper_pose.position.x, front_bumper_pose.position.y); current_loc_ = current_loc; double current_progress = wm_->routeTrackPos(current_loc).downtrack; - + RCLCPP_DEBUG_STREAM(get_logger(),"pose_cb : current_progress" << current_progress); - + // Check if we need to return to route shortest path. // Step 1. Check if another plugin aside from RFP has been in control if (current_maneuver_plan_ != nullptr && GET_MANEUVER_PROPERTY(current_maneuver_plan_->maneuvers[0], parameters.planning_strategic_plugin) \ != planning_strategic_plugin_) { // If another plugin may have brought us off shortest path, check our current lanelet - auto llts = wm_->getLaneletsFromPoint(current_loc, 10); + auto llts = wm_->getLaneletsFromPoint(current_loc, 10); // Remove any candidate lanelets not on the route llts.erase(std::remove_if(llts.begin(), llts.end(), [&](auto lanelet) -> bool { return !wm_->getRoute()->contains(lanelet); }), @@ -620,7 +620,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id RCLCPP_DEBUG_STREAM(get_logger(),"maneuver_end_dist: " << maneuver_end_dist << ", maneuver_start_dist: " << maneuver_start_dist << ", cur_plus_target: " << cur_plus_target); - duration = rclcpp::Duration((maneuver_end_dist - maneuver_start_dist) / (0.5 * cur_plus_target) * 1e9); + duration = rclcpp::Duration::from_nanoseconds((maneuver_end_dist - maneuver_start_dist) / (0.5 * cur_plus_target) * 1e9); return duration; } @@ -744,9 +744,9 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id for (const auto& id : maneuver_msg.lane_following_maneuver.lane_ids) ss << " " << id; - RCLCPP_DEBUG_STREAM(get_logger(),"Creating lane follow id: " << maneuver_msg.lane_following_maneuver.parameters.maneuver_id + RCLCPP_DEBUG_STREAM(get_logger(),"Creating lane follow id: " << maneuver_msg.lane_following_maneuver.parameters.maneuver_id << " start dist: " << start_dist << " end dist: " << end_dist << "lane_ids: " << ss.str()); - + return maneuver_msg; } @@ -777,7 +777,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id std::string RouteFollowingPlugin::getNewManeuverId() const { static auto gen = boost::uuids::random_generator(); // Initialize uuid generator - + return boost::lexical_cast(gen()); // generate uuid and convert to string } @@ -800,7 +800,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id // Set the meta-data for the StopAndWait Maneuver to define the buffer in the route end point stopping location maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(config_.route_end_point_buffer_); maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel); - + // NOTE: The maneuver id is set here because maneuvers are regenerated once per route, so it is acceptable to regenerate them on route updates. // If maneuvers were not generated only on route updates we would want to preserve the ids across plans maneuver_msg.stop_and_wait_maneuver.parameters.maneuver_id = getNewManeuverId(); @@ -836,7 +836,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id } } - void RouteFollowingPlugin::initializeBumperTransformLookup() + void RouteFollowingPlugin::initializeBumperTransformLookup() { tf2_listener_.reset(new tf2_ros::TransformListener(tf2_buffer_)); tf2_buffer_.setUsingDedicatedThread(true); @@ -847,7 +847,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id auto original_shortestpath = wm_->getRoute()->shortestPath(); RCLCPP_DEBUG_STREAM(get_logger(),"The vehicle has left the shortest path"); auto routing_graph = wm_->getMapRoutingGraph(); - + // Obtain the lanelet following the current lanelet in this same lane, this lanelet must exist since the new shortest path cannot begin with a lane change auto current_lane_following_lanelets = routing_graph->following(current_lanelet); lanelet::ConstLanelet current_lane_following_lanelet; @@ -891,7 +891,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id if (new_shortestpath) this->latest_maneuver_plan_ = routeCb(new_shortestpath.get()); } } - + } else { @@ -899,10 +899,10 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id } } - + } #include "rclcpp_components/register_node_macro.hpp" // Register the component with class_loader -RCLCPP_COMPONENTS_REGISTER_NODE(route_following_plugin::RouteFollowingPlugin) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(route_following_plugin::RouteFollowingPlugin) diff --git a/route_following_plugin/test/test_route_following_plugin.cpp b/route_following_plugin/test/test_route_following_plugin.cpp index 073f4fd0c5..e1403d1b4c 100644 --- a/route_following_plugin/test/test_route_following_plugin.cpp +++ b/route_following_plugin/test/test_route_following_plugin.cpp @@ -135,7 +135,7 @@ namespace route_following_plugin std::shared_ptr srv_header; worker->plan_maneuvers_callback(srv_header,plan_request, plan_response); - + //check target speeds in updated response lanelet::Velocity limit = 30_mph; ASSERT_EQ(plan_response->new_plan.maneuvers[0].lane_following_maneuver.end_speed, 11.176); @@ -161,15 +161,15 @@ namespace route_following_plugin lanelet::Id end_id = 111; /*** * VAVLID PATHs (consists of lanenet ids): (This is also the shortest path because certain Lanelets missing) - * 159->160->164->136->135->137->144->121; + * 159->160->164->136->135->137->144->121; * 159->160->164->136->135->137->144->118; * 168->170->111 * 159->161->168->170->111 * 167->169->168->170->111 - * 115->146->140->139->143->167->169->168->170->111 - * 141->139->143->167->169->168->170->111 - * 127->146->140->139->143->167->169->168->170->111 - * 101->100->104->167->169->168->170->111 (a counter cLock circle) + * 115->146->140->139->143->167->169->168->170->111 + * 141->139->143->167->169->168->170->111 + * 127->146->140->139->143->167->169->168->170->111 + * 101->100->104->167->169->168->170->111 (a counter cLock circle) * **/ // Write new map to file int projector_type = 0; @@ -263,7 +263,7 @@ namespace route_following_plugin else ASSERT_EQ(speed, 11.176); } - + TEST(RouteFollowingPlugin, TestHelperfunctions) { auto worker = std::make_shared(rclcpp::NodeOptions()); @@ -274,7 +274,7 @@ namespace route_following_plugin worker->setManeuverStartDist(maneuver, 50.0); ASSERT_EQ(maneuver.lane_following_maneuver.start_dist, 50.0); - rclcpp::Time new_start_time = start_time + rclcpp::Duration(10.0*1e9); + rclcpp::Time new_start_time = start_time + rclcpp::Duration::from_nanoseconds(10.0*1e9); std::vector maneuvers; maneuvers.push_back(maneuver); @@ -361,4 +361,3 @@ int main (int argc, char **argv) { return success; } - diff --git a/sci_strategic_plugin/src/sci_strategic_plugin.cpp b/sci_strategic_plugin/src/sci_strategic_plugin.cpp index 08bd87f8b6..a461ead765 100644 --- a/sci_strategic_plugin/src/sci_strategic_plugin.cpp +++ b/sci_strategic_plugin/src/sci_strategic_plugin.cpp @@ -43,10 +43,10 @@ namespace { * \brief Anonymous function to extract maneuver end speed which can not be optained with GET_MANEUVER_PROPERY calls due to it missing in stop and wait plugin * \param mvr input maneuver * \return end speed - */ + */ double getManeuverEndSpeed(const carma_planning_msgs::msg::Maneuver& mvr) { - switch(mvr.type) + switch(mvr.type) { case carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING: return mvr.lane_following_maneuver.end_speed; @@ -107,31 +107,31 @@ carma_ros2_utils::CallbackReturn SCIStrategicPlugin::on_configure_plugin() get_parameter("vehicle_length", config_.veh_length); get_parameter("vehicle_deceleration_limit", config_.vehicle_decel_limit); get_parameter("vehicle_acceleration_limit", config_.vehicle_accel_limit); - + // Register runtime parameter update callback add_on_set_parameters_callback(std::bind(&SCIStrategicPlugin::parameter_update_callback, this, std_ph::_1)); RCLCPP_INFO_STREAM(rclcpp::get_logger("sci_strategic_plugin"),"Done loading parameters: " << config_); // Mobility Operation Subscriber - mob_operation_sub_ = create_subscription("incoming_mobility_operation", 1, + mob_operation_sub_ = create_subscription("incoming_mobility_operation", 1, std::bind(&SCIStrategicPlugin::mobilityOperationCb,this,std_ph::_1)); - + // Current Pose Subscriber - current_pose_sub_ = create_subscription("current_pose", 1, + current_pose_sub_ = create_subscription("current_pose", 1, std::bind(&SCIStrategicPlugin::currentPoseCb,this,std_ph::_1)); // BSM subscriber - bsm_sub_ = create_subscription("bsm_outbound", 1, + bsm_sub_ = create_subscription("bsm_outbound", 1, std::bind(&SCIStrategicPlugin::BSMCb,this,std_ph::_1)); // Guidance State subscriber - guidance_state_sub_ = create_subscription("guidance_state", 5, + guidance_state_sub_ = create_subscription("guidance_state", 5, std::bind(&SCIStrategicPlugin::guidance_state_cb, this, std::placeholders::_1)); // set world model point form wm listener wm_ = get_world_model(); - + // Setup publishers mobility_operation_pub_ = create_publisher("outgoing_mobility_operation", 1); @@ -181,13 +181,13 @@ SCIStrategicPlugin::VehicleState SCIStrategicPlugin::extractInitialState(const c else { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "No initial plan provided..."); - + state.stamp = req.header.stamp; state.downtrack = req.veh_downtrack; state.speed = req.veh_logitudinal_velocity; state.lane_id = stoi(req.veh_lane_id); } - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "state.stamp: " << std::to_string(state.stamp.seconds())); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "state.downtrack : " << state.downtrack ); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "state.speed: " << state.speed); @@ -212,7 +212,7 @@ void SCIStrategicPlugin::mobilityOperationCb(carma_v2x_msgs::msg::MobilityOperat } previous_strategy_params_ = msg->strategy_params; } - + } void SCIStrategicPlugin::BSMCb(carma_v2x_msgs::msg::BSM::UniquePtr msg) @@ -237,8 +237,8 @@ void SCIStrategicPlugin::currentPoseCb(geometry_msgs::msg::PoseStamped::UniquePt current_downtrack_ = wm_->routeTrackPos(current_loc).downtrack; RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Downtrack from current pose: " << current_downtrack_); } - - + + } void SCIStrategicPlugin::parseStrategyParams(const std::string& strategy_params) @@ -281,7 +281,7 @@ int SCIStrategicPlugin::determine_speed_profile_case(double stop_dist, double cu { int case_num = 0; double estimated_stop_time = calcEstimatedStopTime(stop_dist, current_speed); - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "estimated_stop_time: " << estimated_stop_time); if (estimated_stop_time < schedule_stop_time) { @@ -300,13 +300,13 @@ int SCIStrategicPlugin::determine_speed_profile_case(double stop_dist, double cu case_num = 2; } } - + return case_num; } double SCIStrategicPlugin::calcEstimatedStopTime(double stop_dist, double current_speed) const { - + double t_stop = 0; t_stop = 2*stop_dist/current_speed; return t_stop; @@ -319,7 +319,7 @@ double SCIStrategicPlugin::calc_speed_before_decel(double stop_time, double stop double desired_acceleration = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier; double desired_deceleration = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier; - double sqr_term = sqrt(pow(1 - (desired_acceleration/desired_deceleration), 2) * pow(stop_dist/stop_time, 2) + double sqr_term = sqrt(pow(1 - (desired_acceleration/desired_deceleration), 2) * pow(stop_dist/stop_time, 2) + (1 - (desired_acceleration/desired_deceleration))*(current_speed*current_speed - 2*current_speed*stop_dist/stop_time)); speed_before_decel = (stop_dist/stop_time) + sqr_term/(1 - (desired_acceleration/desired_deceleration)); @@ -349,8 +349,8 @@ std::vector SCIStrategicPlugin::getLaneletsBetweenWithExc void SCIStrategicPlugin::plan_maneuvers_callback( - std::shared_ptr srv_header, - carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "In Maneuver callback..."); @@ -404,7 +404,7 @@ void SCIStrategicPlugin::plan_maneuvers_callback( RCLCPP_WARN_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "There are no stop lines on the closest stop-controlled intersections in the map"); return; } - + // extract the intersection stop line information std::vector stop_lines; @@ -415,12 +415,12 @@ void SCIStrategicPlugin::plan_maneuvers_callback( stop_lines.push_back(stop_bar_dtd); } std::sort(stop_lines.begin(), stop_lines.end()); - + double stop_intersection_down_track = stop_lines.front(); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "stop_intersection_down_track " << stop_intersection_down_track); - - + + double distance_to_stopline = stop_intersection_down_track - current_downtrack_ - config_.stop_line_buffer; RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "distance_to_stopline " << distance_to_stopline); @@ -442,15 +442,15 @@ void SCIStrategicPlugin::plan_maneuvers_callback( intersection_turn_direction_ = getTurnDirectionAtIntersection(intersection_lanelets); uint32_t base_time = street_msg_timestamp_; - + bool time_to_approach_int = int((scheduled_stop_time_) - (street_msg_timestamp_))>0; RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "time_to_approach_int " << time_to_approach_int); - + carma_planning_msgs::msg::Maneuver maneuver_planned; - + if (time_to_approach_int) - { + { auto tmp = (scheduled_stop_time_) - (street_msg_timestamp_); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "tmp " << tmp); double time_to_schedule_stop = (tmp)/1000.0; @@ -463,7 +463,7 @@ void SCIStrategicPlugin::plan_maneuvers_callback( speed_limit_ = findSpeedLimit(crossed_lanelets.front()); // lane following to intersection - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "time_to_schedule_stop " << time_to_schedule_stop); double desired_deceleration = config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier; @@ -491,10 +491,10 @@ void SCIStrategicPlugin::plan_maneuvers_callback( // at the stop line or decelerating to stop line // stop and wait maneuver - + std::vector crossed_lanelets = getLaneletsBetweenWithException(current_downtrack_, stop_intersection_down_track, true, true); - + double stopping_accel = caseThreeSpeedProfile(distance_to_stopline, current_state.speed, time_to_schedule_stop); stopping_accel = std::max(-stopping_accel, desired_deceleration); @@ -503,7 +503,7 @@ void SCIStrategicPlugin::plan_maneuvers_callback( maneuver_planned = composeStopAndWaitManeuverMessage( current_state.downtrack, stop_intersection_down_track, current_state.speed, crossed_lanelets[0].id(), crossed_lanelets[0].id(), stopping_accel, current_state.stamp, - current_state.stamp + rclcpp::Duration(time_to_schedule_stop*1e9)); + current_state.stamp + rclcpp::Duration::from_nanoseconds(time_to_schedule_stop*1e9)); resp->new_plan.maneuvers.push_back(maneuver_planned); @@ -516,7 +516,7 @@ void SCIStrategicPlugin::plan_maneuvers_callback( maneuver_planned = composeStopAndWaitManeuverMessage( current_state.downtrack, stop_intersection_down_track, current_state.speed, crossed_lanelets[0].id(), crossed_lanelets[0].id(), desired_deceleration, current_state.stamp, - current_state.stamp + rclcpp::Duration(time_to_schedule_stop*1e9)); + current_state.stamp + rclcpp::Duration::from_nanoseconds(time_to_schedule_stop*1e9)); } } @@ -539,7 +539,7 @@ void SCIStrategicPlugin::plan_maneuvers_callback( maneuver_planned = composeStopAndWaitManeuverMessage( current_state.downtrack, stop_intersection_down_track, current_state.speed, stop_line_lanelet.id(), stop_line_lanelet.id(), stopping_accel, current_state.stamp, - current_state.stamp + rclcpp::Duration(stop_duration*1e9)); + current_state.stamp + rclcpp::Duration::from_nanoseconds(stop_duration*1e9)); } if (!is_allowed_int_) @@ -557,7 +557,7 @@ void SCIStrategicPlugin::plan_maneuvers_callback( maneuver_planned = composeStopAndWaitManeuverMessage( current_state.downtrack, stop_intersection_down_track, current_state.speed, current_state.lane_id, current_state.lane_id, stop_acc, current_state.stamp, - current_state.stamp + rclcpp::Duration(stop_duration*1e9)); + current_state.stamp + rclcpp::Duration::from_nanoseconds(stop_duration*1e9)); } @@ -573,23 +573,23 @@ void SCIStrategicPlugin::plan_maneuvers_callback( RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Vehicle is crossing the intersection"); // Passing the stop line - + // Compose intersection transit maneuver base_time = std::max(scheduled_enter_time_, street_msg_timestamp_); double intersection_transit_time = (scheduled_depart_time_ - base_time)/1000; RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "intersection_transit_time: " << intersection_transit_time); - + intersection_transit_time = config_.min_maneuver_planning_period;//std::max(intersection_transit_time, config_.min_maneuver_planning_period); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "used intersection_transit_time: " << intersection_transit_time); // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver std::vector crossed_lanelets = getLaneletsBetweenWithException(current_downtrack_, intersection_end_downtrack, true, true); - + // find the turn direction at intersection: intersection_turn_direction_ = getTurnDirectionAtIntersection(crossed_lanelets); - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "turn direction at the intersection is: " << intersection_turn_direction_); double intersection_speed_limit = findSpeedLimit(nearest_stop_intersection->lanelets().front()); @@ -601,9 +601,9 @@ void SCIStrategicPlugin::plan_maneuvers_callback( maneuver_planned = composeIntersectionTransitMessage( current_state.downtrack, current_state.downtrack + end_of_intersection, current_state.speed, intersection_speed_limit, - current_state.stamp, rclcpp::Time(req->header.stamp) + rclcpp::Duration(intersection_transit_time*1e9), intersection_turn_direction_, crossed_lanelets.front().id(), crossed_lanelets.back().id()); - - + current_state.stamp, rclcpp::Time(req->header.stamp) + rclcpp::Duration::from_nanoseconds(intersection_transit_time*1e9), intersection_turn_direction_, crossed_lanelets.front().id(), crossed_lanelets.back().id()); + + if (distance_to_stopline < -end_of_intersection) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Vehicle is out of intersection, stop planning..."); @@ -615,7 +615,7 @@ void SCIStrategicPlugin::plan_maneuvers_callback( } resp->new_plan.maneuvers.push_back(maneuver_planned); - + return; } @@ -649,18 +649,18 @@ TurnDirection SCIStrategicPlugin::getTurnDirectionAtIntersection(std::vector* float_metadata_list) const { double desired_acceleration = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier; - double desired_deceleration = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier; + double desired_deceleration = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier; // Equations obtained from TSMO UC 1 Algorithm draft doc double a_acc = ((1 - desired_acceleration/desired_deceleration)*speed_before_decel - current_speed)/stop_time; RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case one a_acc: " << a_acc); a_acc = std::min(a_acc, desired_acceleration); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Used Case one a_acc: " << a_acc); - + double a_dec = ((desired_deceleration - desired_acceleration)*speed_before_decel - desired_deceleration * current_speed)/(desired_acceleration * stop_time); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case one a_dec: " << a_dec); a_dec = std::max(a_dec, desired_deceleration); @@ -668,7 +668,7 @@ void SCIStrategicPlugin::caseOneSpeedProfile(double speed_before_decel, double c double t_acc = (speed_before_decel - current_speed)/a_acc; RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case one t_acc: " << t_acc); - double t_dec = -speed_before_decel/a_dec; // a_dec is negative so a - is used to make the t_dec positive. + double t_dec = -speed_before_decel/a_dec; // a_dec is negative so a - is used to make the t_dec positive. RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case one t_dec: " << t_dec); float_metadata_list->push_back(a_acc); float_metadata_list->push_back(a_dec); @@ -677,8 +677,8 @@ void SCIStrategicPlugin::caseOneSpeedProfile(double speed_before_decel, double c float_metadata_list->push_back(speed_before_decel); } -void SCIStrategicPlugin::caseTwoSpeedProfile(double stop_dist, double speed_before_decel, - double current_speed, double stop_time, double speed_limit, +void SCIStrategicPlugin::caseTwoSpeedProfile(double stop_dist, double speed_before_decel, + double current_speed, double stop_time, double speed_limit, std::vector* float_metadata_list) const { double desired_acceleration = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier; @@ -689,8 +689,8 @@ void SCIStrategicPlugin::caseTwoSpeedProfile(double stop_dist, double speed_befo speed_before_decel = speed_limit; RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case two speed_before_decel: " << speed_before_decel); } - - double t_c_nom = 2*stop_dist * ((1 - desired_acceleration/desired_deceleration)*speed_limit - current_speed) - + + double t_c_nom = 2*stop_dist * ((1 - desired_acceleration/desired_deceleration)*speed_limit - current_speed) - stop_time * ((1 - desired_acceleration/desired_deceleration)*pow(speed_limit,2) - pow(current_speed, 2)); double t_c_den = pow(speed_limit - current_speed, 2) - (desired_acceleration/desired_deceleration) * pow(speed_limit, 2); double t_cruise = t_c_nom / t_c_den; @@ -709,7 +709,7 @@ void SCIStrategicPlugin::caseTwoSpeedProfile(double stop_dist, double speed_befo double t_acc = (speed_limit - current_speed)/a_acc; RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case two t_acc: " << t_acc); - double t_dec = -speed_limit/a_dec; // a_dec is negative so a - is used to make the t_dec positive. + double t_dec = -speed_limit/a_dec; // a_dec is negative so a - is used to make the t_dec positive. RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Case two t_dec: " << t_dec); float_metadata_list->push_back(a_acc); @@ -737,8 +737,8 @@ carma_planning_msgs::msg::Maneuver SCIStrategicPlugin::composeLaneFollowingManeu carma_planning_msgs::msg::Maneuver empty_msg; maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = config_.strategic_plugin_name; - maneuver_msg.lane_following_maneuver.parameters.presence_vector = - carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_INT_META_DATA | + maneuver_msg.lane_following_maneuver.parameters.presence_vector = + carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_INT_META_DATA | carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA | carma_planning_msgs::msg::ManeuverParameters::HAS_STRING_META_DATA; maneuver_msg.lane_following_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION; maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = config_.lane_following_plugin_name; @@ -747,7 +747,7 @@ carma_planning_msgs::msg::Maneuver SCIStrategicPlugin::composeLaneFollowingManeu maneuver_msg.lane_following_maneuver.start_speed = start_speed; maneuver_msg.lane_following_maneuver.end_speed = target_speed; maneuver_msg.lane_following_maneuver.start_time = start_time; - maneuver_msg.lane_following_maneuver.end_time = start_time + rclcpp::Duration(time_to_stop*1e9); + maneuver_msg.lane_following_maneuver.end_time = start_time + rclcpp::Duration::from_nanoseconds(time_to_stop*1e9); maneuver_msg.lane_following_maneuver.lane_ids = lanelet::utils::transform(lane_ids, [](auto id) { return std::to_string(id); }); @@ -768,12 +768,12 @@ carma_planning_msgs::msg::Maneuver SCIStrategicPlugin::composeLaneFollowingManeu default: return empty_msg; } - - + + maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(case_num); maneuver_msg.lane_following_maneuver.parameters.string_valued_meta_data.push_back(stop_controlled_intersection_strategy_); maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data = float_metadata_list; - + return maneuver_msg; } @@ -807,11 +807,11 @@ carma_v2x_msgs::msg::MobilityOperation SCIStrategicPlugin::generateMobilityOpera if (intersection_turn_direction_ == TurnDirection::Left) intersection_turn_direction = "left"; - mo_.strategy_params = "access: " + std::to_string(is_allowed_int_) + ", max_accel: " + std::to_string(vehicle_acceleration_limit_) + + mo_.strategy_params = "access: " + std::to_string(is_allowed_int_) + ", max_accel: " + std::to_string(vehicle_acceleration_limit_) + ", max_decel: " + std::to_string(vehicle_deceleration_limit_) + ", react_time: " + std::to_string(config_.reaction_time) + - ", min_gap: " + std::to_string(config_.min_gap) + ", depart_pos: " + std::to_string(scheduled_departure_position_) + + ", min_gap: " + std::to_string(config_.min_gap) + ", depart_pos: " + std::to_string(scheduled_departure_position_) + ", turn_direction: " + intersection_turn_direction + ", msg_count: " + std::to_string(bsm_msg_count_) + ", sec_mark: " + std::to_string(bsm_sec_mark_); - + return mo_; } @@ -851,7 +851,7 @@ carma_planning_msgs::msg::Maneuver SCIStrategicPlugin::composeStopAndWaitManeuve maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel); RCLCPP_INFO_STREAM(rclcpp::get_logger("sci_strategic_plugin"), "Creating stop and wait start dist: " << current_dist << " end dist: " << end_dist); - + return maneuver_msg; } @@ -926,8 +926,8 @@ carma_planning_msgs::msg::Maneuver SCIStrategicPlugin::composeIntersectionTransi maneuver_msg.intersection_transit_straight_maneuver.ending_lane_id = std::to_string(ending_lane_id); } - - + + // Start time and end time for maneuver are assigned in updateTimeProgress @@ -953,4 +953,4 @@ std::string SCIStrategicPlugin::get_version_id() #include "rclcpp_components/register_node_macro.hpp" // Register the component with class_loader -RCLCPP_COMPONENTS_REGISTER_NODE(sci_strategic_plugin::SCIStrategicPlugin) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(sci_strategic_plugin::SCIStrategicPlugin) diff --git a/sci_strategic_plugin/test/test_strategic_plugin.cpp b/sci_strategic_plugin/test/test_strategic_plugin.cpp index c41ad9cb47..e37018937c 100644 --- a/sci_strategic_plugin/test/test_strategic_plugin.cpp +++ b/sci_strategic_plugin/test/test_strategic_plugin.cpp @@ -50,7 +50,7 @@ TEST(SCIStrategicPluginTest, composeLaneFollowingManeuverMessage) ASSERT_EQ(5, result.lane_following_maneuver.start_speed); ASSERT_EQ(10, result.lane_following_maneuver.end_speed); ASSERT_EQ(rclcpp::Time(1.2*1e9, RCL_ROS_TIME), result.lane_following_maneuver.start_time); - ASSERT_EQ(rclcpp::Time(1.2*1e9, RCL_ROS_TIME) + rclcpp::Duration(1.0*1e9), result.lane_following_maneuver.end_time); + ASSERT_EQ(rclcpp::Time(1.2*1e9, RCL_ROS_TIME) + rclcpp::Duration::from_nanoseconds(1.0*1e9), result.lane_following_maneuver.end_time); ASSERT_EQ(2, result.lane_following_maneuver.lane_ids.size()); ASSERT_TRUE(result.lane_following_maneuver.lane_ids[0].compare("1200") == 0); ASSERT_TRUE(result.lane_following_maneuver.lane_ids[1].compare("1201") == 0); diff --git a/stop_and_dwell_strategic_plugin/src/stop_and_dwell_strategic_plugin.cpp b/stop_and_dwell_strategic_plugin/src/stop_and_dwell_strategic_plugin.cpp index b40c842bcb..50f65f37d2 100644 --- a/stop_and_dwell_strategic_plugin/src/stop_and_dwell_strategic_plugin.cpp +++ b/stop_and_dwell_strategic_plugin/src/stop_and_dwell_strategic_plugin.cpp @@ -37,10 +37,10 @@ namespace { * \brief Anonymous function to extract maneuver end speed which can not be optained with GET_MANEUVER_PROPERY calls due to it missing in stop and wait plugin * \param mvr input maneuver * \return end speed - */ + */ double getManeuverEndSpeed(const carma_planning_msgs::msg::Maneuver& mvr) { - switch(mvr.type) + switch(mvr.type) { case carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING: return mvr.lane_following_maneuver.end_speed; @@ -109,11 +109,11 @@ carma_ros2_utils::CallbackReturn StopAndDwellStrategicPlugin::on_configure_plugi RCLCPP_INFO_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"),"Done loading parameters: " << config_); // Current Pose Subscriber - current_pose_sub_ = create_subscription("current_pose", 1, + current_pose_sub_ = create_subscription("current_pose", 1, std::bind(&StopAndDwellStrategicPlugin::currentPoseCb,this,std_ph::_1)); // Guidance State subscriber - guidance_state_sub_ = create_subscription("guidance_state", 5, + guidance_state_sub_ = create_subscription("guidance_state", 5, std::bind(&StopAndDwellStrategicPlugin::guidance_state_cb, this, std::placeholders::_1)); // set world model point form wm listener @@ -158,13 +158,13 @@ VehicleState StopAndDwellStrategicPlugin::extractInitialState(const carma_planni else { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "No initial plan provided..."); - + state.stamp = req.header.stamp; state.downtrack = req.veh_downtrack; state.speed = req.veh_logitudinal_velocity; state.lane_id = stoi(req.veh_lane_id); } - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "state.stamp: " << std::to_string(state.stamp.seconds())); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "state.downtrack : " << state.downtrack ); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "state.speed: " << state.speed); @@ -187,7 +187,7 @@ void StopAndDwellStrategicPlugin::currentPoseCb(geometry_msgs::msg::PoseStamped: current_downtrack_ = wm_->routeTrackPos(current_loc).downtrack; RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "Downtrack from current pose: " << current_downtrack_); } - + } std::vector StopAndDwellStrategicPlugin::getLaneletsBetweenWithException(double start_downtrack, @@ -222,7 +222,7 @@ VehicleState StopAndDwellStrategicPlugin::extractInitialState(carma_planning_msg else { RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "No initial plan provided..."); - + state.stamp = rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME); state.downtrack = req->veh_downtrack; state.speed = req->veh_logitudinal_velocity; @@ -237,12 +237,12 @@ VehicleState StopAndDwellStrategicPlugin::extractInitialState(carma_planning_msg } void StopAndDwellStrategicPlugin::plan_maneuvers_callback( - std::shared_ptr srv_header, - carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp) { std::chrono::system_clock::time_point execution_start_time = std::chrono::system_clock::now(); // Start timing the execution time for planning so it can be logged - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "<<<<<<<<<<<<<<<<< STARTING STOP_AND_DWELL_STRATEGIC_PLUIGN!!!!!!!!! >>>>>>>>>>>>>>>>"); if (!wm_->getRoute()) @@ -279,15 +279,15 @@ void StopAndDwellStrategicPlugin::plan_maneuvers_callback( resp->new_plan.maneuvers = {}; RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name_), "Already passed bus stop, sending empty maneuvers"); return; - } + } - constexpr double HALF_MPH_IN_MPS = 0.22352; + constexpr double HALF_MPH_IN_MPS = 0.22352; - if (current_state.speed < HALF_MPH_IN_MPS && fabs(bus_stop_downtrack_ - current_state.downtrack ) < config_.stop_line_buffer) + if (current_state.speed < HALF_MPH_IN_MPS && fabs(bus_stop_downtrack_ - current_state.downtrack ) < config_.stop_line_buffer) { if(first_stop_) { - time_to_move_ = now() + rclcpp::Duration(config_.dwell_time * 1e9); + time_to_move_ = now() + rclcpp::Duration::from_nanoseconds(config_.dwell_time * 1e9); first_stop_ = false; } @@ -296,7 +296,7 @@ void StopAndDwellStrategicPlugin::plan_maneuvers_callback( std::vector crossed_lanelets = getLaneletsBetweenWithException(current_state.downtrack, bus_stop_downtrack_, true, true); auto starting_lane_id = crossed_lanelets.front().id(); auto ending_lane_id = crossed_lanelets.back().id(); - resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(current_state.downtrack ,bus_stop_downtrack_,current_state.speed,starting_lane_id,ending_lane_id,max_comfort_decel_norm_ ,now(),now() + rclcpp::Duration(config_.min_maneuver_planning_period * 1e9) )); + resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(current_state.downtrack ,bus_stop_downtrack_,current_state.speed,starting_lane_id,ending_lane_id,max_comfort_decel_norm_ ,now(),now() + rclcpp::Duration::from_nanoseconds(config_.min_maneuver_planning_period * 1e9) )); } else { @@ -310,30 +310,30 @@ void StopAndDwellStrategicPlugin::plan_maneuvers_callback( else if ( current_state.downtrack>= ( bus_stop_downtrack_ - config_.activation_distance )) { double desired_distance_to_stop = pow(current_state.speed, 2)/(2 * max_comfort_decel_norm_ * config_.deceleration_fraction) + config_.desired_distance_to_stop_buffer; - + if(current_state.downtrack >= ( bus_stop_downtrack_ - desired_distance_to_stop)) { std::vector crossed_lanelets = getLaneletsBetweenWithException(current_state.downtrack, bus_stop_downtrack_, true, true); auto starting_lane_id = crossed_lanelets.front().id(); auto ending_lane_id = crossed_lanelets.back().id(); rclcpp::Time start_time = now(); - rclcpp::Time end_time = now() + rclcpp::Duration(config_.min_maneuver_planning_period * 1e9) ; + rclcpp::Time end_time = now() + rclcpp::Duration::from_nanoseconds(config_.min_maneuver_planning_period * 1e9) ; //Stop at desired distance before bus stop resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(current_state.downtrack ,bus_stop_downtrack_,current_state.speed,starting_lane_id,ending_lane_id,max_comfort_decel_norm_ ,start_time,end_time)); } else - { + { double time_to_stop = (distance_remaining_to_bus_stop - desired_distance_to_stop)/speed_limit_; - rclcpp::Time timestamp_to_stop = now() + rclcpp::Duration(time_to_stop * 1e9); + rclcpp::Time timestamp_to_stop = now() + rclcpp::Duration::from_nanoseconds(time_to_stop * 1e9); std::vector crossed_lanelets = getLaneletsBetweenWithException(current_state.downtrack, (bus_stop_downtrack_ - desired_distance_to_stop) , true, true); std::vector crossed_lanelets_stop = getLaneletsBetweenWithException((bus_stop_downtrack_ - desired_distance_to_stop), bus_stop_downtrack_, true, true); std::vector lane_ids = lanelet::utils::transform(crossed_lanelets, [](const auto& ll) { return ll.id(); }); - + auto starting_lane_id = crossed_lanelets_stop.front().id(); auto ending_lane_id = crossed_lanelets_stop.back().id(); resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(current_state.downtrack ,(bus_stop_downtrack_ - desired_distance_to_stop),current_state.speed,speed_limit_,now(), time_to_stop,lane_ids)); - resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage((bus_stop_downtrack_ - desired_distance_to_stop),bus_stop_downtrack_,speed_limit_,starting_lane_id,ending_lane_id,max_comfort_decel_norm_ ,timestamp_to_stop ,(timestamp_to_stop + rclcpp::Duration(config_.min_maneuver_planning_period * 1e9)))); + resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage((bus_stop_downtrack_ - desired_distance_to_stop),bus_stop_downtrack_,speed_limit_,starting_lane_id,ending_lane_id,max_comfort_decel_norm_ ,timestamp_to_stop ,(timestamp_to_stop + rclcpp::Duration::from_nanoseconds(config_.min_maneuver_planning_period * 1e9)))); } } std::chrono::system_clock::time_point execution_end_time = std::chrono::system_clock::now(); // Planning complete @@ -352,7 +352,7 @@ carma_planning_msgs::msg::Maneuver StopAndDwellStrategicPlugin::composeLaneFollo carma_planning_msgs::msg::Maneuver empty_msg; maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = config_.strategic_plugin_name; - maneuver_msg.lane_following_maneuver.parameters.presence_vector = + maneuver_msg.lane_following_maneuver.parameters.presence_vector = carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN; maneuver_msg.lane_following_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION; maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = config_.lane_following_plugin_name; @@ -361,7 +361,7 @@ carma_planning_msgs::msg::Maneuver StopAndDwellStrategicPlugin::composeLaneFollo maneuver_msg.lane_following_maneuver.start_speed = start_speed; maneuver_msg.lane_following_maneuver.end_speed = target_speed; maneuver_msg.lane_following_maneuver.start_time = start_time; - maneuver_msg.lane_following_maneuver.end_time = start_time + rclcpp::Duration(time_to_stop*1e9); + maneuver_msg.lane_following_maneuver.end_time = start_time + rclcpp::Duration::from_nanoseconds(time_to_stop*1e9); maneuver_msg.lane_following_maneuver.lane_ids = lanelet::utils::transform(lane_ids, [](auto id) { return std::to_string(id); }); @@ -408,7 +408,7 @@ carma_planning_msgs::msg::Maneuver StopAndDwellStrategicPlugin::composeStopAndWa maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel); RCLCPP_INFO_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "Creating stop and wait start dist: " << current_dist << " end dist: " << end_dist); - + return maneuver_msg; } @@ -427,4 +427,4 @@ std::string StopAndDwellStrategicPlugin::get_version_id() #include "rclcpp_components/register_node_macro.hpp" // Register the component with class_loader -RCLCPP_COMPONENTS_REGISTER_NODE(stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin) diff --git a/stop_and_dwell_strategic_plugin/test/test_strategic_plugin.cpp b/stop_and_dwell_strategic_plugin/test/test_strategic_plugin.cpp index b8fa8ad559..6c823784bc 100644 --- a/stop_and_dwell_strategic_plugin/test/test_strategic_plugin.cpp +++ b/stop_and_dwell_strategic_plugin/test/test_strategic_plugin.cpp @@ -49,7 +49,7 @@ TEST(StopAndDwellStrategicPluginTest, composeLaneFollowingManeuverMessage) ASSERT_EQ(5, result.lane_following_maneuver.start_speed); ASSERT_EQ(10, result.lane_following_maneuver.end_speed); ASSERT_EQ(rclcpp::Time(1.2*1e9, RCL_ROS_TIME), result.lane_following_maneuver.start_time); - ASSERT_EQ(rclcpp::Time(1.2*1e9, RCL_ROS_TIME) + rclcpp::Duration(1.0*1e9), result.lane_following_maneuver.end_time); + ASSERT_EQ(rclcpp::Time(1.2*1e9, RCL_ROS_TIME) + rclcpp::Duration::from_nanoseconds(1.0*1e9), result.lane_following_maneuver.end_time); ASSERT_EQ(2, result.lane_following_maneuver.lane_ids.size()); ASSERT_TRUE(result.lane_following_maneuver.lane_ids[0].compare("1200") == 0); ASSERT_TRUE(result.lane_following_maneuver.lane_ids[1].compare("1201") == 0); @@ -87,7 +87,7 @@ TEST(StopAndDwellStrategicPluginTest, findSpeedLimit) auto sd_node = std::make_shared(rclcpp::NodeOptions()); sd_node->configure(); sd_node->activate(); - + std::shared_ptr wm; carma_wm::test::MapOptions options; options.lane_length_ = 25; @@ -102,7 +102,7 @@ TEST(StopAndDwellStrategicPluginTest, findSpeedLimit) auto ll_iterator = wm->getMap()->laneletLayer.find(1200); if (ll_iterator == wm->getMap()->laneletLayer.end()) FAIL() << "Expected lanelet not present in map. Unit test may not be structured correctly"; - + ASSERT_NEAR(11.176, sd_node->findSpeedLimit(*ll_iterator), 0.00001); } @@ -121,7 +121,7 @@ TEST(StopAndDwellStrategicPluginTest, maneuvercbtest) // Create a complete map carma_wm::test::MapOptions mp(5,40); auto cmw_ptr = carma_wm::test::getGuidanceTestMap(mp); - + cmw_ptr->getMutableMap()->update(cmw_ptr->getMutableMap()->laneletLayer.get(1200), bus_stop_rule); carma_wm::test::setRouteByIds({1200, 1201, 1202, 1203}, cmw_ptr); @@ -137,7 +137,7 @@ TEST(StopAndDwellStrategicPluginTest, maneuvercbtest) // approaching bus stop req->veh_x = 2.5; - req->veh_y = 0.0; + req->veh_y = 0.0; req->veh_downtrack = req->veh_y; req->veh_logitudinal_velocity = 1.0; req->veh_lane_id = "1200"; @@ -149,7 +149,7 @@ TEST(StopAndDwellStrategicPluginTest, maneuvercbtest) // approaching to stop resp->new_plan.maneuvers = {}; req->veh_x = 2.5; - req->veh_y = 10.0; + req->veh_y = 10.0; req->veh_downtrack = req->veh_y; req->veh_logitudinal_velocity = 5.0; req->veh_lane_id = "1200"; @@ -161,7 +161,7 @@ TEST(StopAndDwellStrategicPluginTest, maneuvercbtest) // passed bus stop resp->new_plan.maneuvers = {}; req->veh_x = 2.5; - req->veh_y = 20.4; + req->veh_y = 20.4; req->veh_downtrack = req->veh_y; req->veh_logitudinal_velocity = 0.2; req->veh_lane_id = "1200"; @@ -173,14 +173,14 @@ TEST(StopAndDwellStrategicPluginTest, maneuvercbtest) // stopped and waiting resp->new_plan.maneuvers = {}; req->veh_x = 2.5; - req->veh_y = 19.0; + req->veh_y = 19.0; req->veh_downtrack = req->veh_y; req->veh_logitudinal_velocity = 0.0; req->veh_lane_id = "1200"; sd_node->plan_maneuvers_callback(srv_header, req, resp); ASSERT_EQ(1, resp->new_plan.maneuvers.size()); - + } -} // namespace stop_and_dwell_strategic_plugin \ No newline at end of file +} // namespace stop_and_dwell_strategic_plugin diff --git a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp index 9cc9a55cf7..6c06bf2219 100644 --- a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp @@ -390,10 +390,10 @@ std::vector StopandWait::compose_ auto traj = trajectory_from_points_times_orientations(raw_points, times, yaws, start_time); - while (rclcpp::Time(traj.back().target_time) - rclcpp::Time(traj.front().target_time) < rclcpp::Duration(config_.minimal_trajectory_duration * 1e9)) + while (rclcpp::Time(traj.back().target_time) - rclcpp::Time(traj.front().target_time) < rclcpp::Duration::from_nanoseconds(config_.minimal_trajectory_duration * 1e9)) { carma_planning_msgs::msg::TrajectoryPlanPoint new_point = traj.back(); - new_point.target_time = rclcpp::Time(new_point.target_time) + rclcpp::Duration(config_.stop_timestep * 1e9); + new_point.target_time = rclcpp::Time(new_point.target_time) + rclcpp::Duration::from_nanoseconds(config_.stop_timestep * 1e9); new_point.planner_plugin_name = plugin_name_; traj.push_back(new_point); } @@ -423,4 +423,4 @@ void StopandWait::set_yield_client(carma_ros2_utils::ClientPtr(rclcpp::NodeOptions()); node->configure(); node->activate(); - + auto map = carma_wm::test::buildGuidanceTestMap(3.7, 50); wm->setMap(map); - + carma_wm::test::setSpeedLimit(40_mph, wm); /** * Total route length should be 100m @@ -79,19 +79,19 @@ namespace stop_controlled_intersection_tactical_plugin maneuver.lane_following_maneuver.parameters.int_valued_meta_data.push_back(1); //Case number maneuver.lane_following_maneuver.parameters.string_valued_meta_data.push_back("Carma/stop_controlled_intersection"); //Float meta data list - a_acc, a_dec, t_acc, t_dec, speed_before_decel - + //Calculate speed before decel - The speed the vehicle should accelerate before slowing down //Assuming the a_dec to be 0.5m/s^2, the vehicle should be able to stop at that deceleration- over total_dist/2 //So accelerate half way and decelerate the rest double a_dec = -1.5; double assumed_dec_dist = maneuver.lane_following_maneuver.end_dist/2; double speed_before_dec = sqrt(2*std::abs(a_dec)*assumed_dec_dist); - + double a_acc = (pow(speed_before_dec,2) - pow(maneuver.lane_following_maneuver.start_speed,2))/(2*(maneuver.lane_following_maneuver.end_dist - assumed_dec_dist)); double t_acc = (speed_before_dec - maneuver.lane_following_maneuver.start_speed)/a_acc; double t_dec = (speed_before_dec)/std::abs(a_dec); - maneuver.lane_following_maneuver.end_time = rclcpp::Time(maneuver.lane_following_maneuver.start_time) + rclcpp::Duration((t_acc + t_dec)*1e9); + maneuver.lane_following_maneuver.end_time = rclcpp::Time(maneuver.lane_following_maneuver.start_time) + rclcpp::Duration::from_nanoseconds((t_acc + t_dec)*1e9); maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(a_acc); maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(a_dec); @@ -123,18 +123,18 @@ namespace stop_controlled_intersection_tactical_plugin //Ensure acceleration and deceleration is happening double dist_to_cover = maneuver.lane_following_maneuver.end_dist - maneuver.lane_following_maneuver.start_dist; double total_dist_covered = 0; - + double prev_speed = case_one_profile[0].speed; lanelet::BasicPoint2d prev_point = case_one_profile[0].point; double current_speed; lanelet::BasicPoint2d current_point; - + for(int i = 0;i < case_one_profile.size();i++){ current_point = case_one_profile[i].point; current_speed = case_one_profile[i].speed; double delta_d = lanelet::geometry::distance2d(prev_point, current_point); - total_dist_covered += delta_d; - if(total_dist_covered < 50.1){ //According to test conditions acceleration till approx 50 meters + total_dist_covered += delta_d; + if(total_dist_covered < 50.1){ //According to test conditions acceleration till approx 50 meters EXPECT_TRUE(current_speed >= prev_speed); } else{ @@ -154,13 +154,13 @@ namespace stop_controlled_intersection_tactical_plugin TEST(StopControlledIntersectionTacticalPlugin, TestSCIPlanning_case_two){ //Test Stop controlled Intersection tactical plugin generation - + StopControlledIntersectionTacticalPluginConfig config; std::shared_ptr wm = std::make_shared(); auto node = std::make_shared(rclcpp::NodeOptions()); node->configure(); node->activate(); - + auto map = carma_wm::test::buildGuidanceTestMap(3.7, 50); wm->setMap(map); @@ -180,9 +180,9 @@ namespace stop_controlled_intersection_tactical_plugin * START_LINE */ - carma_wm::test::setRouteByIds({1200, 1201, 1202, 1203}, wm); + carma_wm::test::setRouteByIds({1200, 1201, 1202, 1203}, wm); node->wm_ = wm; - + //Create a request and maneuver that meets case 2 criteria //speed_before_stop auto req = std::make_shared(); @@ -205,7 +205,7 @@ namespace stop_controlled_intersection_tactical_plugin maneuver.lane_following_maneuver.parameters.string_valued_meta_data.push_back("Carma/stop_controlled_intersection"); //Float meta data list - a_acc, a_dec, t_acc, t_dec, t_cruise, speed_before_decel - + //Calculate speed before decel - The speed the vehicle should accelerate and then cruise before slowing down //Assuming the a_dec to be 2m/s^2, the vehicle should be able to stop at that deceleration starting from speed limit @@ -220,7 +220,7 @@ namespace stop_controlled_intersection_tactical_plugin double t_dec = speed_before_decel/std::abs(a_dec); double t_cruising = dist_cruising/speed_before_decel; - maneuver.lane_following_maneuver.end_time = rclcpp::Time(maneuver.lane_following_maneuver.start_time) + rclcpp::Duration((t_acc + t_cruising + t_dec)*1e9); + maneuver.lane_following_maneuver.end_time = rclcpp::Time(maneuver.lane_following_maneuver.start_time) + rclcpp::Duration::from_nanoseconds((t_acc + t_cruising + t_dec)*1e9); maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(a_acc); maneuver.lane_following_maneuver.parameters.float_valued_meta_data.push_back(a_dec); @@ -240,9 +240,9 @@ namespace stop_controlled_intersection_tactical_plugin //Test create_case_two_speed_profile std::vector route_geometry_points = wm->sampleRoutePoints( - std::min(maneuver.lane_following_maneuver.start_dist + 1.0, maneuver.lane_following_maneuver.end_dist), + std::min(maneuver.lane_following_maneuver.start_dist + 1.0, maneuver.lane_following_maneuver.end_dist), maneuver.lane_following_maneuver.end_dist, 1.0); - + std::vector case_two_profile = node->create_case_two_speed_profile(wm, maneuver, route_geometry_points, req->vehicle_state.longitudinal_vel); double prev_speed = case_two_profile[0].speed; @@ -257,7 +257,7 @@ namespace stop_controlled_intersection_tactical_plugin else{ EXPECT_TRUE(current_speed < prev_speed); } - + prev_speed = current_speed; } @@ -271,7 +271,7 @@ namespace stop_controlled_intersection_tactical_plugin auto node = std::make_shared(rclcpp::NodeOptions()); node->configure(); node->activate(); - + auto map = carma_wm::test::buildGuidanceTestMap(3.7, 50); wm->setMap(map); @@ -291,7 +291,7 @@ namespace stop_controlled_intersection_tactical_plugin * START_LINE */ - carma_wm::test::setRouteByIds({1200, 1201, 1202, 1203}, wm); + carma_wm::test::setRouteByIds({1200, 1201, 1202, 1203}, wm); node->wm_ = wm; //Create a request and maneuver that meets case 3 criteria //In order to be case 2 - estimated_stop_time > scheduled_stop_time and speed_before_decel = speed_limit @@ -321,15 +321,15 @@ namespace stop_controlled_intersection_tactical_plugin //Test create_case_three_speed_profile std::vector route_geometry_points = wm->sampleRoutePoints( - std::min(maneuver.lane_following_maneuver.start_dist , maneuver.lane_following_maneuver.end_dist), + std::min(maneuver.lane_following_maneuver.start_dist , maneuver.lane_following_maneuver.end_dist), maneuver.lane_following_maneuver.end_dist, 1.0); - + std::vector case_three_profile = node->create_case_three_speed_profile(wm, maneuver, route_geometry_points, req->vehicle_state.longitudinal_vel); double prev_speed = case_three_profile.front().speed; for(int i = 1;i now().nanoseconds() / 1e6; - long start_duration = rclcpp::Duration(config_.startup_duration_, 0).nanoseconds() / 1e6; + long start_duration = rclcpp::Duration::from_nanoseconds(config_.startup_duration_, 0).nanoseconds() / 1e6; auto sys_alert_msg_from_ssc = ssc_driver_manager_->get_latest_system_alert(time_now, start_up_timestamp_, start_duration); diff --git a/yield_plugin/src/yield_plugin.cpp b/yield_plugin/src/yield_plugin.cpp index d9306a91a1..fbe347e954 100644 --- a/yield_plugin/src/yield_plugin.cpp +++ b/yield_plugin/src/yield_plugin.cpp @@ -513,7 +513,7 @@ namespace yield_plugin // Derived from constant accelaration kinematic equation: (vi + vf) / 2 * dt = d_dist // This also handles a case correctly when current_speed is 0, but prev_speed is not 0 yet const double dt = (2 * original_traj_relative_downtracks.at(i)) / (current_speed + prev_speed); - jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration(dt*1e9); + jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration::from_nanoseconds(dt*1e9); if (prev_speed < EPSILON) // Handle a special case if prev_speed (thus current_speed too) is 0 { @@ -522,7 +522,7 @@ namespace yield_plugin // if the vehicle goes past the point, it may cruise toward undesirable location (for example into the intersection). // Keeping the points help the controller steer the vehicle toward direction of travel even when stopping. // Only downside is the trajectory plan is huge where only 15 sec is expected, but since this is stopping case, it shouldn't matter. - jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration(6000 * 1e9); + jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration::from_nanoseconds(6000 * 1e9); RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Zero speed = x: " << jmt_tpp.x << ", y:" << jmt_tpp.y << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds()) << ", prev_speed: " << prev_speed << ", current_speed: " << current_speed); diff --git a/yield_plugin/test/test_yield.cpp b/yield_plugin/test/test_yield.cpp index cfe0979b86..94adb8c8f7 100755 --- a/yield_plugin/test/test_yield.cpp +++ b/yield_plugin/test/test_yield.cpp @@ -123,35 +123,35 @@ TEST(YieldPluginTest, MaxTrajectorySpeed) carma_planning_msgs::msg::TrajectoryPlanPoint point_2; point_2.x = 5.0; point_2.y = 0.0; - point_2.target_time = startTime + rclcpp::Duration(1*1e9); + point_2.target_time = startTime + rclcpp::Duration::from_nanoseconds(1*1e9); point_2.lane_id = "1"; trajectory_points.push_back(point_2); carma_planning_msgs::msg::TrajectoryPlanPoint point_3; point_3.x = 10.0; point_3.y = 0.0; - point_3.target_time = startTime + rclcpp::Duration(2*1e9); + point_3.target_time = startTime + rclcpp::Duration::from_nanoseconds(2*1e9); point_3.lane_id = "1"; trajectory_points.push_back(point_3); carma_planning_msgs::msg::TrajectoryPlanPoint point_4; point_4.x = 15.0; point_4.y = 0.0; - point_4.target_time = startTime + rclcpp::Duration(3*1e9); + point_4.target_time = startTime + rclcpp::Duration::from_nanoseconds(3*1e9); point_4.lane_id = "1"; trajectory_points.push_back(point_4); carma_planning_msgs::msg::TrajectoryPlanPoint point_5; point_5.x = 20.0; point_5.y = 0.0; - point_5.target_time = startTime + rclcpp::Duration(4*1e9); + point_5.target_time = startTime + rclcpp::Duration::from_nanoseconds(4*1e9); point_5.lane_id = "1"; trajectory_points.push_back(point_5); carma_planning_msgs::msg::TrajectoryPlanPoint point_6; point_6.x = 25.0; point_6.y = 0.0; - point_6.target_time = startTime + rclcpp::Duration(5*1e9); + point_6.target_time = startTime + rclcpp::Duration::from_nanoseconds(5*1e9); point_6.lane_id = "1"; trajectory_points.push_back(point_6); @@ -159,7 +159,7 @@ TEST(YieldPluginTest, MaxTrajectorySpeed) carma_planning_msgs::msg::TrajectoryPlanPoint point_7; point_7.x = 40.0; point_7.y = 0.0; - point_7.target_time = startTime + rclcpp::Duration(6*1e9); + point_7.target_time = startTime + rclcpp::Duration::from_nanoseconds(6*1e9); point_7.lane_id = "1"; trajectory_points.push_back(point_7); @@ -677,7 +677,7 @@ TEST(YieldPluginTest, test_update_traj2) carma_planning_msgs::msg::TrajectoryPlanPoint new_tpp; new_tpp.x = original_tp.trajectory_points[i].x; new_tpp.y = original_tp.trajectory_points[i].y; - new_tpp.target_time = rclcpp::Time(new_trajectory_points[0].target_time) + rclcpp::Duration((original_traj_downtracks[i]/dv)*1e9); + new_tpp.target_time = rclcpp::Time(new_trajectory_points[0].target_time) + rclcpp::Duration::from_nanoseconds((original_traj_downtracks[i]/dv)*1e9); new_trajectory_points.push_back(new_tpp); } @@ -784,14 +784,14 @@ TEST(YieldPluginTest, test_update_traj_stop) } // trajectory point is copied to move all the available information, then its target time is updated new_tpp = original_tp.trajectory_points[i]; - new_tpp.target_time = rclcpp::Time(new_trajectory_points[i-1].target_time) + rclcpp::Duration((original_traj_downtracks[i]/dv)*1e9); + new_tpp.target_time = rclcpp::Time(new_trajectory_points[i-1].target_time) + rclcpp::Duration::from_nanoseconds((original_traj_downtracks[i]/dv)*1e9); new_trajectory_points.push_back(new_tpp); } else { RCLCPP_WARN(rclcpp::get_logger("yield_plugin"),"target speed is zero"); new_tpp = new_trajectory_points[i-1]; - new_tpp.target_time = rclcpp::Time(new_trajectory_points[0].target_time) + rclcpp::Duration(traj_target_time*1e9); + new_tpp.target_time = rclcpp::Time(new_trajectory_points[0].target_time) + rclcpp::Duration::from_nanoseconds(traj_target_time*1e9); new_trajectory_points.push_back(new_tpp); } new_speeds.push_back(dv);