Raycasting (carla-simulator#3483)

* Added raycasting and point projection capabilities to python api.

* Added documentation of new functions.

* Fixed missing .f on floats

Co-authored-by: doterop <[email protected]>

Author: Axel1092

.gitignore

@@ -6,6 +6,7 @@ TrafficManager/build
 Util/Build
 Install
 Plugins
+!Unreal/CarlaUE4/Plugins
 
 /ExportedMaps
 /Import/*

Docs/python_api.md

@@ -176,5 +176,25 @@ namespace client {
     _episode.Lock()->EnableEnvironmentObjects(env_objects_ids, enable);
   }
 
+  boost::optional<rpc::LabelledPoint> World::ProjectPoint(
+      geom::Location location, geom::Vector3D direction, float search_distance) const {
+    auto result = _episode.Lock()->ProjectPoint(location, direction, search_distance);
+    if (result.first) {
+      return result.second;
+    }
+    return {};
+  }
+
+  boost::optional<rpc::LabelledPoint> World::GroundProjection(
+      geom::Location location, float search_distance) const {
+    const geom::Vector3D DownVector(0,0,-1);
+    return ProjectPoint(location, DownVector, search_distance);
+  }
+
+  std::vector<rpc::LabelledPoint> World::CastRay(
+      geom::Location start_location, geom::Location end_location) const {
+    return _episode.Lock()->CastRay(start_location, end_location);
+  }
+
 } // namespace client
 } // namespace carla

LibCarla/source/carla/client/World.cpp

@@ -19,6 +19,7 @@
 #include "carla/rpc/AttachmentType.h"
 #include "carla/rpc/EpisodeSettings.h"
 #include "carla/rpc/EnvironmentObject.h"
+#include "carla/rpc/LabelledPoint.h"
 #include "carla/rpc/VehiclePhysicsControl.h"
 #include "carla/rpc/WeatherParameters.h"
 #include "carla/rpc/VehicleLightStateList.h"
@@ -162,6 +163,15 @@ namespace client {
       std::vector<uint64_t> env_objects_ids,
       bool enable) const;
 
+    boost::optional<rpc::LabelledPoint> ProjectPoint(
+        geom::Location location, geom::Vector3D direction, float search_distance = 10000.f) const;
+
+    boost::optional<rpc::LabelledPoint> GroundProjection(
+        geom::Location location, float search_distance = 10000.0) const;
+
+    std::vector<rpc::LabelledPoint> CastRay(
+        geom::Location start_location, geom::Location end_location) const;
+
   private:
 
     detail::EpisodeProxy _episode;

LibCarla/source/carla/client/World.h

@@ -461,6 +461,18 @@ namespace detail {
     _pimpl->AsyncCall("enable_environment_objects", std::move(env_objects_ids), enable);
   }
 
+  std::pair<bool,rpc::LabelledPoint> Client::ProjectPoint(
+      geom::Location location, geom::Vector3D direction, float search_distance) const {
+    using return_t = std::pair<bool,rpc::LabelledPoint>;
+    return _pimpl->CallAndWait<return_t>("project_point", location, direction, search_distance);
+  }
+
+  std::vector<rpc::LabelledPoint> Client::CastRay(
+      geom::Location start_location, geom::Location end_location) const {
+    using return_t = std::vector<rpc::LabelledPoint>;
+    return _pimpl->CallAndWait<return_t>("cast_ray", start_location, end_location);
+  }
+
 } // namespace detail
 } // namespace client
 } // namespace carla

LibCarla/source/carla/client/detail/Client.cpp

@@ -10,13 +10,15 @@
 #include "carla/NonCopyable.h"
 #include "carla/Time.h"
 #include "carla/geom/Transform.h"
+#include "carla/geom/Location.h"
 #include "carla/rpc/Actor.h"
 #include "carla/rpc/ActorDefinition.h"
 #include "carla/rpc/AttachmentType.h"
 #include "carla/rpc/Command.h"
 #include "carla/rpc/CommandResponse.h"
 #include "carla/rpc/EpisodeInfo.h"
 #include "carla/rpc/EpisodeSettings.h"
+#include "carla/rpc/LabelledPoint.h"
 #include "carla/rpc/LightState.h"
 #include "carla/rpc/MapInfo.h"
 #include "carla/rpc/EnvironmentObject.h"
@@ -297,6 +299,12 @@ namespace detail {
       std::vector<uint64_t> env_objects_ids,
       bool enable) const;
 
+    std::pair<bool,rpc::LabelledPoint> ProjectPoint(
+        geom::Location location, geom::Vector3D direction, float search_distance) const;
+
+    std::vector<rpc::LabelledPoint> CastRay(
+        geom::Location start_location, geom::Location end_location) const;
+
   private:
 
     class Pimpl;

LibCarla/source/carla/client/detail/Client.h

@@ -23,6 +23,7 @@
 #include "carla/profiler/LifetimeProfiled.h"
 #include "carla/rpc/TrafficLightState.h"
 #include "carla/rpc/VehicleLightStateList.h"
+#include "carla/rpc/LabelledPoint.h"
 
 #include <boost/optional.hpp>
 
@@ -241,6 +242,16 @@ namespace detail {
       _client.EnableEnvironmentObjects(env_objects_ids, enable);
     }
 
+    std::pair<bool,rpc::LabelledPoint> ProjectPoint(
+        geom::Location location, geom::Vector3D direction, float search_distance) const {
+      return _client.ProjectPoint(location, direction, search_distance);
+    }
+
+    std::vector<rpc::LabelledPoint> CastRay(
+        geom::Location start_location, geom::Location end_location) const {
+      return _client.CastRay(start_location, end_location);
+    }
+
     /// @}
     // =========================================================================
     /// @name AI

LibCarla/source/carla/client/detail/Simulator.h

@@ -0,0 +1,32 @@
+// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/MsgPack.h"
+#include "carla/rpc/Location.h"
+#include "carla/rpc/ObjectLabel.h"
+
+namespace carla {
+namespace rpc {
+
+  struct LabelledPoint {
+
+    LabelledPoint () {}
+    LabelledPoint (Location location, CityObjectLabel label)
+     : _location(location), _label(label)
+     {}
+
+    Location _location;
+
+    CityObjectLabel _label;
+
+    MSGPACK_DEFINE_ARRAY(_location, _label);
+
+  };
+
+}
+}

LibCarla/source/carla/rpc/LabelledPoint.h

@@ -23,10 +23,10 @@ namespace rpc {
     RoadLines    =   6u,
     Roads        =   7u,
     Sidewalks    =   8u,
-    TrafficSigns =  12u,
     Vegetation   =   9u,
     Vehicles     =  10u,
     Walls        =  11u,
+    TrafficSigns =  12u,
     Sky          =  13u,
     Ground       =  14u,
     Bridge       =  15u,

LibCarla/source/carla/rpc/ObjectLabel.h

@@ -201,6 +201,11 @@ void export_world() {
     .value("Terrain", cr::CityObjectLabel::Terrain)
   ;
 
+  class_<cr::LabelledPoint>("LabelledPoint", no_init)
+    .def_readonly("location", &cr::LabelledPoint::_location)
+    .def_readonly("label", &cr::LabelledPoint::_label)
+  ;
+
 #define SPAWN_ACTOR_WITHOUT_GIL(fn) +[]( \
         cc::World &self, \
         const cc::ActorBlueprint &blueprint, \
@@ -247,6 +252,9 @@ void export_world() {
     .def("get_level_bbs", &GetLevelBBs, (arg("actor_type")=cr::CityObjectLabel::None))
     .def("get_environment_objects", &GetEnvironmentObjects)
     .def("enable_environment_objects", &EnableEnvironmentObjects, (arg("env_objects_ids"), arg("enable")))
+    .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))
   ;
 

PythonAPI/carla/source/libcarla/World.cpp

@@ -111,6 +111,16 @@ static boost::python::object OptionalToPythonObject(OptionalT &optional) {
       return OptionalToPythonObject(optional); \
     }
 
+#define CALL_RETURNING_OPTIONAL_2(cls, fn, T1_, T2_) +[](const cls &self, T1_ t1, T2_ t2) { \
+      auto optional = self.fn(std::forward<T1_>(t1), std::forward<T2_>(t2)); \
+      return OptionalToPythonObject(optional); \
+    }
+
+#define CALL_RETURNING_OPTIONAL_3(cls, fn, T1_, T2_, T3_) +[](const cls &self, T1_ t1, T2_ t2, T3_ t3) { \
+      auto optional = self.fn(std::forward<T1_>(t1), std::forward<T2_>(t2), std::forward<T3_>(t3)); \
+      return OptionalToPythonObject(optional); \
+    }
+
 #define CALL_RETURNING_OPTIONAL_WITHOUT_GIL(cls, fn) +[](const cls &self) { \
       carla::PythonUtil::ReleaseGIL unlock; \
       auto optional = self.fn(); \

PythonAPI/carla/source/libcarla/libcarla.cpp

@@ -186,6 +186,20 @@
         An attachment that expands or retracts the position of the actor, depending on its parent. This attachment is only recommended to record videos from the simulation where a smooth movement is needed. SpringArms are an Unreal Engine component so [check the UE docs](https://docs.unrealengine.com/en-US/Gameplay/HowTo/UsingCameras/SpringArmComponents/index.html) to learn more about them. <br><b style="color:red;">Warning:</b> The <b>SpringArm</b> attachment presents weird behaviors when an actor is spawned with a relative translation in the Z-axis (e.g. <code>child_location = Location(0,0,2)</code>).  
     # --------------------------------------
 
+  - class_name: LabelledPoint
+    # - DESCRIPTION ------------------------
+    doc: >
+      Class that represent a position in space with a semantic label.
+    # - PROPERTIES -------------------------
+    instance_variables:
+    - var_name: location
+      doc: >
+        Position in 3D space.
+    - var_name: label
+      doc: >
+        Semantic tag of the point.
+    # --------------------------------------
+
   - class_name: World
     # - DESCRIPTION ------------------------
     doc: >
@@ -421,13 +435,59 @@
           New conditions to be applied. 
       doc: >
         Changes the weather parameteres ruling the simulation to another ones defined in an object. 
-     # --------------------------------------
+    # --------------------------------------
+    - def_name: cast_ray
+      return: list(carla.LabelledPoint)
+      params:
+      - param_name: initial_location
+        type: carla.Location
+        doc: >
+          The initial position of the ray.
+      - param_name: final_location
+        type: carla.Location
+        doc: >
+          The final position of the ray.
+      doc: >
+        Casts a ray from the specified initial_location to final_location. The function then detects all geometries intersecting the ray and returns a list of carla.LabelledPoint in order.
+    # --------------------------------------
+    - def_name: project_point
+      return: carla.LabelledPoint
+      params:
+      - param_name: location
+        type: carla.Location
+        doc: >
+          The point to be projected.
+      - param_name: direction
+        type: carla.Vector3D
+        doc: >
+          The direction of projection.
+      - param_name: search_distance
+        type: float
+        doc: >
+          The maximum distance to perform the projection
+      doc: >
+        Projects the specified point to the desired direction in the scene. The functions casts a ray from location in a direction and returns a carla.Labelled object with the first geometry this ray intersects. If no geometry is found in the search_distance range the function returns `None`.
+    # --------------------------------------
+    - def_name: ground_projection
+      return: carla.LabelledPoint
+      params:
+      - param_name: location
+        type: carla.Location
+        doc: >
+          The point to be projected.
+      - param_name: search_distance
+        type: float
+        doc: >
+          The maximum distance to perform the projection
+      doc: >
+        Projects the specified point downwards in the scene. The functions casts a ray from location in the direction (0,0,-1) (downwards) and returns a carla.Labelled object with the first geometry this ray intersects (usually the ground). If no geometry is found in the search_distance range the function returns `None`.
+    # --------------------------------------
     - def_name: __str__
       return:
         string
       doc: >
         The content of the world is parsed and printed as a brief report of its current state. 
-     # --------------------------------------
+    # --------------------------------------
 
   - class_name: DebugHelper
     # - DESCRIPTION ------------------------

PythonAPI/docs/world.yml

@@ -97,13 +97,15 @@ InitialAverageFrameRate=0.016667
 PhysXTreeRebuildRate=10
 DefaultBroadphaseSettings=(bUseMBPOnClient=False,bUseMBPOnServer=False,MBPBounds=(Min=(X=0.000000,Y=0.000000,Z=0.000000),Max=(X=0.000000,Y=0.000000,Z=0.000000),IsValid=0),MBPNumSubdivs=2)
 
+[/Script/WindowsTargetPlatform.WindowsTargetSettings]
+DefaultGraphicsRHI=DefaultGraphicsRHI_DX12
+
 [/Script/Engine.CollisionProfile]
 +Profiles=(Name="CustomSensorCollision",CollisionEnabled=QueryOnly,bCanModify=True,ObjectTypeName="SensorObject",CustomResponses=((Channel="WorldStatic",Response=ECR_Ignore),(Channel="WorldDynamic",Response=ECR_Ignore),(Channel="Pawn",Response=ECR_Ignore),(Channel="Visibility",Response=ECR_Ignore),(Channel="Camera",Response=ECR_Ignore),(Channel="PhysicsBody",Response=ECR_Ignore),(Channel="Vehicle",Response=ECR_Ignore),(Channel="Destructible",Response=ECR_Ignore),(Channel="SensorObject"),(Channel="SensorTrace")),HelpMessage="Used for custom collision meshes for objects that has very complex meshes but we want them to appear in raycast based sensors")
 +DefaultChannelResponses=(Channel=ECC_GameTraceChannel1,DefaultResponse=ECR_Ignore,bTraceType=False,bStaticObject=False,Name="SensorObject")
 +DefaultChannelResponses=(Channel=ECC_GameTraceChannel2,DefaultResponse=ECR_Ignore,bTraceType=True,bStaticObject=False,Name="SensorTrace")
++DefaultChannelResponses=(Channel=ECC_GameTraceChannel3,DefaultResponse=ECR_Overlap,bTraceType=True,bStaticObject=False,Name="OverlapChannel")
 +EditProfiles=(Name="BlockAll",CustomResponses=((Channel="SensorObject"),(Channel="SensorTrace")))
 +EditProfiles=(Name="OverlapAll",CustomResponses=((Channel="SensorObject",Response=ECR_Overlap),(Channel="SensorTrace",Response=ECR_Overlap)))
 
-[/Script/WindowsTargetPlatform.WindowsTargetSettings]
-DefaultGraphicsRHI=DefaultGraphicsRHI_DX12
 

Unreal/CarlaUE4/Config/DefaultEngine.ini

@@ -10,9 +10,11 @@
 #include "Carla/OpenDrive/OpenDrive.h"
 #include "Carla/Util/DebugShapeDrawer.h"
 #include "Carla/Util/NavigationMesh.h"
+#include "Carla/Util/RayTracer.h"
 #include "Carla/Vehicle/CarlaWheeledVehicle.h"
 #include "Carla/Walker/WalkerController.h"
 #include "GameFramework/CharacterMovementComponent.h"
+#include "Carla/Game/Tagger.h"
 
 #include <compiler/disable-ue4-macros.h>
 #include <carla/Functional.h>
@@ -25,6 +27,7 @@
 #include <carla/rpc/DebugShape.h>
 #include <carla/rpc/EpisodeInfo.h>
 #include <carla/rpc/EpisodeSettings.h>
+#include "carla/rpc/LabelledPoint.h"
 #include <carla/rpc/LightState.h>
 #include <carla/rpc/MapInfo.h>
 #include <carla/rpc/EnvironmentObject.h>
@@ -46,6 +49,7 @@
 
 #include <vector>
 #include <map>
+#include <tuple>
 
 template <typename T>
 using R = carla::rpc::Response<T>;
@@ -1410,6 +1414,28 @@ void FCarlaServer::FPimpl::BindActions()
   };
 
 
+  // ~~ Ray Casting ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+  BIND_SYNC(project_point) << [this]
+      (cr::Location Location, cr::Vector3D Direction, float SearchDistance)
+      -> R<std::pair<bool,cr::LabelledPoint>>
+  {
+    REQUIRE_CARLA_EPISODE();
+    auto *World = Episode->GetWorld();
+    constexpr float meter_to_centimeter = 100.0f;
+    return URayTracer::ProjectPoint(Location, Direction.ToFVector(),
+        meter_to_centimeter * SearchDistance, World);
+  };
+
+  BIND_SYNC(cast_ray) << [this]
+      (cr::Location StartLocation, cr::Location EndLocation)
+      -> R<std::vector<cr::LabelledPoint>>
+  {
+    REQUIRE_CARLA_EPISODE();
+    auto *World = Episode->GetWorld();
+    return URayTracer::CastRay(StartLocation, EndLocation, World);
+  };
+
 }
 
 // =============================================================================

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma
 // de Barcelona (UAB).
 //
 // This work is licensed under the terms of the MIT license.

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma
 // de Barcelona (UAB).
 //
 // This work is licensed under the terms of the MIT license.