RssSensor fixes (#4604)
* RssSensor fixes RssSensor: - make client side calculations threaded to ensure it is not incrementally delaying the whole client - support multiple instances of RssSensor - initialize _last_processed_frame properly RssRestrictor: - enable python set_log_level General: - Update to RSS v4.4.2 and map v2.4.5_hotfix supporting elevation - manual_control_rss: enalbe dynamics log level switching * Update to ad-rss-lib 4.4.3 * really go to ad-rss-lib 4.4.3
This commit is contained in:
berndgassmann
GitHub
parent
4eeb479c45
commit
6fc187f4b3
|
|
@ -1,4 +1,5 @@
|
|||
## Latest
|
||||
* Fixed RSSSensor: made client side calculations threaded
|
||||
* Added the option for users to set a path using locations to a vehicle controlled by the Traffic Manager.
|
||||
* Added a RoadOption element in each SimpleWaypoint to specify which action will the vehicle perform if it follows that route.
|
||||
* Added the option for users to set a route using RoadOption elements to a vehicle controlled by the Traffic Manager.
|
||||
|
|
|
|||
|
|
@ -144,6 +144,11 @@ public:
|
|||
/// @brief destructor
|
||||
~RssCheck();
|
||||
|
||||
/// @brief get the logger of this
|
||||
std::shared_ptr<spdlog::logger> GetLogger() {
|
||||
return _logger;
|
||||
}
|
||||
|
||||
/// @brief main function to trigger the RSS check at a certain point in time
|
||||
///
|
||||
/// This function has to be called cyclic with increasing timestamps to ensure
|
||||
|
|
|
|||
|
|
@ -22,11 +22,19 @@ RssRestrictor::RssRestrictor() {
|
|||
if (!_logger) {
|
||||
_logger = spdlog::create<spdlog::sinks::stdout_color_sink_st>(logger_name);
|
||||
}
|
||||
//_logger->set_level(spdlog::level::debug);
|
||||
|
||||
SetLogLevel(spdlog::level::warn);
|
||||
}
|
||||
|
||||
RssRestrictor::~RssRestrictor() = default;
|
||||
|
||||
void RssRestrictor::SetLogLevel(const uint8_t log_level) {
|
||||
if (log_level < spdlog::level::n_levels) {
|
||||
const auto log_level_value = static_cast<spdlog::level::level_enum>(log_level);
|
||||
_logger->set_level(log_level_value);
|
||||
}
|
||||
}
|
||||
|
||||
carla::rpc::VehicleControl RssRestrictor::RestrictVehicleControl(
|
||||
const carla::rpc::VehicleControl &vehicle_control, const ::ad::rss::state::ProperResponse &proper_response,
|
||||
const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route,
|
||||
|
|
|
|||
|
|
@ -50,6 +50,8 @@ public:
|
|||
const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route,
|
||||
const carla::rpc::VehiclePhysicsControl &vehicle_physics);
|
||||
|
||||
void SetLogLevel(const uint8_t log_level);
|
||||
|
||||
private:
|
||||
/// @brief the logger instance
|
||||
std::shared_ptr<spdlog::logger> _logger;
|
||||
|
|
|
|||
|
|
@ -22,12 +22,15 @@
|
|||
namespace carla {
|
||||
namespace client {
|
||||
|
||||
std::atomic_uint RssSensor::_global_map_initialization_counter_{0u};
|
||||
|
||||
RssSensor::RssSensor(ActorInitializer init) : Sensor(std::move(init)), _on_tick_register_id(0u), _drop_route(false) {}
|
||||
|
||||
RssSensor::~RssSensor() {
|
||||
// ensure there is no processing anymore when deleting rss_check object
|
||||
const std::lock_guard<std::mutex> lock(_processing_lock);
|
||||
_rss_check.reset();
|
||||
// ensure there is no processing anymore
|
||||
if (IsListening()) {
|
||||
Stop();
|
||||
}
|
||||
}
|
||||
|
||||
void RssSensor::RegisterActorConstellationCallback(ActorConstellationCallbackFunctionType callback) {
|
||||
|
|
@ -68,12 +71,16 @@ void RssSensor::Listen(CallbackFunctionType callback) {
|
|||
DEBUG_ASSERT(map != nullptr);
|
||||
std::string const open_drive_content = map->GetOpenDrive();
|
||||
|
||||
_rss_check = nullptr;
|
||||
::ad::map::access::cleanup();
|
||||
auto mapInitializationResult = ::ad::map::access::initFromOpenDriveContent(
|
||||
open_drive_content, 0.2, ::ad::map::intersection::IntersectionType::TrafficLight,
|
||||
::ad::map::landmark::TrafficLightType::LEFT_STRAIGHT_RED_YELLOW_GREEN);
|
||||
|
||||
::ad::map::access::initFromOpenDriveContent(open_drive_content, 0.2,
|
||||
::ad::map::intersection::IntersectionType::TrafficLight,
|
||||
::ad::map::landmark::TrafficLightType::LEFT_STRAIGHT_RED_YELLOW_GREEN);
|
||||
if (!mapInitializationResult) {
|
||||
log_error(GetDisplayId(), ": Initialization of map failed");
|
||||
return;
|
||||
}
|
||||
|
||||
_global_map_initialization_counter_++;
|
||||
|
||||
if (_rss_actor_constellation_callback == nullptr) {
|
||||
_rss_check = std::make_shared<::carla::rss::RssCheck>(max_steering_angle);
|
||||
|
|
@ -84,15 +91,13 @@ void RssSensor::Listen(CallbackFunctionType callback) {
|
|||
|
||||
auto self = boost::static_pointer_cast<RssSensor>(shared_from_this());
|
||||
|
||||
_last_processed_frame=0u;
|
||||
log_debug(GetDisplayId(), ": subscribing to tick event");
|
||||
_on_tick_register_id = GetEpisode().Lock()->RegisterOnTickEvent(
|
||||
[ cb = std::move(callback), weak_self = WeakPtr<RssSensor>(self) ](const auto &snapshot) {
|
||||
auto self = weak_self.lock();
|
||||
if (self != nullptr) {
|
||||
auto data = self->TickRssSensor(snapshot.GetTimestamp());
|
||||
if (data != nullptr) {
|
||||
cb(std::move(data));
|
||||
}
|
||||
self->TickRssSensor(snapshot.GetTimestamp(), cb);
|
||||
}
|
||||
});
|
||||
}
|
||||
|
|
@ -106,6 +111,25 @@ void RssSensor::Stop() {
|
|||
log_debug(GetDisplayId(), ": unsubscribing from tick event");
|
||||
GetEpisode().Lock()->RemoveOnTickEvent(_on_tick_register_id);
|
||||
_on_tick_register_id = 0u;
|
||||
|
||||
if ( bool(_rss_check) ) {
|
||||
_rss_check->GetLogger()->info("RssSensor stopping");
|
||||
}
|
||||
// don't remove the braces since they protect the lock_guard
|
||||
{
|
||||
// ensure there is no processing anymore when deleting rss_check object
|
||||
const std::lock_guard<std::mutex> lock(_processing_lock);
|
||||
|
||||
if ( bool(_rss_check) ) {
|
||||
_rss_check->GetLogger()->info("RssSensor delete checker");
|
||||
}
|
||||
_rss_check.reset();
|
||||
auto const map_initialization_counter_value = _global_map_initialization_counter_--;
|
||||
if (map_initialization_counter_value == 0u) {
|
||||
// last one stop listening is cleaning up the map
|
||||
::ad::map::access::cleanup();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void RssSensor::SetLogLevel(const uint8_t &log_level) {
|
||||
|
|
@ -269,59 +293,84 @@ void RssSensor::DropRoute() {
|
|||
_drop_route = true;
|
||||
}
|
||||
|
||||
SharedPtr<sensor::SensorData> RssSensor::TickRssSensor(const Timestamp ×tamp) {
|
||||
void RssSensor::TickRssSensor(const client::Timestamp ×tamp, CallbackFunctionType callback) {
|
||||
if (_processing_lock.try_lock()) {
|
||||
if (!bool(_rss_check)){
|
||||
_processing_lock.unlock();
|
||||
return;
|
||||
}
|
||||
if ((timestamp.frame < _last_processed_frame) && ((_last_processed_frame - timestamp.frame) < 0xffffffffu)) {
|
||||
_processing_lock.unlock();
|
||||
_rss_check->GetLogger()->warn("RssSensor[{}] outdated tick dropped, LastProcessed={}", timestamp.frame, _last_processed_frame);
|
||||
return;
|
||||
}
|
||||
_last_processed_frame = timestamp.frame;
|
||||
SharedPtr<carla::client::ActorList> actors = GetWorld().GetActors();
|
||||
|
||||
auto const settings = GetWorld().GetSettings();
|
||||
if ( settings.synchronous_mode ) {
|
||||
_rss_check->GetLogger()->trace("RssSensor[{}] sync-tick", timestamp.frame);
|
||||
TickRssSensorThreadLocked(timestamp, actors, callback);
|
||||
}
|
||||
else {
|
||||
// store the future to prevent the destructor of the future from blocked waiting
|
||||
_rss_check->GetLogger()->trace("RssSensor[{}] async-tick", timestamp.frame);
|
||||
_tick_future = std::async(&RssSensor::TickRssSensorThreadLocked, this, timestamp, actors, callback);
|
||||
}
|
||||
} else {
|
||||
if (bool(_rss_check)){
|
||||
_rss_check->GetLogger()->debug("RssSensor[{}] tick dropped", timestamp.frame);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void RssSensor::TickRssSensorThreadLocked(const client::Timestamp ×tamp,
|
||||
SharedPtr<carla::client::ActorList> actors, CallbackFunctionType callback) {
|
||||
try {
|
||||
bool result = false;
|
||||
double const time_since_epoch_check_start_ms =
|
||||
std::chrono::duration<double, std::milli>(std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
|
||||
if (_drop_route) {
|
||||
_drop_route = false;
|
||||
_rss_check->DropRoute();
|
||||
}
|
||||
|
||||
// check all object<->ego pairs with RSS and calculate proper response
|
||||
::ad::rss::state::ProperResponse response;
|
||||
::ad::rss::state::RssStateSnapshot rss_state_snapshot;
|
||||
::ad::rss::situation::SituationSnapshot situation_snapshot;
|
||||
::ad::rss::world::WorldModel world_model;
|
||||
carla::rss::EgoDynamicsOnRoute ego_dynamics_on_route;
|
||||
if (_processing_lock.try_lock()) {
|
||||
spdlog::debug("RssSensor tick: T={}", timestamp.frame);
|
||||
auto const result = _rss_check->CheckObjects(timestamp, actors, GetParent(), response, rss_state_snapshot,
|
||||
situation_snapshot, world_model, ego_dynamics_on_route);
|
||||
|
||||
if ((timestamp.frame < _last_processed_frame) && ((_last_processed_frame - timestamp.frame) < 0xffffffffu))
|
||||
{
|
||||
_processing_lock.unlock();
|
||||
spdlog::warn("RssSensor tick dropped: T={}", timestamp.frame);
|
||||
return nullptr;
|
||||
}
|
||||
_last_processed_frame = timestamp.frame;
|
||||
|
||||
carla::client::World world = GetWorld();
|
||||
SharedPtr<carla::client::ActorList> actors = world.GetActors();
|
||||
|
||||
if (_drop_route) {
|
||||
_drop_route = false;
|
||||
_rss_check->DropRoute();
|
||||
}
|
||||
|
||||
// check all object<->ego pairs with RSS and calculate proper response
|
||||
result = _rss_check->CheckObjects(timestamp, actors, GetParent(), response, rss_state_snapshot,
|
||||
situation_snapshot, world_model, ego_dynamics_on_route);
|
||||
_processing_lock.unlock();
|
||||
|
||||
spdlog::debug(
|
||||
"RssSensor response: T={} S:{}->E:{} DeltaT:{}", timestamp.frame,
|
||||
ego_dynamics_on_route.time_since_epoch_check_start_ms, ego_dynamics_on_route.time_since_epoch_check_end_ms,
|
||||
ego_dynamics_on_route.time_since_epoch_check_end_ms - ego_dynamics_on_route.time_since_epoch_check_start_ms);
|
||||
return MakeShared<sensor::data::RssResponse>(timestamp.frame, timestamp.elapsed_seconds, GetTransform(), result,
|
||||
response, rss_state_snapshot, situation_snapshot, world_model,
|
||||
ego_dynamics_on_route);
|
||||
} else {
|
||||
spdlog::debug("RssSensor tick dropped: T={}", timestamp.frame);
|
||||
return nullptr;
|
||||
double const time_since_epoch_check_end_ms =
|
||||
std::chrono::duration<double, std::milli>(std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
auto const delta_time_ms = time_since_epoch_check_end_ms - time_since_epoch_check_start_ms;
|
||||
_rss_check->GetLogger()->debug("RssSensor[{}] response: S:{}->E:{} DeltaT:{}", timestamp.frame,
|
||||
time_since_epoch_check_start_ms, time_since_epoch_check_end_ms,
|
||||
delta_time_ms);
|
||||
_rss_check_timings.push_back(delta_time_ms);
|
||||
while (_rss_check_timings.size() > 10u) {
|
||||
_rss_check_timings.pop_front();
|
||||
}
|
||||
double agv_time=0.;
|
||||
for (auto run_time: _rss_check_timings) {
|
||||
agv_time += run_time;
|
||||
}
|
||||
agv_time /= _rss_check_timings.size();
|
||||
_rss_check->GetLogger()->info("RssSensor[{}] runtime {} avg {}", timestamp.frame, delta_time_ms, agv_time);
|
||||
_processing_lock.unlock();
|
||||
|
||||
callback(MakeShared<sensor::data::RssResponse>(timestamp.frame, timestamp.elapsed_seconds, GetTransform(), result,
|
||||
response, rss_state_snapshot, situation_snapshot, world_model,
|
||||
ego_dynamics_on_route));
|
||||
} catch (const std::exception &e) {
|
||||
/// @todo do we need to unsubscribe the sensor here?
|
||||
std::cout << e.what() << std::endl;
|
||||
_rss_check->GetLogger()->error("RssSensor[{}] tick exception", timestamp.frame);
|
||||
_processing_lock.unlock();
|
||||
spdlog::error("RssSensor tick exception");
|
||||
return nullptr;
|
||||
} catch (...) {
|
||||
_rss_check->GetLogger()->error("RssSensor[{}] tick exception", timestamp.frame);
|
||||
_processing_lock.unlock();
|
||||
spdlog::error("RssSensor tick exception");
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -5,6 +5,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <future>
|
||||
#include <list>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <vector>
|
||||
|
|
@ -66,6 +69,7 @@ public:
|
|||
void Listen(CallbackFunctionType callback) override;
|
||||
|
||||
/// Stop listening for new measurements.
|
||||
/// Be aware: GIL has to be unlocked to be able to wait for callbacks not active.
|
||||
void Stop() override;
|
||||
|
||||
/// Return whether this Sensor instance is currently listening to the
|
||||
|
|
@ -123,7 +127,12 @@ public:
|
|||
|
||||
private:
|
||||
/// the acutal sensor tick callback function
|
||||
SharedPtr<sensor::SensorData> TickRssSensor(const Timestamp ×tamp);
|
||||
void TickRssSensor(const client::Timestamp ×tamp, CallbackFunctionType callback);
|
||||
void TickRssSensorThreadLocked(const client::Timestamp ×tamp, SharedPtr<carla::client::ActorList> actors,
|
||||
CallbackFunctionType callback);
|
||||
|
||||
//// the object actually performing the RSS processing
|
||||
std::shared_ptr<::carla::rss::RssCheck> _rss_check;
|
||||
|
||||
/// the id got when registering for the on tick event
|
||||
std::size_t _on_tick_register_id;
|
||||
|
|
@ -131,8 +140,11 @@ private:
|
|||
/// the mutex to protect the actual RSS processing and in case it takes too long to process ever frame
|
||||
std::mutex _processing_lock;
|
||||
|
||||
//// the object actually performing the RSS processing
|
||||
std::shared_ptr<::carla::rss::RssCheck> _rss_check;
|
||||
/// the future for the async ticking thread
|
||||
std::future<void> _tick_future;
|
||||
|
||||
/// some debug timings
|
||||
std::list<double> _rss_check_timings;
|
||||
|
||||
//// the rss actor constellation callback function
|
||||
ActorConstellationCallbackFunctionType _rss_actor_constellation_callback;
|
||||
|
|
@ -142,6 +154,8 @@ private:
|
|||
|
||||
/// last processed frame
|
||||
std::size_t _last_processed_frame;
|
||||
|
||||
static std::atomic_uint _global_map_initialization_counter_;
|
||||
};
|
||||
|
||||
} // namespace client
|
||||
|
|
|
|||
|
|
@ -93,6 +93,12 @@ static void RegisterActorConstellationCallback(carla::client::RssSensor &self, b
|
|||
self.RegisterActorConstellationCallback(callback_function);
|
||||
}
|
||||
|
||||
static void Stop(carla::client::RssSensor &self) {
|
||||
// ensure the RssSensor is stopped with GIL released to sync on processing lock
|
||||
carla::PythonUtil::ReleaseGIL unlock;
|
||||
self.Stop();
|
||||
}
|
||||
|
||||
void export_ad() {
|
||||
using namespace boost::python;
|
||||
|
||||
|
|
@ -186,6 +192,7 @@ void export_ad_rss() {
|
|||
.add_property("pedestrian_dynamics", &GetPedestrianDynamics, &cc::RssSensor::SetPedestrianDynamics)
|
||||
.add_property("road_boundaries_mode", &GetRoadBoundariesMode, &cc::RssSensor::SetRoadBoundariesMode)
|
||||
.add_property("routing_targets", &GetRoutingTargets)
|
||||
.def("stop", &Stop)
|
||||
.def("register_actor_constellation_callback", &RegisterActorConstellationCallback, (arg("callback")))
|
||||
.def("append_routing_target", &cc::RssSensor::AppendRoutingTarget, (arg("routing_target")))
|
||||
.def("reset_routing_targets", &cc::RssSensor::ResetRoutingTargets)
|
||||
|
|
@ -199,5 +206,6 @@ void export_ad_rss() {
|
|||
.def(init<>())
|
||||
.def("restrict_vehicle_control", &carla::rss::RssRestrictor::RestrictVehicleControl,
|
||||
(arg("vehicle_control"), arg("proper_response"), arg("ego_dynamics_on_route"), arg("vehicle_physics")))
|
||||
.def("set_log_level", &carla::rss::RssRestrictor::SetLogLevel, (arg("log_level")))
|
||||
.def(self_ns::str(self_ns::self));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -153,6 +153,14 @@
|
|||
carla.VehicleControl
|
||||
doc: >
|
||||
Applies the safety restrictions given by a carla.RssSensor to a carla.VehicleControl.
|
||||
- def_name: set_log_level
|
||||
params:
|
||||
- param_name: log_level
|
||||
type: carla.RssLogLevel
|
||||
doc: >
|
||||
New log level.
|
||||
doc: >
|
||||
Sets the log level.
|
||||
# --------------------------------------
|
||||
|
||||
- class_name: RssRoadBoundariesMode
|
||||
|
|
|
|||
|
|
@ -28,6 +28,10 @@ Use ARROWS or WASD keys for control.
|
|||
R : toggle recording images to disk
|
||||
|
||||
F2 : toggle RSS visualization mode
|
||||
F3 : increase log level
|
||||
F4 : decrease log level
|
||||
F5 : increase map log level
|
||||
F6 : decrease map log level
|
||||
B : toggle RSS Road Boundaries Mode
|
||||
G : RSS check drop current route
|
||||
T : toggle RSS
|
||||
|
|
@ -87,6 +91,10 @@ try:
|
|||
from pygame.locals import K_ESCAPE
|
||||
from pygame.locals import K_F1
|
||||
from pygame.locals import K_F2
|
||||
from pygame.locals import K_F3
|
||||
from pygame.locals import K_F4
|
||||
from pygame.locals import K_F5
|
||||
from pygame.locals import K_F6
|
||||
from pygame.locals import K_LEFT
|
||||
from pygame.locals import K_RIGHT
|
||||
from pygame.locals import K_SLASH
|
||||
|
|
@ -408,6 +416,20 @@ class VehicleControl(object):
|
|||
elif event.key == K_F2:
|
||||
if self._world and self._world.rss_sensor:
|
||||
self._world.rss_sensor.toggle_debug_visualization_mode()
|
||||
elif event.key == K_F3:
|
||||
if self._world and self._world.rss_sensor:
|
||||
self._world.rss_sensor.decrease_log_level()
|
||||
self._restrictor.set_log_level(self._world.rss_sensor.log_level)
|
||||
elif event.key == K_F4:
|
||||
if self._world and self._world.rss_sensor:
|
||||
self._world.rss_sensor.increase_log_level()
|
||||
self._restrictor.set_log_level(self._world.rss_sensor.log_level)
|
||||
elif event.key == K_F5:
|
||||
if self._world and self._world.rss_sensor:
|
||||
self._world.rss_sensor.decrease_map_log_level()
|
||||
elif event.key == K_F6:
|
||||
if self._world and self._world.rss_sensor:
|
||||
self._world.rss_sensor.increase_map_log_level()
|
||||
elif event.key == K_b:
|
||||
if self._world and self._world.rss_sensor:
|
||||
if self._world.rss_sensor.sensor.road_boundaries_mode == carla.RssRoadBoundariesMode.Off:
|
||||
|
|
|
|||
|
|
@ -49,6 +49,18 @@ class RssStateInfo(object):
|
|||
self.distance = math.sqrt((float(ego_dynamics_on_route.ego_center.x) - float(object_state.centerPoint.x))**2 +
|
||||
(float(ego_dynamics_on_route.ego_center.y) - float(object_state.centerPoint.y))**2)
|
||||
|
||||
self.longitudinal_margin = float(rss_state.longitudinalState.rssStateInformation.currentDistance - rss_state.longitudinalState.rssStateInformation.safeDistance)
|
||||
self.margin = max(0, self.longitudinal_margin)
|
||||
self.lateral_margin = None
|
||||
if rss_state.lateralStateLeft.rssStateInformation.evaluator != "None":
|
||||
self.lateral_margin = rss_state.lateralStateLeft.rssStateInformation.currentDistance - rss_state.lateralStateLeft.rssStateInformation.safeDistance
|
||||
if rss_state.lateralStateRight.rssStateInformation.evaluator != "None":
|
||||
lateral_margin_right = rss_state.lateralStateRight.rssStateInformation.currentDistance - rss_state.lateralStateRight.rssStateInformation.safeDistance
|
||||
if self.lateral_margin==None or self.lateral_margin > lateral_margin_right:
|
||||
self.lateral_margin=lateral_margin_right
|
||||
if self.lateral_margin!=None and self.lateral_margin>0:
|
||||
self.margin += self.lateral_margin
|
||||
|
||||
def get_actor(self, world):
|
||||
if self.rss_state.objectId == 18446744073709551614:
|
||||
return None # "Border Left"
|
||||
|
|
@ -103,19 +115,22 @@ class RssSensor(object):
|
|||
if not inspect.getmembers(carla, check_rss_class):
|
||||
raise RuntimeError('CARLA PythonAPI not compiled in RSS variant, please "make PythonAPI.rss"')
|
||||
|
||||
self.log_level = carla.RssLogLevel.warn
|
||||
self.map_log_level = carla.RssLogLevel.warn
|
||||
|
||||
self.set_default_parameters()
|
||||
|
||||
self.sensor.register_actor_constellation_callback(self._on_actor_constellation_request)
|
||||
|
||||
self.sensor.listen(self._on_rss_response)
|
||||
self.sensor.set_log_level(self.log_level)
|
||||
self.sensor.set_map_log_level(self.map_log_level)
|
||||
|
||||
# only relevant if actor constellation callback is not registered
|
||||
# self.sensor.ego_vehicle_dynamics = self.current_vehicle_parameters
|
||||
|
||||
self.sensor.road_boundaries_mode = carla.RssRoadBoundariesMode.Off
|
||||
|
||||
# self.sensor.set_log_level(carla.RssLogLevel.trace)
|
||||
|
||||
self.sensor.reset_routing_targets()
|
||||
if routing_targets:
|
||||
for target in routing_targets:
|
||||
|
|
@ -263,6 +278,7 @@ class RssSensor(object):
|
|||
# print("_on_actor_constellation_result({}) setting accelMax to
|
||||
# zero".format(actor_constellation_data.other_actor.id))
|
||||
actor_constellation_result.actor_dynamics.alphaLon.accelMax = 0.
|
||||
actor_constellation_result.actor_dynamics.alphaLat.accelMax = 0.
|
||||
else:
|
||||
# store route for debug drawings
|
||||
self.route = actor_constellation_data.ego_route
|
||||
|
|
@ -296,6 +312,28 @@ class RssSensor(object):
|
|||
def toggle_debug_visualization_mode(self):
|
||||
self.debug_visualizer.toggleMode()
|
||||
|
||||
def increase_log_level(self):
|
||||
print("inccrease {}".format(self.log_level))
|
||||
if self.log_level < carla.RssLogLevel.off:
|
||||
self.log_level = self.log_level+1
|
||||
self.sensor.set_log_level(self.log_level)
|
||||
|
||||
def decrease_log_level(self):
|
||||
print("decrease {}".format(self.log_level))
|
||||
if self.log_level > carla.RssLogLevel.trace:
|
||||
self.log_level = self.log_level-1
|
||||
self.sensor.set_log_level(self.log_level)
|
||||
|
||||
def increase_map_log_level(self):
|
||||
if self.map_log_level < carla.RssLogLevel.off:
|
||||
self.map_log_level = self.map_log_level+1
|
||||
self.sensor.set_map_log_level(self.map_log_level)
|
||||
|
||||
def decrease_map_log_level(self):
|
||||
if self.map_log_level > carla.RssLogLevel.trace:
|
||||
self.map_log_level = self.map_log_level-1
|
||||
self.sensor.set_map_log_level(self.map_log_level)
|
||||
|
||||
@staticmethod
|
||||
def get_default_parameters():
|
||||
ego_dynamics = ad.rss.world.RssDynamics()
|
||||
|
|
|
|||
|
|
@ -197,13 +197,13 @@ class RssUnstructuredSceneVisualizer(object):
|
|||
else:
|
||||
self._surface = None
|
||||
|
||||
self._calibration = np.identity(3)
|
||||
self._calibration[0, 2] = self._dim[0] / 2.0
|
||||
self._calibration[1, 2] = self._dim[1] / 2.0
|
||||
self._calibration[0, 0] = self._calibration[1, 1] = self._dim[0] / \
|
||||
if spawn_sensor:
|
||||
self._calibration = np.identity(3)
|
||||
self._calibration[0, 2] = self._dim[0] / 2.0
|
||||
self._calibration[1, 2] = self._dim[1] / 2.0
|
||||
self._calibration[0, 0] = self._calibration[1, 1] = self._dim[0] / \
|
||||
(2.0 * np.tan(90.0 * np.pi / 360.0)) # fov default: 90.0
|
||||
|
||||
if spawn_sensor:
|
||||
bp_library = self._world.get_blueprint_library()
|
||||
bp = bp_library.find('sensor.camera.rgb')
|
||||
bp.set_attribute('image_size_x', str(self._dim[0]))
|
||||
|
|
@ -660,14 +660,14 @@ class RssDebugVisualizer(object):
|
|||
|
||||
def visualize_enu_edge(self, edge, color, z_offset):
|
||||
for point in edge:
|
||||
carla_point = carla.Location(x=float(point.x), y=float(-1 * point.y), z=float(point.z + z_offset))
|
||||
carla_point = carla.Location(x=float(point.x), y=-1. * float(point.y), z=float(point.z) + z_offset)
|
||||
self._world.debug.draw_point(carla_point, 0.1, color, 0.1, False)
|
||||
|
||||
def visualize_rss_results(self, state_snapshot):
|
||||
for state in state_snapshot:
|
||||
other_actor = state.get_actor(self._world)
|
||||
if not other_actor:
|
||||
print("Actor not found. Skip visualizing state {}".format(state))
|
||||
# print("Actor not found. Skip visualizing state {}".format(state))
|
||||
continue
|
||||
ego_point = self._player.get_location()
|
||||
ego_point.z += 0.05
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ IFS="," read -r -a PY_VERSION_LIST <<< "${PY_VERSION_LIST}"
|
|||
# -- Get ad-rss -------------------------------------------
|
||||
# ==============================================================================
|
||||
|
||||
ADRSS_VERSION=4.4.0
|
||||
ADRSS_VERSION=4.4.3
|
||||
ADRSS_BASENAME=ad-rss-${ADRSS_VERSION}
|
||||
ADRSS_COLCON_WORKSPACE="${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}"
|
||||
ADRSS_SRC_DIR="${ADRSS_COLCON_WORKSPACE}/src"
|
||||
|
|
@ -42,7 +42,7 @@ if [[ ! -d "${ADRSS_SRC_DIR}" ]]; then
|
|||
|
||||
pushd "${ADRSS_SRC_DIR}" >/dev/null
|
||||
git clone --depth=1 -b v1.7.0 https://github.com/gabime/spdlog.git
|
||||
git clone --depth=1 -b v2.3.0 https://github.com/carla-simulator/map.git
|
||||
git clone --depth=1 -b v2.4.5_hotfix https://github.com/carla-simulator/map.git
|
||||
git clone --depth=1 -b v${ADRSS_VERSION} https://github.com/intel/ad-rss-lib.git
|
||||
popd
|
||||
|
||||
|
|
|
|||
|
|
@ -129,7 +129,7 @@ function build_libcarla {
|
|||
M_TOOLCHAIN=${LIBSTDCPP_TOOLCHAIN_FILE}
|
||||
M_BUILD_FOLDER=${LIBCARLA_BUILD_CLIENT_FOLDER}.rss.$(echo "$2" | tr '[:upper:]' '[:lower:]')
|
||||
M_INSTALL_FOLDER=${LIBCARLA_INSTALL_CLIENT_FOLDER}
|
||||
CMAKE_EXTRA_OPTIONS="${CMAKE_EXTRA_OPTIONS:+${CMAKE_EXTRA_OPTIONS} }-DBUILD_RSS_VARIANT=ON -DADRSS_INSTALL_DIR=${CARLA_BUILD_FOLDER}/ad-rss-4.4.0/install"
|
||||
CMAKE_EXTRA_OPTIONS="${CMAKE_EXTRA_OPTIONS:+${CMAKE_EXTRA_OPTIONS} }-DBUILD_RSS_VARIANT=ON -DADRSS_INSTALL_DIR=${CARLA_BUILD_FOLDER}/ad-rss-4.4.3/install"
|
||||
else
|
||||
fatal_error "Invalid build configuration \"$1\""
|
||||
fi
|
||||
|
|
|
|||
Loading…
Reference in New Issue