p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Debug settings are still on. 

Added extra points in tight curves for PID improvement.
Fixed Hybrid Mode, now moves in the direction of the next waypoint.
Reduced calculations in MotionPlanningStage.cpp
This commit is contained in:
Jacopo Bartiromo 2021-05-13 11:05:58 +02:00 committed by Jacopo Bartiromo
parent 5f0f95879e
commit 0002f0b478
7 changed files with 108 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
LibCarla/source/carla/trafficmanager/Constants.h
View File

@ -54,8 +54,8 @@ static const float HORIZON_RATE = RATE(MAXIMUM_HORIZON_LENGTH,
} // namespace PathBufferUpdate
namespace WaypointSelection {
static const float TARGET_WAYPOINT_TIME_HORIZON = 0.3f;
static const float TARGET_WAYPOINT_HORIZON_LENGTH = 0.8f;
static const float TARGET_WAYPOINT_TIME_HORIZON = 0.1f;
static const float TARGET_WAYPOINT_HORIZON_LENGTH = 0.6f;
static const float JUNCTION_LOOK_AHEAD = 6.0f;
static const float SAFE_DISTANCE_AFTER_JUNCTION = 6.0f;
static const float MIN_JUNCTION_LENGTH = 8.0f;
@ -97,6 +97,7 @@ namespace Map {
static const float INFINITE_DISTANCE = std::numeric_limits<float>::max();
static const float MAX_GEODESIC_GRID_LENGTH = 20.0f;
static const double MAP_RESOLUTION = 5.0;
static const double MAX_WPT_DISTANCE = MAP_RESOLUTION/2.0 + SQUARE(MAP_RESOLUTION);
static const float INV_MAP_RESOLUTION = 0.2f;
} // namespace Map

23
LibCarla/source/carla/trafficmanager/InMemoryMap.cpp
View File

@ -64,7 +64,7 @@ namespace traffic_manager {
return result;
}
void InMemoryMap::SetUp() {
void InMemoryMap::SetUp(cc::DebugHelper &debug_helper) {
// 1. Building segment topology (i.e., defining set of segment predecessors and successors)
assert(_world_map != nullptr && "No map reference found.");
@ -139,6 +139,9 @@ namespace traffic_manager {
auto compare_s = [](const SimpleWaypointPtr &swp1, const SimpleWaypointPtr &swp2) {
return (swp1->GetWaypoint()->GetDistance() < swp2->GetWaypoint()->GetDistance());
};
auto angle = [](cg::Location l1, cg::Location l2) {
return cg::Math::GetVectorAngle(l1, l2);
};
GeoGridId geodesic_grid_id_counter = -1;
for (auto &segment: segment_map) {
@ -154,6 +157,23 @@ namespace traffic_manager {
std::reverse(segment_waypoints.begin(), segment_waypoints.end());
}
// Adding more waypoints if the angle is too tight or if they are too distant,
for (std::size_t i = 0; i < segment_waypoints.size() - 1; ++i) {
float distance = distance_squared(segment_waypoints.at(i)->GetLocation(), segment_waypoints.at(i+1)->GetLocation());
if (angle(segment_waypoints.at(i)->GetLocation(), segment_waypoints.at(i+1)->GetLocation()) > 0.12f) { // 7 deg
debug_helper.DrawPoint(segment_waypoints.at(i)->GetLocation(), 0.15f, {0u, 0u, 255u}, 556.05f);
auto new_waypoint = segment_waypoints.at(i)->GetWaypoint()->GetNext(std::sqrt(distance)/2.0f).front();
i++;
segment_waypoints.insert(segment_waypoints.begin()+static_cast<int64_t>(i), std::make_shared<SimpleWaypoint>(new_waypoint));
} else if (distance > MAX_WPT_DISTANCE) {
auto new_waypoint = segment_waypoints.at(i)->GetWaypoint()->GetNext(std::sqrt(distance)/2.0f).front();
i++;
debug_helper.DrawPoint(segment_waypoints.at(i)->GetLocation(), 0.15f, {0u, 255u, 255u}, 556.05f);
segment_waypoints.insert(segment_waypoints.begin()+static_cast<int64_t>(i), std::make_shared<SimpleWaypoint>(new_waypoint));
}
}
// Placing intra-segment connections.
cg::Location grid_edge_location = segment_waypoints.front()->GetLocation();
for (std::size_t i = 0; i < segment_waypoints.size() - 1; ++i) {
@ -189,6 +209,7 @@ namespace traffic_manager {
for (auto &simple_waypoint: dense_topology) {
if (simple_waypoint != nullptr) {
const cg::Location loc = simple_waypoint->GetLocation();
debug_helper.DrawPoint(loc, 0.1f, {255u, 0u, 0u}, 555.05f);
Point3D point(loc.x, loc.y, loc.z);
rtree.insert(std::make_pair(point, simple_waypoint));
}

2
LibCarla/source/carla/trafficmanager/InMemoryMap.h
View File

@ -68,7 +68,7 @@ namespace bgi = boost::geometry::index;
/// This method constructs the local map with a resolution of
/// sampling_resolution.
void SetUp();
void SetUp(cc::DebugHelper &debug_helper);
/// This method returns the closest waypoint to a given location on the map.
SimpleWaypointPtr GetWaypoint(const cg::Location loc) const;

165
LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp
View File

@ -29,7 +29,8 @@ MotionPlanStage::MotionPlanStage(
const CollisionFrame&collision_frame,
const TLFrame &tl_frame,
const cc::World &world,
ControlFrame &output_array)
ControlFrame &output_array,
cc::DebugHelper &debug_helper)
: vehicle_id_list(vehicle_id_list),
simulation_state(simulation_state),
parameters(parameters),
@ -43,7 +44,8 @@ MotionPlanStage::MotionPlanStage(
collision_frame(collision_frame),
tl_frame(tl_frame),
world(world),
output_array(output_array) {}
output_array(output_array),
debug_helper(debug_helper) {}
void MotionPlanStage::Update(const unsigned long index) {
const ActorId actor_id = vehicle_id_list.at(index);
@ -56,41 +58,12 @@ void MotionPlanStage::Update(const unsigned long index) {
const LocalizationData &localization = localization_frame.at(index);
const CollisionHazardData &collision_hazard = collision_frame.at(index);
const bool &tl_hazard = tl_frame.at(index);
const float target_point_distance = std::max(ego_speed * TARGET_WAYPOINT_TIME_HORIZON,
TARGET_WAYPOINT_HORIZON_LENGTH);
const SimpleWaypointPtr &target_waypoint = GetTargetWaypoint(waypoint_buffer, target_point_distance).first;
const cg::Location target_location = target_waypoint->GetLocation();
float dot_product = DeviationDotProduct(ego_location, ego_heading, target_location);
float cross_product = DeviationCrossProduct(ego_location, ego_heading, target_location);
dot_product = 1.0f - dot_product;
if (cross_product < 0.0f) {
dot_product *= -1.0f;
}
const float current_deviation = dot_product;
current_timestamp = world.GetSnapshot().GetTimestamp();
// If previous state for vehicle not found, initialize state entry.
if (pid_state_map.find(actor_id) == pid_state_map.end()) {
const auto initial_state = StateEntry{current_timestamp, 0.0f, 0.0f, 0.0f, 0.0f};
pid_state_map.insert({actor_id, initial_state});
}
// Retrieving the previous state.
traffic_manager::StateEntry previous_state;
previous_state = pid_state_map.at(actor_id);
// Select PID parameters.
std::vector<float> longitudinal_parameters;
std::vector<float> lateral_parameters;
if (ego_speed > HIGHWAY_SPEED) {
longitudinal_parameters = highway_longitudinal_parameters;
lateral_parameters = highway_lateral_parameters;
} else {
longitudinal_parameters = urban_longitudinal_parameters;
lateral_parameters = urban_lateral_parameters;
}
StateEntry current_state;
// Instanciating teleportation transform.
cg::Transform teleportation_transform;
// Target velocity for vehicle.
const float ego_speed_limit = simulation_state.GetSpeedLimit(actor_id);
float max_target_velocity = parameters.GetVehicleTargetVelocity(actor_id, ego_speed_limit) / 3.6f;
@ -107,13 +80,43 @@ void MotionPlanStage::Update(const unsigned long index) {
// In case of collision or traffic light hazard.
bool emergency_stop = tl_hazard || collision_emergency_stop || !safe_after_junction;
ActuationSignal actuation_signal{0.0f, 0.0f, 0.0f};
cg::Transform teleportation_transform;
// If physics is enabled for the vehicle, use PID controller.
StateEntry current_state;
if (ego_physics_enabled) {
ActuationSignal actuation_signal{0.0f, 0.0f, 0.0f};
const float target_point_distance = std::max(ego_speed * TARGET_WAYPOINT_TIME_HORIZON,
TARGET_WAYPOINT_HORIZON_LENGTH);
const SimpleWaypointPtr &target_waypoint = GetTargetWaypoint(waypoint_buffer, target_point_distance).first;
const cg::Location target_location = target_waypoint->GetLocation();
debug_helper.DrawPoint(target_location + cg::Location(0,0,0.5), 0.2f, {0u, 255u, 0u}, .1f);
float dot_product = DeviationDotProduct(ego_location, ego_heading, target_location);
float cross_product = DeviationCrossProduct(ego_location, ego_heading, target_location);
dot_product = 1.0f - dot_product;
if (cross_product < 0.0f) {
dot_product *= -1.0f;
}
const float current_deviation = dot_product;
// If previous state for vehicle not found, initialize state entry.
if (pid_state_map.find(actor_id) == pid_state_map.end()) {
const auto initial_state = StateEntry{current_timestamp, 0.0f, 0.0f, 0.0f, 0.0f};
pid_state_map.insert({actor_id, initial_state});
}
// Retrieving the previous state.
traffic_manager::StateEntry previous_state;
previous_state = pid_state_map.at(actor_id);
// Select PID parameters.
std::vector<float> longitudinal_parameters;
std::vector<float> lateral_parameters;
if (ego_speed > HIGHWAY_SPEED) {
longitudinal_parameters = highway_longitudinal_parameters;
lateral_parameters = highway_lateral_parameters;
} else {
longitudinal_parameters = urban_longitudinal_parameters;
lateral_parameters = urban_lateral_parameters;
}
// If physics is enabled for the vehicle, use PID controller.
// State update for vehicle.
current_state = PID::StateUpdate(previous_state, ego_speed, dynamic_target_velocity,
current_deviation, current_timestamp);
@ -129,6 +132,20 @@ void MotionPlanStage::Update(const unsigned long index) {
actuation_signal.throttle = 0.0f;
actuation_signal.brake = 1.0f;
}
// Constructing the actuation signal.
carla::rpc::VehicleControl vehicle_control;
vehicle_control.throttle = actuation_signal.throttle;
vehicle_control.brake = actuation_signal.brake;
vehicle_control.steer = actuation_signal.steer;
output_array.at(index) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control);
// Updating PID state.
StateEntry &state = pid_state_map.at(actor_id);
state = current_state;
}
// For physics-less vehicles, determine position and orientation for teleportation.
else {
@ -150,68 +167,26 @@ void MotionPlanStage::Update(const unsigned long index) {
// Target displacement magnitude to achieve target velocity.
const float target_displacement = dynamic_target_velocity * HYBRID_MODE_DT_FL;
// std::cout << "target displacement " << target_displacement << std::endl;
const SimpleWaypointPtr teleport_target_waypoint = waypoint_buffer.front(); //GetTargetWaypoint(waypoint_buffer, target_displacement).first;
// Construct target transform to accurately achieve desired velocity.
float missing_displacement = 0.0f;
const float base_displacement = teleport_target_waypoint->Distance(ego_location);
// std::cout << "base displacement " << base_displacement << std::endl;
if (base_displacement < target_displacement) {
missing_displacement = target_displacement - base_displacement;
}
cg::Transform target_base_transform = teleport_target_waypoint->GetTransform();
SimpleWaypointPtr teleport_target = waypoint_buffer.front();
cg::Transform target_base_transform = teleport_target->GetTransform();
cg::Location target_base_location = target_base_transform.location;
cg::Vector3D target_heading = target_base_transform.GetForwardVector();
cg::Vector3D current_heading = ego_heading.MakeSafeUnitVector(EPSILON);
// float location_distance = cg::Math::Distance(ego_location, target_base_location);
// float z_axis_diff = target_heading.z - current_heading.z;
// float percentage_displacement = 100.0f*target_displacement/location_distance;
cg::Vector3D correct_heading = (target_base_location - ego_location).MakeSafeUnitVector(EPSILON);
float dot_product_a = DeviationDotProduct(ego_location, ego_heading, target_base_location);
float cross_product_a = DeviationCrossProduct(ego_location, ego_heading, target_base_location);
dot_product_a = 1.0f - dot_product_a;
if (cross_product_a < 0.0f) {
dot_product_a *= -1.0f;
if (ego_location.Distance(target_base_location) < target_displacement) {
cg::Location teleportation_location = ego_location + cg::Location(target_heading.MakeSafeUnitVector(EPSILON) * target_displacement);
teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation);
}
else {
cg::Location teleportation_location = ego_location + cg::Location(correct_heading * target_displacement);
teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation);
}
const float current_deviation_a = dot_product_a;
// if (percentage_displacement > 100.0f) {
// percentage_displacement = 100.0f;
// }
// float dot = current_heading.x * target_heading.x + current_heading.y * target_heading.y;
// float det = current_heading.x * target_heading.y + current_heading.y * target_heading.x;
// float angle = atan2(det, dot);
// float perc_deviation = current_deviation_a * target_displacement / location_distance;
// float perc_z_axis = z_axis_diff * target_displacement / location_distance;
// double cos_t = cos(current_deviation);
// double sin_t = sin(current_deviation);
// cg::Vector3D rotated = (v * cos_theta) + (glm::cross(k, v) * sin_theta) + (k * glm::dot(k, v)) * (1 - cos_theta);
cg::Vector3D new_heading = cg::Vector3D(current_heading.x * cos(current_deviation_a) - current_heading.y * sin(current_deviation_a),
current_heading.y * cos(current_deviation_a) + current_heading.x * sin(current_deviation_a),
target_heading.z); //current_heading.z - target_heading.z);
cg::Location teleportation_location = ego_location + cg::Location(new_heading * target_displacement);
teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation);
}
// In case of an emergency stop, stay in the same location.
// Also, teleport only once every dt in asynchronous mode.
else {
} else {
teleportation_transform = cg::Transform(ego_location, simulation_state.GetRotation(actor_id));
}
}
// Updating PID state.
StateEntry &state = pid_state_map.at(actor_id);
state = current_state;
// Constructing the actuation signal.
if (ego_physics_enabled) {
carla::rpc::VehicleControl vehicle_control;
vehicle_control.throttle = actuation_signal.throttle;
vehicle_control.brake = actuation_signal.brake;
vehicle_control.steer = actuation_signal.steer;
output_array.at(index) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control);
} else {
// Constructing the actuation signal.
output_array.at(index) = carla::rpc::Command::ApplyTransform(actor_id, teleportation_transform);
}
}

6
LibCarla/source/carla/trafficmanager/MotionPlanStage.h
View File

@ -11,6 +11,8 @@
#include "carla/trafficmanager/Stage.h"
#include "carla/trafficmanager/TrackTraffic.h"
#include "carla/client/DebugHelper.h"
namespace carla {
namespace traffic_manager {
@ -36,6 +38,7 @@ private:
// in hybrid physics mode.
std::unordered_map<ActorId, cc::Timestamp> teleportation_instance;
ControlFrame &output_array;
cc::DebugHelper debug_helper;
cc::Timestamp current_timestamp;
std::pair<bool, float> CollisionHandling(const CollisionHazardData &collision_hazard,
@ -62,7 +65,8 @@ public:
const CollisionFrame &collision_frame,
const TLFrame &tl_frame,
const cc::World &world,
ControlFrame &output_array);
ControlFrame &output_array,
cc::DebugHelper &debug_helper);
void Update(const unsigned long index);

11
LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp
View File

@ -74,7 +74,8 @@ TrafficManagerLocal::TrafficManagerLocal(
collision_frame,
tl_frame,
world,
control_frame)),
control_frame,
debug_helper)),
alsm(ALSM(registered_vehicles,
buffer_map,
@ -96,7 +97,7 @@ TrafficManagerLocal::TrafficManagerLocal(
registered_vehicles_state = -1;
SetupLocalMap();
SetupLocalMap(debug_helper);
Start();
}
@ -106,10 +107,10 @@ TrafficManagerLocal::~TrafficManagerLocal() {
Release();
}
void TrafficManagerLocal::SetupLocalMap() {
void TrafficManagerLocal::SetupLocalMap(cc::DebugHelper &debug_helper) {
const carla::SharedPtr<cc::Map> world_map = world.GetMap();
local_map = std::make_shared<InMemoryMap>(world_map);
local_map->SetUp();
local_map->SetUp(debug_helper);
}
void TrafficManagerLocal::Start() {
@ -288,7 +289,7 @@ void TrafficManagerLocal::Reset() {
Release();
episode_proxy = episode_proxy.Lock()->GetCurrentEpisode();
world = cc::World(episode_proxy);
SetupLocalMap();
//SetupLocalMap();
Start();
}

2
LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h
View File

@ -136,7 +136,7 @@ public:
virtual ~TrafficManagerLocal();
/// Method to setup InMemoryMap.
void SetupLocalMap();
void SetupLocalMap(cc::DebugHelper &debug_helper);
/// To start the TrafficManager.
void Start();