diff --git a/LibCarla/source/carla/rss/RssCheck.cpp b/LibCarla/source/carla/rss/RssCheck.cpp index 17352a9c7..c478320de 100644 --- a/LibCarla/source/carla/rss/RssCheck.cpp +++ b/LibCarla/source/carla/rss/RssCheck.cpp @@ -5,9 +5,11 @@ #include "carla/rss/RssCheck.h" +#include +#include +#include #include #include -#include #include #include #include @@ -19,8 +21,6 @@ #include #include #include -#include -#include #include #include "carla/client/Map.h" @@ -33,40 +33,42 @@ namespace carla { namespace rss { -void printRoute(std::string const &routeDescr, ::ad::map::route::FullRoute const &route) { - std::cout << routeDescr << std::endl; - for ( auto roadSegment: route.roadSegments) { - for (auto laneSegment: roadSegment.drivableLaneSegments) { - std::cout << "(" << static_cast(laneSegment.laneInterval.laneId) << " | " << std::setprecision(2) << static_cast(laneSegment.laneInterval.start) << ":" << static_cast(laneSegment.laneInterval.end) << ") "; +void printRoute(std::string const &route_descr, + ::ad::map::route::FullRoute const &route) { + std::cout << route_descr << std::endl; + for (auto road_segment : route.roadSegments) { + for (auto lane_segment : road_segment.drivableLaneSegments) { + std::cout << "(" << static_cast(lane_segment.laneInterval.laneId) + << " | " << std::setprecision(2) + << static_cast(lane_segment.laneInterval.start) << ":" + << static_cast(lane_segment.laneInterval.end) << ") "; } std::cout << std::endl; } } // constants for deg-> rad conversion PI / 180 -constexpr float toRadians = static_cast(M_PI) / 180.0f; +constexpr float to_radians = static_cast(M_PI) / 180.0f; EgoDynamicsOnRoute::EgoDynamicsOnRoute() - : ego_speed(0.) - , route_heading(0.) - , route_speed_lat(0.) - , route_speed_lon(0.) - , route_accel_lat(0.) - , route_accel_lon(0.) - , avg_route_accel_lat(0.) - , avg_route_accel_lon(0.) { + : ego_speed(0.), + route_heading(0.), + route_speed_lat(0.), + route_speed_lon(0.), + route_accel_lat(0.), + route_accel_lon(0.), + avg_route_accel_lat(0.), + avg_route_accel_lon(0.) { timestamp.elapsed_seconds = 0.; } -RssCheck::RssCheck(): - _ego_vehicle_dynamics(GetDefaultVehicleDynamics()), - _other_vehicle_dynamics(GetDefaultVehicleDynamics()), - _road_boundaries_mode(RoadBoundariesMode::Off), - _visualization_mode(VisualizationMode::All) { - +RssCheck::RssCheck() + : _ego_vehicle_dynamics(GetDefaultVehicleDynamics()), + _other_vehicle_dynamics(GetDefaultVehicleDynamics()), + _road_boundaries_mode(RoadBoundariesMode::Off), + _visualization_mode(VisualizationMode::All) { _logger = spdlog::get("RssCheck"); - if (!_logger) - { + if (!_logger) { _logger = spdlog::stdout_color_mt("RssCheck"); } spdlog::set_level(spdlog::level::warn); @@ -75,16 +77,13 @@ RssCheck::RssCheck(): ::ad::rss::map::getLogger()->set_level(spdlog::level::warn); _timing_logger = spdlog::get("RssCheckTiming"); - if (!_timing_logger) - { + if (!_timing_logger) { _timing_logger = spdlog::stdout_color_mt("RssCheckTiming"); } _timing_logger->set_level(spdlog::level::off); } -RssCheck::~RssCheck() -{ -} +RssCheck::~RssCheck() {} ::ad::rss::world::RssDynamics RssCheck::GetDefaultVehicleDynamics() const { ::ad::rss::world::RssDynamics default_dynamics; @@ -100,38 +99,44 @@ RssCheck::~RssCheck() return default_dynamics; } -const ::ad::rss::world::RssDynamics& RssCheck::GetEgoVehicleDynamics() const { +const ::ad::rss::world::RssDynamics &RssCheck::GetEgoVehicleDynamics() const { return _ego_vehicle_dynamics; } -void RssCheck::SetEgoVehicleDynamics(const ::ad::rss::world::RssDynamics &ego_vehicle_dynamics) { +void RssCheck::SetEgoVehicleDynamics( + const ::ad::rss::world::RssDynamics &ego_vehicle_dynamics) { _ego_vehicle_dynamics = ego_vehicle_dynamics; } -const ::ad::rss::world::RssDynamics& RssCheck::GetOtherVehicleDynamics() const { +const ::ad::rss::world::RssDynamics &RssCheck::GetOtherVehicleDynamics() const { return _other_vehicle_dynamics; } -void RssCheck::SetOtherVehicleDynamics(const ::ad::rss::world::RssDynamics &other_vehicle_dynamics) { +void RssCheck::SetOtherVehicleDynamics( + const ::ad::rss::world::RssDynamics &other_vehicle_dynamics) { _other_vehicle_dynamics = other_vehicle_dynamics; } -const ::carla::rss::RoadBoundariesMode& RssCheck::GetRoadBoundariesMode() const { +const ::carla::rss::RoadBoundariesMode &RssCheck::GetRoadBoundariesMode() + const { return _road_boundaries_mode; } -void RssCheck::SetRoadBoundariesMode(const ::carla::rss::RoadBoundariesMode &road_boundaries_mode) { +void RssCheck::SetRoadBoundariesMode( + const ::carla::rss::RoadBoundariesMode &road_boundaries_mode) { _road_boundaries_mode = road_boundaries_mode; } -void RssCheck::AppendRoutingTarget(::carla::geom::Transform const &routing_target) { - _routing_targets_to_append.push_back(::ad::map::point::createENUPoint(routing_target.location.x, -1. * routing_target.location.y, 0.)); +void RssCheck::AppendRoutingTarget( + ::carla::geom::Transform const &routing_target) { + _routing_targets_to_append.push_back(::ad::map::point::createENUPoint( + routing_target.location.x, -1. * routing_target.location.y, 0.)); } -const std::vector<::carla::geom::Transform> RssCheck::GetRoutingTargets() const { +const std::vector<::carla::geom::Transform> RssCheck::GetRoutingTargets() + const { std::vector<::carla::geom::Transform> routing_targets; - if ( withinValidInputRange(_routing_targets) ) - { + if (withinValidInputRange(_routing_targets)) { for (auto const &target : _routing_targets) { ::carla::geom::Transform routing_target; routing_target.location.x = static_cast(target.x); @@ -148,12 +153,12 @@ void RssCheck::ResetRoutingTargets() { _routing_targets_to_append.clear(); } - -void RssCheck::SetVisualizationMode(const ::carla::rss::VisualizationMode &visualization_mode) { +void RssCheck::SetVisualizationMode( + const ::carla::rss::VisualizationMode &visualization_mode) { _visualization_mode = visualization_mode; } -const ::carla::rss::VisualizationMode& RssCheck::GetVisualizationMode() const { +const ::carla::rss::VisualizationMode &RssCheck::GetVisualizationMode() const { return _visualization_mode; } @@ -162,39 +167,57 @@ void RssCheck::DropRoute() { _carla_rss_state.ego_route = ::ad::map::route::FullRoute(); } -bool RssCheck::CheckObjects(carla::client::Timestamp const ×tamp, - carla::SharedPtr const &actors, - carla::SharedPtr const &carla_ego_actor, - ::ad::rss::state::ProperResponse &output_response, - ::ad::rss::world::AccelerationRestriction &output_acceleration_restriction, - ::ad::rss::state::RssStateSnapshot &output_rss_state_snapshot, - EgoDynamicsOnRoute &output_rss_ego_dynamics_on_route) { +bool RssCheck::CheckObjects( + carla::client::Timestamp const ×tamp, + carla::SharedPtr const &actors, + carla::SharedPtr const &carla_ego_actor, + ::ad::rss::state::ProperResponse &output_response, + ::ad::rss::world::AccelerationRestriction &output_acceleration_restriction, + ::ad::rss::state::RssStateSnapshot &output_rss_state_snapshot, + EgoDynamicsOnRoute &output_rss_ego_dynamics_on_route) { try { - double const time_since_epoch_check_start_ms = std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(); + double const time_since_epoch_check_start_ms = + std::chrono::duration( + std::chrono::system_clock::now().time_since_epoch()) + .count(); #if DEBUG_TIMING - std::cout << "--- time: " << timestamp.frame << ", " << timestamp.elapsed_seconds << std::endl; - auto tStart = std::chrono::high_resolution_clock::now(); - auto tEnd = std::chrono::high_resolution_clock::now(); - std::cout << "-> SC " << std::chrono::duration(tEnd-tStart).count() << " start checkObjects" << std::endl; + std::cout << "--- time: " << timestamp.frame << ", " + << timestamp.elapsed_seconds << std::endl; + auto t_start = std::chrono::high_resolution_clock::now(); + auto t_end = std::chrono::high_resolution_clock::now(); + std::cout + << "-> SC " + << std::chrono::duration(t_end - t_start).count() + << " start checkObjects" << std::endl; #endif - const auto carla_ego_vehicle = boost::dynamic_pointer_cast(carla_ego_actor); + const auto carla_ego_vehicle = + boost::dynamic_pointer_cast(carla_ego_actor); #if DEBUG_TIMING - tEnd = std::chrono::high_resolution_clock::now(); - std::cout << "-> ME " << std::chrono::duration(tEnd-tStart).count() << " before MapMatching" << std::endl; + t_end = std::chrono::high_resolution_clock::now(); + std::cout + << "-> ME " + << std::chrono::duration(t_end - t_start).count() + << " before MapMatching" << std::endl; #endif - // allow the vehicle to be at least 2.0 m away form the route to not lose the contact to the route - auto const ego_match_object = GetMatchObject(*carla_ego_vehicle, ::ad::physics::Distance(2.0)); + // allow the vehicle to be at least 2.0 m away form the route to not lose + // the contact to the route + auto const ego_match_object = + GetMatchObject(*carla_ego_vehicle, ::ad::physics::Distance(2.0)); - if ( ::ad::map::point::isValid(_carla_rss_state.ego_match_object.enuPosition.centerPoint, false) ) - { + if (::ad::map::point::isValid( + _carla_rss_state.ego_match_object.enuPosition.centerPoint, false)) { // check for bigger position jumps of the ego vehicle - auto const travelled_distance = ::ad::map::point::distance(_carla_rss_state.ego_match_object.enuPosition.centerPoint, ego_match_object.enuPosition.centerPoint); - if ( travelled_distance > ::ad::physics::Distance(10.) ) - { - _logger->warn("Jump in ego vehicle position detected {} -> {}! Force reroute!", _carla_rss_state.ego_match_object.enuPosition.centerPoint, ego_match_object.enuPosition.centerPoint); + auto const travelled_distance = ::ad::map::point::distance( + _carla_rss_state.ego_match_object.enuPosition.centerPoint, + ego_match_object.enuPosition.centerPoint); + if (travelled_distance > ::ad::physics::Distance(10.)) { + _logger->warn( + "Jump in ego vehicle position detected {} -> {}! Force reroute!", + _carla_rss_state.ego_match_object.enuPosition.centerPoint, + ego_match_object.enuPosition.centerPoint); DropRoute(); } } @@ -204,205 +227,264 @@ bool RssCheck::CheckObjects(carla::client::Timestamp const ×tamp, _logger->debug("MapMatch:: {}", _carla_rss_state.ego_match_object); #if DEBUG_TIMING - tEnd = std::chrono::high_resolution_clock::now(); - std::cout << "-> ME " << std::chrono::duration(tEnd-tStart).count() << " after ego MapMatching" << std::endl; + t_end = std::chrono::high_resolution_clock::now(); + std::cout + << "-> ME " + << std::chrono::duration(t_end - t_start).count() + << " after ego MapMatching" << std::endl; #endif UpdateRoute(_carla_rss_state); #if DEBUG_TIMING - tEnd = std::chrono::high_resolution_clock::now(); - std::cout << "-> RU " << std::chrono::duration(tEnd-tStart).count() << " after route update " << std::endl; + t_end = std::chrono::high_resolution_clock::now(); + std::cout + << "-> RU " + << std::chrono::duration(t_end - t_start).count() + << " after route update " << std::endl; #endif - _carla_rss_state.ego_dynamics_on_route = CalculateEgoDynamicsOnRoute(timestamp, - time_since_epoch_check_start_ms, - *carla_ego_vehicle, - _carla_rss_state.ego_match_object, - _carla_rss_state.ego_route, - _carla_rss_state.ego_dynamics_on_route); + _carla_rss_state.ego_dynamics_on_route = CalculateEgoDynamicsOnRoute( + timestamp, time_since_epoch_check_start_ms, *carla_ego_vehicle, + _carla_rss_state.ego_match_object, _carla_rss_state.ego_route, + _carla_rss_state.ego_dynamics_on_route); CreateWorldModel(timestamp, *actors, *carla_ego_vehicle, _carla_rss_state); #if DEBUG_TIMING - tEnd = std::chrono::high_resolution_clock::now(); - std::cout << "-> WM " << std::chrono::duration(tEnd-tStart).count() << " after create world model " << std::endl; + t_end = std::chrono::high_resolution_clock::now(); + std::cout + << "-> WM " + << std::chrono::duration(t_end - t_start).count() + << " after create world model " << std::endl; #endif PerformCheck(_carla_rss_state); #if DEBUG_TIMING - tEnd = std::chrono::high_resolution_clock::now(); - std::cout << "-> CH " << std::chrono::duration(tEnd-tStart).count() << " end RSS check" << std::endl; + t_end = std::chrono::high_resolution_clock::now(); + std::cout + << "-> CH " + << std::chrono::duration(t_end - t_start).count() + << " end RSS check" << std::endl; #endif AnalyseCheckResults(_carla_rss_state); #if DEBUG_TIMING - tEnd = std::chrono::high_resolution_clock::now(); - std::cout << "-> VI " << std::chrono::duration(tEnd-tStart).count() << " start store viz" << std::endl; + t_end = std::chrono::high_resolution_clock::now(); + std::cout + << "-> VI " + << std::chrono::duration(t_end - t_start).count() + << " start store viz" << std::endl; #endif StoreVisualizationResults(_carla_rss_state); #if DEBUG_TIMING - tEnd = std::chrono::high_resolution_clock::now(); - std::cout << "-> VI " << std::chrono::duration(tEnd-tStart).count() << " end store viz" << std::endl; + t_end = std::chrono::high_resolution_clock::now(); + std::cout + << "-> VI " + << std::chrono::duration(t_end - t_start).count() + << " end store viz" << std::endl; #endif - _carla_rss_state.ego_dynamics_on_route.time_since_epoch_check_end_ms = std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(); + _carla_rss_state.ego_dynamics_on_route.time_since_epoch_check_end_ms = + std::chrono::duration( + std::chrono::system_clock::now().time_since_epoch()) + .count(); - //store result + // store result output_response = _carla_rss_state.proper_response; output_acceleration_restriction = _carla_rss_state.acceleration_restriction; output_rss_state_snapshot = _carla_rss_state.rss_state_snapshot; output_rss_ego_dynamics_on_route = _carla_rss_state.ego_dynamics_on_route; if (_carla_rss_state.dangerous_state) { _logger->debug("===== ROUTE NOT SAFE ====="); - } - else { + } else { _logger->debug("===== ROUTE SAFE ====="); } #if DEBUG_TIMING - tEnd = std::chrono::high_resolution_clock::now(); - std::cout << "-> EC " << std::chrono::duration(tEnd-tStart).count() << " end check objects" << std::endl; + t_end = std::chrono::high_resolution_clock::now(); + std::cout + << "-> EC " + << std::chrono::duration(t_end - t_start).count() + << " end check objects" << std::endl; #endif return true; - } - catch (...) { + } catch (...) { _logger->error("Exception -> Check failed"); return false; } } -::ad::map::match::Object RssCheck::GetMatchObject(carla::client::Vehicle const &carla_vehicle, ::ad::physics::Distance const &match_distance) const { - +::ad::map::match::Object RssCheck::GetMatchObject( + carla::client::Vehicle const &carla_vehicle, + ::ad::physics::Distance const &match_distance) const { ::ad::map::match::Object match_object; auto const vehicle_transform = carla_vehicle.GetTransform(); - match_object.enuPosition.centerPoint.x = ::ad::map::point::ENUCoordinate(vehicle_transform.location.x); - match_object.enuPosition.centerPoint.y = ::ad::map::point::ENUCoordinate(-1 * vehicle_transform.location.y); - match_object.enuPosition.centerPoint.z = ::ad::map::point::ENUCoordinate(0); //vehicle_transform.location.z; - match_object.enuPosition.heading = ::ad::map::point::createENUHeading((-1 * vehicle_transform.rotation.yaw) * toRadians); + match_object.enuPosition.centerPoint.x = + ::ad::map::point::ENUCoordinate(vehicle_transform.location.x); + match_object.enuPosition.centerPoint.y = + ::ad::map::point::ENUCoordinate(-1 * vehicle_transform.location.y); + match_object.enuPosition.centerPoint.z = + ::ad::map::point::ENUCoordinate(0); // vehicle_transform.location.z; + match_object.enuPosition.heading = ::ad::map::point::createENUHeading( + (-1 * vehicle_transform.rotation.yaw) * to_radians); const auto &bounding_box = carla_vehicle.GetBoundingBox(); - match_object.enuPosition.dimension.length = ::ad::physics::Distance(2 * bounding_box.extent.x); - match_object.enuPosition.dimension.width = ::ad::physics::Distance(2 * bounding_box.extent.y); - match_object.enuPosition.dimension.height = ::ad::physics::Distance(2 * bounding_box.extent.z); - match_object.enuPosition.enuReferencePoint = ::ad::map::access::getENUReferencePoint(); + match_object.enuPosition.dimension.length = + ::ad::physics::Distance(2 * bounding_box.extent.x); + match_object.enuPosition.dimension.width = + ::ad::physics::Distance(2 * bounding_box.extent.y); + match_object.enuPosition.dimension.height = + ::ad::physics::Distance(2 * bounding_box.extent.z); + match_object.enuPosition.enuReferencePoint = + ::ad::map::access::getENUReferencePoint(); ::ad::map::match::AdMapMatching map_matching; - match_object.mapMatchedBoundingBox = map_matching.getMapMatchedBoundingBox(match_object.enuPosition, match_distance, ::ad::physics::Distance(2.)); + match_object.mapMatchedBoundingBox = map_matching.getMapMatchedBoundingBox( + match_object.enuPosition, match_distance, ::ad::physics::Distance(2.)); return match_object; } -::ad::physics::Speed RssCheck::GetSpeed(carla::client::Vehicle const &carla_vehicle) const { +::ad::physics::Speed RssCheck::GetSpeed( + carla::client::Vehicle const &carla_vehicle) const { auto const velocity = carla_vehicle.GetVelocity(); - ::ad::physics::Speed speed(std::sqrt(velocity.x * velocity.x + velocity.y * velocity.y)); + ::ad::physics::Speed speed( + std::sqrt(velocity.x * velocity.x + velocity.y * velocity.y)); return speed; } void RssCheck::UpdateRoute(CarlaRssState &carla_rss_state) { + _logger->debug("Update route start: {}", carla_rss_state.ego_route); - _logger->debug("Update route start: {}", carla_rss_state.ego_route); - - // remove the parts of the route already taken, try to prepend route sections (i.e. when driving backwards) - // try to ensure that the back of the vehicle is still within the route to support orientation calculation + // remove the parts of the route already taken, try to prepend route sections + // (i.e. when driving backwards) + // try to ensure that the back of the vehicle is still within the route to + // support orientation calculation ::ad::map::point::ParaPointList all_lane_matches; - for (auto reference_point: {::ad::map::match::ObjectReferencePoints::RearRight, ::ad::map::match::ObjectReferencePoints::RearLeft}) { - auto const &reference_position = carla_rss_state.ego_match_object.mapMatchedBoundingBox.referencePointPositions[size_t(reference_point)]; - auto const para_points = ::ad::map::match::getParaPoints(reference_position); - all_lane_matches.insert(all_lane_matches.end(), para_points.begin(), para_points.end()); + for (auto reference_point : + {::ad::map::match::ObjectReferencePoints::RearRight, + ::ad::map::match::ObjectReferencePoints::RearLeft}) { + auto const &reference_position = + carla_rss_state.ego_match_object.mapMatchedBoundingBox + .referencePointPositions[size_t(reference_point)]; + auto const para_points = + ::ad::map::match::getParaPoints(reference_position); + all_lane_matches.insert(all_lane_matches.end(), para_points.begin(), + para_points.end()); } - auto shorten_route_result = ::ad::map::route::shortenRoute(all_lane_matches, carla_rss_state.ego_route, - ::ad::map::route::ShortenRouteMode::DontCutIntersectionAndPrependIfSucceededBeforeRoute); - if ( shorten_route_result == ::ad::map::route::ShortenRouteResult::SucceededIntersectionNotCut ) - { + auto shorten_route_result = ::ad::map::route::shortenRoute( + all_lane_matches, carla_rss_state.ego_route, + ::ad::map::route::ShortenRouteMode:: + DontCutIntersectionAndPrependIfSucceededBeforeRoute); + if (shorten_route_result == + ::ad::map::route::ShortenRouteResult::SucceededIntersectionNotCut) { shorten_route_result = ::ad::map::route::ShortenRouteResult::Succeeded; } bool routing_target_check_finished = false; while ((!_routing_targets.empty()) && (!routing_target_check_finished)) { auto const next_target = _routing_targets.front(); - auto const &distance_to_next_target = ::ad::map::point::distance(next_target, carla_rss_state.ego_match_object.enuPosition.centerPoint); - if ( distance_to_next_target < ::ad::physics::Distance(3.) ) { + auto const &distance_to_next_target = ::ad::map::point::distance( + next_target, carla_rss_state.ego_match_object.enuPosition.centerPoint); + if (distance_to_next_target < ::ad::physics::Distance(3.)) { _routing_targets.erase(_routing_targets.begin()); - _logger->debug("Next target reached: {}; remaining targets: {}", next_target, _routing_targets); - } - else { + _logger->debug("Next target reached: {}; remaining targets: {}", + next_target, _routing_targets); + } else { routing_target_check_finished = true; } } bool reroute_required = false; - if ( !_routing_targets_to_append.empty() ) { + if (!_routing_targets_to_append.empty()) { reroute_required = true; - _routing_targets.insert(_routing_targets.end(), _routing_targets_to_append.begin(), _routing_targets_to_append.end()); - _logger->debug("Appending new routing targets: {}; resulting targets: {}", _routing_targets_to_append, _routing_targets); + _routing_targets.insert(_routing_targets.end(), + _routing_targets_to_append.begin(), + _routing_targets_to_append.end()); + _logger->debug("Appending new routing targets: {}; resulting targets: {}", + _routing_targets_to_append, _routing_targets); _routing_targets_to_append.clear(); } ::ad::physics::Distance const route_target_length(50.); - if ((!reroute_required) && (shorten_route_result == ::ad::map::route::ShortenRouteResult::Succeeded)) { + if ((!reroute_required) && + (shorten_route_result == + ::ad::map::route::ShortenRouteResult::Succeeded)) { std::vector<::ad::map::route::FullRoute> additional_routes; - auto const route_valid = ::ad::map::route::extendRouteToDistance(carla_rss_state.ego_route, route_target_length, - additional_routes); + auto const route_valid = ::ad::map::route::extendRouteToDistance( + carla_rss_state.ego_route, route_target_length, additional_routes); if (route_valid) { if (additional_routes.size() > 0u) { // take a random extension to the route - std::size_t route_index = static_cast(std::rand()) % (additional_routes.size() + 1); - if ( route_index < additional_routes.size() ) { + std::size_t route_index = static_cast(std::rand()) % + (additional_routes.size() + 1); + if (route_index < additional_routes.size()) { // we decided for one of the additional routes - _logger->debug("Additional Routes: {}->{}", additional_routes.size(), route_index); + _logger->debug("Additional Routes: {}->{}", additional_routes.size(), + route_index); carla_rss_state.ego_route = additional_routes[route_index]; - } - else { + } else { // we decided for the extension within route, nothing to be done _logger->debug("Additional Routes: expand current"); } } - } - else { + } else { reroute_required = true; } - } - else { + } else { // on all other results we recreate the route reroute_required = true; } - //create the route if required + // create the route if required if (reroute_required) { - //try to create routes + // try to create routes std::vector<::ad::map::route::FullRoute> all_new_routes; - for (const auto &position : carla_rss_state.ego_match_object.mapMatchedBoundingBox.referencePointPositions[int32_t(::ad::map::match::ObjectReferencePoints::Center)]) { + for (const auto &position : + carla_rss_state.ego_match_object.mapMatchedBoundingBox + .referencePointPositions[int32_t( + ::ad::map::match::ObjectReferencePoints::Center)]) { auto start_point = position.lanePoint.paraPoint; auto projected_start_point = start_point; - if (!::ad::map::lane::isHeadingInLaneDirection(start_point, carla_rss_state.ego_match_object.enuPosition.heading)) { + if (!::ad::map::lane::isHeadingInLaneDirection( + start_point, + carla_rss_state.ego_match_object.enuPosition.heading)) { _logger->debug("EgoVehicle heading in opposite lane direction"); - if (::ad::map::lane::projectPositionToLaneInHeadingDirection(start_point, carla_rss_state.ego_match_object.enuPosition.heading, projected_start_point)) { + if (::ad::map::lane::projectPositionToLaneInHeadingDirection( + start_point, + carla_rss_state.ego_match_object.enuPosition.heading, + projected_start_point)) { _logger->debug("Projected to lane {}", projected_start_point.laneId); } } - _logger->debug("Route start_point: {}, projected_start_point: {}", start_point, projected_start_point); - auto routing_start_point = ::ad::map::route::planning::createRoutingPoint(projected_start_point, carla_rss_state.ego_match_object.enuPosition.heading); - if (!_routing_targets.empty() && ::ad::map::point::isValid(_routing_targets)) { + _logger->debug("Route start_point: {}, projected_start_point: {}", + start_point, projected_start_point); + auto routing_start_point = ::ad::map::route::planning::createRoutingPoint( + projected_start_point, + carla_rss_state.ego_match_object.enuPosition.heading); + if (!_routing_targets.empty() && + ::ad::map::point::isValid(_routing_targets)) { auto new_route = ::ad::map::route::planning::planRoute( - routing_start_point, _routing_targets, ::ad::map::route::RouteCreationMode::AllRoutableLanes); + routing_start_point, _routing_targets, + ::ad::map::route::RouteCreationMode::AllRoutableLanes); all_new_routes.push_back(new_route); - } - else { + } else { auto new_routes = ::ad::map::route::planning::predictRoutesOnDistance( - routing_start_point,route_target_length, ::ad::map::route::RouteCreationMode::AllRoutableLanes); + routing_start_point, route_target_length, + ::ad::map::route::RouteCreationMode::AllRoutableLanes); - for (const auto & new_route : new_routes) { - //extend route with all lanes + for (const auto &new_route : new_routes) { + // extend route with all lanes all_new_routes.push_back(new_route); } } @@ -410,87 +492,114 @@ void RssCheck::UpdateRoute(CarlaRssState &carla_rss_state) { _logger->debug("New routes: {}", all_new_routes.size()); - if ( !all_new_routes.empty() ) { + if (!all_new_routes.empty()) { // take a random route - std::size_t route_index = static_cast(std::rand()) % (all_new_routes.size()); + std::size_t route_index = + static_cast(std::rand()) % (all_new_routes.size()); carla_rss_state.ego_route = all_new_routes[route_index]; } } - _logger->debug("Update route result: {}", carla_rss_state.ego_route); + _logger->debug("Update route result: {}", carla_rss_state.ego_route); } -EgoDynamicsOnRoute RssCheck::CalculateEgoDynamicsOnRoute(carla::client::Timestamp const ¤t_timestamp, - double const &time_since_epoch_check_start_ms, - carla::client::Vehicle const &carla_vehicle, - ::ad::map::match::Object match_object, - ::ad::map::route::FullRoute const &route, - EgoDynamicsOnRoute const &last_dynamics) const -{ +EgoDynamicsOnRoute RssCheck::CalculateEgoDynamicsOnRoute( + carla::client::Timestamp const ¤t_timestamp, + double const &time_since_epoch_check_start_ms, + carla::client::Vehicle const &carla_vehicle, + ::ad::map::match::Object match_object, + ::ad::map::route::FullRoute const &route, + EgoDynamicsOnRoute const &last_dynamics) const { EgoDynamicsOnRoute new_dynamics; new_dynamics.timestamp = current_timestamp; - new_dynamics.time_since_epoch_check_start_ms = time_since_epoch_check_start_ms; + new_dynamics.time_since_epoch_check_start_ms = + time_since_epoch_check_start_ms; new_dynamics.ego_speed = GetSpeed(carla_vehicle); new_dynamics.ego_center = match_object.enuPosition.centerPoint; new_dynamics.ego_heading = match_object.enuPosition.heading; - auto object_route = ::ad::map::route::getRouteSection(match_object, route, ::ad::map::route::RouteSectionCreationMode::AllRouteLanes); + auto object_route = ::ad::map::route::getRouteSection( + match_object, route, + ::ad::map::route::RouteSectionCreationMode::AllRouteLanes); auto border = ::ad::map::route::getENUBorderOfRoute(object_route); StoreDebugVisualization(object_route, border); - new_dynamics.route_heading = ::ad::map::lane::getENUHeading(border, match_object.enuPosition.centerPoint); + new_dynamics.route_heading = ::ad::map::lane::getENUHeading( + border, match_object.enuPosition.centerPoint); - auto const object_center = ::ad::map::route::findCenterWaypoint(match_object, object_route); + auto const object_center = + ::ad::map::route::findCenterWaypoint(match_object, object_route); if (object_center.isValid()) { auto lane_center_point = object_center.queryPosition; - auto lane_center_point_enu = ::ad::map::lane::getENULanePoint(lane_center_point); - if ( std::fabs(new_dynamics.route_heading) > ::ad::map::point::ENUHeading(M_PI) ) - { - // if the actual center point is already outside, try to use this extended object center for the route heading calculation - new_dynamics.route_heading = ::ad::map::lane::getENUHeading(border, lane_center_point_enu); + auto lane_center_point_enu = + ::ad::map::lane::getENULanePoint(lane_center_point); + if (std::fabs(new_dynamics.route_heading) > + ::ad::map::point::ENUHeading(M_PI)) { + // if the actual center point is already outside, try to use this extended + // object center for the route heading calculation + new_dynamics.route_heading = + ::ad::map::lane::getENUHeading(border, lane_center_point_enu); } if (object_center.laneSegmentIterator->laneInterval.wrongWay) { - // driving on the wrong lane, so we have to project to nominal route direction - ::ad::map::lane::projectPositionToLaneInHeadingDirection(lane_center_point, new_dynamics.route_heading, lane_center_point); - lane_center_point_enu = ::ad::map::lane::getENULanePoint(lane_center_point); + // driving on the wrong lane, so we have to project to nominal route + // direction + ::ad::map::lane::projectPositionToLaneInHeadingDirection( + lane_center_point, new_dynamics.route_heading, lane_center_point); + lane_center_point_enu = + ::ad::map::lane::getENULanePoint(lane_center_point); } new_dynamics.route_nominal_center = lane_center_point_enu; - } - else { - // the ego vehicle is completely outside the route, so we can't update the values - new_dynamics.route_nominal_center = last_dynamics.route_nominal_center; - new_dynamics.route_heading = last_dynamics.route_heading; + } else { + // the ego vehicle is completely outside the route, so we can't update the + // values + new_dynamics.route_nominal_center = last_dynamics.route_nominal_center; + new_dynamics.route_heading = last_dynamics.route_heading; } - - new_dynamics.heading_diff = ::ad::map::point::normalizeENUHeading(new_dynamics.route_heading - new_dynamics.ego_heading); - new_dynamics.route_speed_lon = std::fabs(std::cos(static_cast(new_dynamics.heading_diff))) * new_dynamics.ego_speed; - new_dynamics.route_speed_lat = std::sin(static_cast(new_dynamics.heading_diff)) * new_dynamics.ego_speed; + new_dynamics.heading_diff = ::ad::map::point::normalizeENUHeading( + new_dynamics.route_heading - new_dynamics.ego_heading); + new_dynamics.route_speed_lon = + std::fabs(std::cos(static_cast(new_dynamics.heading_diff))) * + new_dynamics.ego_speed; + new_dynamics.route_speed_lat = + std::sin(static_cast(new_dynamics.heading_diff)) * + new_dynamics.ego_speed; bool keep_last_acceleration = true; - if ( last_dynamics.timestamp.elapsed_seconds > 0. ) { - ::ad::physics::Duration const delta_time(current_timestamp.elapsed_seconds - last_dynamics.timestamp.elapsed_seconds); - if ( delta_time > ::ad::physics::Duration(0.0001) ) { + if (last_dynamics.timestamp.elapsed_seconds > 0.) { + ::ad::physics::Duration const delta_time( + current_timestamp.elapsed_seconds - + last_dynamics.timestamp.elapsed_seconds); + if (delta_time > ::ad::physics::Duration(0.0001)) { try { - new_dynamics.route_accel_lat = (new_dynamics.route_speed_lat - last_dynamics.route_speed_lat)/delta_time; - new_dynamics.avg_route_accel_lat = ((last_dynamics.avg_route_accel_lat * 2.) + new_dynamics.route_accel_lat)/3.; - new_dynamics.route_accel_lon = (new_dynamics.route_speed_lon - last_dynamics.route_speed_lon)/delta_time; - new_dynamics.avg_route_accel_lon = ((last_dynamics.avg_route_accel_lon * 2.) + new_dynamics.route_accel_lon)/3.; + new_dynamics.route_accel_lat = + (new_dynamics.route_speed_lat - last_dynamics.route_speed_lat) / + delta_time; + new_dynamics.avg_route_accel_lat = + ((last_dynamics.avg_route_accel_lat * 2.) + + new_dynamics.route_accel_lat) / + 3.; + new_dynamics.route_accel_lon = + (new_dynamics.route_speed_lon - last_dynamics.route_speed_lon) / + delta_time; + new_dynamics.avg_route_accel_lon = + ((last_dynamics.avg_route_accel_lon * 2.) + + new_dynamics.route_accel_lon) / + 3.; - if ( new_dynamics.avg_route_accel_lat == ::ad::physics::Acceleration(0.) ) - { + if (new_dynamics.avg_route_accel_lat == + ::ad::physics::Acceleration(0.)) { // prevent from underrun new_dynamics.avg_route_accel_lat = ::ad::physics::Acceleration(0.); } - if ( new_dynamics.avg_route_accel_lon == ::ad::physics::Acceleration(0.) ) - { + if (new_dynamics.avg_route_accel_lon == + ::ad::physics::Acceleration(0.)) { // prevent from underrun new_dynamics.avg_route_accel_lon = ::ad::physics::Acceleration(0.); } keep_last_acceleration = false; - } - catch(...) { + } catch (...) { } } } @@ -502,52 +611,66 @@ EgoDynamicsOnRoute RssCheck::CalculateEgoDynamicsOnRoute(carla::client::Timestam new_dynamics.avg_route_accel_lon = last_dynamics.avg_route_accel_lon; } - // check if the center point (and only the center point) is still found on the route + // check if the center point (and only the center point) is still found on the + // route ::ad::map::point::ParaPointList in_lane_matches; - for (auto &match_position: match_object.mapMatchedBoundingBox.referencePointPositions[int32_t(::ad::map::match::ObjectReferencePoints::Center)]) { - if ( match_position.type == ::ad::map::match::MapMatchedPositionType::LANE_IN ) { + for (auto &match_position : + match_object.mapMatchedBoundingBox.referencePointPositions[int32_t( + ::ad::map::match::ObjectReferencePoints::Center)]) { + if (match_position.type == + ::ad::map::match::MapMatchedPositionType::LANE_IN) { in_lane_matches.push_back(match_position.lanePoint.paraPoint); } } - auto const object_in_lane_center = ::ad::map::route::findNearestWaypoint(in_lane_matches, route); + auto const object_in_lane_center = + ::ad::map::route::findNearestWaypoint(in_lane_matches, route); new_dynamics.ego_center_within_route = object_in_lane_center.isValid(); // evaluated by AnalyseResults new_dynamics.crossing_border = false; // calculate the ego stopping distance, to be able to reduce the effort - ad::rss::map::RssObjectConversion object_conversion(::ad::rss::world::ObjectId(0u), - ::ad::rss::world::ObjectType::EgoVehicle, - match_object, - new_dynamics.ego_speed, GetEgoVehicleDynamics()); - if ( !object_conversion.calculateMinStoppingDistance(new_dynamics.min_stopping_distance)) - { - _logger->error("CalculateEgoDynamicsOnRoute: calculation of min stopping distance failed. Setting to 100. ({} {} {})", - match_object, new_dynamics.ego_speed, GetEgoVehicleDynamics()); + ad::rss::map::RssObjectConversion object_conversion( + ::ad::rss::world::ObjectId(0u), ::ad::rss::world::ObjectType::EgoVehicle, + match_object, new_dynamics.ego_speed, GetEgoVehicleDynamics()); + if (!object_conversion.calculateMinStoppingDistance( + new_dynamics.min_stopping_distance)) { + _logger->error( + "CalculateEgoDynamicsOnRoute: calculation of min stopping distance " + "failed. Setting to 100. ({} {} {})", + match_object, new_dynamics.ego_speed, GetEgoVehicleDynamics()); new_dynamics.min_stopping_distance = ::ad::physics::Distance(100.); } - _logger->trace("CalculateEgoDynamicsOnRoute: route-section {} -> dynamics: {}", object_route, new_dynamics); + _logger->trace( + "CalculateEgoDynamicsOnRoute: route-section {} -> dynamics: {}", + object_route, new_dynamics); return new_dynamics; } - -::ad::map::landmark::LandmarkIdSet RssCheck::GetGreenTrafficLightsOnRoute(carla::client::ActorList const &actors, - ::ad::map::route::FullRoute const&route) const -{ +::ad::map::landmark::LandmarkIdSet RssCheck::GetGreenTrafficLightsOnRoute( + carla::client::ActorList const &actors, + ::ad::map::route::FullRoute const &route) const { ::ad::map::landmark::LandmarkIdSet green_traffic_lights; - auto next_intersection = ::ad::map::intersection::Intersection::getNextIntersectionOnRoute(route); - if ( next_intersection && (next_intersection->intersectionType() == ::ad::map::intersection::IntersectionType::TrafficLight) ) { - // try to guess the the relevant traffic light with the rule: nearest traffic light in respect to the incoming lane. - // @todo: when OpenDrive maps have the traffic lights incorporated, we only have to fill all green traffic lights into the green_traffic_lights list + auto next_intersection = + ::ad::map::intersection::Intersection::getNextIntersectionOnRoute(route); + if (next_intersection && + (next_intersection->intersectionType() == + ::ad::map::intersection::IntersectionType::TrafficLight)) { + // try to guess the the relevant traffic light with the rule: nearest + // traffic light in respect to the incoming lane. + // @todo: when OpenDrive maps have the traffic lights incorporated, we only + // have to fill all green traffic lights into the green_traffic_lights list auto incoming_lanes = next_intersection->incomingLanesOnRoute(); - // since our route spans the whole street, we have to filter out the incoming lanes with wrong way flag - auto incoming_lanes_iter=incoming_lanes.begin(); - while (incoming_lanes_iter!=incoming_lanes.end()) { - auto find_waypoint = ::ad::map::route::findWaypoint(*incoming_lanes_iter, route); - if (find_waypoint.isValid() && find_waypoint.laneSegmentIterator->laneInterval.wrongWay) { + // since our route spans the whole street, we have to filter out the + // incoming lanes with wrong way flag + auto incoming_lanes_iter = incoming_lanes.begin(); + while (incoming_lanes_iter != incoming_lanes.end()) { + auto find_waypoint = + ::ad::map::route::findWaypoint(*incoming_lanes_iter, route); + if (find_waypoint.isValid() && + find_waypoint.laneSegmentIterator->laneInterval.wrongWay) { incoming_lanes_iter = incoming_lanes.erase(incoming_lanes_iter); - } - else { + } else { incoming_lanes_iter++; } } @@ -555,57 +678,67 @@ EgoDynamicsOnRoute RssCheck::CalculateEgoDynamicsOnRoute(carla::client::Timestam ::ad::map::match::AdMapMatching traffic_light_map_matching; bool found_relevant_traffic_light = false; for (const auto &actor : actors) { - const auto traffic_light = boost::dynamic_pointer_cast(actor); + const auto traffic_light = + boost::dynamic_pointer_cast(actor); if (traffic_light == nullptr) { continue; } auto traffic_light_state = traffic_light->GetState(); - carla::geom::BoundingBox trigger_bounding_box = traffic_light->GetTriggerVolume(); + carla::geom::BoundingBox trigger_bounding_box = + traffic_light->GetTriggerVolume(); auto traffic_light_transform = traffic_light->GetTransform(); auto trigger_box_location = trigger_bounding_box.location; traffic_light_transform.TransformPoint(trigger_box_location); ::ad::map::point::ENUPoint trigger_box_position; - trigger_box_position.x = ::ad::map::point::ENUCoordinate(trigger_box_location.x); - trigger_box_position.y = ::ad::map::point::ENUCoordinate(-1 * trigger_box_location.y); + trigger_box_position.x = + ::ad::map::point::ENUCoordinate(trigger_box_location.x); + trigger_box_position.y = + ::ad::map::point::ENUCoordinate(-1 * trigger_box_location.y); trigger_box_position.z = ::ad::map::point::ENUCoordinate(0.); - auto traffic_light_map_matched_positions = traffic_light_map_matching.getMapMatchedPositions(trigger_box_position, ::ad::physics::Distance(0.25), ::ad::physics::Probability(0.1)); + auto traffic_light_map_matched_positions = + traffic_light_map_matching.getMapMatchedPositions( + trigger_box_position, ::ad::physics::Distance(0.25), + ::ad::physics::Probability(0.1)); - _logger->trace("traffic light map matched positions: {}, {}", trigger_box_position, traffic_light_map_matched_positions); - - for (auto matched_position: traffic_light_map_matched_positions) - { - if ( incoming_lanes.find(matched_position.lanePoint.paraPoint.laneId) != incoming_lanes.end() ) - { - if ( found_relevant_traffic_light && (green_traffic_lights.empty() && (traffic_light_state==carla::rpc::TrafficLightState::Green)) ) - { - _logger->warn("found another relevant traffic light on lane {}; {} state {}", - matched_position.lanePoint.paraPoint.laneId, - traffic_light->GetId(), - (traffic_light_state == carla::rpc::TrafficLightState::Green)?"green":"not green"); - } - else - { - _logger->debug("found relevant traffic light on lane {}; {} state {}", - matched_position.lanePoint.paraPoint.laneId, - traffic_light->GetId(), - (traffic_light_state == carla::rpc::TrafficLightState::Green)?"green":"not green"); + _logger->trace("traffic light map matched positions: {}, {}", + trigger_box_position, traffic_light_map_matched_positions); + for (auto matched_position : traffic_light_map_matched_positions) { + if (incoming_lanes.find(matched_position.lanePoint.paraPoint.laneId) != + incoming_lanes.end()) { + if (found_relevant_traffic_light && + (green_traffic_lights.empty() && + (traffic_light_state == carla::rpc::TrafficLightState::Green))) { + _logger->warn( + "found another relevant traffic light on lane {}; {} state {}", + matched_position.lanePoint.paraPoint.laneId, + traffic_light->GetId(), + (traffic_light_state == carla::rpc::TrafficLightState::Green) + ? "green" + : "not green"); + } else { + _logger->debug( + "found relevant traffic light on lane {}; {} state {}", + matched_position.lanePoint.paraPoint.laneId, + traffic_light->GetId(), + (traffic_light_state == carla::rpc::TrafficLightState::Green) + ? "green" + : "not green"); } found_relevant_traffic_light = true; // found matching traffic light - if ( traffic_light_state == carla::rpc::TrafficLightState::Green ) - { - // @todo: currently there is only this workaround because of missign OpenDrive map support for actual traffic light ids - green_traffic_lights.insert(::ad::map::landmark::LandmarkId::getMax()); - } - else - { + if (traffic_light_state == carla::rpc::TrafficLightState::Green) { + // @todo: currently there is only this workaround because of missign + // OpenDrive map support for actual traffic light ids + green_traffic_lights.insert( + ::ad::map::landmark::LandmarkId::getMax()); + } else { // if the light is not green, we don't have priority green_traffic_lights.clear(); } @@ -617,18 +750,21 @@ EgoDynamicsOnRoute RssCheck::CalculateEgoDynamicsOnRoute(carla::client::Timestam return green_traffic_lights; } +RssCheck::RssObjectChecker::RssObjectChecker( + RssCheck const &rss_check, ::ad::rss::map::RssSceneCreation &scene_creation, + carla::client::Vehicle const &carla_ego_vehicle, + CarlaRssState const &carla_rss_state, + ::ad::map::landmark::LandmarkIdSet const &green_traffic_lights) + : _rss_check(rss_check), + _scene_creation(scene_creation), + _carla_ego_vehicle(carla_ego_vehicle), + _carla_rss_state(carla_rss_state), + _green_traffic_lights(green_traffic_lights) {} -RssCheck::RssObjectChecker::RssObjectChecker(RssCheck const& rss_check, ::ad::rss::map::RssSceneCreation &scene_creation, carla::client::Vehicle const &carla_ego_vehicle, CarlaRssState const &carla_rss_state, - ::ad::map::landmark::LandmarkIdSet const &green_traffic_lights) - : _rss_check(rss_check) - , _scene_creation(scene_creation) - , _carla_ego_vehicle(carla_ego_vehicle) - , _carla_rss_state(carla_rss_state) - , _green_traffic_lights(green_traffic_lights) { -} - -void RssCheck::RssObjectChecker::operator()( const carla::SharedPtr actor ) const { - const auto vehicle = boost::dynamic_pointer_cast(actor); +void RssCheck::RssObjectChecker::operator()( + const carla::SharedPtr actor) const { + const auto vehicle = + boost::dynamic_pointer_cast(actor); if (vehicle == nullptr) { return; } @@ -636,8 +772,12 @@ void RssCheck::RssObjectChecker::operator()( const carla::SharedPtr(_carla_rss_state.ego_dynamics_on_route.min_stopping_distance), 100.); - if (vehicle->GetTransform().location.Distance(_carla_ego_vehicle.GetTransform().location) > relevant_distance) { + auto const relevant_distance = std::max( + static_cast( + _carla_rss_state.ego_dynamics_on_route.min_stopping_distance), + 100.); + if (vehicle->GetTransform().location.Distance( + _carla_ego_vehicle.GetTransform().location) > relevant_distance) { return; } @@ -646,30 +786,30 @@ void RssCheck::RssObjectChecker::operator()( const carla::SharedPtrdebug("OtherVehicleMapMatching: {}", other_match_object.mapMatchedBoundingBox); + _rss_check._logger->debug("OtherVehicleMapMatching: {}", + other_match_object.mapMatchedBoundingBox); - _scene_creation.appendScenes(_carla_ego_vehicle.GetId(), - _carla_rss_state.ego_match_object, - _carla_rss_state.ego_dynamics_on_route.ego_speed, - ego_dynamics, - _carla_rss_state.ego_route, - ::ad::rss::world::ObjectId(vehicle->GetId()), - ::ad::rss::world::ObjectType::OtherVehicle, - other_match_object, - other_speed, - other_dynamics, - ::ad::rss::map::RssSceneCreation::RestrictSpeedLimitMode::IncreasedSpeedLimit10, - _green_traffic_lights); - } - catch(...) { - _rss_check._logger->error("Exception processing vehicle {} -> Ignoring it", vehicle->GetId()); + _scene_creation.appendScenes( + _carla_ego_vehicle.GetId(), _carla_rss_state.ego_match_object, + _carla_rss_state.ego_dynamics_on_route.ego_speed, ego_dynamics, + _carla_rss_state.ego_route, + ::ad::rss::world::ObjectId(vehicle->GetId()), + ::ad::rss::world::ObjectType::OtherVehicle, other_match_object, + other_speed, other_dynamics, + ::ad::rss::map::RssSceneCreation::RestrictSpeedLimitMode:: + IncreasedSpeedLimit10, + _green_traffic_lights); + } catch (...) { + _rss_check._logger->error("Exception processing vehicle {} -> Ignoring it", + vehicle->GetId()); } } @@ -677,62 +817,67 @@ void RssCheck::CreateWorldModel(carla::client::Timestamp const ×tamp, carla::client::ActorList const &actors, carla::client::Vehicle const &carla_ego_vehicle, CarlaRssState &carla_rss_state) const { - - ::ad::map::landmark::LandmarkIdSet green_traffic_lights = GetGreenTrafficLightsOnRoute(actors, carla_rss_state.ego_route); + ::ad::map::landmark::LandmarkIdSet green_traffic_lights = + GetGreenTrafficLightsOnRoute(actors, carla_rss_state.ego_route); auto ego_vehicle_dynamics = GetEgoVehicleDynamics(); - auto const abs_avg_route_accel_lat = std::fabs(carla_rss_state.ego_dynamics_on_route.avg_route_accel_lat); - if ( abs_avg_route_accel_lat > ego_vehicle_dynamics.alphaLat.accelMax ) - { - _logger->info("!! Route lateral dynamics exceed expectations: route:{} expected:{} !!", - abs_avg_route_accel_lat, - ego_vehicle_dynamics.alphaLat.accelMax); - ego_vehicle_dynamics.alphaLat.accelMax = std::min(::ad::physics::Acceleration(20.), abs_avg_route_accel_lat); + auto const abs_avg_route_accel_lat = + std::fabs(carla_rss_state.ego_dynamics_on_route.avg_route_accel_lat); + if (abs_avg_route_accel_lat > ego_vehicle_dynamics.alphaLat.accelMax) { + _logger->info( + "!! Route lateral dynamics exceed expectations: route:{} expected:{} " + "!!", + abs_avg_route_accel_lat, ego_vehicle_dynamics.alphaLat.accelMax); + ego_vehicle_dynamics.alphaLat.accelMax = + std::min(::ad::physics::Acceleration(20.), abs_avg_route_accel_lat); } - ::ad::rss::map::RssSceneCreation scene_creation(timestamp.frame, ego_vehicle_dynamics); + ::ad::rss::map::RssSceneCreation scene_creation(timestamp.frame, + ego_vehicle_dynamics); - tbb::parallel_for_each( actors.begin(), actors.end(), RssObjectChecker(*this, scene_creation, carla_ego_vehicle, carla_rss_state, green_traffic_lights) ); + tbb::parallel_for_each( + actors.begin(), actors.end(), + RssObjectChecker(*this, scene_creation, carla_ego_vehicle, + carla_rss_state, green_traffic_lights)); - if ( _road_boundaries_mode != RoadBoundariesMode::Off ) { + if (_road_boundaries_mode != RoadBoundariesMode::Off) { // add artifical objects on the road boundaries for "stay-on-road" feature // use 'smart' dynamics ego_vehicle_dynamics.alphaLat.accelMax = ::ad::physics::Acceleration(0.); - scene_creation.appendRoadBoundaries(carla_ego_vehicle.GetId(), - carla_rss_state.ego_match_object, - carla_rss_state.ego_dynamics_on_route.ego_speed, - ego_vehicle_dynamics, - carla_rss_state.ego_route, - // since the route always expanded, route isn't required to expand any more - ::ad::rss::map::RssSceneCreation::AppendRoadBoundariesMode::RouteOnly); + scene_creation.appendRoadBoundaries( + carla_ego_vehicle.GetId(), carla_rss_state.ego_match_object, + carla_rss_state.ego_dynamics_on_route.ego_speed, ego_vehicle_dynamics, + carla_rss_state.ego_route, + // since the route always expanded, route isn't required to expand any + // more + ::ad::rss::map::RssSceneCreation::AppendRoadBoundariesMode::RouteOnly); } carla_rss_state.world_model = scene_creation.getWorldModel(); } - -void RssCheck::PerformCheck(CarlaRssState &carla_rss_state) const -{ - bool result = carla_rss_state.rss_check.calculateAccelerationRestriction(carla_rss_state.world_model, - carla_rss_state.situation_snapshot, - carla_rss_state.rss_state_snapshot, - carla_rss_state.proper_response, - carla_rss_state.acceleration_restriction); +void RssCheck::PerformCheck(CarlaRssState &carla_rss_state) const { + bool result = carla_rss_state.rss_check.calculateAccelerationRestriction( + carla_rss_state.world_model, carla_rss_state.situation_snapshot, + carla_rss_state.rss_state_snapshot, carla_rss_state.proper_response, + carla_rss_state.acceleration_restriction); if (!result) { _logger->warn("calculateAccelerationRestriction failed!"); - carla_rss_state.proper_response.lateralResponseRight = ::ad::rss::state::LateralResponse::None; - carla_rss_state.proper_response.lateralResponseLeft = ::ad::rss::state::LateralResponse::None; - carla_rss_state.proper_response.longitudinalResponse = ::ad::rss::state::LongitudinalResponse::None; + carla_rss_state.proper_response.lateralResponseRight = + ::ad::rss::state::LateralResponse::None; + carla_rss_state.proper_response.lateralResponseLeft = + ::ad::rss::state::LateralResponse::None; + carla_rss_state.proper_response.longitudinalResponse = + ::ad::rss::state::LongitudinalResponse::None; } - if(!carla_rss_state.proper_response.isSafe) - { - _logger->info("Unsafe route: {}, {}", carla_rss_state.proper_response, carla_rss_state.acceleration_restriction); + if (!carla_rss_state.proper_response.isSafe) { + _logger->info("Unsafe route: {}, {}", carla_rss_state.proper_response, + carla_rss_state.acceleration_restriction); } } - void RssCheck::AnalyseCheckResults(CarlaRssState &carla_rss_state) const { carla_rss_state.dangerous_state = false; carla_rss_state.dangerous_vehicle = false; @@ -742,35 +887,43 @@ void RssCheck::AnalyseCheckResults(CarlaRssState &carla_rss_state) const { bool vehicle_triggered_left_response = false; bool vehicle_triggered_right_response = false; bool vehicle_triggered_longitudinal_response = false; - for (auto const state: carla_rss_state.rss_state_snapshot.individualResponses) { + for (auto const state : + carla_rss_state.rss_state_snapshot.individualResponses) { if (::ad::rss::state::isDangerous(state)) { carla_rss_state.dangerous_state = true; _logger->trace("DangerousState: {}", state); - auto dangerous_sitation_iter = std::find_if(carla_rss_state.situation_snapshot.situations.begin(), - carla_rss_state.situation_snapshot.situations.end(), - [&state](::ad::rss::situation::Situation const &situation) { - return situation.situationId == state.situationId; }); - if ( dangerous_sitation_iter != carla_rss_state.situation_snapshot.situations.end() ) { + auto dangerous_sitation_iter = std::find_if( + carla_rss_state.situation_snapshot.situations.begin(), + carla_rss_state.situation_snapshot.situations.end(), + [&state](::ad::rss::situation::Situation const &situation) { + return situation.situationId == state.situationId; + }); + if (dangerous_sitation_iter != + carla_rss_state.situation_snapshot.situations.end()) { _logger->trace("Situation: {}", *dangerous_sitation_iter); - if (dangerous_sitation_iter->objectId == ::ad::rss::map::RssSceneCreator::getRightBorderObjectId()) { + if (dangerous_sitation_iter->objectId == + ::ad::rss::map::RssSceneCreator::getRightBorderObjectId()) { right_border_is_dangerous = true; - } - else if ( dangerous_sitation_iter->objectId == ::ad::rss::map::RssSceneCreator::getLeftBorderObjectId()) { + } else if (dangerous_sitation_iter->objectId == + ::ad::rss::map::RssSceneCreator::getLeftBorderObjectId()) { left_border_is_dangerous = true; - } - else { + } else { carla_rss_state.dangerous_vehicle = true; - if (state.longitudinalState.response != ::ad::rss::state::LongitudinalResponse::None) { + if (state.longitudinalState.response != + ::ad::rss::state::LongitudinalResponse::None) { vehicle_triggered_longitudinal_response = true; } - if (state.lateralStateLeft.response != ::ad::rss::state::LateralResponse::None) { + if (state.lateralStateLeft.response != + ::ad::rss::state::LateralResponse::None) { vehicle_triggered_left_response = true; } - if (state.lateralStateRight.response != ::ad::rss::state::LateralResponse::None) { + if (state.lateralStateRight.response != + ::ad::rss::state::LateralResponse::None) { vehicle_triggered_right_response = true; } } - if (dangerous_sitation_iter->situationType == ::ad::rss::situation::SituationType::OppositeDirection) { + if (dangerous_sitation_iter->situationType == + ::ad::rss::situation::SituationType::OppositeDirection) { carla_rss_state.dangerous_opposite_state = true; } } @@ -778,77 +931,98 @@ void RssCheck::AnalyseCheckResults(CarlaRssState &carla_rss_state) const { } // border are restricting potentially too much, fix this - if ( !vehicle_triggered_longitudinal_response && - (carla_rss_state.proper_response.longitudinalResponse != ::ad::rss::state::LongitudinalResponse::None) ) - { - _logger->debug("!! longitudinalResponse only triggered by borders: ignore !!"); - carla_rss_state.proper_response.longitudinalResponse = ::ad::rss::state::LongitudinalResponse::None; - carla_rss_state.acceleration_restriction.longitudinalRange.maximum = GetEgoVehicleDynamics().alphaLon.accelMax; + if (!vehicle_triggered_longitudinal_response && + (carla_rss_state.proper_response.longitudinalResponse != + ::ad::rss::state::LongitudinalResponse::None)) { + _logger->debug( + "!! longitudinalResponse only triggered by borders: ignore !!"); + carla_rss_state.proper_response.longitudinalResponse = + ::ad::rss::state::LongitudinalResponse::None; + carla_rss_state.acceleration_restriction.longitudinalRange.maximum = + GetEgoVehicleDynamics().alphaLon.accelMax; } - if ( !vehicle_triggered_left_response && !left_border_is_dangerous && - (carla_rss_state.proper_response.lateralResponseLeft != ::ad::rss::state::LateralResponse::None) ) { - _logger->debug("!! lateralResponseLeft only triggered by right border: ignore !!"); - carla_rss_state.proper_response.lateralResponseLeft = ::ad::rss::state::LateralResponse::None; - carla_rss_state.acceleration_restriction.lateralLeftRange.maximum = GetEgoVehicleDynamics().alphaLat.accelMax; + if (!vehicle_triggered_left_response && !left_border_is_dangerous && + (carla_rss_state.proper_response.lateralResponseLeft != + ::ad::rss::state::LateralResponse::None)) { + _logger->debug( + "!! lateralResponseLeft only triggered by right border: ignore !!"); + carla_rss_state.proper_response.lateralResponseLeft = + ::ad::rss::state::LateralResponse::None; + carla_rss_state.acceleration_restriction.lateralLeftRange.maximum = + GetEgoVehicleDynamics().alphaLat.accelMax; carla_rss_state.ego_dynamics_on_route.crossing_border = true; } - if ( !vehicle_triggered_right_response && !right_border_is_dangerous && - (carla_rss_state.proper_response.lateralResponseRight != ::ad::rss::state::LateralResponse::None) ) { - _logger->debug("!! lateralResponseRight only triggered by left border: ignore !!"); - carla_rss_state.proper_response.lateralResponseRight = ::ad::rss::state::LateralResponse::None; - carla_rss_state.acceleration_restriction.lateralRightRange.maximum = GetEgoVehicleDynamics().alphaLat.accelMax; + if (!vehicle_triggered_right_response && !right_border_is_dangerous && + (carla_rss_state.proper_response.lateralResponseRight != + ::ad::rss::state::LateralResponse::None)) { + _logger->debug( + "!! lateralResponseRight only triggered by left border: ignore !!"); + carla_rss_state.proper_response.lateralResponseRight = + ::ad::rss::state::LateralResponse::None; + carla_rss_state.acceleration_restriction.lateralRightRange.maximum = + GetEgoVehicleDynamics().alphaLat.accelMax; carla_rss_state.ego_dynamics_on_route.crossing_border = true; } - _logger->debug("RouteResponse: {}, AccelerationRestriction: {}", carla_rss_state.proper_response, carla_rss_state.acceleration_restriction); + _logger->debug("RouteResponse: {}, AccelerationRestriction: {}", + carla_rss_state.proper_response, + carla_rss_state.acceleration_restriction); } - /// /// visualization /// -void RssCheck::VisualizeResults(carla::client::World &world, - carla::SharedPtr const &carla_ego_actor) const { - if (_visualization_mode==VisualizationMode::Off) { +void RssCheck::VisualizeResults( + carla::client::World &world, + carla::SharedPtr const &carla_ego_actor) const { + if (_visualization_mode == VisualizationMode::Off) { return; } if (_visualization_mutex.try_lock()) { carla::client::DebugHelper dh = world.MakeDebugHelper(); - if ((_visualization_mode==VisualizationMode::RouteOnly) - || (_visualization_mode==VisualizationMode::VehicleStateAndRoute) - || (_visualization_mode==VisualizationMode::All)) { - VisualizeRouteLocked(dh, _visualization_route.first, carla_ego_actor, _visualization_route.second); + if ((_visualization_mode == VisualizationMode::RouteOnly) || + (_visualization_mode == VisualizationMode::VehicleStateAndRoute) || + (_visualization_mode == VisualizationMode::All)) { + VisualizeRouteLocked(dh, _visualization_route.first, carla_ego_actor, + _visualization_route.second); } - if ((_visualization_mode==VisualizationMode::VehicleStateOnly) - || (_visualization_mode==VisualizationMode::VehicleStateAndRoute) - || (_visualization_mode==VisualizationMode::All)) { - VisualizeRssResultsLocked(dh, world, carla_ego_actor, _visualization_state_snapshot); + if ((_visualization_mode == VisualizationMode::VehicleStateOnly) || + (_visualization_mode == VisualizationMode::VehicleStateAndRoute) || + (_visualization_mode == VisualizationMode::All)) { + VisualizeRssResultsLocked(dh, world, carla_ego_actor, + _visualization_state_snapshot); } - if ((_visualization_mode==VisualizationMode::All) - || (_visualization_mode==VisualizationMode::DebugRouteOrientationObjectRoute) - || (_visualization_mode==VisualizationMode::DebugRouteOrientationBorders) - || (_visualization_mode==VisualizationMode::DebugRouteOrientationBoth)) { + if ((_visualization_mode == VisualizationMode::All) || + (_visualization_mode == + VisualizationMode::DebugRouteOrientationObjectRoute) || + (_visualization_mode == + VisualizationMode::DebugRouteOrientationBorders) || + (_visualization_mode == VisualizationMode::DebugRouteOrientationBoth)) { VisualizeEgoDynamics(dh, carla_ego_actor, _visualization_ego_dynamics); } - if ((_visualization_mode==VisualizationMode::DebugRouteOrientationObjectRoute) - || (_visualization_mode==VisualizationMode::DebugRouteOrientationBoth)) { - VisualizeRouteLocked(dh, _visualization_debug_route, carla_ego_actor, false); + if ((_visualization_mode == + VisualizationMode::DebugRouteOrientationObjectRoute) || + (_visualization_mode == VisualizationMode::DebugRouteOrientationBoth)) { + VisualizeRouteLocked(dh, _visualization_debug_route, carla_ego_actor, + false); } - if ((_visualization_mode==VisualizationMode::DebugRouteOrientationBorders) - || (_visualization_mode==VisualizationMode::DebugRouteOrientationBoth)) { + if ((_visualization_mode == + VisualizationMode::DebugRouteOrientationBorders) || + (_visualization_mode == VisualizationMode::DebugRouteOrientationBoth)) { carla::sensor::data::Color color_left{0u, 255u, 255u}; carla::sensor::data::Color color_right{122u, 255u, 255u}; - for (auto const &border: _visualization_debug_enu_border) - { - VisualizeENUEdgeLocked(dh, border.left, color_left, carla_ego_actor->GetLocation().z); - VisualizeENUEdgeLocked(dh, border.right, color_right, carla_ego_actor->GetLocation().z); + for (auto const &border : _visualization_debug_enu_border) { + VisualizeENUEdgeLocked(dh, border.left, color_left, + carla_ego_actor->GetLocation().z); + VisualizeENUEdgeLocked(dh, border.right, color_right, + carla_ego_actor->GetLocation().z); } } @@ -859,14 +1033,16 @@ void RssCheck::VisualizeResults(carla::client::World &world, void RssCheck::StoreVisualizationResults(CarlaRssState const &carla_rss_state) { if (_visualization_mutex.try_lock()) { _visualization_state_snapshot = carla_rss_state.rss_state_snapshot; - _visualization_route = std::make_pair(carla_rss_state.ego_route, carla_rss_state.dangerous_state); + _visualization_route = std::make_pair(carla_rss_state.ego_route, + carla_rss_state.dangerous_state); _visualization_ego_dynamics = carla_rss_state.ego_dynamics_on_route; _visualization_mutex.unlock(); } } -void RssCheck::StoreDebugVisualization(::ad::map::route::FullRoute const &debug_route, - std::vector<::ad::map::lane::ENUBorder> const &enu_border) const { +void RssCheck::StoreDebugVisualization( + ::ad::map::route::FullRoute const &debug_route, + std::vector<::ad::map::lane::ENUBorder> const &enu_border) const { if (_visualization_mutex.try_lock()) { _visualization_debug_route = debug_route; _visualization_debug_enu_border = enu_border; @@ -874,27 +1050,29 @@ void RssCheck::StoreDebugVisualization(::ad::map::route::FullRoute const &debug_ } } -void RssCheck::VisualizeRssResultsLocked(carla::client::DebugHelper &dh, - carla::client::World &world, - carla::SharedPtr const &carla_ego_actor, - ::ad::rss::state::RssStateSnapshot state_snapshot) const { - - const auto carla_ego_vehicle = boost::dynamic_pointer_cast(carla_ego_actor); +void RssCheck::VisualizeRssResultsLocked( + carla::client::DebugHelper &dh, carla::client::World &world, + carla::SharedPtr const &carla_ego_actor, + ::ad::rss::state::RssStateSnapshot state_snapshot) const { + const auto carla_ego_vehicle = + boost::dynamic_pointer_cast(carla_ego_actor); carla::geom::Location ego_vehicle_location = carla_ego_vehicle->GetLocation(); auto ego_vehicle_transform = carla_ego_vehicle->GetTransform(); - for (auto const &state: state_snapshot.individualResponses) { - carla::rpc::ActorId vehicle_id = static_cast(state.objectId); + for (auto const &state : state_snapshot.individualResponses) { + carla::rpc::ActorId vehicle_id = + static_cast(state.objectId); carla::SharedPtr vehicle_list = - world.GetActors(std::vector{vehicle_id}); + world.GetActors(std::vector{vehicle_id}); carla::geom::Location ego_point = ego_vehicle_location; ego_point.z += 0.05f; const auto yaw = ego_vehicle_transform.rotation.yaw; - const float cosine = std::cos(yaw * toRadians); - const float sine = std::sin(yaw * toRadians); + const float cosine = std::cos(yaw * to_radians); + const float sine = std::sin(yaw * to_radians); carla::geom::Location line_offset{-sine * 0.1f, cosine * 0.1f, 0.0f}; for (const auto &actor : *vehicle_list) { - const auto vehicle = boost::dynamic_pointer_cast(actor); + const auto vehicle = + boost::dynamic_pointer_cast(actor); carla::geom::Location point = vehicle->GetLocation(); point.z += 0.05f; carla::sensor::data::Color indicator_color{0u, 255u, 0u}; @@ -902,7 +1080,7 @@ void RssCheck::VisualizeRssResultsLocked(carla::client::DebugHelper &dh, if (dangerous) { indicator_color = carla::sensor::data::Color{255u, 0u, 0u}; } - if (_visualization_mode==VisualizationMode::All) { + if (_visualization_mode == VisualizationMode::All) { // the car connections are only visualized if All is requested carla::sensor::data::Color lon_color = indicator_color; carla::sensor::data::Color lat_l_color = indicator_color; @@ -932,8 +1110,10 @@ void RssCheck::VisualizeRssResultsLocked(carla::client::DebugHelper &dh, } } dh.DrawLine(ego_point, point, 0.1f, lon_color, 0.02f, false); - dh.DrawLine(ego_point - line_offset, point - line_offset, 0.1f, lat_l_color, 0.02f, false); - dh.DrawLine(ego_point + line_offset, point + line_offset, 0.1f, lat_r_color, 0.02f, false); + dh.DrawLine(ego_point - line_offset, point - line_offset, 0.1f, + lat_l_color, 0.02f, false); + dh.DrawLine(ego_point + line_offset, point + line_offset, 0.1f, + lat_r_color, 0.02f, false); } point.z += 3.f; dh.DrawPoint(point, 0.2f, indicator_color, 0.02f, false); @@ -945,21 +1125,20 @@ void RssCheck::VisualizeENUEdgeLocked(carla::client::DebugHelper &dh, ::ad::map::point::ENUEdge const &edge, carla::sensor::data::Color const &color, float const z_offset) const { - for (auto const &point : edge) { - carla::geom::Location carla_point(static_cast(static_cast(point.x)), - static_cast(static_cast(-1. * point.y)), - static_cast(static_cast(point.z))); + carla::geom::Location carla_point( + static_cast(static_cast(point.x)), + static_cast(static_cast(-1. * point.y)), + static_cast(static_cast(point.z))); carla_point.z += z_offset; dh.DrawPoint(carla_point, 0.1f, color, 0.1f, false); } } -void RssCheck::VisualizeRouteLocked(carla::client::DebugHelper &dh, - const ::ad::map::route::FullRoute &route, - carla::SharedPtr const &carla_ego_actor, - bool dangerous) const { - +void RssCheck::VisualizeRouteLocked( + carla::client::DebugHelper &dh, const ::ad::map::route::FullRoute &route, + carla::SharedPtr const &carla_ego_actor, + bool dangerous) const { std::map<::ad::map::lane::LaneId, ::ad::map::point::ENUEdge> right_lane_edges; std::map<::ad::map::lane::LaneId, ::ad::map::point::ENUEdge> left_lane_edges; @@ -967,13 +1146,15 @@ void RssCheck::VisualizeRouteLocked(carla::client::DebugHelper &dh, for (auto const &road_segment : route.roadSegments) { { auto &right_most_lane = road_segment.drivableLaneSegments.front(); - if (right_lane_edges.find(right_most_lane.laneInterval.laneId) - == right_lane_edges.end()) { - ::ad::map::point::ENUEdge edge = ::ad::map::route::getRightProjectedENUEdge(right_most_lane.laneInterval); - right_lane_edges.insert( { right_most_lane.laneInterval.laneId, edge }); + if (right_lane_edges.find(right_most_lane.laneInterval.laneId) == + right_lane_edges.end()) { + ::ad::map::point::ENUEdge edge = + ::ad::map::route::getRightProjectedENUEdge( + right_most_lane.laneInterval); + right_lane_edges.insert({right_most_lane.laneInterval.laneId, edge}); bool intersection_lane = - ::ad::map::intersection::Intersection::isLanePartOfAnIntersection( - right_most_lane.laneInterval.laneId); + ::ad::map::intersection::Intersection::isLanePartOfAnIntersection( + right_most_lane.laneInterval.laneId); carla::sensor::data::Color color((dangerous ? 128 : 255), 0, 0); if (intersection_lane) { @@ -984,13 +1165,15 @@ void RssCheck::VisualizeRouteLocked(carla::client::DebugHelper &dh, } { auto &left_most_lane = road_segment.drivableLaneSegments.back(); - if (left_lane_edges.find(left_most_lane.laneInterval.laneId) - == left_lane_edges.end()) { - ::ad::map::point::ENUEdge edge = ::ad::map::route::getLeftProjectedENUEdge(left_most_lane.laneInterval); - left_lane_edges.insert( { left_most_lane.laneInterval.laneId, edge }); + if (left_lane_edges.find(left_most_lane.laneInterval.laneId) == + left_lane_edges.end()) { + ::ad::map::point::ENUEdge edge = + ::ad::map::route::getLeftProjectedENUEdge( + left_most_lane.laneInterval); + left_lane_edges.insert({left_most_lane.laneInterval.laneId, edge}); bool intersection_lane = - ::ad::map::intersection::Intersection::isLanePartOfAnIntersection( - left_most_lane.laneInterval.laneId); + ::ad::map::intersection::Intersection::isLanePartOfAnIntersection( + left_most_lane.laneInterval.laneId); carla::sensor::data::Color color(0, (dangerous ? 128 : 255), 0); if (intersection_lane) { color.b = (dangerous ? 128 : 255); @@ -1001,17 +1184,20 @@ void RssCheck::VisualizeRouteLocked(carla::client::DebugHelper &dh, } } -void RssCheck::VisualizeEgoDynamics(carla::client::DebugHelper &dh, - carla::SharedPtr const &carla_ego_actor, - EgoDynamicsOnRoute const &ego_dynamics_on_route) const -{ - const auto carla_ego_vehicle = boost::dynamic_pointer_cast(carla_ego_actor); +void RssCheck::VisualizeEgoDynamics( + carla::client::DebugHelper &dh, + carla::SharedPtr const &carla_ego_actor, + EgoDynamicsOnRoute const &ego_dynamics_on_route) const { + const auto carla_ego_vehicle = + boost::dynamic_pointer_cast(carla_ego_actor); carla::geom::Location ego_vehicle_location = carla_ego_vehicle->GetLocation(); carla::sensor::data::Color color{0u, 0u, 255u}; - auto sin_heading = static_cast(std::sin(static_cast(ego_dynamics_on_route.route_heading))); - auto cos_heading = static_cast(std::cos(static_cast(ego_dynamics_on_route.route_heading))); + auto sin_heading = static_cast( + std::sin(static_cast(ego_dynamics_on_route.route_heading))); + auto cos_heading = static_cast( + std::cos(static_cast(ego_dynamics_on_route.route_heading))); carla::geom::Location heading_location_start = ego_vehicle_location; heading_location_start.x -= cos_heading * 10.f; @@ -1022,10 +1208,13 @@ void RssCheck::VisualizeEgoDynamics(carla::client::DebugHelper &dh, heading_location_end.y -= sin_heading * 10.f; heading_location_end.z += 0.5f; - dh.DrawArrow(heading_location_start, heading_location_end, 0.1f, 0.1f, color, 0.02f, false); + dh.DrawArrow(heading_location_start, heading_location_end, 0.1f, 0.1f, color, + 0.02f, false); - auto sin_center = static_cast(std::sin(static_cast(ego_dynamics_on_route.route_heading)+M_PI_2)); - auto cos_center = static_cast(std::cos(static_cast(ego_dynamics_on_route.route_heading)+M_PI_2)); + auto sin_center = static_cast(std::sin( + static_cast(ego_dynamics_on_route.route_heading) + M_PI_2)); + auto cos_center = static_cast(std::cos( + static_cast(ego_dynamics_on_route.route_heading) + M_PI_2)); carla::geom::Location center_location_start = ego_vehicle_location; center_location_start.x -= cos_center * 2.f; center_location_start.y += sin_center * 2.f; @@ -1035,9 +1224,9 @@ void RssCheck::VisualizeEgoDynamics(carla::client::DebugHelper &dh, center_location_end.y -= sin_center * 2.f; center_location_end.z += 0.5f; - dh.DrawLine(center_location_start, center_location_end, 0.1f, color, 0.02f, false); + dh.DrawLine(center_location_start, center_location_end, 0.1f, color, 0.02f, + false); } - } // namespace rss } // namespace carla diff --git a/LibCarla/source/carla/rss/RssCheck.h b/LibCarla/source/carla/rss/RssCheck.h index 92d1df664..328f0218e 100644 --- a/LibCarla/source/carla/rss/RssCheck.h +++ b/LibCarla/source/carla/rss/RssCheck.h @@ -5,6 +5,7 @@ #pragma once +#include #include #include #include @@ -13,10 +14,9 @@ #include #include #include +#include #include #include -#include -#include #include "carla/client/ActorList.h" #include "carla/client/Vehicle.h" #include "carla/road/Map.h" @@ -27,28 +27,35 @@ namespace rss { /// @brief struct defining the different supported handling of road boundaries enum class RoadBoundariesMode { Off, /// No road boundaries considered by RSS check - On /// The road boundaries of the current route are considered by RSS check + On /// The road boundaries of the current route are considered by RSS check }; -/// @brief struct defining the different supported handling of debug visualization +/// @brief struct defining the different supported handling of debug +/// visualization enum class VisualizationMode { - Off, /// Nothing is visualized - RouteOnly, /// Only the route is visualized - DebugRouteOrientationObjectRoute, /// Only the route section around the object is visualized - DebugRouteOrientationBorders, /// Only the route section transformed into borders around the object is visualized - DebugRouteOrientationBoth, /// The route section around the object and the transformed borders are visualized - VehicleStateOnly, /// Only the states of the vehicles is visualized - VehicleStateAndRoute, /// The states of the vehicles and the route is visualized - All /// All (except debug) is visualized + Off, /// Nothing is visualized + RouteOnly, /// Only the route is visualized + DebugRouteOrientationObjectRoute, /// Only the route section around the + /// object + /// is visualized + DebugRouteOrientationBorders, /// Only the route section transformed into + /// borders around the object is visualized + DebugRouteOrientationBoth, /// The route section around the object and the + /// transformed borders are visualized + VehicleStateOnly, /// Only the states of the vehicles is visualized + VehicleStateAndRoute, /// The states of the vehicles and the route is + /// visualized + All /// All (except debug) is visualized }; -/// @brief struct defining the ego vehicles current dynamics in respect to the current route +/// @brief struct defining the ego vehicles current dynamics in respect to the +/// current route /// -/// Especially the velocity of the vehicle is split into lateral and longitudinal components +/// Especially the velocity of the vehicle is split into lateral and +/// longitudinal components /// according to the current route /// struct EgoDynamicsOnRoute { - /// @brief constructor EgoDynamicsOnRoute(); @@ -68,7 +75,8 @@ struct EgoDynamicsOnRoute { ::ad::map::point::ENUHeading ego_heading; /// @brief check if the ego center is within route bool ego_center_within_route; - /// @brief flag indicating if the current state is already crossing one of the borders + /// @brief flag indicating if the current state is already crossing one of the + /// borders /// this is only evaluated if the border checks are active! /// It is a hint to oversteer a bit on countersteering bool crossing_border; @@ -94,9 +102,9 @@ struct EgoDynamicsOnRoute { ::ad::physics::Acceleration avg_route_accel_lon; }; -/// @brief class implementing the actual RSS checks based on CARLA world description +/// @brief class implementing the actual RSS checks based on CARLA world +/// description class RssCheck { - public: /// @brief constructor RssCheck(); @@ -109,48 +117,63 @@ public: /// This function has to be called cyclic with increasing timestamps to ensure /// proper RSS evaluation. /// - bool CheckObjects(carla::client::Timestamp const ×tamp, - carla::SharedPtr const &actors, - carla::SharedPtr const &carla_ego_actor, - ::ad::rss::state::ProperResponse &output_response, - ::ad::rss::world::AccelerationRestriction &output_acceleration_restriction, - ::ad::rss::state::RssStateSnapshot &output_rss_state_snapshot, - EgoDynamicsOnRoute &output_rss_ego_dynamics_on_route); + bool CheckObjects( + carla::client::Timestamp const ×tamp, + carla::SharedPtr const &actors, + carla::SharedPtr const &carla_ego_actor, + ::ad::rss::state::ProperResponse &output_response, + ::ad::rss::world::AccelerationRestriction + &output_acceleration_restriction, + ::ad::rss::state::RssStateSnapshot &output_rss_state_snapshot, + EgoDynamicsOnRoute &output_rss_ego_dynamics_on_route); /// @brief function to visualize the RSS check results /// - void VisualizeResults(carla::client::World &world, - carla::SharedPtr const &carla_ego_actor) const; + void VisualizeResults( + carla::client::World &world, + carla::SharedPtr const &carla_ego_actor) const; /// @returns the used vehicle dynamics for ego vehcile - const ::ad::rss::world::RssDynamics& GetEgoVehicleDynamics() const; + const ::ad::rss::world::RssDynamics &GetEgoVehicleDynamics() const; /// @brief sets the vehicle dynamics to be used for the ego vehcile - void SetEgoVehicleDynamics(const ::ad::rss::world::RssDynamics &ego_vehicle_dynamics); + void SetEgoVehicleDynamics( + const ::ad::rss::world::RssDynamics &ego_vehicle_dynamics); /// @returns the used vehicle dynamics for other vehciles - const ::ad::rss::world::RssDynamics& GetOtherVehicleDynamics() const; + const ::ad::rss::world::RssDynamics &GetOtherVehicleDynamics() const; /// @brief sets the vehicle dynamics to be used for other vehciles - void SetOtherVehicleDynamics(const ::ad::rss::world::RssDynamics &other_vehicle_dynamics); + void SetOtherVehicleDynamics( + const ::ad::rss::world::RssDynamics &other_vehicle_dynamics); - /// @returns the current mode for respecting the road boundaries (@see also RssSensor::GetRoadBoundariesMode()) - const ::carla::rss::RoadBoundariesMode& GetRoadBoundariesMode() const; - /// @brief sets the current mode for respecting the road boundaries (@see also RssSensor::SetRoadBoundariesMode()) - void SetRoadBoundariesMode(const ::carla::rss::RoadBoundariesMode &road_boundaries_mode); + /// @returns the current mode for respecting the road boundaries (@see also + /// RssSensor::GetRoadBoundariesMode()) + const ::carla::rss::RoadBoundariesMode &GetRoadBoundariesMode() const; + /// @brief sets the current mode for respecting the road boundaries (@see also + /// RssSensor::SetRoadBoundariesMode()) + void SetRoadBoundariesMode( + const ::carla::rss::RoadBoundariesMode &road_boundaries_mode); - /// @returns the current routing targets (@see also RssSensor::GetRoutingTargets()) + /// @returns the current routing targets (@see also + /// RssSensor::GetRoutingTargets()) const std::vector<::carla::geom::Transform> GetRoutingTargets() const; - /// @brief appends a routing target to the current routing target list (@see also RssSensor::AppendRoutingTargets()) + /// @brief appends a routing target to the current routing target list (@see + /// also RssSensor::AppendRoutingTargets()) void AppendRoutingTarget(const ::carla::geom::Transform &routing_target); - /// @brief resets the current routing targets (@see also RssSensor::ResetRoutingTargets()) + /// @brief resets the current routing targets (@see also + /// RssSensor::ResetRoutingTargets()) void ResetRoutingTargets(); - /// @brief sets the visualization mode (@see also ::carla::rss::VisualizationMode) - void SetVisualizationMode(const ::carla::rss::VisualizationMode &visualization_mode); - /// @returns get the current visualization mode (@see also ::carla::rss::VisualizationMode) - const ::carla::rss::VisualizationMode& GetVisualizationMode() const; + /// @brief sets the visualization mode (@see also + /// ::carla::rss::VisualizationMode) + void SetVisualizationMode( + const ::carla::rss::VisualizationMode &visualization_mode); + /// @returns get the current visualization mode (@see also + /// ::carla::rss::VisualizationMode) + const ::carla::rss::VisualizationMode &GetVisualizationMode() const; /// @brief drop the current route /// - /// Afterwards a new route is selected randomly (if multiple routes are possible). + /// Afterwards a new route is selected randomly (if multiple routes are + /// possible). /// void DropRoute(); @@ -199,20 +222,26 @@ private: ::ad::rss::world::AccelerationRestriction acceleration_restriction; /// @brief flag indicating if the current state is overall dangerous bool dangerous_state; - /// @brief flag indicating if the current state is dangerous because of a vehicle + /// @brief flag indicating if the current state is dangerous because of a + /// vehicle bool dangerous_vehicle; - /// @brief flag indicating if the current state is dangerous because of an opposite vehicle + /// @brief flag indicating if the current state is dangerous because of an + /// opposite vehicle bool dangerous_opposite_state; }; - class RssObjectChecker { public: - RssObjectChecker(RssCheck const& rss_check, ::ad::rss::map::RssSceneCreation &scene_creation, carla::client::Vehicle const &carla_ego_vehicle, CarlaRssState const &carla_rss_state, - ::ad::map::landmark::LandmarkIdSet const &green_traffic_lights); - void operator()( const carla::SharedPtr actor ) const; + RssObjectChecker( + RssCheck const &rss_check, + ::ad::rss::map::RssSceneCreation &scene_creation, + carla::client::Vehicle const &carla_ego_vehicle, + CarlaRssState const &carla_rss_state, + ::ad::map::landmark::LandmarkIdSet const &green_traffic_lights); + void operator()(const carla::SharedPtr actor) const; + private: - RssCheck const& _rss_check; + RssCheck const &_rss_check; ::ad::rss::map::RssSceneCreation &_scene_creation; carla::client::Vehicle const &_carla_ego_vehicle; CarlaRssState const &_carla_rss_state; @@ -228,26 +257,30 @@ private: ::ad::rss::world::RssDynamics GetDefaultVehicleDynamics() const; /// @brief calculate the map matched object from the carla_vehicle - ::ad::map::match::Object GetMatchObject(carla::client::Vehicle const &carla_vehicle, - ::ad::physics::Distance const &match_distance) const; + ::ad::map::match::Object GetMatchObject( + carla::client::Vehicle const &carla_vehicle, + ::ad::physics::Distance const &match_distance) const; /// @brief calculate the speed from the carla_vehicle - ::ad::physics::Speed GetSpeed(carla::client::Vehicle const &carla_vehicle) const; + ::ad::physics::Speed GetSpeed( + carla::client::Vehicle const &carla_vehicle) const; /// @brief update the desired ego vehicle route void UpdateRoute(CarlaRssState &carla_rss_state); /// @brief calculate ego vehicle dynamics on the route - EgoDynamicsOnRoute CalculateEgoDynamicsOnRoute(carla::client::Timestamp const ¤t_timestamp, - double const &time_since_epoch_check_start_ms, - carla::client::Vehicle const &carla_vehicle, - ::ad::map::match::Object match_object, - ::ad::map::route::FullRoute const &route, - EgoDynamicsOnRoute const &last_dynamics) const; + EgoDynamicsOnRoute CalculateEgoDynamicsOnRoute( + carla::client::Timestamp const ¤t_timestamp, + double const &time_since_epoch_check_start_ms, + carla::client::Vehicle const &carla_vehicle, + ::ad::map::match::Object match_object, + ::ad::map::route::FullRoute const &route, + EgoDynamicsOnRoute const &last_dynamics) const; /// @brief collect the green traffic lights on the current route - ::ad::map::landmark::LandmarkIdSet GetGreenTrafficLightsOnRoute(carla::client::ActorList const &actors, - ::ad::map::route::FullRoute const&route) const; + ::ad::map::landmark::LandmarkIdSet GetGreenTrafficLightsOnRoute( + carla::client::ActorList const &actors, + ::ad::map::route::FullRoute const &route) const; /// @brief Create the RSS world model void CreateWorldModel(carla::client::Timestamp const ×tamp, @@ -266,24 +299,25 @@ private: /// void StoreVisualizationResults(CarlaRssState const &carla_rss_state); - void StoreDebugVisualization(::ad::map::route::FullRoute const &debug_route, - std::vector<::ad::map::lane::ENUBorder> const &enu_border) const; + void StoreDebugVisualization( + ::ad::map::route::FullRoute const &debug_route, + std::vector<::ad::map::lane::ENUBorder> const &enu_border) const; /// mutex to protect the visualization content mutable std::mutex _visualization_mutex; /// the RssStateSnapshot to be visualized ::ad::rss::state::RssStateSnapshot _visualization_state_snapshot; - void VisualizeRssResultsLocked(carla::client::DebugHelper &dh, - carla::client::World &world, - carla::SharedPtr const &carla_ego_actor, - ::ad::rss::state::RssStateSnapshot state_snapshot) const; + void VisualizeRssResultsLocked( + carla::client::DebugHelper &dh, carla::client::World &world, + carla::SharedPtr const &carla_ego_actor, + ::ad::rss::state::RssStateSnapshot state_snapshot) const; /// the FullRoute to be visualized std::pair<::ad::map::route::FullRoute, bool> _visualization_route; - void VisualizeRouteLocked(carla::client::DebugHelper &dh, - ::ad::map::route::FullRoute const &route, - carla::SharedPtr const &carla_ego_actor, - bool dangerous) const; + void VisualizeRouteLocked( + carla::client::DebugHelper &dh, ::ad::map::route::FullRoute const &route, + carla::SharedPtr const &carla_ego_actor, + bool dangerous) const; void VisualizeENUEdgeLocked(carla::client::DebugHelper &dh, ::ad::map::point::ENUEdge const &edge, @@ -292,12 +326,14 @@ private: /// the EgoDynamicsOnRoute to be visualized EgoDynamicsOnRoute _visualization_ego_dynamics; - void VisualizeEgoDynamics(carla::client::DebugHelper &dh, - carla::SharedPtr const &carla_ego_actor, - EgoDynamicsOnRoute const &ego_dynamics_on_route) const; + void VisualizeEgoDynamics( + carla::client::DebugHelper &dh, + carla::SharedPtr const &carla_ego_actor, + EgoDynamicsOnRoute const &ego_dynamics_on_route) const; mutable ::ad::map::route::FullRoute _visualization_debug_route; - mutable std::vector<::ad::map::lane::ENUBorder> _visualization_debug_enu_border; + mutable std::vector<::ad::map::lane::ENUBorder> + _visualization_debug_enu_border; }; } // namespace rss @@ -313,15 +349,21 @@ namespace std { * \returns The stream object. * */ -inline std::ostream &operator<<(std::ostream &out, const ::carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route) { +inline std::ostream &operator<<( + std::ostream &out, + const ::carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route) { out << "EgoDynamicsOnRoute(timestamp=" << ego_dynamics_on_route.timestamp - << ", time_since_epoch_check_start_ms=" << ego_dynamics_on_route.time_since_epoch_check_start_ms - << ", time_since_epoch_check_end_ms=" << ego_dynamics_on_route.time_since_epoch_check_end_ms + << ", time_since_epoch_check_start_ms=" + << ego_dynamics_on_route.time_since_epoch_check_start_ms + << ", time_since_epoch_check_end_ms=" + << ego_dynamics_on_route.time_since_epoch_check_end_ms << ", ego_speed=" << ego_dynamics_on_route.ego_speed - << ", min_stopping_distance=" << ego_dynamics_on_route.min_stopping_distance + << ", min_stopping_distance=" + << ego_dynamics_on_route.min_stopping_distance << ", ego_center=" << ego_dynamics_on_route.ego_center << ", ego_heading=" << ego_dynamics_on_route.ego_heading - << ", ego_center_within_route=" << ego_dynamics_on_route.ego_center_within_route + << ", ego_center_within_route=" + << ego_dynamics_on_route.ego_center_within_route << ", crossing_border=" << ego_dynamics_on_route.crossing_border << ", route_heading=" << ego_dynamics_on_route.route_heading << ", route_nominal_center=" << ego_dynamics_on_route.route_nominal_center @@ -335,4 +377,4 @@ inline std::ostream &operator<<(std::ostream &out, const ::carla::rss::EgoDynami << ')'; return out; } -} // namespace std +} // namespace std diff --git a/LibCarla/source/carla/rss/RssRestrictor.cpp b/LibCarla/source/carla/rss/RssRestrictor.cpp index 91f84af0c..770078574 100644 --- a/LibCarla/source/carla/rss/RssRestrictor.cpp +++ b/LibCarla/source/carla/rss/RssRestrictor.cpp @@ -4,24 +4,22 @@ // For a copy, see . #include "carla/rss/RssRestrictor.h" -#include "carla/rss/RssCheck.h" #include "carla/rpc/VehicleControl.h" #include "carla/rpc/VehiclePhysicsControl.h" +#include "carla/rss/RssCheck.h" +#include #include #include -#include namespace carla { namespace rss { -RssRestrictor::RssRestrictor() -{ - std::string loggerName = "RssRestrictor"; - _logger = spdlog::get(loggerName); - if (!_logger) - { - _logger = spdlog::create(loggerName); +RssRestrictor::RssRestrictor() { + std::string logger_name = "RssRestrictor"; + _logger = spdlog::get(logger_name); + if (!_logger) { + _logger = spdlog::create(logger_name); } //_logger->set_level(spdlog::level::debug); } @@ -29,21 +27,23 @@ RssRestrictor::RssRestrictor() RssRestrictor::~RssRestrictor() = default; carla::rpc::VehicleControl RssRestrictor::RestrictVehicleControl( - const carla::rpc::VehicleControl &vehicle_control, - const ::ad::rss::world::AccelerationRestriction &restriction, - const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route, - const carla::rpc::VehiclePhysicsControl &vehicle_physics) { + const carla::rpc::VehicleControl &vehicle_control, + const ::ad::rss::world::AccelerationRestriction &restriction, + const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route, + const carla::rpc::VehiclePhysicsControl &vehicle_physics) { carla::rpc::VehicleControl restricted_vehicle_control(vehicle_control); // Pretty basic implementation of a RSS restrictor modifying the // VehicleControl according to the given // restrictions. Basic braking and countersteering actions are applied. - // In case countersteering is not possible anymore (i.e. the lateral velocity reached zero), - // as a fallback longitudinal braking is applied instead (escalation strategy). + // In case countersteering is not possible anymore (i.e. the lateral velocity + // reached zero), + // as a fallback longitudinal braking is applied instead (escalation + // strategy). ::ad::physics::Acceleration zero_accel(0.0); - float mass = vehicle_physics.mass; + float mass = vehicle_physics.mass; float max_steer_angle_deg = 0.f; float sum_max_brake_torque = 0.f; float radius = 1.f; @@ -55,41 +55,60 @@ carla::rpc::VehicleControl RssRestrictor::RestrictVehicleControl( // do not apply any restrictions when in reverse gear if (!vehicle_control.reverse) { - _logger->debug("Lon {}, L {}, R {}; LatSpeed {}, Accel {}, Avg {}", restriction.longitudinalRange, restriction.lateralLeftRange, restriction.lateralRightRange, ego_dynamics_on_route.route_speed_lat, ego_dynamics_on_route.route_accel_lat, ego_dynamics_on_route.avg_route_accel_lat); - if (restriction.lateralLeftRange.maximum <= ::ad::physics::Acceleration(0.0)) { + _logger->debug("Lon {}, L {}, R {}; LatSpeed {}, Accel {}, Avg {}", + restriction.longitudinalRange, restriction.lateralLeftRange, + restriction.lateralRightRange, + ego_dynamics_on_route.route_speed_lat, + ego_dynamics_on_route.route_accel_lat, + ego_dynamics_on_route.avg_route_accel_lat); + if (restriction.lateralLeftRange.maximum <= + ::ad::physics::Acceleration(0.0)) { if (ego_dynamics_on_route.route_speed_lat < ::ad::physics::Speed(0.0)) { // driving to the left - if (ego_dynamics_on_route.route_speed_lon != ::ad::physics::Speed(0.0)) { - double angle_rad = std::atan(ego_dynamics_on_route.route_speed_lat / ego_dynamics_on_route.route_speed_lon); - float desired_steer_ratio = -180.f * static_cast(angle_rad / M_PI) / max_steer_angle_deg; - if ( ego_dynamics_on_route.crossing_border ) - { + if (ego_dynamics_on_route.route_speed_lon != + ::ad::physics::Speed(0.0)) { + double angle_rad = std::atan(ego_dynamics_on_route.route_speed_lat / + ego_dynamics_on_route.route_speed_lon); + float desired_steer_ratio = -180.f * + static_cast(angle_rad / M_PI) / + max_steer_angle_deg; + if (ego_dynamics_on_route.crossing_border) { desired_steer_ratio += 0.1f; } float orig_steer = restricted_vehicle_control.steer; - restricted_vehicle_control.steer = std::max(restricted_vehicle_control.steer, desired_steer_ratio); - restricted_vehicle_control.steer = std::min(restricted_vehicle_control.steer, 1.0f); + restricted_vehicle_control.steer = + std::max(restricted_vehicle_control.steer, desired_steer_ratio); + restricted_vehicle_control.steer = + std::min(restricted_vehicle_control.steer, 1.0f); _logger->debug("EgoVelocity: {}", ego_dynamics_on_route); - _logger->debug("Countersteer left to right: {} -> {}", orig_steer, restricted_vehicle_control.steer); + _logger->debug("Countersteer left to right: {} -> {}", orig_steer, + restricted_vehicle_control.steer); } } } - if (restriction.lateralRightRange.maximum <= ::ad::physics::Acceleration(0.0)) { + if (restriction.lateralRightRange.maximum <= + ::ad::physics::Acceleration(0.0)) { if (ego_dynamics_on_route.route_speed_lat > ::ad::physics::Speed(0.0)) { // driving to the right - if (ego_dynamics_on_route.route_speed_lon != ::ad::physics::Speed(0.0)) { - double angle_rad = std::atan(ego_dynamics_on_route.route_speed_lat / ego_dynamics_on_route.route_speed_lon); - float desired_steer_ratio = -180.f * static_cast(angle_rad / M_PI) / max_steer_angle_deg; - if ( ego_dynamics_on_route.crossing_border ) - { + if (ego_dynamics_on_route.route_speed_lon != + ::ad::physics::Speed(0.0)) { + double angle_rad = std::atan(ego_dynamics_on_route.route_speed_lat / + ego_dynamics_on_route.route_speed_lon); + float desired_steer_ratio = -180.f * + static_cast(angle_rad / M_PI) / + max_steer_angle_deg; + if (ego_dynamics_on_route.crossing_border) { desired_steer_ratio -= 0.1f; } float orig_steer = restricted_vehicle_control.steer; - restricted_vehicle_control.steer = std::min(restricted_vehicle_control.steer, desired_steer_ratio); - restricted_vehicle_control.steer = std::max(restricted_vehicle_control.steer, -1.0f); + restricted_vehicle_control.steer = + std::min(restricted_vehicle_control.steer, desired_steer_ratio); + restricted_vehicle_control.steer = + std::max(restricted_vehicle_control.steer, -1.0f); _logger->debug("EgoVelocity: {}", ego_dynamics_on_route); - _logger->debug("Countersteer right to left: {} -> {}", orig_steer, restricted_vehicle_control.steer); + _logger->debug("Countersteer right to left: {} -> {}", orig_steer, + restricted_vehicle_control.steer); } } } @@ -103,16 +122,20 @@ carla::rpc::VehicleControl RssRestrictor::RestrictVehicleControl( if (restriction.longitudinalRange.maximum < zero_accel) { restricted_vehicle_control.throttle = 0.0f; - double brake_acceleration = std::fabs(static_cast(restriction.longitudinalRange.minimum)); + double brake_acceleration = + std::fabs(static_cast(restriction.longitudinalRange.minimum)); double sum_brake_torque = mass * brake_acceleration * radius / 100.0; - restricted_vehicle_control.brake = std::min(static_cast(sum_brake_torque / sum_max_brake_torque), 1.0f); + restricted_vehicle_control.brake = std::min( + static_cast(sum_brake_torque / sum_max_brake_torque), 1.0f); } } - if(restricted_vehicle_control != vehicle_control) { - _logger->debug("Restrictor active: throttle({} -> {}), brake ({} -> {}). steer ({} -> {})", - vehicle_control.throttle, restricted_vehicle_control.throttle, - vehicle_control.brake, restricted_vehicle_control.brake, - vehicle_control.steer, restricted_vehicle_control.steer); + if (restricted_vehicle_control != vehicle_control) { + _logger->debug( + "Restrictor active: throttle({} -> {}), brake ({} -> {}). steer ({} -> " + "{})", + vehicle_control.throttle, restricted_vehicle_control.throttle, + vehicle_control.brake, restricted_vehicle_control.brake, + vehicle_control.steer, restricted_vehicle_control.steer); } return restricted_vehicle_control; } diff --git a/LibCarla/source/carla/rss/RssRestrictor.h b/LibCarla/source/carla/rss/RssRestrictor.h index 8d7512bd5..7c6bcbe8a 100644 --- a/LibCarla/source/carla/rss/RssRestrictor.h +++ b/LibCarla/source/carla/rss/RssRestrictor.h @@ -5,10 +5,10 @@ #pragma once -#include #include +#include -namespace ad{ +namespace ad { namespace rss { namespace world { @@ -27,27 +27,31 @@ class VehiclePhysicsControl; namespace rss { -/// @brief forward declararion for ego vehicles current dynamics in respect to the current route +/// @brief forward declararion for ego vehicles current dynamics in respect to +/// the current route struct EgoDynamicsOnRoute; /// @brief class implementing the RSS restrictions within CARLA class RssRestrictor { - public: +public: /// @brief constructor RssRestrictor(); /// @brief destructor ~RssRestrictor(); - /// @brief the actual function to restrict the given vehicle control input to mimick + /// @brief the actual function to restrict the given vehicle control input to + /// mimick /// RSS conform behavior by braking - /// Lateral braking is achieved by counter-steering, so is only a very rough solution - carla::rpc::VehicleControl RestrictVehicleControl(const carla::rpc::VehicleControl &vehicle_control, - const ::ad::rss::world::AccelerationRestriction &restriction, - const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route, - const carla::rpc::VehiclePhysicsControl &vehicle_physics); + /// Lateral braking is achieved by counter-steering, so is only a very + /// rough solution + carla::rpc::VehicleControl RestrictVehicleControl( + const carla::rpc::VehicleControl &vehicle_control, + const ::ad::rss::world::AccelerationRestriction &restriction, + const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route, + const carla::rpc::VehiclePhysicsControl &vehicle_physics); - private: +private: /// @brief the logger instance std::shared_ptr _logger; }; diff --git a/LibCarla/source/carla/rss/RssSensor.cpp b/LibCarla/source/carla/rss/RssSensor.cpp index 949ace6d4..8de6c1d41 100644 --- a/LibCarla/source/carla/rss/RssSensor.cpp +++ b/LibCarla/source/carla/rss/RssSensor.cpp @@ -13,22 +13,20 @@ #include #include "carla/Logging.h" -#include "carla/client/Sensor.h" #include "carla/client/Map.h" +#include "carla/client/Sensor.h" #include "carla/client/detail/Simulator.h" #include "carla/rss/RssCheck.h" #include "carla/sensor/data/RssResponse.h" - namespace carla { namespace client { RssSensor::RssSensor(ActorInitializer init) - : Sensor(std::move(init)) - , _on_tick_register_id(0u) - , _rss_check(std::make_shared<::carla::rss::RssCheck>()) - , _drop_route(false) { -} + : Sensor(std::move(init)), + _on_tick_register_id(0u), + _rss_check(std::make_shared<::carla::rss::RssCheck>()), + _drop_route(false) {} RssSensor::~RssSensor() = default; @@ -39,7 +37,8 @@ void RssSensor::Listen(CallbackFunctionType callback) { } if (GetParent() == nullptr) { - throw_exception(std::runtime_error(GetDisplayId() + ": not attached to vehicle")); + throw_exception( + std::runtime_error(GetDisplayId() + ": not attached to vehicle")); return; } @@ -49,23 +48,24 @@ void RssSensor::Listen(CallbackFunctionType callback) { ::ad::map::access::cleanup(); ::ad::map::access::initFromOpenDriveContent( - open_drive_content, 0.2, - ::ad::map::intersection::IntersectionType::TrafficLight, - ::ad::map::landmark::TrafficLightType::LEFT_STRAIGHT_RED_YELLOW_GREEN); + open_drive_content, 0.2, + ::ad::map::intersection::IntersectionType::TrafficLight, + ::ad::map::landmark::TrafficLightType::LEFT_STRAIGHT_RED_YELLOW_GREEN); auto self = boost::static_pointer_cast(shared_from_this()); log_debug(GetDisplayId(), ": subscribing to tick event"); - _on_tick_register_id = GetEpisode().Lock()->RegisterOnTickEvent( - [cb = std::move(callback), weak_self = WeakPtr(self)](const auto &snapshot) { - auto self = weak_self.lock(); - if (self != nullptr) { - auto data = self->TickRssSensor(snapshot.GetTimestamp()); - if (data != nullptr) { - cb(std::move(data)); - } - } - }); + _on_tick_register_id = GetEpisode().Lock()->RegisterOnTickEvent([ + cb = std::move(callback), weak_self = WeakPtr(self) + ](const auto &snapshot) { + auto self = weak_self.lock(); + if (self != nullptr) { + auto data = self->TickRssSensor(snapshot.GetTimestamp()); + if (data != nullptr) { + cb(std::move(data)); + } + } + }); } void RssSensor::Stop() { @@ -84,35 +84,42 @@ void RssSensor::Stop() { ::ad::map::access::cleanup(); } -const ::ad::rss::world::RssDynamics& RssSensor::GetEgoVehicleDynamics() const { +const ::ad::rss::world::RssDynamics &RssSensor::GetEgoVehicleDynamics() const { return _rss_check->GetEgoVehicleDynamics(); } -void RssSensor::SetEgoVehicleDynamics(const ::ad::rss::world::RssDynamics &ego_dynamics) { +void RssSensor::SetEgoVehicleDynamics( + const ::ad::rss::world::RssDynamics &ego_dynamics) { _rss_check->SetEgoVehicleDynamics(ego_dynamics); } -const ::ad::rss::world::RssDynamics& RssSensor::GetOtherVehicleDynamics() const { +const ::ad::rss::world::RssDynamics &RssSensor::GetOtherVehicleDynamics() + const { return _rss_check->GetOtherVehicleDynamics(); } -void RssSensor::SetOtherVehicleDynamics(const ::ad::rss::world::RssDynamics &other_vehicle_dynamics) { +void RssSensor::SetOtherVehicleDynamics( + const ::ad::rss::world::RssDynamics &other_vehicle_dynamics) { _rss_check->SetOtherVehicleDynamics(other_vehicle_dynamics); } -const ::carla::rss::RoadBoundariesMode& RssSensor::GetRoadBoundariesMode() const { +const ::carla::rss::RoadBoundariesMode &RssSensor::GetRoadBoundariesMode() + const { return _rss_check->GetRoadBoundariesMode(); } -void RssSensor::SetRoadBoundariesMode(const ::carla::rss::RoadBoundariesMode &road_boundaries_mode) { +void RssSensor::SetRoadBoundariesMode( + const ::carla::rss::RoadBoundariesMode &road_boundaries_mode) { _rss_check->SetRoadBoundariesMode(road_boundaries_mode); } -void RssSensor::AppendRoutingTarget(const ::carla::geom::Transform &routing_target) { +void RssSensor::AppendRoutingTarget( + const ::carla::geom::Transform &routing_target) { _rss_check->AppendRoutingTarget(routing_target); } -const std::vector<::carla::geom::Transform> RssSensor::GetRoutingTargets() const { +const std::vector<::carla::geom::Transform> RssSensor::GetRoutingTargets() + const { return _rss_check->GetRoutingTargets(); } @@ -120,21 +127,23 @@ void RssSensor::ResetRoutingTargets() { _rss_check->ResetRoutingTargets(); } -void RssSensor::SetVisualizationMode(const ::carla::rss::VisualizationMode &visualization_mode) { +void RssSensor::SetVisualizationMode( + const ::carla::rss::VisualizationMode &visualization_mode) { _rss_check->SetVisualizationMode(visualization_mode); } -const ::carla::rss::VisualizationMode& RssSensor::GetVisualizationMode() const { +const ::carla::rss::VisualizationMode &RssSensor::GetVisualizationMode() const { return _rss_check->GetVisualizationMode(); } void RssSensor::DropRoute() { // don't execute this immediately as it might break calculations completely // postpone to next sensor tick - _drop_route=true; + _drop_route = true; } -SharedPtr RssSensor::TickRssSensor(const Timestamp ×tamp) { +SharedPtr RssSensor::TickRssSensor( + const Timestamp ×tamp) { try { bool result = false; ::ad::rss::state::ProperResponse response; @@ -146,24 +155,29 @@ SharedPtr RssSensor::TickRssSensor(const Timestamp ×tam carla::client::World world = GetWorld(); SharedPtr actors = world.GetActors(); - if ( _drop_route ) - { - _drop_route=false; + if (_drop_route) { + _drop_route = false; _rss_check->DropRoute(); } // check all object<->ego pairs with RSS and calculate proper response - result = _rss_check->CheckObjects(timestamp, actors, GetParent(), response, acceleration_restriction, rss_state_snapshot, - ego_dynamics_on_route); + result = _rss_check->CheckObjects( + timestamp, actors, GetParent(), response, acceleration_restriction, + rss_state_snapshot, ego_dynamics_on_route); _processing_lock.unlock(); _rss_check->VisualizeResults(world, GetParent()); - spdlog::debug("RssSensor response: T={} S:{}->E:{} DeltaT:{}", timestamp.frame, ego_dynamics_on_route.time_since_epoch_check_start_ms, + spdlog::debug("RssSensor response: T={} S:{}->E:{} DeltaT:{}", + timestamp.frame, + ego_dynamics_on_route.time_since_epoch_check_start_ms, ego_dynamics_on_route.time_since_epoch_check_end_ms, - ego_dynamics_on_route.time_since_epoch_check_end_ms-ego_dynamics_on_route.time_since_epoch_check_start_ms); - return MakeShared(timestamp.frame, timestamp.elapsed_seconds, GetTransform(), - result, response, acceleration_restriction, rss_state_snapshot, ego_dynamics_on_route); + ego_dynamics_on_route.time_since_epoch_check_end_ms - + ego_dynamics_on_route.time_since_epoch_check_start_ms); + return MakeShared( + timestamp.frame, timestamp.elapsed_seconds, GetTransform(), result, + response, acceleration_restriction, rss_state_snapshot, + ego_dynamics_on_route); } else { spdlog::debug("RssSensor tick dropped: T={}", timestamp.frame); return nullptr; @@ -172,11 +186,11 @@ SharedPtr RssSensor::TickRssSensor(const Timestamp ×tam /// @todo do we need to unsubscribe the sensor here? std::cout << e.what() << std::endl; _processing_lock.unlock(); - spdlog::error("RssSensor tick exception"); + spdlog::error("RssSensor tick exception"); return nullptr; } catch (...) { _processing_lock.unlock(); - spdlog::error("RssSensor tick exception"); + spdlog::error("RssSensor tick exception"); return nullptr; } } diff --git a/LibCarla/source/carla/rss/RssSensor.h b/LibCarla/source/carla/rss/RssSensor.h index bcb200b63..e08faa282 100644 --- a/LibCarla/source/carla/rss/RssSensor.h +++ b/LibCarla/source/carla/rss/RssSensor.h @@ -17,9 +17,9 @@ namespace world { /// forward declaration of the RssDynamics struct struct RssDynamics; -} //namespace world -} //namespace rss -} // namespace ad +} // namespace world +} // namespace rss +} // namespace ad namespace carla { @@ -32,14 +32,14 @@ enum class VisualizationMode; /// forward declaration of the RssCheck class class RssCheck; -} //namespace rss +} // namespace rss namespace client { /// The RSS Sensor class implementing the carla::client::Sensor interface /// This class is a proxy to the RssCheck class class RssSensor : public Sensor { - public: +public: using Sensor::Sensor; /// @brief constructor @@ -62,47 +62,63 @@ class RssSensor : public Sensor { /// Return whether this Sensor instance is currently listening to the /// associated sensor in the simulator. - bool IsListening() const override { return _on_tick_register_id != 0u; } + bool IsListening() const override { + return _on_tick_register_id != 0u; + } - /// @returns the currently used dynamics of the ego vehicle (@see also RssCheck::GetEgoVehicleDynamics()) - const ::ad::rss::world::RssDynamics& GetEgoVehicleDynamics() const; - /// @brief sets the ego vehicle dynamics to be used by the ego vehicle (@see also RssCheck::SetEgoVehicleDynamics()) + /// @returns the currently used dynamics of the ego vehicle (@see also + /// RssCheck::GetEgoVehicleDynamics()) + const ::ad::rss::world::RssDynamics &GetEgoVehicleDynamics() const; + /// @brief sets the ego vehicle dynamics to be used by the ego vehicle (@see + /// also RssCheck::SetEgoVehicleDynamics()) void SetEgoVehicleDynamics(const ::ad::rss::world::RssDynamics &ego_dynamics); - /// @returns the currently used dynamics of other vehicles (@see also RssCheck::GetOtherVehicleDynamics()) - const ::ad::rss::world::RssDynamics& GetOtherVehicleDynamics() const; - /// @brief sets the ego vehicle dynamics to be used by other vehicles (@see also RssCheck::SetOtherVehicleDynamics()) - void SetOtherVehicleDynamics(const ::ad::rss::world::RssDynamics &other_vehicle_dynamics); + /// @returns the currently used dynamics of other vehicles (@see also + /// RssCheck::GetOtherVehicleDynamics()) + const ::ad::rss::world::RssDynamics &GetOtherVehicleDynamics() const; + /// @brief sets the ego vehicle dynamics to be used by other vehicles (@see + /// also RssCheck::SetOtherVehicleDynamics()) + void SetOtherVehicleDynamics( + const ::ad::rss::world::RssDynamics &other_vehicle_dynamics); - /// @returns the current mode for respecting the road boundaries (@see also RssCheck::GetRoadBoundariesMode()) - const ::carla::rss::RoadBoundariesMode& GetRoadBoundariesMode() const; - /// @brief sets the current mode for respecting the road boundaries (@see also RssCheck::SetRoadBoundariesMode()) - void SetRoadBoundariesMode(const ::carla::rss::RoadBoundariesMode &road_boundaries_mode); + /// @returns the current mode for respecting the road boundaries (@see also + /// RssCheck::GetRoadBoundariesMode()) + const ::carla::rss::RoadBoundariesMode &GetRoadBoundariesMode() const; + /// @brief sets the current mode for respecting the road boundaries (@see also + /// RssCheck::SetRoadBoundariesMode()) + void SetRoadBoundariesMode( + const ::carla::rss::RoadBoundariesMode &road_boundaries_mode); - /// @returns the current routing targets (@see also RssCheck::GetRoutingTargets()) + /// @returns the current routing targets (@see also + /// RssCheck::GetRoutingTargets()) const std::vector<::carla::geom::Transform> GetRoutingTargets() const; - /// @brief appends a routing target to the current routing target list (@see also RssCheck::AppendRoutingTarget()) + /// @brief appends a routing target to the current routing target list (@see + /// also RssCheck::AppendRoutingTarget()) void AppendRoutingTarget(const ::carla::geom::Transform &routing_target); - /// @brief resets the current routing target (@see also RssCheck::ResetRoutingTargets()) + /// @brief resets the current routing target (@see also + /// RssCheck::ResetRoutingTargets()) void ResetRoutingTargets(); - /// @brief sets the visualization mode (@see also RssCheck::SetVisualizationMode()) - void SetVisualizationMode(const ::carla::rss::VisualizationMode &visualization_mode); - /// @returns get the current visualization mode (@see also RssCheck::GetVisualizationMode()) - const ::carla::rss::VisualizationMode& GetVisualizationMode() const; + /// @brief sets the visualization mode (@see also + /// RssCheck::SetVisualizationMode()) + void SetVisualizationMode( + const ::carla::rss::VisualizationMode &visualization_mode); + /// @returns get the current visualization mode (@see also + /// RssCheck::GetVisualizationMode()) + const ::carla::rss::VisualizationMode &GetVisualizationMode() const; /// @brief drop the current route (@see also RssCheck::DropRoute()) void DropRoute(); private: - /// the acutal sensor tick callback function SharedPtr TickRssSensor(const Timestamp ×tamp); /// the id got when registering for the on tick event std::size_t _on_tick_register_id; - /// the mutex to protect the actual RSS processing and decouple the (slow) visualization + /// the mutex to protect the actual RSS processing and decouple the (slow) + /// visualization std::mutex _processing_lock; //// the object actually performing the RSS processing diff --git a/LibCarla/source/carla/sensor/data/RssResponse.h b/LibCarla/source/carla/sensor/data/RssResponse.h index 6a393e625..4ede252e9 100644 --- a/LibCarla/source/carla/sensor/data/RssResponse.h +++ b/LibCarla/source/carla/sensor/data/RssResponse.h @@ -5,69 +5,66 @@ #pragma once -#include "carla/sensor/SensorData.h" #include "carla/rss/RssCheck.h" +#include "carla/sensor/SensorData.h" namespace carla { namespace sensor { namespace data { - /// A RSS Response - class RssResponse : public SensorData { - public: +/// A RSS Response +class RssResponse : public SensorData { +public: + explicit RssResponse( + size_t frame_number, double timestamp, + const rpc::Transform &sensor_transform, const bool &response_valid, + const ::ad::rss::state::ProperResponse &response, + const ::ad::rss::world::AccelerationRestriction &acceleration_restriction, + const ::ad::rss::state::RssStateSnapshot &rss_state_snapshot, + const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route) + : SensorData(frame_number, timestamp, sensor_transform), + _response_valid(response_valid), + _response(response), + _acceleration_restriction(acceleration_restriction), + _rss_state_snapshot(rss_state_snapshot), + _ego_dynamics_on_route(ego_dynamics_on_route) {} - explicit RssResponse( - size_t frame_number, - double timestamp, - const rpc::Transform &sensor_transform, - const bool &response_valid, - const ::ad::rss::state::ProperResponse &response, - const ::ad::rss::world::AccelerationRestriction &acceleration_restriction, - const ::ad::rss::state::RssStateSnapshot &rss_state_snapshot, - const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route) - : SensorData(frame_number, timestamp, sensor_transform), - _response_valid(response_valid), - _response(response), - _acceleration_restriction(acceleration_restriction), - _rss_state_snapshot(rss_state_snapshot), - _ego_dynamics_on_route(ego_dynamics_on_route){} + bool GetResponseValid() const { + return _response_valid; + } - bool GetResponseValid() const { - return _response_valid; - } + const ::ad::rss::state::ProperResponse &GetProperResponse() const { + return _response; + } - const ::ad::rss::state::ProperResponse &GetProperResponse() const { - return _response; - } + const ::ad::rss::world::AccelerationRestriction &GetAccelerationRestriction() + const { + return _acceleration_restriction; + } - const ::ad::rss::world::AccelerationRestriction &GetAccelerationRestriction() const { - return _acceleration_restriction; - } + const ::ad::rss::state::RssStateSnapshot &GetRssStateSnapshot() const { + return _rss_state_snapshot; + } - const ::ad::rss::state::RssStateSnapshot &GetRssStateSnapshot() const { - return _rss_state_snapshot; - } + const carla::rss::EgoDynamicsOnRoute &GetEgoDynamicsOnRoute() const { + return _ego_dynamics_on_route; + } - const carla::rss::EgoDynamicsOnRoute &GetEgoDynamicsOnRoute() const { - return _ego_dynamics_on_route; - } +private: + /*! + * The validity of RSS calculation. + */ + bool _response_valid; - private: + ::ad::rss::state::ProperResponse _response; - /*! - * The validity of RSS calculation. - */ - bool _response_valid; + ::ad::rss::world::AccelerationRestriction _acceleration_restriction; - ::ad::rss::state::ProperResponse _response; + ::ad::rss::state::RssStateSnapshot _rss_state_snapshot; - ::ad::rss::world::AccelerationRestriction _acceleration_restriction; + carla::rss::EgoDynamicsOnRoute _ego_dynamics_on_route; +}; - ::ad::rss::state::RssStateSnapshot _rss_state_snapshot; - - carla::rss::EgoDynamicsOnRoute _ego_dynamics_on_route; - }; - -} // namespace data -} // namespace sensor -} // namespace carla +} // namespace data +} // namespace sensor +} // namespace carla diff --git a/PythonAPI/carla/source/libcarla/AdRss.cpp b/PythonAPI/carla/source/libcarla/AdRss.cpp index 8757364e0..9249d3f13 100644 --- a/PythonAPI/carla/source/libcarla/AdRss.cpp +++ b/PythonAPI/carla/source/libcarla/AdRss.cpp @@ -3,8 +3,8 @@ // This work is licensed under the terms of the MIT license. // For a copy, see . -#include #include +#include #include #include @@ -12,46 +12,45 @@ #ifdef LIBCARLA_PYTHON_MAJOR_2 extern "C" { - void initlibad_physics_python(); - void initlibad_rss_python(); - void initlibad_map_access_python(); - void initlibad_rss_map_integration_python(); +void initlibad_physics_python(); +void initlibad_rss_python(); +void initlibad_map_access_python(); +void initlibad_rss_map_integration_python(); } #endif #ifdef LIBCARLA_PYTHON_MAJOR_3 extern "C" { - void PyInit_libad_physics_python(); - void PyInit_libad_rss_python(); - void PyInit_libad_map_access_python(); - void PyInit_libad_rss_map_integration_python(); +void PyInit_libad_physics_python(); +void PyInit_libad_rss_python(); +void PyInit_libad_map_access_python(); +void PyInit_libad_rss_map_integration_python(); } #endif namespace carla { namespace rss { - std::ostream &operator<<(std::ostream &out, const RssRestrictor &) { - out << "RssRestrictor()"; - return out; - } +std::ostream &operator<<(std::ostream &out, const RssRestrictor &) { + out << "RssRestrictor()"; + return out; +} -} // namespace rss +} // namespace rss namespace sensor { namespace data { - std::ostream &operator<<(std::ostream &out, const RssResponse &resp) { - out << "RssResponse(frame=" << resp.GetFrame() - << ", timestamp=" << resp.GetTimestamp() - << ", valid=" << resp.GetResponseValid() - << ')'; - return out; - } +std::ostream &operator<<(std::ostream &out, const RssResponse &resp) { + out << "RssResponse(frame=" << resp.GetFrame() + << ", timestamp=" << resp.GetTimestamp() + << ", valid=" << resp.GetResponseValid() << ')'; + return out; +} -} // namespace data -} // namespace sensor -} // namespace carla +} // namespace data +} // namespace sensor +} // namespace carla static auto GetEgoVehicleDynamics(const carla::client::RssSensor &self) { ad::rss::world::RssDynamics ego_dynamics(self.GetEgoVehicleDynamics()); @@ -64,7 +63,8 @@ static auto GetOtherVehicleDynamics(const carla::client::RssSensor &self) { } static auto GetRoadBoundariesMode(const carla::client::RssSensor &self) { - carla::rss::RoadBoundariesMode road_boundaries_mode(self.GetRoadBoundariesMode()); + carla::rss::RoadBoundariesMode road_boundaries_mode( + self.GetRoadBoundariesMode()); return road_boundaries_mode; } @@ -79,7 +79,6 @@ static auto GetVisualizationMode(const carla::client::RssSensor &self) { } void export_ad_rss() { - #ifdef LIBCARLA_PYTHON_MAJOR_2 initlibad_physics_python(); initlibad_rss_python(); @@ -94,73 +93,98 @@ void export_ad_rss() { PyInit_libad_rss_map_integration_python(); #endif - using namespace boost::python; namespace cc = carla::client; namespace cs = carla::sensor; namespace csd = carla::sensor::data; class_("EgoDynamicsOnRoute") - .def_readwrite("timestamp", &carla::rss::EgoDynamicsOnRoute::timestamp) - .def_readwrite("time_since_epoch_check_start_ms", &carla::rss::EgoDynamicsOnRoute::time_since_epoch_check_start_ms) - .def_readwrite("time_since_epoch_check_end_ms", &carla::rss::EgoDynamicsOnRoute::time_since_epoch_check_end_ms) - .def_readwrite("ego_speed", &carla::rss::EgoDynamicsOnRoute::ego_speed) - .def_readwrite("min_stopping_distance", &carla::rss::EgoDynamicsOnRoute::min_stopping_distance) - .def_readwrite("ego_center", &carla::rss::EgoDynamicsOnRoute::ego_center) - .def_readwrite("ego_heading", &carla::rss::EgoDynamicsOnRoute::ego_heading) - .def_readwrite("ego_center_within_route", &carla::rss::EgoDynamicsOnRoute::ego_center_within_route) - .def_readwrite("crossing_border", &carla::rss::EgoDynamicsOnRoute::crossing_border) - .def_readwrite("route_heading", &carla::rss::EgoDynamicsOnRoute::route_heading) - .def_readwrite("route_nominal_center", &carla::rss::EgoDynamicsOnRoute::route_nominal_center) - .def_readwrite("heading_diff", &carla::rss::EgoDynamicsOnRoute::heading_diff) - .def_readwrite("route_speed_lat", &carla::rss::EgoDynamicsOnRoute::route_speed_lat) - .def_readwrite("route_speed_lon", &carla::rss::EgoDynamicsOnRoute::route_speed_lon) - .def_readwrite("route_accel_lat", &carla::rss::EgoDynamicsOnRoute::route_accel_lat) - .def_readwrite("route_accel_lon", &carla::rss::EgoDynamicsOnRoute::route_accel_lon) - .def_readwrite("avg_route_accel_lat", &carla::rss::EgoDynamicsOnRoute::avg_route_accel_lat) - .def_readwrite("avg_route_accel_lon", &carla::rss::EgoDynamicsOnRoute::avg_route_accel_lon) - .def(self_ns::str(self_ns::self)) - ; + .def_readwrite("timestamp", &carla::rss::EgoDynamicsOnRoute::timestamp) + .def_readwrite( + "time_since_epoch_check_start_ms", + &carla::rss::EgoDynamicsOnRoute::time_since_epoch_check_start_ms) + .def_readwrite( + "time_since_epoch_check_end_ms", + &carla::rss::EgoDynamicsOnRoute::time_since_epoch_check_end_ms) + .def_readwrite("ego_speed", &carla::rss::EgoDynamicsOnRoute::ego_speed) + .def_readwrite("min_stopping_distance", + &carla::rss::EgoDynamicsOnRoute::min_stopping_distance) + .def_readwrite("ego_center", &carla::rss::EgoDynamicsOnRoute::ego_center) + .def_readwrite("ego_heading", + &carla::rss::EgoDynamicsOnRoute::ego_heading) + .def_readwrite("ego_center_within_route", + &carla::rss::EgoDynamicsOnRoute::ego_center_within_route) + .def_readwrite("crossing_border", + &carla::rss::EgoDynamicsOnRoute::crossing_border) + .def_readwrite("route_heading", + &carla::rss::EgoDynamicsOnRoute::route_heading) + .def_readwrite("route_nominal_center", + &carla::rss::EgoDynamicsOnRoute::route_nominal_center) + .def_readwrite("heading_diff", + &carla::rss::EgoDynamicsOnRoute::heading_diff) + .def_readwrite("route_speed_lat", + &carla::rss::EgoDynamicsOnRoute::route_speed_lat) + .def_readwrite("route_speed_lon", + &carla::rss::EgoDynamicsOnRoute::route_speed_lon) + .def_readwrite("route_accel_lat", + &carla::rss::EgoDynamicsOnRoute::route_accel_lat) + .def_readwrite("route_accel_lon", + &carla::rss::EgoDynamicsOnRoute::route_accel_lon) + .def_readwrite("avg_route_accel_lat", + &carla::rss::EgoDynamicsOnRoute::avg_route_accel_lat) + .def_readwrite("avg_route_accel_lon", + &carla::rss::EgoDynamicsOnRoute::avg_route_accel_lon) + .def(self_ns::str(self_ns::self)); enum_("RoadBoundariesMode") - .value("Off", carla::rss::RoadBoundariesMode::Off) - .value("On", carla::rss::RoadBoundariesMode::On) - ; + .value("Off", carla::rss::RoadBoundariesMode::Off) + .value("On", carla::rss::RoadBoundariesMode::On); enum_("VisualizationMode") - .value("Off", carla::rss::VisualizationMode::Off) - .value("RouteOnly", carla::rss::VisualizationMode::RouteOnly) - .value("VehicleStateOnly", carla::rss::VisualizationMode::VehicleStateOnly) - .value("VehicleStateAndRoute", carla::rss::VisualizationMode::VehicleStateAndRoute) - .value("All", carla::rss::VisualizationMode::All) - ; + .value("Off", carla::rss::VisualizationMode::Off) + .value("RouteOnly", carla::rss::VisualizationMode::RouteOnly) + .value("VehicleStateOnly", + carla::rss::VisualizationMode::VehicleStateOnly) + .value("VehicleStateAndRoute", + carla::rss::VisualizationMode::VehicleStateAndRoute) + .value("All", carla::rss::VisualizationMode::All); - class_, boost::noncopyable, boost::shared_ptr>("RssResponse", no_init) - .add_property("response_valid", &csd::RssResponse::GetResponseValid) - .add_property("proper_response", CALL_RETURNING_COPY(csd::RssResponse, GetProperResponse)) - .add_property("acceleration_restriction", CALL_RETURNING_COPY(csd::RssResponse, GetAccelerationRestriction)) - .add_property("rss_state_snapshot", CALL_RETURNING_COPY(csd::RssResponse, GetRssStateSnapshot)) - .add_property("ego_dynamics_on_route", CALL_RETURNING_COPY(csd::RssResponse, GetEgoDynamicsOnRoute)) - .def(self_ns::str(self_ns::self)) - ; + class_, boost::noncopyable, + boost::shared_ptr>("RssResponse", no_init) + .add_property("response_valid", &csd::RssResponse::GetResponseValid) + .add_property("proper_response", + CALL_RETURNING_COPY(csd::RssResponse, GetProperResponse)) + .add_property( + "acceleration_restriction", + CALL_RETURNING_COPY(csd::RssResponse, GetAccelerationRestriction)) + .add_property("rss_state_snapshot", + CALL_RETURNING_COPY(csd::RssResponse, GetRssStateSnapshot)) + .add_property( + "ego_dynamics_on_route", + CALL_RETURNING_COPY(csd::RssResponse, GetEgoDynamicsOnRoute)) + .def(self_ns::str(self_ns::self)); - class_, boost::noncopyable, boost::shared_ptr> - ("RssSensor", no_init) - .add_property("ego_vehicle_dynamics", &GetEgoVehicleDynamics, &cc::RssSensor::SetEgoVehicleDynamics) - .add_property("other_vehicle_dynamics", &GetOtherVehicleDynamics, &cc::RssSensor::SetOtherVehicleDynamics) - .add_property("road_boundaries_mode", &GetRoadBoundariesMode, &cc::RssSensor::SetRoadBoundariesMode) - .add_property("visualization_mode", &GetVisualizationMode, &cc::RssSensor::SetVisualizationMode) - .add_property("routing_targets", &GetRoutingTargets) - .def("append_routing_target", &cc::RssSensor::AppendRoutingTarget) - .def("reset_routing_targets", &cc::RssSensor::ResetRoutingTargets) - .def("drop_route", &cc::RssSensor::DropRoute) - .def(self_ns::str(self_ns::self)) - ; + class_, boost::noncopyable, + boost::shared_ptr>("RssSensor", no_init) + .add_property("ego_vehicle_dynamics", &GetEgoVehicleDynamics, + &cc::RssSensor::SetEgoVehicleDynamics) + .add_property("other_vehicle_dynamics", &GetOtherVehicleDynamics, + &cc::RssSensor::SetOtherVehicleDynamics) + .add_property("road_boundaries_mode", &GetRoadBoundariesMode, + &cc::RssSensor::SetRoadBoundariesMode) + .add_property("visualization_mode", &GetVisualizationMode, + &cc::RssSensor::SetVisualizationMode) + .add_property("routing_targets", &GetRoutingTargets) + .def("append_routing_target", &cc::RssSensor::AppendRoutingTarget) + .def("reset_routing_targets", &cc::RssSensor::ResetRoutingTargets) + .def("drop_route", &cc::RssSensor::DropRoute) + .def(self_ns::str(self_ns::self)); - class_> - ("RssRestrictor", no_init) - .def(init<>()) - .def("restrict_vehicle_control", &carla::rss::RssRestrictor::RestrictVehicleControl, (arg("restriction"), arg("vehicleControl"))) - .def(self_ns::str(self_ns::self)) - ; + class_>("RssRestrictor", no_init) + .def(init<>()) + .def("restrict_vehicle_control", + &carla::rss::RssRestrictor::RestrictVehicleControl, + (arg("restriction"), arg("vehicleControl"))) + .def(self_ns::str(self_ns::self)); } diff --git a/PythonAPI/carla/source/libcarla/Geom.cpp b/PythonAPI/carla/source/libcarla/Geom.cpp index 64cba8e3a..c516637e5 100644 --- a/PythonAPI/carla/source/libcarla/Geom.cpp +++ b/PythonAPI/carla/source/libcarla/Geom.cpp @@ -176,7 +176,7 @@ void export_geom() { .def(self_ns::str(self_ns::self)) ; - class_>("vector_of_Transform") + class_>("vector_of_transform") .def(boost::python::vector_indexing_suite>()) .def(self_ns::str(self_ns::self)) ; diff --git a/Util/Formatting/clang-format b/Util/Formatting/clang-format new file mode 100644 index 000000000..a8fa89d1c --- /dev/null +++ b/Util/Formatting/clang-format @@ -0,0 +1,95 @@ +--- +Language: Cpp +# BasedOnStyle: Google +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlinesLeft: true +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: Empty +AllowShortIfStatementsOnASingleLine: true +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: true +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 2 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: true +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] +IncludeCategories: + - Regex: '^<.*\.h>' + Priority: 1 + - Regex: '^<.*' + Priority: 2 + - Regex: '.*' + Priority: 3 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IndentCaseLabels: true +IndentWidth: 2 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: false +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +ReflowComments: true +SortIncludes: true +SpaceAfterCStyleCast: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Auto +TabWidth: 8 +UseTab: Never +... + diff --git a/Util/Formatting/codeformat.py b/Util/Formatting/codeformat.py new file mode 100755 index 000000000..11a753d17 --- /dev/null +++ b/Util/Formatting/codeformat.py @@ -0,0 +1,429 @@ +#!/usr/bin/python + +# +# Copyright (c) 2017-2020 Intel Corporation +# +# Helper script for code formatting using clang-format-3.9 and autopep + +import argparse +import filecmp +import os +import re +import sets +import subprocess +import sys +from termcolor import cprint + +SCRIPT_VERSION = "1.3" + + +class CodeFormatter: + + def __init__(self, command, expectedVersion, formatCommandArguments, verifyCommandArguments, verifyOutputIsDiff, fileEndings, fileDescription, installCommand): + self.command = command + self.expectedVersion = expectedVersion + self.formatCommandArguments = formatCommandArguments + self.verifyCommandArguments = verifyCommandArguments + self.verifyOutputIsDiff = verifyOutputIsDiff + self.fileEndings = fileEndings + self.fileDescription = fileDescription + self.installCommand = installCommand + + def verifyFormatterVersion(self): + try: + versionOutput = subprocess.check_output([self.command, "--version"]).rstrip('\r\n') + if self.expectedVersion != "": + if versionOutput.startswith(self.expectedVersion): + print("[OK] Found formatter '" + versionOutput + "'") + return + else: + cprint("[NOT OK] Found '" + versionOutput + "'", "red") + cprint("[NOT OK] Version string does not start with '" + self.expectedVersion + "'", "red") + else: + return + except: + cprint("[ERROR] Could not run " + self.command, "red") + cprint("[INFO] Please check if correct version is installed or install with '" + + self.installCommand + "'", "blue") + sys.exit(1) + + def printInputFiles(self): + if len(self.inputFiles) > 0: + print("Found " + self.fileDescription + " files:") + for fileName in self.inputFiles: + print(fileName) + print("") + + def formatFile(self, fileName): + commandList = [self.command] + commandList.extend(self.formatCommandArguments) + commandList.append(fileName) + try: + subprocess.check_output(commandList, stderr=subprocess.STDOUT) + print("[OK] " + fileName) + except subprocess.CalledProcessError as e: + cprint("[ERROR] " + fileName + " (" + e.output.rstrip('\r\n') + ")", "red") + return True + return False + + def performGitDiff(self, fileName, verifyOutput): + try: + diffProcess = subprocess.Popen( + ["git", "diff", "--color=always", "--exit-code", "--no-index", "--", fileName, "-"], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE) + diffOutput, _ = diffProcess.communicate(verifyOutput) + if diffProcess.returncode == 0: + diffOutput = "" + except OSError: + cprint("[ERROR] Failed to run git diff on " + fileName, "red") + return (True, "") + return (False, diffOutput) + + def verifyFile(self, fileName, printDiff): + commandList = [self.command] + commandList.extend(self.verifyCommandArguments) + commandList.append(fileName) + try: + verifyOutput = subprocess.check_output(commandList, stderr=subprocess.STDOUT) + except subprocess.CalledProcessError as e: + cprint("[ERROR] " + fileName + " (" + e.output.rstrip('\r\n') + ")", "red") + return True + + diffOutput = "" + if self.verifyOutputIsDiff: + diffOutput = verifyOutput + else: + status, diffOutput = self.performGitDiff(fileName, verifyOutput) + if status: + return True + + if diffOutput != "": + cprint("[NOT OK] " + fileName, "red") + if printDiff: + print(diffOutput.rstrip('\r\n')) + return True + + print("[OK] " + fileName) + return False + + +class CodeFormatterClang(CodeFormatter): + CLANG_FORMAT_FILE = ".clang-format" + CHECKED_IN_CLANG_FORMAT_FILE = "clang-format" + CODE_FORMAT_IGNORE_FILE = ".codeformatignore" + + def __init__(self): + CodeFormatter.__init__(self, + command="clang-format-3.9", + expectedVersion="clang-format version 3.9", + formatCommandArguments=["-style=file", "-fallback-style=none", "-i"], + verifyCommandArguments=["-style=file", "-fallback-style=none"], + verifyOutputIsDiff=False, + fileEndings=["cpp", "hpp", "c", "h", "cc"], + fileDescription="source and header", + installCommand="sudo apt-get install clang-format-3.9") + self.scriptPath = os.path.dirname(os.path.abspath(__file__)) + self.checkedInClangFormatFile = os.path.join(self.scriptPath, CodeFormatterClang.CHECKED_IN_CLANG_FORMAT_FILE) + + def verifyFormatterVersion(self): + CodeFormatter.verifyFormatterVersion(self) + self.verifyClangFormatFileExistsAndMatchesCheckedIn() + + def verifyCheckedInClangFormatFileExists(self): + if os.path.exists(self.checkedInClangFormatFile): + print("[OK] Found " + CodeFormatterClang.CHECKED_IN_CLANG_FORMAT_FILE + " file (the one that should be in a repository) " + + self.checkedInClangFormatFile) + else: + cprint("[WARN] Not found " + CodeFormatterClang.CHECKED_IN_CLANG_FORMAT_FILE + " file " + + self.checkedInClangFormatFile, "yellow") + self.confirmWithUserClangFormatFileCantBeVerified() + + def confirmWithUserClangFormatFileCantBeVerified(self): + if not self.args.yes: + answer = raw_input("Are you sure your .clang-format file is up-to-date and you want to continue? (y/N)") + if answer != "y": + sys.exit(1) + + def verifyClangFormatFileExistsAndMatchesCheckedIn(self): + self.verifyCheckedInClangFormatFileExists() + foundClangFormatFiles = sets.Set() + for fileName in self.inputFiles: + dirName = os.path.dirname(os.path.abspath(fileName)) + if not self.findClangFormatFileStartingFrom(dirName, fileName, foundClangFormatFiles): + sys.exit(1) + + def findClangFormatFileStartingFrom(self, dirName, fileName, foundClangFormatFiles): + clangFormatFile = os.path.join(dirName, CodeFormatterClang.CLANG_FORMAT_FILE) + if os.path.exists(clangFormatFile): + if clangFormatFile not in foundClangFormatFiles: + foundClangFormatFiles.add(clangFormatFile) + if os.path.exists(self.checkedInClangFormatFile) and \ + not filecmp.cmp(self.checkedInClangFormatFile, clangFormatFile): + cprint("[WARN] " + clangFormatFile + " does not match " + self.checkedInClangFormatFile, "yellow") + self.confirmWithUserClangFormatFileCantBeVerified() + else: + print("[OK] Found " + CodeFormatterClang.CLANG_FORMAT_FILE + + " file (used by the formatter) " + clangFormatFile) + return True + else: + dirNameOneLevelUp = os.path.dirname(dirName) + if dirNameOneLevelUp == dirName: + # dirName was already root folder -> clang-format file not found + cprint("[ERROR] Not found " + CodeFormatterClang.CLANG_FORMAT_FILE + " for " + + fileName + " in same directory or in any parent directory", "red") + return False + else: + return self.findClangFormatFileStartingFrom(dirNameOneLevelUp, fileName, foundClangFormatFiles) + + +class CodeFormatterAutopep(CodeFormatter): + + def __init__(self): + CodeFormatter.__init__(self, + command="autopep8", + expectedVersion="", + formatCommandArguments=["--in-place", "--max-line-length=119"], + verifyCommandArguments=["--diff", "--max-line-length=119"], + verifyOutputIsDiff=True, + fileEndings=["py"], + fileDescription="python", + installCommand="sudo apt-get install python-pep8 python-autopep8") + + +class CodeFormat: + + def __init__(self): + self.failure = False + self.codeFormatterInstances = [] + return + + def parseCommandLine(self): + parser = argparse.ArgumentParser( + description="Helper script for code formatting.") + parser.add_argument("input", nargs="+", + help="files or directories to process") + parser.add_argument("-v", "--verify", action="store_true", + help="do not change file, but only verify the format is correct") + parser.add_argument("-d", "--diff", action="store_true", + help="show diff, implies verify mode") + parser.add_argument("-e", "--exclude", nargs="+", metavar="exclude", + help="exclude files or directories containing words from the exclude list in their names") + parser.add_argument("-y", "--yes", action="store_true", + help="do not ask for confirmation before formatting more than one file") + parser.add_argument("--version", action="version", version="%(prog)s " + SCRIPT_VERSION) + self.args = parser.parse_args() + if self.args.diff: + self.args.verify = True + + def addCodeFormatter(self, codeFormatterInstance): + self.codeFormatterInstances.append(codeFormatterInstance) + + def scanForInputFiles(self): + for formatterInstance in self.codeFormatterInstances: + filePattern = re.compile("^[^.].*\.(" + "|".join(formatterInstance.fileEndings) + ")$") + formatterInstance.inputFiles = [] + for fileOrDirectory in self.args.input: + if os.path.exists(fileOrDirectory): + formatterInstance.inputFiles.extend(self.scanFileOrDirectory(fileOrDirectory, filePattern)) + else: + cprint("[WARN] Cannot find '" + fileOrDirectory + "'", "yellow") + + def scanFileOrDirectory(self, fileOrDirectory, filePattern): + fileList = [] + if os.path.isdir(fileOrDirectory): + for root, directories, fileNames in os.walk(fileOrDirectory): + directories[:] = self.filterDirectories(root, directories) + for filename in filter(lambda name: filePattern.match(name), fileNames): + fullFilename = os.path.join(root, filename) + if self.isFileNotExcluded(fullFilename): + fileList.append(fullFilename) + else: + if self.isFileNotExcluded(fileOrDirectory) and (filePattern.match(os.path.basename(fileOrDirectory)) is not None): + fileList.append(fileOrDirectory) + return fileList + + def filterDirectories(self, root, directories): + # Exclude hidden directories and all directories that have a CODE_FORMAT_IGNORE_FILE + directories[:] = [directory for directory in directories if + not directory.startswith(".") and + not os.path.exists(os.path.join(root, directory, CodeFormatterClang.CODE_FORMAT_IGNORE_FILE))] + return directories + + def isFileNotExcluded(self, fileName): + if self.args.exclude is not None: + for excluded in self.args.exclude: + if excluded in fileName: + return False + if os.path.islink(fileName): + return False + + return True + + def confirmWithUserFileIsOutsideGit(self, fileName): + cprint("[WARN] File is not in a Git repo: " + fileName, "yellow") + answer = raw_input("Are you sure to code format it anyway? (y/Q)") + if answer != "y": + sys.exit(1) + + def confirmWithUserFileIsUntracked(self, fileName): + cprint("[WARN] File is untracked in Git: " + fileName, "yellow") + answer = raw_input("Are you sure to code format it anyway? (y/Q)") + if answer != "y": + sys.exit(1) + + def confirmWithUserGitRepoIsNotClean(self, gitRepo): + cprint("[WARN] Git repo is not clean: " + gitRepo, "yellow") + answer = raw_input("Are you sure to code format files in it anyway? (y/Q)") + if answer != "y": + sys.exit(1) + + def checkInputFilesAreInCleanGitReposAndAreTracked(self): + if self.args.verify or self.args.yes: + return + gitRepos = sets.Set() + for formatterInstance in self.codeFormatterInstances: + for fileName in formatterInstance.inputFiles: + gitRepo = self.getGitRepoForFile(fileName) + if gitRepo is None: + self.confirmWithUserFileIsOutsideGit(fileName) + else: + self.gitUpdateIndexRefresh(gitRepo) + if not self.isTrackedFile(fileName): + self.confirmWithUserFileIsUntracked(fileName) + elif gitRepo not in gitRepos: + gitRepos.add(gitRepo) + if not self.isCleanGitRepo(gitRepo): + self.confirmWithUserGitRepoIsNotClean(gitRepo) + + def getGitRepoForFile(self, fileName): + if not self.isInsideGitRepo(fileName): + return None + try: + gitProcess = subprocess.Popen(["git", "rev-parse", "--show-toplevel"], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + cwd=os.path.dirname(fileName)) + gitOutput, _ = gitProcess.communicate() + if gitProcess.returncode == 0: + return gitOutput.rstrip('\r\n') + except OSError: + cprint("[ERROR] Failed to run 'git rev-parse --show-toplevel' for " + fileName, "red") + return None + + def isInsideGitRepo(self, fileName): + try: + gitProcess = subprocess.Popen(["git", "rev-parse", "--is-inside-work-tree"], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + cwd=os.path.dirname(fileName)) + gitOutput, _ = gitProcess.communicate() + if gitProcess.returncode == 0: + return gitOutput.rstrip('\r\n') == "true" + except OSError: + cprint("[ERROR] Failed to run 'git rev-parse --is-inside-work-tree' for " + fileName, "red") + return False + + def isTrackedFile(self, fileName): + try: + gitProcess = subprocess.Popen(["git", "ls-files", "--error-unmatch", "--", os.path.basename(fileName)], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + cwd=os.path.dirname(fileName)) + _, _ = gitProcess.communicate() + if gitProcess.returncode == 0: + return True + except OSError: + cprint("[ERROR] Failed to run 'git ls-files --error-unmatch' for " + fileName, "red") + return False + + def isCleanGitRepo(self, gitRepo): + try: + gitProcess = subprocess.Popen(["git", "diff-index", "--quiet", "HEAD", "--"], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + cwd=gitRepo) + _, _ = gitProcess.communicate() + if gitProcess.returncode == 0: + return True + except OSError: + cprint("[ERROR] Failed to run 'git diff-index --quiet HEAD --' for " + gitRepo, "red") + return False + + def gitUpdateIndexRefresh(self, gitRepo): + try: + gitProcess = subprocess.Popen(["git", "update-index", "-q", "--ignore-submodules", "--refresh"], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + cwd=gitRepo) + _, _ = gitProcess.communicate() + if gitProcess.returncode == 0: + return True + except OSError: + cprint("[ERROR] Failed to run 'git update-index -q --ignore-submodules --refresh' for " + gitRepo, "red") + return False + + def verifyFormatterVersion(self): + for formatterInstance in self.codeFormatterInstances: + if len(formatterInstance.inputFiles) > 0: + formatterInstance.verifyFormatterVersion() + + def printMode(self): + if self.args.verify: + cprint("VERIFY MODE", attrs=["bold"]) + else: + cprint("FORMAT MODE", attrs=["bold"]) + + def processFiles(self): + for formatterInstance in self.codeFormatterInstances: + for fileName in formatterInstance.inputFiles: + if self.args.verify: + self.failure |= formatterInstance.verifyFile(fileName, self.args.diff) + else: + self.failure |= formatterInstance.formatFile(fileName) + + def numberOfInputFiles(self): + count = 0 + for formatterInstance in self.codeFormatterInstances: + count += len(formatterInstance.inputFiles) + return count + + def confirmWithUser(self): + if self.numberOfInputFiles() == 0: + cprint("[WARN] No input files (or file endings unknown)", "yellow") + elif (not self.args.verify) and (not self.args.yes) and self.numberOfInputFiles() > 1: + for formatterInstance in self.codeFormatterInstances: + formatterInstance.printInputFiles() + answer = raw_input("Are you sure to code format " + str(self.numberOfInputFiles()) + " files? (y/N)") + if answer != "y": + sys.exit(1) + + +def main(): + codeFormat = CodeFormat() + codeFormat.parseCommandLine() + codeFormat.printMode() + + codeFormat.addCodeFormatter(CodeFormatterClang()) + codeFormat.addCodeFormatter(CodeFormatterAutopep()) + + codeFormat.scanForInputFiles() + codeFormat.verifyFormatterVersion() + codeFormat.confirmWithUser() + codeFormat.checkInputFilesAreInCleanGitReposAndAreTracked() + codeFormat.processFiles() + if codeFormat.failure: + cprint("FAILURE", "red") + sys.exit(1) + else: + cprint("SUCCESS", "green") + sys.exit(0) + +if __name__ == "__main__": + main()