From ee96e47e1ff4f908898520a1ff969b4c2d251146 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Thu, 30 Nov 2023 15:03:42 -0500 Subject: [PATCH 01/18] carma_wm --- carma_wm/src/CARMAWorldModel.cpp | 34 +++++------ carma_wm/src/WMListenerWorker.cpp | 58 +++++++++---------- carma_wm/src/WorldModelUtils.cpp | 94 +++++++++++++++++++------------ 3 files changed, 105 insertions(+), 81 deletions(-) diff --git a/carma_wm/src/CARMAWorldModel.cpp b/carma_wm/src/CARMAWorldModel.cpp index 36d3fa3194..9d9ee8d615 100755 --- a/carma_wm/src/CARMAWorldModel.cpp +++ b/carma_wm/src/CARMAWorldModel.cpp @@ -82,10 +82,10 @@ namespace carma_wm } lanelet::Id CARMAWorldModel::getTrafficSignalId(uint16_t intersection_id, uint8_t signal_group_id) - { + { lanelet::Id inter_id = lanelet::InvalId; lanelet::Id signal_id = lanelet::InvalId; - + if (sim_.intersection_id_to_regem_id_.find(intersection_id) != sim_.intersection_id_to_regem_id_.end()) { inter_id = sim_.intersection_id_to_regem_id_[intersection_id]; @@ -95,7 +95,7 @@ namespace carma_wm { signal_id = sim_.signal_group_to_traffic_light_id_[signal_group_id]; } - + return signal_id; } @@ -133,15 +133,15 @@ namespace carma_wm std::vector bus_stop_list; auto curr_downtrack = routeTrackPos(loc).downtrack; // shortpath is already sorted by distance - + for (const auto &ll : route_->shortestPath()) { auto bus_stops = semantic_map_->laneletLayer.get(ll.id()).regulatoryElementsAs(); - if (bus_stops.empty()) + if (bus_stops.empty()) { continue; } - + for (auto bus_stop : bus_stops) { auto stop_line = bus_stop->stopAndWaitLine(); @@ -335,7 +335,7 @@ namespace carma_wm if (!bounds_inclusive) // reduce bounds slightly to avoid including exact bounds { - if (std::max(min.downtrack, start + 0.00001) > std::min(max.downtrack, end - 0.00001) + if (std::max(min.downtrack, start + 0.00001) > std::min(max.downtrack, end - 0.00001) || (start == end && (min.downtrack >= start || max.downtrack <= end))) { // Check for 1d intersection // No intersection so continue @@ -344,7 +344,7 @@ namespace carma_wm } else { - if (std::max(min.downtrack, start) > std::min(max.downtrack, end) + if (std::max(min.downtrack, start) > std::min(max.downtrack, end) || (start == end && (min.downtrack > start || max.downtrack < end))) { // Check for 1d intersection // No intersection so continue @@ -699,7 +699,7 @@ namespace carma_wm // Append distance to current centerline size_t offset = lineStrings.size() == 0 || lineStrings.back().size() == 0 ? - 0 : + 0 : 1; // Prevent duplicate points when concatenating. Not clear if causes an issue at lane changes if ((nextCenterline.size() <= 1) || (nextCenterline.size() <= 2 && offset == 1)) { @@ -1222,7 +1222,7 @@ namespace carma_wm std::vector light_list; auto curr_downtrack = routeTrackPos(loc).downtrack; // shortpath is already sorted by distance - + for (const auto &ll : route_->shortestPath()) { auto lights = semantic_map_->laneletLayer.get(ll.id()).regulatoryElementsAs(); @@ -1230,7 +1230,7 @@ namespace carma_wm { continue; } - + for (auto light : lights) { auto stop_line = light->getStopLine(ll); @@ -1380,7 +1380,7 @@ namespace carma_wm } lanelet::CarmaTrafficSignalPtr CARMAWorldModel::getTrafficSignal(const lanelet::Id& id) const - { + { auto general_regem = semantic_map_->regulatoryElementLayer.get(id); auto lanelets_general = semantic_map_->laneletLayer.findUsages(general_regem); @@ -1464,9 +1464,9 @@ namespace carma_wm RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm"), "No intersection_state_list in the newly received SPAT msg. Returning..."); return; } - + for (const auto& curr_intersection : spat_msg.intersection_state_list) - { + { for (const auto& current_movement_state : curr_intersection.movement_list) { @@ -1478,7 +1478,7 @@ namespace carma_wm } lanelet::CarmaTrafficSignalPtr curr_light = getTrafficSignal(curr_light_id); - + if (curr_light == nullptr) { continue; @@ -1518,11 +1518,11 @@ namespace carma_wm start_time_dynamic=min_end_time_converter_minute_of_year(start_time_dynamic,curr_intersection.moy_exists,curr_intersection.moy, use_sim_time); // Accounting minute of the year auto received_state_dynamic = static_cast(current_movement_event.event_state.movement_phase_state); - + sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].push_back(std::make_pair(min_end_time_dynamic, received_state_dynamic)); sim_.traffic_signal_start_times_[curr_intersection.id.id][current_movement_state.signal_group].push_back( start_time_dynamic); - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "intersection id: " << (int)curr_intersection.id.id << ", signal: " << (int)current_movement_state.signal_group << ", start_time: " << std::to_string(lanelet::time::toSec(start_time_dynamic)) << ", end_time: " << std::to_string(lanelet::time::toSec(min_end_time_dynamic)) diff --git a/carma_wm/src/WMListenerWorker.cpp b/carma_wm/src/WMListenerWorker.cpp index 8beb6736dd..ffd8135171 100644 --- a/carma_wm/src/WMListenerWorker.cpp +++ b/carma_wm/src/WMListenerWorker.cpp @@ -25,7 +25,7 @@ namespace carma_wm { -enum class GeofenceType{ INVALID, DIGITAL_SPEED_LIMIT, PASSING_CONTROL_LINE, REGION_ACCESS_RULE, DIGITAL_MINIMUM_GAP, +enum class GeofenceType{ INVALID, DIGITAL_SPEED_LIMIT, PASSING_CONTROL_LINE, REGION_ACCESS_RULE, DIGITAL_MINIMUM_GAP, DIRECTION_OF_TRAVEL, STOP_RULE, CARMA_TRAFFIC_LIGHT, SIGNALIZED_INTERSECTION/* ... others */ }; // helper function that return geofence type as an enum, which makes it cleaner by allowing switch statement GeofenceType resolveGeofenceType(const std::string& rule_name) @@ -66,7 +66,7 @@ void WMListenerWorker::mapCallback(const autoware_lanelet2_msgs::msg::MapBin::Un // After setting map evaluate the current update queue to apply any updates that arrived before the map bool more_updates_to_apply = true; while(!map_update_queue_.empty() && more_updates_to_apply) { - + auto update = std::move(map_update_queue_.front()); // Get first update map_update_queue_.pop(); // Remove update from queue @@ -121,32 +121,32 @@ void logSignalizedIntersectionManager(const carma_wm::SignalizedIntersectionMana { for (auto pair : sim.intersection_id_to_regem_id_) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "inter id: " << (int)pair.first << ", regem id: " << pair.second); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "inter id: " << (int)pair.first << ", regem id: " << pair.second); } for (auto pair : sim.signal_group_to_entry_lanelet_ids_) { for (auto iter = pair.second.begin(); iter != pair.second.end(); iter++) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", entry llt id: " << *iter); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", entry llt id: " << *iter); } } for (auto pair : sim.signal_group_to_exit_lanelet_ids_) { for (auto iter = pair.second.begin(); iter != pair.second.end(); iter++) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", exit llt id: " << *iter); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", exit llt id: " << *iter); } } for (auto pair : sim.signal_group_to_traffic_light_id_) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", regem id: " << pair.second); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", regem id: " << pair.second); } } void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::UniquePtr geofence_msg) { RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Map Update Being Evaluated. SeqNum: " << geofence_msg->seq_id); - if (rerouting_flag_) // no update should be applied if rerouting + if (rerouting_flag_) // no update should be applied if rerouting { RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Currently new route is being processed. Queueing this update. Received seq: " << geofence_msg->seq_id << " prev seq: " << most_recent_update_msg_seq_); map_update_queue_.emplace(std::move(geofence_msg)); @@ -170,7 +170,7 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un if(geofence_msg->invalidates_route==true && world_model_->getRoute()) - { + { rerouting_flag_=true; recompute_route_flag_ = true; @@ -187,21 +187,21 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un most_recent_update_msg_seq_ = geofence_msg->seq_id; // Update current sequence count auto gf_ptr = std::shared_ptr(new carma_wm::TrafficControl); - + // convert ros msg to geofence object carma_wm::fromBinMsg(*geofence_msg, gf_ptr, world_model_->getMutableMap()); RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Processing Map Update with Geofence Id:" << gf_ptr->id_); - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " requests addition of lanelets size: " << gf_ptr->lanelet_additions_.size()); for (auto llt : gf_ptr->lanelet_additions_) { // world model here should blindly accept the map update received RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Adding new lanelet with id: " << llt.id()); auto left = llt.leftBound3d(); //new lanelet coming in - + // updating incoming points' memory addresses with local ones of same ids - // so that lanelet library can recognize they are same objects + // so that lanelet library can recognize they are same objects for (size_t i = 0; i < left.size(); i ++) { if (world_model_->getMutableMap()->pointLayer.exists(left[i].id())) //rewrite the memory address of new pts with that of local @@ -251,7 +251,7 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Regems left in lanelet after removal: " << parent_llt.regulatoryElements().size()); } - + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " requests update of size: " << gf_ptr->update_list_.size()); // we should extract general regem to specific type of regem the geofence specifies @@ -264,20 +264,20 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un // if this regem is already in the map. // This section is expected to be called to add back regulations which were previously removed by expired geofences. - if (regemptr_it != world_model_->getMutableMap()->regulatoryElementLayer.end()) + if (regemptr_it != world_model_->getMutableMap()->regulatoryElementLayer.end()) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Reapplying previously existing element for lanelet id:" << parent_llt.id() << ", and regem id: " << regemptr_it->get()->id()); // again we should use the element with correct data address to be consistent world_model_->getMutableMap()->update(parent_llt, *regemptr_it); } - else // Updates are treated as new regulations after the old value was removed. In both cases we enter this block. + else // Updates are treated as new regulations after the old value was removed. In both cases we enter this block. { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "New regulatory element at lanelet: " << parent_llt.id() << ", and id: " << pair.second->id()); newRegemUpdateHelper(parent_llt, pair.second.get()); } } - + // set the Map to trigger a new route graph construction if rerouting was required by the updates and a new graph was not provided world_model_->setMap(world_model_->getMutableMap(), current_map_version_, recompute_route_flag_ && !geofence_msg->has_routing_graph ); @@ -298,9 +298,9 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un // no need to reroute again unless received invalidated msg again if (recompute_route_flag_) recompute_route_flag_ = false; - - RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Finished Applying the Map Update with Geofence Id:" << gf_ptr->id_); + + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Finished Applying the Map Update with Geofence Id:" << gf_ptr->id_); // Call user defined map callback if (map_callback_) @@ -365,7 +365,7 @@ void WMListenerWorker::newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet { std::invalid_argument("Dynamic Pointer cast failed on getting valid region access rule"); } - + break; } case GeofenceType::DIGITAL_MINIMUM_GAP: @@ -380,7 +380,7 @@ void WMListenerWorker::newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet { std::invalid_argument("Dynamic Pointer cast failed on getting valid minimum gap rule"); } - + break; } case GeofenceType::DIRECTION_OF_TRAVEL: @@ -395,7 +395,7 @@ void WMListenerWorker::newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet { std::invalid_argument("Dynamic Pointer cast failed on getting valid direction of travel"); } - + break; } case GeofenceType::STOP_RULE: @@ -436,7 +436,7 @@ void WMListenerWorker::newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet { std::invalid_argument("Dynamic Pointer cast failed on getting valid signalized intersection"); } - + break; } default: @@ -449,8 +449,8 @@ LaneletRoutingGraphPtr WMListenerWorker::routingGraphFromMsg(const autoware_lane if (msg.participant_type.compare(getVehicleParticipationType()) != 0) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"Received routing graph does not have matching vehicle type for world model. WM Type: " - << getVehicleParticipationType() + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"Received routing graph does not have matching vehicle type for world model. WM Type: " + << getVehicleParticipationType() << " graph type: " << msg.participant_type ); @@ -543,14 +543,14 @@ LaneletRoutingGraphPtr WMListenerWorker::routingGraphFromMsg(const autoware_lane // Create edge graph->addEdge( - lanelet, - map->laneletLayer.get(vertex.lanelet_or_area_ids[j]), + lanelet, + map->laneletLayer.get(vertex.lanelet_or_area_ids[j]), lanelet::routing::internal::EdgeInfo{vertex.edge_routing_costs[j], vertex.edge_routing_cost_source_ids[j], relation} ); } catch(const lanelet::NoSuchPrimitiveError& e) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"Received routing graph specifies lanelets which do not match the current map version. Not found lanelet or area: " + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"Received routing graph specifies lanelets which do not match the current map version. Not found lanelet or area: " << vertex.lanelet_or_area_ids[j] << " Actual exception: " << e.what()); return nullptr; @@ -601,7 +601,7 @@ void WMListenerWorker::routeCallback(const carma_planning_msgs::msg::Route::Uniq while(!map_update_queue_.empty() && more_updates_to_apply) { auto update = std::move(map_update_queue_.front()); // Get first update - map_update_queue_.pop(); // Remove update from queue + map_update_queue_.pop(); // Remove update from queue if (update->map_version < current_map_version_) { // Drop any so far unapplied updates for the current map RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Apply from reroute: There were unapplied updates in carma_wm when a new map was recieved."); @@ -701,7 +701,7 @@ double WMListenerWorker::getConfigSpeedLimit() const } void WMListenerWorker::setVehicleParticipationType(std::string participant) -{ +{ //Function to load participation type into CarmaWorldModel world_model_->setVehicleParticipationType(participant); } diff --git a/carma_wm/src/WorldModelUtils.cpp b/carma_wm/src/WorldModelUtils.cpp index 998f5ba4d9..c07ca60db3 100644 --- a/carma_wm/src/WorldModelUtils.cpp +++ b/carma_wm/src/WorldModelUtils.cpp @@ -15,6 +15,9 @@ */ #include +#include +#include +#include namespace carma_wm { @@ -24,6 +27,9 @@ namespace query std::vector getLaneletsFromPoint(const lanelet::LaneletMapConstPtr& semantic_map, const lanelet::BasicPoint2d& point, const unsigned int n) { + using Point = boost::geometry::model::d2::point_xy; + using Polygon = boost::geometry::model::polygon; + // Check if the map is loaded yet if (!semantic_map || semantic_map->laneletLayer.size() == 0) { @@ -34,15 +40,33 @@ std::vector getLaneletsFromPoint(const lanelet::LaneletMa if (nearestLanelets.size() == 0) return {}; - size_t id = 0; // closest ones are in the back + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::query"), "check: x: " << point.x() << ", y: " << point.y()); + // closest ones are in the back // loop through until the point is no longer geometrically in the lanelet - - while (boost::geometry::within(point, nearestLanelets[id].second.polygon2d())) + for (size_t id = 0; id < nearestLanelets.size(); id++) { - possible_lanelets.push_back(nearestLanelets[id].second); - id++; - if (id >= nearestLanelets.size()) - break; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::query"), "nearest size: " << nearestLanelets.size() << ", checking: " << nearestLanelets[id].second.id()); + auto poly_linestrings = nearestLanelets[id].second.leftBound2d(); + auto poly_linestrings1 = nearestLanelets[id].second.rightBound2d(); + //RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::query"), "======start===="); + + + Polygon new_poly; + for (const auto& temp_point : poly_linestrings) { + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::query"), "x: " << temp_point.x() << ", y: " << temp_point.y()); + boost::geometry::append(new_poly, Point(temp_point.x(), temp_point.y())); + } + for (const auto& temp_point : poly_linestrings1.invert()) { + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::query"), "x: " << temp_point.x() << ", y: " << temp_point.y()); + boost::geometry::append(new_poly, Point(temp_point.x(), temp_point.y())); + } + boost::geometry::append(new_poly, Point(poly_linestrings.front().x(), poly_linestrings.front().y())); + //RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::query"), "x: " << poly_linestrings.front().x() << ", y: " << poly_linestrings.front().y()); + + //RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::query"), "======END===="); + + if (boost::geometry::within(point, new_poly)) + possible_lanelets.push_back(nearestLanelets[id].second); } return possible_lanelets; } @@ -63,11 +87,11 @@ std::vector getLaneletsFromPoint(const lanelet::LaneletMapPtr& std::vector nonConnectedAdjacentLeft(const lanelet::LaneletMapPtr& semantic_map, const lanelet::BasicPoint2d& point, const unsigned int n) { - + lanelet::LaneletMapConstPtr const_ptr = semantic_map; auto possible_lanelets = nonConnectedAdjacentLeft(const_ptr, point, n); - - + + std::vector return_lanelets; for (auto llt : possible_lanelets) { @@ -79,24 +103,24 @@ std::vector nonConnectedAdjacentLeft(const lanelet::LaneletMap std::vector nonConnectedAdjacentLeft(const lanelet::LaneletMapConstPtr& semantic_map, const lanelet::BasicPoint2d& input_point, const unsigned int n) { - + // Check if the map is loaded yet if (!semantic_map || semantic_map->laneletLayer.size() == 0) { throw std::invalid_argument("Map is not set or does not contain lanelets"); } - + auto input_lanelets = getLaneletsFromPoint(semantic_map, input_point); - + if (input_lanelets.empty()) { throw std::invalid_argument("Input point x: " + std::to_string(input_point.x()) + ", y: " + std::to_string(input_point.y()) + " is not in the map"); } - - auto input_lanelet = input_lanelets[0]; - + + auto input_lanelet = input_lanelets[0]; + auto point_downtrack = carma_wm::geometry::trackPos(input_lanelet, input_point).downtrack; - + auto point_downtrack_ratio = point_downtrack / carma_wm::geometry::trackPos(input_lanelet, input_lanelet.centerline().back().basicPoint2d()).downtrack; auto point_on_ls = input_lanelet.leftBound2d()[std::round((input_lanelet.leftBound2d().size()- 1) * point_downtrack_ratio) ]; @@ -125,13 +149,13 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& std::unordered_set possible_lanelets; // This loop identifes the lanelets which this point lies within that could be impacted by the geofence - // This loop somewhat inefficiently calls the findNearest method iteratively until all the possible lanelets are identified. + // This loop somewhat inefficiently calls the findNearest method iteratively until all the possible lanelets are identified. // The reason findNearest is used instead of nearestUntil is because that method orders results by bounding box which - // can give invalid sequences when dealing with large curved lanelets. - bool continue_search = true; + // can give invalid sequences when dealing with large curved lanelets. + bool continue_search = true; size_t nearest_count = 0; while (continue_search) { - + nearest_count += 10; // Increase the index search radius by 10 each loop until all nearby lanelets are found for (const auto& ll_pair : lanelet::geometry::findNearest(lanelet_map->laneletLayer, gf_pts[idx].basicPoint2d(), nearest_count)) { // Get the nearest lanelets and iterate over them @@ -143,7 +167,7 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& double dist = std::get<0>(ll_pair); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Distance to lanelet " << ll.id() << ": " << dist << " max_lane_width: " << max_lane_width); - + if (dist > max_lane_width) { // Only save values closer than max_lane_width. Since we are iterating in distance order when we reach this distance the search can stop continue_search = false; break; @@ -184,12 +208,12 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Overlaps starting line... Picking llt: " << llt.id()); affected_lanelets.insert(llt); - } + } } } break; - } + } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Checking possible lanelets"); // check if each lines connecting end points of the llt is crossing with the line connecting current and next gf_pts @@ -198,7 +222,7 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Evaluating lanelet: " << llt.id()); lanelet::BasicLineString2d gf_dir_line({gf_pts[idx].basicPoint2d(), gf_pts[idx+1].basicPoint2d()}); lanelet::BasicLineString2d llt_boundary({(llt.leftBound2d().end() -1)->basicPoint2d(), (llt.rightBound2d().end() - 1)->basicPoint2d()}); - + // record the llts that are on the same dir if (boost::geometry::intersects(llt_boundary, gf_dir_line)) { @@ -206,11 +230,11 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& affected_lanelets.insert(llt); } // check condition if two geofence points are in one lanelet then check matching direction and record it also - else if (boost::geometry::within(gf_pts[idx+1].basicPoint2d(), llt.polygon2d()) && + else if (boost::geometry::within(gf_pts[idx+1].basicPoint2d(), llt.polygon2d()) && affected_lanelets.find(llt) == affected_lanelets.end()) - { + { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Within new lanelet"); - lanelet::BasicPoint2d median({((llt.leftBound2d().end() - 1)->basicPoint2d().x() + (llt.rightBound2d().end() - 1)->basicPoint2d().x())/2 , + lanelet::BasicPoint2d median({((llt.leftBound2d().end() - 1)->basicPoint2d().x() + (llt.rightBound2d().end() - 1)->basicPoint2d().x())/2 , ((llt.leftBound2d().end() - 1)->basicPoint2d().y() + (llt.rightBound2d().end() - 1)->basicPoint2d().y())/2}); // turn into vectors Eigen::Vector2d vec_to_median(median); @@ -234,8 +258,8 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "interior_angle: " << interior_angle); // Save the lanelet if the direction of two points inside aligns with that of the lanelet - if (interior_angle < M_PI_2 && interior_angle >= 0) - affected_lanelets.insert(llt); + if (interior_angle < M_PI_2 && interior_angle >= 0) + affected_lanelets.insert(llt); } else { @@ -244,9 +268,9 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& } } - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "affected_lanelets size: " << affected_lanelets.size()); - // Currently only returning lanelet, but this could be expanded to LanelerOrArea compound object + // Currently only returning lanelet, but this could be expanded to LanelerOrArea compound object // by implementing non-const version of that LaneletOrArea lanelet::ConstLaneletOrAreas affected_parts; // return results in ascending downtrack order from the first point of geofence @@ -262,7 +286,7 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& { affected_parts.push_back(pair.second); } - + return affected_parts; } @@ -273,10 +297,10 @@ std::unordered_set filterSuccessorLanelets(const std::unordere if (!routing_graph) { throw std::invalid_argument("No routing graph available"); } - + std::unordered_set filtered_lanelets; // we utilize routes to filter llts that are overlapping but not connected - // as this is the last lanelet + // as this is the last lanelet // we have to filter the llts that are only geometrically overlapping yet not connected to prev llts for (auto recorded_llt: root_lanelets) { From a95025f4021446d5750d84b81873887c13c728ee Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Thu, 30 Nov 2023 15:09:25 -0500 Subject: [PATCH 02/18] yield_plugin fixes --- yield_plugin/config/parameters.yaml | 10 +- yield_plugin/src/yield_plugin.cpp | 281 +++++++++++++++++----------- 2 files changed, 173 insertions(+), 118 deletions(-) diff --git a/yield_plugin/config/parameters.yaml b/yield_plugin/config/parameters.yaml index ca5b321613..fe7e5b5264 100644 --- a/yield_plugin/config/parameters.yaml +++ b/yield_plugin/config/parameters.yaml @@ -1,8 +1,8 @@ -# Adjustment factor for safe and comfortable acceleration/deceleration +# Adjustment factor for safe and comfortable acceleration/deceleration # Value type: Desired # No unit -acceleration_adjustment_factor: 4.0 +acceleration_adjustment_factor: 4.0 # Time horizon for collision detection for vehicles on the route # NOTE: not applied for pedestrians or other non-vehicle obstacles on the road # Units: s @@ -11,7 +11,7 @@ on_route_vehicle_collision_horizon: 10.0 # Minimum speed for moving obstacle # Units: m/s # Value type: Desired -min_obstacle_speed: 2.0 +min_obstacle_speed: 0.5 # Minimum object avoidance planning time # Units: s # Value type: Desired @@ -39,10 +39,10 @@ vehicle_height: 3.0 # Maximum speed value to consider the ego vehicle stopped # Units: m/s # Value type: Desired -max_stop_speed: 1.0 +max_stop_speed: 1.0 # parameter to enable cooperative behavior # Value type: Desired -enable_cooperative_behavior: true +enable_cooperative_behavior: true # parameter to always accept mobility request # Value type: Desired always_accept_mobility_request: true diff --git a/yield_plugin/src/yield_plugin.cpp b/yield_plugin/src/yield_plugin.cpp index e767af3b5e..036fe97a53 100644 --- a/yield_plugin/src/yield_plugin.cpp +++ b/yield_plugin/src/yield_plugin.cpp @@ -60,7 +60,7 @@ namespace yield_plugin for (size_t i=0; iprojectECEF( { (double)ecef_point.ecef_x/100.0, (double)ecef_point.ecef_y/100.0, (double)ecef_point.ecef_z/100.0 } , 1); - + return lanelet::traits::to2D(map_point); - } - - + } + + carma_v2x_msgs::msg::MobilityResponse YieldPlugin::compose_mobility_response(const std::string& resp_recipient_id, const std::string& req_plan_id, bool response) const { @@ -123,7 +123,7 @@ namespace yield_plugin out_mobility_response.is_accepted = true; } else out_mobility_response.is_accepted = false; - + return out_mobility_response; } @@ -140,12 +140,12 @@ namespace yield_plugin } if (incoming_request.plan_type.type == carma_v2x_msgs::msg::PlanType::CHANGE_LANE_LEFT || incoming_request.plan_type.type == carma_v2x_msgs::msg::PlanType::CHANGE_LANE_RIGHT) { - RCLCPP_DEBUG(nh_->get_logger(),"Cooperative Lane Change Request Received"); + RCLCPP_ERROR(nh_->get_logger(),"Cooperative Lane Change Request Received"); lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_RECEIVED; lc_status_msg.description = "Received lane merge request"; if (incoming_request.m_header.recipient_id == config_.vehicle_id) { - RCLCPP_DEBUG(nh_->get_logger(),"CLC Request correctly received"); + RCLCPP_ERROR(nh_->get_logger(),"CLC Request correctly received"); } // extract mobility header std::string req_sender_id = incoming_request.m_header.sender_id; @@ -155,7 +155,7 @@ namespace yield_plugin carma_v2x_msgs::msg::Trajectory incoming_trajectory = incoming_request.trajectory; std::string req_strategy_params = incoming_request.strategy_params; clc_urgency_ = incoming_request.urgency; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"received urgency: " << clc_urgency_); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"received urgency: " << clc_urgency_); // Parse strategy parameters using boost::property_tree::ptree; @@ -167,9 +167,9 @@ namespace yield_plugin int start_lanelet_id = pt.get("sl"); int end_lanelet_id = pt.get("el"); double req_traj_speed = (double)req_traj_speed_full + (double)(req_traj_fractional)/10.0; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_traj_speed" << req_traj_speed); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"start_lanelet_id" << start_lanelet_id); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"end_lanelet_id" << end_lanelet_id); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"req_traj_speed" << req_traj_speed); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"start_lanelet_id" << start_lanelet_id); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"end_lanelet_id" << end_lanelet_id); std::vector req_traj_plan = {}; @@ -184,30 +184,30 @@ namespace yield_plugin double req_timestamp = (double)incoming_request.m_header.timestamp / 1000.0 - current_time_sec; set_incoming_request_info(req_traj_plan, req_traj_speed, req_plan_time, req_timestamp); - + if (req_expiration_sec - current_time_sec >= config_.tpmin && cooperative_request_acceptable_) { timesteps_since_last_req_ = 0; lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_ACCEPTED; lc_status_msg.description = "Accepted lane merge request"; - response_to_clc_req = true; - RCLCPP_DEBUG(nh_->get_logger(),"CLC accepted"); + response_to_clc_req = true; + RCLCPP_ERROR(nh_->get_logger(),"CLC accepted"); } else { lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_REJECTED; lc_status_msg.description = "Rejected lane merge request"; response_to_clc_req = false; - RCLCPP_DEBUG(nh_->get_logger(),"CLC rejected"); + RCLCPP_ERROR(nh_->get_logger(),"CLC rejected"); } carma_v2x_msgs::msg::MobilityResponse outgoing_response = compose_mobility_response(req_sender_id, req_plan_id, response_to_clc_req); mobility_response_publisher_(outgoing_response); lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::RESPONSE_SENT; - RCLCPP_DEBUG(nh_->get_logger(),"response sent"); + RCLCPP_ERROR(nh_->get_logger(),"response sent"); } } lc_status_publisher_(lc_status_msg); - + } void YieldPlugin::set_incoming_request_info(std::vector req_trajectory, double req_speed, double req_planning_time, double req_timestamp) @@ -215,7 +215,7 @@ namespace yield_plugin req_trajectory_points_ = req_trajectory; req_target_speed_ = req_speed; req_target_plan_time_ = req_planning_time; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_target_plan_time_" << req_target_plan_time_); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"req_target_plan_time_" << req_target_plan_time_); req_timestamp_ = req_timestamp; } @@ -226,9 +226,11 @@ namespace yield_plugin } void YieldPlugin::plan_trajectory_callback( - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { + RCLCPP_ERROR(nh_->get_logger(),"yield_plugin was called!"); + if (req->initial_trajectory_plan.trajectory_points.size() < 2){ throw std::invalid_argument("Empty Trajectory received by Yield"); } @@ -241,35 +243,36 @@ namespace yield_plugin // seperating cooperative yield with regular object detection for better performance. if (config_.enable_cooperative_behavior && clc_urgency_ > config_.acceptable_urgency) { - RCLCPP_DEBUG(nh_->get_logger(),"Only consider high urgency clc"); + RCLCPP_ERROR(nh_->get_logger(),"Only consider high urgency clc"); if (timesteps_since_last_req_ < config_.acceptable_passed_timesteps) { - RCLCPP_DEBUG(nh_->get_logger(),"Yield for CLC. We haven't received an updated negotiation this timestep"); + RCLCPP_ERROR(nh_->get_logger(),"Yield for CLC. We haven't received an updated negotiation this timestep"); yield_trajectory = update_traj_for_cooperative_behavior(original_trajectory, req->vehicle_state.longitudinal_vel); timesteps_since_last_req_++; } else { - RCLCPP_DEBUG(nh_->get_logger(),"unreliable CLC communication, switching to object avoidance"); + RCLCPP_ERROR(nh_->get_logger(),"unreliable CLC communication, switching to object avoidance"); yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, req->vehicle_state.longitudinal_vel); // Compute the trajectory - } + } } else { - RCLCPP_DEBUG(nh_->get_logger(),"Yield for object avoidance"); + RCLCPP_ERROR(nh_->get_logger(),"Yield for object avoidance"); yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, req->vehicle_state.longitudinal_vel); // Compute the trajectory } yield_trajectory.header.frame_id = "map"; yield_trajectory.header.stamp = nh_->now(); yield_trajectory.trajectory_id = original_trajectory.trajectory_id; - yield_trajectory.initial_longitudinal_velocity = original_trajectory.initial_longitudinal_velocity;//copy the original trajectory's desired speed for now. + yield_trajectory.initial_longitudinal_velocity = original_trajectory.initial_longitudinal_velocity;//copy the original trajectory's desired speed for now. resp->trajectory_plan = yield_trajectory; rclcpp::Time end_time = system_clock.now(); // Planning complete auto duration = end_time - start_time; - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ExecutionTime: " << std::to_string(duration.seconds())); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "ExecutionTime: " << std::to_string(duration.seconds())); + RCLCPP_ERROR(nh_->get_logger(),"yield_plugin was ended!"); } @@ -300,14 +303,14 @@ namespace yield_plugin double dy = original_tp.trajectory_points[0].y - intersection_point.y(); // check if a digital_gap is available double digital_gap = check_traj_for_digital_min_gap(original_tp); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap); goal_pos = sqrt(dx*dx + dy*dy) - config_.x_gap; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Goal position (goal_pos): " << goal_pos); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"Goal position (goal_pos): " << goal_pos); double collision_time = req_timestamp_ + (intersection_points[0].first * ecef_traj_timestep_) - config_.safety_collision_time_gap; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req time stamp: " << req_timestamp_); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Collision time: " << collision_time); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"intersection num: " << intersection_points[0].first); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Planning time: " << planning_time); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"req time stamp: " << req_timestamp_); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"Collision time: " << collision_time); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"intersection num: " << intersection_points[0].first); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"Planning time: " << planning_time); // calculate distance traveled from beginning of trajectory to collision point double dx2 = intersection_point.x() - req_trajectory_points_[0].x(); double dy2 = intersection_point.y() - req_trajectory_points_[0].y(); @@ -317,33 +320,33 @@ namespace yield_plugin goal_velocity = std::min(goal_velocity, incoming_trajectory_speed); double min_time = (initial_velocity - goal_velocity)/config_.yield_max_deceleration; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"goal_velocity: " << goal_velocity); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"incoming_trajectory_speed: " << incoming_trajectory_speed); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"goal_velocity: " << goal_velocity); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"incoming_trajectory_speed: " << incoming_trajectory_speed); if (planning_time > min_time) { cooperative_request_acceptable_ = true; - cooperative_trajectory = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time); + cooperative_trajectory = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time); } else { cooperative_request_acceptable_ = false; - RCLCPP_DEBUG(nh_->get_logger(),"The incoming requested trajectory is rejected, due to insufficient gap"); + RCLCPP_ERROR(nh_->get_logger(),"The incoming requested trajectory is rejected, due to insufficient gap"); cooperative_trajectory = original_tp; } - + } else { cooperative_request_acceptable_ = true; - RCLCPP_DEBUG(nh_->get_logger(),"The incoming requested trajectory does not overlap with host vehicle's trajectory"); + RCLCPP_ERROR(nh_->get_logger(),"The incoming requested trajectory does not overlap with host vehicle's trajectory"); cooperative_trajectory = original_tp; } return cooperative_trajectory; } - carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::generate_JMT_trajectory(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time) + carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::generate_JMT_trajectory(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time) { carma_planning_msgs::msg::TrajectoryPlan jmt_trajectory; std::vector jmt_trajectory_points; @@ -354,19 +357,23 @@ namespace yield_plugin double goal_accel = 0.0; double original_max_speed = max_trajectory_speed(original_tp.trajectory_points); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"original_max_speed" << original_max_speed); - std::vector values = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos, + RCLCPP_ERROR_STREAM(nh_->get_logger(),"original_max_speed" << original_max_speed); + std::vector values = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos, goal_pos, - initial_velocity, - goal_velocity, - initial_accel, - goal_accel, - initial_time, + initial_velocity, + goal_velocity, + initial_accel, + goal_accel, + initial_time, planning_time); - + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 1 and size for original_tp.trajectory_points: " << original_tp.trajectory_points.size()); + std::vector original_traj_downtracks = get_relative_downtracks(original_tp); std::vector calculated_speeds = {}; calculated_speeds.push_back(initial_velocity); + + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 2 and size: " << original_tp.trajectory_points.size()); + for(size_t i = 1; i < original_tp.trajectory_points.size(); i++ ) { double traj_target_time = i * planning_time / original_tp.trajectory_points.size(); @@ -379,65 +386,86 @@ namespace yield_plugin } calculated_speeds.push_back(dv); } + + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 3"); + // moving average filter std::vector filtered_speeds = basic_autonomy::smoothing::moving_average_filter(calculated_speeds, config_.speed_moving_average_window_size); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 4 and filtered_speeds size: " << filtered_speeds.size()); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 4 and original_tp.trajectory_points.size(): " << original_tp.trajectory_points.size()); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 4 and original_traj_downtracks: " << original_traj_downtracks.size()); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 4 and original_traj_downtracks: " << original_traj_downtracks.size()); + + + double prev_speed = filtered_speeds[0]; + for(size_t i = 1; i < original_tp.trajectory_points.size(); i++ ) { carma_planning_msgs::msg::TrajectoryPlanPoint jmt_tpp; + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 4.a and index: " << i); + double current_speed = filtered_speeds.at(i); double traj_target_time = i * planning_time / original_tp.trajectory_points.size(); if (current_speed >= config_.max_stop_speed) { + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 4.b and jmt_trajectory_points.size(): " << jmt_trajectory_points.size()); + double dt = (2 * original_traj_downtracks.at(i)) / (current_speed + prev_speed); jmt_tpp = original_tp.trajectory_points.at(i); - jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.at(i-1).target_time) + rclcpp::Duration(dt*1e9); - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "non-empty x: " << jmt_tpp.x << ", y:" << jmt_tpp.y << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())); + jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration(dt*1e9); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "non-empty x: " << jmt_tpp.x << ", y:" << jmt_tpp.y << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())); jmt_trajectory_points.push_back(jmt_tpp); } else { - RCLCPP_DEBUG(nh_->get_logger(),"target speed is zero"); + RCLCPP_ERROR(nh_->get_logger(),"target speed is zero"); // if speed is zero, the vehicle will stay in previous location. - jmt_tpp = jmt_trajectory_points.at(i-1); - // MISH: Potential bug where if purepursuit accidentally picks a point that has previous time, then it may speed up. target_time is assumed to increase - jmt_tpp.target_time = rclcpp::Time(std::max((rclcpp::Time(jmt_trajectory_points[0].target_time) + rclcpp::Duration(traj_target_time*1e9)).seconds(), rclcpp::Time(jmt_trajectory_points[i -1 ].target_time).seconds()) * 1e9); - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "empty x: " << jmt_tpp.x << ", y:" << jmt_tpp.y << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())); + jmt_tpp = jmt_trajectory_points.back(); + // MISH: Potential bug where if purepursuit accidentally picks a point that has previous time, then it may speed up. target_time is assumed to increase + jmt_tpp.target_time = rclcpp::Time(std::max((rclcpp::Time(jmt_trajectory_points[0].target_time) + rclcpp::Duration(traj_target_time*1e9)).seconds(), rclcpp::Time(jmt_trajectory_points.back().target_time).seconds()) * 1e9); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "empty x: " << jmt_tpp.x << ", y:" << jmt_tpp.y << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())); jmt_trajectory_points.push_back(jmt_tpp); } prev_speed = current_speed; } - + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 5 and jmt_trajectory_points size: " << jmt_trajectory_points.size()); + + jmt_trajectory.header = original_tp.header; jmt_trajectory.trajectory_id = original_tp.trajectory_id; jmt_trajectory.trajectory_points = jmt_trajectory_points; return jmt_trajectory; } - - std::optional YieldPlugin::detect_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory1, const std::vector& trajectory2, double collision_radius) + + std::optional YieldPlugin::detect_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory1, const std::vector& trajectory2, double collision_radius) { // Iterate through each pair of consecutive points in the trajectories - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Starting a new collision detection, trajectory size: " << trajectory1.trajectory_points.size() << ". prediction size: " << trajectory2.size()); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Starting a new collision detection, trajectory size: " << trajectory1.trajectory_points.size() << ". prediction size: " << trajectory2.size()); // Iterate through the object to check if it's on the route bool on_route = false; int on_route_idx = 0; + for (const auto& route_id : route_llt_ids_) + { + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Route llts: " << route_id); + } - for (size_t j = 0; j < trajectory2.size(); ++j) + for (size_t j = 0; j < trajectory2.size(); j+=4) { lanelet::BasicPoint2d curr_point; curr_point.x() = trajectory2.at(j).predicted_position.position.x; curr_point.y() = trajectory2.at(j).predicted_position.position.y; - auto corresponding_lanelets = wm_->getLaneletsFromPoint(curr_point, 4); // get 4 lanelets + auto corresponding_lanelets = wm_->getLaneletsFromPoint(curr_point, 10); // get 4 lanelets for (const auto& llt: corresponding_lanelets) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Checking llt: " << llt.id()); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Checking llt: " << llt.id()); if (route_llt_ids_.find(llt.id()) != route_llt_ids_.end()) { @@ -452,16 +480,16 @@ namespace yield_plugin if (!on_route) { - RCLCPP_DEBUG(nh_->get_logger(), "Predicted states are not on the route! ignoring"); + RCLCPP_ERROR(nh_->get_logger(), "Predicted states are not on the route! ignoring"); return std::nullopt; } - + double smallest_dist = std::numeric_limits::max(); - for (size_t i = 0; i < trajectory1.trajectory_points.size() - 1; ++i) + for (size_t i = 0; i < trajectory1.trajectory_points.size() - 1; ++i) { auto p1a = trajectory1.trajectory_points.at(i); auto p1b = trajectory1.trajectory_points.at(i + 1); - for (size_t j = on_route_idx; j < trajectory2.size() - 1; ++j) + for (size_t j = on_route_idx; j < trajectory2.size() - 1; ++j) { auto p2a = trajectory2.at(j); auto p2b = trajectory2.at(j + 1); @@ -470,13 +498,13 @@ namespace yield_plugin double p2a_t = rclcpp::Time(p2a.header.stamp).seconds(); double p2b_t = rclcpp::Time(p2b.header.stamp).seconds(); - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1a.target_time: " << std::to_string(p1a_t) << ", p1b.target_time: " << std::to_string(p1b_t)); - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2a.target_time: " << std::to_string(p2a_t) << ", p2b.target_time: " << std::to_string(p2b_t)); - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1a.x: " << p1a.x << ", p1a.y: " << p1a.y); - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1b.x: " << p1b.x << ", p1b.y: " << p1b.y); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "p1a.target_time: " << std::to_string(p1a_t) << ", p1b.target_time: " << std::to_string(p1b_t)); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "p2a.target_time: " << std::to_string(p2a_t) << ", p2b.target_time: " << std::to_string(p2b_t)); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "p1a.x: " << p1a.x << ", p1a.y: " << p1a.y); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "p1b.x: " << p1b.x << ", p1b.y: " << p1b.y); - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2a.x: " << p2a.predicted_position.position.x << ", p2a.y: " << p2a.predicted_position.position.y); - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2b.x: " << p2b.predicted_position.position.x << ", p2b.y: " << p2b.predicted_position.position.y); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "p2a.x: " << p2a.predicted_position.position.x << ", p2a.y: " << p2a.predicted_position.position.y); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "p2b.x: " << p2b.predicted_position.position.x << ", p2b.y: " << p2b.predicted_position.position.y); // Linearly interpolate positions at a common timestamp for both trajectories double dt = (p2a_t - p1a_t) / (p1b_t - p1a_t); @@ -488,15 +516,15 @@ namespace yield_plugin // Calculate the distance between the two interpolated points double distance = std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)); smallest_dist = std::min(distance, smallest_dist); - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Smallest_dist: " << smallest_dist << ", distance: " << distance << ", dt: " << dt << ", x1: " << x1 << ", y1: " <get_logger(), "Smallest_dist: " << smallest_dist << ", distance: " << distance << ", dt: " << dt << ", x1: " << x1 << ", y1: " < config_.collision_check_radius) { - RCLCPP_DEBUG(nh_->get_logger(), "Too far away" ); + RCLCPP_ERROR(nh_->get_logger(), "Too far away" ); return std::nullopt; } - if (distance > collision_radius) + if (distance > collision_radius) { continue; } @@ -516,7 +544,7 @@ namespace yield_plugin { RCLCPP_WARN_STREAM(nh_->get_logger(), "Collision detected at timestamp " << std::to_string(p2a_t) << ", x: " << x1 << ", y: " << y1 << ", within distance: " << distance); return rclcpp::Time(p2a.header.stamp); - } + } } } @@ -524,9 +552,14 @@ namespace yield_plugin return std::nullopt; } - carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector& external_objects, double initial_velocity) - { - if (original_tp.trajectory_points.empty()) + carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector& external_objects, double initial_velocity) + { + if (original_tp.trajectory_points.size() < 2) + { + RCLCPP_ERROR(nh_->get_logger(), "Yield plugin received less than 2 points in update_traj_for_object, returning unchanged..."); + return original_tp; + } + else if (original_tp.trajectory_points.empty()) { RCLCPP_ERROR(nh_->get_logger(), "Yield plugin received empty trajectory plan in update_traj_for_object"); throw std::invalid_argument("Yield plugin received empty trajectory plan in update_traj_for_object"); @@ -541,7 +574,7 @@ namespace yield_plugin std::set checked_external_object_ids; std::vector new_list; - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ExternalObjects size: " << external_objects.size()); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "ExternalObjects size: " << external_objects.size()); if (wm_->getRoute() == nullptr) { @@ -557,11 +590,23 @@ namespace yield_plugin double earliest_collision_obj_time = std::numeric_limits::max(); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"External Object List (external_objects) size: " << external_objects.size()); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"External Object List (external_objects) size: " << external_objects.size()); for (const auto& curr_obstacle : external_objects) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Object's back time: " << std::to_string(rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds()) << ", plan_start_time: " << std::to_string(plan_start_time.seconds())); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Object's back time: " << std::to_string(rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds()) << ", plan_start_time: " << std::to_string(plan_start_time.seconds())); + if (curr_obstacle.object_type != 4) //TODO + { + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Skipping not pedestrian"); + continue; + } + + if (curr_obstacle.velocity.twist.linear.x < 1.0) //TODO + { + RCLCPP_ERROR_STREAM(nh_->get_logger(), "pedestrian but not moving!"); + continue; + } + RCLCPP_ERROR_STREAM(nh_->get_logger(), "===================== Checking Collision! ===================="); // do not process outdated objects if (rclcpp::Time(curr_obstacle.predictions.back().header.stamp) > plan_start_time) @@ -576,7 +621,7 @@ namespace yield_plugin new_list.insert(new_list.end(), curr_obstacle.predictions.cbegin(), curr_obstacle.predictions.cend()); auto collision_time = detect_collision_time(original_tp, new_list, config_.intervehicle_collision_distance); - + if (collision_time != std::nullopt) { checked_external_object_ids.insert(curr_obstacle.id); @@ -593,25 +638,25 @@ namespace yield_plugin lanelet::BasicPoint2d point(original_tp.trajectory_points[0].x,original_tp.trajectory_points[0].y); double vehicle_downtrack = wm_->routeTrackPos(point).downtrack; - - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"vehicle_downtrack: " << vehicle_downtrack); - // Issue (https://github.com/usdot-fhwa-stol/carma-platform/issues/2155): If the yield_plugin can detect if the roadway object is moving along the route, - // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon. + RCLCPP_ERROR_STREAM(nh_->get_logger(),"vehicle_downtrack: " << vehicle_downtrack); + + // Issue (https://github.com/usdot-fhwa-stol/carma-platform/issues/2155): If the yield_plugin can detect if the roadway object is moving along the route, + // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon. if(earliest_collision_obj.has_value()) { - RCLCPP_WARN_STREAM(nh_->get_logger(),"Collision Detected!"); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"Collision Detected!"); lanelet::BasicPoint2d point_o(earliest_collision_obj.value().pose.pose.position.x, earliest_collision_obj.value().pose.pose.position.y); double object_downtrack = wm_->routeTrackPos(point_o).downtrack; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack: " << object_downtrack); - + RCLCPP_ERROR_STREAM(nh_->get_logger(),"object_downtrack: " << object_downtrack); + double dist_to_object = object_downtrack - vehicle_downtrack; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"dist_to_object: " << dist_to_object); - - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object speed: " << earliest_collision_obj.value().velocity.twist.linear.x); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"dist_to_object: " << dist_to_object); + + RCLCPP_ERROR_STREAM(nh_->get_logger(),"object speed: " << earliest_collision_obj.value().velocity.twist.linear.x); // Distance from the original trajectory point to the lead vehicle/object double dist_x = earliest_collision_obj.value().pose.pose.position.x - original_tp.trajectory_points[0].x; @@ -628,15 +673,16 @@ namespace yield_plugin { // check if a digital_gap is available double digital_gap = check_traj_for_digital_min_gap(original_tp); - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap); - // if a digital gap is available, it is replaced as safety gap + RCLCPP_ERROR_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap); + // if a digital gap is available, it is replaced as safety gap safety_gap = std::max(safety_gap, digital_gap); } // safety gap is implemented - double goal_pos = x_lead - safety_gap; + double goal_pos = x_lead - safety_gap; if (goal_velocity <= config_.min_obstacle_speed){ - RCLCPP_WARN(nh_->get_logger(),"The obstacle is not moving"); + RCLCPP_ERROR(nh_->get_logger(),"The obstacle is not moving, goal velocity is set to 0"); + goal_velocity = 0.0; } double initial_time = 0; @@ -666,15 +712,15 @@ namespace yield_plugin { tp = t_ref; } - - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Object avoidance planning time: " << tp); + + RCLCPP_ERROR_STREAM(nh_->get_logger(),"Object avoidance planning time: " << tp); update_tpp_vector = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, tp); return update_tpp_vector; } - - RCLCPP_DEBUG(nh_->get_logger(),"No collision detection, so trajectory not modified."); + + RCLCPP_ERROR(nh_->get_logger(),"No collision detection, so trajectory not modified."); return original_tp; } @@ -682,14 +728,23 @@ namespace yield_plugin std::vector YieldPlugin::get_relative_downtracks(const carma_planning_msgs::msg::TrajectoryPlan& trajectory_plan) const { std::vector downtracks; + RCLCPP_ERROR(nh_->get_logger(),"Sub yield 1"); + downtracks.reserve(trajectory_plan.trajectory_points.size()); + RCLCPP_ERROR(nh_->get_logger(),"Sub yield 2"); + // relative downtrack distance of the fist Point is 0.0 - downtracks[0] = 0.0; + downtracks.push_back(0.0); + RCLCPP_ERROR(nh_->get_logger(),"Sub yield 2a"); for (size_t i=1; i < trajectory_plan.trajectory_points.size(); i++){ + RCLCPP_ERROR_STREAM(nh_->get_logger(),"Sub yield 3, id: " << (int)i); + double dx = trajectory_plan.trajectory_points.at(i).x - trajectory_plan.trajectory_points.at(i-1).x; double dy = trajectory_plan.trajectory_points.at(i).y - trajectory_plan.trajectory_points.at(i-1).y; - downtracks.at(i) = sqrt(dx*dx + dy*dy); + downtracks.push_back(sqrt(dx*dx + dy*dy)); } + RCLCPP_ERROR_STREAM(nh_->get_logger(),"Sub yield 4 and size: " << downtracks.size()); + return downtracks; } @@ -707,7 +762,7 @@ namespace yield_plugin double YieldPlugin::polynomial_calc_d(std::vector coeff, double x) const { double result = 0; - for (size_t i = 0; i < coeff.size()-1; i++) + for (size_t i = 0; i < coeff.size()-1; i++) { double value = (int)(coeff.size() - 1 - i) * coeff.at(i) * pow(x, (int)(coeff.size() - 2 - i)); result = result + value; @@ -722,7 +777,7 @@ namespace yield_plugin { double dx = trajectory_points.at(i + 1).x - trajectory_points.at(i).x; double dy = trajectory_points.at(i + 1).y - trajectory_points.at(i).y; - double d = sqrt(dx*dx + dy*dy); + double d = sqrt(dx*dx + dy*dy); double t = (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() - rclcpp::Time(trajectory_points.at(i).target_time).seconds()); double v = d/t; if(v > max_speed) @@ -744,28 +799,28 @@ namespace yield_plugin if (llts.empty()) { RCLCPP_WARN_STREAM(nh_->get_logger(),"Trajectory point: x= " << original_tp.trajectory_points.at(i).x << "y="<< original_tp.trajectory_points.at(i).y); - + throw std::invalid_argument("Trajectory Point is not on a valid lanelet."); } auto digital_min_gap = llts[0].regulatoryElementsAs(); //Returns a list of these elements) - if (!digital_min_gap.empty()) + if (!digital_min_gap.empty()) { double digital_gap = digital_min_gap[0]->getMinimumGap(); // Provided gap is in meters - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Digital Gap found with value: " << digital_gap); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"Digital Gap found with value: " << digital_gap); desired_gap = std::max(desired_gap, digital_gap); } } return desired_gap; } - void YieldPlugin::set_georeference_string(const std::string& georeference) + void YieldPlugin::set_georeference_string(const std::string& georeference) { map_projector_ = std::make_shared(georeference.c_str()); // Build projector from proj string } void YieldPlugin::set_external_objects(const std::vector& object_list) { - external_objects_ = object_list; + external_objects_ = object_list; } } // namespace yield_plugin From 441c97598028cbb734790cfa2de3f9ee2cf0c850 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Thu, 30 Nov 2023 15:12:47 -0500 Subject: [PATCH 03/18] increase timeouts --- .../src/inlanecruising_plugin.cpp | 39 ++++++++++--------- .../config/plan_delegator_params.yaml | 4 +- 2 files changed, 22 insertions(+), 21 deletions(-) diff --git a/inlanecruising_plugin/src/inlanecruising_plugin.cpp b/inlanecruising_plugin/src/inlanecruising_plugin.cpp index 3b2c83e347..5db2c2636f 100644 --- a/inlanecruising_plugin/src/inlanecruising_plugin.cpp +++ b/inlanecruising_plugin/src/inlanecruising_plugin.cpp @@ -37,9 +37,9 @@ using oss = std::ostringstream; namespace inlanecruising_plugin { -InLaneCruisingPlugin::InLaneCruisingPlugin(std::shared_ptr nh, - carma_wm::WorldModelConstPtr wm, - const InLaneCruisingPluginConfig& config, +InLaneCruisingPlugin::InLaneCruisingPlugin(std::shared_ptr nh, + carma_wm::WorldModelConstPtr wm, + const InLaneCruisingPluginConfig& config, const DebugPublisher& debug_publisher, const std::string& plugin_name, const std::string& version_id) @@ -47,7 +47,7 @@ InLaneCruisingPlugin::InLaneCruisingPlugin(std::shared_ptrvehicle_state, wpg_general_config, wpg_detail_config); @@ -97,8 +97,8 @@ void InLaneCruisingPlugin::plan_trajectory_callback( original_trajectory.header.stamp = nh_->now(); original_trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()()); - original_trajectory.trajectory_points = basic_autonomy:: waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds, - req->vehicle_state, req->header.stamp, wm_, ending_state_before_buffer_, debug_msg_, + original_trajectory.trajectory_points = basic_autonomy:: waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds, + req->vehicle_state, req->header.stamp, wm_, ending_state_before_buffer_, debug_msg_, wpg_detail_config); // Compute the trajectory original_trajectory.initial_longitudinal_velocity = std::max(req->vehicle_state.longitudinal_vel, config_.minimum_speed); @@ -106,7 +106,7 @@ void InLaneCruisingPlugin::plan_trajectory_callback( for (auto& p : original_trajectory.trajectory_points) { p.planner_plugin_name = plugin_name_; } - + // Aside from the flag, ILC should not call yield_plugin on invalid trajectories if (config_.enable_object_avoidance && original_trajectory.trajectory_points.size() >= 2) { @@ -115,14 +115,14 @@ void InLaneCruisingPlugin::plan_trajectory_callback( if (yield_client_ && yield_client_->service_is_ready()) { RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Yield Client is valid"); - + auto yield_srv = std::make_shared(); yield_srv->initial_trajectory_plan = original_trajectory; yield_srv->vehicle_state = req->vehicle_state; auto yield_resp = yield_client_->async_send_request(yield_srv); - auto future_status = yield_resp.wait_for(std::chrono::milliseconds(100)); + auto future_status = yield_resp.wait_for(std::chrono::milliseconds(300)); if (future_status == std::future_status::ready) { @@ -140,14 +140,15 @@ void InLaneCruisingPlugin::plan_trajectory_callback( } else { - throw std::invalid_argument("Unable to Call Yield Plugin"); + RCLCPP_WARN_STREAM(nh_->get_logger(), "UNABLE TO CALL YIELD PLUGIN!!!!"); + resp->trajectory_plan = original_trajectory; } } else { throw std::invalid_argument("Yield Client is unavailable"); } - + } else { @@ -157,9 +158,9 @@ void InLaneCruisingPlugin::plan_trajectory_callback( if (config_.publish_debug) { // Publish the debug message if in debug logging mode debug_msg_.trajectory_plan = resp->trajectory_plan; - debug_publisher_(debug_msg_); + debug_publisher_(debug_msg_); } - + resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); // Planning complete @@ -190,7 +191,7 @@ bool InLaneCruisingPlugin::validate_yield_plan(const carma_planning_msgs::msg::T } else { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Invalid Yield Trajectory"); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Invalid Yield Trajectory"); } return false; } diff --git a/plan_delegator/config/plan_delegator_params.yaml b/plan_delegator/config/plan_delegator_params.yaml index 4c54e3cd7f..104553ebb2 100644 --- a/plan_delegator/config/plan_delegator_params.yaml +++ b/plan_delegator/config/plan_delegator_params.yaml @@ -26,9 +26,9 @@ min_speed: 2.2352 # Units: Seconds duration_to_signal_before_lane_change: 2.5 -# Int: The maximum duration that Plan Delegator will wait after calling a tactical plugin's trajectory planning service; if trajectory +# Int: The maximum duration that Plan Delegator will wait after calling a tactical plugin's trajectory planning service; if trajectory # generation takes longer than this, then trajectory planning will immediately end for the current trajectory planning iteration. # NOTE: It is highly desirable to maintain a timeout of 100 ms or less, but trajectory generation success cannot be guaranteed with this duration # for tactical plugins (primarily cooperative_lanechange) in all test scenarios at this time. # Units: Milliseconds -tactical_plugin_service_call_timeout: 100 \ No newline at end of file +tactical_plugin_service_call_timeout: 350 \ No newline at end of file From 2b9b6992637f6d5729d78848d80461aa4b87b5a1 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Thu, 30 Nov 2023 15:13:05 -0500 Subject: [PATCH 04/18] enable mc predictions --- motion_computation/config/parameters.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/motion_computation/config/parameters.yaml b/motion_computation/config/parameters.yaml index 6ac8cadac4..d3a2fe079d 100644 --- a/motion_computation/config/parameters.yaml +++ b/motion_computation/config/parameters.yaml @@ -2,7 +2,7 @@ prediction_time_step: 0.1 # Period of prediction in seconds -prediction_period: 2.0 +prediction_period: 3.0 # CV Model X-Axis Acceleration Noise cv_x_accel_noise: 9.0 @@ -16,18 +16,18 @@ prediction_process_noise_max: 1000.0 # Percentage of initial confidence to propagate to next time step prediction_confidence_drop_rate: 0.95 -# Boolean: If true then BSM messages will be converted to ExternalObjects. +# Boolean: If true then BSM messages will be converted to ExternalObjects. # If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) enable_bsm_processing: false -# Boolean: If true then PSM messages will be converted to ExternalObjects. +# Boolean: If true then PSM messages will be converted to ExternalObjects. # If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) -enable_psm_processing: true +enable_psm_processing: false -# Boolean: If true then MobilityPath messages will be converted to ExternalObjects. +# Boolean: If true then MobilityPath messages will be converted to ExternalObjects. # If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) -enable_mobility_path_processing: true +enable_mobility_path_processing: false -# Boolean: If true then ExternalObjects generated from sensor data will be processed. +# Boolean: If true then ExternalObjects generated from sensor data will be processed. # If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) -enable_sensor_processing: false \ No newline at end of file +enable_sensor_processing: true \ No newline at end of file From 883228456837aebc69295d84bbe744ea0daa5004 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Thu, 30 Nov 2023 15:14:55 -0500 Subject: [PATCH 05/18] increase timeouts in subsystem --- .../config/drivers_controller_config.yaml | 32 ++++-------- ...ironment_perception_controller_config.yaml | 20 ++----- .../config/guidance_controller_config.yaml | 52 +++++-------------- .../localization_controller_config.yaml | 49 ++++------------- .../config/v2x_controller_config.yaml | 19 ++----- 5 files changed, 43 insertions(+), 129 deletions(-) diff --git a/subsystem_controllers/config/drivers_controller_config.yaml b/subsystem_controllers/config/drivers_controller_config.yaml index 185d29c9c0..f5ffbb5f82 100644 --- a/subsystem_controllers/config/drivers_controller_config.yaml +++ b/subsystem_controllers/config/drivers_controller_config.yaml @@ -2,37 +2,27 @@ ros__parameters: # Long: Timeout for each service to be detected as available in milliseconds # Units: milliseconds - service_timeout_ms : 200 + service_timeout_ms : 2000 # Long: Timeout for each service call in milliseconds # Units: milliseconds - call_timeout_ms : 1000 + call_timeout_ms : 10000 - # String: The namespace for nodes in this subsystem. All nodes under this namespace will have their lifecycle's managed by this controller subsystem_namespace: /hardware_interface - - # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed required_subsystem_nodes: [''] - - # List of nodes which will not be directly managed by this subsystem controller - # but which are required to be operational for the subsystem to function unmanaged_required_nodes: [''] - - # List of nodes in the namespace which will not be managed by this subsystem controller - # Specifically includes the lidar and gps nodes which are handled in other subsystem controllers - excluded_namespace_nodes : [''] - - # List of ros1 controller drivers (node name) to consider required and who's failure shall result in automation abort - ros1_required_drivers: [''] - - # List of ros1 camera drivers (node name) to consider required and who's failure shall result in automation abort. - ros1_camera_drivers: [''] - - # Boolean: If this flag is true then all nodes under subsystem_namespace are treated as required in addition to any nodes in required_subsystem_nodes + excluded_namespace_nodes: + - /hardware_interface/carla_lidar_driver + - /hardware_interface/carla_gnss_driver + ros1_required_drivers: + - /hardware_interface/carla_driver + ros1_camera_drivers: + - /hardware_interface/carla_camera_driver full_subsystem_required: false + # Int: The time allocated for system startup in seconds startup_duration: 30 # Double: The timeout threshold for essential drivers in ms - required_driver_timeout: 3000.0 \ No newline at end of file + required_driver_timeout: 30000.0 \ No newline at end of file diff --git a/subsystem_controllers/config/environment_perception_controller_config.yaml b/subsystem_controllers/config/environment_perception_controller_config.yaml index 2cbc6f877e..c5fdf5053e 100644 --- a/subsystem_controllers/config/environment_perception_controller_config.yaml +++ b/subsystem_controllers/config/environment_perception_controller_config.yaml @@ -2,24 +2,14 @@ ros__parameters: # Long: Timeout for each service to be detected as available in milliseconds # Units: milliseconds - service_timeout_ms : 200 + service_timeout_ms : 2000 # Long: Timeout for each service call in milliseconds # Units: milliseconds - call_timeout_ms : 1500 + call_timeout_ms : 15000 - # String: The namespace for nodes in this subsystem. All nodes under this namespace will have their lifecycle's managed by this controller subsystem_namespace: /environment - - # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed - # Since CARMA assumes connected vehicles in most cases the current minimum node set is from motion prediction up - required_subsystem_nodes: - - /environment/external_object - - # List of nodes which will not be directly managed by this subsystem controller - # but which are required to be operational for the subsystem to function - unmanaged_required_nodes: + required_subsystem_nodes: [''] + unmanaged_required_nodes: - /hardware_interface/velodyne_lidar_driver_wrapper_node - - # Boolean: If this flag is true then all nodes under subsystem_namespace are treated as required in addition to any nodes in required_subsystem_nodes - full_subsystem_required: true \ No newline at end of file + full_subsystem_required: true diff --git a/subsystem_controllers/config/guidance_controller_config.yaml b/subsystem_controllers/config/guidance_controller_config.yaml index b132b7b407..68cefc6a6d 100644 --- a/subsystem_controllers/config/guidance_controller_config.yaml +++ b/subsystem_controllers/config/guidance_controller_config.yaml @@ -2,47 +2,21 @@ ros__parameters: # Long: Timeout for each service to be detected as available in milliseconds # Units: milliseconds - service_timeout_ms : 200 + service_timeout_ms : 2000 # Long: Timeout for each service call in milliseconds # Units: milliseconds - call_timeout_ms : 1000 + call_timeout_ms : 10000 - # String: The namespace for nodes in this subsystem. All nodes under this namespace will have their lifecycle's managed by this controller subsystem_namespace: /guidance - - # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed - required_subsystem_nodes: - - /guidance/arbitrator - - /guidance/guidance_node - - /guidance/plan_delegator - - /guidance/trajectory_executor - - /guidance/twist_filter - - /guidance/twist_gate - - /guidance/plugins/route_following_plugin # The minimal set of guidance plugins for system operation are route_following/inlanecruising/pure_pursuit_wrapper - - /guidance/plugins/inlanecruising_plugin - - /guidance/pure_pursuit_wrapper - - /guidance/yield_plugin - - # List of nodes which will not be directly managed by this subsystem controller - # but which are required to be operational for the subsystem to function + required_subsystem_nodes: [''] unmanaged_required_nodes: [''] # TODO add the controller driver once it is integrated with ROS2 - - # Boolean: If this flag is true then all nodes under subsystem_namespace are treated as required in addition to any nodes in required_subsystem_nodes - full_subsystem_required: false - - # List of guidance plugins (node name) to consider required and who's failure shall result in automation abort. - # Required plugins will be automatically activated at startup - # Required plugins cannot be deactivated by the user + full_subsystem_required: true required_plugins: - /guidance/plugins/route_following_plugin - /guidance/plugins/pure_pursuit_wrapper - /guidance/plugins/inlanecruising_plugin - /guidance/plugins/cooperative_lanechange - - # List of guidance plugins which are not required but the user wishes to have automatically activated - # so that the user doesn't need to manually activate them via the UI on each launch (though they still can) - # this list should have zero intersection with the required_plugins auto_activated_plugins: - /guidance/plugins/lci_strategic_plugin - /guidance/plugins/intersection_transit_maneuvering @@ -51,18 +25,20 @@ - /guidance/plugins/sci_strategic_plugin - /guidance/plugins/stop_controlled_intersection_tactical_plugin - /guidance/plugins/platoon_control_ihp - - /guidance/plugins/platoon_strategic_ihp + - /guidance/plugins/platoon_strategic_ihp_node - /guidance/plugins/platooning_tactical_plugin_node - /guidance/plugins/yield_plugin - - # List of guidance plugins that are ported to ROS2. If not in this list, it is assumed to be ROS1, and not managed. ros2_initial_plugins: - /guidance/plugins/inlanecruising_plugin - - /guidance/plugins/yield_plugin - - /guidance/plugins/light_controlled_intersection_tactical_plugin + - /guidance/plugins/platoon_strategic_ihp_node + - /guidance/plugins/stop_and_wait_plugin + - /guidance/plugins/route_following_plugin - /guidance/plugins/platooning_tactical_plugin_node - - /guidance/plugins/sci_strategic_plugin - /guidance/plugins/cooperative_lanechange - - /guidance/plugins/platooning_tactical_plugin_node + - /guidance/plugins/light_controlled_intersection_tactical_plugin + - /guidance/plugins/sci_strategic_plugin + - /guidance/plugins/stop_controlled_intersection_tactical_plugin - /guidance/plugins/yield_plugin - - /guidance/plugins/pure_pursuit_wrapper \ No newline at end of file + - /guidance/plugins/pure_pursuit_wrapper + - /guidance/plugins/lci_strategic_plugin + - /guidance/plugins/intersection_transit_maneuvering diff --git a/subsystem_controllers/config/localization_controller_config.yaml b/subsystem_controllers/config/localization_controller_config.yaml index a95a313dfc..d4654737b2 100644 --- a/subsystem_controllers/config/localization_controller_config.yaml +++ b/subsystem_controllers/config/localization_controller_config.yaml @@ -2,56 +2,25 @@ ros__parameters: # Long: Timeout for each service to be detected as available in milliseconds # Units: milliseconds - service_timeout_ms : 200 + service_timeout_ms : 2000 # Long: Timeout for each service call in milliseconds # Units: milliseconds - call_timeout_ms : 1000 + call_timeout_ms : 10000 - # String: The namespace for nodes in this subsystem. All nodes under this namespace will have their lifecycle's managed by this controller subsystem_namespace: /localization - - # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed - # Once initialized the localization subsystem requires a GNSS, localization manage, and ekf_localizer in order to continue functioning - required_subsystem_nodes: - - /localization/ekf_localizer - - /localization/gnss_to_map_convertor - - /localization/localization_manager - - # List of nodes which will not be directly managed by this subsystem controller - # but which are required to be operational for the subsystem to function - unmanaged_required_nodes: [''] # TODO localization needs GPS and lidar but this will be handled with more complex logic then this list - - # Boolean: If this flag is true then all nodes under subsystem_namespace are treated as required in addition to any nodes in required_subsystem_nodes + required_subsystem_nodes: [''] + unmanaged_required_nodes: [''] full_subsystem_required: true - - # List of nodes which are sensors used by the localization system and have their fault behavior described by - # the sensor_fault_map parameter sensor_nodes: - /hardware_interface/velodyne_lidar_driver_wrapper_node - - /hardware_interface/temp_lidar_2 - - /hardware_interface/temp_gps - - # JSON String: Sensor Fault Map - # NOTE: It would have been preferable to use a 2d array here but ROS2 Foxy does not support this type of parameter - # The sensor_fault_map is described as a 2d array where the last element is the desired status alert type - # The set of values in front should have the same size as the sensor_nodes list and describe the status of those nodes in that order - # Input Enum Values: - # Operational: 1 - # Failed: 0 - # Output Enum Values: - # FATAL: 0 - # OPERATIONAL: 1 - # CAUTION: 2 - # WARNING: 3 + # TODO update the fault map once ros2 gps is integrated sensor_fault_map: ' { - "sensor_fault_map": + "sensor_fault_map": [ - [0,1,1,2], - [1,0,1,2], - [0,0,1,3], - [0,0,0,0] - ], + [0,0] + ] } ' + diff --git a/subsystem_controllers/config/v2x_controller_config.yaml b/subsystem_controllers/config/v2x_controller_config.yaml index adf433b081..b0341b6cc4 100644 --- a/subsystem_controllers/config/v2x_controller_config.yaml +++ b/subsystem_controllers/config/v2x_controller_config.yaml @@ -2,25 +2,14 @@ ros__parameters: # Long: Timeout for each service to be detected as available in milliseconds # Units: milliseconds - service_timeout_ms : 200 + service_timeout_ms : 2000 # Long: Timeout for each service call in milliseconds # Units: milliseconds - call_timeout_ms : 1000 + call_timeout_ms : 10000 - # String: The namespace for nodes in this subsystem. All nodes under this namespace will have their lifecycle's managed by this controller subsystem_namespace: /message - - # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed - # The V2X stack requires the message node and j2735_convertor node to be functional - required_subsystem_nodes: - - /message/cpp_message_node - - /message/j2735_convertor_node - - # List of nodes which will not be directly managed by this subsystem controller - # but which are required to be operational for the subsystem to function - unmanaged_required_nodes: + required_subsystem_nodes: [''] + unmanaged_required_nodes: - /hardware_interface/dsrc_driver_node - - # Boolean: If this flag is true then all nodes under subsystem_namespace are treated as required in addition to any nodes in required_subsystem_nodes full_subsystem_required: true \ No newline at end of file From a033b8a6f15a049527622f88c677115f7707053d Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Mon, 4 Dec 2023 20:00:20 +0000 Subject: [PATCH 06/18] clean --- carma_wm/src/WMListenerWorker.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/carma_wm/src/WMListenerWorker.cpp b/carma_wm/src/WMListenerWorker.cpp index ffd8135171..8fe335fe1b 100644 --- a/carma_wm/src/WMListenerWorker.cpp +++ b/carma_wm/src/WMListenerWorker.cpp @@ -121,25 +121,25 @@ void logSignalizedIntersectionManager(const carma_wm::SignalizedIntersectionMana { for (auto pair : sim.intersection_id_to_regem_id_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "inter id: " << (int)pair.first << ", regem id: " << pair.second); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "inter id: " << (int)pair.first << ", regem id: " << pair.second); } for (auto pair : sim.signal_group_to_entry_lanelet_ids_) { for (auto iter = pair.second.begin(); iter != pair.second.end(); iter++) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", entry llt id: " << *iter); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", entry llt id: " << *iter); } } for (auto pair : sim.signal_group_to_exit_lanelet_ids_) { for (auto iter = pair.second.begin(); iter != pair.second.end(); iter++) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", exit llt id: " << *iter); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", exit llt id: " << *iter); } } for (auto pair : sim.signal_group_to_traffic_light_id_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", regem id: " << pair.second); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", regem id: " << pair.second); } } From 0d47498c4d6fbb73f13744621715c3262851a3d1 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Mon, 4 Dec 2023 20:23:04 +0000 Subject: [PATCH 07/18] clean yield_plugin file --- yield_plugin/src/yield_plugin.cpp | 265 ++++++++++++------------------ 1 file changed, 109 insertions(+), 156 deletions(-) diff --git a/yield_plugin/src/yield_plugin.cpp b/yield_plugin/src/yield_plugin.cpp index 036fe97a53..f46708e6d8 100644 --- a/yield_plugin/src/yield_plugin.cpp +++ b/yield_plugin/src/yield_plugin.cpp @@ -60,7 +60,7 @@ namespace yield_plugin for (size_t i=0; iprojectECEF( { (double)ecef_point.ecef_x/100.0, (double)ecef_point.ecef_y/100.0, (double)ecef_point.ecef_z/100.0 } , 1); - + return lanelet::traits::to2D(map_point); - } - - + } + + carma_v2x_msgs::msg::MobilityResponse YieldPlugin::compose_mobility_response(const std::string& resp_recipient_id, const std::string& req_plan_id, bool response) const { @@ -123,7 +123,7 @@ namespace yield_plugin out_mobility_response.is_accepted = true; } else out_mobility_response.is_accepted = false; - + return out_mobility_response; } @@ -140,12 +140,12 @@ namespace yield_plugin } if (incoming_request.plan_type.type == carma_v2x_msgs::msg::PlanType::CHANGE_LANE_LEFT || incoming_request.plan_type.type == carma_v2x_msgs::msg::PlanType::CHANGE_LANE_RIGHT) { - RCLCPP_ERROR(nh_->get_logger(),"Cooperative Lane Change Request Received"); + RCLCPP_DEBUG(nh_->get_logger(),"Cooperative Lane Change Request Received"); lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_RECEIVED; lc_status_msg.description = "Received lane merge request"; if (incoming_request.m_header.recipient_id == config_.vehicle_id) { - RCLCPP_ERROR(nh_->get_logger(),"CLC Request correctly received"); + RCLCPP_DEBUG(nh_->get_logger(),"CLC Request correctly received"); } // extract mobility header std::string req_sender_id = incoming_request.m_header.sender_id; @@ -155,7 +155,7 @@ namespace yield_plugin carma_v2x_msgs::msg::Trajectory incoming_trajectory = incoming_request.trajectory; std::string req_strategy_params = incoming_request.strategy_params; clc_urgency_ = incoming_request.urgency; - RCLCPP_ERROR_STREAM(nh_->get_logger(),"received urgency: " << clc_urgency_); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"received urgency: " << clc_urgency_); // Parse strategy parameters using boost::property_tree::ptree; @@ -167,9 +167,9 @@ namespace yield_plugin int start_lanelet_id = pt.get("sl"); int end_lanelet_id = pt.get("el"); double req_traj_speed = (double)req_traj_speed_full + (double)(req_traj_fractional)/10.0; - RCLCPP_ERROR_STREAM(nh_->get_logger(),"req_traj_speed" << req_traj_speed); - RCLCPP_ERROR_STREAM(nh_->get_logger(),"start_lanelet_id" << start_lanelet_id); - RCLCPP_ERROR_STREAM(nh_->get_logger(),"end_lanelet_id" << end_lanelet_id); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_traj_speed" << req_traj_speed); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"start_lanelet_id" << start_lanelet_id); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"end_lanelet_id" << end_lanelet_id); std::vector req_traj_plan = {}; @@ -184,30 +184,30 @@ namespace yield_plugin double req_timestamp = (double)incoming_request.m_header.timestamp / 1000.0 - current_time_sec; set_incoming_request_info(req_traj_plan, req_traj_speed, req_plan_time, req_timestamp); - + if (req_expiration_sec - current_time_sec >= config_.tpmin && cooperative_request_acceptable_) { timesteps_since_last_req_ = 0; lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_ACCEPTED; lc_status_msg.description = "Accepted lane merge request"; - response_to_clc_req = true; - RCLCPP_ERROR(nh_->get_logger(),"CLC accepted"); + response_to_clc_req = true; + RCLCPP_DEBUG(nh_->get_logger(),"CLC accepted"); } else { lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_REJECTED; lc_status_msg.description = "Rejected lane merge request"; response_to_clc_req = false; - RCLCPP_ERROR(nh_->get_logger(),"CLC rejected"); + RCLCPP_DEBUG(nh_->get_logger(),"CLC rejected"); } carma_v2x_msgs::msg::MobilityResponse outgoing_response = compose_mobility_response(req_sender_id, req_plan_id, response_to_clc_req); mobility_response_publisher_(outgoing_response); lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::RESPONSE_SENT; - RCLCPP_ERROR(nh_->get_logger(),"response sent"); + RCLCPP_DEBUG(nh_->get_logger(),"response sent"); } } lc_status_publisher_(lc_status_msg); - + } void YieldPlugin::set_incoming_request_info(std::vector req_trajectory, double req_speed, double req_planning_time, double req_timestamp) @@ -215,7 +215,7 @@ namespace yield_plugin req_trajectory_points_ = req_trajectory; req_target_speed_ = req_speed; req_target_plan_time_ = req_planning_time; - RCLCPP_ERROR_STREAM(nh_->get_logger(),"req_target_plan_time_" << req_target_plan_time_); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_target_plan_time_" << req_target_plan_time_); req_timestamp_ = req_timestamp; } @@ -226,11 +226,10 @@ namespace yield_plugin } void YieldPlugin::plan_trajectory_callback( - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { - RCLCPP_ERROR(nh_->get_logger(),"yield_plugin was called!"); - + RCLCPP_DEBUG(nh_->get_logger(),"Yield_plugin was called!"); if (req->initial_trajectory_plan.trajectory_points.size() < 2){ throw std::invalid_argument("Empty Trajectory received by Yield"); } @@ -243,36 +242,35 @@ namespace yield_plugin // seperating cooperative yield with regular object detection for better performance. if (config_.enable_cooperative_behavior && clc_urgency_ > config_.acceptable_urgency) { - RCLCPP_ERROR(nh_->get_logger(),"Only consider high urgency clc"); + RCLCPP_DEBUG(nh_->get_logger(),"Only consider high urgency clc"); if (timesteps_since_last_req_ < config_.acceptable_passed_timesteps) { - RCLCPP_ERROR(nh_->get_logger(),"Yield for CLC. We haven't received an updated negotiation this timestep"); + RCLCPP_DEBUG(nh_->get_logger(),"Yield for CLC. We haven't received an updated negotiation this timestep"); yield_trajectory = update_traj_for_cooperative_behavior(original_trajectory, req->vehicle_state.longitudinal_vel); timesteps_since_last_req_++; } else { - RCLCPP_ERROR(nh_->get_logger(),"unreliable CLC communication, switching to object avoidance"); + RCLCPP_DEBUG(nh_->get_logger(),"unreliable CLC communication, switching to object avoidance"); yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, req->vehicle_state.longitudinal_vel); // Compute the trajectory - } + } } else { - RCLCPP_ERROR(nh_->get_logger(),"Yield for object avoidance"); + RCLCPP_DEBUG(nh_->get_logger(),"Yield for object avoidance"); yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, req->vehicle_state.longitudinal_vel); // Compute the trajectory } yield_trajectory.header.frame_id = "map"; yield_trajectory.header.stamp = nh_->now(); yield_trajectory.trajectory_id = original_trajectory.trajectory_id; - yield_trajectory.initial_longitudinal_velocity = original_trajectory.initial_longitudinal_velocity;//copy the original trajectory's desired speed for now. + yield_trajectory.initial_longitudinal_velocity = original_trajectory.initial_longitudinal_velocity;//copy the original trajectory's desired speed for now. resp->trajectory_plan = yield_trajectory; rclcpp::Time end_time = system_clock.now(); // Planning complete auto duration = end_time - start_time; - RCLCPP_ERROR_STREAM(nh_->get_logger(), "ExecutionTime: " << std::to_string(duration.seconds())); - RCLCPP_ERROR(nh_->get_logger(),"yield_plugin was ended!"); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ExecutionTime: " << std::to_string(duration.seconds())); } @@ -303,14 +301,14 @@ namespace yield_plugin double dy = original_tp.trajectory_points[0].y - intersection_point.y(); // check if a digital_gap is available double digital_gap = check_traj_for_digital_min_gap(original_tp); - RCLCPP_ERROR_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap); goal_pos = sqrt(dx*dx + dy*dy) - config_.x_gap; - RCLCPP_ERROR_STREAM(nh_->get_logger(),"Goal position (goal_pos): " << goal_pos); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Goal position (goal_pos): " << goal_pos); double collision_time = req_timestamp_ + (intersection_points[0].first * ecef_traj_timestep_) - config_.safety_collision_time_gap; - RCLCPP_ERROR_STREAM(nh_->get_logger(),"req time stamp: " << req_timestamp_); - RCLCPP_ERROR_STREAM(nh_->get_logger(),"Collision time: " << collision_time); - RCLCPP_ERROR_STREAM(nh_->get_logger(),"intersection num: " << intersection_points[0].first); - RCLCPP_ERROR_STREAM(nh_->get_logger(),"Planning time: " << planning_time); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req time stamp: " << req_timestamp_); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Collision time: " << collision_time); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"intersection num: " << intersection_points[0].first); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Planning time: " << planning_time); // calculate distance traveled from beginning of trajectory to collision point double dx2 = intersection_point.x() - req_trajectory_points_[0].x(); double dy2 = intersection_point.y() - req_trajectory_points_[0].y(); @@ -320,33 +318,33 @@ namespace yield_plugin goal_velocity = std::min(goal_velocity, incoming_trajectory_speed); double min_time = (initial_velocity - goal_velocity)/config_.yield_max_deceleration; - RCLCPP_ERROR_STREAM(nh_->get_logger(),"goal_velocity: " << goal_velocity); - RCLCPP_ERROR_STREAM(nh_->get_logger(),"incoming_trajectory_speed: " << incoming_trajectory_speed); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"goal_velocity: " << goal_velocity); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"incoming_trajectory_speed: " << incoming_trajectory_speed); if (planning_time > min_time) { cooperative_request_acceptable_ = true; - cooperative_trajectory = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time); + cooperative_trajectory = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time); } else { cooperative_request_acceptable_ = false; - RCLCPP_ERROR(nh_->get_logger(),"The incoming requested trajectory is rejected, due to insufficient gap"); + RCLCPP_DEBUG(nh_->get_logger(),"The incoming requested trajectory is rejected, due to insufficient gap"); cooperative_trajectory = original_tp; } - + } else { cooperative_request_acceptable_ = true; - RCLCPP_ERROR(nh_->get_logger(),"The incoming requested trajectory does not overlap with host vehicle's trajectory"); + RCLCPP_DEBUG(nh_->get_logger(),"The incoming requested trajectory does not overlap with host vehicle's trajectory"); cooperative_trajectory = original_tp; } return cooperative_trajectory; } - carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::generate_JMT_trajectory(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time) + carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::generate_JMT_trajectory(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time) { carma_planning_msgs::msg::TrajectoryPlan jmt_trajectory; std::vector jmt_trajectory_points; @@ -357,23 +355,19 @@ namespace yield_plugin double goal_accel = 0.0; double original_max_speed = max_trajectory_speed(original_tp.trajectory_points); - RCLCPP_ERROR_STREAM(nh_->get_logger(),"original_max_speed" << original_max_speed); - std::vector values = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos, + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"original_max_speed" << original_max_speed); + std::vector values = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos, goal_pos, - initial_velocity, - goal_velocity, - initial_accel, - goal_accel, - initial_time, + initial_velocity, + goal_velocity, + initial_accel, + goal_accel, + initial_time, planning_time); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 1 and size for original_tp.trajectory_points: " << original_tp.trajectory_points.size()); - + std::vector original_traj_downtracks = get_relative_downtracks(original_tp); std::vector calculated_speeds = {}; calculated_speeds.push_back(initial_velocity); - - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 2 and size: " << original_tp.trajectory_points.size()); - for(size_t i = 1; i < original_tp.trajectory_points.size(); i++ ) { double traj_target_time = i * planning_time / original_tp.trajectory_points.size(); @@ -386,86 +380,65 @@ namespace yield_plugin } calculated_speeds.push_back(dv); } - - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 3"); - // moving average filter std::vector filtered_speeds = basic_autonomy::smoothing::moving_average_filter(calculated_speeds, config_.speed_moving_average_window_size); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 4 and filtered_speeds size: " << filtered_speeds.size()); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 4 and original_tp.trajectory_points.size(): " << original_tp.trajectory_points.size()); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 4 and original_traj_downtracks: " << original_traj_downtracks.size()); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 4 and original_traj_downtracks: " << original_traj_downtracks.size()); - - - double prev_speed = filtered_speeds[0]; - for(size_t i = 1; i < original_tp.trajectory_points.size(); i++ ) { carma_planning_msgs::msg::TrajectoryPlanPoint jmt_tpp; - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 4.a and index: " << i); - double current_speed = filtered_speeds.at(i); double traj_target_time = i * planning_time / original_tp.trajectory_points.size(); if (current_speed >= config_.max_stop_speed) { - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 4.b and jmt_trajectory_points.size(): " << jmt_trajectory_points.size()); - double dt = (2 * original_traj_downtracks.at(i)) / (current_speed + prev_speed); jmt_tpp = original_tp.trajectory_points.at(i); jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration(dt*1e9); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "non-empty x: " << jmt_tpp.x << ", y:" << jmt_tpp.y << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "non-empty x: " << jmt_tpp.x << ", y:" << jmt_tpp.y << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())); jmt_trajectory_points.push_back(jmt_tpp); } else { - RCLCPP_ERROR(nh_->get_logger(),"target speed is zero"); + RCLCPP_DEBUG(nh_->get_logger(),"Target speed is zero"); // if speed is zero, the vehicle will stay in previous location. jmt_tpp = jmt_trajectory_points.back(); - // MISH: Potential bug where if purepursuit accidentally picks a point that has previous time, then it may speed up. target_time is assumed to increase + // MISH: Potential bug where if purepursuit accidentally picks a point that has previous time, then it may speed up. target_time is assumed to increase jmt_tpp.target_time = rclcpp::Time(std::max((rclcpp::Time(jmt_trajectory_points[0].target_time) + rclcpp::Duration(traj_target_time*1e9)).seconds(), rclcpp::Time(jmt_trajectory_points.back().target_time).seconds()) * 1e9); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "empty x: " << jmt_tpp.x << ", y:" << jmt_tpp.y << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "empty x: " << jmt_tpp.x << ", y:" << jmt_tpp.y << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())); jmt_trajectory_points.push_back(jmt_tpp); } prev_speed = current_speed; } - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Yield 5 and jmt_trajectory_points size: " << jmt_trajectory_points.size()); - - + jmt_trajectory.header = original_tp.header; jmt_trajectory.trajectory_id = original_tp.trajectory_id; jmt_trajectory.trajectory_points = jmt_trajectory_points; return jmt_trajectory; } - - std::optional YieldPlugin::detect_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory1, const std::vector& trajectory2, double collision_radius) + + std::optional YieldPlugin::detect_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory1, const std::vector& trajectory2, double collision_radius) { // Iterate through each pair of consecutive points in the trajectories - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Starting a new collision detection, trajectory size: " << trajectory1.trajectory_points.size() << ". prediction size: " << trajectory2.size()); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Starting a new collision detection, trajectory size: " << trajectory1.trajectory_points.size() << ". prediction size: " << trajectory2.size()); // Iterate through the object to check if it's on the route bool on_route = false; int on_route_idx = 0; - for (const auto& route_id : route_llt_ids_) - { - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Route llts: " << route_id); - } - for (size_t j = 0; j < trajectory2.size(); j+=4) + for (size_t j = 0; j < trajectory2.size(); j+=4) // Checking every 4th point to save computation time { lanelet::BasicPoint2d curr_point; curr_point.x() = trajectory2.at(j).predicted_position.position.x; curr_point.y() = trajectory2.at(j).predicted_position.position.y; - auto corresponding_lanelets = wm_->getLaneletsFromPoint(curr_point, 10); // get 4 lanelets + auto corresponding_lanelets = wm_->getLaneletsFromPoint(curr_point, 8); // some intersection can have 8 overlapping lanelets for (const auto& llt: corresponding_lanelets) { - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Checking llt: " << llt.id()); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Checking llt: " << llt.id()); if (route_llt_ids_.find(llt.id()) != route_llt_ids_.end()) { @@ -480,16 +453,16 @@ namespace yield_plugin if (!on_route) { - RCLCPP_ERROR(nh_->get_logger(), "Predicted states are not on the route! ignoring"); + RCLCPP_DEBUG(nh_->get_logger(), "Predicted states are not on the route! ignoring"); return std::nullopt; } - + double smallest_dist = std::numeric_limits::max(); - for (size_t i = 0; i < trajectory1.trajectory_points.size() - 1; ++i) + for (size_t i = 0; i < trajectory1.trajectory_points.size() - 1; ++i) { auto p1a = trajectory1.trajectory_points.at(i); auto p1b = trajectory1.trajectory_points.at(i + 1); - for (size_t j = on_route_idx; j < trajectory2.size() - 1; ++j) + for (size_t j = on_route_idx; j < trajectory2.size() - 1; ++j) { auto p2a = trajectory2.at(j); auto p2b = trajectory2.at(j + 1); @@ -498,13 +471,13 @@ namespace yield_plugin double p2a_t = rclcpp::Time(p2a.header.stamp).seconds(); double p2b_t = rclcpp::Time(p2b.header.stamp).seconds(); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "p1a.target_time: " << std::to_string(p1a_t) << ", p1b.target_time: " << std::to_string(p1b_t)); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "p2a.target_time: " << std::to_string(p2a_t) << ", p2b.target_time: " << std::to_string(p2b_t)); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "p1a.x: " << p1a.x << ", p1a.y: " << p1a.y); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "p1b.x: " << p1b.x << ", p1b.y: " << p1b.y); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1a.target_time: " << std::to_string(p1a_t) << ", p1b.target_time: " << std::to_string(p1b_t)); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2a.target_time: " << std::to_string(p2a_t) << ", p2b.target_time: " << std::to_string(p2b_t)); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1a.x: " << p1a.x << ", p1a.y: " << p1a.y); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1b.x: " << p1b.x << ", p1b.y: " << p1b.y); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "p2a.x: " << p2a.predicted_position.position.x << ", p2a.y: " << p2a.predicted_position.position.y); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "p2b.x: " << p2b.predicted_position.position.x << ", p2b.y: " << p2b.predicted_position.position.y); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2a.x: " << p2a.predicted_position.position.x << ", p2a.y: " << p2a.predicted_position.position.y); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2b.x: " << p2b.predicted_position.position.x << ", p2b.y: " << p2b.predicted_position.position.y); // Linearly interpolate positions at a common timestamp for both trajectories double dt = (p2a_t - p1a_t) / (p1b_t - p1a_t); @@ -516,15 +489,15 @@ namespace yield_plugin // Calculate the distance between the two interpolated points double distance = std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)); smallest_dist = std::min(distance, smallest_dist); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Smallest_dist: " << smallest_dist << ", distance: " << distance << ", dt: " << dt << ", x1: " << x1 << ", y1: " <get_logger(), "Smallest_dist: " << smallest_dist << ", distance: " << distance << ", dt: " << dt << ", x1: " << x1 << ", y1: " < config_.collision_check_radius) { - RCLCPP_ERROR(nh_->get_logger(), "Too far away" ); + RCLCPP_DEBUG(nh_->get_logger(), "Too far away" ); return std::nullopt; } - if (distance > collision_radius) + if (distance > collision_radius) { continue; } @@ -544,7 +517,7 @@ namespace yield_plugin { RCLCPP_WARN_STREAM(nh_->get_logger(), "Collision detected at timestamp " << std::to_string(p2a_t) << ", x: " << x1 << ", y: " << y1 << ", within distance: " << distance); return rclcpp::Time(p2a.header.stamp); - } + } } } @@ -552,11 +525,11 @@ namespace yield_plugin return std::nullopt; } - carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector& external_objects, double initial_velocity) - { + carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector& external_objects, double initial_velocity) + { if (original_tp.trajectory_points.size() < 2) { - RCLCPP_ERROR(nh_->get_logger(), "Yield plugin received less than 2 points in update_traj_for_object, returning unchanged..."); + RCLCPP_WARN(nh_->get_logger(), "Yield plugin received less than 2 points in update_traj_for_object, returning unchanged..."); return original_tp; } else if (original_tp.trajectory_points.empty()) @@ -574,7 +547,7 @@ namespace yield_plugin std::set checked_external_object_ids; std::vector new_list; - RCLCPP_ERROR_STREAM(nh_->get_logger(), "ExternalObjects size: " << external_objects.size()); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ExternalObjects size: " << external_objects.size()); if (wm_->getRoute() == nullptr) { @@ -590,23 +563,11 @@ namespace yield_plugin double earliest_collision_obj_time = std::numeric_limits::max(); - RCLCPP_ERROR_STREAM(nh_->get_logger(),"External Object List (external_objects) size: " << external_objects.size()); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"External Object List (external_objects) size: " << external_objects.size()); for (const auto& curr_obstacle : external_objects) { - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Object's back time: " << std::to_string(rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds()) << ", plan_start_time: " << std::to_string(plan_start_time.seconds())); - if (curr_obstacle.object_type != 4) //TODO - { - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Skipping not pedestrian"); - continue; - } - - if (curr_obstacle.velocity.twist.linear.x < 1.0) //TODO - { - RCLCPP_ERROR_STREAM(nh_->get_logger(), "pedestrian but not moving!"); - continue; - } - RCLCPP_ERROR_STREAM(nh_->get_logger(), "===================== Checking Collision! ===================="); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Object's back time: " << std::to_string(rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds()) << ", plan_start_time: " << std::to_string(plan_start_time.seconds())); // do not process outdated objects if (rclcpp::Time(curr_obstacle.predictions.back().header.stamp) > plan_start_time) @@ -621,7 +582,7 @@ namespace yield_plugin new_list.insert(new_list.end(), curr_obstacle.predictions.cbegin(), curr_obstacle.predictions.cend()); auto collision_time = detect_collision_time(original_tp, new_list, config_.intervehicle_collision_distance); - + if (collision_time != std::nullopt) { checked_external_object_ids.insert(curr_obstacle.id); @@ -638,25 +599,25 @@ namespace yield_plugin lanelet::BasicPoint2d point(original_tp.trajectory_points[0].x,original_tp.trajectory_points[0].y); double vehicle_downtrack = wm_->routeTrackPos(point).downtrack; + + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"vehicle_downtrack: " << vehicle_downtrack); - RCLCPP_ERROR_STREAM(nh_->get_logger(),"vehicle_downtrack: " << vehicle_downtrack); - - // Issue (https://github.com/usdot-fhwa-stol/carma-platform/issues/2155): If the yield_plugin can detect if the roadway object is moving along the route, - // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon. + // Issue (https://github.com/usdot-fhwa-stol/carma-platform/issues/2155): If the yield_plugin can detect if the roadway object is moving along the route, + // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon. if(earliest_collision_obj.has_value()) { - RCLCPP_ERROR_STREAM(nh_->get_logger(),"Collision Detected!"); + RCLCPP_WARN_STREAM(nh_->get_logger(),"Collision Detected!"); lanelet::BasicPoint2d point_o(earliest_collision_obj.value().pose.pose.position.x, earliest_collision_obj.value().pose.pose.position.y); double object_downtrack = wm_->routeTrackPos(point_o).downtrack; - RCLCPP_ERROR_STREAM(nh_->get_logger(),"object_downtrack: " << object_downtrack); - + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack: " << object_downtrack); + double dist_to_object = object_downtrack - vehicle_downtrack; - RCLCPP_ERROR_STREAM(nh_->get_logger(),"dist_to_object: " << dist_to_object); - - RCLCPP_ERROR_STREAM(nh_->get_logger(),"object speed: " << earliest_collision_obj.value().velocity.twist.linear.x); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"dist_to_object: " << dist_to_object); + + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object speed: " << earliest_collision_obj.value().velocity.twist.linear.x); // Distance from the original trajectory point to the lead vehicle/object double dist_x = earliest_collision_obj.value().pose.pose.position.x - original_tp.trajectory_points[0].x; @@ -673,15 +634,15 @@ namespace yield_plugin { // check if a digital_gap is available double digital_gap = check_traj_for_digital_min_gap(original_tp); - RCLCPP_ERROR_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap); - // if a digital gap is available, it is replaced as safety gap + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap); + // if a digital gap is available, it is replaced as safety gap safety_gap = std::max(safety_gap, digital_gap); } // safety gap is implemented - double goal_pos = x_lead - safety_gap; + double goal_pos = x_lead - safety_gap; if (goal_velocity <= config_.min_obstacle_speed){ - RCLCPP_ERROR(nh_->get_logger(),"The obstacle is not moving, goal velocity is set to 0"); + RCLCPP_WARN(nh_->get_logger(),"The obstacle is not moving, goal velocity is set to 0"); goal_velocity = 0.0; } @@ -712,15 +673,15 @@ namespace yield_plugin { tp = t_ref; } - - RCLCPP_ERROR_STREAM(nh_->get_logger(),"Object avoidance planning time: " << tp); + + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Object avoidance planning time: " << tp); update_tpp_vector = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, tp); return update_tpp_vector; } - - RCLCPP_ERROR(nh_->get_logger(),"No collision detection, so trajectory not modified."); + + RCLCPP_DEBUG(nh_->get_logger(),"No collision detection, so trajectory not modified."); return original_tp; } @@ -728,23 +689,15 @@ namespace yield_plugin std::vector YieldPlugin::get_relative_downtracks(const carma_planning_msgs::msg::TrajectoryPlan& trajectory_plan) const { std::vector downtracks; - RCLCPP_ERROR(nh_->get_logger(),"Sub yield 1"); - downtracks.reserve(trajectory_plan.trajectory_points.size()); - RCLCPP_ERROR(nh_->get_logger(),"Sub yield 2"); - // relative downtrack distance of the fist Point is 0.0 downtracks.push_back(0.0); - RCLCPP_ERROR(nh_->get_logger(),"Sub yield 2a"); for (size_t i=1; i < trajectory_plan.trajectory_points.size(); i++){ - RCLCPP_ERROR_STREAM(nh_->get_logger(),"Sub yield 3, id: " << (int)i); double dx = trajectory_plan.trajectory_points.at(i).x - trajectory_plan.trajectory_points.at(i-1).x; double dy = trajectory_plan.trajectory_points.at(i).y - trajectory_plan.trajectory_points.at(i-1).y; downtracks.push_back(sqrt(dx*dx + dy*dy)); } - RCLCPP_ERROR_STREAM(nh_->get_logger(),"Sub yield 4 and size: " << downtracks.size()); - return downtracks; } @@ -762,7 +715,7 @@ namespace yield_plugin double YieldPlugin::polynomial_calc_d(std::vector coeff, double x) const { double result = 0; - for (size_t i = 0; i < coeff.size()-1; i++) + for (size_t i = 0; i < coeff.size()-1; i++) { double value = (int)(coeff.size() - 1 - i) * coeff.at(i) * pow(x, (int)(coeff.size() - 2 - i)); result = result + value; @@ -777,7 +730,7 @@ namespace yield_plugin { double dx = trajectory_points.at(i + 1).x - trajectory_points.at(i).x; double dy = trajectory_points.at(i + 1).y - trajectory_points.at(i).y; - double d = sqrt(dx*dx + dy*dy); + double d = sqrt(dx*dx + dy*dy); double t = (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() - rclcpp::Time(trajectory_points.at(i).target_time).seconds()); double v = d/t; if(v > max_speed) @@ -799,28 +752,28 @@ namespace yield_plugin if (llts.empty()) { RCLCPP_WARN_STREAM(nh_->get_logger(),"Trajectory point: x= " << original_tp.trajectory_points.at(i).x << "y="<< original_tp.trajectory_points.at(i).y); - + throw std::invalid_argument("Trajectory Point is not on a valid lanelet."); } auto digital_min_gap = llts[0].regulatoryElementsAs(); //Returns a list of these elements) - if (!digital_min_gap.empty()) + if (!digital_min_gap.empty()) { double digital_gap = digital_min_gap[0]->getMinimumGap(); // Provided gap is in meters - RCLCPP_ERROR_STREAM(nh_->get_logger(),"Digital Gap found with value: " << digital_gap); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Digital Gap found with value: " << digital_gap); desired_gap = std::max(desired_gap, digital_gap); } } return desired_gap; } - void YieldPlugin::set_georeference_string(const std::string& georeference) + void YieldPlugin::set_georeference_string(const std::string& georeference) { map_projector_ = std::make_shared(georeference.c_str()); // Build projector from proj string } void YieldPlugin::set_external_objects(const std::vector& object_list) { - external_objects_ = object_list; + external_objects_ = object_list; } } // namespace yield_plugin From 591a6f9056ed1f9f0008836fefd9410324967986 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Mon, 4 Dec 2023 20:23:51 +0000 Subject: [PATCH 08/18] clean --- carma_wm/src/WorldModelUtils.cpp | 94 ++++++++++++-------------------- 1 file changed, 35 insertions(+), 59 deletions(-) diff --git a/carma_wm/src/WorldModelUtils.cpp b/carma_wm/src/WorldModelUtils.cpp index c07ca60db3..998f5ba4d9 100644 --- a/carma_wm/src/WorldModelUtils.cpp +++ b/carma_wm/src/WorldModelUtils.cpp @@ -15,9 +15,6 @@ */ #include -#include -#include -#include namespace carma_wm { @@ -27,9 +24,6 @@ namespace query std::vector getLaneletsFromPoint(const lanelet::LaneletMapConstPtr& semantic_map, const lanelet::BasicPoint2d& point, const unsigned int n) { - using Point = boost::geometry::model::d2::point_xy; - using Polygon = boost::geometry::model::polygon; - // Check if the map is loaded yet if (!semantic_map || semantic_map->laneletLayer.size() == 0) { @@ -40,33 +34,15 @@ std::vector getLaneletsFromPoint(const lanelet::LaneletMa if (nearestLanelets.size() == 0) return {}; - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::query"), "check: x: " << point.x() << ", y: " << point.y()); - // closest ones are in the back + size_t id = 0; // closest ones are in the back // loop through until the point is no longer geometrically in the lanelet - for (size_t id = 0; id < nearestLanelets.size(); id++) - { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::query"), "nearest size: " << nearestLanelets.size() << ", checking: " << nearestLanelets[id].second.id()); - auto poly_linestrings = nearestLanelets[id].second.leftBound2d(); - auto poly_linestrings1 = nearestLanelets[id].second.rightBound2d(); - //RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::query"), "======start===="); - - - Polygon new_poly; - for (const auto& temp_point : poly_linestrings) { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::query"), "x: " << temp_point.x() << ", y: " << temp_point.y()); - boost::geometry::append(new_poly, Point(temp_point.x(), temp_point.y())); - } - for (const auto& temp_point : poly_linestrings1.invert()) { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::query"), "x: " << temp_point.x() << ", y: " << temp_point.y()); - boost::geometry::append(new_poly, Point(temp_point.x(), temp_point.y())); - } - boost::geometry::append(new_poly, Point(poly_linestrings.front().x(), poly_linestrings.front().y())); - //RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::query"), "x: " << poly_linestrings.front().x() << ", y: " << poly_linestrings.front().y()); - - //RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::query"), "======END===="); - if (boost::geometry::within(point, new_poly)) - possible_lanelets.push_back(nearestLanelets[id].second); + while (boost::geometry::within(point, nearestLanelets[id].second.polygon2d())) + { + possible_lanelets.push_back(nearestLanelets[id].second); + id++; + if (id >= nearestLanelets.size()) + break; } return possible_lanelets; } @@ -87,11 +63,11 @@ std::vector getLaneletsFromPoint(const lanelet::LaneletMapPtr& std::vector nonConnectedAdjacentLeft(const lanelet::LaneletMapPtr& semantic_map, const lanelet::BasicPoint2d& point, const unsigned int n) { - + lanelet::LaneletMapConstPtr const_ptr = semantic_map; auto possible_lanelets = nonConnectedAdjacentLeft(const_ptr, point, n); - - + + std::vector return_lanelets; for (auto llt : possible_lanelets) { @@ -103,24 +79,24 @@ std::vector nonConnectedAdjacentLeft(const lanelet::LaneletMap std::vector nonConnectedAdjacentLeft(const lanelet::LaneletMapConstPtr& semantic_map, const lanelet::BasicPoint2d& input_point, const unsigned int n) { - + // Check if the map is loaded yet if (!semantic_map || semantic_map->laneletLayer.size() == 0) { throw std::invalid_argument("Map is not set or does not contain lanelets"); } - + auto input_lanelets = getLaneletsFromPoint(semantic_map, input_point); - + if (input_lanelets.empty()) { throw std::invalid_argument("Input point x: " + std::to_string(input_point.x()) + ", y: " + std::to_string(input_point.y()) + " is not in the map"); } - - auto input_lanelet = input_lanelets[0]; - + + auto input_lanelet = input_lanelets[0]; + auto point_downtrack = carma_wm::geometry::trackPos(input_lanelet, input_point).downtrack; - + auto point_downtrack_ratio = point_downtrack / carma_wm::geometry::trackPos(input_lanelet, input_lanelet.centerline().back().basicPoint2d()).downtrack; auto point_on_ls = input_lanelet.leftBound2d()[std::round((input_lanelet.leftBound2d().size()- 1) * point_downtrack_ratio) ]; @@ -149,13 +125,13 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& std::unordered_set possible_lanelets; // This loop identifes the lanelets which this point lies within that could be impacted by the geofence - // This loop somewhat inefficiently calls the findNearest method iteratively until all the possible lanelets are identified. + // This loop somewhat inefficiently calls the findNearest method iteratively until all the possible lanelets are identified. // The reason findNearest is used instead of nearestUntil is because that method orders results by bounding box which - // can give invalid sequences when dealing with large curved lanelets. - bool continue_search = true; + // can give invalid sequences when dealing with large curved lanelets. + bool continue_search = true; size_t nearest_count = 0; while (continue_search) { - + nearest_count += 10; // Increase the index search radius by 10 each loop until all nearby lanelets are found for (const auto& ll_pair : lanelet::geometry::findNearest(lanelet_map->laneletLayer, gf_pts[idx].basicPoint2d(), nearest_count)) { // Get the nearest lanelets and iterate over them @@ -167,7 +143,7 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& double dist = std::get<0>(ll_pair); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Distance to lanelet " << ll.id() << ": " << dist << " max_lane_width: " << max_lane_width); - + if (dist > max_lane_width) { // Only save values closer than max_lane_width. Since we are iterating in distance order when we reach this distance the search can stop continue_search = false; break; @@ -208,12 +184,12 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Overlaps starting line... Picking llt: " << llt.id()); affected_lanelets.insert(llt); - } + } } } break; - } + } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Checking possible lanelets"); // check if each lines connecting end points of the llt is crossing with the line connecting current and next gf_pts @@ -222,7 +198,7 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Evaluating lanelet: " << llt.id()); lanelet::BasicLineString2d gf_dir_line({gf_pts[idx].basicPoint2d(), gf_pts[idx+1].basicPoint2d()}); lanelet::BasicLineString2d llt_boundary({(llt.leftBound2d().end() -1)->basicPoint2d(), (llt.rightBound2d().end() - 1)->basicPoint2d()}); - + // record the llts that are on the same dir if (boost::geometry::intersects(llt_boundary, gf_dir_line)) { @@ -230,11 +206,11 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& affected_lanelets.insert(llt); } // check condition if two geofence points are in one lanelet then check matching direction and record it also - else if (boost::geometry::within(gf_pts[idx+1].basicPoint2d(), llt.polygon2d()) && + else if (boost::geometry::within(gf_pts[idx+1].basicPoint2d(), llt.polygon2d()) && affected_lanelets.find(llt) == affected_lanelets.end()) - { + { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Within new lanelet"); - lanelet::BasicPoint2d median({((llt.leftBound2d().end() - 1)->basicPoint2d().x() + (llt.rightBound2d().end() - 1)->basicPoint2d().x())/2 , + lanelet::BasicPoint2d median({((llt.leftBound2d().end() - 1)->basicPoint2d().x() + (llt.rightBound2d().end() - 1)->basicPoint2d().x())/2 , ((llt.leftBound2d().end() - 1)->basicPoint2d().y() + (llt.rightBound2d().end() - 1)->basicPoint2d().y())/2}); // turn into vectors Eigen::Vector2d vec_to_median(median); @@ -258,8 +234,8 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "interior_angle: " << interior_angle); // Save the lanelet if the direction of two points inside aligns with that of the lanelet - if (interior_angle < M_PI_2 && interior_angle >= 0) - affected_lanelets.insert(llt); + if (interior_angle < M_PI_2 && interior_angle >= 0) + affected_lanelets.insert(llt); } else { @@ -268,9 +244,9 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& } } - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "affected_lanelets size: " << affected_lanelets.size()); - // Currently only returning lanelet, but this could be expanded to LanelerOrArea compound object + // Currently only returning lanelet, but this could be expanded to LanelerOrArea compound object // by implementing non-const version of that LaneletOrArea lanelet::ConstLaneletOrAreas affected_parts; // return results in ascending downtrack order from the first point of geofence @@ -286,7 +262,7 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& { affected_parts.push_back(pair.second); } - + return affected_parts; } @@ -297,10 +273,10 @@ std::unordered_set filterSuccessorLanelets(const std::unordere if (!routing_graph) { throw std::invalid_argument("No routing graph available"); } - + std::unordered_set filtered_lanelets; // we utilize routes to filter llts that are overlapping but not connected - // as this is the last lanelet + // as this is the last lanelet // we have to filter the llts that are only geometrically overlapping yet not connected to prev llts for (auto recorded_llt: root_lanelets) { From 2cdf6a958e7bf5d3f50fb2d11e0615c249ad9615 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Mon, 4 Dec 2023 20:25:09 +0000 Subject: [PATCH 09/18] remove subsystem timeouts --- .../config/drivers_controller_config.yaml | 32 ++++++++---- ...ironment_perception_controller_config.yaml | 20 +++++-- .../config/guidance_controller_config.yaml | 52 ++++++++++++++----- .../localization_controller_config.yaml | 49 +++++++++++++---- .../config/v2x_controller_config.yaml | 19 +++++-- 5 files changed, 129 insertions(+), 43 deletions(-) diff --git a/subsystem_controllers/config/drivers_controller_config.yaml b/subsystem_controllers/config/drivers_controller_config.yaml index f5ffbb5f82..185d29c9c0 100644 --- a/subsystem_controllers/config/drivers_controller_config.yaml +++ b/subsystem_controllers/config/drivers_controller_config.yaml @@ -2,27 +2,37 @@ ros__parameters: # Long: Timeout for each service to be detected as available in milliseconds # Units: milliseconds - service_timeout_ms : 2000 + service_timeout_ms : 200 # Long: Timeout for each service call in milliseconds # Units: milliseconds - call_timeout_ms : 10000 + call_timeout_ms : 1000 + # String: The namespace for nodes in this subsystem. All nodes under this namespace will have their lifecycle's managed by this controller subsystem_namespace: /hardware_interface + + # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed required_subsystem_nodes: [''] + + # List of nodes which will not be directly managed by this subsystem controller + # but which are required to be operational for the subsystem to function unmanaged_required_nodes: [''] - excluded_namespace_nodes: - - /hardware_interface/carla_lidar_driver - - /hardware_interface/carla_gnss_driver - ros1_required_drivers: - - /hardware_interface/carla_driver - ros1_camera_drivers: - - /hardware_interface/carla_camera_driver - full_subsystem_required: false + # List of nodes in the namespace which will not be managed by this subsystem controller + # Specifically includes the lidar and gps nodes which are handled in other subsystem controllers + excluded_namespace_nodes : [''] + + # List of ros1 controller drivers (node name) to consider required and who's failure shall result in automation abort + ros1_required_drivers: [''] + + # List of ros1 camera drivers (node name) to consider required and who's failure shall result in automation abort. + ros1_camera_drivers: [''] + + # Boolean: If this flag is true then all nodes under subsystem_namespace are treated as required in addition to any nodes in required_subsystem_nodes + full_subsystem_required: false # Int: The time allocated for system startup in seconds startup_duration: 30 # Double: The timeout threshold for essential drivers in ms - required_driver_timeout: 30000.0 \ No newline at end of file + required_driver_timeout: 3000.0 \ No newline at end of file diff --git a/subsystem_controllers/config/environment_perception_controller_config.yaml b/subsystem_controllers/config/environment_perception_controller_config.yaml index c5fdf5053e..2cbc6f877e 100644 --- a/subsystem_controllers/config/environment_perception_controller_config.yaml +++ b/subsystem_controllers/config/environment_perception_controller_config.yaml @@ -2,14 +2,24 @@ ros__parameters: # Long: Timeout for each service to be detected as available in milliseconds # Units: milliseconds - service_timeout_ms : 2000 + service_timeout_ms : 200 # Long: Timeout for each service call in milliseconds # Units: milliseconds - call_timeout_ms : 15000 + call_timeout_ms : 1500 + # String: The namespace for nodes in this subsystem. All nodes under this namespace will have their lifecycle's managed by this controller subsystem_namespace: /environment - required_subsystem_nodes: [''] - unmanaged_required_nodes: + + # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed + # Since CARMA assumes connected vehicles in most cases the current minimum node set is from motion prediction up + required_subsystem_nodes: + - /environment/external_object + + # List of nodes which will not be directly managed by this subsystem controller + # but which are required to be operational for the subsystem to function + unmanaged_required_nodes: - /hardware_interface/velodyne_lidar_driver_wrapper_node - full_subsystem_required: true + + # Boolean: If this flag is true then all nodes under subsystem_namespace are treated as required in addition to any nodes in required_subsystem_nodes + full_subsystem_required: true \ No newline at end of file diff --git a/subsystem_controllers/config/guidance_controller_config.yaml b/subsystem_controllers/config/guidance_controller_config.yaml index 68cefc6a6d..b132b7b407 100644 --- a/subsystem_controllers/config/guidance_controller_config.yaml +++ b/subsystem_controllers/config/guidance_controller_config.yaml @@ -2,21 +2,47 @@ ros__parameters: # Long: Timeout for each service to be detected as available in milliseconds # Units: milliseconds - service_timeout_ms : 2000 + service_timeout_ms : 200 # Long: Timeout for each service call in milliseconds # Units: milliseconds - call_timeout_ms : 10000 + call_timeout_ms : 1000 + # String: The namespace for nodes in this subsystem. All nodes under this namespace will have their lifecycle's managed by this controller subsystem_namespace: /guidance - required_subsystem_nodes: [''] + + # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed + required_subsystem_nodes: + - /guidance/arbitrator + - /guidance/guidance_node + - /guidance/plan_delegator + - /guidance/trajectory_executor + - /guidance/twist_filter + - /guidance/twist_gate + - /guidance/plugins/route_following_plugin # The minimal set of guidance plugins for system operation are route_following/inlanecruising/pure_pursuit_wrapper + - /guidance/plugins/inlanecruising_plugin + - /guidance/pure_pursuit_wrapper + - /guidance/yield_plugin + + # List of nodes which will not be directly managed by this subsystem controller + # but which are required to be operational for the subsystem to function unmanaged_required_nodes: [''] # TODO add the controller driver once it is integrated with ROS2 - full_subsystem_required: true + + # Boolean: If this flag is true then all nodes under subsystem_namespace are treated as required in addition to any nodes in required_subsystem_nodes + full_subsystem_required: false + + # List of guidance plugins (node name) to consider required and who's failure shall result in automation abort. + # Required plugins will be automatically activated at startup + # Required plugins cannot be deactivated by the user required_plugins: - /guidance/plugins/route_following_plugin - /guidance/plugins/pure_pursuit_wrapper - /guidance/plugins/inlanecruising_plugin - /guidance/plugins/cooperative_lanechange + + # List of guidance plugins which are not required but the user wishes to have automatically activated + # so that the user doesn't need to manually activate them via the UI on each launch (though they still can) + # this list should have zero intersection with the required_plugins auto_activated_plugins: - /guidance/plugins/lci_strategic_plugin - /guidance/plugins/intersection_transit_maneuvering @@ -25,20 +51,18 @@ - /guidance/plugins/sci_strategic_plugin - /guidance/plugins/stop_controlled_intersection_tactical_plugin - /guidance/plugins/platoon_control_ihp - - /guidance/plugins/platoon_strategic_ihp_node + - /guidance/plugins/platoon_strategic_ihp - /guidance/plugins/platooning_tactical_plugin_node - /guidance/plugins/yield_plugin + + # List of guidance plugins that are ported to ROS2. If not in this list, it is assumed to be ROS1, and not managed. ros2_initial_plugins: - /guidance/plugins/inlanecruising_plugin - - /guidance/plugins/platoon_strategic_ihp_node - - /guidance/plugins/stop_and_wait_plugin - - /guidance/plugins/route_following_plugin - - /guidance/plugins/platooning_tactical_plugin_node - - /guidance/plugins/cooperative_lanechange + - /guidance/plugins/yield_plugin - /guidance/plugins/light_controlled_intersection_tactical_plugin + - /guidance/plugins/platooning_tactical_plugin_node - /guidance/plugins/sci_strategic_plugin - - /guidance/plugins/stop_controlled_intersection_tactical_plugin + - /guidance/plugins/cooperative_lanechange + - /guidance/plugins/platooning_tactical_plugin_node - /guidance/plugins/yield_plugin - - /guidance/plugins/pure_pursuit_wrapper - - /guidance/plugins/lci_strategic_plugin - - /guidance/plugins/intersection_transit_maneuvering + - /guidance/plugins/pure_pursuit_wrapper \ No newline at end of file diff --git a/subsystem_controllers/config/localization_controller_config.yaml b/subsystem_controllers/config/localization_controller_config.yaml index d4654737b2..a95a313dfc 100644 --- a/subsystem_controllers/config/localization_controller_config.yaml +++ b/subsystem_controllers/config/localization_controller_config.yaml @@ -2,25 +2,56 @@ ros__parameters: # Long: Timeout for each service to be detected as available in milliseconds # Units: milliseconds - service_timeout_ms : 2000 + service_timeout_ms : 200 # Long: Timeout for each service call in milliseconds # Units: milliseconds - call_timeout_ms : 10000 + call_timeout_ms : 1000 + # String: The namespace for nodes in this subsystem. All nodes under this namespace will have their lifecycle's managed by this controller subsystem_namespace: /localization - required_subsystem_nodes: [''] - unmanaged_required_nodes: [''] + + # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed + # Once initialized the localization subsystem requires a GNSS, localization manage, and ekf_localizer in order to continue functioning + required_subsystem_nodes: + - /localization/ekf_localizer + - /localization/gnss_to_map_convertor + - /localization/localization_manager + + # List of nodes which will not be directly managed by this subsystem controller + # but which are required to be operational for the subsystem to function + unmanaged_required_nodes: [''] # TODO localization needs GPS and lidar but this will be handled with more complex logic then this list + + # Boolean: If this flag is true then all nodes under subsystem_namespace are treated as required in addition to any nodes in required_subsystem_nodes full_subsystem_required: true + + # List of nodes which are sensors used by the localization system and have their fault behavior described by + # the sensor_fault_map parameter sensor_nodes: - /hardware_interface/velodyne_lidar_driver_wrapper_node - # TODO update the fault map once ros2 gps is integrated + - /hardware_interface/temp_lidar_2 + - /hardware_interface/temp_gps + + # JSON String: Sensor Fault Map + # NOTE: It would have been preferable to use a 2d array here but ROS2 Foxy does not support this type of parameter + # The sensor_fault_map is described as a 2d array where the last element is the desired status alert type + # The set of values in front should have the same size as the sensor_nodes list and describe the status of those nodes in that order + # Input Enum Values: + # Operational: 1 + # Failed: 0 + # Output Enum Values: + # FATAL: 0 + # OPERATIONAL: 1 + # CAUTION: 2 + # WARNING: 3 sensor_fault_map: ' { - "sensor_fault_map": + "sensor_fault_map": [ - [0,0] - ] + [0,1,1,2], + [1,0,1,2], + [0,0,1,3], + [0,0,0,0] + ], } ' - diff --git a/subsystem_controllers/config/v2x_controller_config.yaml b/subsystem_controllers/config/v2x_controller_config.yaml index b0341b6cc4..adf433b081 100644 --- a/subsystem_controllers/config/v2x_controller_config.yaml +++ b/subsystem_controllers/config/v2x_controller_config.yaml @@ -2,14 +2,25 @@ ros__parameters: # Long: Timeout for each service to be detected as available in milliseconds # Units: milliseconds - service_timeout_ms : 2000 + service_timeout_ms : 200 # Long: Timeout for each service call in milliseconds # Units: milliseconds - call_timeout_ms : 10000 + call_timeout_ms : 1000 + # String: The namespace for nodes in this subsystem. All nodes under this namespace will have their lifecycle's managed by this controller subsystem_namespace: /message - required_subsystem_nodes: [''] - unmanaged_required_nodes: + + # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed + # The V2X stack requires the message node and j2735_convertor node to be functional + required_subsystem_nodes: + - /message/cpp_message_node + - /message/j2735_convertor_node + + # List of nodes which will not be directly managed by this subsystem controller + # but which are required to be operational for the subsystem to function + unmanaged_required_nodes: - /hardware_interface/dsrc_driver_node + + # Boolean: If this flag is true then all nodes under subsystem_namespace are treated as required in addition to any nodes in required_subsystem_nodes full_subsystem_required: true \ No newline at end of file From c512ccb49ab927c07f6aa3ecf5d06631bd961aaf Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Mon, 4 Dec 2023 20:26:30 +0000 Subject: [PATCH 10/18] remove plan_delegator --- plan_delegator/config/plan_delegator_params.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plan_delegator/config/plan_delegator_params.yaml b/plan_delegator/config/plan_delegator_params.yaml index 104553ebb2..4c54e3cd7f 100644 --- a/plan_delegator/config/plan_delegator_params.yaml +++ b/plan_delegator/config/plan_delegator_params.yaml @@ -26,9 +26,9 @@ min_speed: 2.2352 # Units: Seconds duration_to_signal_before_lane_change: 2.5 -# Int: The maximum duration that Plan Delegator will wait after calling a tactical plugin's trajectory planning service; if trajectory +# Int: The maximum duration that Plan Delegator will wait after calling a tactical plugin's trajectory planning service; if trajectory # generation takes longer than this, then trajectory planning will immediately end for the current trajectory planning iteration. # NOTE: It is highly desirable to maintain a timeout of 100 ms or less, but trajectory generation success cannot be guaranteed with this duration # for tactical plugins (primarily cooperative_lanechange) in all test scenarios at this time. # Units: Milliseconds -tactical_plugin_service_call_timeout: 350 \ No newline at end of file +tactical_plugin_service_call_timeout: 100 \ No newline at end of file From 0274bdcdf134da6044dec5332a2115f40e708149 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Tue, 5 Dec 2023 04:14:29 +0000 Subject: [PATCH 11/18] address comments --- carma_wm/src/WMListenerWorker.cpp | 43 +++---- .../src/inlanecruising_plugin.cpp | 4 +- yield_plugin/src/yield_plugin.cpp | 114 +++++++++--------- 3 files changed, 83 insertions(+), 78 deletions(-) diff --git a/carma_wm/src/WMListenerWorker.cpp b/carma_wm/src/WMListenerWorker.cpp index 8fe335fe1b..427821d631 100644 --- a/carma_wm/src/WMListenerWorker.cpp +++ b/carma_wm/src/WMListenerWorker.cpp @@ -119,27 +119,28 @@ void WMListenerWorker::enableUpdatesWithoutRoute() // helper function to log SignalizedIntersectionManager content void logSignalizedIntersectionManager(const carma_wm::SignalizedIntersectionManager& sim) { - for (auto pair : sim.intersection_id_to_regem_id_) + for (auto const &[intersection_id, regulatory_element_id] : sim.intersection_id_to_regem_id_) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "inter id: " << (int)pair.first << ", regem id: " << pair.second); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), + "inter id: " << intersection_id << ", regem id: " << regulatory_element_id); } - for (auto pair : sim.signal_group_to_entry_lanelet_ids_) + for (auto const &[signal_id, entry_llt_ids] : sim.signal_group_to_entry_lanelet_ids_) { - for (auto iter = pair.second.begin(); iter != pair.second.end(); iter++) + for (auto iter = entry_llt_ids.begin(); iter != entry_llt_ids.end(); iter++) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", entry llt id: " << *iter); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", entry llt id: " << *iter); } } - for (auto pair : sim.signal_group_to_exit_lanelet_ids_) + for (auto const &[signal_id, exit_llt_ids] : sim.signal_group_to_exit_lanelet_ids_) { - for (auto iter = pair.second.begin(); iter != pair.second.end(); iter++) + for (auto iter = exit_llt_ids.begin(); iter != exit_llt_ids.end(); iter++) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", exit llt id: " << *iter); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", exit llt id: " << *iter); } } - for (auto pair : sim.signal_group_to_traffic_light_id_) + for (auto const &[signal_id, regem_id] : sim.signal_group_to_traffic_light_id_) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", regem id: " << pair.second); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", regem id: " << regem_id); } } @@ -222,10 +223,10 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " sends record of traffic_lights_id size: " << gf_ptr->traffic_light_id_lookup_.size()); - for (auto pair : gf_ptr->traffic_light_id_lookup_) + for (auto const &[traffic_light_id, lanelet_id] : gf_ptr->traffic_light_id_lookup_) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Adding new pair for traffic light ids: " << pair.first << ", and lanelet::Id: " << pair.second); - world_model_->setTrafficLightIds(pair.first, pair.second); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Adding new pair for traffic light ids: " << traffic_light_id << ", and lanelet::Id: " << lanelet_id); + world_model_->setTrafficLightIds(traffic_light_id, lanelet_id); } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " sends record of intersections size: " << gf_ptr->sim_.intersection_id_to_regem_id_.size()); @@ -236,9 +237,9 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " requests removal of size: " << gf_ptr->remove_list_.size()); - for (auto pair : gf_ptr->remove_list_) + for (auto const &[lanelet_id, lanelet_to_remove] : gf_ptr->remove_list_) { - auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(pair.first); + auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(lanelet_id); // we can only check by id, if the element is there // this is only for speed optimization, as world model here should blindly accept the map update received auto regems_copy_to_check = parent_llt.regulatoryElements(); // save local copy as the regem can be deleted during iteration @@ -246,7 +247,7 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un for (auto regem: regems_copy_to_check) { // we can't use the deserialized element as its data address conflicts the one in this node - if (pair.second->id() == regem->id()) world_model_->getMutableMap()->remove(parent_llt, regem); + if (lanelet_to_remove->id() == regem->id()) world_model_->getMutableMap()->remove(parent_llt, regem); } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Regems left in lanelet after removal: " << parent_llt.regulatoryElements().size()); @@ -255,12 +256,12 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " requests update of size: " << gf_ptr->update_list_.size()); // we should extract general regem to specific type of regem the geofence specifies - for (auto pair : gf_ptr->update_list_) + for (auto const &[lanelet_id, lanelet_to_update]: gf_ptr->update_list_) { - auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(pair.first); + auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(lanelet_id); - auto regemptr_it = world_model_->getMutableMap()->regulatoryElementLayer.find(pair.second->id()); + auto regemptr_it = world_model_->getMutableMap()->regulatoryElementLayer.find(lanelet_to_update->id()); // if this regem is already in the map. // This section is expected to be called to add back regulations which were previously removed by expired geofences. @@ -273,8 +274,8 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un } else // Updates are treated as new regulations after the old value was removed. In both cases we enter this block. { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "New regulatory element at lanelet: " << parent_llt.id() << ", and id: " << pair.second->id()); - newRegemUpdateHelper(parent_llt, pair.second.get()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "New regulatory element at lanelet: " << parent_llt.id() << ", and id: " << lanelet_to_update->id()); + newRegemUpdateHelper(parent_llt, lanelet_to_update.get()); } } diff --git a/inlanecruising_plugin/src/inlanecruising_plugin.cpp b/inlanecruising_plugin/src/inlanecruising_plugin.cpp index 5db2c2636f..451a8b29a2 100644 --- a/inlanecruising_plugin/src/inlanecruising_plugin.cpp +++ b/inlanecruising_plugin/src/inlanecruising_plugin.cpp @@ -140,7 +140,9 @@ void InLaneCruisingPlugin::plan_trajectory_callback( } else { - RCLCPP_WARN_STREAM(nh_->get_logger(), "UNABLE TO CALL YIELD PLUGIN!!!!"); + // Sometimes the yield plugin's service call may be unsuccessful due to its computationally expensive logic. + // However, consecutive calls can be successful, so return original trajectory for now + RCLCPP_WARN_STREAM(nh_->get_logger(), "Unable to Call Yield Plugin"); resp->trajectory_plan = original_trajectory; } } diff --git a/yield_plugin/src/yield_plugin.cpp b/yield_plugin/src/yield_plugin.cpp index f46708e6d8..2223c5362a 100644 --- a/yield_plugin/src/yield_plugin.cpp +++ b/yield_plugin/src/yield_plugin.cpp @@ -60,7 +60,7 @@ namespace yield_plugin for (size_t i=0; iprojectECEF( { (double)ecef_point.ecef_x/100.0, (double)ecef_point.ecef_y/100.0, (double)ecef_point.ecef_z/100.0 } , 1); - + return lanelet::traits::to2D(map_point); - } - - + } + + carma_v2x_msgs::msg::MobilityResponse YieldPlugin::compose_mobility_response(const std::string& resp_recipient_id, const std::string& req_plan_id, bool response) const { @@ -123,7 +123,7 @@ namespace yield_plugin out_mobility_response.is_accepted = true; } else out_mobility_response.is_accepted = false; - + return out_mobility_response; } @@ -143,10 +143,12 @@ namespace yield_plugin RCLCPP_DEBUG(nh_->get_logger(),"Cooperative Lane Change Request Received"); lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_RECEIVED; lc_status_msg.description = "Received lane merge request"; + if (incoming_request.m_header.recipient_id == config_.vehicle_id) { RCLCPP_DEBUG(nh_->get_logger(),"CLC Request correctly received"); } + // extract mobility header std::string req_sender_id = incoming_request.m_header.sender_id; std::string req_plan_id = incoming_request.m_header.plan_id; @@ -184,30 +186,30 @@ namespace yield_plugin double req_timestamp = (double)incoming_request.m_header.timestamp / 1000.0 - current_time_sec; set_incoming_request_info(req_traj_plan, req_traj_speed, req_plan_time, req_timestamp); - + if (req_expiration_sec - current_time_sec >= config_.tpmin && cooperative_request_acceptable_) { timesteps_since_last_req_ = 0; lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_ACCEPTED; lc_status_msg.description = "Accepted lane merge request"; - response_to_clc_req = true; - RCLCPP_DEBUG(nh_->get_logger(),"CLC accepted"); + response_to_clc_req = true; + RCLCPP_DEBUG(nh_->get_logger(),"CLC accepted"); } else { lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_REJECTED; lc_status_msg.description = "Rejected lane merge request"; response_to_clc_req = false; - RCLCPP_DEBUG(nh_->get_logger(),"CLC rejected"); + RCLCPP_DEBUG(nh_->get_logger(),"CLC rejected"); } carma_v2x_msgs::msg::MobilityResponse outgoing_response = compose_mobility_response(req_sender_id, req_plan_id, response_to_clc_req); mobility_response_publisher_(outgoing_response); lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::RESPONSE_SENT; - RCLCPP_DEBUG(nh_->get_logger(),"response sent"); + RCLCPP_DEBUG(nh_->get_logger(),"response sent"); } } lc_status_publisher_(lc_status_msg); - + } void YieldPlugin::set_incoming_request_info(std::vector req_trajectory, double req_speed, double req_planning_time, double req_timestamp) @@ -215,7 +217,7 @@ namespace yield_plugin req_trajectory_points_ = req_trajectory; req_target_speed_ = req_speed; req_target_plan_time_ = req_planning_time; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_target_plan_time_" << req_target_plan_time_); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_target_plan_time_" << req_target_plan_time_); req_timestamp_ = req_timestamp; } @@ -226,7 +228,7 @@ namespace yield_plugin } void YieldPlugin::plan_trajectory_callback( - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { RCLCPP_DEBUG(nh_->get_logger(),"Yield_plugin was called!"); @@ -253,7 +255,7 @@ namespace yield_plugin { RCLCPP_DEBUG(nh_->get_logger(),"unreliable CLC communication, switching to object avoidance"); yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, req->vehicle_state.longitudinal_vel); // Compute the trajectory - } + } } else { @@ -263,7 +265,7 @@ namespace yield_plugin yield_trajectory.header.frame_id = "map"; yield_trajectory.header.stamp = nh_->now(); yield_trajectory.trajectory_id = original_trajectory.trajectory_id; - yield_trajectory.initial_longitudinal_velocity = original_trajectory.initial_longitudinal_velocity;//copy the original trajectory's desired speed for now. + yield_trajectory.initial_longitudinal_velocity = original_trajectory.initial_longitudinal_velocity;//copy the original trajectory's desired speed for now. resp->trajectory_plan = yield_trajectory; @@ -324,7 +326,7 @@ namespace yield_plugin if (planning_time > min_time) { cooperative_request_acceptable_ = true; - cooperative_trajectory = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time); + cooperative_trajectory = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time); } else { @@ -332,7 +334,7 @@ namespace yield_plugin RCLCPP_DEBUG(nh_->get_logger(),"The incoming requested trajectory is rejected, due to insufficient gap"); cooperative_trajectory = original_tp; } - + } else { @@ -344,7 +346,7 @@ namespace yield_plugin return cooperative_trajectory; } - carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::generate_JMT_trajectory(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time) + carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::generate_JMT_trajectory(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time) { carma_planning_msgs::msg::TrajectoryPlan jmt_trajectory; std::vector jmt_trajectory_points; @@ -356,15 +358,15 @@ namespace yield_plugin double original_max_speed = max_trajectory_speed(original_tp.trajectory_points); RCLCPP_DEBUG_STREAM(nh_->get_logger(),"original_max_speed" << original_max_speed); - std::vector values = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos, + std::vector values = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos, goal_pos, - initial_velocity, - goal_velocity, - initial_accel, - goal_accel, - initial_time, + initial_velocity, + goal_velocity, + initial_accel, + goal_accel, + initial_time, planning_time); - + std::vector original_traj_downtracks = get_relative_downtracks(original_tp); std::vector calculated_speeds = {}; calculated_speeds.push_back(initial_velocity); @@ -403,7 +405,7 @@ namespace yield_plugin RCLCPP_DEBUG(nh_->get_logger(),"Target speed is zero"); // if speed is zero, the vehicle will stay in previous location. jmt_tpp = jmt_trajectory_points.back(); - // MISH: Potential bug where if purepursuit accidentally picks a point that has previous time, then it may speed up. target_time is assumed to increase + // MISH: Potential bug where if purepursuit accidentally picks a point that has previous time, then it may speed up. target_time is assumed to increase jmt_tpp.target_time = rclcpp::Time(std::max((rclcpp::Time(jmt_trajectory_points[0].target_time) + rclcpp::Duration(traj_target_time*1e9)).seconds(), rclcpp::Time(jmt_trajectory_points.back().target_time).seconds()) * 1e9); RCLCPP_DEBUG_STREAM(nh_->get_logger(), "empty x: " << jmt_tpp.x << ", y:" << jmt_tpp.y << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())); @@ -411,14 +413,14 @@ namespace yield_plugin } prev_speed = current_speed; } - + jmt_trajectory.header = original_tp.header; jmt_trajectory.trajectory_id = original_tp.trajectory_id; jmt_trajectory.trajectory_points = jmt_trajectory_points; return jmt_trajectory; } - - std::optional YieldPlugin::detect_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory1, const std::vector& trajectory2, double collision_radius) + + std::optional YieldPlugin::detect_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory1, const std::vector& trajectory2, double collision_radius) { // Iterate through each pair of consecutive points in the trajectories @@ -456,13 +458,13 @@ namespace yield_plugin RCLCPP_DEBUG(nh_->get_logger(), "Predicted states are not on the route! ignoring"); return std::nullopt; } - + double smallest_dist = std::numeric_limits::max(); - for (size_t i = 0; i < trajectory1.trajectory_points.size() - 1; ++i) + for (size_t i = 0; i < trajectory1.trajectory_points.size() - 1; ++i) { auto p1a = trajectory1.trajectory_points.at(i); auto p1b = trajectory1.trajectory_points.at(i + 1); - for (size_t j = on_route_idx; j < trajectory2.size() - 1; ++j) + for (size_t j = on_route_idx; j < trajectory2.size() - 1; ++j) { auto p2a = trajectory2.at(j); auto p2b = trajectory2.at(j + 1); @@ -497,7 +499,7 @@ namespace yield_plugin return std::nullopt; } - if (distance > collision_radius) + if (distance > collision_radius) { continue; } @@ -517,7 +519,7 @@ namespace yield_plugin { RCLCPP_WARN_STREAM(nh_->get_logger(), "Collision detected at timestamp " << std::to_string(p2a_t) << ", x: " << x1 << ", y: " << y1 << ", within distance: " << distance); return rclcpp::Time(p2a.header.stamp); - } + } } } @@ -525,8 +527,8 @@ namespace yield_plugin return std::nullopt; } - carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector& external_objects, double initial_velocity) - { + carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector& external_objects, double initial_velocity) + { if (original_tp.trajectory_points.size() < 2) { RCLCPP_WARN(nh_->get_logger(), "Yield plugin received less than 2 points in update_traj_for_object, returning unchanged..."); @@ -582,7 +584,7 @@ namespace yield_plugin new_list.insert(new_list.end(), curr_obstacle.predictions.cbegin(), curr_obstacle.predictions.cend()); auto collision_time = detect_collision_time(original_tp, new_list, config_.intervehicle_collision_distance); - + if (collision_time != std::nullopt) { checked_external_object_ids.insert(curr_obstacle.id); @@ -599,11 +601,11 @@ namespace yield_plugin lanelet::BasicPoint2d point(original_tp.trajectory_points[0].x,original_tp.trajectory_points[0].y); double vehicle_downtrack = wm_->routeTrackPos(point).downtrack; - + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"vehicle_downtrack: " << vehicle_downtrack); - // Issue (https://github.com/usdot-fhwa-stol/carma-platform/issues/2155): If the yield_plugin can detect if the roadway object is moving along the route, - // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon. + // Issue (https://github.com/usdot-fhwa-stol/carma-platform/issues/2155): If the yield_plugin can detect if the roadway object is moving along the route, + // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon. if(earliest_collision_obj.has_value()) { @@ -613,10 +615,10 @@ namespace yield_plugin double object_downtrack = wm_->routeTrackPos(point_o).downtrack; RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack: " << object_downtrack); - + double dist_to_object = object_downtrack - vehicle_downtrack; RCLCPP_DEBUG_STREAM(nh_->get_logger(),"dist_to_object: " << dist_to_object); - + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object speed: " << earliest_collision_obj.value().velocity.twist.linear.x); // Distance from the original trajectory point to the lead vehicle/object @@ -635,11 +637,11 @@ namespace yield_plugin // check if a digital_gap is available double digital_gap = check_traj_for_digital_min_gap(original_tp); RCLCPP_DEBUG_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap); - // if a digital gap is available, it is replaced as safety gap + // if a digital gap is available, it is replaced as safety gap safety_gap = std::max(safety_gap, digital_gap); } // safety gap is implemented - double goal_pos = x_lead - safety_gap; + double goal_pos = x_lead - safety_gap; if (goal_velocity <= config_.min_obstacle_speed){ RCLCPP_WARN(nh_->get_logger(),"The obstacle is not moving, goal velocity is set to 0"); @@ -673,14 +675,14 @@ namespace yield_plugin { tp = t_ref; } - + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Object avoidance planning time: " << tp); update_tpp_vector = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, tp); return update_tpp_vector; } - + RCLCPP_DEBUG(nh_->get_logger(),"No collision detection, so trajectory not modified."); return original_tp; } @@ -715,7 +717,7 @@ namespace yield_plugin double YieldPlugin::polynomial_calc_d(std::vector coeff, double x) const { double result = 0; - for (size_t i = 0; i < coeff.size()-1; i++) + for (size_t i = 0; i < coeff.size()-1; i++) { double value = (int)(coeff.size() - 1 - i) * coeff.at(i) * pow(x, (int)(coeff.size() - 2 - i)); result = result + value; @@ -730,7 +732,7 @@ namespace yield_plugin { double dx = trajectory_points.at(i + 1).x - trajectory_points.at(i).x; double dy = trajectory_points.at(i + 1).y - trajectory_points.at(i).y; - double d = sqrt(dx*dx + dy*dy); + double d = sqrt(dx*dx + dy*dy); double t = (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() - rclcpp::Time(trajectory_points.at(i).target_time).seconds()); double v = d/t; if(v > max_speed) @@ -752,11 +754,11 @@ namespace yield_plugin if (llts.empty()) { RCLCPP_WARN_STREAM(nh_->get_logger(),"Trajectory point: x= " << original_tp.trajectory_points.at(i).x << "y="<< original_tp.trajectory_points.at(i).y); - + throw std::invalid_argument("Trajectory Point is not on a valid lanelet."); } auto digital_min_gap = llts[0].regulatoryElementsAs(); //Returns a list of these elements) - if (!digital_min_gap.empty()) + if (!digital_min_gap.empty()) { double digital_gap = digital_min_gap[0]->getMinimumGap(); // Provided gap is in meters RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Digital Gap found with value: " << digital_gap); @@ -766,14 +768,14 @@ namespace yield_plugin return desired_gap; } - void YieldPlugin::set_georeference_string(const std::string& georeference) + void YieldPlugin::set_georeference_string(const std::string& georeference) { map_projector_ = std::make_shared(georeference.c_str()); // Build projector from proj string } void YieldPlugin::set_external_objects(const std::vector& object_list) { - external_objects_ = object_list; + external_objects_ = object_list; } } // namespace yield_plugin From b4df0d72a148608ea18ecf0ded2ae9db6f21a6a1 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Tue, 5 Dec 2023 08:13:33 +0000 Subject: [PATCH 12/18] plan_delegator timeout --- plan_delegator/config/plan_delegator_params.yaml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/plan_delegator/config/plan_delegator_params.yaml b/plan_delegator/config/plan_delegator_params.yaml index 4c54e3cd7f..6bedaa2e4d 100644 --- a/plan_delegator/config/plan_delegator_params.yaml +++ b/plan_delegator/config/plan_delegator_params.yaml @@ -26,9 +26,10 @@ min_speed: 2.2352 # Units: Seconds duration_to_signal_before_lane_change: 2.5 -# Int: The maximum duration that Plan Delegator will wait after calling a tactical plugin's trajectory planning service; if trajectory +# Int: The maximum duration that Plan Delegator will wait after calling a tactical plugin's trajectory planning service; if trajectory # generation takes longer than this, then trajectory planning will immediately end for the current trajectory planning iteration. # NOTE: It is highly desirable to maintain a timeout of 100 ms or less, but trajectory generation success cannot be guaranteed with this duration # for tactical plugins (primarily cooperative_lanechange) in all test scenarios at this time. # Units: Milliseconds -tactical_plugin_service_call_timeout: 100 \ No newline at end of file +# Configured in VehicleConfigPrams.yaml in carma-config +# tactical_plugin_service_call_timeout: 100 \ No newline at end of file From 53f8a42584b5a7ba1d42bce12b75ed7c43e1bc3d Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Tue, 5 Dec 2023 08:15:47 +0000 Subject: [PATCH 13/18] launch file --- carma/launch/plugins.launch.py | 88 +++++++++++++++++----------------- 1 file changed, 45 insertions(+), 43 deletions(-) diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index 2dbf8bf905..391c43672d 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -42,7 +42,7 @@ def generate_launch_description(): strategic_plugins_to_validate = LaunchConfiguration('strategic_plugins_to_validate') tactical_plugins_to_validate = LaunchConfiguration('tactical_plugins_to_validate') control_plugins_to_validate = LaunchConfiguration('control_plugins_to_validate') - + vehicle_config_param_file = LaunchConfiguration('vehicle_config_param_file') inlanecruising_plugin_file_path = os.path.join( @@ -52,37 +52,37 @@ def generate_launch_description(): get_package_share_directory('route_following_plugin'), 'config/parameters.yaml') stop_and_wait_plugin_param_file = os.path.join( - get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') + get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') light_controlled_intersection_tactical_plugin_param_file = os.path.join( - get_package_share_directory('light_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') + get_package_share_directory('light_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') cooperative_lanechange_param_file = os.path.join( - get_package_share_directory('cooperative_lanechange'), 'config/parameters.yaml') + get_package_share_directory('cooperative_lanechange'), 'config/parameters.yaml') platoon_strategic_ihp_param_file = os.path.join( - get_package_share_directory('platoon_strategic_ihp'), 'config/parameters.yaml') + get_package_share_directory('platoon_strategic_ihp'), 'config/parameters.yaml') sci_strategic_plugin_file_path = os.path.join( - get_package_share_directory('sci_strategic_plugin'), 'config/parameters.yaml') + get_package_share_directory('sci_strategic_plugin'), 'config/parameters.yaml') lci_strategic_plugin_file_path = os.path.join( - get_package_share_directory('lci_strategic_plugin'), 'config/parameters.yaml') + get_package_share_directory('lci_strategic_plugin'), 'config/parameters.yaml') stop_and_dwell_strategic_plugin_container_file_path = os.path.join( - get_package_share_directory('stop_and_dwell_strategic_plugin'), 'config/parameters.yaml') - + get_package_share_directory('stop_and_dwell_strategic_plugin'), 'config/parameters.yaml') + yield_plugin_file_path = os.path.join( - get_package_share_directory('yield_plugin'), 'config/parameters.yaml') + get_package_share_directory('yield_plugin'), 'config/parameters.yaml') platoon_tactical_ihp_param_file = os.path.join( - get_package_share_directory('platooning_tactical_plugin'), 'config/parameters.yaml') + get_package_share_directory('platooning_tactical_plugin'), 'config/parameters.yaml') approaching_emergency_vehicle_plugin_param_file = os.path.join( get_package_share_directory('approaching_emergency_vehicle_plugin'), 'config/parameters.yaml') - + stop_controlled_intersection_tactical_plugin_file_path = os.path.join( - get_package_share_directory('stop_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') + get_package_share_directory('stop_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') @@ -101,7 +101,7 @@ def generate_launch_description(): plugin='inlanecruising_plugin::InLaneCruisingPluginNode', name='inlanecruising_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('inlanecruising_plugin', env_log_levels) } ], remappings = [ @@ -132,7 +132,7 @@ def generate_launch_description(): plugin='route_following_plugin::RouteFollowingPlugin', name='route_following_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('route_following_plugin', env_log_levels) } ], remappings = [ @@ -165,7 +165,7 @@ def generate_launch_description(): plugin='approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin', name='approaching_emergency_vehicle_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('approaching_emergency_vehicle_plugin', env_log_levels) } ], remappings = [ @@ -206,7 +206,7 @@ def generate_launch_description(): plugin='stop_and_wait_plugin::StopandWaitNode', name='stop_and_wait_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('stop_and_wait_plugin', env_log_levels) } ], remappings = [ @@ -236,7 +236,7 @@ def generate_launch_description(): plugin='sci_strategic_plugin::SCIStrategicPlugin', name='sci_strategic_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('sci_strategic_plugin', env_log_levels) } ], remappings = [ @@ -271,7 +271,7 @@ def generate_launch_description(): plugin='lci_strategic_plugin::LCIStrategicPlugin', name='lci_strategic_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('lci_strategic_plugin', env_log_levels) } ], remappings = [ @@ -306,7 +306,7 @@ def generate_launch_description(): plugin='stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin', name='stop_controlled_intersection_tactical_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('stop_controlled_intersection_tactical_plugin', env_log_levels) } ], remappings = [ @@ -336,7 +336,7 @@ def generate_launch_description(): plugin='cooperative_lanechange::CooperativeLaneChangePlugin', name='cooperative_lanechange', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('cooperative_lanechange', env_log_levels) } ], remappings = [ @@ -374,7 +374,7 @@ def generate_launch_description(): plugin='yield_plugin::YieldPluginNode', name='yield_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('yield_plugin', env_log_levels) } ], remappings = [ @@ -409,7 +409,7 @@ def generate_launch_description(): plugin='light_controlled_intersection_tactical_plugin::LightControlledIntersectionTransitPluginNode', name='light_controlled_intersection_tactical_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('light_controlled_intersection_tactical_plugin', env_log_levels) } ], remappings = [ @@ -423,7 +423,7 @@ def generate_launch_description(): parameters=[ vehicle_config_param_file, light_controlled_intersection_tactical_plugin_param_file - ] + ] ), ] ) @@ -439,7 +439,7 @@ def generate_launch_description(): plugin='pure_pursuit_wrapper::PurePursuitWrapperNode', name='pure_pursuit_wrapper', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('pure_pursuit_wrapper', env_log_levels) } ], remappings = [ @@ -456,7 +456,7 @@ def generate_launch_description(): ), ] ) - + platooning_strategic_plugin_container = ComposableNodeContainer( package='carma_ros2_utils', name='platooning_strategic_plugin_container', @@ -490,14 +490,14 @@ def generate_launch_description(): ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ), ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), ], - parameters=[ + parameters=[ platoon_strategic_ihp_param_file, vehicle_config_param_file ] ), ] ) - + platooning_tactical_plugin_container = ComposableNodeContainer( package='carma_ros2_utils', name='platooning_tactical_plugin_container', @@ -537,7 +537,7 @@ def generate_launch_description(): plugin='stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin', name='stop_and_dwell_strategic_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('stop_and_dwell_strategic_plugin', env_log_levels) } ], remappings = [ @@ -570,29 +570,31 @@ def generate_launch_description(): plugin='intersection_transit_maneuvering::IntersectionTransitManeuveringNode', name='intersection_transit_maneuvering', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('intersection_transit_maneuvering', env_log_levels) } ], remappings = [], - parameters=[] + parameters=[ + vehicle_config_param_file + ] ), ] ) - return LaunchDescription([ - carma_inlanecruising_plugin_container, - carma_route_following_plugin_container, + return LaunchDescription([ + carma_inlanecruising_plugin_container, + carma_route_following_plugin_container, carma_approaching_emergency_vehicle_plugin_container, - carma_stop_and_wait_plugin_container, - carma_sci_strategic_plugin_container, + carma_stop_and_wait_plugin_container, + carma_sci_strategic_plugin_container, carma_stop_and_dwell_strategic_plugin_container, - carma_lci_strategic_plugin_container, - carma_stop_controlled_intersection_tactical_plugin_container, - carma_cooperative_lanechange_plugins_container, - carma_yield_plugin_container, - carma_light_controlled_intersection_plugins_container, - carma_pure_pursuit_wrapper_container, - #platooning_strategic_plugin_container, + carma_lci_strategic_plugin_container, + carma_stop_controlled_intersection_tactical_plugin_container, + carma_cooperative_lanechange_plugins_container, + carma_yield_plugin_container, + carma_light_controlled_intersection_plugins_container, + carma_pure_pursuit_wrapper_container, + #platooning_strategic_plugin_container, platooning_tactical_plugin_container, intersection_transit_maneuvering_container From b4b3c0c4b2d6e69bfcb6ad8ba7d1453cb109b2b6 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Tue, 5 Dec 2023 08:16:09 +0000 Subject: [PATCH 14/18] yield to basic_autonomy and add to other plugins --- .../include/basic_autonomy/basic_autonomy.hpp | 132 +++++++----- basic_autonomy/src/basic_autonomy.cpp | 204 ++++++++++++------ .../test/test_waypoint_generation.cpp | 94 ++++++-- inlanecruising_plugin/config/parameters.yaml | 4 +- .../inlanecruising_config.hpp | 4 +- .../inlanecruising_plugin.hpp | 31 +-- .../inlanecruising_plugin_node.hpp | 12 +- .../src/inlanecruising_plugin.cpp | 67 +----- .../src/inlanecruising_plugin_node.cpp | 31 +-- .../test/test_inlanecruising_plugin.cpp | 67 +----- ...ontrolled_intersection_tactical_plugin.hpp | 37 ++-- ...ed_intersection_tactical_plugin_config.hpp | 6 +- ...lled_intersection_tactical_plugin_node.hpp | 19 +- ...ontrolled_intersection_tactical_plugin.cpp | 106 +++++---- ...lled_intersection_tactical_plugin_node.cpp | 23 +- .../test/node_test.cpp | 60 +++--- .../include/stop_and_wait_config.hpp | 5 + .../include/stop_and_wait_node.hpp | 15 +- .../include/stop_and_wait_plugin.hpp | 21 +- .../src/stop_and_wait_plugin.cpp | 43 ++-- .../src/stop_and_wait_plugin_node.cpp | 19 +- 21 files changed, 551 insertions(+), 449 deletions(-) diff --git a/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp b/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp index cb2308272d..989063e3d9 100644 --- a/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp +++ b/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp @@ -89,34 +89,34 @@ namespace basic_autonomy double buffer_ending_downtrack = 20.0; //The additional downtrack beyond requested end dist used to fit points along spline std::string desired_controller_plugin = "default"; //The desired controller plugin for the generated trajectory }; - + /** * \brief Applies the provided speed limits to the provided speeds such that each element is capped at its corresponding speed limit if needed - * + * * \param speeds The speeds to limit * \param speed_limits The speed limits to apply. Must have the same size as speeds - * + * * \return The capped speed limits. Has the same size as speeds */ std::vector apply_speed_limits(const std::vector speeds, const std::vector speed_limits); /** * \brief Returns a 2D coordinate frame which is located at p1 and oriented so p2 lies on the +X axis - * + * * \param p1 The origin point for the frame in the parent frame * \param p2 A point in the parent frame that will define the +X axis relative to p1 - * + * * \return A 2D coordinate frame transform */ Eigen::Isometry2d compute_heading_frame(const lanelet::BasicPoint2d &p1, const lanelet::BasicPoint2d &p2); /** * \brief Reduces the input points to only those points that fit within the provided time boundary - * + * * \param points The input point speed pairs to reduce * \param time_span The time span in seconds which the output points will fit within - * + * * \return The subset of points that fit within time_span */ std::vector constrain_to_time_boundary(const std::vector &points, double time_span); @@ -125,33 +125,33 @@ namespace basic_autonomy * \brief Returns the min, and its idx, from the vector of values, excluding given set of values * \param values vector of values * \param excluded set of excluded values - * + * * \return minimum value and its idx */ std::pair min_with_exclusions(const std::vector &values, const std::unordered_set &excluded); /** * \brief Applies the longitudinal acceleration limit to each point's speed - * + * * \param downtracks downtrack distances corresponding to each speed * \param curv_speeds vehicle velocity in m/s. * \param accel_limit vehicle longitudinal acceleration in m/s^2. - * + * * \return optimized speeds for each dowtrack points that satisfies longitudinal acceleration */ std::vector optimize_speed(const std::vector &downtracks, const std::vector &curv_speeds, double accel_limit); /** * \brief Method combines input points, times, orientations, and an absolute start time to form a valid carma platform trajectory - * + * * NOTE: All input vectors must be the same size. The output vector will take this size. - * + * * \param points The points in the map frame that the trajectory will follow. Units m * \param times The times which at the vehicle should arrive at the specified points. First point should have a value of 0. Units s * \param yaws The orientation the vehicle should achieve at each point. Units radians * \param startTime The absolute start time which will be used to update the input relative times. Units s * \param desired_controller_plugin The name of the controller plugin for the generated trajectory. - * + * * \return A list of trajectory points built from the provided inputs. */ std::vector trajectory_from_points_times_orientations( @@ -160,12 +160,12 @@ namespace basic_autonomy /** * \brief Attaches back_distance length of points behind the future points - * + * * \param points_set all point speed pairs * \param future_points future points before which to attach the points - * \param nearest_pt_index idx of the first future_point in points_set + * \param nearest_pt_index idx of the first future_point in points_set * \param back_distance the back distance to be added, in meters - * + * * \return point speed pairs with back distance length of points in front of future points * NOTE- used to add past points to future trajectory for smooth spline calculation */ @@ -175,7 +175,7 @@ namespace basic_autonomy /** * \brief Computes a spline based on the provided points * \param basic_points The points to use for fitting the spline - * + * * \return A spline which has been fit to the provided points */ std::unique_ptr compute_fit(const std::vector &basic_points); @@ -184,7 +184,7 @@ namespace basic_autonomy * \brief Given the curvature fit, computes the curvature at the given step along the curve * \param step_along_the_curve Value in double from 0.0 (curvature start) to 1.0 (curvature end) representing where to calculate the curvature * \param fit_curve curvature fit - * + * * \return Curvature (k = 1/r, 1/meter) */ double compute_curvature_at(const basic_autonomy::smoothing::SplineI &fit_curve, double step_along_the_curve); @@ -193,7 +193,7 @@ namespace basic_autonomy * \brief Creates geometry profile to return a point speed pair struct for LANE FOLLOW and LANE CHANGE maneuver types * \param maneuvers The list of maneuvers to convert to geometry points and calculate associated speed * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier. - * If the first maneuver exceeds this then it's downtrack will be shifted to this value. + * If the first maneuver exceeds this then it's downtrack will be shifted to this value. * \param wm Pointer to intialized world model for semantic map access * \param ending_state_before_buffer reference to Vehicle state, which is state before applying extra points for curvature calculation that are removed later * \param state The vehicle state at the time the function is called @@ -206,20 +206,20 @@ namespace basic_autonomy const carma_planning_msgs::msg::VehicleState& state,const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config); /** - * \brief Converts a set of requested LANE_FOLLOWING maneuvers to point speed limit pairs. + * \brief Converts a set of requested LANE_FOLLOWING maneuvers to point speed limit pairs. * \param maneuvers The list of maneuvers to convert geometry points and calculate associated speed * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier. * If the first maneuver exceeds this then it's downtrack will be shifted to this value. * \param wm Pointer to intialized world model for semantic map access * \param general_config Basic autonomy struct defined to load general config parameters from tactical plugins * \param detailed_config Basic autonomy struct defined to load detailed config parameters from tactical plugins - * + * * \return List of centerline points paired with speed limits */ std::vector create_lanefollow_geometry(const carma_planning_msgs::msg::Maneuver &maneuver, double max_starting_downtrack, - const carma_wm::WorldModelConstPtr &wm, const GeneralTrajConfig &general_config, + const carma_wm::WorldModelConstPtr &wm, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config, std::unordered_set& visited_lanelets); - + /** * \brief Adds extra centerline points beyond required message length to lane follow maneuver points so that there's always enough points to calculate trajectory * (BUFFER POINTS SHOULD BE REMOVED BEFORE RETURNING FINAL TRAJECTORY) @@ -228,50 +228,50 @@ namespace basic_autonomy * \param maneuvers The list of lane follow maneuvers which were converted to geometry points and associated speed * \param ending_state_before_buffer reference to Vehicle state, which is state before applying extra points for curvature calculation that are removed later * \param detailed_config Basic autonomy struct defined to load detailed config parameters from tactical plugins - * - * \return List of centerline points paired with speed limits returned with added buffer - */ + * + * \return List of centerline points paired with speed limits returned with added buffer + */ std::vector add_lanefollow_buffer(const carma_wm::WorldModelConstPtr &wm, std::vector& points_and_target_speeds, const std::vector &maneuvers, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const DetailedTrajConfig &detailed_config); /** * \brief Method converts a list of lanelet centerline points and current vehicle state into a usable list of trajectory points for trajectory planning for a Lane following maneuver. - * \param points The set of points that define the current lane the vehicle is in and are defined based on the request planning maneuvers. - * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. + * \param points The set of points that define the current lane the vehicle is in and are defined based on the request planning maneuvers. + * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. * \param state The current state of the vehicle * \param state_time The abosolute time which the provided vehicle state corresponds to - * + * * \return A list of trajectory points to send to the carma planning stack */ std::vector compose_lanefollow_trajectory_from_path(const std::vector &points, const carma_planning_msgs::msg::VehicleState &state, - const rclcpp::Time &state_time, const carma_wm::WorldModelConstPtr &wm, + const rclcpp::Time &state_time, const carma_wm::WorldModelConstPtr &wm, const carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds& debug_msg, const DetailedTrajConfig &detailed_config); //Functions specific to lane change /** - * \brief Converts a set of requested LANE_CHANGE maneuvers to point speed limit pairs. - * + * \brief Converts a set of requested LANE_CHANGE maneuvers to point speed limit pairs. + * * \param maneuvers The list of maneuvers to convert * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier. * If the first maneuver exceeds this then it's downtrack will be shifted to this value. - * + * * \param wm Pointer to intialized world model for semantic map access * \param ending_state_before_buffer reference to Vehicle state, which is state before applying extra points for curvature calculation that are removed later * \param state The vehicle state at the time the function is called * \param general_config Basic autonomy struct defined to load general config parameters from tactical plugins * \param detailed_config Basic autonomy struct defined to load detailed config parameters from tactical plugins - * + * * \return A vector of point speed pair struct which contains geometry points as basicpoint::lanelet2d and speed as a double for the maneuver */ std::vector get_lanechange_points_from_maneuver(const carma_planning_msgs::msg::Maneuver &maneuver, double max_starting_downtrack, const carma_wm::WorldModelConstPtr &wm, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const carma_planning_msgs::msg::VehicleState &state, const GeneralTrajConfig &general_config,const DetailedTrajConfig &detailed_config); - + /** - * \brief Creates a vector of lane change points using parameters defined. - * + * \brief Creates a vector of lane change points using parameters defined. + * * \param starting_lane_id lanelet id for where lane change plan should start * \param ending_lane_id lanelet id for where lane change plan should end * \param starting_downtrack The downtrack distance from which the lane change maneuver starts @@ -279,19 +279,19 @@ namespace basic_autonomy * \param wm Pointer to intialized world model for semantic map access * \param downsample_ratio TODO: add description * \param buffer_ending_downtrack The additional downtrack beyond requested end dist used to fit points along spline - * + * * \return A vector of geometry points as lanelet::basicpoint2d */ std::vector create_lanechange_geometry(lanelet::Id starting_lane_id, lanelet::Id ending_lane_id, double starting_downtrack, double ending_downtrack, const carma_wm::WorldModelConstPtr &wm, int downsample_ratio, double buffer_ending_downtrack); - - + + /** - * \brief Resamples a pair of basicpoint2d lines to get lines of same number of points. - * + * \brief Resamples a pair of basicpoint2d lines to get lines of same number of points. + * * \param line_1 a vector of points to be resampled * \param line_2 a vector of points to be resampled - * + * * \return A 2d vector with input lines resampled at same rate. The first iteration is the resampled line_1 and the resampled line_2 is the second iteration * Assumption here is for lane change to happen between two adjacent lanelets, they must share a lane boundary (linestring) */ @@ -299,14 +299,14 @@ namespace basic_autonomy /** * \brief Method converts a list of lanelet centerline points and current vehicle state into a usable list of trajectory points for trajectory planning for a Lane following maneuver. - * - * \param points The set of points that define the current lane the vehicle is in and are defined based on the request planning maneuvers. - * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. + * + * \param points The set of points that define the current lane the vehicle is in and are defined based on the request planning maneuvers. + * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. * \param state The current state of the vehicle * \param state_time The abosolute time which the provided vehicle state corresponds to * \param wm The carma world model object which the vehicle is operating in. * \param ending_state_before_buffer The vehicle state before a buffer was added to the points. Used to revert the trajectory to required distance before returning. - * + * * \return A list of trajectory points to send to the carma planning stack */ std::vector compose_lanechange_trajectory_from_path( @@ -315,11 +315,11 @@ namespace basic_autonomy const DetailedTrajConfig &detailed_config); /** - * \brief Creates a Lanelet2 Linestring from a vector or points along the geometry + * \brief Creates a Lanelet2 Linestring from a vector or points along the geometry * \param starting_downtrack downtrack along route where maneuver starts * \param ending_downtrack downtrack along route where maneuver starts * \param wm Pointer to intialized world model for semantic map access - * + * * \return Points in a path from starting downtrack to ending downtrack */ std::vector create_route_geom(double starting_downtrack, int starting_lane_id, @@ -329,7 +329,7 @@ namespace basic_autonomy * \brief Given a start and end point, create a vector of points fit through a spline between the points (using a Spline library) * \param start_lanelet The lanelet from which lane change starts * \param end_lanelet The lanelet in which lane change ends - * + * * \return A linestring path from start to end fit through Spline Library */ lanelet::BasicLineString2d create_lanechange_path(const lanelet::ConstLanelet &start_lanelet, const lanelet::ConstLanelet &end_lanelet); @@ -348,12 +348,12 @@ namespace basic_autonomy GeneralTrajConfig compose_general_trajectory_config(const std::string& trajectory_type, int default_downsample_ratio, int turn_downsample_ratio); - + /** * \brief Given a carma type of trajectory_plan, generate autoware type of trajectory accounting for speed_lag and stopping case * Generated trajectory is meant to be used in autoware.auto's pure_pursuit library using set_trajectory() function * \param tp trajectory plan from tactical plugins - * + * * \return trajectory plan of autoware_auto_msgs type */ autoware_auto_msgs::msg::Trajectory process_trajectory_plan(const carma_planning_msgs::msg::TrajectoryPlan& tp, double vehicle_response_lag); @@ -363,10 +363,34 @@ namespace basic_autonomy * \param speeds Velocity profile to shift. The first point should be the current vehicle speed * \param downtrack Distance points for each velocity point. Should have the same size as speeds and start from 0 * \param response_lag The lag in seconds before which the vehicle will not meaningfully accelerate - * + * * \return A Shifted trajectory - */ + */ std::vector apply_response_lag(const std::vector& speeds, const std::vector downtracks, double response_lag); + + /** + * \brief Applies a yield trajectory to the original trajectory set in response + * \param node_handler a node interface to use the logger and time + * \param req The service request containing the maneuvers to plan trajectories for and current vehicle state + * \param resp The original response containing the planned trajectory to be modified + * \param yield_client yield_client to call for + * \param yield_plugin_service_call_timeout yield client's timeout in milliseconds int + * + * \return The original response modified to contain the modified planned trajectory + */ + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr modify_trajectory_to_yield_to_obstacles( + std::shared_ptr node_handler, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp, + carma_ros2_utils::ClientPtr yield_client, int yield_plugin_service_call_timeout); + + /** + * \brief Helper function to verify if the input yield trajectory plan is valid + * \param node_handler node of which time to compare against the trajectory's time + * \param yield_plan input yield trajectory plan + * + * \return true or false + */ + bool validate_yield_plan(std::shared_ptr node_handler, const carma_planning_msgs::msg::TrajectoryPlan& yield_plan); } } // basic_autonomy \ No newline at end of file diff --git a/basic_autonomy/src/basic_autonomy.cpp b/basic_autonomy/src/basic_autonomy.cpp index 309b2ffaf8..7ebe56c6ba 100644 --- a/basic_autonomy/src/basic_autonomy.cpp +++ b/basic_autonomy/src/basic_autonomy.cpp @@ -25,7 +25,7 @@ namespace basic_autonomy carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,const carma_planning_msgs::msg::VehicleState& state, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config){ std::vector points_and_target_speeds; - + bool first = true; std::unordered_set visited_lanelets; @@ -33,7 +33,7 @@ namespace basic_autonomy for(const auto &maneuver : maneuvers) { double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist); - + if(first){ starting_downtrack = std::min(starting_downtrack, max_starting_downtrack); first = false; @@ -53,10 +53,10 @@ namespace basic_autonomy else{ throw std::invalid_argument("This maneuver type is not supported"); } - + } - //Add buffer ending to lane follow points at the end of maneuver(s) end dist + //Add buffer ending to lane follow points at the end of maneuver(s) end dist if(maneuvers.back().type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING){ points_and_target_speeds = add_lanefollow_buffer(wm, points_and_target_speeds, maneuvers, ending_state_before_buffer, detailed_config); @@ -66,7 +66,7 @@ namespace basic_autonomy } std::vector create_lanefollow_geometry(const carma_planning_msgs::msg::Maneuver &maneuver, double starting_downtrack, - const carma_wm::WorldModelConstPtr &wm, const GeneralTrajConfig &general_config, + const carma_wm::WorldModelConstPtr &wm, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config, std::unordered_set &visited_lanelets) { if(maneuver.type != carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING){ @@ -110,7 +110,7 @@ namespace basic_autonomy // Add extra lanelet to ensure there are sufficient points for buffer auto extra_following_lanelets = wm->getMapRoutingGraph()->following(lanelets.back()); - + for (auto llt : wm->getRoute()->shortestPath()) { for (size_t i = 0; i < extra_following_lanelets.size(); i++) @@ -122,7 +122,7 @@ namespace basic_autonomy } } } - + if (lanelets.empty()) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Detected no lanelets between starting downtrack: "<< starting_downtrack << ", and lane_following_maneuver.end_dist: "<< lane_following_maneuver.end_dist); @@ -132,10 +132,10 @@ namespace basic_autonomy RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Maneuver"); lanelet::BasicLineString2d downsampled_centerline; - // 400 value here is an arbitrary attempt at improving inlane-cruising performance by reducing copy operations. + // 400 value here is an arbitrary attempt at improving inlane-cruising performance by reducing copy operations. // Value picked based on annecdotal evidence from STOL system testing downsampled_centerline.reserve(400); - + //getLaneletsBetween is inclusive of lanelets between its two boundaries //which may return lanechange lanelets, so //exclude lanechanges and plan for only the straight part @@ -151,7 +151,7 @@ namespace basic_autonomy else { // skip all lanechanges until lane follow starts - while (curr_idx + 1 < lanelets.size() && + while (curr_idx + 1 < lanelets.size() && std::find(following_lanelets.begin(),following_lanelets.end(), lanelets[curr_idx + 1]) == following_lanelets.end()) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "As there were no directly following lanelets after this, skipping lanelet id: " << lanelets[curr_idx].id()); @@ -163,7 +163,7 @@ namespace basic_autonomy // guaranteed to have at least one "straight" lanelet (e.g the last one in the list) straight_lanelets.push_back(lanelets[curr_idx]); // add all lanelets on the straight road until next lanechange - while (curr_idx + 1 < lanelets.size() && + while (curr_idx + 1 < lanelets.size() && std::find(following_lanelets.begin(),following_lanelets.end(), lanelets[curr_idx + 1]) != following_lanelets.end()) { curr_idx++; @@ -171,9 +171,9 @@ namespace basic_autonomy straight_lanelets.push_back(lanelets[curr_idx]); following_lanelets = wm->getMapRoutingGraph()->following(lanelets[curr_idx]); } - + } - + for (auto l : straight_lanelets) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Processing lanelet ID: " << l.id()); @@ -185,7 +185,7 @@ namespace basic_autonomy std::string turn_direction = l.attribute("turn_direction").value(); is_turn = turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0; } - + lanelet::BasicLineString2d centerline = l.centerline2d().basicLineString(); lanelet::BasicLineString2d downsampled_points; if (is_turn) { @@ -193,7 +193,7 @@ namespace basic_autonomy } else { downsampled_points = carma_ros2_utils::containers::downsample_vector(centerline, general_config.default_downsample_ratio); } - + if(downsampled_centerline.size() != 0 && downsampled_points.size() != 0 // If this is not the first lanelet and the points are closer than 1m drop the first point to prevent overlap && lanelet::geometry::distance2d(downsampled_points.front(), downsampled_centerline.back()) <1.2){ downsampled_points = lanelet::BasicLineString2d(downsampled_points.begin() + 1, downsampled_points.end()); @@ -217,19 +217,19 @@ namespace basic_autonomy pair.speed = lane_following_maneuver.end_speed; points_and_target_speeds.push_back(pair); } - + return points_and_target_speeds; } std::vector add_lanefollow_buffer(const carma_wm::WorldModelConstPtr &wm, std::vector& points_and_target_speeds, const std::vector &maneuvers, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const DetailedTrajConfig &detailed_config){ - + double starting_route_downtrack = wm->routeTrackPos(points_and_target_speeds.front().point).downtrack; // Always try to add the maximum buffer. Even if the route ends it may still be possible to add buffered points. - // This does mean that downstream components might not be able to assume the buffer points are on the route + // This does mean that downstream components might not be able to assume the buffer points are on the route // though this is not likely to be an issue as they are buffer only double ending_downtrack = maneuvers.back().lane_following_maneuver.end_dist + detailed_config.buffer_ending_downtrack; @@ -245,7 +245,7 @@ namespace basic_autonomy boost::optional delta_point; for (size_t i = 0; i < points_and_target_speeds.size(); ++i) { auto current_point = points_and_target_speeds[i].point; - + if (i == 0) { prev_point = current_point; continue; @@ -254,7 +254,7 @@ namespace basic_autonomy double delta_d = lanelet::geometry::distance2d(prev_point, current_point); dist_accumulator += delta_d; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Index i: " << i << ", delta_d: " << delta_d << ", dist_accumulator:" << dist_accumulator <<", current_point.x():" << current_point.x() << + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Index i: " << i << ", delta_d: " << delta_d << ", dist_accumulator:" << dist_accumulator <<", current_point.x():" << current_point.x() << "current_point.y():" << current_point.y()); if (dist_accumulator > maneuvers.back().lane_following_maneuver.end_dist && !found_unbuffered_idx) { @@ -262,7 +262,7 @@ namespace basic_autonomy RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Found index unbuffered_idx at: " << unbuffered_idx); found_unbuffered_idx = true; } - + if (dist_accumulator > ending_downtrack) { max_i = i; RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Max_i breaking at: i: " << i << ", max_i: " << max_i); @@ -281,7 +281,7 @@ namespace basic_autonomy delta_point = (current_point - prev_point) * 0.25; // Use a smaller step size then default to help ensure enough points are generated; } - // Create an extrapolated new point + // Create an extrapolated new point auto new_point = current_point + delta_point.get(); PointSpeedPair new_pair; @@ -296,7 +296,7 @@ namespace basic_autonomy ending_state_before_buffer.x_pos_global = points_and_target_speeds[unbuffered_idx].point.x(); ending_state_before_buffer.y_pos_global = points_and_target_speeds[unbuffered_idx].point.y(); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Here ending_state_before_buffer.x_pos_global: " << ending_state_before_buffer.x_pos_global << + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Here ending_state_before_buffer.x_pos_global: " << ending_state_before_buffer.x_pos_global << ", and y_pos_global" << ending_state_before_buffer.y_pos_global); std::vector constrained_points(points_and_target_speeds.begin(), points_and_target_speeds.begin() + max_i); @@ -312,17 +312,17 @@ namespace basic_autonomy //Get starting lanelet and ending lanelets lanelet::ConstLanelet starting_lanelet = wm->getMap()->laneletLayer.get(starting_lane_id); lanelet::ConstLanelet ending_lanelet = wm->getMap()->laneletLayer.get(ending_lane_id); - + lanelet::ConstLanelets starting_lane; starting_lane.push_back(starting_lanelet); std::vector reference_centerline; - // 400 value here is an arbitrary attempt at improving performance by reducing copy operations. + // 400 value here is an arbitrary attempt at improving performance by reducing copy operations. // Value picked based on annecdotal evidence from STOL system testing reference_centerline.reserve(400); bool shared_boundary_found = false; bool is_lanechange_left = false; - + lanelet::BasicLineString2d current_lanelet_centerline = starting_lanelet.centerline2d().basicLineString(); lanelet::ConstLanelet current_lanelet = starting_lanelet; reference_centerline.insert(reference_centerline.end(), current_lanelet_centerline.begin(), current_lanelet_centerline.end()); @@ -330,7 +330,7 @@ namespace basic_autonomy RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Searching for shared boundary with starting lanechange lanelet " << std::to_string(current_lanelet.id()) << " and ending lanelet " << std::to_string(ending_lanelet.id())); while(!shared_boundary_found){ //Assumption- Adjacent lanelets share lane boundary - if(current_lanelet.leftBound() == ending_lanelet.rightBound()){ + if(current_lanelet.leftBound() == ending_lanelet.rightBound()){ RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Lanelet " << std::to_string(current_lanelet.id()) << " shares left boundary with " << std::to_string(ending_lanelet.id())); is_lanechange_left = true; shared_boundary_found = true; @@ -351,14 +351,14 @@ namespace basic_autonomy throw(std::invalid_argument("No following lanelets from current lanelet reachable without a lane change, incorrectly chosen end lanelet")); } - current_lanelet = wm->getMapRoutingGraph()->following(current_lanelet, false).front(); + current_lanelet = wm->getMapRoutingGraph()->following(current_lanelet, false).front(); if(current_lanelet.id() == starting_lanelet.id()){ //Looped back to starting lanelet throw(std::invalid_argument("No lane change in path")); } RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Now checking for shared lane boundary with lanelet " << std::to_string(current_lanelet.id()) << " and ending lanelet " << std::to_string(ending_lanelet.id())); - auto current_lanelet_linestring = current_lanelet.centerline2d().basicLineString(); - //Concatenate linestring starting from + 1 to avoid overlap + auto current_lanelet_linestring = current_lanelet.centerline2d().basicLineString(); + //Concatenate linestring starting from + 1 to avoid overlap reference_centerline.insert(reference_centerline.end(), current_lanelet_linestring.begin() + 1, current_lanelet_linestring.end()); starting_lane.push_back(current_lanelet); } @@ -368,9 +368,9 @@ namespace basic_autonomy std::vector target_lane_centerline; for(size_t i = 0;igetMapRoutingGraph()->left(starting_lane[i])){ curr_end_lanelet = wm->getMapRoutingGraph()->left(starting_lane[i]).get(); @@ -389,15 +389,15 @@ namespace basic_autonomy curr_end_lanelet = wm->getMapRoutingGraph()->adjacentRight(starting_lane[i]).get(); } } - + auto target_lane_linestring = curr_end_lanelet.centerline2d().basicLineString(); //Concatenate linestring starting from + 1 to avoid overlap target_lane_centerline.insert(target_lane_centerline.end(), target_lane_linestring.begin() + 1, target_lane_linestring.end()); - + } //Downsample centerlines - // 400 value here is an arbitrary attempt at improving performance by reducing copy operations. + // 400 value here is an arbitrary attempt at improving performance by reducing copy operations. // Value picked based on annecdotal evidence from STOL system testing std::vector downsampled_starting_centerline; @@ -451,7 +451,7 @@ namespace basic_autonomy // Add points from the remaining length of the target lanelet to provide sufficient distance for adding buffer double dist_to_target_lane_end = lanelet::geometry::distance2d(centerline_points.back(), downsampled_target_centerline.back()); centerline_points.insert(centerline_points.end(), downsampled_target_centerline.begin() + end_index_target_centerline, downsampled_target_centerline.end()); - + // If the additional distance from the remaining length of the target lanelet does not provide more than the required // buffer_ending_downtrack, then also add points from the lanelet following the target lanelet if (dist_to_target_lane_end < buffer_ending_downtrack) { @@ -459,7 +459,7 @@ namespace basic_autonomy if(!following_lanelets.empty()){ //Arbitrarily choosing first following lanelet for buffer since points are only being used to fit spline auto following_lanelet_centerline = following_lanelets.front().centerline2d().basicLineString(); - centerline_points.insert(centerline_points.end(), following_lanelet_centerline.begin(), + centerline_points.insert(centerline_points.end(), following_lanelet_centerline.begin(), following_lanelet_centerline.end()); } } @@ -468,11 +468,11 @@ namespace basic_autonomy } std::vector> resample_linestring_pair_to_same_size(std::vector& line_1, std::vector& line_2){ - + auto start_time = std::chrono::high_resolution_clock::now(); // Start timing the execution time for planning so it can be logged std::vector> output; - + //Fit centerlines to a spline std::unique_ptr fit_curve_1 = compute_fit(line_1); // Compute splines based on curve points if (!fit_curve_1) @@ -498,17 +498,17 @@ namespace basic_autonomy //double step_threshold_line1 = (double)total_step_along_curve1 / (double)total_point_size; //TODO: are we missing some computation here? step_threshold_line1 and step_threshold_line2 are not used anywhere // and these calcs can be deleted (see below also). - + all_sampling_points_line2.reserve(1 + total_point_size * 2); std::vector downtracks_raw_line2 = carma_wm::geometry::compute_arc_lengths(line_2); //TODO: unused variable: int total_step_along_curve2 = static_cast(downtracks_raw_line2.back() / 2.0); //TODO: unused variable: double step_threshold_line2 = (double)total_step_along_curve2 / (double)total_point_size; double scaled_steps_along_curve = 0.0; // from 0 (start) to 1 (end) for the whole trajectory - - + + all_sampling_points_line2.reserve(1 + total_point_size * 2); - + for(size_t i = 0;i(end_time - start_time); RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "ExecutionTime for resample lane change centerlines: " << duration.count() << " milliseconds"); return output; } - + std::vector get_lanechange_points_from_maneuver(const carma_planning_msgs::msg::Maneuver &maneuver, double starting_downtrack, const carma_wm::WorldModelConstPtr &wm, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const carma_planning_msgs::msg::VehicleState &state, const GeneralTrajConfig &general_config,const DetailedTrajConfig &detailed_config) @@ -562,10 +562,10 @@ namespace basic_autonomy ending_state_before_buffer.x_pos_global = route_geometry[ending_pt_index].x(); ending_state_before_buffer.y_pos_global = route_geometry[ending_pt_index].y(); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "ending_state_before_buffer_:"<getRouteEndTrackPos().downtrack; if (ending_downtrack + detailed_config.buffer_ending_downtrack < route_length) @@ -593,13 +593,13 @@ namespace basic_autonomy //If current speed is above min speed, keep at current speed. Otherwise use end speed from maneuver. pair.speed = (state.longitudinal_vel > detailed_config.minimum_speed) ? state.longitudinal_vel : lane_change_maneuver.end_speed; points_and_target_speeds.push_back(pair); - + } RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Const speed assigned:"< apply_speed_limits(const std::vector speeds, const std::vector speed_limits) @@ -639,7 +639,7 @@ namespace basic_autonomy size_t time_boundary_exclusive_index = trajectory_utils::time_boundary_index(downtracks, speeds, time_span); - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "time_boundary_exclusive_index = " << time_boundary_exclusive_index); if (time_boundary_exclusive_index == 0) @@ -767,7 +767,7 @@ namespace basic_autonomy tpp.x = points[i].x(); tpp.y = points[i].y(); tpp.yaw = yaws[i]; - + tpp.controller_plugin_name = desired_controller_plugin; //tpp.planner_plugin_name //Planner plugin name is filled in the tactical plugin @@ -810,13 +810,13 @@ namespace basic_autonomy RCLCPP_WARN_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Insufficient Spline Points"); return nullptr; } - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Original basic_points size: " << basic_points.size()); std::vector resized_basic_points = basic_points; // The large the number of points, longer it takes to calculate a spline fit - // So if the basic_points vector size is large, only the first 400 points are used to compute a spline fit. + // So if the basic_points vector size is large, only the first 400 points are used to compute a spline fit. if (resized_basic_points.size() > 400) { resized_basic_points.resize(400); @@ -832,7 +832,7 @@ namespace basic_autonomy RCLCPP_WARN_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "More than half of basic points are ignored for spline fitting"); } } - + std::unique_ptr spl = std::make_unique(); spl->setPoints(resized_basic_points); @@ -965,7 +965,7 @@ namespace basic_autonomy RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Corresponding to point: x: " << all_sampling_points[buffer_pt_index].x() << ", y:" << all_sampling_points[buffer_pt_index].y()); if(nearest_pt_index + 1 >= buffer_pt_index){ - + lanelet::BasicPoint2d current_pos(state.x_pos_global, state.y_pos_global); lanelet::BasicPoint2d ending_pos(ending_state_before_buffer.x_pos_global, ending_state_before_buffer.y_pos_global); @@ -1061,8 +1061,8 @@ namespace basic_autonomy msg.lat_accel_limit = detailed_config.lateral_accel_limit; msg.lon_accel_limit = detailed_config.max_accel; msg.starting_state = state; - debug_msg = msg; - + debug_msg = msg; + return traj_points; } @@ -1139,7 +1139,7 @@ namespace basic_autonomy current_vehicle_point); // Add current vehicle position to front of future geometry points final_actual_speeds.insert(final_actual_speeds.begin(), state.longitudinal_vel); - + //Compute points to local downtracks std::vector downtracks = carma_wm::geometry::compute_arc_lengths(future_geom_points); @@ -1152,7 +1152,7 @@ namespace basic_autonomy all_sampling_points.push_back(p); - scaled_steps_along_curve += 1.0 / total_step_along_curve; + scaled_steps_along_curve += 1.0 / total_step_along_curve; } RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Got sampled points with size:" << all_sampling_points.size()); @@ -1220,7 +1220,7 @@ namespace basic_autonomy if (stopping_index != 0 && i >= stopping_index - 1) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Made it to 0, i: " << i); - + speeds[i] = 0.0; //stopping case } else @@ -1231,13 +1231,13 @@ namespace basic_autonomy std::vector lag_speeds; lag_speeds = apply_response_lag(speeds, downtracks, vehicle_response_lag); // This call requires that the first speed point be current speed to work as expected - + autoware_auto_msgs::msg::Trajectory autoware_trajectory; autoware_trajectory.header = tp.header; RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "size: " << trajectory_points.size()); - + auto max_size = std::min(99, (int)trajectory_points.size()); //NOTE: more than this size autoware auto raises exception with "Exceeded upper bound while in ACTIVE state." - //large portion of the points are not needed anyways + //large portion of the points are not needed anyways for (int i = 0; i < max_size; i++) { autoware_auto_msgs::msg::TrajectoryPoint autoware_point; @@ -1247,7 +1247,7 @@ namespace basic_autonomy autoware_point.longitudinal_velocity_mps = lag_speeds[i]; autoware_point.time_from_start = rclcpp::Duration(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 << + 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"); autoware_trajectory.points.push_back(autoware_point); @@ -1256,8 +1256,8 @@ namespace basic_autonomy return autoware_trajectory; } - - std::vector apply_response_lag(const std::vector& speeds, const std::vector downtracks, double response_lag) + + std::vector apply_response_lag(const std::vector& speeds, const std::vector downtracks, double response_lag) { // Note first speed is assumed to be vehicle speed if (speeds.size() != downtracks.size()) { throw std::invalid_argument("Speed list and downtrack list are not the same size."); @@ -1275,6 +1275,76 @@ namespace basic_autonomy output = trajectory_utils::shift_by_lookahead(speeds, (unsigned int) lookahead_count); return output; } + + bool validate_yield_plan(std::shared_ptr node_handler, const carma_planning_msgs::msg::TrajectoryPlan& yield_plan) + { + if (yield_plan.trajectory_points.size()>= 2) + { + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Yield Trajectory Time" << rclcpp::Time(yield_plan.trajectory_points[0].target_time).seconds()); + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Now:" << node_handler->now().seconds()); + + if (rclcpp::Time(yield_plan.trajectory_points[0].target_time) + rclcpp::Duration(5.0, 0) > node_handler->now()) + { + return true; + } + else + { + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Old Yield Trajectory"); + } + } + else + { + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Invalid Yield Trajectory"); + } + return false; + } + + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr modify_trajectory_to_yield_to_obstacles(std::shared_ptr node_handler, carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp, + carma_ros2_utils::ClientPtr yield_client, int yield_plugin_service_call_timeout) + { + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Activate Object Avoidance"); + + if (yield_client && yield_client->service_is_ready()) + { + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Yield Client is valid"); + + auto yield_srv = std::make_shared(); + yield_srv->initial_trajectory_plan = resp->trajectory_plan; + yield_srv->vehicle_state = req->vehicle_state; + + auto yield_resp = yield_client->async_send_request(yield_srv); + + auto future_status = yield_resp.wait_for(std::chrono::milliseconds(yield_plugin_service_call_timeout)); + + if (future_status == std::future_status::ready) + { + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Received Traj from Yield"); + carma_planning_msgs::msg::TrajectoryPlan yield_plan = yield_resp.get()->trajectory_plan; + if (validate_yield_plan(node_handler, yield_plan)) + { + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Yield trajectory validated"); + resp->trajectory_plan = yield_plan; + } + else + { + throw std::invalid_argument("Invalid Yield Trajectory"); + } + } + else + { + // Sometimes the yield plugin's service call may be unsuccessful due to its computationally expensive logic. + // However, consecutive calls can be successful, so return original trajectory for now + RCLCPP_WARN_STREAM(node_handler->get_logger(), "Unable to Call Yield Plugin"); + } + } + else + { + throw std::invalid_argument("Yield Client is unavailable"); + } + + return resp; + } + } // namespace waypoint_generation } // basic_autonomy \ No newline at end of file diff --git a/basic_autonomy/test/test_waypoint_generation.cpp b/basic_autonomy/test/test_waypoint_generation.cpp index 31bf7efaed..26312d01c5 100644 --- a/basic_autonomy/test/test_waypoint_generation.cpp +++ b/basic_autonomy/test/test_waypoint_generation.cpp @@ -645,10 +645,10 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) waypoint_generation::GeneralTrajConfig general_config = waypoint_generation::compose_general_trajectory_config(trajectory_type, 1, 1); const waypoint_generation::DetailedTrajConfig config = waypoint_generation::compose_detailed_trajectory_config(0, 0, 0, 0, 0, 5, 0, 0, 20); carma_planning_msgs::msg::VehicleState ending_state; - - std::vector points = basic_autonomy::waypoint_generation::create_geometry_profile(maneuvers, + + std::vector points = basic_autonomy::waypoint_generation::create_geometry_profile(maneuvers, starting_downtrack, cwm, ending_state, state, general_config, config); - + EXPECT_EQ(points.back().speed, state.longitudinal_vel); @@ -656,9 +656,9 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) int centerline_size = shortest_path.back().centerline2d().size(); maneuver.lane_change_maneuver.end_dist = cwm->routeTrackPos(shortest_path.back().centerline2d()[centerline_size/2]).downtrack; maneuvers[0] = maneuver; - points = basic_autonomy::waypoint_generation::create_geometry_profile(maneuvers, + points = basic_autonomy::waypoint_generation::create_geometry_profile(maneuvers, starting_downtrack, cwm, ending_state, state, general_config, config); - + EXPECT_TRUE(points.size() > 4); //Test resample linestring @@ -732,8 +732,8 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) waypoint_generation::GeneralTrajConfig general_config = waypoint_generation::compose_general_trajectory_config(trajectory_type, 1, 1); const waypoint_generation::DetailedTrajConfig config = waypoint_generation::compose_detailed_trajectory_config(0, 1, 0, 0, 0, 5, 0, 0, 20); carma_planning_msgs::msg::VehicleState ending_state; - - std::vector points = basic_autonomy::waypoint_generation::create_geometry_profile(maneuvers, + + std::vector points = basic_autonomy::waypoint_generation::create_geometry_profile(maneuvers, starting_downtrack, cmw, ending_state, state, general_config, config); rclcpp::Time state_time(10 * 1e9); // Arbitrarily selected 10 seconds (converted to nanoseconds) EXPECT_EQ(points.back().speed, state.longitudinal_vel); @@ -773,9 +773,9 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) maneuver.lane_following_maneuver.end_speed = 6.7056; maneuver.lane_following_maneuver.end_time = rclcpp::Time(4.4704*1e9); // 4.4704 seconds converted to nanoseconds - - std::vector points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, + + std::vector points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets); EXPECT_EQ(visited_lanelets.size(), 3); @@ -815,12 +815,12 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) maneuver.lane_following_maneuver.end_time = rclcpp::Time(4.4704*1e9); // 4.4704 seconds converted to nanoseconds // No defined lanelet ids in the maneuver msg - EXPECT_THROW(basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, + EXPECT_THROW(basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets), std::invalid_argument); - - // Defined lanelet ids in the maneuver msg + + // Defined lanelet ids in the maneuver msg maneuver.lane_following_maneuver.lane_ids.push_back(std::to_string(1200)); - std::vector points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, + std::vector points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets); EXPECT_EQ(visited_lanelets.size(), 3); EXPECT_TRUE(visited_lanelets.find(id1) != visited_lanelets.end()); // the lanelet previously in the visited lanelet set @@ -828,11 +828,73 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) // Non-following lanelet id in the maneuver msg maneuver.lane_following_maneuver.lane_ids.push_back(std::to_string(1202)); - EXPECT_THROW(basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, - starting_downtrack, wm, general_config, + EXPECT_THROW(basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, + starting_downtrack, wm, general_config, detailed_config, visited_lanelets), std::invalid_argument); } + + TEST(BasicAutonomyTest, test_verify_yield) + { + auto node = std::make_shared(rclcpp::NodeOptions()); + + std::vector trajectory_points; + + rclcpp::Time startTime = node->now(); + + 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, 0); + 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, 0); + point_3.lane_id = "1"; + trajectory_points.push_back(point_3); + + + carma_planning_msgs::msg::TrajectoryPlan tp; + tp.trajectory_points = trajectory_points; + + bool res = basic_autonomy::waypoint_generation::validate_yield_plan(node, tp); + ASSERT_TRUE(basic_autonomy::waypoint_generation::validate_yield_plan(node, tp)); + + carma_planning_msgs::msg::TrajectoryPlan tp2; + + carma_planning_msgs::msg::TrajectoryPlanPoint point_4; + point_4.x = 5.0; + point_4.y = 0.0; + point_4.target_time = startTime + rclcpp::Duration(1, 0); + point_4.lane_id = "1"; + tp2.trajectory_points.push_back(point_4); + + ASSERT_FALSE(basic_autonomy::waypoint_generation::validate_yield_plan(node, tp2)); + + carma_planning_msgs::msg::TrajectoryPlan tp3; + + carma_planning_msgs::msg::TrajectoryPlanPoint point_5; + point_5.x = 5.0; + point_5.y = 0.0; + point_5.target_time = startTime; + point_5.lane_id = "1"; + tp3.trajectory_points.push_back(point_5); + + carma_planning_msgs::msg::TrajectoryPlanPoint point_6; + point_6.x = 10.0; + point_6.y = 0.0; + point_6.target_time = startTime + rclcpp::Duration(1, 0); + point_6.lane_id = "1"; + tp3.trajectory_points.push_back(point_6); + + ASSERT_FALSE(basic_autonomy::waypoint_generation::validate_yield_plan(node, tp2)); + + } + + } // namespace basic_autonomy // Run all the tests @@ -849,4 +911,4 @@ int main(int argc, char ** argv) rclcpp::shutdown(); return success; -} \ No newline at end of file +} \ No newline at end of file diff --git a/inlanecruising_plugin/config/parameters.yaml b/inlanecruising_plugin/config/parameters.yaml index e4183d2647..243badbef3 100644 --- a/inlanecruising_plugin/config/parameters.yaml +++ b/inlanecruising_plugin/config/parameters.yaml @@ -9,5 +9,5 @@ curvature_moving_average_window_size: 9 # Size of the window used in the moving back_distance: 20.0 # Number of meters behind the first maneuver that need to be included in points for curvature calculation max_accel_multiplier: 0.85 # Multiplier of max_accel to bring the value under max_accel lat_accel_multiplier: 0.50 # Multiplier of lat_accel to bring the value under lat_accel TODO needs to be optimized -enable_object_avoidance: true # Activates object avoidance logic -buffer_ending_downtrack: 20.0 # Additional distance beyond ending downtrack to ensure sufficient points \ No newline at end of file +buffer_ending_downtrack: 20.0 # Additional distance beyond ending downtrack to ensure sufficient points +#enable_object_avoidance: true # Activates object avoidance logic - set in VehicleConfigParams \ No newline at end of file diff --git a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_config.hpp b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_config.hpp index 078a3ea632..a61fadb661 100644 --- a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_config.hpp +++ b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_config.hpp @@ -40,7 +40,8 @@ struct InLaneCruisingPluginConfig bool enable_object_avoidance = true; // Activate object avoidance logic bool publish_debug = false; // True if debug publishing will be enabled double buffer_ending_downtrack = 20.0; - + int tactical_plugin_service_call_timeout = 100; // Tactical plugin service call request timeout in milliseconds + friend std::ostream& operator<<(std::ostream& output, const InLaneCruisingPluginConfig& c) { output << "InLaneCruisingPluginConfig { " << std::endl @@ -59,6 +60,7 @@ struct InLaneCruisingPluginConfig << "enable_object_avoidance: " << c.enable_object_avoidance << std::endl << "publish_debug: " << c.publish_debug << std::endl << "buffer_ending_downtrack: " << c.buffer_ending_downtrack << std::endl + << "tactical_plugin_service_call_timeout: " << c.tactical_plugin_service_call_timeout << std::endl << "}" << std::endl; return output; } diff --git a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.hpp b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.hpp index cd7ee62a3a..e4e88c7860 100644 --- a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.hpp +++ b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.hpp @@ -45,8 +45,8 @@ static const std::string ILC_LOGGER = "inlanecruising_plugin"; /** * \brief Class containing primary business logic for the In-Lane Cruising Plugin - * - */ + * + */ class InLaneCruisingPlugin { public: @@ -58,10 +58,10 @@ class InLaneCruisingPlugin * \param debug_publisher Callback which will publish a debug message. The callback defaults to no-op. * \param plugin_name Retrieved from the plugin node * \param version_id Retrieved from the plugin node - */ - InLaneCruisingPlugin(std::shared_ptr nh, - carma_wm::WorldModelConstPtr wm, - const InLaneCruisingPluginConfig& config, + */ + InLaneCruisingPlugin(std::shared_ptr nh, + carma_wm::WorldModelConstPtr wm, + const InLaneCruisingPluginConfig& config, const DebugPublisher& debug_publisher=[](const auto& msg){}, const std::string& plugin_name = "inlanecruising_plugin", const std::string& version_id = "v1.0"); @@ -71,28 +71,19 @@ class InLaneCruisingPlugin * \param srv_header header * \param req The service request * \param resp The service response - * - */ + * + */ void plan_trajectory_callback( - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp); /** * \brief set the yield service - * + * * \param yield_srv input yield service */ void set_yield_client(carma_ros2_utils::ClientPtr client); - /** - * \brief verify if the input yield trajectory plan is valid - * - * \param yield_plan input yield trajectory plan - * - * \return true or false - */ - bool validate_yield_plan(const carma_planning_msgs::msg::TrajectoryPlan& yield_plan) const; - carma_planning_msgs::msg::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later private: @@ -105,7 +96,7 @@ class InLaneCruisingPlugin DebugPublisher debug_publisher_; carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds debug_msg_; std::shared_ptr nh_; - + // Access members for unit test FRIEND_TEST(InLaneCruisingPluginTest, rostest1); }; diff --git a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.hpp b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.hpp index 421c4be85a..a30d68e43f 100644 --- a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.hpp +++ b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.hpp @@ -33,13 +33,13 @@ namespace inlanecruising_plugin { /** * \brief ROS node for the InLaneCruisingPlugin - */ + */ class InLaneCruisingPluginNode : public carma_guidance_plugins::TacticalPlugin { public: - + /** - * \brief Node constructor + * \brief Node constructor */ explicit InLaneCruisingPluginNode(const rclcpp::NodeOptions &); @@ -47,7 +47,7 @@ class InLaneCruisingPluginNode : public carma_guidance_plugins::TacticalPlugin // Overrides //// carma_ros2_utils::CallbackReturn on_configure_plugin(); - + bool get_availability() override; std::string get_version_id() override final; @@ -55,8 +55,8 @@ class InLaneCruisingPluginNode : public carma_guidance_plugins::TacticalPlugin rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector ¶meters); void plan_trajectory_callback( - std::shared_ptr srv_header, - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override; private: diff --git a/inlanecruising_plugin/src/inlanecruising_plugin.cpp b/inlanecruising_plugin/src/inlanecruising_plugin.cpp index 451a8b29a2..b732516d19 100644 --- a/inlanecruising_plugin/src/inlanecruising_plugin.cpp +++ b/inlanecruising_plugin/src/inlanecruising_plugin.cpp @@ -107,55 +107,16 @@ void InLaneCruisingPlugin::plan_trajectory_callback( p.planner_plugin_name = plugin_name_; } + resp->trajectory_plan = original_trajectory; + // Aside from the flag, ILC should not call yield_plugin on invalid trajectories if (config_.enable_object_avoidance && original_trajectory.trajectory_points.size() >= 2) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Activate Object Avoidance"); - - if (yield_client_ && yield_client_->service_is_ready()) - { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Yield Client is valid"); - - auto yield_srv = std::make_shared(); - yield_srv->initial_trajectory_plan = original_trajectory; - yield_srv->vehicle_state = req->vehicle_state; - - auto yield_resp = yield_client_->async_send_request(yield_srv); - - auto future_status = yield_resp.wait_for(std::chrono::milliseconds(300)); - - if (future_status == std::future_status::ready) - { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Received Traj from Yield"); - carma_planning_msgs::msg::TrajectoryPlan yield_plan = yield_resp.get()->trajectory_plan; - if (validate_yield_plan(yield_plan)) - { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Yield trajectory validated"); - resp->trajectory_plan = yield_plan; - } - else - { - throw std::invalid_argument("Invalid Yield Trajectory"); - } - } - else - { - // Sometimes the yield plugin's service call may be unsuccessful due to its computationally expensive logic. - // However, consecutive calls can be successful, so return original trajectory for now - RCLCPP_WARN_STREAM(nh_->get_logger(), "Unable to Call Yield Plugin"); - resp->trajectory_plan = original_trajectory; - } - } - else - { - throw std::invalid_argument("Yield Client is unavailable"); - } - + basic_autonomy::waypoint_generation::modify_trajectory_to_yield_to_obstacles(nh_, req, resp, yield_client_, config_.tactical_plugin_service_call_timeout); } else { RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Ignored Object Avoidance"); - resp->trajectory_plan = original_trajectory; } if (config_.publish_debug) { // Publish the debug message if in debug logging mode @@ -176,27 +137,5 @@ void InLaneCruisingPlugin::set_yield_client(carma_ros2_utils::ClientPtr= 2) - { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Yield Trajectory Time" << rclcpp::Time(yield_plan.trajectory_points[0].target_time).seconds()); - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Now:" << nh_->now().seconds()); - if (rclcpp::Time(yield_plan.trajectory_points[0].target_time) + rclcpp::Duration(5.0, 0) > nh_->now()) - { - return true; - } - else - { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Old Yield Trajectory"); - } - } - else - { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Invalid Yield Trajectory"); - } - return false; -} - } // namespace inlanecruising_plugin \ No newline at end of file diff --git a/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp b/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp index 86ef0d129c..e2a257ee92 100644 --- a/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp +++ b/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp @@ -19,11 +19,11 @@ namespace inlanecruising_plugin { namespace std_ph = std::placeholders; - + InLaneCruisingPluginNode::InLaneCruisingPluginNode(const rclcpp::NodeOptions &options) - : carma_guidance_plugins::TacticalPlugin(options), - plugin_name_(get_plugin_name_and_ns()), - version_id_("v4.0"), + : carma_guidance_plugins::TacticalPlugin(options), + plugin_name_(get_plugin_name_and_ns()), + version_id_("v4.0"), config_(InLaneCruisingPluginConfig()) { // Declare parameters @@ -41,8 +41,9 @@ namespace inlanecruising_plugin config_.max_accel = declare_parameter("vehicle_acceleration_limit", config_.max_accel); config_.lateral_accel_limit = declare_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit); config_.enable_object_avoidance = declare_parameter("enable_object_avoidance", config_.enable_object_avoidance); + config_.tactical_plugin_service_call_timeout = declare_parameter("tactical_plugin_service_call_timeout", config_.tactical_plugin_service_call_timeout); } - + carma_ros2_utils::CallbackReturn InLaneCruisingPluginNode::on_configure_plugin() { trajectory_debug_pub_ = create_publisher("debug/trajectory_planning", 1); @@ -63,29 +64,29 @@ namespace inlanecruising_plugin get_parameter("vehicle_acceleration_limit", config_.max_accel); get_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit); get_parameter("enable_object_avoidance", config_.enable_object_avoidance); - + get_parameter("tactical_plugin_service_call_timeout", config_.tactical_plugin_service_call_timeout); // Register runtime parameter update callback add_on_set_parameters_callback(std::bind(&InLaneCruisingPluginNode::parameter_update_callback, this, std_ph::_1)); RCLCPP_INFO_STREAM(rclcpp::get_logger("inlanecruising_plugin"), "InLaneCruisingPlugin Params" << config_); - + config_.lateral_accel_limit = config_.lateral_accel_limit * config_.lat_accel_multiplier; config_.max_accel = config_.max_accel * config_.max_accel_multiplier; - + // Determine if we will enable debug publishing by checking the current log level of the node auto level = rcutils_logging_get_logger_level(get_logger().get_name()); config_.publish_debug = level == RCUTILS_LOG_SEVERITY_DEBUG; RCLCPP_INFO_STREAM(rclcpp::get_logger("inlanecruising_plugin"), "InLaneCruisingPlugin Params After Accel Change" << config_); - + worker_ = std::make_shared(shared_from_this(), get_world_model(), config_, [this](const carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds& msg) { trajectory_debug_pub_->publish(msg); }, plugin_name_, version_id_); - //TODO: Update yield client to use the Plugin Manager capabilities query, in case someone else wants to add an alternate yield implementation + //TODO: Update yield client to use the Plugin Manager capabilities query, in case someone else wants to add an alternate yield implementation yield_client_ = create_client("yield_plugin/plan_trajectory"); worker_->set_yield_client(yield_client_); RCLCPP_INFO_STREAM(rclcpp::get_logger("inlanecruising_plugin"), "Yield Client Set"); @@ -110,9 +111,9 @@ namespace inlanecruising_plugin {"enable_object_avoidance", config_.enable_object_avoidance}}, parameters); auto error_int = update_params({ - {"default_downsample_ratio", config_.default_downsample_ratio}, - {"turn_downsample_ratio", config_.turn_downsample_ratio}, - {"speed_moving_average_window_size", config_.speed_moving_average_window_size}, + {"default_downsample_ratio", config_.default_downsample_ratio}, + {"turn_downsample_ratio", config_.turn_downsample_ratio}, + {"speed_moving_average_window_size", config_.speed_moving_average_window_size}, {"curvature_moving_average_window_size", config_.curvature_moving_average_window_size}}, parameters); rcl_interfaces::msg::SetParametersResult result; @@ -133,8 +134,8 @@ namespace inlanecruising_plugin } void InLaneCruisingPluginNode::plan_trajectory_callback( - std::shared_ptr srv_header, - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { worker_->plan_trajectory_callback(req, resp); diff --git a/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp b/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp index b092a82732..f29acc149f 100644 --- a/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp +++ b/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp @@ -54,69 +54,4 @@ TEST(InLaneCruisingPluginTest, validate_eigen) ASSERT_NEAR(2.0, P_in_A.translation()[0], 0.000000001); ASSERT_NEAR(1.5, P_in_A.translation()[1], 0.000000001); ASSERT_NEAR(M_PI_2, P_in_A_rot.smallestAngle(), 0.000000001); -} - -TEST(InLaneCruisingPluginTest, test_verify_yield) -{ - InLaneCruisingPluginConfig config; - config.enable_object_avoidance = true; - std::shared_ptr wm = std::make_shared(); - auto node = std::make_shared(rclcpp::NodeOptions()); - - InLaneCruisingPlugin plugin(node, wm, config, [&](auto msg) {}); - - std::vector trajectory_points; - - rclcpp::Time startTime = node->now(); - - 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, 0); - 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, 0); - point_3.lane_id = "1"; - trajectory_points.push_back(point_3); - - - carma_planning_msgs::msg::TrajectoryPlan tp; - tp.trajectory_points = trajectory_points; - - bool res = plugin.validate_yield_plan(tp); - ASSERT_TRUE(plugin.validate_yield_plan(tp)); - - carma_planning_msgs::msg::TrajectoryPlan tp2; - - carma_planning_msgs::msg::TrajectoryPlanPoint point_4; - point_4.x = 5.0; - point_4.y = 0.0; - point_4.target_time = startTime + rclcpp::Duration(1, 0); - point_4.lane_id = "1"; - tp2.trajectory_points.push_back(point_4); - - ASSERT_FALSE(plugin.validate_yield_plan(tp2)); - - carma_planning_msgs::msg::TrajectoryPlan tp3; - - carma_planning_msgs::msg::TrajectoryPlanPoint point_5; - point_5.x = 5.0; - point_5.y = 0.0; - point_5.target_time = startTime; - point_5.lane_id = "1"; - tp3.trajectory_points.push_back(point_5); - - carma_planning_msgs::msg::TrajectoryPlanPoint point_6; - point_6.x = 10.0; - point_6.y = 0.0; - point_6.target_time = startTime + rclcpp::Duration(1, 0); - point_6.lane_id = "1"; - tp3.trajectory_points.push_back(point_6); - - ASSERT_FALSE(plugin.validate_yield_plan(tp2)); - -} +} \ No newline at end of file diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp index 54c929285e..9b394b14ba 100644 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp @@ -82,20 +82,19 @@ namespace light_controlled_intersection_tactical_plugin /** * \brief Class containing primary business logic for the Light Controlled Intersection Tactical Plugin - * + * */ class LightControlledIntersectionTacticalPlugin { - private: + private: // World Model object carma_wm::WorldModelConstPtr wm_; // Config for this object Config config_; - // Logger for this object - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_; + std::shared_ptr nh_; // CARMA Streets Variables // timestamp for msg received from carma streets @@ -115,6 +114,7 @@ namespace light_controlled_intersection_tactical_plugin boost::optional last_case_; boost::optional is_last_case_successful_; carma_planning_msgs::msg::TrajectoryPlan last_trajectory_; + carma_ros2_utils::ClientPtr yield_client_; carma_planning_msgs::msg::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later @@ -133,7 +133,7 @@ namespace light_controlled_intersection_tactical_plugin std::string light_controlled_intersection_strategy_ = "Carma/signalized_intersection"; // Strategy carma-streets is sending. Could be more verbose but needs to be changed on both ends /** - * \brief Creates a speed profile according to case one or two of the light controlled intersection, where the vehicle accelerates (then cruises if needed) and decelerates into the intersection. + * \brief Creates a speed profile according to case one or two of the light controlled intersection, where the vehicle accelerates (then cruises if needed) and decelerates into the intersection. * \param wm world_model pointer * \param points_and_target_speeds of centerline points paired with speed limits whose speeds are to be modified: * \param start_dist starting downtrack of the maneuver to be planned (excluding buffer points) in m @@ -144,7 +144,7 @@ namespace light_controlled_intersection_tactical_plugin * NOTE: Cruising speed profile is applied (case 1) if speed before deceleration is higher than speed limit. Otherwise Case 2. * NOTE: when applying the speed profile, the function ignores buffer points beyond start_dist and end_dist. Internally uses: config_.back_distance and speed_limit_ */ - void applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, + void applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, double starting_speed, double departure_speed, TrajectoryParams tsp); /** @@ -153,8 +153,8 @@ namespace light_controlled_intersection_tactical_plugin * \param maneuver Maneuver associated that has starting downtrack and desired entry time * \param starting_speed Starting speed of the vehicle * \param points The set of points with raw speed limits whose speed profile to be changed. - * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. - * + * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. + * */ void applyOptimizedTargetSpeedProfile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds); @@ -162,7 +162,7 @@ namespace light_controlled_intersection_tactical_plugin * \brief Creates geometry profile to return a point speed pair struct for INTERSECTION_TRANSIT maneuver types (by converting it to LANE_FOLLOW) * \param maneuvers The list of maneuvers to convert to geometry points and calculate associated raw speed limits * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier. - * If the first maneuver exceeds this then it's downtrack will be shifted to this value. + * If the first maneuver exceeds this then it's downtrack will be shifted to this value. * \param wm Pointer to intialized world model for semantic map access * \param ending_state_before_buffer reference to Vehicle state, which is state before applying extra points for curvature calculation that are removed later * \param state The vehicle state at the time the function is called @@ -197,22 +197,29 @@ namespace light_controlled_intersection_tactical_plugin /*! * \brief LightControlledIntersectionTacticalPlugin constructor */ - LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm, const Config& config, const std::string& plugin_name, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger); + LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm, const Config& config, const std::string& plugin_name, + std::shared_ptr nh); /** * \brief Function to process the light controlled intersection tactical plugin service call for trajectory planning * \param req The service request * \param resp The service response - */ - void planTrajectoryCB( - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + */ + void planTrajectoryCB( + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp); + /** + * \brief set the yield service + * + * \param yield_srv input yield service + */ + void set_yield_client(carma_ros2_utils::ClientPtr client); + /** * \brief Setter function to set a new config for this object * \param config The new config to be used by this object - */ + */ void setConfig(const Config& config); }; diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp index 430753e94f..ebe7786898 100644 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp @@ -45,7 +45,9 @@ namespace light_controlled_intersection_tactical_plugin double algorithm_evaluation_period = 4.5; double lateral_accel_limit = 2.5; double vehicle_accel_limit = 2.0; - double vehicle_decel_limit = 2.0; + double vehicle_decel_limit = 2.0; + int tactical_plugin_service_call_timeout = 100; + bool enable_object_avoidance = false; // Stream operator for this config friend std::ostream &operator<<(std::ostream &output, const Config &c) @@ -70,6 +72,8 @@ namespace light_controlled_intersection_tactical_plugin << "lateral_accel_limit: " << c.lateral_accel_limit << std::endl << "vehicle_accel_limit: " << c.vehicle_accel_limit << std::endl << "vehicle_decel_limit: " << c.vehicle_decel_limit << std::endl + << "tactical_plugin_service_call_timeout: " << c.tactical_plugin_service_call_timeout << std::endl + << "enable_object_avoidance: " << c.enable_object_avoidance << std::endl << "}" << std::endl; return output; } diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp index 867e594c1d..0a45425be0 100644 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp @@ -27,46 +27,47 @@ namespace light_controlled_intersection_tactical_plugin { /** * \brief ROS node for the LightControlledIntersectionTransitPluginNode - * + * */ class LightControlledIntersectionTransitPluginNode : public carma_guidance_plugins::TacticalPlugin { - private: + private: // Config for this object Config config_; + carma_ros2_utils::ClientPtr yield_client_; // Worker object std::shared_ptr worker_; public: - + /** - * \brief LightControlledIntersectionTransitPluginNode constructor + * \brief LightControlledIntersectionTransitPluginNode constructor */ explicit LightControlledIntersectionTransitPluginNode(const rclcpp::NodeOptions &); /** * \brief Callback for dynamic parameter updates */ - rcl_interfaces::msg::SetParametersResult + rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector ¶meters); //// // Overrides //// void plan_trajectory_callback( - std::shared_ptr, - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + std::shared_ptr, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override; bool get_availability() override; std::string get_version_id() override; - + /** * \brief This method should be used to load parameters and will be called on the configure state transition. - */ + */ carma_ros2_utils::CallbackReturn on_configure_plugin() override; }; 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 5d07d0bb96..9f8252983b 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 @@ -19,24 +19,24 @@ namespace light_controlled_intersection_tactical_plugin { - LightControlledIntersectionTacticalPlugin::LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm, const Config& config, const std::string& plugin_name, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger) - :wm_(wm), config_(config), plugin_name_(plugin_name), logger_(logger) + LightControlledIntersectionTacticalPlugin::LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm, const Config& config, const std::string& plugin_name, + std::shared_ptr nh) + :wm_(wm), config_(config), plugin_name_(plugin_name), nh_(nh) { } - void LightControlledIntersectionTacticalPlugin::planTrajectoryCB( - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + void LightControlledIntersectionTacticalPlugin::planTrajectoryCB( + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { std::chrono::system_clock::time_point 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("light_controlled_intersection_tactical_plugin"), "Starting light controlled intersection trajectory planning"); - + if(req->maneuver_index_to_plan >= req->maneuver_plan.maneuvers.size()) { throw std::invalid_argument( - "Light Control Intersection Plugin asked to plan invalid maneuver index: " + std::to_string(req->maneuver_index_to_plan) + + "Light Control Intersection Plugin asked to plan invalid maneuver index: " + std::to_string(req->maneuver_index_to_plan) + " for plan of size: " + std::to_string(req->maneuver_plan.maneuvers.size())); } @@ -48,7 +48,7 @@ namespace light_controlled_intersection_tactical_plugin maneuver_plan.push_back(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan]); resp->related_maneuvers.push_back(req->maneuver_index_to_plan); } - else + else { throw std::invalid_argument("Light Control Intersection Plugin asked to plan unsupported maneuver"); } @@ -62,7 +62,7 @@ namespace light_controlled_intersection_tactical_plugin auto current_lanelets = wm_->getLaneletsFromPoint({req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global}); lanelet::ConstLanelet current_lanelet; - + if (current_lanelets.empty()) { RCLCPP_ERROR_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Given vehicle position is not on the road! Returning..."); @@ -93,11 +93,11 @@ namespace light_controlled_intersection_tactical_plugin config_.default_downsample_ratio, config_.turn_downsample_ratio); - wpg_detail_config = basic_autonomy:: waypoint_generation::compose_detailed_trajectory_config(config_.trajectory_time_length, - config_.curve_resample_step_size, config_.minimum_speed, + wpg_detail_config = basic_autonomy:: waypoint_generation::compose_detailed_trajectory_config(config_.trajectory_time_length, + config_.curve_resample_step_size, config_.minimum_speed, config_.vehicle_accel_limit, - config_.lateral_accel_limit, - config_.speed_moving_average_window_size, + config_.lateral_accel_limit, + config_.speed_moving_average_window_size, config_.curvature_moving_average_window_size, config_.back_distance, config_.buffer_ending_downtrack); @@ -137,7 +137,7 @@ namespace light_controlled_intersection_tactical_plugin last_final_speeds_ = reduced_final_speeds; last_trajectory_.trajectory_points = reduced_last_traj.trajectory_points; - + if (is_last_case_successful_ != boost::none && last_case_ != boost::none && last_case_.get() == new_case && is_new_case_successful == true @@ -159,7 +159,7 @@ namespace light_controlled_intersection_tactical_plugin 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()) << ", and scheduled: " << std::to_string(last_successful_scheduled_entry_time_)); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "EDGE CASE: USING LAST TRAJ: " << (int)last_case_.get()); - } + } if (!resp->trajectory_plan.trajectory_points.empty()) // if has valid trajectory saved from before return { @@ -176,10 +176,10 @@ namespace light_controlled_intersection_tactical_plugin return; } - + // IF NOT USING LAST TRAJECTORY PLAN NEW TRAJECTORY - // Create curve-fitting compatible trajectories (with extra back and front attached points) with raw speed limits from maneuver + // Create curve-fitting compatible trajectories (with extra back and front attached points) with raw speed limits from maneuver auto points_and_target_speeds = createGeometryProfile(maneuver_plan, std::max((double)0, current_downtrack_ - config_.back_distance), wm_, ending_state_before_buffer_, req->vehicle_state, wpg_general_config, wpg_detail_config); @@ -197,30 +197,30 @@ namespace light_controlled_intersection_tactical_plugin trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()()); // Compose smooth trajectory/speed by resampling - trajectory.trajectory_points = basic_autonomy::waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds, - req->vehicle_state, req->header.stamp, wm_, ending_state_before_buffer_, debug_msg_, + trajectory.trajectory_points = basic_autonomy::waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds, + req->vehicle_state, req->header.stamp, wm_, ending_state_before_buffer_, debug_msg_, wpg_detail_config); // Compute the trajectory // Set the planning plugin field name for (auto& p : trajectory.trajectory_points) { p.planner_plugin_name = plugin_name_; } - + if (trajectory.trajectory_points.size () < 2) { if (last_trajectory_.trajectory_points.size() >= 2 && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time) > rclcpp::Time(req->header.stamp)) { resp->trajectory_plan = last_trajectory_; - RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Failed to generate new trajectory, so using last valid trajectory!"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Failed to generate new trajectory, so using last valid trajectory!"); } else { resp->trajectory_plan = trajectory; - RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Failed to generate new trajectory or use old valid trajectory, so returning empty/invalid trajectory!"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Failed to generate new trajectory or use old valid trajectory, so returning empty/invalid trajectory!"); } } - else + else { last_trajectory_ = trajectory; resp->trajectory_plan = trajectory; @@ -235,12 +235,23 @@ namespace light_controlled_intersection_tactical_plugin RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "last_successful_ending_downtrack_:" << last_successful_ending_downtrack_ << ", last_successful_scheduled_entry_time_: " << std::to_string(last_successful_scheduled_entry_time_)); } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "USING NEW CASE!!! : " << (int)last_case_.get()); - + } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Debug: new case:" << (int) new_case << ", is_new_case_successful: " << is_new_case_successful); resp->trajectory_plan.initial_longitudinal_velocity = last_final_speeds_.front(); + // Yield for potential obstacles in the road + // Aside from the flag, yield_plugin should not be called on invalid trajectories + if (config_.enable_object_avoidance && resp->trajectory_plan.trajectory_points.size() >= 2) + { + basic_autonomy::waypoint_generation::modify_trajectory_to_yield_to_obstacles(nh_, req, resp, yield_client_, config_.tactical_plugin_service_call_timeout); + } + else + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Ignored Object Avoidance"); + } + resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); // Planning complete @@ -251,14 +262,14 @@ namespace light_controlled_intersection_tactical_plugin return; } - void LightControlledIntersectionTacticalPlugin::applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, + void LightControlledIntersectionTacticalPlugin::applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, double starting_speed, double departure_speed, TrajectoryParams tsp) { if (points_and_target_speeds.empty()) { throw std::invalid_argument("Point and target speed list is empty! Unable to apply case one speed profile..."); } - + // Checking route geometry start against start_dist and adjust profile double planning_downtrack_start = wm->routeTrackPos(points_and_target_speeds[0].point).downtrack; // this can include buffered points earlier than maneuver start_dist @@ -283,23 +294,23 @@ namespace light_controlled_intersection_tactical_plugin if (planning_downtrack_start < start_dist) { //Account for the buffer distance that is technically not part of this maneuver - + total_dist_planned = planning_downtrack_start - start_dist; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "buffered section is present. Adjusted total_dist_planned to: " << total_dist_planned); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "buffered section is present. Adjusted total_dist_planned to: " << total_dist_planned); } - + double prev_speed = starting_speed; auto prev_point = points_and_target_speeds.front(); - + for(auto& p : points_and_target_speeds) { double delta_d = lanelet::geometry::distance2d(prev_point.point, p.point); - total_dist_planned += delta_d; + total_dist_planned += delta_d; //Apply the speed from algorithm at dist covered //Kinematic: v_f = sqrt(v_o^2 + 2*a*d) double speed_i; - if (total_dist_planned <= epsilon_) + if (total_dist_planned <= epsilon_) { //Keep target speed same for buffer distance portion speed_i = starting_speed; @@ -317,7 +328,7 @@ namespace light_controlled_intersection_tactical_plugin //Third segment speed_i = sqrt(std::max(pow(tsp.v2_, 2) + 2 * tsp.a3_ * (total_dist_planned - dist2), 0.0)); //std::max to ensure negative value is not sqrt } - else + else { //buffer points that will be cut speed_i = prev_speed; @@ -330,7 +341,7 @@ namespace light_controlled_intersection_tactical_plugin } p.speed = std::max({speed_i, config_.minimum_speed, algo_min_speed}); - p.speed = std::min({p.speed, speed_limit_, algo_max_speed}); + p.speed = std::min({p.speed, speed_limit_, algo_max_speed}); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Applied speed: " << p.speed << ", at dist: " << total_dist_planned); prev_point = p; @@ -340,7 +351,7 @@ namespace light_controlled_intersection_tactical_plugin void LightControlledIntersectionTacticalPlugin::applyOptimizedTargetSpeedProfile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds) { - if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).size() < 9 || + if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).size() < 9 || GET_MANEUVER_PROPERTY(maneuver, parameters.int_valued_meta_data).size() < 2 ){ throw std::invalid_argument("There must be 9 float_valued_meta_data and 2 int_valued_meta_data to apply algorithm's parameters."); } @@ -366,7 +377,7 @@ namespace light_controlled_intersection_tactical_plugin double entry_dist = ending_downtrack - starting_downtrack; // change speed profile depending on algorithm case starting from maneuver start_dist - applyTrajectorySmoothingAlgorithm(wm_, points_and_target_speeds, starting_downtrack, entry_dist, starting_speed, + applyTrajectorySmoothingAlgorithm(wm_, points_and_target_speeds, starting_downtrack, entry_dist, starting_speed, departure_speed, tsp); } @@ -388,7 +399,7 @@ namespace light_controlled_intersection_tactical_plugin const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config) { std::vector points_and_target_speeds; - + bool first = true; std::unordered_set visited_lanelets; std::vector processed_maneuvers; @@ -400,7 +411,7 @@ namespace light_controlled_intersection_tactical_plugin auto maneuver = maneuvers.front(); double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist); - + starting_downtrack = std::min(starting_downtrack, max_starting_downtrack); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Used downtrack: " << starting_downtrack); @@ -413,26 +424,31 @@ namespace light_controlled_intersection_tactical_plugin RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Creating Lane Follow Geometry"); std::vector lane_follow_points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets); - points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end()); - processed_maneuvers.push_back(maneuver); + points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end()); + processed_maneuvers.push_back(maneuver); } - else + else { throw std::invalid_argument("Light Control Intersection Plugin can only create a geometry profile for one maneuver"); } - //Add buffer ending to lane follow points at the end of maneuver(s) end dist + //Add buffer ending to lane follow points at the end of maneuver(s) end dist if(!processed_maneuvers.empty() && processed_maneuvers.back().type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING){ points_and_target_speeds = add_lanefollow_buffer(wm, points_and_target_speeds, processed_maneuvers, ending_state_before_buffer, detailed_config); } - + return points_and_target_speeds; } - void LightControlledIntersectionTacticalPlugin::setConfig(const Config& config) + void LightControlledIntersectionTacticalPlugin::setConfig(const Config& config) { config_ = config; } + void LightControlledIntersectionTacticalPlugin::set_yield_client(carma_ros2_utils::ClientPtr client) + { + yield_client_ = client; + } + } // light_controlled_intersection_tactical_plugin diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp index 7cea76134f..940094d328 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp @@ -46,6 +46,9 @@ namespace light_controlled_intersection_tactical_plugin config_.lateral_accel_limit = declare_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit); config_.vehicle_accel_limit = declare_parameter("vehicle_acceleration_limit", config_.vehicle_accel_limit); config_.vehicle_decel_limit = declare_parameter("vehicle_deceleration_limit", config_.vehicle_decel_limit); + config_.tactical_plugin_service_call_timeout = declare_parameter("tactical_plugin_service_call_timeout", config_.tactical_plugin_service_call_timeout); + config_.enable_object_avoidance = declare_parameter("enable_object_avoidance", config_.enable_object_avoidance); + } rcl_interfaces::msg::SetParametersResult LightControlledIntersectionTransitPluginNode::parameter_update_callback(const std::vector ¶meters) @@ -58,10 +61,10 @@ namespace light_controlled_intersection_tactical_plugin {"buffer_ending_downtrack", config_.buffer_ending_downtrack}, {"vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier}, {"vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier}, - {"lat_accel_multiplier", config_.lat_accel_multiplier}, - {"stop_line_buffer", config_.stop_line_buffer}, - {"minimum_speed", config_.minimum_speed}, - {"algorithm_evaluation_distance", config_.algorithm_evaluation_distance}, + {"lat_accel_multiplier", config_.lat_accel_multiplier}, + {"stop_line_buffer", config_.stop_line_buffer}, + {"minimum_speed", config_.minimum_speed}, + {"algorithm_evaluation_distance", config_.algorithm_evaluation_distance}, {"algorithm_evaluation_period", config_.algorithm_evaluation_period}}, parameters); // Global acceleration limits not allowed to dynamically update auto error_2 = update_params( {{"default_downsample_ratio", config_.default_downsample_ratio}, @@ -108,6 +111,8 @@ namespace light_controlled_intersection_tactical_plugin get_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit); get_parameter("vehicle_acceleration_limit", config_.vehicle_accel_limit); get_parameter("vehicle_deceleration_limit", config_.vehicle_decel_limit); + get_parameter("tactical_plugin_service_call_timeout", config_.tactical_plugin_service_call_timeout); + get_parameter("enable_object_avoidance", config_.enable_object_avoidance); // Use the configured multipliers to update the accel limits config_.lateral_accel_limit = config_.lateral_accel_limit * config_.lat_accel_multiplier; @@ -120,7 +125,11 @@ namespace light_controlled_intersection_tactical_plugin RCLCPP_INFO_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Loaded params: " << config_); // Initialize worker object - worker_ = std::make_shared(get_world_model(), config_, get_plugin_name(), get_node_logging_interface()); + worker_ = std::make_shared(get_world_model(), config_, get_plugin_name(), shared_from_this()); + + yield_client_ = create_client("yield_plugin/plan_trajectory"); + worker_->set_yield_client(yield_client_); + RCLCPP_INFO_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Yield Client Set"); // Return success if everything initialized successfully return CallbackReturn::SUCCESS; @@ -135,8 +144,8 @@ namespace light_controlled_intersection_tactical_plugin } void LightControlledIntersectionTransitPluginNode::plan_trajectory_callback( - std::shared_ptr, - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + std::shared_ptr, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { worker_->planTrajectoryCB(req, resp); diff --git a/light_controlled_intersection_tactical_plugin/test/node_test.cpp b/light_controlled_intersection_tactical_plugin/test/node_test.cpp index 6fdb936dcb..22451ac5fa 100644 --- a/light_controlled_intersection_tactical_plugin/test/node_test.cpp +++ b/light_controlled_intersection_tactical_plugin/test/node_test.cpp @@ -27,7 +27,7 @@ namespace light_controlled_intersection_tactical_plugin { // TODO: The package requires additional unit tests to improve unit test coverage. These unit tests will be created // in a follow-on story. - + TEST(LCITacticalPluginTest, applyTrajectorySmoothingAlgorithm) { std::shared_ptr wm = std::make_shared(); @@ -35,8 +35,8 @@ namespace light_controlled_intersection_tactical_plugin wm->setMap(map); carma_wm::test::setSpeedLimit(15_mph, wm); carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); - - + + rclcpp::NodeOptions options; auto lci_node = std::make_shared(options); Config config; @@ -44,8 +44,8 @@ namespace light_controlled_intersection_tactical_plugin std::vector points_and_target_speeds; - auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node->get_node_logging_interface()); - + auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node); + TrajectoryParams tp; tp.v1_ = 1.0; tp.v2_ = 2.0; @@ -74,7 +74,7 @@ namespace light_controlled_intersection_tactical_plugin points_and_target_speeds.push_back(pair); lci_tactical.applyTrajectorySmoothingAlgorithm(wm, points_and_target_speeds, 1, 4, 1, 1, tp); - + EXPECT_NEAR(points_and_target_speeds.front().speed, 1.0, 0.001); EXPECT_NEAR(points_and_target_speeds.back().speed, 2.0, 0.001); EXPECT_NEAR(points_and_target_speeds.front().point.y(), 1.0, 0.001); @@ -123,7 +123,7 @@ namespace light_controlled_intersection_tactical_plugin EXPECT_NEAR(points_and_target_speeds.back().point.y(), 5.0, 0.001); } - + TEST(LCITacticalPluginTest, applyOptimizedTargetSpeedProfile) { std::shared_ptr wm = std::make_shared(); @@ -131,8 +131,8 @@ namespace light_controlled_intersection_tactical_plugin wm->setMap(map); carma_wm::test::setSpeedLimit(15_mph, wm); carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); - - + + rclcpp::NodeOptions options; auto lci_node = std::make_shared(options); Config config; @@ -140,8 +140,8 @@ namespace light_controlled_intersection_tactical_plugin std::vector points_and_target_speeds; - auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node->get_node_logging_interface()); - + auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node); + carma_planning_msgs::msg::Maneuver maneuver_msg; maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; maneuver_msg.lane_following_maneuver.parameters.negotiation_type = @@ -152,7 +152,7 @@ namespace light_controlled_intersection_tactical_plugin maneuver_msg.lane_following_maneuver.start_speed = 1; maneuver_msg.lane_following_maneuver.end_dist = 4; maneuver_msg.lane_following_maneuver.end_speed = 1; - + TrajectoryParams tsp; tsp.v1_ = 1.0; tsp.v2_ = 2.0; @@ -193,7 +193,7 @@ namespace light_controlled_intersection_tactical_plugin points_and_target_speeds.push_back(pair); EXPECT_THROW(lci_tactical.applyOptimizedTargetSpeedProfile(maneuver_msg, maneuver_msg.lane_following_maneuver.start_speed, points_and_target_speeds), std::invalid_argument); - + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x3_); lci_tactical.applyOptimizedTargetSpeedProfile(maneuver_msg, maneuver_msg.lane_following_maneuver.start_speed, points_and_target_speeds); @@ -204,7 +204,7 @@ namespace light_controlled_intersection_tactical_plugin EXPECT_NEAR(points_and_target_speeds.back().point.y(), 5.0, 0.001); } - + TEST(LCITacticalPluginTest, createGeometryProfile) { std::shared_ptr wm = std::make_shared(); @@ -212,15 +212,15 @@ namespace light_controlled_intersection_tactical_plugin wm->setMap(map); carma_wm::test::setSpeedLimit(15_mph, wm); carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); - - + + rclcpp::NodeOptions options; auto lci_node = std::make_shared(options); Config config; config.minimum_speed = 1; - auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node->get_node_logging_interface()); - + auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node); + carma_planning_msgs::msg::Maneuver maneuver_msg; maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; maneuver_msg.lane_following_maneuver.parameters.negotiation_type = @@ -231,7 +231,7 @@ namespace light_controlled_intersection_tactical_plugin maneuver_msg.lane_following_maneuver.start_speed = 1; maneuver_msg.lane_following_maneuver.end_dist = 4; maneuver_msg.lane_following_maneuver.end_speed = 2; - + TrajectoryParams tsp; tsp.v1_ = 1.0; tsp.v2_ = 2.0; @@ -277,9 +277,9 @@ namespace light_controlled_intersection_tactical_plugin EXPECT_EQ(lci_tactical.createGeometryProfile({maneuver_msg}, 1, wm, ending_state, state, wpg_general_config, wpg_detail_config).size(), 7); - + auto points_and_target_speeds = lci_tactical.createGeometryProfile({maneuver_msg}, 1, wm, ending_state, state, wpg_general_config, wpg_detail_config); - + EXPECT_NEAR(points_and_target_speeds.front().speed, 2.0, 0.001); EXPECT_NEAR(points_and_target_speeds.back().speed, 2.0, 0.001); EXPECT_NEAR(points_and_target_speeds.front().point.y(), 0.0, 0.001); @@ -293,16 +293,16 @@ namespace light_controlled_intersection_tactical_plugin wm->setMap(map); carma_wm::test::setSpeedLimit(15_mph, wm); carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); - - + + rclcpp::NodeOptions options; auto lci_node = std::make_shared(options); Config config; config.minimum_speed = 1; config.default_downsample_ratio = 1; - auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node->get_node_logging_interface()); - + auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node); + carma_planning_msgs::msg::Maneuver maneuver_msg; maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; maneuver_msg.lane_following_maneuver.parameters.negotiation_type = @@ -313,7 +313,7 @@ namespace light_controlled_intersection_tactical_plugin maneuver_msg.lane_following_maneuver.start_speed = 1; maneuver_msg.lane_following_maneuver.end_dist = 11.0; maneuver_msg.lane_following_maneuver.end_speed = 2; - + TrajectoryParams tsp; tsp.v1_ = 1.0; tsp.v2_ = 2.0; @@ -347,20 +347,20 @@ namespace light_controlled_intersection_tactical_plugin auto req = std::make_shared(); auto resp = std::make_shared(); - + req->maneuver_plan.maneuvers.push_back(maneuver_msg); req->maneuver_index_to_plan = 0; req->vehicle_state = state; lci_tactical.planTrajectoryCB(req, resp); - + EXPECT_NEAR(rclcpp::Time(resp->trajectory_plan.trajectory_points.front().target_time).seconds(), 0.0, 0.001); EXPECT_NEAR(rclcpp::Time(resp->trajectory_plan.trajectory_points.back().target_time).seconds(), 9.23, 0.1); EXPECT_NEAR(resp->trajectory_plan.trajectory_points.front().y, 0.1, 0.001); } - + } // namespace light_controlled_intersection_tactical_plugin @@ -377,4 +377,4 @@ int main(int argc, char ** argv) rclcpp::shutdown(); return success; -} \ No newline at end of file +} \ No newline at end of file diff --git a/stop_and_wait_plugin/include/stop_and_wait_config.hpp b/stop_and_wait_plugin/include/stop_and_wait_config.hpp index 0063bd62d9..6126d50c96 100644 --- a/stop_and_wait_plugin/include/stop_and_wait_config.hpp +++ b/stop_and_wait_plugin/include/stop_and_wait_config.hpp @@ -32,6 +32,9 @@ struct StopandWaitConfig double centerline_sampling_spacing = 1.0; // The gap in meters between points sampled from the lanelet centerlines for planning trajectory positions double default_stopping_buffer = 3.0; // The default buffer in meters used for where the vehicle can come to a stop during execution. This value is overriden by the first value in maneuver.parameters.float_valued_meta_data[] double moving_average_window_size = 11.0; // Moving Average filter window size + int tactical_plugin_service_call_timeout = 100; // Tactical plugin service call request timeout in milliseconds + bool enable_object_avoidance = false; // True to enable object avoidance using yield_plugin + friend std::ostream& operator<<(std::ostream& output, const StopandWaitConfig& c) { output << "StopandWaitConfig { " << std::endl @@ -43,6 +46,8 @@ struct StopandWaitConfig << "crawl_speed: " << c.crawl_speed << std::endl << "centerline_sampling_spacing: " << c.crawl_speed << std::endl << "default_stopping_buffer: " << c.crawl_speed << std::endl + << "tactical_plugin_service_call_timeout: " << c.tactical_plugin_service_call_timeout << std::endl + << "enable_object_avoidance: " << c.enable_object_avoidance << std::endl << "}" << std::endl; return output; } diff --git a/stop_and_wait_plugin/include/stop_and_wait_node.hpp b/stop_and_wait_plugin/include/stop_and_wait_node.hpp index 378eb46403..b8a0fbf00a 100644 --- a/stop_and_wait_plugin/include/stop_and_wait_node.hpp +++ b/stop_and_wait_plugin/include/stop_and_wait_node.hpp @@ -55,6 +55,9 @@ class StopandWaitNode : public carma_guidance_plugins::TacticalPlugin StopandWaitConfig config_; + // Service Clients + carma_ros2_utils::ClientPtr yield_client_; + // Worker std::shared_ptr plugin_; @@ -62,21 +65,21 @@ class StopandWaitNode : public carma_guidance_plugins::TacticalPlugin std::string plugin_name_; public: - + /** - * \brief Node constructor + * \brief Node constructor */ explicit StopandWaitNode(const rclcpp::NodeOptions &); /** * \brief This method should be used to load parameters and will be called on the configure state transition. - */ + */ carma_ros2_utils::CallbackReturn on_configure_plugin(); /** * \brief Callback for dynamic parameter updates */ - rcl_interfaces::msg::SetParametersResult + rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector ¶meters); //// @@ -84,8 +87,8 @@ class StopandWaitNode : public carma_guidance_plugins::TacticalPlugin //// void plan_trajectory_callback( - std::shared_ptr srv_header, - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override; bool get_availability() override; diff --git a/stop_and_wait_plugin/include/stop_and_wait_plugin.hpp b/stop_and_wait_plugin/include/stop_and_wait_plugin.hpp index 575fa1e5e9..b16aea3e64 100644 --- a/stop_and_wait_plugin/include/stop_and_wait_plugin.hpp +++ b/stop_and_wait_plugin/include/stop_and_wait_plugin.hpp @@ -53,9 +53,9 @@ class StopandWait /** * \brief Constructor */ - StopandWait(std::shared_ptr nh, - carma_wm::WorldModelConstPtr wm, - const StopandWaitConfig& config, + StopandWait(std::shared_ptr nh, + carma_wm::WorldModelConstPtr wm, + const StopandWaitConfig& config, const std::string& plugin_name, const std::string& version_id); @@ -77,9 +77,9 @@ class StopandWait * the vehicle position or earlier. If the first maneuver exceeds this then it's downtrack will be shifted to this * value. * - * ASSUMPTION: Since the vehicle is trying to stop the assumption made is that the speed limit is irrelevant. + * ASSUMPTION: Since the vehicle is trying to stop the assumption made is that the speed limit is irrelevant. * ASSUMPTION: The provided maneuver lies on the route shortest path - * + * * \return List of centerline points paired with speed limits. All output points will have speed matching state.logitudinal_velocity */ std::vector maneuvers_to_points(const std::vector& maneuvers, @@ -109,6 +109,13 @@ class StopandWait const std::vector& points, const std::vector& times, const std::vector& yaws, rclcpp::Time startTime); + /** + * \brief set the yield service + * + * \param yield_srv input yield service + */ + void set_yield_client(carma_ros2_utils::ClientPtr client); + private: double epsilon_ = 0.001; //small constant to compare double @@ -119,6 +126,8 @@ class StopandWait carma_wm::WorldModelConstPtr wm_; StopandWaitConfig config_; std::shared_ptr nh_; - + // Service Clients + carma_ros2_utils::ClientPtr yield_client_; + }; } // namespace stop_and_wait_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 962f5eb3ff..06799e0454 100644 --- a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp @@ -48,9 +48,9 @@ using oss = std::ostringstream; namespace stop_and_wait_plugin { -StopandWait::StopandWait(std::shared_ptr nh, - carma_wm::WorldModelConstPtr wm, - const StopandWaitConfig& config, +StopandWait::StopandWait(std::shared_ptr nh, + carma_wm::WorldModelConstPtr wm, + const StopandWaitConfig& config, const std::string& plugin_name, const std::string& version_id) : version_id_ (version_id),plugin_name_(plugin_name),config_(config),nh_(nh), wm_(wm) @@ -80,7 +80,7 @@ bool StopandWait::plan_trajectory_cb(carma_planning_msgs::srv::PlanTrajectory::R if (req->vehicle_state.longitudinal_vel < epsilon_) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_wait_plugin"),"Detected that car is already stopped! Ignoring the request to plan Stop&Wait"); - + return true; } @@ -114,7 +114,7 @@ bool StopandWait::plan_trajectory_cb(carma_planning_msgs::srv::PlanTrajectory::R // Extract the stopping buffer used to consider a stopping behavior complete double stop_location_buffer = config_.default_stopping_buffer; // If no maneuver meta data is provided we will use the default buffer - + double stopping_accel = 0.0; if (maneuver_plan[0].stop_and_wait_maneuver.parameters.presence_vector & carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA) @@ -132,7 +132,7 @@ bool StopandWait::plan_trajectory_cb(carma_planning_msgs::srv::PlanTrajectory::R throw std::invalid_argument("stop and wait maneuver message missing required float meta data"); } - double initial_speed = req->vehicle_state.longitudinal_vel; //will be modified after compose_trajectory_from_centerline + double initial_speed = req->vehicle_state.longitudinal_vel; //will be modified after compose_trajectory_from_centerline RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_wait_plugin"),"Original size: " << points_and_target_speeds.size()); @@ -146,6 +146,16 @@ bool StopandWait::plan_trajectory_cb(carma_planning_msgs::srv::PlanTrajectory::R resp->trajectory_plan = trajectory; + // Yield for potential obstacles in the road + // Aside from the flag, yield_plugin should not be called on invalid trajectories + if (config_.enable_object_avoidance && resp->trajectory_plan.trajectory_points.size() >= 2) + { + basic_autonomy::waypoint_generation::modify_trajectory_to_yield_to_obstacles(nh_, req, resp, yield_client_, config_.tactical_plugin_service_call_timeout); + } + else + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_wait_plugin"), "Ignored Object Avoidance"); + } std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); // Planning complete @@ -184,7 +194,7 @@ std::vector StopandWait::maneuvers_to_points(const std::vector StopandWait::trajecto tpp.yaw = yaws[i]; tpp.planner_plugin_name = plugin_name_; tpp.controller_plugin_name = "default"; - + traj.push_back(tpp); } @@ -253,7 +263,7 @@ std::vector StopandWait::compose_ break; } } - + if (req_dist > remaining_distance) { @@ -290,7 +300,7 @@ std::vector StopandWait::compose_ prev_pair = pair; continue; } - + if (reached_end || v_i >= starting_speed) { // We are walking backward, so if the prev speed is greater than or equal to the starting speed then we are done // backtracking @@ -302,7 +312,7 @@ std::vector StopandWait::compose_ prev_pair = pair; continue; // continue until loop end } - + double v_f = sqrt(v_i * v_i + 2 * target_accel * dx); PointSpeedPair pair = points[i]; @@ -315,7 +325,7 @@ std::vector StopandWait::compose_ // Now we have a trajectory that decelerates from our end point to somewhere in the maneuver std::reverse(final_points.begin(), - final_points.end()); + final_points.end()); std::vector speeds; std::vector raw_points; @@ -325,7 +335,7 @@ std::vector StopandWait::compose_ double max_downtrack = inverse_downtracks.back(); std::vector downtracks = lanelet::utils::transform(inverse_downtracks, [max_downtrack](const auto& d) { return max_downtrack - d; }); std::reverse(downtracks.begin(), - downtracks.end()); + downtracks.end()); bool in_range = false; double stopped_downtrack = 0; @@ -343,7 +353,7 @@ std::vector StopandWait::compose_ if (downtracks.back() - downtrack < stop_location_buffer && filtered_speeds[i] < config_.crawl_speed + one_mph_in_mps) { // if we are within the stopping buffer and going at near crawl speed then command stop - + // To avoid any issues in control plugin behavior we only command 0 if the vehicle is inside the buffer if (vehicle_in_buffer || (i == filtered_speeds.size() - 1)) { // Vehicle is in the buffer filtered_speeds[i] = 0.0; @@ -408,4 +418,9 @@ void StopandWait::splitPointSpeedPairs(const std::vector& points } } +void StopandWait::set_yield_client(carma_ros2_utils::ClientPtr client) +{ + yield_client_ = client; +} + } // namespace stop_and_wait_plugin \ No newline at end of file diff --git a/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp b/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp index b3caf08fa9..3773269525 100644 --- a/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp @@ -25,7 +25,7 @@ namespace stop_and_wait_plugin { // Create initial config config_ = StopandWaitConfig(); - + // Declare parameters config_.minimal_trajectory_duration = declare_parameter("minimal_trajectory_duration", config_.minimal_trajectory_duration); config_.stop_timestep = declare_parameter("stop_timestep", config_.stop_timestep); @@ -35,6 +35,9 @@ namespace stop_and_wait_plugin config_.crawl_speed = declare_parameter("crawl_speed", config_.crawl_speed); config_.centerline_sampling_spacing = declare_parameter("centerline_sampling_spacing", config_.centerline_sampling_spacing); config_.default_stopping_buffer = declare_parameter("default_stopping_buffer", config_.default_stopping_buffer); + config_.tactical_plugin_service_call_timeout = declare_parameter("tactical_plugin_service_call_timeout", config_.tactical_plugin_service_call_timeout); + config_.enable_object_avoidance = declare_parameter("enable_object_avoidance", config_.enable_object_avoidance); + } rcl_interfaces::msg::SetParametersResult StopandWaitNode::parameter_update_callback(const std::vector ¶meters) @@ -68,21 +71,27 @@ namespace stop_and_wait_plugin get_parameter("crawl_speed", config_.crawl_speed); get_parameter("centerline_sampling_spacing", config_.centerline_sampling_spacing); get_parameter("default_stopping_buffer", config_.default_stopping_buffer); + get_parameter("tactical_plugin_service_call_timeout", config_.tactical_plugin_service_call_timeout); + get_parameter("enable_object_avoidance", config_.enable_object_avoidance); RCLCPP_INFO_STREAM(rclcpp::get_logger("stop_and_wait_plugin"),"Done loading parameters: " << config_); // Register runtime parameter update callback add_on_set_parameters_callback(std::bind(&StopandWaitNode::parameter_update_callback, this, std_ph::_1)); - + plugin_ = std::make_shared(shared_from_this(), get_world_model(), config_,plugin_name_,version_id_); - + + yield_client_ = create_client("yield_plugin/plan_trajectory"); + plugin_->set_yield_client(yield_client_); + RCLCPP_INFO_STREAM(rclcpp::get_logger("stop_and_wait_plugin"), "Yield Client Set"); + // Return success if everything initialized successfully return CallbackReturn::SUCCESS; } void StopandWaitNode::plan_trajectory_callback( - std::shared_ptr srv_header, - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { plugin_->plan_trajectory_cb(req, resp); From dda10db1e898814b5e9c55fb8034dfc1bbb473ce Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Tue, 5 Dec 2023 08:37:11 +0000 Subject: [PATCH 15/18] address PR comments --- basic_autonomy/src/basic_autonomy.cpp | 53 +++++++++++++-------------- carma_wm/src/WMListenerWorker.cpp | 8 ++-- yield_plugin/src/yield_plugin.cpp | 15 ++++---- 3 files changed, 37 insertions(+), 39 deletions(-) diff --git a/basic_autonomy/src/basic_autonomy.cpp b/basic_autonomy/src/basic_autonomy.cpp index 7ebe56c6ba..493dafca44 100644 --- a/basic_autonomy/src/basic_autonomy.cpp +++ b/basic_autonomy/src/basic_autonomy.cpp @@ -1304,42 +1304,39 @@ namespace basic_autonomy { RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Activate Object Avoidance"); - if (yield_client && yield_client->service_is_ready()) + if (!yield_client || !yield_client->service_is_ready()) { - RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Yield Client is valid"); + throw std::runtime_error("Yield Client is not set or unavailable after configuration state of lifecycle"); + } - auto yield_srv = std::make_shared(); - yield_srv->initial_trajectory_plan = resp->trajectory_plan; - yield_srv->vehicle_state = req->vehicle_state; + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Yield Client is valid"); - auto yield_resp = yield_client->async_send_request(yield_srv); + auto yield_srv = std::make_shared(); + yield_srv->initial_trajectory_plan = resp->trajectory_plan; + yield_srv->vehicle_state = req->vehicle_state; - auto future_status = yield_resp.wait_for(std::chrono::milliseconds(yield_plugin_service_call_timeout)); + auto yield_resp = yield_client->async_send_request(yield_srv); - if (future_status == std::future_status::ready) - { - RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Received Traj from Yield"); - carma_planning_msgs::msg::TrajectoryPlan yield_plan = yield_resp.get()->trajectory_plan; - if (validate_yield_plan(node_handler, yield_plan)) - { - RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Yield trajectory validated"); - resp->trajectory_plan = yield_plan; - } - else - { - throw std::invalid_argument("Invalid Yield Trajectory"); - } - } - else - { - // Sometimes the yield plugin's service call may be unsuccessful due to its computationally expensive logic. - // However, consecutive calls can be successful, so return original trajectory for now - RCLCPP_WARN_STREAM(node_handler->get_logger(), "Unable to Call Yield Plugin"); - } + auto future_status = yield_resp.wait_for(std::chrono::milliseconds(yield_plugin_service_call_timeout)); + + if (future_status != std::future_status::ready) + { + // Sometimes the yield plugin's service call may be unsuccessful due to its computationally expensive logic. + // However, consecutive calls can be successful, so return original trajectory for now + RCLCPP_WARN_STREAM(node_handler->get_logger(), "Service request to yield plugin timed out waiting on a reply from the service server"); + return resp; + } + + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Received Traj from Yield"); + carma_planning_msgs::msg::TrajectoryPlan yield_plan = yield_resp.get()->trajectory_plan; + if (validate_yield_plan(node_handler, yield_plan)) + { + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Yield trajectory validated"); + resp->trajectory_plan = yield_plan; } else { - throw std::invalid_argument("Yield Client is unavailable"); + throw std::invalid_argument("Invalid Yield Trajectory"); } return resp; diff --git a/carma_wm/src/WMListenerWorker.cpp b/carma_wm/src/WMListenerWorker.cpp index 427821d631..517ef490f2 100644 --- a/carma_wm/src/WMListenerWorker.cpp +++ b/carma_wm/src/WMListenerWorker.cpp @@ -126,16 +126,16 @@ void logSignalizedIntersectionManager(const carma_wm::SignalizedIntersectionMana } for (auto const &[signal_id, entry_llt_ids] : sim.signal_group_to_entry_lanelet_ids_) { - for (auto iter = entry_llt_ids.begin(); iter != entry_llt_ids.end(); iter++) + for (const auto & entry_llt_id : entry_llt_ids) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", entry llt id: " << *iter); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", entry llt id: " << entry_llt_id); } } for (auto const &[signal_id, exit_llt_ids] : sim.signal_group_to_exit_lanelet_ids_) { - for (auto iter = exit_llt_ids.begin(); iter != exit_llt_ids.end(); iter++) + for (const auto & exit_llt_id : exit_llt_ids) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", exit llt id: " << *iter); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", exit llt id: " << exit_llt_id); } } for (auto const &[signal_id, regem_id] : sim.signal_group_to_traffic_light_id_) diff --git a/yield_plugin/src/yield_plugin.cpp b/yield_plugin/src/yield_plugin.cpp index 2223c5362a..f63e9de099 100644 --- a/yield_plugin/src/yield_plugin.cpp +++ b/yield_plugin/src/yield_plugin.cpp @@ -101,7 +101,7 @@ namespace yield_plugin throw std::invalid_argument("No map projector available for ecef conversion"); } - lanelet::BasicPoint3d map_point = map_projector_->projectECEF( { (double)ecef_point.ecef_x/100.0, (double)ecef_point.ecef_y/100.0, (double)ecef_point.ecef_z/100.0 } , 1); + lanelet::BasicPoint3d map_point = map_projector_->projectECEF( { static_cast(ecef_point.ecef_x)/100.0, static_cast(ecef_point.ecef_y)/100.0, static_cast(ecef_point.ecef_z)/100.0 } , 1); return lanelet::traits::to2D(map_point); } @@ -168,7 +168,7 @@ namespace yield_plugin int req_traj_fractional = pt.get("f"); int start_lanelet_id = pt.get("sl"); int end_lanelet_id = pt.get("el"); - double req_traj_speed = (double)req_traj_speed_full + (double)(req_traj_fractional)/10.0; + double req_traj_speed = static_cast(req_traj_speed_full) + static_cast(req_traj_fractional)/10.0; RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_traj_speed" << req_traj_speed); RCLCPP_DEBUG_STREAM(nh_->get_logger(),"start_lanelet_id" << start_lanelet_id); RCLCPP_DEBUG_STREAM(nh_->get_logger(),"end_lanelet_id" << end_lanelet_id); @@ -177,13 +177,13 @@ namespace yield_plugin req_traj_plan = convert_eceftrajectory_to_mappoints(incoming_trajectory); - double req_expiration_sec = (double)incoming_request.expiration; + double req_expiration_sec = static_cast(incoming_request.expiration); double current_time_sec = nh_->now().seconds(); bool response_to_clc_req = false; // ensure there is enough time for the yield double req_plan_time = req_expiration_sec - current_time_sec; - double req_timestamp = (double)incoming_request.m_header.timestamp / 1000.0 - current_time_sec; + double req_timestamp = static_cast(incoming_request.m_header.timestamp) / 1000.0 - current_time_sec; set_incoming_request_info(req_traj_plan, req_traj_speed, req_plan_time, req_timestamp); @@ -534,7 +534,8 @@ namespace yield_plugin RCLCPP_WARN(nh_->get_logger(), "Yield plugin received less than 2 points in update_traj_for_object, returning unchanged..."); return original_tp; } - else if (original_tp.trajectory_points.empty()) + + if (original_tp.trajectory_points.empty()) { RCLCPP_ERROR(nh_->get_logger(), "Yield plugin received empty trajectory plan in update_traj_for_object"); throw std::invalid_argument("Yield plugin received empty trajectory plan in update_traj_for_object"); @@ -708,7 +709,7 @@ namespace yield_plugin double result = 0; for (size_t i = 0; i < coeff.size(); i++) { - double value = coeff.at(i) * pow(x, (int)(coeff.size() - 1 - i)); + double value = coeff.at(i) * pow(x, static_cast(coeff.size() - 1 - i)); result = result + value; } return result; @@ -719,7 +720,7 @@ namespace yield_plugin double result = 0; for (size_t i = 0; i < coeff.size()-1; i++) { - double value = (int)(coeff.size() - 1 - i) * coeff.at(i) * pow(x, (int)(coeff.size() - 2 - i)); + double value = static_cast(coeff.size() - 1 - i) * coeff.at(i) * pow(x, static_cast(coeff.size() - 2 - i)); result = result + value; } return result; From 7f4f1b5c1a625969eafa07e9c66aec81118fb002 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Tue, 5 Dec 2023 16:54:57 +0000 Subject: [PATCH 16/18] address PR comments --- .../include/basic_autonomy/basic_autonomy.hpp | 2 +- basic_autonomy/src/basic_autonomy.cpp | 45 ++++++++++--------- .../test/test_waypoint_generation.cpp | 8 ++-- 3 files changed, 29 insertions(+), 26 deletions(-) diff --git a/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp b/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp index 989063e3d9..ca1c2b7d60 100644 --- a/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp +++ b/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp @@ -390,7 +390,7 @@ namespace basic_autonomy * * \return true or false */ - bool validate_yield_plan(std::shared_ptr node_handler, const carma_planning_msgs::msg::TrajectoryPlan& yield_plan); + bool is_valid_yield_plan(std::shared_ptr node_handler, const carma_planning_msgs::msg::TrajectoryPlan& yield_plan); } } // basic_autonomy \ No newline at end of file diff --git a/basic_autonomy/src/basic_autonomy.cpp b/basic_autonomy/src/basic_autonomy.cpp index 493dafca44..1d5cc5e441 100644 --- a/basic_autonomy/src/basic_autonomy.cpp +++ b/basic_autonomy/src/basic_autonomy.cpp @@ -1276,40 +1276,43 @@ namespace basic_autonomy return output; } - bool validate_yield_plan(std::shared_ptr node_handler, const carma_planning_msgs::msg::TrajectoryPlan& yield_plan) + bool is_valid_yield_plan(const std::shared_ptr node_handler, const carma_planning_msgs::msg::TrajectoryPlan& yield_plan) { - if (yield_plan.trajectory_points.size()>= 2) + if (yield_plan.trajectory_points.size() < 2) { - RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Yield Trajectory Time" << rclcpp::Time(yield_plan.trajectory_points[0].target_time).seconds()); - RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Now:" << node_handler->now().seconds()); + RCLCPP_WARN(node_handler->get_logger(), "Invalid Yield Trajectory"); + } - if (rclcpp::Time(yield_plan.trajectory_points[0].target_time) + rclcpp::Duration(5.0, 0) > node_handler->now()) - { - return true; - } - else - { - RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Old Yield Trajectory"); - } + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Yield Trajectory Time" << rclcpp::Time(yield_plan.trajectory_points[0].target_time).seconds()); + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Now:" << node_handler->now().seconds()); + + if (rclcpp::Time(yield_plan.trajectory_points[0].target_time) + rclcpp::Duration(5.0, 0) > node_handler->now()) + { + return true; } else { - RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Invalid Yield Trajectory"); + RCLCPP_DEBUG(node_handler->get_logger(), "Old Yield Trajectory"); } + return false; } - carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr modify_trajectory_to_yield_to_obstacles(std::shared_ptr node_handler, carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp, - carma_ros2_utils::ClientPtr yield_client, int yield_plugin_service_call_timeout) + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr modify_trajectory_to_yield_to_obstacles( + const std::shared_ptr node_handler, + const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + const carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp, + const carma_ros2_utils::ClientPtr yield_client, + int yield_plugin_service_call_timeout) { - RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Activate Object Avoidance"); + RCLCPP_DEBUG(node_handler->get_logger(), "Activate Object Avoidance"); if (!yield_client || !yield_client->service_is_ready()) { throw std::runtime_error("Yield Client is not set or unavailable after configuration state of lifecycle"); } - RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Yield Client is valid"); + RCLCPP_DEBUG(node_handler->get_logger(), "Yield Client is valid"); auto yield_srv = std::make_shared(); yield_srv->initial_trajectory_plan = resp->trajectory_plan; @@ -1323,15 +1326,15 @@ namespace basic_autonomy { // Sometimes the yield plugin's service call may be unsuccessful due to its computationally expensive logic. // However, consecutive calls can be successful, so return original trajectory for now - RCLCPP_WARN_STREAM(node_handler->get_logger(), "Service request to yield plugin timed out waiting on a reply from the service server"); + RCLCPP_WARN(node_handler->get_logger(), "Service request to yield plugin timed out waiting on a reply from the service server"); return resp; } - RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Received Traj from Yield"); + RCLCPP_DEBUG(node_handler->get_logger(), "Received Traj from Yield"); carma_planning_msgs::msg::TrajectoryPlan yield_plan = yield_resp.get()->trajectory_plan; - if (validate_yield_plan(node_handler, yield_plan)) + if (is_valid_yield_plan(node_handler, yield_plan)) { - RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Yield trajectory validated"); + RCLCPP_DEBUG(node_handler->get_logger(), "Yield trajectory validated"); resp->trajectory_plan = yield_plan; } else diff --git a/basic_autonomy/test/test_waypoint_generation.cpp b/basic_autonomy/test/test_waypoint_generation.cpp index 26312d01c5..8196db4678 100644 --- a/basic_autonomy/test/test_waypoint_generation.cpp +++ b/basic_autonomy/test/test_waypoint_generation.cpp @@ -860,8 +860,8 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) carma_planning_msgs::msg::TrajectoryPlan tp; tp.trajectory_points = trajectory_points; - bool res = basic_autonomy::waypoint_generation::validate_yield_plan(node, tp); - ASSERT_TRUE(basic_autonomy::waypoint_generation::validate_yield_plan(node, tp)); + bool res = basic_autonomy::waypoint_generation::is_valid_yield_plan(node, tp); + ASSERT_TRUE(basic_autonomy::waypoint_generation::is_valid_yield_plan(node, tp)); carma_planning_msgs::msg::TrajectoryPlan tp2; @@ -872,7 +872,7 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) point_4.lane_id = "1"; tp2.trajectory_points.push_back(point_4); - ASSERT_FALSE(basic_autonomy::waypoint_generation::validate_yield_plan(node, tp2)); + ASSERT_FALSE(basic_autonomy::waypoint_generation::is_valid_yield_plan(node, tp2)); carma_planning_msgs::msg::TrajectoryPlan tp3; @@ -890,7 +890,7 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) point_6.lane_id = "1"; tp3.trajectory_points.push_back(point_6); - ASSERT_FALSE(basic_autonomy::waypoint_generation::validate_yield_plan(node, tp2)); + ASSERT_FALSE(basic_autonomy::waypoint_generation::is_valid_yield_plan(node, tp2)); } From fe614929624704313a5c5ae29a10f859b6c46013 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Tue, 5 Dec 2023 16:57:16 +0000 Subject: [PATCH 17/18] STREAM to LOG --- inlanecruising_plugin/src/inlanecruising_plugin.cpp | 2 +- inlanecruising_plugin/src/inlanecruising_plugin_node.cpp | 2 +- .../src/light_controlled_intersection_tactical_plugin.cpp | 2 +- .../src/light_controlled_intersection_tactical_plugin_node.cpp | 2 +- stop_and_wait_plugin/src/stop_and_wait_plugin.cpp | 2 +- stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/inlanecruising_plugin/src/inlanecruising_plugin.cpp b/inlanecruising_plugin/src/inlanecruising_plugin.cpp index b732516d19..2ff6c6ee00 100644 --- a/inlanecruising_plugin/src/inlanecruising_plugin.cpp +++ b/inlanecruising_plugin/src/inlanecruising_plugin.cpp @@ -116,7 +116,7 @@ void InLaneCruisingPlugin::plan_trajectory_callback( } else { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Ignored Object Avoidance"); + RCLCPP_DEBUG(nh_->get_logger(), "Ignored Object Avoidance"); } if (config_.publish_debug) { // Publish the debug message if in debug logging mode diff --git a/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp b/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp index e2a257ee92..64672ade24 100644 --- a/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp +++ b/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp @@ -89,7 +89,7 @@ namespace inlanecruising_plugin //TODO: Update yield client to use the Plugin Manager capabilities query, in case someone else wants to add an alternate yield implementation yield_client_ = create_client("yield_plugin/plan_trajectory"); worker_->set_yield_client(yield_client_); - RCLCPP_INFO_STREAM(rclcpp::get_logger("inlanecruising_plugin"), "Yield Client Set"); + RCLCPP_INFO(rclcpp::get_logger("inlanecruising_plugin"), "Yield Client Set"); // Return success if everything initialized successfully return CallbackReturn::SUCCESS; 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 9f8252983b..83f84c7ef7 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 @@ -249,7 +249,7 @@ namespace light_controlled_intersection_tactical_plugin } else { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Ignored Object Avoidance"); + RCLCPP_DEBUG(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Ignored Object Avoidance"); } resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp index 940094d328..6594b4f5c1 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp @@ -129,7 +129,7 @@ namespace light_controlled_intersection_tactical_plugin yield_client_ = create_client("yield_plugin/plan_trajectory"); worker_->set_yield_client(yield_client_); - RCLCPP_INFO_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Yield Client Set"); + RCLCPP_INFO(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Yield Client Set"); // Return success if everything initialized successfully return CallbackReturn::SUCCESS; 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 06799e0454..9cc9a55cf7 100644 --- a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp @@ -154,7 +154,7 @@ bool StopandWait::plan_trajectory_cb(carma_planning_msgs::srv::PlanTrajectory::R } else { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_wait_plugin"), "Ignored Object Avoidance"); + RCLCPP_DEBUG(rclcpp::get_logger("stop_and_wait_plugin"), "Ignored Object Avoidance"); } std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); // Planning complete diff --git a/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp b/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp index 3773269525..3e90dc1050 100644 --- a/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp @@ -83,7 +83,7 @@ namespace stop_and_wait_plugin yield_client_ = create_client("yield_plugin/plan_trajectory"); plugin_->set_yield_client(yield_client_); - RCLCPP_INFO_STREAM(rclcpp::get_logger("stop_and_wait_plugin"), "Yield Client Set"); + RCLCPP_INFO(rclcpp::get_logger("stop_and_wait_plugin"), "Yield Client Set"); // Return success if everything initialized successfully return CallbackReturn::SUCCESS; From e69fe10b3bfdaf04c814294c59a4827554be081c Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Tue, 5 Dec 2023 18:16:57 +0000 Subject: [PATCH 18/18] address comments --- .../include/basic_autonomy/basic_autonomy.hpp | 10 ++++++---- basic_autonomy/src/basic_autonomy.cpp | 10 +++++----- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp b/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp index ca1c2b7d60..2cb8abb8dc 100644 --- a/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp +++ b/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp @@ -379,9 +379,11 @@ namespace basic_autonomy * \return The original response modified to contain the modified planned trajectory */ carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr modify_trajectory_to_yield_to_obstacles( - std::shared_ptr node_handler, - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp, - carma_ros2_utils::ClientPtr yield_client, int yield_plugin_service_call_timeout); + const std::shared_ptr& node_handler, + const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr& req, + const carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr& resp, + const carma_ros2_utils::ClientPtr& yield_client, + int yield_plugin_service_call_timeout); /** * \brief Helper function to verify if the input yield trajectory plan is valid @@ -390,7 +392,7 @@ namespace basic_autonomy * * \return true or false */ - bool is_valid_yield_plan(std::shared_ptr node_handler, const carma_planning_msgs::msg::TrajectoryPlan& yield_plan); + bool is_valid_yield_plan(const std::shared_ptr& node_handler, const carma_planning_msgs::msg::TrajectoryPlan& yield_plan); } } // basic_autonomy \ No newline at end of file diff --git a/basic_autonomy/src/basic_autonomy.cpp b/basic_autonomy/src/basic_autonomy.cpp index 1d5cc5e441..7174a825c0 100644 --- a/basic_autonomy/src/basic_autonomy.cpp +++ b/basic_autonomy/src/basic_autonomy.cpp @@ -1276,7 +1276,7 @@ namespace basic_autonomy return output; } - bool is_valid_yield_plan(const std::shared_ptr node_handler, const carma_planning_msgs::msg::TrajectoryPlan& yield_plan) + bool is_valid_yield_plan(const std::shared_ptr& node_handler, const carma_planning_msgs::msg::TrajectoryPlan& yield_plan) { if (yield_plan.trajectory_points.size() < 2) { @@ -1299,10 +1299,10 @@ namespace basic_autonomy } carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr modify_trajectory_to_yield_to_obstacles( - const std::shared_ptr node_handler, - const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, - const carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp, - const carma_ros2_utils::ClientPtr yield_client, + const std::shared_ptr& node_handler, + const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr& req, + const carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr& resp, + const carma_ros2_utils::ClientPtr& yield_client, int yield_plugin_service_call_timeout) { RCLCPP_DEBUG(node_handler->get_logger(), "Activate Object Avoidance");