Fixing orientation of pedestrians

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

@@ -43,6 +43,8 @@ namespace detail {
     for (auto handle : *walkers) {
       // get the transform of the walker
       if (_nav.GetWalkerTransform(handle.walker, trans)) {
+        // set current height of the walker, ignoring height from recast
+        trans.location.z = state.GetActorState(handle.walker).transform.location.z;
         // logging::log("Nav: walker at ", trans.location.x, trans.location.y, trans.location.z);
         commands.emplace_back(Cmd::ApplyTransform{ handle.walker, trans });
       }

LibCarla/source/carla/nav/Navigation.cpp

@@ -18,6 +18,7 @@ namespace nav {
   static const int MAX_AGENTS = 128;
   static const float AGENT_RADIUS = 0.6f;
   static const float AGENT_HEIGHT = 2.0f;
+  static const float AGENT_HEIGHT_HALF = AGENT_HEIGHT / 2.0f;
 
   Navigation::Navigation() {
   }
@@ -256,6 +257,7 @@ namespace nav {
   void Navigation::AddWalker(ActorId id) {
     // add to the queue to be added later and next tick where we can access the transform
     _walkersQueueToAdd.push_back(id);
+    logging::log("Nav: Added new walker in queue");
   }
 
   // create a new walker in crowd
@@ -266,12 +268,17 @@ namespace nav {
       return false;
     }
 
+    // set from Unreal coordinates (and adjust center of walker, from middle to bottom)
+    float y = from.y;
+    from.y = from.z;
+    from.z = y - AGENT_HEIGHT_HALF;
+
     // set parameters
     memset(&params, 0, sizeof(params));
     params.radius = AGENT_RADIUS;
     params.height = AGENT_HEIGHT;
     params.maxAcceleration = 8.0f;
-    params.maxSpeed = 3.5f;
+    params.maxSpeed = 2.5f;
     params.collisionQueryRange = params.radius * 12.0f;
     params.pathOptimizationRange = params.radius * 30.0f;
 
@@ -283,17 +290,28 @@ namespace nav {
     params.updateFlags |= DT_CROWD_OBSTACLE_AVOIDANCE;
     params.updateFlags |= DT_CROWD_SEPARATION;
     params.obstacleAvoidanceType = 3;
-    params.separationWeight = 2.0;
+    params.separationWeight = 0.5;
 
     // add walker
     float PointFrom[3] = { from.x, from.y, from.z };
     int index = _crowd->addAgent(PointFrom, &params);
-    if (index != -1) {
-      // save the id
-      _mappedId[id] = index;
+    if (index == -1) {
+      return false;
     }
 
+    // save the id
+    _mappedId[id] = index;
+    logging::log("Nav: Set index ", index, " to id ", id);
+
+    int total = 0;
+    for (int i=0; i<_crowd->getAgentCount(); ++i) {
+      if (_crowd->getAgent(i)->active)
+        ++total;
+    }
+    logging::log("Nav: agents now ", total);
+
     return true;
+
   }
 
   // set a new target point to go
@@ -308,10 +326,10 @@ namespace nav {
     if (index == -1)
       return false;
 
-    logging::log("Nav: ", index, it->first);
+    logging::log("Nav: Found index ", index, " with id ", it->first);
 
     // set target position
-    float pointTo[3] = { to.x, to.y, to.z };
+    float pointTo[3] = { to.x, to.z, to.y };
     float nearest[3];
     const dtQueryFilter *filter = _crowd->getFilter(0);
     dtPolyRef targetRef;
@@ -324,17 +342,19 @@ namespace nav {
 
   // update all walkers in crowd
   void Navigation::UpdateCrowd(const client::detail::EpisodeState &state) {
-    std::lock_guard<std::mutex> lock(_mutex);
+
     if (!_navMesh || !_crowd) {
       return;
     }
 
+    // force single thread running this
+    std::lock_guard<std::mutex> lock(_mutex);
+
     // check if we have more walkers in the queue to add
-    while (_walkersQueueToAdd.size() > 0) {
+    while (!_walkersQueueToAdd.empty()) {
       // add it
       ActorId id = _walkersQueueToAdd.back();
       carla::geom::Transform trans = state.GetActorState(id).transform;
-      trans.location.z -= (AGENT_HEIGHT / 2.0f);
       if (!AddWalkerInCrowd(id, trans.location)) {
         break;
       }
@@ -345,20 +365,6 @@ namespace nav {
     // update all
     double deltaTime = state.GetTimestamp().delta_seconds;
     _crowd->update(static_cast<float>(deltaTime), nullptr);
-/*
-    // get all walker positions
-    for (int i = 0; i < _crowd->getAgentCount(); ++i)
-    {
-      const dtCrowdAgent* ag = _crowd->getAgent(i);
-      if (!ag->active)
-        continue;
-
-      // Update agent movement trail.
-      // AgentTrail* trail = &m_trails[i];
-      // trail->htrail = (trail->htrail + 1) % AGENT_MAX_TRAIL;
-      // dtVcopy(&trail->trail[trail->htrail*3], ag->npos);
-    }
-*/
   }
 
   // get the walker current transform
@@ -375,13 +381,18 @@ namespace nav {
 
     // get the walker
     const dtCrowdAgent *agent = _crowd->getAgent(index);
-    // set its position
+
+    if (!agent->active) {
+      return false;
+    }
+
+    // set its position in Unreal coordinates
     trans.location.x = agent->npos[0];
     trans.location.y = agent->npos[2];
-    trans.location.z = agent->npos[1] + (AGENT_HEIGHT / 2.0f);
+    trans.location.z = agent->npos[1] + AGENT_HEIGHT_HALF;
     // set its rotation
-    trans.rotation.pitch = asin(agent->nvel[2]);
-    trans.rotation.yaw = asin(agent->nvel[0]);
+    trans.rotation.pitch = 0; // asin(agent->nvel[2]) * 360;
+    trans.rotation.yaw = asin(agent->nvel[0]) * (180.0f / 3.14159265f);
     trans.rotation.roll = 0;
 
     return true;

PythonAPI/examples/draw_path.py

@@ -56,26 +56,41 @@ def main():
         m = world.get_map()
         debug = world.debug
 
-        spawn_point = carla.Transform()
-        spawn_point.location.x = -18
-        spawn_point.location.y = 1.2
-        spawn_point.location.z = 18
-        spawn_point.rotation.roll = 0.0
-        spawn_point.rotation.pitch = 0.0
+        # spawn_point1 = carla.Transform()
+        # spawn_point1.location.x = -18
+        # spawn_point1.location.y = 1.2
+        # spawn_point1.location.z = 18
+        # spawn_point1.rotation.roll = 0.0
+        # spawn_point1.rotation.pitch = 0.0
+
+        # spawn_point2 = carla.Transform()
+        # spawn_point2.location.x = -20
+        # spawn_point2.location.y = 1.2
+        # spawn_point2.location.z = 18
+        # spawn_point2.rotation.roll = 0.0
+        # spawn_point2.rotation.pitch = 0.0
 
         # player
         # print(world.get_blueprint_library())
         # exit(1)
-        blueprint = random.choice(world.get_blueprint_library().filter('walker.pedestrian.0002'))
-        player = world.spawn_actor(blueprint, spawn_point)
 
-        # controller
-        blueprint = random.choice(world.get_blueprint_library().filter('controller.ai.walker'))
-        walker_controller = world.spawn_actor(blueprint, spawn_point, attach_to=player)
-        walker_controller.start()
-        time.sleep(0.5)
-        walker_controller.go_to_location(carla.Location(-48, 1.2, 27))
-        # walker_controller.set_max_speed(40)
+
+        for i in range(10):
+            spawn_point = carla.Transform()
+            spawn_point.location.x = -25 + random.randint(-5, 5)
+            spawn_point.location.y = 12 + random.randint(-5, 5)
+            spawn_point.location.z = 1.2
+            spawn_point.rotation.roll = 0.0
+            spawn_point.rotation.pitch = 0.0
+
+            blueprint = random.choice(world.get_blueprint_library().filter('walker.pedestrian.*'))
+            player = world.spawn_actor(blueprint, spawn_point)
+            blueprint = random.choice(world.get_blueprint_library().filter('controller.ai.walker'))
+            walker_controller = world.spawn_actor(blueprint, spawn_point, attach_to=player)
+            walker_controller.start()
+            time.sleep(0.5)
+            walker_controller.go_to_location(carla.Location(-15, 32, 1.2))
+            time.sleep(0.5)
 
         while (1):
             time.sleep(1);