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

LibCarla/source/carla/client/TrafficLight.cpp

@@ -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

LibCarla/source/carla/client/TrafficLight.h

@@ -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

LibCarla/source/carla/client/World.cpp

@@ -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()));
+        SharedPtr<Actor> traffic_light = GetTrafficLight(*(landmark.get()));
-        if (TrafficLight) {
+        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);
+        SharedPtr<Actor> traffic_light = GetTrafficLightFromOpenDRIVE(sign_id);
-        if (TrafficLight) {
+        if (traffic_light) {
-          Result.emplace_back(TrafficLight);
+          Result.emplace_back(traffic_light);
         }
       }
     }

LibCarla/source/carla/geom/Transform.h

@@ -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;

PythonAPI/carla/source/libcarla/Actor.cpp

@@ -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))
   ;
 }

PythonAPI/carla/source/libcarla/Geom.cpp

@@ -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)

PythonAPI/carla/source/libcarla/World.cpp

@@ -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))
   ;