CARLA
WalkerNavigation.cpp
Go to the documentation of this file.
1 // Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
8 
12 #include "carla/nav/Navigation.h"
13 #include "carla/rpc/Command.h"
14 #include "carla/rpc/DebugShape.h"
16 
17 #include <sstream>
18 
19 namespace carla {
20 namespace client {
21 namespace detail {
22 
23  WalkerNavigation::WalkerNavigation(Client &client) : _client(client), _next_check_index(0) {
24  // Here call the server to retrieve the navmesh data.
25  auto files = _client.GetRequiredFiles("Nav");
26  if (!files.empty()) {
27  _nav.Load(_client.GetCacheFile(files[0]));
28  }
29  }
30 
31  void WalkerNavigation::Tick(std::shared_ptr<Episode> episode) {
32  auto walkers = _walkers.Load();
33  if (walkers->empty()) {
34  return;
35  }
36 
37  // get current state
38  std::shared_ptr<const EpisodeState> state = episode->GetState();
39 
40  // purge all possible dead walkers
41  CheckIfWalkerExist(*walkers, *state);
42 
43  // add/update/delete all vehicles in crowd
44  UpdateVehiclesInCrowd(episode, false);
45 
46  // update crowd in navigation module
47  _nav.UpdateCrowd(*state);
48 
50  using Cmd = rpc::Command;
51  std::vector<Cmd> commands;
52  commands.reserve(walkers->size());
53  for (auto handle : *walkers) {
54  // get the transform of the walker
55  if (_nav.GetWalkerTransform(handle.walker, trans)) {
56  float speed = _nav.GetWalkerSpeed(handle.walker);
57  commands.emplace_back(Cmd::ApplyWalkerState{ handle.walker, trans, speed });
58  }
59  }
60 
61  _client.ApplyBatch(std::move(commands), false);
62  }
63 
64  void WalkerNavigation::CheckIfWalkerExist(std::vector<WalkerHandle> walkers, const EpisodeState &state) {
65 
66  // check with total
67  if (_next_check_index >= walkers.size())
69 
70  // check the existence
71  if (!state.ContainsActorSnapshot(walkers[_next_check_index].walker)) {
72  // remove from the crowd
73  _nav.RemoveAgent(walkers[_next_check_index].walker);
74  // destroy the controller
75  _client.DestroyActor(walkers[_next_check_index].controller);
76  // unregister from list
77  UnregisterWalker(walkers[_next_check_index].walker, walkers[_next_check_index].controller);
78  }
79 
81 
82  }
83 
84  // add/update/delete all vehicles in crowd
85  void WalkerNavigation::UpdateVehiclesInCrowd(std::shared_ptr<Episode> episode, bool show_debug) {
86  std::vector<carla::nav::VehicleCollisionInfo> vehicles;
87 
88  // get current state
89  std::shared_ptr<const EpisodeState> state = episode->GetState();
90 
91  // get all vehicles from episode
92  for (auto &&actor : episode->GetActors()) {
93  // only vehicles
94  if (actor.description.id.rfind("vehicle.", 0) == 0) {
95  // get the snapshot
96  ActorSnapshot snapshot = state->GetActorSnapshot(actor.id);
97  // add to the vector
98  vehicles.emplace_back(carla::nav::VehicleCollisionInfo{actor.id, snapshot.transform, actor.bounding_box});
99  }
100  }
101 
102  // update the vehicles found
103  _nav.UpdateVehicles(vehicles);
104 
105  // optional debug info
106  if (show_debug) {
107  if (_nav.GetCrowd() == nullptr) return;
108 
109  // draw bounding boxes for debug
110  for (int i = 0; i < _nav.GetCrowd()->getAgentCount(); ++i) {
111  // get the agent
112  const dtCrowdAgent *agent = _nav.GetCrowd()->getAgent(i);
113  if (agent && agent->params.useObb) {
114  // draw for debug
115  carla::geom::Location p1, p2, p3, p4;
116  p1.x = agent->params.obb[0];
117  p1.z = agent->params.obb[1];
118  p1.y = agent->params.obb[2];
119  p2.x = agent->params.obb[3];
120  p2.z = agent->params.obb[4];
121  p2.y = agent->params.obb[5];
122  p3.x = agent->params.obb[6];
123  p3.z = agent->params.obb[7];
124  p3.y = agent->params.obb[8];
125  p4.x = agent->params.obb[9];
126  p4.z = agent->params.obb[10];
127  p4.y = agent->params.obb[11];
129  line1.life_time = 0.01f;
130  line1.persistent_lines = false;
131  // line 1
132  line1.primitive = carla::rpc::DebugShape::Line {p1, p2, 0.2f};
133  line1.color = { 0, 255, 0 };
134  _client.DrawDebugShape(line1);
135  // line 2
136  line1.primitive = carla::rpc::DebugShape::Line {p2, p3, 0.2f};
137  line1.color = { 255, 0, 0 };
138  _client.DrawDebugShape(line1);
139  // line 3
140  line1.primitive = carla::rpc::DebugShape::Line {p3, p4, 0.2f};
141  line1.color = { 0, 0, 255 };
142  _client.DrawDebugShape(line1);
143  // line 4
144  line1.primitive = carla::rpc::DebugShape::Line {p4, p1, 0.2f};
145  line1.color = { 255, 255, 0 };
146  _client.DrawDebugShape(line1);
147  }
148  }
149 
150  // draw some text for debug
151  for (int i = 0; i < _nav.GetCrowd()->getAgentCount(); ++i) {
152  // get the agent
153  const dtCrowdAgent *agent = _nav.GetCrowd()->getAgent(i);
154  if (agent) {
155  // draw for debug
156  carla::geom::Location p1(agent->npos[0], agent->npos[2], agent->npos[1] + 1);
157  if (agent->params.userData) {
158  std::ostringstream out;
159  out << *(reinterpret_cast<const float *>(agent->params.userData));
161  text.life_time = 0.01f;
162  text.persistent_lines = false;
163  text.primitive = carla::rpc::DebugShape::String {p1, out.str(), false};
164  text.color = { 0, 255, 0 };
165  _client.DrawDebugShape(text);
166  }
167  }
168  }
169  }
170  }
171 
172 } // namespace detail
173 } // namespace client
174 } // namespace carla
void CheckIfWalkerExist(std::vector< WalkerHandle > walkers, const EpisodeState &state)
check a few walkers and if they don&#39;t exist then remove from the crowd
bool RemoveAgent(ActorId id)
remove an agent
Definition: Navigation.cpp:656
carla::rpc::ActorId id
Definition: Navigation.h:48
std::vector< uint8_t > GetCacheFile(const std::string &name, const bool request_otherwise=true) const
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
std::vector< std::string > GetRequiredFiles(const std::string &folder="", const bool download=true) const
void DrawDebugShape(const rpc::DebugShape &shape)
Represents the state of all the actors of an episode at a given frame.
Definition: EpisodeState.h:27
dtCrowd * GetCrowd()
Definition: Navigation.h:112
bool ContainsActorSnapshot(ActorId actor_id) const
Definition: EpisodeState.h:63
void UpdateVehiclesInCrowd(std::shared_ptr< Episode > episode, bool show_debug=false)
add/update/delete all vehicles in crowd
void ApplyBatch(std::vector< rpc::Command > commands, bool do_tick_cue)
boost::variant< Point, Line, Arrow, Box, String > primitive
Definition: DebugShape.h:57
void UpdateCrowd(const client::detail::EpisodeState &state)
update all walkers in crowd
Definition: Navigation.cpp:827
Provides communication with the rpc and streaming servers of a CARLA simulator.
void Tick(std::shared_ptr< Episode > episode)
bool GetWalkerTransform(ActorId id, carla::geom::Transform &trans)
get the walker current transform
Definition: Navigation.cpp:915
void UnregisterWalker(ActorId walker_id, ActorId controller_id)
float GetWalkerSpeed(ActorId id)
get the walker current transform
bool DestroyActor(rpc::ActorId actor)
struct to send info about vehicles to the crowd
Definition: Navigation.h:47
bool Load(const std::string &filename)
load navigation data
Definition: Navigation.cpp:76
bool UpdateVehicles(std::vector< VehicleCollisionInfo > vehicles)
add/update/delete vehicles in crowd
Definition: Navigation.cpp:702