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

Added get_stop_waypoints() to traffic light. Renamed affected_lane_waypoints function. Added transform_vector function.

This commit is contained in:
Axel 2021-05-04 09:48:20 +02:00 committed by bernat
parent 138a282fe1
commit c62b4bb280
7 changed files with 53 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
LibCarla/source/carla/client/TrafficLight.cpp
View File

@ -8,6 +8,9 @@
#include "carla/client/detail/Simulator.h"
#include "carla/client/ActorList.h"
#include <unordered_map>
#include <unordered_set>
namespace carla {
namespace client {
@ -75,7 +78,7 @@ namespace client {
GetEpisode().Lock()->ResetTrafficLightGroup(*this);
}
std::vector<SharedPtr<Waypoint>> TrafficLight::GetEffectPositions() const {
std::vector<SharedPtr<Waypoint>> TrafficLight::GetAffectedLaneWaypoints() const {
std::vector<SharedPtr<Waypoint>> result;
SharedPtr<Map> carla_map = GetEpisode().Lock()->GetCurrentMap();
std::vector<SharedPtr<Landmark>> landmarks = carla_map->GetLandmarksFromId(GetOpenDRIVEID());
@ -86,14 +89,14 @@ namespace client {
if(lane_id == 0) continue;
result.emplace_back(
carla_map->GetWaypointXODR(
landmark->GetRoadId(), lane_id, landmark->GetS()));
landmark->GetRoadId(), lane_id, static_cast<float>(landmark->GetS())));
}
} else {
for (int lane_id = validity._from_lane; lane_id >= validity._to_lane; --lane_id) {
if(lane_id == 0) continue;
result.emplace_back(
carla_map->GetWaypointXODR(
landmark->GetRoadId(), lane_id, landmark->GetS()));
landmark->GetRoadId(), lane_id, static_cast<float>(landmark->GetS())));
}
}
}
@ -109,5 +112,29 @@ namespace client {
return GetEpisode().Lock()->GetActorSnapshot(*this).state.traffic_light_data.sign_id;
}
std::vector<SharedPtr<Waypoint>> TrafficLight::GetStopWaypoints() const {
std::vector<SharedPtr<Waypoint>> result;
SharedPtr<Map> carla_map = GetEpisode().Lock()->GetCurrentMap();
geom::BoundingBox box = GetTriggerVolume();
geom::Transform transform = GetTransform();
geom::Location box_position = box.location;
transform.TransformPoint(box_position);
geom::Vector3D right_direction = transform.GetForwardVector();
float min_x = -0.9f*box.extent.x;
float max_x = 0.9f*box.extent.x;
float current_x = min_x;
std::unordered_map<road::RoadId, std::unordered_set<road::LaneId>> road_lanes_map;
while (current_x < max_x) {
geom::Location query_point = box_position + geom::Location(right_direction*current_x);
SharedPtr<Waypoint> waypoint = carla_map->GetWaypoint(query_point);
if (road_lanes_map[waypoint->GetRoadId()].count(waypoint->GetLaneId()) == 0) {
road_lanes_map[waypoint->GetRoadId()].insert(waypoint->GetLaneId());
result.emplace_back(waypoint);
}
current_x += 1.f;
}
return result;
}
} // namespace client
} // namespace carla

4
LibCarla/source/carla/client/TrafficLight.h
View File

@ -68,12 +68,14 @@ namespace client {
// resets the timers and states of all groups
void ResetGroup();
std::vector<SharedPtr<Waypoint>> GetEffectPositions() const;
std::vector<SharedPtr<Waypoint>> GetAffectedLaneWaypoints() const;
std::vector<geom::BoundingBox> GetLightBoxes() const;
road::SignId GetOpenDRIVEID() const;
std::vector<SharedPtr<Waypoint>> GetStopWaypoints() const;
};
} // namespace client

13
LibCarla/source/carla/client/World.cpp
View File

@ -14,6 +14,7 @@
#include "carla/StringUtil.h"
#include "carla/road/SignalType.h"
#include "carla/road/Junction.h"
#include "carla/client/TrafficLight.h"
#include <exception>
@ -260,10 +261,10 @@ namespace client {
std::set<std::string> added_signals;
for (auto& landmark : landmarks) {
if (road::SignalType::IsTrafficLight(landmark->GetType())) {
SharedPtr<Actor> TrafficLight = GetTrafficLight(*(landmark.get()));
if (TrafficLight) {
SharedPtr<Actor> traffic_light = GetTrafficLight(*(landmark.get()));
if (traffic_light) {
if(added_signals.count(landmark->GetId()) == 0) {
Result.emplace_back(TrafficLight);
Result.emplace_back(traffic_light);
added_signals.insert(landmark->GetId());
}
}
@ -281,9 +282,9 @@ namespace client {
const std::unique_ptr<road::Controller>& controller =
map->GetMap().GetControllers().at(cont_id);
for (road::SignId sign_id : controller->GetSignals()) {
SharedPtr<Actor> TrafficLight = GetTrafficLightFromOpenDRIVE(sign_id);
if (TrafficLight) {
Result.emplace_back(TrafficLight);
SharedPtr<Actor> traffic_light = GetTrafficLightFromOpenDRIVE(sign_id);
if (traffic_light) {
Result.emplace_back(traffic_light);
}
}
}

7
LibCarla/source/carla/geom/Transform.h
View File

@ -69,6 +69,13 @@ namespace geom {
in_point = out_point;
}
/// Applies this transformation to @a in_vector (rotation only).
void TransformVector(Vector3D &in_vector) const {
auto out_vector = in_vector;
rotation.RotateVector(out_vector); // First rotate
in_vector = out_vector;
}
/// Applies the inverse of this transformation to @a in_point
void InverseTransformPoint(Vector3D &in_point) const {
auto out_point = in_point;

3
PythonAPI/carla/source/libcarla/Actor.cpp
View File

@ -222,9 +222,10 @@ void export_actor() {
.def("get_pole_index", &cc::TrafficLight::GetPoleIndex)
.def("get_group_traffic_lights", &GetGroupTrafficLights)
.def("reset_group", &cc::TrafficLight::ResetGroup)
.def("get_effect_waypoints", CALL_RETURNING_LIST(cc::TrafficLight, GetEffectPositions))
.def("get_affected_lane_waypoints", CALL_RETURNING_LIST(cc::TrafficLight, GetAffectedLaneWaypoints))
.def("get_light_boxes", &GetLightBoxes)
.def("get_opendrive_id", &cc::TrafficLight::GetOpenDRIVEID)
.def("get_stop_waypoints", CALL_RETURNING_LIST(cc::TrafficLight, GetStopWaypoints))
.def(self_ns::str(self_ns::self))
;
}

4
PythonAPI/carla/source/libcarla/Geom.cpp
View File

@ -191,6 +191,10 @@ void export_geom() {
self.TransformPoint(location);
return location;
}, arg("in_point"))
.def("transform_vector", +[](const cg::Transform &self, cg::Vector3D &vector) {
self.TransformVector(vector);
return vector;
}, arg("in_point"))
.def("get_forward_vector", &cg::Transform::GetForwardVector)
.def("get_right_vector", &cg::Transform::GetRightVector)
.def("get_up_vector", &cg::Transform::GetUpVector)

1
PythonAPI/carla/source/libcarla/World.cpp
View File

@ -299,7 +299,6 @@ void export_world() {
.def("cast_ray", CALL_RETURNING_LIST_2(cc::World, CastRay, cg::Location, cg::Location), (arg("initial_location"), arg("final_location")))
.def("project_point", CALL_RETURNING_OPTIONAL_3(cc::World, ProjectPoint, cg::Location, cg::Vector3D, float), (arg("location"), arg("direction"), arg("search_distance")=10000.f))
.def("ground_projection", CALL_RETURNING_OPTIONAL_2(cc::World, GroundProjection, cg::Location, float), (arg("location"), arg("search_distance")=10000.f))
.def(self_ns::str(self_ns::self))
;