Added function to remove walkers

LibCarla/source/carla/AtomicList.h

@@ -34,6 +34,13 @@ namespace detail {
       _list = new_list;
     }
 
+    void Delete(unsigned int index) {
+      std::lock_guard<std::mutex> lock(_mutex);
+      auto new_list = std::make_shared<ListT>(*Load());
+      new_list->erase(new_list->begin() + index);
+      _list = new_list;
+    }
+
     void Clear() {
       std::lock_guard<std::mutex> lock(_mutex);
       _list = std::make_shared<ListT>();

LibCarla/source/carla/client/WalkerAIController.cpp

@@ -27,6 +27,19 @@ namespace client {
     }
   }
 
+  void WalkerAIController::Stop() {
+    GetEpisode().Lock()->UnregisterAIController(*this);
+
+    // remove the walker from the Recast & Detour
+    auto walker = GetParent();
+    if (walker != nullptr) {
+      auto nav = GetEpisode().Lock()->GetNavigation();
+      if (nav != nullptr) {
+        nav->RemoveWalker(walker->GetId());
+      }
+    }
+  }
+
   geom::Location WalkerAIController::GetRandomLocation() {
     auto nav = GetEpisode().Lock()->GetNavigation();
     if (nav != nullptr) {

LibCarla/source/carla/client/WalkerAIController.h

@@ -20,6 +20,8 @@ namespace client {
 
     void Start(carla::geom::Location location);
 
+    void Stop();
+
     geom::Location GetRandomLocation();
 
     void GoToLocation(const carla::geom::Location &destination);

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

@@ -157,6 +157,18 @@ namespace detail {
     navigation->RegisterWalker(walker->GetId(), controller.GetId());
   }
 
+  void Simulator::UnregisterAIController(const WalkerAIController &controller) {
+    auto walker = controller.GetParent();
+    if (walker == nullptr) {
+      throw_exception(std::runtime_error(controller.GetDisplayId() + ": not attached to walker"));
+      return;
+    }
+    DEBUG_ASSERT(_episode != nullptr);
+    auto navigation = _episode->CreateNavigationIfMissing();
+    DEBUG_ASSERT(navigation != nullptr);
+    navigation->UnregisterWalker(walker->GetId(), controller.GetId());
+  }
+
   geom::Location Simulator::GetRandomLocationFromNavigation() {
     DEBUG_ASSERT(_episode != nullptr);
     auto navigation = _episode->CreateNavigationIfMissing();

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

@@ -182,6 +182,8 @@ namespace detail {
 
     void RegisterAIController(const WalkerAIController &controller);
 
+    void UnregisterAIController(const WalkerAIController &controller);
+
     geom::Location GetRandomLocationFromNavigation();
 
     std::shared_ptr<WalkerNavigation> GetNavigation() {

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

@@ -35,6 +35,25 @@ namespace detail {
       _walkers.Push(WalkerHandle { walker_id, controller_id });
     }
 
+    void UnregisterWalker(ActorId walker_id, ActorId controller_id) {
+      // remove from list
+      auto list = _walkers.Load();
+      unsigned int i = 0;
+      while (i < (*list).size()) {
+        if ((*list)[i].walker == walker_id &&
+            (*list)[i].controller == controller_id) {
+          _walkers.Delete(i);
+          break;
+        }
+        ++i;
+      }
+    }
+
+    void RemoveWalker(ActorId walker_id) {
+      // remove the walker in the crowd
+      _nav.RemoveWalker(walker_id);
+    }
+
     void AddWalker(ActorId walker_id, carla::geom::Location location) {
       float h = _client.GetWalkerBaseOffset(walker_id) / 100.0f;
 

LibCarla/source/carla/nav/Navigation.cpp

@@ -318,7 +318,7 @@ namespace nav {
     params.separationWeight = 0.5f;
 
 
-    // set from Unreal coordinates (and adjust center of walker, from middle to bottom)
+    // from Unreal coordinates
     float PointFrom[3] = { from.x, from.z, from.y };
     // add walker
     int index = _crowd->addAgent(PointFrom, &params);
@@ -337,6 +337,22 @@ namespace nav {
     return true;
   }
 
+  // remove a walker
+  bool Navigation::RemoveWalker(ActorId id) {
+    // get the internal index
+    auto it = _mappedId.find(id);
+    if (it == _mappedId.end())
+      return false;
+
+    // remove from crowd
+    _crowd->removeAgent(it->second);
+
+    // remove from mapping
+    _mappedId.erase(it);
+
+    return true;
+  }
+
   // set new max speed
   bool Navigation::SetWalkerMaxSpeed(ActorId id, float max_speed) {
     // get the internal index

LibCarla/source/carla/nav/Navigation.h

@@ -38,6 +38,8 @@ namespace nav {
     void CreateCrowd(void);
     // create a new walker
     bool AddWalker(ActorId id, carla::geom::Location from, float base_offset);
+    // remove a walker
+    bool RemoveWalker(ActorId id);
     // set new max speed
     bool SetWalkerMaxSpeed(ActorId id, float max_speed);
     // set a new target point to go

PythonAPI/carla/source/libcarla/Actor.cpp

@@ -114,6 +114,7 @@ void export_actor() {
 
   class_<cc::WalkerAIController, bases<cc::Actor>, boost::noncopyable, boost::shared_ptr<cc::WalkerAIController>>("WalkerAIController", no_init)
     .def("start", &cc::WalkerAIController::Start)
+    .def("stop", &cc::WalkerAIController::Stop)
     .def("go_to_location", &cc::WalkerAIController::GoToLocation, (arg("destination")))
     .def("set_max_speed", &cc::WalkerAIController::SetMaxSpeed, (arg("speed")))
     .def(self_ns::str(self_ns::self))

PythonAPI/examples/spawn_npc.py

@@ -157,28 +157,22 @@ def main():
             else:
                 info[i]["con"] = results[i].actor_id
 
-        # get whole list of actors (child and parents)
+        # get whole list of actors (child and parents in same list so world.get_actors() can find parents also)
         all_id = []
         for i in range(len(info)):
-            all_id.append(info[i]["id"])
             all_id.append(info[i]["con"])
+            all_id.append(info[i]["id"])
         all_actors = world.get_actors(all_id)
 
-        # initialize each controller and set target to walk to
-        for i in range(len(all_id)):
-            # check it if it is a controller or a walker
-            index = -1
-            for j in range(len(info)):
-                if (info[j]["con"] == all_id[i]):
-                    index = j
-                    break
-            if (index != -1):
-                # init
-                all_actors[i].start(info[index]["trans"].location)
-                # walk to random point
-                target = world.get_random_location_from_navigation()
-                all_actors[i].go_to_location(target)
-                all_actors[i].set_max_speed(1 + random.random())
+        # initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...])
+        for i in range(0, len(all_id), 2):
+            # index in the info list
+            index = int(i / 2)
+            all_actors[i].start(info[index]["trans"].location)
+            # walk to random point
+            target = world.get_random_location_from_navigation()
+            all_actors[i].go_to_location(target)
+            all_actors[i].set_max_speed(1 + random.random())    # max speed between 1 and 2
 
         print('spawned %d walkers, press Ctrl+C to exit.' % len(info))
 
@@ -190,6 +184,10 @@ def main():
         print('\ndestroying %d vehicles' % len(actor_list))
         client.apply_batch([carla.command.DestroyActor(x) for x in actor_list])
 
+        # stop walker controllers (list is [controler, actor, controller, actor ...])
+        for i in range(0, len(all_id), 2):
+            all_actors[i].stop()
+
         print('\ndestroying %d walkers' % len(info))
         client.apply_batch([carla.command.DestroyActor(x) for x in all_id])