From 3c8571e34a7911a5716c0cd1e7c00d4f0546f2bb Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 10 Aug 2024 00:03:58 +0200 Subject: [PATCH] Update codebase to new clang-format style --- modules/comms/include/mvsim/Comms/Client.h | 47 ++-- modules/comms/include/mvsim/Comms/Server.h | 16 +- modules/comms/include/mvsim/Comms/common.h | 17 +- .../comms/include/mvsim/Comms/zmq_monitor.h | 6 +- modules/comms/src/Comms/Client.cpp | 143 ++++------ modules/comms/src/Comms/Server.cpp | 107 +++----- modules/comms/src/Comms/common.cpp | 9 +- modules/simulator/include/mvsim/Block.h | 14 +- .../simulator/include/mvsim/ClassFactory.h | 19 +- .../include/mvsim/CollisionShapeCache.h | 5 +- .../simulator/include/mvsim/ControllerBase.h | 22 +- .../mvsim/FrictionModels/DefaultFriction.h | 3 +- .../mvsim/FrictionModels/FrictionBase.h | 19 +- .../FrictionModels/WardIagnemmaFriction.h | 3 +- modules/simulator/include/mvsim/Joystick.h | 3 +- .../include/mvsim/RemoteResourcesManager.h | 3 +- .../include/mvsim/Sensors/CameraSensor.h | 11 +- .../include/mvsim/Sensors/DepthCameraSensor.h | 9 +- modules/simulator/include/mvsim/Sensors/IMU.h | 6 +- .../include/mvsim/Sensors/LaserScanner.h | 6 +- .../simulator/include/mvsim/Sensors/Lidar3D.h | 11 +- .../include/mvsim/Sensors/SensorBase.h | 27 +- modules/simulator/include/mvsim/Shape2p5.h | 16 +- modules/simulator/include/mvsim/Simulable.h | 6 +- modules/simulator/include/mvsim/VehicleBase.h | 40 +-- .../mvsim/VehicleDynamics/VehicleAckermann.h | 33 +-- .../VehicleAckermann_Drivetrain.h | 27 +- .../VehicleDynamics/VehicleDifferential.h | 29 +-- .../simulator/include/mvsim/VisualObject.h | 11 +- modules/simulator/include/mvsim/Wheel.h | 15 +- modules/simulator/include/mvsim/World.h | 110 +++----- .../mvsim/WorldElements/ElevationMap.h | 6 +- .../include/mvsim/WorldElements/GroundGrid.h | 3 +- .../mvsim/WorldElements/HorizontalPlane.h | 3 +- .../mvsim/WorldElements/OccupancyGridMap.h | 3 +- .../include/mvsim/WorldElements/PointCloud.h | 3 +- .../include/mvsim/WorldElements/SkyBox.h | 3 +- .../mvsim/WorldElements/VerticalPlane.h | 3 +- .../mvsim/WorldElements/WorldElementBase.h | 13 +- modules/simulator/src/Block.cpp | 83 ++---- modules/simulator/src/CollisionShapeCache.cpp | 54 ++-- modules/simulator/src/CsvLogger.cpp | 16 +- .../src/FrictionModels/DefaultFriction.cpp | 38 +-- .../src/FrictionModels/FrictionBase.cpp | 8 +- .../FrictionModels/WardIagnemmaFriction.cpp | 39 ++- modules/simulator/src/JointXMLnode.h | 20 +- modules/simulator/src/Joystick.cpp | 22 +- modules/simulator/src/ModelsCache.cpp | 6 +- modules/simulator/src/ModelsCache.h | 3 +- modules/simulator/src/PID_Controller.cpp | 4 +- .../simulator/src/RemoteResourcesManager.cpp | 23 +- .../simulator/src/Sensors/CameraSensor.cpp | 30 +-- .../src/Sensors/DepthCameraSensor.cpp | 94 +++---- modules/simulator/src/Sensors/IMU.cpp | 19 +- .../simulator/src/Sensors/LaserScanner.cpp | 111 +++----- modules/simulator/src/Sensors/Lidar3D.cpp | 130 ++++----- modules/simulator/src/Sensors/SensorBase.cpp | 47 ++-- modules/simulator/src/Shape2p5.cpp | 79 +++--- modules/simulator/src/Simulable.cpp | 140 ++++------ modules/simulator/src/VehicleBase.cpp | 136 ++++------ .../src/VehicleDynamics/VehicleAckermann.cpp | 56 ++-- ...hicleAckermann_ControllerFrontSteerPID.cpp | 9 +- .../VehicleAckermann_ControllerRaw.cpp | 6 +- ...Ackermann_ControllerTwistFrontSteerPID.cpp | 32 +-- .../VehicleAckermann_Drivetrain.cpp | 89 +++---- .../VehicleDynamics/VehicleDifferential.cpp | 33 +-- .../VehicleDifferential_ControllerRaw.cpp | 3 +- ...hicleDifferential_ControllerTwistIdeal.cpp | 6 +- ...VehicleDifferential_ControllerTwistPID.cpp | 12 +- ...rivetrain_ControllerTwistFrontSteerPID.cpp | 11 +- modules/simulator/src/VisualObject.cpp | 25 +- modules/simulator/src/Wheel.cpp | 19 +- modules/simulator/src/World.cpp | 65 ++--- .../src/WorldElements/ElevationMap.cpp | 61 ++--- .../src/WorldElements/GroundGrid.cpp | 18 +- .../src/WorldElements/HorizontalPlane.cpp | 15 +- .../src/WorldElements/OccupancyGridMap.cpp | 82 +++--- .../src/WorldElements/PointCloud.cpp | 8 +- .../simulator/src/WorldElements/SkyBox.cpp | 22 +- .../src/WorldElements/VerticalPlane.cpp | 18 +- modules/simulator/src/World_gui.cpp | 246 +++++++----------- modules/simulator/src/World_load_xml.cpp | 84 ++---- modules/simulator/src/World_services.cpp | 43 ++- modules/simulator/src/World_walls.cpp | 24 +- modules/simulator/src/XMLClassesRegistry.cpp | 21 +- modules/simulator/src/XMLClassesRegistry.h | 8 +- modules/simulator/src/parse_utils.cpp | 35 +-- modules/simulator/src/parse_utils.h | 3 +- modules/simulator/src/xml_utils.cpp | 136 ++++------ modules/simulator/src/xml_utils.h | 26 +- 90 files changed, 1100 insertions(+), 2048 deletions(-) diff --git a/modules/comms/include/mvsim/Comms/Client.h b/modules/comms/include/mvsim/Comms/Client.h index a7456ffb..ec6c4f09 100644 --- a/modules/comms/include/mvsim/Comms/Client.h +++ b/modules/comms/include/mvsim/Comms/Client.h @@ -81,13 +81,11 @@ class Client : public mrpt::system::COutputLogger template void advertiseTopic(const std::string& topicName); - void publishTopic( - const std::string& topicName, const google::protobuf::Message& msg); + void publishTopic(const std::string& topicName, const google::protobuf::Message& msg); template void subscribeTopic( - const std::string& topicName, - const std::function& callback); + const std::string& topicName, const std::function& callback); template void advertiseService( @@ -96,19 +94,17 @@ class Client : public mrpt::system::COutputLogger template void callService( - const std::string& serviceName, const INPUT_MSG_T& input, - OUTPUT_MSG_T& output); + const std::string& serviceName, const INPUT_MSG_T& input, OUTPUT_MSG_T& output); /// Overload for python wrapper - std::string callService( - const std::string& serviceName, const std::string& inputSerializedMsg); + std::string callService(const std::string& serviceName, const std::string& inputSerializedMsg); /// Overload for python wrapper (callback accepts bytes-string) void subscribeTopic( const std::string& topicName, - const std::function& /*serializedMsg*/)>& callback); + const std::function< + void(const std::string& /*msgType*/, const std::vector& /*serializedMsg*/)>& + callback); struct InfoPerNode { @@ -126,8 +122,7 @@ class Client : public mrpt::system::COutputLogger using topic_callback_t = std::function; - void subscribe_topic_raw( - const std::string& topicName, const topic_callback_t& callback); + void subscribe_topic_raw(const std::string& topicName, const topic_callback_t& callback); /** @} */ @@ -156,22 +151,17 @@ class Client : public mrpt::system::COutputLogger void internalTopicUpdatesThread(); void internalTopicSubscribeThread(internal::InfoPerSubscribedTopic& ipt); - using service_callback_t = - std::function( - const std::string& /*inAsString*/)>; + using service_callback_t = std::function( + const std::string& /*inAsString*/)>; void doAdvertiseTopic( - const std::string& topicName, - const google::protobuf::Descriptor* descriptor); + const std::string& topicName, const google::protobuf::Descriptor* descriptor); void doAdvertiseService( - const std::string& serviceName, - const google::protobuf::Descriptor* descIn, - const google::protobuf::Descriptor* descOut, - service_callback_t callback); + const std::string& serviceName, const google::protobuf::Descriptor* descIn, + const google::protobuf::Descriptor* descOut, service_callback_t callback); void doSubscribeTopic( - const std::string& topicName, - const google::protobuf::Descriptor* descriptor, + const std::string& topicName, const google::protobuf::Descriptor* descriptor, const topic_callback_t& callback); void doCallService( const std::string& serviceName, const std::string& inputSerializedMsg, @@ -191,8 +181,7 @@ void Client::advertiseTopic(const std::string& topicName) template void Client::advertiseService( - const std::string& serviceName, - const std::function& callback) + const std::string& serviceName, const std::function& callback) { doAdvertiseService( serviceName, INPUT_MSG_T::descriptor(), OUTPUT_MSG_T::descriptor(), @@ -207,8 +196,7 @@ void Client::advertiseService( template void Client::subscribeTopic( - const std::string& topicName, - const std::function& callback) + const std::string& topicName, const std::function& callback) { doSubscribeTopic( topicName, MSG_T::descriptor(), @@ -223,8 +211,7 @@ void Client::subscribeTopic( template void Client::callService( - const std::string& serviceName, const INPUT_MSG_T& input, - OUTPUT_MSG_T& output) + const std::string& serviceName, const INPUT_MSG_T& input, OUTPUT_MSG_T& output) { doCallService(serviceName, input.SerializeAsString(), output); } diff --git a/modules/comms/include/mvsim/Comms/Server.h b/modules/comms/include/mvsim/Comms/Server.h index eafe0538..ca97ac09 100644 --- a/modules/comms/include/mvsim/Comms/Server.h +++ b/modules/comms/include/mvsim/Comms/Server.h @@ -124,8 +124,7 @@ class Server : public mrpt::system::COutputLogger const std::string& serviceName, std::string& publisherEndpoint, std::string& nodeName) const; - void db_add_topic_subscriber( - const std::string& topicName, const std::string& updatesEndPoint); + void db_add_topic_subscriber(const std::string& topicName, const std::string& updatesEndPoint); /** Send to updatesEndPoint only, if given; otherwise, send to all * subscribers */ @@ -153,8 +152,7 @@ class Server : public mrpt::system::COutputLogger struct InfoPerPublisher { InfoPerPublisher( - const std::string& topic_name, - const std::string& publisher_node_name, + const std::string& topic_name, const std::string& publisher_node_name, const std::string& publisher_endpoint) : topicName(topic_name), publisherNodeName(publisher_node_name), @@ -168,11 +166,8 @@ class Server : public mrpt::system::COutputLogger struct InfoPerSubscriber { - InfoPerSubscriber( - const std::string& topic_name, - const std::string& sub_updates_endpoint) - : topicName(topic_name), - subscriberUpdatesEndpoint(sub_updates_endpoint) + InfoPerSubscriber(const std::string& topic_name, const std::string& sub_updates_endpoint) + : topicName(topic_name), subscriberUpdatesEndpoint(sub_updates_endpoint) { } const std::string topicName; @@ -182,8 +177,7 @@ class Server : public mrpt::system::COutputLogger struct InfoPerTopic { InfoPerTopic() = default; - InfoPerTopic( - const std::string& name, const std::string& topic_type_name) + InfoPerTopic(const std::string& name, const std::string& topic_type_name) : topicName(name), topicTypeName(topic_type_name) { } diff --git a/modules/comms/include/mvsim/Comms/common.h b/modules/comms/include/mvsim/Comms/common.h index 110b3d95..844e5dfd 100644 --- a/modules/comms/include/mvsim/Comms/common.h +++ b/modules/comms/include/mvsim/Comms/common.h @@ -43,24 +43,19 @@ zmq::message_t receiveMessage(zmq::socket_t& s); * * \exception std::runtime_error If the message type does not match with out. */ -void parseMessage( - const zmq::message_t& msg, google::protobuf::MessageLite& out); +void parseMessage(const zmq::message_t& msg, google::protobuf::MessageLite& out); class UnexpectedMessageException : public std::runtime_error { public: - UnexpectedMessageException(const char* reason) : std::runtime_error(reason) - { - } + UnexpectedMessageException(const char* reason) : std::runtime_error(reason) {} }; namespace internal { -std::tuple parseMessageToParts( - const zmq::message_t& msg); +std::tuple parseMessageToParts(const zmq::message_t& msg); template -variant_t recursiveParse( - const std::string& typeName, const std::string& serializedData) +variant_t recursiveParse(const std::string& typeName, const std::string& serializedData) { if constexpr (IDX < std::variant_size_v) { @@ -81,9 +76,7 @@ variant_t recursiveParse( return recursiveParse(typeName, serializedData); } throw UnexpectedMessageException( - mrpt::format( - "Type '%s' not found in expected list of variant arguments.", - typeName.c_str()) + mrpt::format("Type '%s' not found in expected list of variant arguments.", typeName.c_str()) .c_str()); } } // namespace internal diff --git a/modules/comms/include/mvsim/Comms/zmq_monitor.h b/modules/comms/include/mvsim/Comms/zmq_monitor.h index 99d9fc62..f597d8f3 100644 --- a/modules/comms/include/mvsim/Comms/zmq_monitor.h +++ b/modules/comms/include/mvsim/Comms/zmq_monitor.h @@ -71,15 +71,13 @@ class SocketMonitor : public zmq::monitor_t } void on_event_disconnected( - [[maybe_unused]] const zmq_event_t& event_, - [[maybe_unused]] const char* addr_) override + [[maybe_unused]] const zmq_event_t& event_, [[maybe_unused]] const char* addr_) override { setConnected(false); } void on_event_connected( - [[maybe_unused]] const zmq_event_t& event_, - [[maybe_unused]] const char* addr_) override + [[maybe_unused]] const zmq_event_t& event_, [[maybe_unused]] const char* addr_) override { setConnected(true); } diff --git a/modules/comms/src/Comms/Client.cpp b/modules/comms/src/Comms/Client.cpp index d3bb0ad9..68faf390 100644 --- a/modules/comms/src/Comms/Client.cpp +++ b/modules/comms/src/Comms/Client.cpp @@ -117,8 +117,7 @@ struct Client::ZMQImpl }; Client::Client() - : mrpt::system::COutputLogger("mvsim::Client"), - zmq_(std::make_unique()) + : mrpt::system::COutputLogger("mvsim::Client"), zmq_(std::make_unique()) { } Client::Client(const std::string& nodeName) : Client() { setName(nodeName); } @@ -139,9 +138,7 @@ bool Client::connected() const void Client::connect() { using namespace std::string_literals; - ASSERTMSG_( - !zmq_->mainReqSocket || !zmq_->mainReqSocket, - "Client is already running."); + ASSERTMSG_(!zmq_->mainReqSocket || !zmq_->mainReqSocket, "Client is already running."); #if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF) @@ -155,8 +152,7 @@ void Client::connect() zmq_->mainReqSocketMonitor.monitor(zmq_->mainReqSocket.value()); zmq_->mainReqSocket->connect( - "tcp://"s + serverHostAddress_ + ":"s + - std::to_string(MVSIM_PORTNO_MAIN_REP)); + "tcp://"s + serverHostAddress_ + ":"s + std::to_string(MVSIM_PORTNO_MAIN_REP)); // Let the server know about this new node: doRegisterClient(); @@ -165,15 +161,11 @@ void Client::connect() zmq_->srvListenSocket.emplace(zmq_->context, ZMQ_REP); zmq_->srvListenSocket->bind("tcp://0.0.0.0:*"s); - if (!zmq_->srvListenSocket) - THROW_EXCEPTION("Error binding service listening socket."); + if (!zmq_->srvListenSocket) THROW_EXCEPTION("Error binding service listening socket."); - ASSERTMSG_( - !serviceInvokerThread_.joinable(), - "Client service thread is already running!"); + ASSERTMSG_(!serviceInvokerThread_.joinable(), "Client service thread is already running!"); - serviceInvokerThread_ = - std::thread(&Client::internalServiceServingThread, this); + serviceInvokerThread_ = std::thread(&Client::internalServiceServingThread, this); #if MRPT_VERSION >= 0x204 mrpt::system::thread_name("services_"s + nodeName_, serviceInvokerThread_); #endif @@ -185,18 +177,13 @@ void Client::connect() if (!zmq_->topicNotificationsSocket) THROW_EXCEPTION("Error binding topic updates listening socket."); - zmq_->topicNotificationsEndPoint = - get_zmq_endpoint(*zmq_->topicNotificationsSocket); + zmq_->topicNotificationsEndPoint = get_zmq_endpoint(*zmq_->topicNotificationsSocket); - ASSERTMSG_( - !topicUpdatesThread_.joinable(), - "Client topic updates thread is already running!"); + ASSERTMSG_(!topicUpdatesThread_.joinable(), "Client topic updates thread is already running!"); - topicUpdatesThread_ = - std::thread(&Client::internalTopicUpdatesThread, this); + topicUpdatesThread_ = std::thread(&Client::internalTopicUpdatesThread, this); #if MRPT_VERSION >= 0x204 - mrpt::system::thread_name( - "topicUpdates_"s + nodeName_, topicUpdatesThread_); + mrpt::system::thread_name("topicUpdates_"s + nodeName_, topicUpdatesThread_); #endif #else @@ -221,8 +208,7 @@ void Client::shutdown() noexcept } catch (const std::exception& e) { - MRPT_LOG_ERROR_STREAM( - "shutdown: Exception: " << mrpt::exception_to_str(e)); + MRPT_LOG_ERROR_STREAM("shutdown: Exception: " << mrpt::exception_to_str(e)); } #if CPPZMQ_VERSIONZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 4, 0) @@ -260,8 +246,7 @@ void Client::doRegisterClient() if (!rna.success()) { THROW_EXCEPTION_FMT( - "Server did not allow registering node: %s", - rna.errormessage().c_str()); + "Server did not allow registering node: %s", rna.errormessage().c_str()); } MRPT_LOG_DEBUG("Successfully registered in the server."); #else @@ -290,8 +275,7 @@ void Client::doUnregisterClient() if (!rna.success()) { THROW_EXCEPTION_FMT( - "Server answered an error unregistering node: %s", - rna.errormessage().c_str()); + "Server answered an error unregistering node: %s", rna.errormessage().c_str()); } MRPT_LOG_DEBUG("Successfully unregistered in the server."); #else @@ -373,8 +357,7 @@ std::vector Client::requestListOfTopics() } void Client::doAdvertiseTopic( - const std::string& topicName, - const google::protobuf::Descriptor* descriptor) + const std::string& topicName, const google::protobuf::Descriptor* descriptor) { #if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF) mrpt::system::CTimeLoggerEntry tle(profiler_, "doAdvertiseTopic"); @@ -391,8 +374,7 @@ void Client::doAdvertiseTopic( // the ctor of InfoPerAdvertisedTopic automatically creates a ZMQ_PUB // socket in pubSocket internal::InfoPerAdvertisedTopic& ipat = - advTopics.emplace_hint(advTopics.begin(), topicName, zmq_->context) - ->second; + advTopics.emplace_hint(advTopics.begin(), topicName, zmq_->context)->second; lck.unlock(); @@ -457,8 +439,7 @@ void Client::doAdvertiseService( if (services.find(serviceName) != services.end()) THROW_EXCEPTION_FMT( - "Service `%s` already registered in this same client!", - serviceName.c_str()); + "Service `%s` already registered in this same client!", serviceName.c_str()); internal::InfoPerService& ips = services[serviceName]; @@ -473,9 +454,8 @@ void Client::doAdvertiseService( ips.descOutput = descOut; MRPT_LOG_DEBUG_FMT( - "Advertising service `%s` [%s->%s] on endpoint `%s`", - serviceName.c_str(), descIn->full_name().c_str(), - descOut->full_name().c_str(), assignedPort.c_str()); + "Advertising service `%s` [%s->%s] on endpoint `%s`", serviceName.c_str(), + descIn->full_name().c_str(), descOut->full_name().c_str(), assignedPort.c_str()); mvsim_msgs::AdvertiseServiceRequest req; req.set_servicename(ips.serviceName); @@ -495,22 +475,20 @@ void Client::doAdvertiseService( if (!ans.success()) THROW_EXCEPTION_FMT( - "Error registering service `%s` in server: `%s`", - serviceName.c_str(), ans.errormessage().c_str()); + "Error registering service `%s` in server: `%s`", serviceName.c_str(), + ans.errormessage().c_str()); #else THROW_EXCEPTION("MVSIM built without ZMQ & PROTOBUF"); #endif } -void Client::publishTopic( - const std::string& topicName, const google::protobuf::Message& msg) +void Client::publishTopic(const std::string& topicName, const google::protobuf::Message& msg) { MRPT_START #if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF) ASSERTMSG_( - zmq_ && zmq_->mainReqSocket && zmq_->mainReqSocket, - "Client not connected to Server"); + zmq_ && zmq_->mainReqSocket && zmq_->mainReqSocket, "Client not connected to Server"); mrpt::system::CTimeLoggerEntry tle(profiler_, "publishTopic"); std::shared_lock lck(zmq_->advertisedTopics_mtx); @@ -556,8 +534,7 @@ void Client::internalServiceServingThread() #if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF) try { - MRPT_LOG_INFO_STREAM( - "[" << nodeName_ << "] Client service thread started."); + MRPT_LOG_INFO_STREAM("[" << nodeName_ << "] Client service thread started."); zmq::socket_t& s = *zmq_->srvListenSocket; @@ -579,8 +556,8 @@ void Client::internalServiceServingThread() // Error: unknown service: mvsim_msgs::GenericAnswer ans; ans.set_success(false); - ans.set_errormessage(mrpt::format( - "Requested unknown service `%s`", srvName.c_str())); + ans.set_errormessage( + mrpt::format("Requested unknown service `%s`", srvName.c_str())); MRPT_LOG_ERROR_STREAM(ans.errormessage()); mvsim::sendMessage(ans, s); @@ -609,15 +586,13 @@ void Client::internalServiceServingThread() } else { - MRPT_LOG_ERROR_STREAM( - "internalServiceServingThread: ZMQ error: " << e.what()); + MRPT_LOG_ERROR_STREAM("internalServiceServingThread: ZMQ error: " << e.what()); } } catch (const std::exception& e) { MRPT_LOG_ERROR_STREAM( - "internalServiceServingThread: Exception: " - << mrpt::exception_to_str(e)); + "internalServiceServingThread: Exception: " << mrpt::exception_to_str(e)); } MRPT_LOG_DEBUG_STREAM("internalServiceServingThread quitted."); @@ -631,8 +606,7 @@ void Client::internalTopicUpdatesThread() #if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF) try { - MRPT_LOG_DEBUG_STREAM( - "[" << nodeName_ << "] Client topic updates thread started."); + MRPT_LOG_DEBUG_STREAM("[" << nodeName_ << "] Client topic updates thread started."); zmq::socket_t& s = *zmq_->topicNotificationsSocket; @@ -664,16 +638,13 @@ void Client::internalTopicUpdatesThread() { // This shouldn't happen (?). MRPT_LOG_WARN_STREAM( - "Received a topic `" - << topicName - << "` update message from server, but this node is not " - "subscribed to it (!)."); + "Received a topic `" << topicName + << "` update message from server, but this node is not " + "subscribed to it (!)."); continue; } - MRPT_LOG_DEBUG_STREAM( - "[internalTopicUpdatesThread] Received: " - << tiMsg.DebugString()); + MRPT_LOG_DEBUG_STREAM("[internalTopicUpdatesThread] Received: " << tiMsg.DebugString()); internal::InfoPerSubscribedTopic& ipt = itTopic->second; @@ -697,18 +668,15 @@ void Client::internalTopicUpdatesThread() } else { - MRPT_LOG_ERROR_STREAM( - "internalTopicUpdatesThread: ZMQ error: " << e.what()); + MRPT_LOG_ERROR_STREAM("internalTopicUpdatesThread: ZMQ error: " << e.what()); } } catch (const std::exception& e) { MRPT_LOG_ERROR_STREAM( - "internalTopicUpdatesThread: Exception: " - << mrpt::exception_to_str(e)); + "internalTopicUpdatesThread: Exception: " << mrpt::exception_to_str(e)); } - MRPT_LOG_DEBUG_STREAM( - "[" << nodeName_ << "] Client topic updates thread quitted."); + MRPT_LOG_DEBUG_STREAM("[" << nodeName_ << "] Client topic updates thread quitted."); #endif } @@ -722,8 +690,7 @@ std::string Client::callService( mrpt::system::CTimeLoggerEntry tle(profiler_, "callService"); std::string outMsgData, outMsgType; - doCallService( - serviceName, inputSerializedMsg, std::nullopt, outMsgData, outMsgType); + doCallService(serviceName, inputSerializedMsg, std::nullopt, outMsgData, outMsgType); return outMsgData; #endif MRPT_END @@ -732,8 +699,7 @@ std::string Client::callService( void Client::subscribeTopic( const std::string& topicName, const std::function& /*serializedMsg*/)>& callback) + const std::string& /*msgType*/, const std::vector& /*serializedMsg*/)>& callback) { MRPT_START #if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF) @@ -765,8 +731,7 @@ void Client::doCallService( std::string srvEndpoint; auto lckCache = mrpt::lockHelper(serviceToEndPointCacheMtx_); - if (auto it = serviceToEndPointCache_.find(serviceName); - it != serviceToEndPointCache_.end()) + if (auto it = serviceToEndPointCache_.find(serviceName); it != serviceToEndPointCache_.end()) { srvEndpoint = it->second; } @@ -787,8 +752,8 @@ void Client::doCallService( if (!gsia.success()) THROW_EXCEPTION_FMT( - "Error requesting information about service `%s`: %s", - serviceName.c_str(), gsia.errormessage().c_str()); + "Error requesting information about service `%s`: %s", serviceName.c_str(), + gsia.errormessage().c_str()); srvEndpoint = gsia.serviceendpoint(); @@ -813,8 +778,7 @@ void Client::doCallService( } if (outputSerializedMsg) { - const auto [typeName, serializedData] = - internal::parseMessageToParts(m); + const auto [typeName, serializedData] = internal::parseMessageToParts(m); outputSerializedMsg.value().get() = serializedData; if (outputMsgTypeName) outputMsgTypeName.value().get() = typeName; @@ -823,15 +787,13 @@ void Client::doCallService( MRPT_END } -void Client::subscribe_topic_raw( - const std::string& topicName, const topic_callback_t& callback) +void Client::subscribe_topic_raw(const std::string& topicName, const topic_callback_t& callback) { doSubscribeTopic(topicName, nullptr, callback); } void Client::doSubscribeTopic( - const std::string& topicName, - [[maybe_unused]] const google::protobuf::Descriptor* descriptor, + const std::string& topicName, [[maybe_unused]] const google::protobuf::Descriptor* descriptor, const topic_callback_t& callback) { MRPT_START @@ -859,8 +821,7 @@ void Client::doSubscribeTopic( lck.unlock(); - ipt.topicThread = - std::thread([&]() { this->internalTopicSubscribeThread(ipt); }); + ipt.topicThread = std::thread([&]() { this->internalTopicSubscribeThread(ipt); }); // Let the server know about our interest in the topic: mvsim_msgs::SubscribeRequest subReq; @@ -893,8 +854,8 @@ void Client::internalTopicSubscribeThread(internal::InfoPerSubscribedTopic& ipt) try { MRPT_LOG_DEBUG_STREAM( - "[" << nodeName_ << "] Client topic subscribe thread for `" - << ipt.topicName << "` started."); + "[" << nodeName_ << "] Client topic subscribe thread for `" << ipt.topicName + << "` started."); zmq::socket_t& s = ipt.subSocket; @@ -912,8 +873,7 @@ void Client::internalTopicSubscribeThread(internal::InfoPerSubscribedTopic& ipt) { MRPT_LOG_ERROR_STREAM( "Exception in topic `" - << ipt.topicName - << "` subscription callback:" << mrpt::exception_to_str(e)); + << ipt.topicName << "` subscription callback:" << mrpt::exception_to_str(e)); } } } @@ -930,18 +890,15 @@ void Client::internalTopicSubscribeThread(internal::InfoPerSubscribedTopic& ipt) } else { - MRPT_LOG_ERROR_STREAM( - "internalTopicSubscribeThread: ZMQ error: " << e.what()); + MRPT_LOG_ERROR_STREAM("internalTopicSubscribeThread: ZMQ error: " << e.what()); } } catch (const std::exception& e) { MRPT_LOG_ERROR_STREAM( - "internalTopicSubscribeThread: Exception: " - << mrpt::exception_to_str(e)); + "internalTopicSubscribeThread: Exception: " << mrpt::exception_to_str(e)); } - MRPT_LOG_DEBUG_STREAM( - "[" << nodeName_ << "] Client topic subscribe thread quitted."); + MRPT_LOG_DEBUG_STREAM("[" << nodeName_ << "] Client topic subscribe thread quitted."); #endif } diff --git a/modules/comms/src/Comms/Server.cpp b/modules/comms/src/Comms/Server.cpp index d982a155..771cc8d6 100644 --- a/modules/comms/src/Comms/Server.cpp +++ b/modules/comms/src/Comms/Server.cpp @@ -51,8 +51,7 @@ void Server::start() #endif #else - THROW_EXCEPTION( - "MVSIM needs building with ZMQ and PROTOBUF to enable client/server"); + THROW_EXCEPTION("MVSIM needs building with ZMQ and PROTOBUF to enable client/server"); #endif } @@ -69,8 +68,7 @@ void Server::shutdown() noexcept } catch (const std::exception& e) { - MRPT_LOG_ERROR_STREAM( - "shutdown: Exception: " << mrpt::exception_to_str(e)); + MRPT_LOG_ERROR_STREAM("shutdown: Exception: " << mrpt::exception_to_str(e)); } } @@ -103,18 +101,15 @@ void Server::internalServerThread() // Variant with all valid client requests: using client_requests_t = std::variant< - mvsim_msgs::RegisterNodeRequest, - mvsim_msgs::UnregisterNodeRequest, mvsim_msgs::SubscribeRequest, - mvsim_msgs::ListNodesRequest, mvsim_msgs::ListTopicsRequest, - mvsim_msgs::AdvertiseTopicRequest, - mvsim_msgs::AdvertiseServiceRequest, - mvsim_msgs::GetServiceInfoRequest>; + mvsim_msgs::RegisterNodeRequest, mvsim_msgs::UnregisterNodeRequest, + mvsim_msgs::SubscribeRequest, mvsim_msgs::ListNodesRequest, + mvsim_msgs::ListTopicsRequest, mvsim_msgs::AdvertiseTopicRequest, + mvsim_msgs::AdvertiseServiceRequest, mvsim_msgs::GetServiceInfoRequest>; // Parse and dispatch: try { - client_requests_t req = - mvsim::parseMessageVariant(request); + client_requests_t req = mvsim::parseMessageVariant(request); std::visit( overloaded{ @@ -134,19 +129,16 @@ void Server::internalServerThread() { // This simply means someone called requestMainThreadTermination(). // Just exit silently. - MRPT_LOG_DEBUG_STREAM( - "Server thread about to exit for ZMQ term signal."); + MRPT_LOG_DEBUG_STREAM("Server thread about to exit for ZMQ term signal."); } else { - MRPT_LOG_ERROR_STREAM( - "internalServerThread: ZMQ error: " << e.what()); + MRPT_LOG_ERROR_STREAM("internalServerThread: ZMQ error: " << e.what()); } } catch (const std::exception& e) { - MRPT_LOG_ERROR_STREAM( - "internalServerThread: Exception: " << mrpt::exception_to_str(e)); + MRPT_LOG_ERROR_STREAM("internalServerThread: Exception: " << mrpt::exception_to_str(e)); } MRPT_LOG_DEBUG_STREAM("Server thread quitted."); @@ -205,20 +197,17 @@ void Server::db_advertise_topic( // 1) Add as a source of this topic: auto& dbTopic = knownTopics_[topicName]; - if (!dbTopic.topicTypeName.empty() && - dbTopic.topicTypeName != topicTypeName) + if (!dbTopic.topicTypeName.empty() && dbTopic.topicTypeName != topicTypeName) { throw std::runtime_error(mrpt::format( "Trying to register topic `%s` [%s] but already known with type " "[%s]", - topicName.c_str(), topicTypeName.c_str(), - dbTopic.topicTypeName.c_str())); + topicName.c_str(), topicTypeName.c_str(), dbTopic.topicTypeName.c_str())); } dbTopic.topicName = topicName; dbTopic.topicTypeName = topicTypeName; - dbTopic.publishers.try_emplace( - nodeName, topicName, nodeName, publisherEndpoint); + dbTopic.publishers.try_emplace(nodeName, topicName, nodeName, publisherEndpoint); // 2) If clients are already waiting for this topic, inform them so they // can subscribe to this new source of data: @@ -232,17 +221,14 @@ void Server::db_add_topic_subscriber( auto& dbTopic = knownTopics_[topicName]; - dbTopic.subscribers.try_emplace( - updatesEndPoint, topicName, updatesEndPoint); + dbTopic.subscribers.try_emplace(updatesEndPoint, topicName, updatesEndPoint); // Send all currently-existing publishers: - send_topic_publishers_to_subscribed_clients( - topicName, updatesEndPoint /*only this one*/); + send_topic_publishers_to_subscribed_clients(topicName, updatesEndPoint /*only this one*/); } void Server::send_topic_publishers_to_subscribed_clients( - const std::string& topicName, - const std::optional& updatesEndPoint) + const std::string& topicName, const std::optional& updatesEndPoint) { #if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF) auto& dbTopic = knownTopics_[topicName]; @@ -265,9 +251,8 @@ void Server::send_topic_publishers_to_subscribed_clients( { MRPT_LOG_DEBUG_STREAM( "[send_topic_publishers_to_subscribed_clients] Letting " - << subUpdtEndPoint << " know about " - << dbTopic.publishers.size() << " publishers for topic '" - << topicName << "'"); + << subUpdtEndPoint << " know about " << dbTopic.publishers.size() + << " publishers for topic '" << topicName << "'"); zmq::socket_t s(*mainThreadZMQcontext_, ZMQ_PAIR); s.connect(subUpdtEndPoint); @@ -286,8 +271,7 @@ void Server::send_topic_publishers_to_subscribed_clients( catch (const std::exception& e) { MRPT_LOG_ERROR_STREAM( - "Error sending topic updates to endpoint " << subUpdtEndPoint - << ":\n" + "Error sending topic updates to endpoint " << subUpdtEndPoint << ":\n" << e.what()); } }; @@ -319,8 +303,7 @@ void Server::db_advertise_service( auto& dbSrv = knownServices_[serviceName]; if (!dbSrv.inputTypeName.empty() && - (dbSrv.inputTypeName != inputTypeName || - dbSrv.outputTypeName != outputTypeName)) + (dbSrv.inputTypeName != inputTypeName || dbSrv.outputTypeName != outputTypeName)) { throw std::runtime_error(mrpt::format( "Trying to register service `%s` [%s->%s] but already known " @@ -338,8 +321,7 @@ void Server::db_advertise_service( } bool Server::db_get_service_info( - const std::string& serviceName, std::string& publisherEndpoint, - std::string& nodeName) const + const std::string& serviceName, std::string& publisherEndpoint, std::string& nodeName) const { std::shared_lock lck(dbMutex); @@ -361,8 +343,7 @@ bool Server::db_get_service_info( void Server::handle(const mvsim_msgs::RegisterNodeRequest& m, zmq::socket_t& s) { // Send reply back to client - MRPT_LOG_DEBUG_STREAM( - "Registering new node named '" << m.nodename() << "'"); + MRPT_LOG_DEBUG_STREAM("Registering new node named '" << m.nodename() << "'"); // Make sure we don't have already a node named like this: // Don't raise an error if the name was already registered, since it @@ -377,8 +358,7 @@ void Server::handle(const mvsim_msgs::RegisterNodeRequest& m, zmq::socket_t& s) } // mvsim_msgs::UnregisterNodeRequest -void Server::handle( - const mvsim_msgs::UnregisterNodeRequest& m, zmq::socket_t& s) +void Server::handle(const mvsim_msgs::UnregisterNodeRequest& m, zmq::socket_t& s) { // Send reply back to client MRPT_LOG_DEBUG_STREAM("Unregistering node named '" << m.nodename() << "'"); @@ -394,8 +374,7 @@ void Server::handle( void Server::handle(const mvsim_msgs::SubscribeRequest& m, zmq::socket_t& s) { // Send reply back to client - MRPT_LOG_DEBUG_STREAM( - "Subscription request for topic " << m.topic() << "'"); + MRPT_LOG_DEBUG_STREAM("Subscription request for topic " << m.topic() << "'"); // Include in our DB of subscriptions: // This also sends the subcriber the list of existing endpoints it must @@ -409,12 +388,10 @@ void Server::handle(const mvsim_msgs::SubscribeRequest& m, zmq::socket_t& s) } // mvsim_msgs::GetServiceInfoRequest -void Server::handle( - const mvsim_msgs::GetServiceInfoRequest& m, zmq::socket_t& s) +void Server::handle(const mvsim_msgs::GetServiceInfoRequest& m, zmq::socket_t& s) { // Send reply back to client - MRPT_LOG_DEBUG_STREAM( - "GetServiceInfo request for service '" << m.servicename() << "'"); + MRPT_LOG_DEBUG_STREAM("GetServiceInfo request for service '" << m.servicename() << "'"); mvsim_msgs::GetServiceInfoAnswer ans; std::string node, endpoint; @@ -428,8 +405,7 @@ void Server::handle( else { ans.set_success(false); - ans.set_errormessage(mrpt::format( - "Could not find service `%s`", m.servicename().c_str())); + ans.set_errormessage(mrpt::format("Could not find service `%s`", m.servicename().c_str())); } mvsim::sendMessage(ans, s); @@ -453,8 +429,7 @@ void Server::handle(const mvsim_msgs::ListTopicsRequest& m, zmq::socket_t& s) const auto& t = kv.second; const auto& name = t.topicName; - if (!queryPrefix.empty() || - name.substr(0, queryPrefix.size()) == queryPrefix) + if (!queryPrefix.empty() || name.substr(0, queryPrefix.size()) == queryPrefix) { auto tInfo = ans.add_topics(); tInfo->set_topicname(name); @@ -484,8 +459,7 @@ void Server::handle(const mvsim_msgs::ListNodesRequest& m, zmq::socket_t& s) { const auto& name = n.second.nodeName; - if (!queryPrefix.empty() || - name.substr(0, queryPrefix.size()) == queryPrefix) + if (!queryPrefix.empty() || name.substr(0, queryPrefix.size()) == queryPrefix) { ans.add_nodes(name); } @@ -494,20 +468,17 @@ void Server::handle(const mvsim_msgs::ListNodesRequest& m, zmq::socket_t& s) } // mvsim_msgs::AdvertiseTopicRequest -void Server::handle( - const mvsim_msgs::AdvertiseTopicRequest& m, zmq::socket_t& s) +void Server::handle(const mvsim_msgs::AdvertiseTopicRequest& m, zmq::socket_t& s) { // Send reply back to client MRPT_LOG_DEBUG_FMT( - "Received new topic advertiser: `%s` [%s] @ %s (%s)", - m.topicname().c_str(), m.topictypename().c_str(), m.endpoint().c_str(), - m.nodename().c_str()); + "Received new topic advertiser: `%s` [%s] @ %s (%s)", m.topicname().c_str(), + m.topictypename().c_str(), m.endpoint().c_str(), m.nodename().c_str()); mvsim_msgs::GenericAnswer ans; try { - db_advertise_topic( - m.topicname(), m.topictypename(), m.endpoint(), m.nodename()); + db_advertise_topic(m.topicname(), m.topictypename(), m.endpoint(), m.nodename()); ans.set_success(true); } catch (const std::exception& e) @@ -519,21 +490,19 @@ void Server::handle( } // mvsim_msgs::AdvertiseServiceRequest -void Server::handle( - const mvsim_msgs::AdvertiseServiceRequest& m, zmq::socket_t& s) +void Server::handle(const mvsim_msgs::AdvertiseServiceRequest& m, zmq::socket_t& s) { // Send reply back to client MRPT_LOG_DEBUG_FMT( - "Received new service offering: `%s` [%s->%s] @ %s (%s)", - m.servicename().c_str(), m.inputtypename().c_str(), - m.outputtypename().c_str(), m.endpoint().c_str(), m.nodename().c_str()); + "Received new service offering: `%s` [%s->%s] @ %s (%s)", m.servicename().c_str(), + m.inputtypename().c_str(), m.outputtypename().c_str(), m.endpoint().c_str(), + m.nodename().c_str()); mvsim_msgs::GenericAnswer ans; try { db_advertise_service( - m.servicename(), m.inputtypename(), m.outputtypename(), - m.endpoint(), m.nodename()); + m.servicename(), m.inputtypename(), m.outputtypename(), m.endpoint(), m.nodename()); ans.set_success(true); } catch (const std::exception& e) diff --git a/modules/comms/src/Comms/common.cpp b/modules/comms/src/Comms/common.cpp index 7e9ce9a4..96c3d76a 100644 --- a/modules/comms/src/Comms/common.cpp +++ b/modules/comms/src/Comms/common.cpp @@ -17,8 +17,7 @@ using namespace mvsim; -void mvsim::sendMessage( - const google::protobuf::MessageLite& m, zmq::socket_t& socket) +void mvsim::sendMessage(const google::protobuf::MessageLite& m, zmq::socket_t& socket) { mrpt::io::CMemoryStream buf; auto arch = mrpt::serialization::archiveFrom(buf); @@ -34,8 +33,7 @@ void mvsim::sendMessage( #endif } -std::tuple mvsim::internal::parseMessageToParts( - const zmq::message_t& msg) +std::tuple mvsim::internal::parseMessageToParts(const zmq::message_t& msg) { mrpt::io::CMemoryStream buf; buf.assignMemoryNotOwn(msg.data(), msg.size()); @@ -47,8 +45,7 @@ std::tuple mvsim::internal::parseMessageToParts( return {typeName, serializedData}; } -void mvsim::parseMessage( - const zmq::message_t& msg, google::protobuf::MessageLite& out) +void mvsim::parseMessage(const zmq::message_t& msg, google::protobuf::MessageLite& out) { const auto [typeName, serializedData] = internal::parseMessageToParts(msg); diff --git a/modules/simulator/include/mvsim/Block.h b/modules/simulator/include/mvsim/Block.h index 30bbd5f9..4e84edc2 100644 --- a/modules/simulator/include/mvsim/Block.h +++ b/modules/simulator/include/mvsim/Block.h @@ -44,16 +44,14 @@ class Block : public VisualObject, public Simulable /** Register a new class of blocks from XML description of type * "...". */ - static void register_block_class( - const World& parent, const rapidxml::xml_node* xml_node); + static void register_block_class(const World& parent, const rapidxml::xml_node* xml_node); // ------- Interface with "World" ------ virtual void simul_pre_timestep(const TSimulContext& context) override; virtual void simul_post_timestep(const TSimulContext& context) override; virtual void apply_force( const mrpt::math::TVector2D& force, - const mrpt::math::TPoint2D& applyPoint = - mrpt::math::TPoint2D(0, 0)) override; + const mrpt::math::TPoint2D& applyPoint = mrpt::math::TPoint2D(0, 0)) override; /** Create bodies, fixtures, etc. for the dynamical simulation. May be * overrided by derived classes */ @@ -126,8 +124,7 @@ class Block : public VisualObject, public Simulable protected: virtual void internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) override; + const mrpt::optional_ref& physical, bool childrenOnly) override; /** user-supplied index number: must be set/get'ed with setblockIndex() * getblockIndex() (default=0) */ @@ -227,10 +224,7 @@ class DummyInvisibleBlock : public VisualObject, public Simulable /** Get the block mass */ virtual double getMass() const { return 0; } - void add_sensor(const SensorBase::Ptr& sensor) - { - sensors_.push_back(sensor); - } + void add_sensor(const SensorBase::Ptr& sensor) { sensors_.push_back(sensor); } protected: void internalGuiUpdate( diff --git a/modules/simulator/include/mvsim/ClassFactory.h b/modules/simulator/include/mvsim/ClassFactory.h index 05447376..1abc393a 100644 --- a/modules/simulator/include/mvsim/ClassFactory.h +++ b/modules/simulator/include/mvsim/ClassFactory.h @@ -42,13 +42,10 @@ class ClassFactory auto it = classes_.find(class_name); if (it == classes_.end()) throw std::runtime_error( - (std::string("ClassFactory: Unknown class ") + class_name) - .c_str()); + (std::string("ClassFactory: Unknown class ") + class_name).c_str()); if (!it->second.ptr_factory1) throw std::runtime_error( - (std::string( - "ClassFactory: factory(1) pointer is nullptr for ") + - class_name) + (std::string("ClassFactory: factory(1) pointer is nullptr for ") + class_name) .c_str()); return Ptr((*it->second.ptr_factory1)(a1)); } @@ -57,13 +54,10 @@ class ClassFactory auto it = classes_.find(class_name); if (it == classes_.end()) throw std::runtime_error( - (std::string("ClassFactory: Unknown class ") + class_name) - .c_str()); + (std::string("ClassFactory: Unknown class ") + class_name).c_str()); if (!it->second.ptr_factory2) throw std::runtime_error( - (std::string( - "ClassFactory: factory(2) pointer is nullptr for ") + - class_name) + (std::string("ClassFactory: factory(2) pointer is nullptr for ") + class_name) .c_str()); return Ptr((*it->second.ptr_factory2)(a1, a2)); } @@ -77,10 +71,7 @@ class ClassFactory static BASE_CLASS* Create(ARG1 a1) { return new CLASS_NAME(a1); } #define DECLARES_REGISTER_CLASS2(CLASS_NAME, BASE_CLASS, ARG1, ARG2) \ public: \ - static BASE_CLASS* Create(ARG1 a1, ARG2 a2) \ - { \ - return new CLASS_NAME(a1, a2); \ - } + static BASE_CLASS* Create(ARG1 a1, ARG2 a2) { return new CLASS_NAME(a1, a2); } #define REGISTER_CLASS1(FACTORY_TYPE, FACTORY_OBJ, TEXTUAL_NAME, CLASS_NAME) \ { \ diff --git a/modules/simulator/include/mvsim/CollisionShapeCache.h b/modules/simulator/include/mvsim/CollisionShapeCache.h index 67c62d8e..ed5b2fa1 100644 --- a/modules/simulator/include/mvsim/CollisionShapeCache.h +++ b/modules/simulator/include/mvsim/CollisionShapeCache.h @@ -52,9 +52,8 @@ class CollisionShapeCache const mrpt::poses::CPose3D& modelPose, const float modelScale); Shape2p5 processCylinderLike( - const size_t actualEdgeCount, double actualRadius, float zMin, - float zMax, const mrpt::poses::CPose3D& modelPose, - const float modelScale); + const size_t actualEdgeCount, double actualRadius, float zMin, float zMax, + const mrpt::poses::CPose3D& modelPose, const float modelScale); }; } // namespace mvsim diff --git a/modules/simulator/include/mvsim/ControllerBase.h b/modules/simulator/include/mvsim/ControllerBase.h index 2fb92590..c45ffb8a 100644 --- a/modules/simulator/include/mvsim/ControllerBase.h +++ b/modules/simulator/include/mvsim/ControllerBase.h @@ -35,8 +35,7 @@ class ControllerBaseInterface }; virtual void teleop_interface( - [[maybe_unused]] const TeleopInput& in, - [[maybe_unused]] TeleopOutput& out) + [[maybe_unused]] const TeleopInput& in, [[maybe_unused]] TeleopOutput& out) { /*default: do nothing*/ } @@ -64,8 +63,7 @@ class ControllerBaseTempl : public ControllerBaseInterface ControllerBaseTempl(VEH_DYNAMICS& veh) : veh_(veh) {} virtual ~ControllerBaseTempl() {} /** This is to handle basic need of all the controllers.*/ - virtual void teleop_interface( - const TeleopInput& in, TeleopOutput& out) override + virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out) override { /*default: handle logging events*/ static bool isRecording = false; @@ -96,12 +94,11 @@ class ControllerBaseTempl : public ControllerBaseInterface break; } - out.append_gui_lines += - std::string( - "Toggle logging [L]. Clear logs[C]. New log session [N]. " - "Now ") + - std::string(isRecording ? "logging" : "not logging") + - std::string("\n"); + out.append_gui_lines += std::string( + "Toggle logging [L]. Clear logs[C]. New log session [N]. " + "Now ") + + std::string(isRecording ? "logging" : "not logging") + + std::string("\n"); } /** The core of the controller: will be called at each timestep before the @@ -118,10 +115,7 @@ class ControllerBaseTempl : public ControllerBaseInterface { /*default: do nothing*/ } - virtual void setLogRecording(bool recording) - { - veh_.setRecording(recording); - } + virtual void setLogRecording(bool recording) { veh_.setRecording(recording); } virtual void clearLogs() { veh_.clearLogs(); } virtual void newLogSession() { veh_.newLogSession(); } diff --git a/modules/simulator/include/mvsim/FrictionModels/DefaultFriction.h b/modules/simulator/include/mvsim/FrictionModels/DefaultFriction.h index f82b63b0..47e45383 100644 --- a/modules/simulator/include/mvsim/FrictionModels/DefaultFriction.h +++ b/modules/simulator/include/mvsim/FrictionModels/DefaultFriction.h @@ -24,8 +24,7 @@ class DefaultFriction : public FrictionBase { DECLARES_REGISTER_FRICTION(DefaultFriction) public: - DefaultFriction( - VehicleBase& my_vehicle, const rapidxml::xml_node* node); + DefaultFriction(VehicleBase& my_vehicle, const rapidxml::xml_node* node); // See docs in base class. virtual mrpt::math::TVector2D evaluate_friction( diff --git a/modules/simulator/include/mvsim/FrictionModels/FrictionBase.h b/modules/simulator/include/mvsim/FrictionModels/FrictionBase.h index 1a0ed814..62a1c3d4 100644 --- a/modules/simulator/include/mvsim/FrictionModels/FrictionBase.h +++ b/modules/simulator/include/mvsim/FrictionModels/FrictionBase.h @@ -27,8 +27,7 @@ class FrictionBase /** Class factory: Creates a friction object from XML description of type * "...". */ - static FrictionBase::Ptr factory( - VehicleBase& parent, const rapidxml::xml_node* xml_node); + static FrictionBase::Ptr factory(VehicleBase& parent, const rapidxml::xml_node* xml_node); struct TFrictionInput { @@ -71,18 +70,14 @@ class FrictionBase using FrictionBasePtr = std::shared_ptr; // Class factory: -typedef ClassFactory< - FrictionBase, VehicleBase&, const rapidxml::xml_node*> +typedef ClassFactory*> TClassFactory_friction; extern TClassFactory_friction classFactory_friction; -#define DECLARES_REGISTER_FRICTION(CLASS_NAME) \ - DECLARES_REGISTER_CLASS2( \ - CLASS_NAME, FrictionBase, VehicleBase&, \ - const rapidxml::xml_node*) +#define DECLARES_REGISTER_FRICTION(CLASS_NAME) \ + DECLARES_REGISTER_CLASS2( \ + CLASS_NAME, FrictionBase, VehicleBase&, const rapidxml::xml_node*) -#define REGISTER_FRICTION(TEXTUAL_NAME, CLASS_NAME) \ - REGISTER_CLASS2( \ - TClassFactory_friction, classFactory_friction, TEXTUAL_NAME, \ - CLASS_NAME) +#define REGISTER_FRICTION(TEXTUAL_NAME, CLASS_NAME) \ + REGISTER_CLASS2(TClassFactory_friction, classFactory_friction, TEXTUAL_NAME, CLASS_NAME) } // namespace mvsim diff --git a/modules/simulator/include/mvsim/FrictionModels/WardIagnemmaFriction.h b/modules/simulator/include/mvsim/FrictionModels/WardIagnemmaFriction.h index 7ec59187..4a2eabf4 100644 --- a/modules/simulator/include/mvsim/FrictionModels/WardIagnemmaFriction.h +++ b/modules/simulator/include/mvsim/FrictionModels/WardIagnemmaFriction.h @@ -26,8 +26,7 @@ class WardIagnemmaFriction : public FrictionBase { DECLARES_REGISTER_FRICTION(WardIagnemmaFriction) public: - WardIagnemmaFriction( - VehicleBase& my_vehicle, const rapidxml::xml_node* node); + WardIagnemmaFriction(VehicleBase& my_vehicle, const rapidxml::xml_node* node); // See docs in base class. virtual mrpt::math::TVector2D evaluate_friction( diff --git a/modules/simulator/include/mvsim/Joystick.h b/modules/simulator/include/mvsim/Joystick.h index deeb33da..ffa674c6 100644 --- a/modules/simulator/include/mvsim/Joystick.h +++ b/modules/simulator/include/mvsim/Joystick.h @@ -113,8 +113,7 @@ class Joystick * * \sa getJoystickPosition */ - void setLimits( - const std::vector& minPerAxis, const std::vector& maxPerAxis); + void setLimits(const std::vector& minPerAxis, const std::vector& maxPerAxis); }; // End of class def. diff --git a/modules/simulator/include/mvsim/RemoteResourcesManager.h b/modules/simulator/include/mvsim/RemoteResourcesManager.h index 23c4253f..d247a12c 100644 --- a/modules/simulator/include/mvsim/RemoteResourcesManager.h +++ b/modules/simulator/include/mvsim/RemoteResourcesManager.h @@ -50,8 +50,7 @@ class RemoteResourcesManager : public mrpt::system::COutputLogger /** Returns {true, zip_uri, internal_uri} if the URI refers to a ZIP file, * or {false, uri, ""} otherwise. */ - static std::tuple zip_uri_split( - const std::string& uri); + static std::tuple zip_uri_split(const std::string& uri); static std::string cache_directory(); diff --git a/modules/simulator/include/mvsim/Sensors/CameraSensor.h b/modules/simulator/include/mvsim/Sensors/CameraSensor.h index 71e90e61..5503f31c 100644 --- a/modules/simulator/include/mvsim/Sensors/CameraSensor.h +++ b/modules/simulator/include/mvsim/Sensors/CameraSensor.h @@ -40,15 +40,11 @@ class CameraSensor : public SensorBase protected: virtual void internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) override; + const mrpt::optional_ref& physical, bool childrenOnly) override; void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override; - mrpt::math::TPose3D getRelativePose() const override - { - return sensor_params_.sensorPose(); - } + mrpt::math::TPose3D getRelativePose() const override { return sensor_params_.sensorPose(); } void setRelativePose(const mrpt::math::TPose3D& p) override { sensor_params_.setSensorPose(mrpt::poses::CPose3D(p)); @@ -74,8 +70,7 @@ class CameraSensor : public SensorBase float rgbClipMin_ = 1e-2, rgbClipMax_ = 1e+4; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, - gl_sensor_origin_corner_; + mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; mrpt::opengl::CSetOfObjects::Ptr gl_sensor_fov_, gl_sensor_frustum_; }; } // namespace mvsim diff --git a/modules/simulator/include/mvsim/Sensors/DepthCameraSensor.h b/modules/simulator/include/mvsim/Sensors/DepthCameraSensor.h index 82309eaf..8bfc9e29 100644 --- a/modules/simulator/include/mvsim/Sensors/DepthCameraSensor.h +++ b/modules/simulator/include/mvsim/Sensors/DepthCameraSensor.h @@ -51,8 +51,7 @@ class DepthCameraSensor : public SensorBase protected: virtual void internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) override; + const mrpt::optional_ref& physical, bool childrenOnly) override; void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override; @@ -75,8 +74,7 @@ class DepthCameraSensor : public SensorBase mrpt::obs::CObservation3DRangeScan::Ptr last_obs2gui_; // Note: we need 2 to support different resolutions for RGB vs Depth. - std::shared_ptr fbo_renderer_rgb_, - fbo_renderer_depth_; + std::shared_ptr fbo_renderer_rgb_, fbo_renderer_depth_; /** Whether gl_scan_ has to be updated upon next call of * internalGuiUpdate() from last_scan2gui_ */ @@ -96,8 +94,7 @@ class DepthCameraSensor : public SensorBase float depth_noise_sigma_ = 1e-3; bool show_3d_pointcloud_ = false; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, - gl_sensor_origin_corner_; + mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; mrpt::opengl::CSetOfObjects::Ptr gl_sensor_fov_, gl_sensor_frustum_; mrpt::math::CMatrixFloat depthImage_; // to avoid memory allocs diff --git a/modules/simulator/include/mvsim/Sensors/IMU.h b/modules/simulator/include/mvsim/Sensors/IMU.h index c365aeb4..b8289b1f 100644 --- a/modules/simulator/include/mvsim/Sensors/IMU.h +++ b/modules/simulator/include/mvsim/Sensors/IMU.h @@ -40,8 +40,7 @@ class IMU : public SensorBase protected: void internalGuiUpdate( const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& - physical, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) override; void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override; @@ -65,8 +64,7 @@ class IMU : public SensorBase /** Last simulated obs */ mrpt::obs::CObservationIMU::Ptr last_obs_; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, - gl_sensor_origin_corner_; + mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; mrpt::random::CRandomGenerator rng_; }; diff --git a/modules/simulator/include/mvsim/Sensors/LaserScanner.h b/modules/simulator/include/mvsim/Sensors/LaserScanner.h index 74551810..089cfb0e 100644 --- a/modules/simulator/include/mvsim/Sensors/LaserScanner.h +++ b/modules/simulator/include/mvsim/Sensors/LaserScanner.h @@ -49,8 +49,7 @@ class LaserScanner : public SensorBase protected: virtual void internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) override; + const mrpt::optional_ref& physical, bool childrenOnly) override; void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override; @@ -101,8 +100,7 @@ class LaserScanner : public SensorBase bool gui_uptodate_ = false; mrpt::opengl::CPlanarLaserScan::Ptr gl_scan_; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, - gl_sensor_origin_corner_; + mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; mrpt::opengl::CSetOfObjects::Ptr gl_sensor_fov_; std::optional has_to_render_; diff --git a/modules/simulator/include/mvsim/Sensors/Lidar3D.h b/modules/simulator/include/mvsim/Sensors/Lidar3D.h index 46c1fbe6..7d517503 100644 --- a/modules/simulator/include/mvsim/Sensors/Lidar3D.h +++ b/modules/simulator/include/mvsim/Sensors/Lidar3D.h @@ -45,15 +45,11 @@ class Lidar3D : public SensorBase protected: virtual void internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) override; + const mrpt::optional_ref& physical, bool childrenOnly) override; void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override; - mrpt::math::TPose3D getRelativePose() const override - { - return sensorPoseOnVeh_.asTPose(); - } + mrpt::math::TPose3D getRelativePose() const override { return sensorPoseOnVeh_.asTPose(); } void setRelativePose(const mrpt::math::TPose3D& p) override { sensorPoseOnVeh_ = mrpt::poses::CPose3D(p); @@ -93,8 +89,7 @@ class Lidar3D : public SensorBase bool gui_uptodate_ = false; mrpt::opengl::CPointCloudColoured::Ptr glPoints_; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, - gl_sensor_origin_corner_; + mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; mrpt::opengl::CSetOfObjects::Ptr gl_sensor_fov_; std::optional has_to_render_; diff --git a/modules/simulator/include/mvsim/Sensors/SensorBase.h b/modules/simulator/include/mvsim/Sensors/SensorBase.h index 74a56031..6d4eaeba 100644 --- a/modules/simulator/include/mvsim/Sensors/SensorBase.h +++ b/modules/simulator/include/mvsim/Sensors/SensorBase.h @@ -33,8 +33,7 @@ class SensorBase : public VisualObject, public Simulable /** Class factory: Creates a sensor from XML description of type "...". */ - static SensorBase::Ptr factory( - Simulable& parent, const rapidxml::xml_node* xml_node); + static SensorBase::Ptr factory(Simulable& parent, const rapidxml::xml_node* xml_node); /** Loads the parameters common to all sensors. Should be overriden, and * they call to this base method. */ @@ -48,14 +47,11 @@ class SensorBase : public VisualObject, public Simulable } // Get all sensors visuals API: - static std::shared_ptr - GetAllSensorsOriginViz(); + static std::shared_ptr GetAllSensorsOriginViz(); static std::shared_ptr GetAllSensorsFOVViz(); - static void RegisterSensorFOVViz( - const std::shared_ptr& o); - static void RegisterSensorOriginViz( - const std::shared_ptr& o); + static void RegisterSensorFOVViz(const std::shared_ptr& o); + static void RegisterSensorOriginViz(const std::shared_ptr& o); double sensor_period() const { return sensor_period_; } @@ -92,12 +88,10 @@ class SensorBase : public VisualObject, public Simulable std::map varValues_; bool parseSensorPublish( - const rapidxml::xml_node* node, - const std::map& varValues); + const rapidxml::xml_node* node, const std::map& varValues); void reportNewObservation( - const std::shared_ptr& obs, - const TSimulContext& context); + const std::shared_ptr& obs, const TSimulContext& context); void reportNewObservation_lidar_2d( const std::shared_ptr& obs, @@ -110,16 +104,13 @@ class SensorBase : public VisualObject, public Simulable using TListSensors = std::vector; // Class factory: -using TClassFactory_sensors = - ClassFactory*>; +using TClassFactory_sensors = ClassFactory*>; extern TClassFactory_sensors classFactory_sensors; #define DECLARES_REGISTER_SENSOR(CLASS_NAME) \ - DECLARES_REGISTER_CLASS2( \ - CLASS_NAME, SensorBase, Simulable&, const rapidxml::xml_node*) + DECLARES_REGISTER_CLASS2(CLASS_NAME, SensorBase, Simulable&, const rapidxml::xml_node*) #define REGISTER_SENSOR(TEXTUAL_NAME, CLASS_NAME) \ - REGISTER_CLASS2( \ - TClassFactory_sensors, classFactory_sensors, TEXTUAL_NAME, CLASS_NAME) + REGISTER_CLASS2(TClassFactory_sensors, classFactory_sensors, TEXTUAL_NAME, CLASS_NAME) } // namespace mvsim diff --git a/modules/simulator/include/mvsim/Shape2p5.h b/modules/simulator/include/mvsim/Shape2p5.h index c2c1f3a4..6f8bb72d 100644 --- a/modules/simulator/include/mvsim/Shape2p5.h +++ b/modules/simulator/include/mvsim/Shape2p5.h @@ -33,8 +33,7 @@ class Shape2p5 Shape2p5() = default; void buildInit( - const mrpt::math::TPoint2Df& bbMin, const mrpt::math::TPoint2Df& bbMax, - int numCells = 100); + const mrpt::math::TPoint2Df& bbMin, const mrpt::math::TPoint2Df& bbMax, int numCells = 100); void buildAddPoint(const mrpt::math::TPoint3Df& pt); void buildAddTriangle(const mrpt::opengl::TTriangle& t); @@ -44,9 +43,7 @@ class Shape2p5 void mergeWith(const Shape2p5& s); - void setShapeManual( - const mrpt::math::TPolygon2D& contour, const float zMin, - const float zMax); + void setShapeManual(const mrpt::math::TPolygon2D& contour, const float zMin, const float zMax); float zMin() const { return zMin_; } float zMax() const { return zMax_; } @@ -65,8 +62,7 @@ class Shape2p5 public: template SimpleOccGrid(Args&&... args) - : mrpt::containers::CDynamicGrid( - std::forward(args)...) + : mrpt::containers::CDynamicGrid(std::forward(args)...) { } @@ -82,8 +78,7 @@ class Shape2p5 void internalGridFilterSpurious() const; void internalGridFloodFill() const; mrpt::math::TPolygon2D internalGridContour() const; - mrpt::math::TPolygon2D internalPrunePolygon( - const mrpt::math::TPolygon2D& poly) const; + mrpt::math::TPolygon2D internalPrunePolygon(const mrpt::math::TPolygon2D& poly) const; struct RemovalCandidate { @@ -95,8 +90,7 @@ class Shape2p5 size_t i, const mrpt::math::TPolygon2D& p, bool allowApproxEdges) const; void debugSaveGridTo3DSceneFile( - const mrpt::math::TPolygon2D& rawGridContour, - const std::string& debugStr = {}) const; + const mrpt::math::TPolygon2D& rawGridContour, const std::string& debugStr = {}) const; }; } // namespace mvsim diff --git a/modules/simulator/include/mvsim/Simulable.h b/modules/simulator/include/mvsim/Simulable.h index fb364f2c..6360200d 100644 --- a/modules/simulator/include/mvsim/Simulable.h +++ b/modules/simulator/include/mvsim/Simulable.h @@ -136,15 +136,13 @@ class Simulable */ b2Body* b2dBody_ = nullptr; - bool parseSimulable( - const JointXMLnode<>& node, const ParseSimulableParams& p = {}); + bool parseSimulable(const JointXMLnode<>& node, const ParseSimulableParams& p = {}); void internalHandlePublish(const TSimulContext& context); /** Will be called after the global pose of the object has changed due to a * direct call to setPose() */ - virtual void notifySimulableSetPose( - [[maybe_unused]] const mrpt::math::TPose3D& newPose) + virtual void notifySimulableSetPose([[maybe_unused]] const mrpt::math::TPose3D& newPose) { // Default: do nothing } diff --git a/modules/simulator/include/mvsim/VehicleBase.h b/modules/simulator/include/mvsim/VehicleBase.h index 7f953701..c4f211f9 100644 --- a/modules/simulator/include/mvsim/VehicleBase.h +++ b/modules/simulator/include/mvsim/VehicleBase.h @@ -59,8 +59,7 @@ class VehicleBase : public VisualObject, public Simulable virtual void simul_post_timestep(const TSimulContext& context) override; virtual void apply_force( const mrpt::math::TVector2D& force, - const mrpt::math::TPoint2D& applyPoint = - mrpt::math::TPoint2D(0, 0)) override; + const mrpt::math::TPoint2D& applyPoint = mrpt::math::TPoint2D(0, 0)) override; /** Create bodies, fixtures, etc. for the dynamical simulation. May be * overrided by derived classes */ @@ -78,10 +77,7 @@ class VehicleBase : public VisualObject, public Simulable } //!< In local coordinates (this excludes the mass of wheels) size_t getNumWheels() const { return wheels_info_.size(); } - const Wheel& getWheelInfo(const size_t idx) const - { - return wheels_info_[idx]; - } + const Wheel& getWheelInfo(const size_t idx) const { return wheels_info_[idx]; } Wheel& getWheelInfo(const size_t idx) { return wheels_info_[idx]; } /** Current velocity of each wheel's center point (in local coords). Call @@ -104,10 +100,7 @@ class VehicleBase : public VisualObject, public Simulable /** Get the 2D shape of the vehicle chassis, as set from the config file * (only used for collision detection) */ - const mrpt::math::TPolygon2D& getChassisShape() const - { - return chassis_poly_; - } + const mrpt::math::TPolygon2D& getChassisShape() const { return chassis_poly_; } /** Set the vehicle index in the World */ void setVehicleIndex(size_t idx) { vehicle_index_ = idx; } @@ -133,10 +126,7 @@ class VehicleBase : public VisualObject, public Simulable b2Fixture* get_fixture_chassis() { return fixture_chassis_; } std::vector& get_fixture_wheels() { return fixture_wheels_; } const b2Fixture* get_fixture_chassis() const { return fixture_chassis_; } - const std::vector& get_fixture_wheels() const - { - return fixture_wheels_; - } + const std::vector& get_fixture_wheels() const { return fixture_wheels_; } void freeOpenGLResources() override { @@ -152,8 +142,7 @@ class VehicleBase : public VisualObject, public Simulable virtual void writeLogStrings(); virtual void internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) override; + const mrpt::optional_ref& physical, bool childrenOnly) override; protected: // Protected ctor for class factory @@ -162,14 +151,11 @@ class VehicleBase : public VisualObject, public Simulable /** Parse node : The derived-class part of load_params_from_xml(), * also called in factory(). Includes parsing the * block. */ - virtual void dynamics_load_params_from_xml( - const rapidxml::xml_node* xml_node) = 0; + virtual void dynamics_load_params_from_xml(const rapidxml::xml_node* xml_node) = 0; - virtual std::vector invoke_motor_controllers( - const TSimulContext& context) = 0; + virtual std::vector invoke_motor_controllers(const TSimulContext& context) = 0; - virtual void invoke_motor_controllers_post_step( - [[maybe_unused]] const TSimulContext& context) + virtual void invoke_motor_controllers_post_step([[maybe_unused]] const TSimulContext& context) { } @@ -224,8 +210,7 @@ class VehicleBase : public VisualObject, public Simulable void internal_internalGuiUpdate_forces(mrpt::opengl::COpenGLScene& scene); mrpt::opengl::CSetOfObjects::Ptr glChassisViz_, glChassisPhysical_; - std::vector glWheelsViz_, - glWheelsPhysical_; + std::vector glWheelsViz_, glWheelsPhysical_; mrpt::opengl::CSetOfLines::Ptr glForces_; mrpt::opengl::CSetOfLines::Ptr glMotorTorques_; @@ -263,8 +248,7 @@ extern TClassFactory_vehicleDynamics classFactory_vehicleDynamics; #define DECLARES_REGISTER_VEHICLE_DYNAMICS(CLASS_NAME) \ DECLARES_REGISTER_CLASS1(CLASS_NAME, VehicleBase, World*) -#define REGISTER_VEHICLE_DYNAMICS(TEXTUAL_NAME, CLASS_NAME) \ - REGISTER_CLASS1( \ - TClassFactory_vehicleDynamics, classFactory_vehicleDynamics, \ - TEXTUAL_NAME, CLASS_NAME) +#define REGISTER_VEHICLE_DYNAMICS(TEXTUAL_NAME, CLASS_NAME) \ + REGISTER_CLASS1( \ + TClassFactory_vehicleDynamics, classFactory_vehicleDynamics, TEXTUAL_NAME, CLASS_NAME) } // namespace mvsim diff --git a/modules/simulator/include/mvsim/VehicleDynamics/VehicleAckermann.h b/modules/simulator/include/mvsim/VehicleDynamics/VehicleAckermann.h index 93a558b8..5531523a 100644 --- a/modules/simulator/include/mvsim/VehicleDynamics/VehicleAckermann.h +++ b/modules/simulator/include/mvsim/VehicleDynamics/VehicleAckermann.h @@ -47,12 +47,7 @@ class DynamicsAckermann : public VehicleBase { double fl_torque, fr_torque, rl_torque, rr_torque; double steer_ang; //!< Equivalent Ackermann steering angle - TControllerOutput() - : fl_torque(0), - fr_torque(0), - rl_torque(0), - rr_torque(0), - steer_ang(0) + TControllerOutput() : fl_torque(0), fr_torque(0), rl_torque(0), rr_torque(0), steer_ang(0) { } }; @@ -67,14 +62,12 @@ class DynamicsAckermann : public VehicleBase static const char* class_name() { return "raw"; } //!< Directly set these values to tell the controller the desired //! setpoints - double setpoint_wheel_torque_l, setpoint_wheel_torque_r, - setpoint_steer_ang; + double setpoint_wheel_torque_l, setpoint_wheel_torque_r, setpoint_steer_ang; virtual void control_step( const DynamicsAckermann::TControllerInput& ci, DynamicsAckermann::TControllerOutput& co) override; virtual void load_config(const rapidxml::xml_node& node) override; - virtual void teleop_interface( - const TeleopInput& in, TeleopOutput& out) override; + virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out) override; }; /** PID controller that controls the vehicle with front traction & steering @@ -92,8 +85,7 @@ class DynamicsAckermann : public VehicleBase const DynamicsAckermann::TControllerInput& ci, DynamicsAckermann::TControllerOutput& co) override; virtual void load_config(const rapidxml::xml_node& node) override; - virtual void teleop_interface( - const TeleopInput& in, TeleopOutput& out) override; + virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out) override; double KP, KI, KD; //!< PID controller parameters double max_torque; //!< Maximum abs. value torque (for clamp) [Nm] @@ -130,8 +122,7 @@ class DynamicsAckermann : public VehicleBase const DynamicsAckermann::TControllerInput& ci, DynamicsAckermann::TControllerOutput& co) override; virtual void load_config(const rapidxml::xml_node& node) override; - virtual void teleop_interface( - const TeleopInput& in, TeleopOutput& out) override; + virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out) override; double KP, KI, KD; //!< PID controller parameters double max_torque; //!< Maximum abs. value torque (for clamp) [Nm] @@ -145,10 +136,7 @@ class DynamicsAckermann : public VehicleBase const ControllerBase::Ptr& getController() const { return controller_; } ControllerBase::Ptr& getController() { return controller_; } - virtual ControllerBaseInterface* getControllerInterface() override - { - return controller_.get(); - } + virtual ControllerBaseInterface* getControllerInterface() override { return controller_.get(); } /** @} */ // end controllers @@ -160,16 +148,13 @@ class DynamicsAckermann : public VehicleBase * in the object. */ void computeFrontWheelAngles( - const double desired_equiv_steer_ang, double& out_fl_ang, - double& out_fr_ang) const; + const double desired_equiv_steer_ang, double& out_fl_ang, double& out_fr_ang) const; protected: // See base class docs - virtual void dynamics_load_params_from_xml( - const rapidxml::xml_node* xml_node) override; + virtual void dynamics_load_params_from_xml(const rapidxml::xml_node* xml_node) override; // See base class doc - virtual std::vector invoke_motor_controllers( - const TSimulContext& context) override; + virtual std::vector invoke_motor_controllers(const TSimulContext& context) override; private: ControllerBase::Ptr controller_; //!< The installed controller diff --git a/modules/simulator/include/mvsim/VehicleDynamics/VehicleAckermann_Drivetrain.h b/modules/simulator/include/mvsim/VehicleDynamics/VehicleAckermann_Drivetrain.h index 85d469b9..55b68959 100644 --- a/modules/simulator/include/mvsim/VehicleDynamics/VehicleAckermann_Drivetrain.h +++ b/modules/simulator/include/mvsim/VehicleDynamics/VehicleAckermann_Drivetrain.h @@ -84,8 +84,7 @@ class DynamicsAckermannDrivetrain : public VehicleBase const DynamicsAckermannDrivetrain::TControllerInput& ci, DynamicsAckermannDrivetrain::TControllerOutput& co) override; virtual void load_config(const rapidxml::xml_node& node) override; - virtual void teleop_interface( - const TeleopInput& in, TeleopOutput& out) override; + virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out) override; }; /** PID controller that controls the vehicle with front traction & steering @@ -103,8 +102,7 @@ class DynamicsAckermannDrivetrain : public VehicleBase const DynamicsAckermannDrivetrain::TControllerInput& ci, DynamicsAckermannDrivetrain::TControllerOutput& co) override; virtual void load_config(const rapidxml::xml_node& node) override; - virtual void teleop_interface( - const TeleopInput& in, TeleopOutput& out) override; + virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out) override; double KP, KI, KD; //!< PID controller parameters double max_torque; //!< Maximum abs. value torque (for clamp) [Nm] @@ -139,8 +137,7 @@ class DynamicsAckermannDrivetrain : public VehicleBase const DynamicsAckermannDrivetrain::TControllerInput& ci, DynamicsAckermannDrivetrain::TControllerOutput& co) override; virtual void load_config(const rapidxml::xml_node& node) override; - virtual void teleop_interface( - const TeleopInput& in, TeleopOutput& out) override; + virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out) override; double KP, KI, KD; //!< PID controller parameters double max_torque; //!< Maximum abs. value torque (for clamp) [Nm] @@ -151,10 +148,7 @@ class DynamicsAckermannDrivetrain : public VehicleBase const ControllerBase::Ptr& getController() const { return controller_; } ControllerBase::Ptr& getController() { return controller_; } - virtual ControllerBaseInterface* getControllerInterface() override - { - return controller_.get(); - } + virtual ControllerBaseInterface* getControllerInterface() override { return controller_.get(); } /** @} */ // end controllers @@ -166,22 +160,19 @@ class DynamicsAckermannDrivetrain : public VehicleBase * in the object. */ void computeFrontWheelAngles( - const double desired_equiv_steer_ang, double& out_fl_ang, - double& out_fr_ang) const; + const double desired_equiv_steer_ang, double& out_fl_ang, double& out_fr_ang) const; /** Computes differential split for Torsen-like limited slip differentials. */ void computeDiffTorqueSplit( - const double w1, const double w2, const double diffBias, - const double defaultSplitRatio, double& t1, double& t2); + const double w1, const double w2, const double diffBias, const double defaultSplitRatio, + double& t1, double& t2); protected: // See base class docs - virtual void dynamics_load_params_from_xml( - const rapidxml::xml_node* xml_node) override; + virtual void dynamics_load_params_from_xml(const rapidxml::xml_node* xml_node) override; // See base class doc - virtual std::vector invoke_motor_controllers( - const TSimulContext& context) override; + virtual std::vector invoke_motor_controllers(const TSimulContext& context) override; private: ControllerBase::Ptr controller_; //!< The installed controller diff --git a/modules/simulator/include/mvsim/VehicleDynamics/VehicleDifferential.h b/modules/simulator/include/mvsim/VehicleDynamics/VehicleDifferential.h index 5bd1cf12..8e947d57 100644 --- a/modules/simulator/include/mvsim/VehicleDynamics/VehicleDifferential.h +++ b/modules/simulator/include/mvsim/VehicleDynamics/VehicleDifferential.h @@ -42,8 +42,7 @@ class DynamicsDifferential : public VehicleBase struct ConfigPerWheel { ConfigPerWheel() = default; - ConfigPerWheel( - const std::string& _name, const mrpt::math::TPoint2D& _pos) + ConfigPerWheel(const std::string& _name, const mrpt::math::TPoint2D& _pos) : name(_name), pos(_pos) { } @@ -61,8 +60,7 @@ class DynamicsDifferential : public VehicleBase { } - DynamicsDifferential( - World* parent, const std::vector& cfgPerWheel); + DynamicsDifferential(World* parent, const std::vector& cfgPerWheel); /** @name Controllers @{ */ @@ -99,8 +97,7 @@ class DynamicsDifferential : public VehicleBase virtual void control_step( const DynamicsDifferential::TControllerInput& ci, DynamicsDifferential::TControllerOutput& co) override; - virtual void teleop_interface( - const TeleopInput& in, TeleopOutput& out) override; + virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out) override; }; /** PID controller that controls the vehicle twist: linear & angular @@ -116,8 +113,7 @@ class DynamicsDifferential : public VehicleBase DynamicsDifferential::TControllerOutput& co) override; virtual void load_config(const rapidxml::xml_node& node) override; - virtual void teleop_interface( - const TeleopInput& in, TeleopOutput& out) override; + virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out) override; /// PID controller parameters double KP = 10, KI = 0, KD = 0; @@ -167,8 +163,7 @@ class DynamicsDifferential : public VehicleBase DynamicsDifferential::TControllerOutput& co) override; void on_post_step(const TSimulContext& context) override; - virtual void teleop_interface( - const TeleopInput& in, TeleopOutput& out) override; + virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out) override; // See base docs. bool setTwistCommand(const mrpt::math::TTwist2D& t) override @@ -199,10 +194,7 @@ class DynamicsDifferential : public VehicleBase const ControllerBase::Ptr& getController() const { return controller_; } ControllerBase::Ptr& getController() { return controller_; } - virtual ControllerBaseInterface* getControllerInterface() override - { - return controller_.get(); - } + virtual ControllerBaseInterface* getControllerInterface() override { return controller_.get(); } /** @} */ // end controllers @@ -210,13 +202,10 @@ class DynamicsDifferential : public VehicleBase protected: // See base class docs - virtual void dynamics_load_params_from_xml( - const rapidxml::xml_node* xml_node) override; + virtual void dynamics_load_params_from_xml(const rapidxml::xml_node* xml_node) override; // See base class docs - virtual std::vector invoke_motor_controllers( - const TSimulContext& context) override; - virtual void invoke_motor_controllers_post_step( - const TSimulContext& context) override; + virtual std::vector invoke_motor_controllers(const TSimulContext& context) override; + virtual void invoke_motor_controllers_post_step(const TSimulContext& context) override; /// Defined at ctor time: const std::vector configPerWheel_; diff --git a/modules/simulator/include/mvsim/VisualObject.h b/modules/simulator/include/mvsim/VisualObject.h index dca35442..e00dfcbc 100644 --- a/modules/simulator/include/mvsim/VisualObject.h +++ b/modules/simulator/include/mvsim/VisualObject.h @@ -30,8 +30,7 @@ class VisualObject { public: VisualObject( - World* parent, bool insertCustomVizIntoViz = true, - bool insertCustomVizIntoPhysical = true); + World* parent, bool insertCustomVizIntoViz = true, bool insertCustomVizIntoPhysical = true); virtual ~VisualObject(); @@ -51,10 +50,7 @@ class VisualObject /** Returns the collision shape, if defined (should be for regular entities * after correct initialization). */ - const std::optional& collisionShape() const - { - return collisionShape_; - } + const std::optional& collisionShape() const { return collisionShape_; } void showCollisionShape(bool show); @@ -85,8 +81,7 @@ class VisualObject bool childrenOnly = false) = 0; void addCustomVisualization( - const mrpt::opengl::CRenderizable::Ptr& glModel, - const mrpt::poses::CPose3D& modelPose = {}, + const mrpt::opengl::CRenderizable::Ptr& glModel, const mrpt::poses::CPose3D& modelPose = {}, const float modelScale = 1.0f, const std::string& modelName = "group", const std::optional& modelURI = std::nullopt, const bool initialShowBoundingBox = false); diff --git a/modules/simulator/include/mvsim/Wheel.h b/modules/simulator/include/mvsim/Wheel.h index 313f316f..ea918399 100644 --- a/modules/simulator/include/mvsim/Wheel.h +++ b/modules/simulator/include/mvsim/Wheel.h @@ -78,17 +78,10 @@ class Wheel : public VisualObject void internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) override; - - double getPhi() const - { - return phi; - } //!< Orientation (rad) wrt vehicle local frame - void setPhi(double val) - { - phi = val; - } //!< Orientation (rad) wrt vehicle local frame + const mrpt::optional_ref& physical, bool childrenOnly) override; + + double getPhi() const { return phi; } //!< Orientation (rad) wrt vehicle local frame + void setPhi(double val) { phi = val; } //!< Orientation (rad) wrt vehicle local frame double getW() const { return w; } //!< Spinning velocity (rad/s) wrt shaft void setW(double val) { w = val; } //!< Spinning velocity (rad/s) wrt shaft void recalcInertia(); //!< Recompute Iyy from mass, diameter and height. diff --git a/modules/simulator/include/mvsim/World.h b/modules/simulator/include/mvsim/World.h index 184a7b75..33998f80 100644 --- a/modules/simulator/include/mvsim/World.h +++ b/modules/simulator/include/mvsim/World.h @@ -91,8 +91,7 @@ class World : public mrpt::system::COutputLogger * error message */ void load_from_XML( - const std::string& xml_text, - const std::string& fileNameForPath = std::string(".")); + const std::string& xml_text, const std::string& fileNameForPath = std::string(".")); /** @} */ /** \name Simulation execution @@ -121,8 +120,7 @@ class World : public mrpt::system::COutputLogger { auto lck = mrpt::lockHelper(simul_time_mtx_); ASSERT_(simul_start_wallclock_time_.has_value()); - return mrpt::Clock::fromDouble( - simulTime_ + simul_start_wallclock_time_.value()); + return mrpt::Clock::fromDouble(simulTime_ + simul_start_wallclock_time_.value()); } /// Simulation fixed-time interval for numerical integration @@ -177,23 +175,16 @@ class World : public mrpt::system::COutputLogger */ void update_GUI(TUpdateGUIParams* params = nullptr); - const mrpt::gui::CDisplayWindowGUI::Ptr& gui_window() const - { - return gui_.gui_win; - } + const mrpt::gui::CDisplayWindowGUI::Ptr& gui_window() const { return gui_.gui_win; } - const mrpt::math::TPoint3D& gui_mouse_point() const - { - return gui_.clickedPt; - } + const mrpt::math::TPoint3D& gui_mouse_point() const { return gui_.clickedPt; } /** If !=null, a set of objects to be rendered merged with the default * visualization. Lock the mutex gui_user_objects_mtx_ while writing. * There are two sets of objects: "viz" for visualization only, "physical" * for objects which should be detected by sensors. */ - mrpt::opengl::CSetOfObjects::Ptr guiUserObjectsPhysical_, - guiUserObjectsViz_; + mrpt::opengl::CSetOfObjects::Ptr guiUserObjectsPhysical_, guiUserObjectsViz_; std::mutex guiUserObjectsMtx_; /// Update 3D vehicles, sensors, run render-based sensors, etc: @@ -201,8 +192,7 @@ class World : public mrpt::system::COutputLogger /// mode. void internalGraphicsLoopTasksForSimulation(); - void internalRunSensorsOn3DScene( - mrpt::opengl::COpenGLScene& physicalObjects); + void internalRunSensorsOn3DScene(mrpt::opengl::COpenGLScene& physicalObjects); void internalUpdate3DSceneObjects( mrpt::opengl::COpenGLScene& viz, mrpt::opengl::COpenGLScene& physical); @@ -297,26 +287,17 @@ class World : public mrpt::system::COutputLogger /** \name Access inner working objects @{*/ std::unique_ptr& getBox2DWorld() { return box2d_world_; } - const std::unique_ptr& getBox2DWorld() const - { - return box2d_world_; - } + const std::unique_ptr& getBox2DWorld() const { return box2d_world_; } b2Body* getBox2DGroundBody() { return b2_ground_body_; } const VehicleList& getListOfVehicles() const { return vehicles_; } VehicleList& getListOfVehicles() { return vehicles_; } const BlockList& getListOfBlocks() const { return blocks_; } BlockList& getListOfBlocks() { return blocks_; } - const WorldElementList& getListOfWorldElements() const - { - return worldElements_; - } + const WorldElementList& getListOfWorldElements() const { return worldElements_; } /// Always lock/unlock getListOfSimulableObjectsMtx() before using this: SimulableList& getListOfSimulableObjects() { return simulableObjects_; } - const SimulableList& getListOfSimulableObjects() const - { - return simulableObjects_; - } + const SimulableList& getListOfSimulableObjects() const { return simulableObjects_; } auto& getListOfSimulableObjectsMtx() { return simulableObjectsMtx_; } mrpt::system::CTimeLogger& getTimeLogger() { return timlogger_; } @@ -357,8 +338,8 @@ class World : public mrpt::system::COutputLogger /** \name Optional user hooks @{*/ - using on_observation_callback_t = std::function; + using on_observation_callback_t = + std::function; void registerCallbackOnObservation(const on_observation_callback_t& f) { @@ -366,8 +347,7 @@ class World : public mrpt::system::COutputLogger } /** Calls all registered callbacks: */ - void dispatchOnObservation( - const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs); + void dispatchOnObservation(const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs); /** @} */ @@ -453,8 +433,7 @@ class World : public mrpt::system::COutputLogger {"joystick_enabled", {"%bool", &joystickEnabled_}}, {"save_to_rawlog", {"%s", &save_to_rawlog_}}, {"rawlog_odometry_rate", {"%lf", &rawlog_odometry_rate_}}, - {"save_ground_truth_trajectory", - {"%s", &save_ground_truth_trajectory_}}, + {"save_ground_truth_trajectory", {"%s", &save_ground_truth_trajectory_}}, {"ground_truth_rate", {"%lf", &ground_truth_rate_}}, }; @@ -473,10 +452,8 @@ class World : public mrpt::system::COutputLogger /// This private container will be filled with objects in the public /// gui_user_objects_ - mrpt::opengl::CSetOfObjects::Ptr glUserObjsPhysical_ = - mrpt::opengl::CSetOfObjects::Create(); - mrpt::opengl::CSetOfObjects::Ptr glUserObjsViz_ = - mrpt::opengl::CSetOfObjects::Create(); + mrpt::opengl::CSetOfObjects::Ptr glUserObjsPhysical_ = mrpt::opengl::CSetOfObjects::Create(); + mrpt::opengl::CSetOfObjects::Ptr glUserObjsViz_ = mrpt::opengl::CSetOfObjects::Create(); // ------- GUI options ----- struct TGUI_Options @@ -519,8 +496,7 @@ class World : public mrpt::system::COutputLogger }; TGUI_Options() = default; - void parse_from( - const rapidxml::xml_node& node, COutputLogger& logger); + void parse_from(const rapidxml::xml_node& node, COutputLogger& logger); }; /** Some of these options are only used the first time the GUI window is @@ -531,8 +507,7 @@ class World : public mrpt::system::COutputLogger { LightOptions() = default; - void parse_from( - const rapidxml::xml_node& node, COutputLogger& logger); + void parse_from(const rapidxml::xml_node& node, COutputLogger& logger); bool enable_shadows = true; int shadow_map_size = 2048; @@ -565,10 +540,8 @@ class World : public mrpt::system::COutputLogger {"shadow_bias_cam2frag", {"%f", &shadow_bias_cam2frag}}, {"shadow_bias_normal", {"%f", &shadow_bias_normal}}, {"light_ambient", {"%f", &light_ambient}}, - {"eye_distance_to_shadow_map_extension", - {"%f", &eye_distance_to_shadow_map_extension}}, - {"minimum_shadow_map_extension_ratio", - {"%f", &minimum_shadow_map_extension_ratio}}, + {"eye_distance_to_shadow_map_extension", {"%f", &eye_distance_to_shadow_map_extension}}, + {"minimum_shadow_map_extension_ratio", {"%f", &minimum_shadow_map_extension_ratio}}, }; }; @@ -640,8 +613,7 @@ class World : public mrpt::system::COutputLogger /** 3D scene with all visual objects (vehicles, obstacles, markers, etc.) * \sa worldPhysical_ */ - mrpt::opengl::COpenGLScene::Ptr worldVisual_ = - mrpt::opengl::COpenGLScene::Create(); + mrpt::opengl::COpenGLScene::Ptr worldVisual_ = mrpt::opengl::COpenGLScene::Create(); /** 3D scene with all physically observable objects: we will use this * scene as input to simulated sensors like cameras, where we don't wont @@ -660,14 +632,11 @@ class World : public mrpt::system::COutputLogger std::set reset_collision_flags_; std::mutex reset_collision_flags_mtx_; - void internal_gui_on_observation( - const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs); + void internal_gui_on_observation(const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs); void internal_gui_on_observation_3Dscan( - const Simulable& veh, - const std::shared_ptr& obs); + const Simulable& veh, const std::shared_ptr& obs); void internal_gui_on_observation_image( - const Simulable& veh, - const std::shared_ptr& obs); + const Simulable& veh, const std::shared_ptr& obs); mrpt::math::TPoint2D internal_gui_on_image( const std::string& label, const mrpt::img::CImage& im, int winPosX); @@ -676,8 +645,7 @@ class World : public mrpt::system::COutputLogger /** Changes the light source direction from azimuth and elevation angles (in * radians) */ - void setLightDirectionFromAzimuthElevation( - const float azimuth, const float elevation); + void setLightDirectionFromAzimuthElevation(const float azimuth, const float elevation); /** @} */ // end GUI stuff @@ -689,8 +657,7 @@ class World : public mrpt::system::COutputLogger struct XmlParserContext { - XmlParserContext( - const rapidxml::xml_node* n, const std::string& basePath) + XmlParserContext(const rapidxml::xml_node* n, const std::string& basePath) : node(n), currentBasePath(basePath) { } @@ -702,25 +669,21 @@ class World : public mrpt::system::COutputLogger /// This will parse a main XML file, or its included void internal_recursive_parse_XML(const XmlParserContext& ctx); - using xml_tag_parser_function_t = - std::function; + using xml_tag_parser_function_t = std::function; std::map xmlParsers_; void register_standard_xml_tag_parsers(); - void register_tag_parser( - const std::string& xmlTagName, const xml_tag_parser_function_t& f) + void register_tag_parser(const std::string& xmlTagName, const xml_tag_parser_function_t& f) { xmlParsers_.emplace(xmlTagName, f); } void register_tag_parser( - const std::string& xmlTagName, - void (World::*f)(const XmlParserContext& ctx)) + const std::string& xmlTagName, void (World::*f)(const XmlParserContext& ctx)) { xmlParsers_.emplace( - xmlTagName, - [this, f](const XmlParserContext& ctx) { (this->*f)(ctx); }); + xmlTagName, [this, f](const XmlParserContext& ctx) { (this->*f)(ctx); }); } // ======== XML parser tags ======== @@ -743,15 +706,13 @@ class World : public mrpt::system::COutputLogger mutable RemoteResourcesManager remoteResources_; - void internalOnObservation( - const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs); + void internalOnObservation(const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs); void internalPostSimulStepForRawlog(); void internalPostSimulStepForTrajectory(); std::mutex rawlog_io_mtx_; - std::map> - rawlog_io_per_veh_; + std::map> rawlog_io_per_veh_; std::optional rawlog_last_odom_time_; std::mutex gt_io_mtx_; @@ -763,14 +724,11 @@ class World : public mrpt::system::COutputLogger #if MVSIM_HAS_ZMQ && MVSIM_HAS_PROTOBUF - mvsim_msgs::SrvSetPoseAnswer srv_set_pose( - const mvsim_msgs::SrvSetPose& req); - mvsim_msgs::SrvGetPoseAnswer srv_get_pose( - const mvsim_msgs::SrvGetPose& req); + mvsim_msgs::SrvSetPoseAnswer srv_set_pose(const mvsim_msgs::SrvSetPose& req); + mvsim_msgs::SrvGetPoseAnswer srv_get_pose(const mvsim_msgs::SrvGetPose& req); mvsim_msgs::SrvSetControllerTwistAnswer srv_set_controller_twist( const mvsim_msgs::SrvSetControllerTwist& req); - mvsim_msgs::SrvShutdownAnswer srv_shutdown( - const mvsim_msgs::SrvShutdown& req); + mvsim_msgs::SrvShutdownAnswer srv_shutdown(const mvsim_msgs::SrvShutdown& req); #endif }; } // namespace mvsim diff --git a/modules/simulator/include/mvsim/WorldElements/ElevationMap.h b/modules/simulator/include/mvsim/WorldElements/ElevationMap.h index fbcdba74..bfc98875 100644 --- a/modules/simulator/include/mvsim/WorldElements/ElevationMap.h +++ b/modules/simulator/include/mvsim/WorldElements/ElevationMap.h @@ -29,14 +29,12 @@ class ElevationMap : public WorldElementBase virtual void simul_pre_timestep(const TSimulContext& context) override; virtual void simul_post_timestep(const TSimulContext& context) override; - bool getElevationAt( - double x, double y, float& z) const; //!< return false if out of bounds + bool getElevationAt(double x, double y, float& z) const; //!< return false if out of bounds protected: virtual void internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) override; + const mrpt::optional_ref& physical, bool childrenOnly) override; /** This object holds both, the mesh data, and is in charge of 3D rendering. */ diff --git a/modules/simulator/include/mvsim/WorldElements/GroundGrid.h b/modules/simulator/include/mvsim/WorldElements/GroundGrid.h index 79ec5ffe..e8de6db2 100644 --- a/modules/simulator/include/mvsim/WorldElements/GroundGrid.h +++ b/modules/simulator/include/mvsim/WorldElements/GroundGrid.h @@ -27,8 +27,7 @@ class GroundGrid : public WorldElementBase protected: virtual void internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) override; + const mrpt::optional_ref& physical, bool childrenOnly) override; bool is_floating_; std::string float_center_at_vehicle_name_; diff --git a/modules/simulator/include/mvsim/WorldElements/HorizontalPlane.h b/modules/simulator/include/mvsim/WorldElements/HorizontalPlane.h index b884854e..58b80aff 100644 --- a/modules/simulator/include/mvsim/WorldElements/HorizontalPlane.h +++ b/modules/simulator/include/mvsim/WorldElements/HorizontalPlane.h @@ -32,8 +32,7 @@ class HorizontalPlane : public WorldElementBase protected: virtual void internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) override; + const mrpt::optional_ref& physical, bool childrenOnly) override; float x_min_ = -10, x_max_ = 10, y_min_ = -10, y_max_ = 10; mrpt::img::TColor color_ = {0xa0, 0xa0, 0xa0, 0xff}; diff --git a/modules/simulator/include/mvsim/WorldElements/OccupancyGridMap.h b/modules/simulator/include/mvsim/WorldElements/OccupancyGridMap.h index ed0ac0e5..3dc4437e 100644 --- a/modules/simulator/include/mvsim/WorldElements/OccupancyGridMap.h +++ b/modules/simulator/include/mvsim/WorldElements/OccupancyGridMap.h @@ -43,8 +43,7 @@ class OccupancyGridMap : public WorldElementBase protected: virtual void internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) override; + const mrpt::optional_ref& physical, bool childrenOnly) override; mrpt::maps::COccupancyGridMap2D grid_; diff --git a/modules/simulator/include/mvsim/WorldElements/PointCloud.h b/modules/simulator/include/mvsim/WorldElements/PointCloud.h index 206cd33f..2e084851 100644 --- a/modules/simulator/include/mvsim/WorldElements/PointCloud.h +++ b/modules/simulator/include/mvsim/WorldElements/PointCloud.h @@ -40,8 +40,7 @@ class PointCloud : public WorldElementBase protected: virtual void internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) override; + const mrpt::optional_ref& physical, bool childrenOnly) override; mrpt::maps::CPointsMap::Ptr points_; diff --git a/modules/simulator/include/mvsim/WorldElements/SkyBox.h b/modules/simulator/include/mvsim/WorldElements/SkyBox.h index c0431bfa..f45764c3 100644 --- a/modules/simulator/include/mvsim/WorldElements/SkyBox.h +++ b/modules/simulator/include/mvsim/WorldElements/SkyBox.h @@ -30,8 +30,7 @@ class SkyBox : public WorldElementBase protected: virtual void internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) override; + const mrpt::optional_ref& physical, bool childrenOnly) override; // It holds a CSkyBox object. Stored as base CRenderizable to prevent // depending in this public header on mrpt >=2.7.0 so mvsim can still be diff --git a/modules/simulator/include/mvsim/WorldElements/VerticalPlane.h b/modules/simulator/include/mvsim/WorldElements/VerticalPlane.h index 63c98513..1439c12b 100644 --- a/modules/simulator/include/mvsim/WorldElements/VerticalPlane.h +++ b/modules/simulator/include/mvsim/WorldElements/VerticalPlane.h @@ -36,8 +36,7 @@ class VerticalPlane : public WorldElementBase protected: virtual void internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) override; + const mrpt::optional_ref& physical, bool childrenOnly) override; float x0_ = -10, x1_ = -10, y0_ = -10, y1_ = 10; mrpt::img::TColor color_ = {0xa0, 0xa0, 0xa0, 0xff}; diff --git a/modules/simulator/include/mvsim/WorldElements/WorldElementBase.h b/modules/simulator/include/mvsim/WorldElements/WorldElementBase.h index 40d597ca..0a6bbf68 100644 --- a/modules/simulator/include/mvsim/WorldElements/WorldElementBase.h +++ b/modules/simulator/include/mvsim/WorldElements/WorldElementBase.h @@ -28,8 +28,7 @@ class WorldElementBase : public VisualObject, public Simulable * class_name. Otherwise, class_name is ignored. */ static Ptr factory( - World* parent, const rapidxml::xml_node* xml_node, - const char* class_name = nullptr); + World* parent, const rapidxml::xml_node* xml_node, const char* class_name = nullptr); virtual void loadConfigFrom(const rapidxml::xml_node* root) = 0; @@ -43,11 +42,9 @@ using TClassFactory_worldElements = extern TClassFactory_worldElements classFactory_worldElements; #define DECLARES_REGISTER_WORLD_ELEMENT(CLASS_NAME) \ - DECLARES_REGISTER_CLASS2( \ - CLASS_NAME, WorldElementBase, World*, const rapidxml::xml_node*) + DECLARES_REGISTER_CLASS2(CLASS_NAME, WorldElementBase, World*, const rapidxml::xml_node*) -#define REGISTER_WORLD_ELEMENT(TEXTUAL_NAME, CLASS_NAME) \ - REGISTER_CLASS2( \ - TClassFactory_worldElements, classFactory_worldElements, TEXTUAL_NAME, \ - CLASS_NAME) +#define REGISTER_WORLD_ELEMENT(TEXTUAL_NAME, CLASS_NAME) \ + REGISTER_CLASS2( \ + TClassFactory_worldElements, classFactory_worldElements, TEXTUAL_NAME, CLASS_NAME) } // namespace mvsim diff --git a/modules/simulator/src/Block.cpp b/modules/simulator/src/Block.cpp index 3d5b1be9..c029968e 100644 --- a/modules/simulator/src/Block.cpp +++ b/modules/simulator/src/Block.cpp @@ -46,13 +46,10 @@ Block::Block(World* parent) : VisualObject(parent), Simulable(parent) /** Register a new class of vehicles from XML description of type * "...". */ -void Block::register_block_class( - const World& parent, const rapidxml::xml_node* xml_node) +void Block::register_block_class(const World& parent, const rapidxml::xml_node* xml_node) { // Sanity checks: - if (!xml_node) - throw runtime_error( - "[Block::register_vehicle_class] XML node is nullptr"); + if (!xml_node) throw runtime_error("[Block::register_vehicle_class] XML node is nullptr"); if (0 != strcmp(xml_node->name(), "block:class")) throw runtime_error(mrpt::format( "[Block::register_block_class] XML element is '%s' " @@ -71,8 +68,7 @@ Block::Ptr Block::factory(World* parent, const rapidxml::xml_node* root) if (!root) throw runtime_error("[Block::factory] XML node is nullptr"); if (0 != strcmp(root->name(), "block")) throw runtime_error(mrpt::format( - "[Block::factory] XML root element is '%s' ('block' expected)", - root->name())); + "[Block::factory] XML root element is '%s' ('block' expected)", root->name())); // "class": When there is a 'class="XXX"' attribute, look for each parameter // in the set of "root" + "class_root" XML nodes: @@ -82,15 +78,12 @@ Block::Ptr Block::factory(World* parent, const rapidxml::xml_node* root) { // Always search in root. Also in the class root, if any: nodes.add(root); - if (const xml_attribute<>* block_class = root->first_attribute("class"); - block_class) + if (const xml_attribute<>* block_class = root->first_attribute("class"); block_class) { const string sClassName = block_class->value(); - if (class_root = block_classes_registry.get(sClassName); - !class_root) + if (class_root = block_classes_registry.get(sClassName); !class_root) THROW_EXCEPTION_FMT( - "[Block::factory] Block class '%s' undefined", - sClassName.c_str()); + "[Block::factory] Block class '%s' undefined", sClassName.c_str()); nodes.add(class_root); } @@ -124,12 +117,10 @@ Block::Ptr Block::factory(World* parent, const rapidxml::xml_node* root) // Params: // ----------------------------------------------------------- parse_xmlnode_children_as_param( - *root, block->params_, parent->user_defined_variables(), - "[Block::factory]"); + *root, block->params_, parent->user_defined_variables(), "[Block::factory]"); if (class_root) parse_xmlnode_children_as_param( - *class_root, block->params_, parent->user_defined_variables(), - "[Block::factory]"); + *class_root, block->params_, parent->user_defined_variables(), "[Block::factory]"); // Custom visualization 3D model: // ----------------------------------------------------------- @@ -138,8 +129,7 @@ Block::Ptr Block::factory(World* parent, const rapidxml::xml_node* root) // Shape node (optional, fallback to default shape if none found) if (const auto* xml_shape = nodes.first_node("shape"); xml_shape) { - mvsim::parse_xmlnode_shape( - *xml_shape, block->block_poly_, "[Block::factory]"); + mvsim::parse_xmlnode_shape(*xml_shape, block->block_poly_, "[Block::factory]"); block->updateMaxRadiusFromPoly(); } else if (const auto* xml_geom = nodes.first_node("geometry"); xml_geom) @@ -148,8 +138,7 @@ Block::Ptr Block::factory(World* parent, const rapidxml::xml_node* root) } // Auto shape node from visual? - if (const rapidxml::xml_node* xml_shape_viz = - nodes.first_node("shape_from_visual"); + if (const rapidxml::xml_node* xml_shape_viz = nodes.first_node("shape_from_visual"); xml_shape_viz) { const auto& bbVis = block->collisionShape(); @@ -219,11 +208,10 @@ Block::Ptr Block::factory(World* parent, const std::string& xml_text) } catch (rapidxml::parse_error& e) { - unsigned int line = - static_cast(std::count(input_str, e.where(), '\n') + 1); + unsigned int line = static_cast(std::count(input_str, e.where(), '\n') + 1); throw std::runtime_error(mrpt::format( - "[Block::factory] XML parse error (Line %u): %s", - static_cast(line), e.what())); + "[Block::factory] XML parse error (Line %u): %s", static_cast(line), + e.what())); } return Block::factory(parent, xml.first_node()); } @@ -242,8 +230,7 @@ void Block::simul_post_timestep(const TSimulContext& context) void Block::internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) + const mrpt::optional_ref& physical, bool childrenOnly) { // 1st time call?? -> Create objects // ---------------------------------- @@ -349,8 +336,7 @@ void Block::create_multibody_system(b2World& world) ASSERT_(nPts >= 3); ASSERT_LE_(nPts, (size_t)b2_maxPolygonVertices); std::vector pts(nPts); - for (size_t i = 0; i < nPts; i++) - pts[i] = b2Vec2(block_poly_[i].x, block_poly_[i].y); + for (size_t i = 0; i < nPts; i++) pts[i] = b2Vec2(block_poly_[i].x, block_poly_[i].y); b2PolygonShape blockPoly; blockPoly.Set(&pts[0], nPts); @@ -387,15 +373,13 @@ void Block::create_multibody_system(b2World& world) // Create "archor points" to simulate friction with the ground: // ----------------------------------------------------------------- const size_t nContactPoints = 2; - const double weight_per_contact_point = - mass_ * parent()->get_gravity() / nContactPoints; + const double weight_per_contact_point = mass_ * parent()->get_gravity() / nContactPoints; const double mu = groundFriction_; const double max_friction = mu * weight_per_contact_point; // Location (local coords) of each contact-point: const mrpt::math::TPoint2D pt_loc[nContactPoints] = { - mrpt::math::TPoint2D(maxRadius_, 0), - mrpt::math::TPoint2D(-maxRadius_, 0)}; + mrpt::math::TPoint2D(maxRadius_, 0), mrpt::math::TPoint2D(-maxRadius_, 0)}; b2FrictionJointDef fjd; @@ -411,20 +395,18 @@ void Block::create_multibody_system(b2World& world) fjd.maxForce = max_friction; fjd.maxTorque = 0; - b2FrictionJoint* b2_friction = dynamic_cast( - world_->getBox2DWorld()->CreateJoint(&fjd)); + b2FrictionJoint* b2_friction = + dynamic_cast(world_->getBox2DWorld()->CreateJoint(&fjd)); friction_joints_.push_back(b2_friction); } } -void Block::apply_force( - const mrpt::math::TVector2D& force, const mrpt::math::TPoint2D& applyPoint) +void Block::apply_force(const mrpt::math::TVector2D& force, const mrpt::math::TPoint2D& applyPoint) { if (intangible_) return; ASSERT_(b2dBody_); // Application point -> world coords - const b2Vec2 wPt = - b2dBody_->GetWorldPoint(b2Vec2(applyPoint.x, applyPoint.y)); + const b2Vec2 wPt = b2dBody_->GetWorldPoint(b2Vec2(applyPoint.x, applyPoint.y)); b2dBody_->ApplyForce(b2Vec2(force.x, force.y), wPt, true /*wake up*/); } @@ -443,10 +425,7 @@ void Block::setIsStatic(bool b) } // Protected ctor: -DummyInvisibleBlock::DummyInvisibleBlock(World* parent) - : VisualObject(parent), Simulable(parent) -{ -} +DummyInvisibleBlock::DummyInvisibleBlock(World* parent) : VisualObject(parent), Simulable(parent) {} void DummyInvisibleBlock::internalGuiUpdate( const mrpt::optional_ref& viz, @@ -458,8 +437,7 @@ void DummyInvisibleBlock::internalGuiUpdate( for (auto& s : sensors_) s->guiUpdate(viz, physical); } -void Block::internal_parseGeometry( - const rapidxml::xml_node& xml_geom_node) +void Block::internal_parseGeometry(const rapidxml::xml_node& xml_geom_node) { std::string type; // cylinder, sphere, etc. float radius = 0; @@ -477,8 +455,7 @@ void Block::internal_parseGeometry( }; parse_xmlnode_attribs( - xml_geom_node, params, world_->user_defined_variables(), - "[Block::internal_parseGeometry]"); + xml_geom_node, params, world_->user_defined_variables(), "[Block::internal_parseGeometry]"); if (type.empty()) { @@ -489,10 +466,8 @@ void Block::internal_parseGeometry( if (type == "cylinder") { - ASSERTMSG_( - radius > 0, "Missing 'radius' attribute for cylinder geometry"); - ASSERTMSG_( - length > 0, "Missing 'length' attribute for cylinder geometry"); + ASSERTMSG_(radius > 0, "Missing 'radius' attribute for cylinder geometry"); + ASSERTMSG_(length > 0, "Missing 'length' attribute for cylinder geometry"); if (vertex_count == 0) vertex_count = 10; // default @@ -505,8 +480,7 @@ void Block::internal_parseGeometry( } else if (type == "sphere") { - ASSERTMSG_( - radius > 0, "Missing 'radius' attribute for cylinder geometry"); + ASSERTMSG_(radius > 0, "Missing 'radius' attribute for cylinder geometry"); if (vertex_count == 0) vertex_count = 10; // default @@ -527,8 +501,7 @@ void Block::internal_parseGeometry( } else { - THROW_EXCEPTION_FMT( - "Unknown type in ", type.c_str()); + THROW_EXCEPTION_FMT("Unknown type in ", type.c_str()); } } diff --git a/modules/simulator/src/CollisionShapeCache.cpp b/modules/simulator/src/CollisionShapeCache.cpp index 32d8124a..41da5c10 100644 --- a/modules/simulator/src/CollisionShapeCache.cpp +++ b/modules/simulator/src/CollisionShapeCache.cpp @@ -24,15 +24,13 @@ CollisionShapeCache& CollisionShapeCache::Instance() } Shape2p5 CollisionShapeCache::get( - mrpt::opengl::CRenderizable& obj, float zMin, float zMax, - const mrpt::poses::CPose3D& modelPose, const float modelScale, - const std::optional& modelFile) + mrpt::opengl::CRenderizable& obj, float zMin, float zMax, const mrpt::poses::CPose3D& modelPose, + const float modelScale, const std::optional& modelFile) { // already cached? if (modelFile) { - if (auto it = cache.find(modelFile.value()); it != cache.end()) - return it->second.shape; + if (auto it = cache.find(modelFile.value()); it != cache.end()) return it->second.shape; } // No, it's a new model path, create its placeholder: @@ -40,8 +38,7 @@ Shape2p5 CollisionShapeCache::get( Shape2p5& ret = modelFile ? cache[*modelFile].shape : retVal; // Now, decide whether it's a simple geometry, or an arbitrary model: - const auto simpleGeom = - processSimpleGeometries(obj, zMin, zMax, modelPose, modelScale); + const auto simpleGeom = processSimpleGeometries(obj, zMin, zMax, modelPose, modelScale); if (simpleGeom) { @@ -93,13 +90,11 @@ std::optional CollisionShapeCache::processSimpleGeometries( // Cylinder // =============================== // If the cylinder is not upright, skip and go for the generic algorithm - if (std::abs(modelPose.pitch()) > 0.02_deg || - std::abs(modelPose.roll()) > 0.02_deg) + if (std::abs(modelPose.pitch()) > 0.02_deg || std::abs(modelPose.roll()) > 0.02_deg) return {}; const size_t actualEdgeCount = oCyl->getSlicesCount(); - double actualRadius = - std::max(oCyl->getTopRadius(), oCyl->getBottomRadius()); + double actualRadius = std::max(oCyl->getTopRadius(), oCyl->getBottomRadius()); return processCylinderLike( actualEdgeCount, actualRadius, zMin, zMax, modelPose, modelScale); @@ -128,8 +123,7 @@ std::optional CollisionShapeCache::processSimpleGeometries( // Box // =============================== // If the object is not upright, skip and go for the generic algorithm - if (std::abs(modelPose.pitch()) > 0.02_deg || - std::abs(modelPose.roll()) > 0.02_deg) + if (std::abs(modelPose.pitch()) > 0.02_deg || std::abs(modelPose.roll()) > 0.02_deg) return {}; mrpt::math::TPoint3D p1, p2; @@ -138,14 +132,11 @@ std::optional CollisionShapeCache::processSimpleGeometries( p2 *= modelScale; const mrpt::math::TPoint3D corners[4] = { - modelPose.composePoint({p1.x, p1.y, 0}), - modelPose.composePoint({p1.x, p2.y, 0}), - modelPose.composePoint({p2.x, p2.y, 0}), - modelPose.composePoint({p2.x, p1.y, 0})}; + modelPose.composePoint({p1.x, p1.y, 0}), modelPose.composePoint({p1.x, p2.y, 0}), + modelPose.composePoint({p2.x, p2.y, 0}), modelPose.composePoint({p2.x, p1.y, 0})}; mrpt::math::TPolygon2D contour; - for (int i = 0; i < 4; i++) - contour.emplace_back(corners[i].x, corners[i].y); + for (int i = 0; i < 4; i++) contour.emplace_back(corners[i].x, corners[i].y); Shape2p5 s; s.setShapeManual(contour, zMin, zMax); @@ -159,8 +150,8 @@ std::optional CollisionShapeCache::processSimpleGeometries( } Shape2p5 CollisionShapeCache::processGenericGeometry( - mrpt::opengl::CRenderizable& obj, float zMin, float zMax, - const mrpt::poses::CPose3D& modelPose, const float modelScale) + mrpt::opengl::CRenderizable& obj, float zMin, float zMax, const mrpt::poses::CPose3D& modelPose, + const float modelScale) { Shape2p5 ret; @@ -171,20 +162,17 @@ Shape2p5 CollisionShapeCache::processGenericGeometry( { oAssimp->onUpdateBuffers_all(); } - auto* oRSWF = - dynamic_cast(&obj); + auto* oRSWF = dynamic_cast(&obj); if (oRSWF) { oRSWF->onUpdateBuffers_Wireframe(); } - auto* oRST = - dynamic_cast(&obj); + auto* oRST = dynamic_cast(&obj); if (oRST) { oRST->onUpdateBuffers_Triangles(); } - auto* oRSTT = - dynamic_cast(&obj); + auto* oRSTT = dynamic_cast(&obj); if (oRSTT) { oRSTT->onUpdateBuffers_TexturedTriangles(); @@ -220,8 +208,7 @@ Shape2p5 CollisionShapeCache::processGenericGeometry( numTotalPts += 3; // transform the whole triangle, then compare with [z,z] limits: mrpt::opengl::TTriangle t = tri; - for (int i = 0; i < 3; i++) - t.vertex(i) = modelPose.composePoint(t.vertex(i) * modelScale); + for (int i = 0; i < 3; i++) t.vertex(i) = modelPose.composePoint(t.vertex(i) * modelScale); // does any of the point lie within the valid Z range, or is the // triangle going all the way down to top? @@ -251,8 +238,7 @@ Shape2p5 CollisionShapeCache::processGenericGeometry( } if (oRSTT) { - auto lck = - mrpt::lockHelper(oRSTT->shaderTexturedTrianglesBufferMutex().data); + auto lck = mrpt::lockHelper(oRSTT->shaderTexturedTrianglesBufferMutex().data); const auto& tris = oRSTT->shaderTexturedTrianglesBuffer(); for (const auto& tri : tris) lambdaUpdateTri(tri); } @@ -277,8 +263,7 @@ Shape2p5 CollisionShapeCache::processGenericGeometry( { if (!o) continue; - auto lck = - mrpt::lockHelper(o->shaderTexturedTrianglesBufferMutex().data); + auto lck = mrpt::lockHelper(o->shaderTexturedTrianglesBufferMutex().data); const auto& tris = o->shaderTexturedTrianglesBuffer(); for (const auto& tri : tris) lambdaUpdateTri(tri); } @@ -326,8 +311,7 @@ Shape2p5 CollisionShapeCache::processCylinderLike( for (size_t i = 0; i < nFaces; i++) { const double ang = i * 2 * M_PI / nFaces; - const mrpt::math::TPoint3D localPt = { - cos(ang) * actualRadius, sin(ang) * actualRadius, .0}; + const mrpt::math::TPoint3D localPt = {cos(ang) * actualRadius, sin(ang) * actualRadius, .0}; const auto pt = modelPose.composePoint(localPt * modelScale); contour.emplace_back(pt.x, pt.y); } diff --git a/modules/simulator/src/CsvLogger.cpp b/modules/simulator/src/CsvLogger.cpp index da4cb4ae..09783145 100644 --- a/modules/simulator/src/CsvLogger.cpp +++ b/modules/simulator/src/CsvLogger.cpp @@ -1,16 +1,10 @@ #include "mvsim/CsvLogger.h" -CSVLogger::CSVLogger() -{ - file_ = std::make_shared(std::ofstream()); -} +CSVLogger::CSVLogger() { file_ = std::make_shared(std::ofstream()); } CSVLogger::~CSVLogger() { close(); } void CSVLogger::addColumn(std::string name) { columns_[name] = 0.0; } -void CSVLogger::updateColumn(std::string name, double value) -{ - columns_[name] = value; -} +void CSVLogger::updateColumn(std::string name, double value) { columns_[name] = value; } bool CSVLogger::writeHeader() { @@ -56,9 +50,9 @@ bool CSVLogger::open() { if (file_) { - file_->open((std::string("session") + std::to_string(currentSession) + - std::string("-") + filepath_) - .c_str()); + file_->open( + (std::string("session") + std::to_string(currentSession) + std::string("-") + filepath_) + .c_str()); return isOpen(); } return false; diff --git a/modules/simulator/src/FrictionModels/DefaultFriction.cpp b/modules/simulator/src/FrictionModels/DefaultFriction.cpp index cc3f5e4a..43a9c246 100644 --- a/modules/simulator/src/FrictionModels/DefaultFriction.cpp +++ b/modules/simulator/src/FrictionModels/DefaultFriction.cpp @@ -17,19 +17,15 @@ using namespace mvsim; -DefaultFriction::DefaultFriction( - VehicleBase& my_vehicle, const rapidxml::xml_node* node) +DefaultFriction::DefaultFriction(VehicleBase& my_vehicle, const rapidxml::xml_node* node) : FrictionBase(my_vehicle), mu_(0.8), C_damping_(1.0) { // Sanity: we can tolerate node==nullptr (=> means use default params). if (node && 0 != strcmp(node->name(), "friction")) - throw std::runtime_error( - "... XML node was expected!!"); + throw std::runtime_error("... XML node was expected!!"); // Parse XML params: - if (node) - parse_xmlnode_children_as_param( - *node, params_, world_->user_defined_variables()); + if (node) parse_xmlnode_children_as_param(*node, params_, world_->user_defined_variables()); } // See docs in base class. @@ -40,8 +36,7 @@ mrpt::math::TVector2D DefaultFriction::evaluate_friction( const mrpt::poses::CPose2D wRot(0, 0, input.wheel.yaw); // Velocity of the wheel cog in the frame of the wheel itself: - const mrpt::math::TVector2D vel_w = - wRot.inverseComposePoint(input.wheelCogLocalVel); + const mrpt::math::TVector2D vel_w = wRot.inverseComposePoint(input.wheelCogLocalVel); // Action/Reaction, slippage, etc: // -------------------------------------- @@ -57,8 +52,7 @@ mrpt::math::TVector2D DefaultFriction::evaluate_friction( // Impulse required to step the lateral slippage: wheel_lat_friction = -vel_w.y * partial_mass / input.context.dt; - wheel_lat_friction = - b2Clamp(wheel_lat_friction, -max_friction, max_friction); + wheel_lat_friction = b2Clamp(wheel_lat_friction, -max_friction, max_friction); } // 2) Longitudinal friction (decoupled sub-problem) @@ -73,11 +67,9 @@ mrpt::math::TVector2D DefaultFriction::evaluate_friction( const double R = 0.5 * input.wheel.diameter; // Wheel radius const double lon_constraint_desired_wheel_w = vel_w.x / R; - const double desired_wheel_w_impulse = - (lon_constraint_desired_wheel_w - input.wheel.getW()); + const double desired_wheel_w_impulse = (lon_constraint_desired_wheel_w - input.wheel.getW()); - const double desired_wheel_alpha = - desired_wheel_w_impulse / input.context.dt; + const double desired_wheel_alpha = desired_wheel_w_impulse / input.context.dt; // (eq. 3)==> Find out F_r // Iyy_w * \Delta\omega_w = dt*\tau- R*dt*Fri -C_damp * \omega_w * dt @@ -87,28 +79,24 @@ mrpt::math::TVector2D DefaultFriction::evaluate_friction( // input.wheel_speed.x, 0.0); const double I_yy = input.wheel.Iyy; - double F_friction_lon = (input.motorTorque - I_yy * desired_wheel_alpha - - C_damping * input.wheel.getW()) / - R; + double F_friction_lon = + (input.motorTorque - I_yy * desired_wheel_alpha - C_damping * input.wheel.getW()) / R; // Slippage: The friction with the ground is not infinite: F_friction_lon = b2Clamp(F_friction_lon, -max_friction, max_friction); // Recalc wheel ang. velocity impulse with this reduced force: - const double actual_wheel_alpha = (input.motorTorque - R * F_friction_lon - - C_damping * input.wheel.getW()) / - I_yy; + const double actual_wheel_alpha = + (input.motorTorque - R * F_friction_lon - C_damping * input.wheel.getW()) / I_yy; // Apply impulse to wheel's spinning: - input.wheel.setW( - input.wheel.getW() + actual_wheel_alpha * input.context.dt); + input.wheel.setW(input.wheel.getW() + actual_wheel_alpha * input.context.dt); wheel_long_friction = F_friction_lon; // Resultant force: In local (x,y) coordinates (Newtons) wrt the Wheel // ----------------------------------------------------------------------- - const mrpt::math::TPoint2D result_force_wrt_wheel( - wheel_long_friction, wheel_lat_friction); + const mrpt::math::TPoint2D result_force_wrt_wheel(wheel_long_friction, wheel_lat_friction); // Rotate to put: Wheel frame ==> vehicle local framework: mrpt::math::TVector2D res; diff --git a/modules/simulator/src/FrictionModels/FrictionBase.cpp b/modules/simulator/src/FrictionModels/FrictionBase.cpp index ab17df58..851349b0 100644 --- a/modules/simulator/src/FrictionModels/FrictionBase.cpp +++ b/modules/simulator/src/FrictionModels/FrictionBase.cpp @@ -49,8 +49,7 @@ FrictionBase::Ptr FrictionBase::factory( using namespace rapidxml; if (!xml_node || 0 != strcmp(xml_node->name(), "friction")) - throw runtime_error( - "[FrictionBase::factory] Expected XML node "); + throw runtime_error("[FrictionBase::factory] Expected XML node "); // Parse: const xml_attribute<>* frict_class = xml_node->first_attribute("class"); @@ -62,7 +61,4 @@ FrictionBase::Ptr FrictionBase::factory( return classFactory_friction.create(frict_class->value(), parent, xml_node); } -void FrictionBase::setLogger(const std::weak_ptr& logger) -{ - logger_ = logger; -} +void FrictionBase::setLogger(const std::weak_ptr& logger) { logger_ = logger; } diff --git a/modules/simulator/src/FrictionModels/WardIagnemmaFriction.cpp b/modules/simulator/src/FrictionModels/WardIagnemmaFriction.cpp index 9a22ee2d..3214da37 100644 --- a/modules/simulator/src/FrictionModels/WardIagnemmaFriction.cpp +++ b/modules/simulator/src/FrictionModels/WardIagnemmaFriction.cpp @@ -24,8 +24,7 @@ WardIagnemmaFriction::WardIagnemmaFriction( { // Sanity: we can tolerate node==nullptr (=> means use default params). if (node && 0 != strcmp(node->name(), "friction")) - throw std::runtime_error( - "... XML node was expected!!"); + throw std::runtime_error("... XML node was expected!!"); if (node) { @@ -38,8 +37,8 @@ WardIagnemmaFriction::WardIagnemmaFriction( params["R2"] = TParamEntry("%lf", &R2_); // Parse XML params: parse_xmlnode_children_as_param( - *node, params, world_->user_defined_variables(), - "WardIagnemmaFriction", my_vehicle.parent() /*for logger*/); + *node, params, world_->user_defined_variables(), "WardIagnemmaFriction", + my_vehicle.parent() /*for logger*/); } } @@ -67,8 +66,7 @@ mrpt::math::TVector2D WardIagnemmaFriction::evaluate_friction( // Impulse required to step the lateral slippage: wheel_lat_friction = -vel_w.y * partial_mass / input.context.dt; - wheel_lat_friction = - b2Clamp(wheel_lat_friction, -max_friction, max_friction); + wheel_lat_friction = b2Clamp(wheel_lat_friction, -max_friction, max_friction); } // 2) Longitudinal friction (decoupled sub-problem) @@ -82,10 +80,8 @@ mrpt::math::TVector2D WardIagnemmaFriction::evaluate_friction( // required wheel \omega:case '4': const double R = 0.5 * input.wheel.diameter; // Wheel radius const double lon_constraint_desired_wheel_w = vel_w.x / R; - const double desired_wheel_w_impulse = - (lon_constraint_desired_wheel_w - input.wheel.getW()); - const double desired_wheel_alpha = - desired_wheel_w_impulse / input.context.dt; + const double desired_wheel_w_impulse = (lon_constraint_desired_wheel_w - input.wheel.getW()); + const double desired_wheel_alpha = desired_wheel_w_impulse / input.context.dt; // (eq. 3)==> Find out F_r // Iyy_w * \Delta\omega_w = dt*\tau- R*dt*Fri -C_damp * \omega_w * dt @@ -97,9 +93,8 @@ mrpt::math::TVector2D WardIagnemmaFriction::evaluate_friction( // Actually, Ward-Iagnemma rolling resistance is here (longitudal one): - const double F_rr = - -sign(vel_w.x) * partial_mass * gravity * - (R1_ * (1 - exp(-A_roll_ * fabs(vel_w.x))) + R2_ * fabs(vel_w.x)); + const double F_rr = -sign(vel_w.x) * partial_mass * gravity * + (R1_ * (1 - exp(-A_roll_ * fabs(vel_w.x))) + R2_ * fabs(vel_w.x)); if (!logger_.expired()) { @@ -108,29 +103,25 @@ mrpt::math::TVector2D WardIagnemmaFriction::evaluate_friction( const double I_yy = input.wheel.Iyy; // There are torques this is force v - double F_friction_lon = (input.motorTorque - I_yy * desired_wheel_alpha - - C_damping * input.wheel.getW()) / - R + - F_rr; + double F_friction_lon = + (input.motorTorque - I_yy * desired_wheel_alpha - C_damping * input.wheel.getW()) / R + + F_rr; // Slippage: The friction with the ground is not infinite: F_friction_lon = b2Clamp(F_friction_lon, -max_friction, max_friction); // Recalc wheel ang. velocity impulse with this reduced force: - const double actual_wheel_alpha = (input.motorTorque - R * F_friction_lon - - C_damping * input.wheel.getW()) / - I_yy; + const double actual_wheel_alpha = + (input.motorTorque - R * F_friction_lon - C_damping * input.wheel.getW()) / I_yy; // Apply impulse to wheel's spinning: - input.wheel.setW( - input.wheel.getW() + actual_wheel_alpha * input.context.dt); + input.wheel.setW(input.wheel.getW() + actual_wheel_alpha * input.context.dt); wheel_long_friction = F_friction_lon; // Resultant force: In local (x,y) coordinates (Newtons) wrt the Wheel // ----------------------------------------------------------------------- - const mrpt::math::TPoint2D result_force_wrt_wheel( - wheel_long_friction, wheel_lat_friction); + const mrpt::math::TPoint2D result_force_wrt_wheel(wheel_long_friction, wheel_lat_friction); // Rotate to put: Wheel frame ==> vehicle local framework: mrpt::math::TVector2D res; diff --git a/modules/simulator/src/JointXMLnode.h b/modules/simulator/src/JointXMLnode.h index 2f46337f..00d6e6cb 100644 --- a/modules/simulator/src/JointXMLnode.h +++ b/modules/simulator/src/JointXMLnode.h @@ -51,8 +51,7 @@ class JointXMLnode // ++it iterator& operator++() { - if (!current) - throw std::runtime_error("++ called on end() iterator!?"); + if (!current) throw std::runtime_error("++ called on end() iterator!?"); current = current->next_sibling(); JointXMLnode::TListNodes& lst = parent.getListOfNodes(); while (!current && lst_idx < lst.size()) @@ -68,30 +67,26 @@ class JointXMLnode rapidxml::xml_node* operator->() const { - if (!current) - throw std::runtime_error("-> called on end() iterator!?"); + if (!current) throw std::runtime_error("-> called on end() iterator!?"); return current; } rapidxml::xml_node* operator*() const { - if (!current) - throw std::runtime_error("* called on end() iterator!?"); + if (!current) throw std::runtime_error("* called on end() iterator!?"); return current; } bool operator==(const iterator& it) const { - return (this->current == it.current) && - (this->lst_idx == it.lst_idx) && + return (this->current == it.current) && (this->lst_idx == it.lst_idx) && (&this->parent == &it.parent); } bool operator!=(const iterator& it) const { return !(*this == it); } private: // begin(): - iterator(JointXMLnode& pa) - : parent(pa), lst_idx(0), current(nullptr) + iterator(JointXMLnode& pa) : parent(pa), lst_idx(0), current(nullptr) { JointXMLnode::TListNodes& lst = parent.getListOfNodes(); while (!current && lst_idx < lst.size()) @@ -101,10 +96,7 @@ class JointXMLnode } } // end() - iterator(JointXMLnode& pa, size_t idx) - : parent(pa), lst_idx(idx), current(nullptr) - { - } + iterator(JointXMLnode& pa, size_t idx) : parent(pa), lst_idx(idx), current(nullptr) {} JointXMLnode& parent; size_t lst_idx; // => lst.size() means this is "end()" diff --git a/modules/simulator/src/Joystick.cpp b/modules/simulator/src/Joystick.cpp index 8e1918bc..daa48991 100644 --- a/modules/simulator/src/Joystick.cpp +++ b/modules/simulator/src/Joystick.cpp @@ -88,9 +88,7 @@ int Joystick::getJoysticksCount() do { - if (-1 != - (joy_fd = open( - mrpt::format("/dev/input/js%i", nJoys).c_str(), O_RDONLY))) + if (-1 != (joy_fd = open(mrpt::format("/dev/input/js%i", nJoys).c_str(), O_RDONLY))) { nJoys++; close(joy_fd); @@ -161,8 +159,7 @@ bool Joystick::getJoystickPosition(int nJoy, State& output) if (m_joy_fd != -1) close(m_joy_fd); // Go, try open joystick: - if ((m_joy_fd = open( - mrpt::format("/dev/input/js%i", nJoy).c_str(), O_RDONLY)) < 0) + if ((m_joy_fd = open(mrpt::format("/dev/input/js%i", nJoy).c_str(), O_RDONLY)) < 0) return false; // Perfect! @@ -174,8 +171,7 @@ bool Joystick::getJoystickPosition(int nJoy, State& output) struct js_event js; - while (read(m_joy_fd, &js, sizeof(struct js_event)) == - sizeof(struct js_event)) + while (read(m_joy_fd, &js, sizeof(struct js_event)) == sizeof(struct js_event)) { // js.number: Button number const size_t jsNum = static_cast(js.number); @@ -183,8 +179,7 @@ bool Joystick::getJoystickPosition(int nJoy, State& output) // Button? if (js.type & JS_EVENT_BUTTON) { - if (m_joystate_btns.size() < jsNum + 1) - m_joystate_btns.resize(jsNum + 1); + if (m_joystate_btns.size() < jsNum + 1) m_joystate_btns.resize(jsNum + 1); m_joystate_btns[jsNum] = (js.value != 0); } @@ -192,8 +187,7 @@ bool Joystick::getJoystickPosition(int nJoy, State& output) // Axes? if (js.type & JS_EVENT_AXIS) { - if (m_joystate_axes.size() < jsNum + 1) - m_joystate_axes.resize(jsNum + 1); + if (m_joystate_axes.size() < jsNum + 1) m_joystate_axes.resize(jsNum + 1); m_joystate_axes[jsNum] = static_cast(js.value); } @@ -218,8 +212,7 @@ bool Joystick::getJoystickPosition(int nJoy, State& output) { output.axes_raw[i] = m_joystate_axes[i]; - const int calib_min = - m_minPerAxis.size() > i ? m_minPerAxis[i] : -32767; + const int calib_min = m_minPerAxis.size() > i ? m_minPerAxis[i] : -32767; const int calib_max = m_maxPerAxis.size() > i ? m_maxPerAxis[i] : 32767; output.axes[i] = -1.0f + 2.0f * (m_joystate_axes[i] - calib_min) / @@ -234,8 +227,7 @@ bool Joystick::getJoystickPosition(int nJoy, State& output) MRPT_END } -void Joystick::setLimits( - const std::vector& minPerAxis, const std::vector& maxPerAxis) +void Joystick::setLimits(const std::vector& minPerAxis, const std::vector& maxPerAxis) { m_minPerAxis = minPerAxis; m_maxPerAxis = maxPerAxis; diff --git a/modules/simulator/src/ModelsCache.cpp b/modules/simulator/src/ModelsCache.cpp index 0af06107..792dd761 100644 --- a/modules/simulator/src/ModelsCache.cpp +++ b/modules/simulator/src/ModelsCache.cpp @@ -28,8 +28,7 @@ mrpt::opengl::CAssimpModel::Ptr ModelsCache::get( const std::string& localFileName, const Options& options) { // already cached? - if (auto it = cache.find(localFileName); it != cache.end()) - return it->second; + if (auto it = cache.find(localFileName); it != cache.end()) return it->second; // No, it's a new model path, create its placeholder: auto m = cache[localFileName] = mrpt::opengl::CAssimpModel::Create(); @@ -50,8 +49,7 @@ mrpt::opengl::CAssimpModel::Ptr ModelsCache::get( m->loadScene(localFileName, loadFlags); - m->cullFaces(mrpt::typemeta::TEnumType::name2value( - options.modelCull)); + m->cullFaces(mrpt::typemeta::TEnumType::name2value(options.modelCull)); return m; } diff --git a/modules/simulator/src/ModelsCache.h b/modules/simulator/src/ModelsCache.h index e4dfdd00..433591a1 100644 --- a/modules/simulator/src/ModelsCache.h +++ b/modules/simulator/src/ModelsCache.h @@ -24,8 +24,7 @@ class ModelsCache std::string modelCull = "NONE"; }; - mrpt::opengl::CAssimpModel::Ptr get( - const std::string& url, const Options& options); + mrpt::opengl::CAssimpModel::Ptr get(const std::string& url, const Options& options); void clear() { cache.clear(); } diff --git a/modules/simulator/src/PID_Controller.cpp b/modules/simulator/src/PID_Controller.cpp index 43491472..df437589 100644 --- a/modules/simulator/src/PID_Controller.cpp +++ b/modules/simulator/src/PID_Controller.cpp @@ -18,8 +18,8 @@ double PID_Controller::compute(double err, double dt) e_n_1 = e_n; e_n = err; - double output = lastOutput + KP * (e_n - e_n_1) + KI * e_n * dt + - KD * (e_n - 2 * e_n_1 + e_n_2) / dt; + double output = + lastOutput + KP * (e_n - e_n_1) + KI * e_n * dt + KD * (e_n - 2 * e_n_1 + e_n_2) / dt; // prevent integral windup if (max_out != 0.0 && (output < -max_out || output > max_out)) diff --git a/modules/simulator/src/RemoteResourcesManager.cpp b/modules/simulator/src/RemoteResourcesManager.cpp index e7f02d6d..7a30e8f9 100644 --- a/modules/simulator/src/RemoteResourcesManager.cpp +++ b/modules/simulator/src/RemoteResourcesManager.cpp @@ -57,8 +57,8 @@ std::string RemoteResourcesManager::cache_directory() return local_directory; } -std::tuple - RemoteResourcesManager::zip_uri_split(const std::string& uri) +std::tuple RemoteResourcesManager::zip_uri_split( + const std::string& uri) { auto pos = uri.find(".zip/"); @@ -94,15 +94,13 @@ std::string RemoteResourcesManager::handle_remote_uri(const std::string& uri) { // get usage stats: if (0 == mrpt::system::executeCommand( - mrpt::format("du -sh0 \"%s\"", localDir.c_str()), - &cacheUsageStats)) + mrpt::format("du -sh0 \"%s\"", localDir.c_str()), &cacheUsageStats)) { // usage stats are valid. } } MRPT_LOG_ONCE_INFO( - "Using local storage directory: '"s + localDir + "' (Usage: "s + - cacheUsageStats + ")"s); + "Using local storage directory: '"s + localDir + "' (Usage: "s + cacheUsageStats + ")"s); const auto [isZipPkg, zipOrFileURI, internalURI] = zip_uri_split(uri); @@ -117,11 +115,10 @@ std::string RemoteResourcesManager::handle_remote_uri(const std::string& uri) // Download if it does not exist already from a past download: if (!mrpt::system::fileExists(localFil)) { - const auto cmd = mrpt::format( - "wget -q -O \"%s\" %s", localFil.c_str(), zipOrFileURI.c_str()); + const auto cmd = + mrpt::format("wget -q -O \"%s\" %s", localFil.c_str(), zipOrFileURI.c_str()); - MRPT_LOG_INFO_STREAM( - "Downloading remote resources from: '" << uri << "'"); + MRPT_LOG_INFO_STREAM("Downloading remote resources from: '" << uri << "'"); if (int ret = ::system(cmd.c_str()); ret != 0) { @@ -156,16 +153,14 @@ std::string RemoteResourcesManager::handle_local_zip_package( ASSERT_EQUAL_(filExtension, "zip"); // already decompressed? - const auto zipOutDir = - mrpt::system::fileNameChangeExtension(localZipFil, "out"); + const auto zipOutDir = mrpt::system::fileNameChangeExtension(localZipFil, "out"); if (!mrpt::system::directoryExists(zipOutDir)) { mrpt::system::createDirectory(zipOutDir); ASSERT_DIRECTORY_EXISTS_(zipOutDir); - const std::string cmd = - "unzip -q \""s + localZipFil + "\" -d \""s + zipOutDir + "\""s; + const std::string cmd = "unzip -q \""s + localZipFil + "\" -d \""s + zipOutDir + "\""s; if (int ret = ::system(cmd.c_str()); ret != 0) { diff --git a/modules/simulator/src/Sensors/CameraSensor.cpp b/modules/simulator/src/Sensors/CameraSensor.cpp index 959d407f..f02076c6 100644 --- a/modules/simulator/src/Sensors/CameraSensor.cpp +++ b/modules/simulator/src/Sensors/CameraSensor.cpp @@ -22,8 +22,7 @@ using namespace mvsim; using namespace rapidxml; -CameraSensor::CameraSensor( - Simulable& parent, const rapidxml::xml_node* root) +CameraSensor::CameraSensor(Simulable& parent, const rapidxml::xml_node* root) : SensorBase(parent) { this->loadConfigFrom(root); @@ -41,8 +40,7 @@ void CameraSensor::loadConfigFrom(const rapidxml::xml_node* root) fbo_renderer_rgb_.reset(); using namespace mrpt; // _deg - sensor_params_.cameraPose = - mrpt::poses::CPose3D(0, 0, 0.5, 90.0_deg, 0, 90.0_deg); + sensor_params_.cameraPose = mrpt::poses::CPose3D(0, 0, 0.5, 90.0_deg, 0, 90.0_deg); // Default values: { @@ -84,8 +82,7 @@ void CameraSensor::loadConfigFrom(const rapidxml::xml_node* root) void CameraSensor::internalGuiUpdate( const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& - physical, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { if (!gl_sensor_origin_ && viz) @@ -94,8 +91,7 @@ void CameraSensor::internalGuiUpdate( #if MRPT_VERSION >= 0x270 gl_sensor_origin_->castShadows(false); #endif - gl_sensor_origin_corner_ = - mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); + gl_sensor_origin_corner_ = mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); gl_sensor_origin_->insert(gl_sensor_origin_corner_); @@ -124,8 +120,8 @@ void CameraSensor::internalGuiUpdate( gl_sensor_frustum_ = mrpt::opengl::CSetOfObjects::Create(); const float frustumScale = 0.4e-3; - auto frustum = mrpt::opengl::CFrustum::Create( - last_obs2gui_->cameraParams, frustumScale); + auto frustum = + mrpt::opengl::CFrustum::Create(last_obs2gui_->cameraParams, frustumScale); gl_sensor_frustum_->insert(frustum); gl_sensor_fov_->insert(gl_sensor_frustum_); @@ -135,8 +131,7 @@ void CameraSensor::internalGuiUpdate( gl_sensor_frustum_->setPose( last_obs2gui_->cameraPose + - (-mrpt::poses::CPose3D::FromYawPitchRoll( - -90.0_deg, 0.0_deg, -90.0_deg))); + (-mrpt::poses::CPose3D::FromYawPitchRoll(-90.0_deg, 0.0_deg, -90.0_deg))); last_obs2gui_.reset(); } @@ -155,10 +150,7 @@ void CameraSensor::internalGuiUpdate( if (glCustomVisual_) glCustomVisual_->setPose(globalSensorPose); } -void CameraSensor::simul_pre_timestep( - [[maybe_unused]] const TSimulContext& context) -{ -} +void CameraSensor::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) {} void CameraSensor::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) { @@ -169,8 +161,7 @@ void CameraSensor::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) if (!has_to_render_.has_value()) return; } - auto tleWhole = - mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGB"); + auto tleWhole = mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGB"); if (glCustomVisual_) glCustomVisual_->setVisibility(false); @@ -217,8 +208,7 @@ void CameraSensor::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) ASSERT_(fbo_renderer_rgb_); - auto tle2 = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.RGB.render"); + auto tle2 = mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGB.render"); // viewport->setCustomBackgroundColor({0.3f, 0.3f, 0.3f, 1.0f}); viewport->setViewportClipDistances(rgbClipMin_, rgbClipMax_); diff --git a/modules/simulator/src/Sensors/DepthCameraSensor.cpp b/modules/simulator/src/Sensors/DepthCameraSensor.cpp index 2b3d7089..da970802 100644 --- a/modules/simulator/src/Sensors/DepthCameraSensor.cpp +++ b/modules/simulator/src/Sensors/DepthCameraSensor.cpp @@ -22,8 +22,7 @@ using namespace mvsim; using namespace rapidxml; -DepthCameraSensor::DepthCameraSensor( - Simulable& parent, const rapidxml::xml_node* root) +DepthCameraSensor::DepthCameraSensor(Simulable& parent, const rapidxml::xml_node* root) : SensorBase(parent) { DepthCameraSensor::loadConfigFrom(root); @@ -42,8 +41,7 @@ void DepthCameraSensor::loadConfigFrom(const rapidxml::xml_node* root) fbo_renderer_rgb_.reset(); using namespace mrpt; // _deg - sensor_params_.sensorPose = - mrpt::poses::CPose3D(0, 0, 0.5, 90.0_deg, 0, 90.0_deg); + sensor_params_.sensorPose = mrpt::poses::CPose3D(0, 0, 0.5, 90.0_deg, 0, 90.0_deg); // Default values: { @@ -113,8 +111,7 @@ void DepthCameraSensor::loadConfigFrom(const rapidxml::xml_node* root) void DepthCameraSensor::internalGuiUpdate( const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& - physical, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { mrpt::opengl::CSetOfObjects::Ptr glVizSensors; @@ -130,8 +127,7 @@ void DepthCameraSensor::internalGuiUpdate( { gl_obs_ = mrpt::opengl::CPointCloudColoured::Create(); gl_obs_->setPointSize(2.0f); - gl_obs_->setLocalRepresentativePoint( - sensor_params_.sensorPose.translation()); + gl_obs_->setLocalRepresentativePoint(sensor_params_.sensorPose.translation()); glVizSensors->insert(gl_obs_); } @@ -141,8 +137,7 @@ void DepthCameraSensor::internalGuiUpdate( #if MRPT_VERSION >= 0x270 gl_sensor_origin_->castShadows(false); #endif - gl_sensor_origin_corner_ = - mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); + gl_sensor_origin_corner_ = mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); gl_sensor_origin_->insert(gl_sensor_origin_corner_); @@ -179,8 +174,8 @@ void DepthCameraSensor::internalGuiUpdate( gl_sensor_frustum_ = mrpt::opengl::CSetOfObjects::Create(); const float frustumScale = 0.4e-3; - auto frustum = mrpt::opengl::CFrustum::Create( - last_obs2gui_->cameraParams, frustumScale); + auto frustum = + mrpt::opengl::CFrustum::Create(last_obs2gui_->cameraParams, frustumScale); gl_sensor_frustum_->insert(frustum); gl_sensor_fov_->insert(gl_sensor_frustum_); @@ -201,17 +196,12 @@ void DepthCameraSensor::internalGuiUpdate( if (gl_sensor_fov_) gl_sensor_fov_->setPose(p); if (gl_sensor_origin_) gl_sensor_origin_->setPose(p); - if (glCustomVisual_) - glCustomVisual_->setPose(p + sensor_params_.sensorPose.asTPose()); + if (glCustomVisual_) glCustomVisual_->setPose(p + sensor_params_.sensorPose.asTPose()); } -void DepthCameraSensor::simul_pre_timestep( - [[maybe_unused]] const TSimulContext& context) -{ -} +void DepthCameraSensor::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) {} -void DepthCameraSensor::simulateOn3DScene( - mrpt::opengl::COpenGLScene& world3DScene) +void DepthCameraSensor::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) { using namespace mrpt; // _deg @@ -220,11 +210,9 @@ void DepthCameraSensor::simulateOn3DScene( if (!has_to_render_.has_value()) return; } - auto tleWhole = - mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGBD"); + auto tleWhole = mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGBD"); - auto tle1 = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.RGBD.acqGuiMtx"); + auto tle1 = mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGBD.acqGuiMtx"); tle1.stop(); @@ -240,8 +228,8 @@ void DepthCameraSensor::simulateOn3DScene( // Create FBO on first use, now that we are here at the GUI / OpenGL thread. if (!fbo_renderer_rgb_ && sense_rgb_) { - auto tle2 = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.RGBD.createFBO"); + auto tle2 = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGBD.createFBO"); mrpt::opengl::CFBORender::Parameters p; p.width = sensor_params_.cameraParamsIntensity.ncols; @@ -253,8 +241,8 @@ void DepthCameraSensor::simulateOn3DScene( if (!fbo_renderer_depth_ && sense_depth_) { - auto tle2 = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.RGBD.createFBO"); + auto tle2 = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGBD.createFBO"); mrpt::opengl::CFBORender::Parameters p; p.width = sensor_params_.cameraParams.ncols; @@ -266,12 +254,8 @@ void DepthCameraSensor::simulateOn3DScene( auto viewport = world3DScene.getViewport(); - auto* camDepth = fbo_renderer_depth_ - ? &fbo_renderer_depth_->getCamera(world3DScene) - : nullptr; - auto* camRGB = fbo_renderer_rgb_ - ? &fbo_renderer_rgb_->getCamera(world3DScene) - : nullptr; + auto* camDepth = fbo_renderer_depth_ ? &fbo_renderer_depth_->getCamera(world3DScene) : nullptr; + auto* camRGB = fbo_renderer_rgb_ ? &fbo_renderer_rgb_->getCamera(world3DScene) : nullptr; const auto fixedAxisConventionRot = mrpt::poses::CPose3D(0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg); @@ -288,16 +272,15 @@ void DepthCameraSensor::simulateOn3DScene( const auto vehiclePose = mrpt::poses::CPose3D(vehicle_.getPose()); - const auto depthSensorPose = - vehiclePose + curObs.sensorPose + fixedAxisConventionRot; + const auto depthSensorPose = vehiclePose + curObs.sensorPose + fixedAxisConventionRot; const auto rgbSensorPose = vehiclePose + curObs.sensorPose + curObs.relativePoseIntensityWRTDepth; if (fbo_renderer_rgb_) { - auto tle2 = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.RGBD.renderRGB"); + auto tle2 = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGBD.renderRGB"); camRGB->set6DOFMode(true); camRGB->setProjectiveFromPinhole(curObs.cameraParamsIntensity); @@ -320,8 +303,7 @@ void DepthCameraSensor::simulateOn3DScene( // ---------------------------------------------------------- if (fbo_renderer_depth_) { - auto tle2 = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.RGBD.renderD"); + auto tle2 = mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGBD.renderD"); camDepth->setProjectiveFromPinhole(curObs.cameraParams); @@ -334,8 +316,8 @@ void DepthCameraSensor::simulateOn3DScene( // viewport->setCustomBackgroundColor({0.3f, 0.3f, 0.3f, 1.0f}); viewport->setViewportClipDistances(depth_clip_min_, depth_clip_max_); - auto tle2c = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.RGBD.renderD_core"); + auto tle2c = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGBD.renderD_core"); fbo_renderer_depth_->render_depth(world3DScene, depthImage_); @@ -345,14 +327,13 @@ void DepthCameraSensor::simulateOn3DScene( curObs.hasRangeImage = true; curObs.range_is_depth = true; - auto tle2cnv = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.RGBD.renderD_cast"); + auto tle2cnv = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGBD.renderD_cast"); // float -> uint16_t with "curObs.rangeUnits" units: curObs.rangeImage_setSize(depthImage_.rows(), depthImage_.cols()); - curObs.rangeImage = (depthImage_.asEigen().cwiseMin(curObs.maxRange) / - curObs.rangeUnits) - .cast(); + curObs.rangeImage = + (depthImage_.asEigen().cwiseMin(curObs.maxRange) / curObs.rangeUnits).cast(); tle2cnv.stop(); @@ -370,8 +351,7 @@ void DepthCameraSensor::simulateOn3DScene( for (size_t i = 0; i < noiseLen; i++) { noiseSeq.push_back(static_cast(mrpt::round( - rng.drawGaussian1D(0.0, depth_noise_sigma_) / - curObs.rangeUnits))); + rng.drawGaussian1D(0.0, depth_noise_sigma_) / curObs.rangeUnits))); } } @@ -381,15 +361,13 @@ void DepthCameraSensor::simulateOn3DScene( uint16_t* d = curObs.rangeImage.data(); const size_t N = curObs.rangeImage.size(); - const int16_t maxRangeInts = - static_cast(curObs.maxRange / curObs.rangeUnits); + const int16_t maxRangeInts = static_cast(curObs.maxRange / curObs.rangeUnits); for (size_t i = 0; i < N; i++) { if (d[i] == 0) continue; // it was an invalid ray return. - const int16_t dNoisy = - static_cast(d[i]) + noiseSeq[noiseIdx++]; + const int16_t dNoisy = static_cast(d[i]) + noiseSeq[noiseIdx++]; if (noiseIdx >= noiseLen) noiseIdx = 0; @@ -406,8 +384,8 @@ void DepthCameraSensor::simulateOn3DScene( // Store generated obs: { - auto tle3 = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.RGBD.acqObsMtx"); + auto tle3 = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGBD.acqObsMtx"); std::lock_guard csl(last_obs_cs_); last_obs_ = std::move(curObsPtr); @@ -417,8 +395,7 @@ void DepthCameraSensor::simulateOn3DScene( { auto lckHasTo = mrpt::lockHelper(has_to_render_mtx_); - auto tlePub = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.RGBD.report"); + auto tlePub = mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGBD.report"); SensorBase::reportNewObservation(last_obs_, *has_to_render_); @@ -447,8 +424,7 @@ void DepthCameraSensor::simul_post_timestep(const TSimulContext& context) Simulable::setPose(globalSensorPose, false /*do not notify*/); } -void DepthCameraSensor::notifySimulableSetPose( - const mrpt::math::TPose3D& newPose) +void DepthCameraSensor::notifySimulableSetPose(const mrpt::math::TPose3D& newPose) { // The editor has moved the sensor in global coordinates. // Convert back to local: diff --git a/modules/simulator/src/Sensors/IMU.cpp b/modules/simulator/src/Sensors/IMU.cpp index 6605c201..a064701b 100644 --- a/modules/simulator/src/Sensors/IMU.cpp +++ b/modules/simulator/src/Sensors/IMU.cpp @@ -23,8 +23,7 @@ using namespace mvsim; using namespace rapidxml; -IMU::IMU(Simulable& parent, const rapidxml::xml_node* root) - : SensorBase(parent) +IMU::IMU(Simulable& parent, const rapidxml::xml_node* root) : SensorBase(parent) { IMU::loadConfigFrom(root); } @@ -40,10 +39,8 @@ void IMU::loadConfigFrom(const rapidxml::xml_node* root) params["pose"] = TParamEntry("%pose2d_ptr3d", &obs_model_.sensorPose); params["pose_3d"] = TParamEntry("%pose3d", &obs_model_.sensorPose); params["sensor_period"] = TParamEntry("%lf", &sensor_period_); - params["angular_velocity_std_noise"] = - TParamEntry("%lf", &angularVelocityStdNoise_); - params["linear_acceleration_std_noise"] = - TParamEntry("%lf", &linearAccelerationStdNoise_); + params["angular_velocity_std_noise"] = TParamEntry("%lf", &angularVelocityStdNoise_); + params["linear_acceleration_std_noise"] = TParamEntry("%lf", &linearAccelerationStdNoise_); // Parse XML params: parse_xmlnode_children_as_param(*root, params, varValues_); @@ -54,8 +51,7 @@ void IMU::loadConfigFrom(const rapidxml::xml_node* root) void IMU::internalGuiUpdate( const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& - physical, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { // 1st time? @@ -65,8 +61,7 @@ void IMU::internalGuiUpdate( #if MRPT_VERSION >= 0x270 gl_sensor_origin_->castShadows(false); #endif - gl_sensor_origin_corner_ = - mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); + gl_sensor_origin_corner_ = mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); gl_sensor_origin_->insert(gl_sensor_origin_corner_); @@ -124,9 +119,7 @@ void IMU::internal_simulate_imu(const TSimulContext& context) rng_.drawGaussian1DVector(linAccNoise, 0.0, linearAccelerationStdNoise_); const mrpt::math::TVector3D linAccLocal = - vehicle_.getPose().inverseComposePoint( - vehicle_.getLinearAcceleration() + g) + - linAccNoise; + vehicle_.getPose().inverseComposePoint(vehicle_.getLinearAcceleration() + g) + linAccNoise; outObs->set(mrpt::obs::IMU_X_ACC, linAccLocal.x); outObs->set(mrpt::obs::IMU_Y_ACC, linAccLocal.y); diff --git a/modules/simulator/src/Sensors/LaserScanner.cpp b/modules/simulator/src/Sensors/LaserScanner.cpp index 4546a719..d2173a00 100644 --- a/modules/simulator/src/Sensors/LaserScanner.cpp +++ b/modules/simulator/src/Sensors/LaserScanner.cpp @@ -28,8 +28,7 @@ using namespace rapidxml; int z_order_cnt = 0; -LaserScanner::LaserScanner( - Simulable& parent, const rapidxml::xml_node* root) +LaserScanner::LaserScanner(Simulable& parent, const rapidxml::xml_node* root) : SensorBase(parent), z_order_(++z_order_cnt) { LaserScanner::loadConfigFrom(root); @@ -85,8 +84,7 @@ void LaserScanner::loadConfigFrom(const rapidxml::xml_node* root) void LaserScanner::internalGuiUpdate( const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& - physical, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { using namespace std::string_literals; @@ -111,14 +109,12 @@ void LaserScanner::internalGuiUpdate( gl_scan_->enableSurface(viz_visiblePlane_); const mrpt::img::TColorf planeCol(viz_planeColor_); - gl_scan_->setSurfaceColor( - planeCol.R, planeCol.G, planeCol.B, planeCol.A); + gl_scan_->setSurfaceColor(planeCol.R, planeCol.G, planeCol.B, planeCol.A); gl_scan_->enableLine(viz_visibleLines_); gl_scan_->setLocalRepresentativePoint({0, 0, 0.10f}); - gl_scan_->setName( - "glScan veh:"s + vehicle_.getName() + " sensor:"s + this->name_); + gl_scan_->setName("glScan veh:"s + vehicle_.getName() + " sensor:"s + this->name_); glVizSensors->insert(gl_scan_); } @@ -128,8 +124,7 @@ void LaserScanner::internalGuiUpdate( #if MRPT_VERSION >= 0x270 gl_sensor_origin_->castShadows(false); #endif - gl_sensor_origin_corner_ = - mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); + gl_sensor_origin_corner_ = mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); gl_sensor_origin_->insert(gl_sensor_origin_corner_); @@ -184,8 +179,8 @@ void LaserScanner::internalGuiUpdate( const double z_offset = 1e-3; if (gl_scan_) - gl_scan_->setPose(mrpt::poses::CPose3D( - p.x(), p.y(), z_offset + z_incrs * z_order_, p.phi(), 0.0, 0.0)); + gl_scan_->setPose( + mrpt::poses::CPose3D(p.x(), p.y(), z_offset + z_incrs * z_order_, p.phi(), 0.0, 0.0)); if (gl_sensor_fov_) gl_sensor_fov_->setPose(p); @@ -194,10 +189,7 @@ void LaserScanner::internalGuiUpdate( if (glCustomVisual_) glCustomVisual_->setPose(p + scan_model_.sensorPose); } -void LaserScanner::simul_pre_timestep( - [[maybe_unused]] const TSimulContext& context) -{ -} +void LaserScanner::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) {} // Simulate sensor AFTER timestep, with the updated vehicle dynamical state: void LaserScanner::simul_post_timestep(const TSimulContext& context) @@ -249,8 +241,7 @@ void LaserScanner::internal_simulate_lidar_2d_mode(const TSimulContext& context) using mrpt::maps::COccupancyGridMap2D; using mrpt::obs::CObservation2DRangeScan; - auto tle = - mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "LaserScanner"); + auto tle = mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "LaserScanner"); // Create an array of scans, each reflecting ranges to one kind of world // objects. @@ -272,8 +263,7 @@ void LaserScanner::internal_simulate_lidar_2d_mode(const TSimulContext& context) for (const auto& element : elements) { // If not a grid map, ignore: - const OccupancyGridMap* grid = - dynamic_cast(element.get()); + const OccupancyGridMap* grid = dynamic_cast(element.get()); if (!grid) continue; const COccupancyGridMap2D& occGrid = grid->getOccGrid(); @@ -283,8 +273,7 @@ void LaserScanner::internal_simulate_lidar_2d_mode(const TSimulContext& context) // Ray tracing over the gridmap: occGrid.laserScanSimulator( - scan, vehPose, 0.5f, scan_model_.getScanSize(), rangeStdNoise_, 1, - angleStdNoise_); + scan, vehPose, 0.5f, scan_model_.getScanSize(), rangeStdNoise_, 1, angleStdNoise_); } world_->getTimeLogger().leave("LaserScanner.scan.1.gridmap"); @@ -307,8 +296,7 @@ void LaserScanner::internal_simulate_lidar_2d_mode(const TSimulContext& context) }; auto undoInvisibleFixtures = [&]() { - for (auto& kv : orgUserData) - kv.first->GetUserData().pointer = kv.second; + for (auto& kv : orgUserData) kv.first->GetUserData().pointer = kv.second; }; if (auto v = dynamic_cast(&vehicle_); v) @@ -329,8 +317,7 @@ void LaserScanner::internal_simulate_lidar_2d_mode(const TSimulContext& context) b2Fixture* fixture, const b2Vec2& point, const b2Vec2& normal, float fraction) override { - if (!see_fixtures_ || fixture->GetUserData().pointer == - INVISIBLE_FIXTURE_USER_DATA) + if (!see_fixtures_ || fixture->GetUserData().pointer == INVISIBLE_FIXTURE_USER_DATA) { // By returning -1, we instruct the calling code to ignore // this fixture and @@ -356,8 +343,7 @@ void LaserScanner::internal_simulate_lidar_2d_mode(const TSimulContext& context) b2Vec2 normal_{0, 0}; }; - const mrpt::poses::CPose2D sensorPose = - vehPose + mrpt::poses::CPose2D(scan.sensorPose); + const mrpt::poses::CPose2D sensorPose = vehPose + mrpt::poses::CPose2D(scan.sensorPose); const b2Vec2 sensorPt = b2Vec2(sensorPose.x(), sensorPose.y()); RayCastClosestCallback callback; @@ -366,18 +352,16 @@ void LaserScanner::internal_simulate_lidar_2d_mode(const TSimulContext& context) // Scan size: ASSERT_(nRays >= 2); scan.resizeScanAndAssign(nRays, maxRange, false); - double A = - sensorPose.phi() + (scan.rightToLeft ? -0.5 : +0.5) * scan.aperture; - const double AA = - (scan.rightToLeft ? 1.0 : -1.0) * (scan.aperture / (nRays - 1)); + double A = sensorPose.phi() + (scan.rightToLeft ? -0.5 : +0.5) * scan.aperture; + const double AA = (scan.rightToLeft ? 1.0 : -1.0) * (scan.aperture / (nRays - 1)); // Each thread must create its own rng: thread_local mrpt::random::CRandomGenerator rnd; for (size_t i = 0; i < nRays; i++, A += AA) { - const b2Vec2 endPt = b2Vec2( - sensorPt.x + cos(A) * maxRange, sensorPt.y + sin(A) * maxRange); + const b2Vec2 endPt = + b2Vec2(sensorPt.x + cos(A) * maxRange, sensorPt.y + sin(A) * maxRange); callback.hit_ = false; world_->getBox2DWorld()->RayCast(&callback, sensorPt, endPt); @@ -422,8 +406,7 @@ void LaserScanner::internal_simulate_lidar_2d_mode(const TSimulContext& context) if (scan.getScanRangeValidity(i)) { lastScan->setScanRange( - i, - std::min(lastScan->getScanRange(i), scan.getScanRange(i))); + i, std::min(lastScan->getScanRange(i), scan.getScanRange(i))); lastScan->setScanRangeValidity(i, true); } } @@ -460,11 +443,9 @@ void LaserScanner::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) if (!has_to_render_.has_value()) return; } - auto tleWhole = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.2Dlidar"); + auto tleWhole = mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.2Dlidar"); - auto tle1 = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.2Dlidar.acqGuiMtx"); + auto tle1 = mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.2Dlidar.acqGuiMtx"); tle1.stop(); @@ -521,10 +502,9 @@ void LaserScanner::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) const bool scanIsCW = (lastAngle > firstAngle); ASSERT_NEAR_(std::abs(lastAngle - firstAngle), curObs->aperture, 1e-3); - const unsigned int numRenders = - std::ceil((curObs->aperture / camModel_FOV) - 1e-3); - const auto numRaysPerRender = mrpt::round( - nRays * std::min(1.0, (camModel_FOV / curObs->aperture))); + const unsigned int numRenders = std::ceil((curObs->aperture / camModel_FOV) - 1e-3); + const auto numRaysPerRender = + mrpt::round(nRays * std::min(1.0, (camModel_FOV / curObs->aperture))); ASSERT_(numRaysPerRender > 0); @@ -541,12 +521,10 @@ void LaserScanner::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) for (int i = 0; i < numRaysPerRender; i++) { const auto ang = (scanIsCW ? -1 : 1) * - (camModel_FOV * 0.5 - - i * camModel_FOV / (numRaysPerRender - 1)); + (camModel_FOV * 0.5 - i * camModel_FOV / (numRaysPerRender - 1)); const auto pixelIdx = mrpt::saturate_val( - mrpt::round(camModel.cx() - camModel.fx() * std::tan(ang)), 0, - camModel.ncols - 1); + mrpt::round(camModel.cx() - camModel.fx() * std::tan(ang)), 0, camModel.ncols - 1); angleIdx2pixelIdx_.at(i) = pixelIdx; angleIdx2secant_.at(i) = 1.0f / std::cos(ang); @@ -591,21 +569,20 @@ void LaserScanner::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) for (size_t renderIdx = 0; renderIdx < numRenders; renderIdx++) { const double thisRenderMidAngle = - firstAngle + (camModel_FOV / 2.0 + camModel_FOV * renderIdx) * - (scanIsCW ? 1 : -1); + firstAngle + (camModel_FOV / 2.0 + camModel_FOV * renderIdx) * (scanIsCW ? 1 : -1); - const auto depthSensorPose = vehiclePose + curObs->sensorPose + - mrpt::poses::CPose3D::FromYawPitchRoll( - thisRenderMidAngle, 0.0, 0.0) + - fixedAxisConventionRot; + const auto depthSensorPose = + vehiclePose + curObs->sensorPose + + mrpt::poses::CPose3D::FromYawPitchRoll(thisRenderMidAngle, 0.0, 0.0) + + fixedAxisConventionRot; // Camera pose: vehicle + relativePoseOnVehicle: // Note: relativePoseOnVehicle should be (y,p,r)=(90deg,0,90deg) to make // the camera to look forward: cam.setPose(depthSensorPose); - auto tleRender = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.2Dlidar.renderSubScan"); + auto tleRender = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.2Dlidar.renderSubScan"); fbo_renderer_depth_->render_depth(world3DScene, depthImage); @@ -614,8 +591,8 @@ void LaserScanner::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) // Add random noise: if (rangeStdNoise_ > 0) { - auto tleStore = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.2Dlidar.noise"); + auto tleStore = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.2Dlidar.noise"); // Each thread must create its own rng: thread_local mrpt::random::CRandomGenerator rng; @@ -626,8 +603,7 @@ void LaserScanner::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) { if (d[i] == 0) continue; // it was an invalid ray return. - const float dNoisy = - d[i] + rng.drawGaussian1D(0, rangeStdNoise_); + const float dNoisy = d[i] + rng.drawGaussian1D(0, rangeStdNoise_); if (dNoisy < 0 || dNoisy > curObs->maxRange) continue; @@ -635,8 +611,8 @@ void LaserScanner::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) } } - auto tleStore = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.2Dlidar.storeObs"); + auto tleStore = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.2Dlidar.storeObs"); // Convert depth into range and store into scan observation: for (int i = 0; i < numRaysPerRender; i++) @@ -670,8 +646,8 @@ void LaserScanner::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) // Store generated obs: { - auto tle3 = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.2Dlidar.acqObsMtx"); + auto tle3 = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.2Dlidar.acqObsMtx"); std::lock_guard csl(last_scan_cs_); last_scan_ = std::move(curObs); @@ -681,8 +657,8 @@ void LaserScanner::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) { auto lckHasTo = mrpt::lockHelper(has_to_render_mtx_); - auto tlePub = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.2Dlidar.report"); + auto tlePub = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.2Dlidar.report"); // publish as generic Protobuf (mrpt serialized) object: SensorBase::reportNewObservation(last_scan_, *has_to_render_); @@ -709,7 +685,6 @@ void LaserScanner::registerOnServer(mvsim::Client& c) #if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF) // Topic: if (!publishTopic_.empty()) - c.advertiseTopic( - publishTopic_ + "_scan"s); + c.advertiseTopic(publishTopic_ + "_scan"s); #endif } diff --git a/modules/simulator/src/Sensors/Lidar3D.cpp b/modules/simulator/src/Sensors/Lidar3D.cpp index 2409e51e..2fe5eaf1 100644 --- a/modules/simulator/src/Sensors/Lidar3D.cpp +++ b/modules/simulator/src/Sensors/Lidar3D.cpp @@ -39,8 +39,7 @@ using namespace rapidxml; // TODO(jlbc): Also store obs as CObservationRotatingScan?? -Lidar3D::Lidar3D(Simulable& parent, const rapidxml::xml_node* root) - : SensorBase(parent) +Lidar3D::Lidar3D(Simulable& parent, const rapidxml::xml_node* root) : SensorBase(parent) { Lidar3D::loadConfigFrom(root); } @@ -65,15 +64,12 @@ void Lidar3D::loadConfigFrom(const rapidxml::xml_node* root) params["ignore_parent_body"] = TParamEntry("%bool", &ignore_parent_body_); params["vert_fov_degrees"] = TParamEntry("%lf", &vertical_fov_); - params["vertical_ray_angles"] = - TParamEntry("%s", &vertical_ray_angles_str_); + params["vertical_ray_angles"] = TParamEntry("%s", &vertical_ray_angles_str_); params["vert_nrays"] = TParamEntry("%i", &vertNumRays_); params["horz_nrays"] = TParamEntry("%i", &horzNumRays_); - params["horz_resolution_factor"] = - TParamEntry("%lf", &horzResolutionFactor_); - params["vert_resolution_factor"] = - TParamEntry("%lf", &vertResolutionFactor_); + params["horz_resolution_factor"] = TParamEntry("%lf", &horzResolutionFactor_); + params["vert_resolution_factor"] = TParamEntry("%lf", &vertResolutionFactor_); params["max_vert_relative_depth_to_interpolatate"] = TParamEntry("%f", &maxDepthInterpolationStepVert_); @@ -86,8 +82,7 @@ void Lidar3D::loadConfigFrom(const rapidxml::xml_node* root) void Lidar3D::internalGuiUpdate( const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& - physical, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { mrpt::opengl::CSetOfObjects::Ptr glVizSensors; @@ -113,8 +108,7 @@ void Lidar3D::internalGuiUpdate( #if MRPT_VERSION >= 0x270 gl_sensor_origin_->castShadows(false); #endif - gl_sensor_origin_corner_ = - mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); + gl_sensor_origin_corner_ = mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); gl_sensor_origin_->insert(gl_sensor_origin_corner_); @@ -157,9 +151,7 @@ void Lidar3D::internalGuiUpdate( if (glCustomVisual_) glCustomVisual_->setPose(p); } -void Lidar3D::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) -{ -} +void Lidar3D::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) {} // Simulate sensor AFTER timestep, with the updated vehicle dynamical state: void Lidar3D::simul_post_timestep(const TSimulContext& context) @@ -209,15 +201,12 @@ void Lidar3D::freeOpenGLResources() // since only a few depth points are actually used: // (older mrpt versions already returned the linearized depth) constexpr int DEPTH_LOG2LIN_BITS = 20; -using depth_log2lin_t = - mrpt::opengl::OpenGLDepth2LinearLUTs; +using depth_log2lin_t = mrpt::opengl::OpenGLDepth2LinearLUTs; #endif static float safeInterpolateRangeImage( - const mrpt::math::CMatrixFloat& depthImage, - const float maxDepthInterpolationStepVert, - const float maxDepthInterpolationStepHorz, const int NCOLS, const int NROWS, - float v, float u + const mrpt::math::CMatrixFloat& depthImage, const float maxDepthInterpolationStepVert, + const float maxDepthInterpolationStepHorz, const int NCOLS, const int NROWS, float v, float u #if MRPT_VERSION >= 0x270 , const depth_log2lin_t::lut_t& depth_log2lin_lut @@ -243,14 +232,10 @@ static float safeInterpolateRangeImage( // since only a few depth points are actually used: // map d in [-1.0f,+1.0f] ==> real depth values: - const float d00 = depth_log2lin_lut - [(raw_d00 + 1.0f) * (depth_log2lin_t::NUM_ENTRIES - 1) / 2]; - const float d01 = depth_log2lin_lut - [(raw_d01 + 1.0f) * (depth_log2lin_t::NUM_ENTRIES - 1) / 2]; - const float d10 = depth_log2lin_lut - [(raw_d10 + 1.0f) * (depth_log2lin_t::NUM_ENTRIES - 1) / 2]; - const float d11 = depth_log2lin_lut - [(raw_d11 + 1.0f) * (depth_log2lin_t::NUM_ENTRIES - 1) / 2]; + const float d00 = depth_log2lin_lut[(raw_d00 + 1.0f) * (depth_log2lin_t::NUM_ENTRIES - 1) / 2]; + const float d01 = depth_log2lin_lut[(raw_d01 + 1.0f) * (depth_log2lin_t::NUM_ENTRIES - 1) / 2]; + const float d10 = depth_log2lin_lut[(raw_d10 + 1.0f) * (depth_log2lin_t::NUM_ENTRIES - 1) / 2]; + const float d11 = depth_log2lin_lut[(raw_d11 + 1.0f) * (depth_log2lin_t::NUM_ENTRIES - 1) / 2]; #else // "d" is already linear depth const float d00 = raw_d00; @@ -309,11 +294,9 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) if (!has_to_render_.has_value()) return; } - auto tleWhole = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.3Dlidar"); + auto tleWhole = mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.3Dlidar"); - auto tle1 = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.3Dlidar.acqGuiMtx"); + auto tle1 = mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.3Dlidar.acqGuiMtx"); tle1.stop(); @@ -340,8 +323,8 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) // This FBO is for camModel_hFOV only: // Minimum horz resolution=360deg /120 deg - const int FBO_NCOLS = mrpt::round( - horzResolutionFactor_ * horzNumRays_ / (2 * M_PI / camModel_hFOV)); + const int FBO_NCOLS = + mrpt::round(horzResolutionFactor_ * horzNumRays_ / (2 * M_PI / camModel_hFOV)); mrpt::img::TCamera camModel; camModel.ncols = FBO_NCOLS; @@ -358,18 +341,15 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) vertical_ray_angles_.resize(vertNumRays_); for (int i = 0; i < vertNumRays_; i++) { - vertical_ray_angles_[i] = - vertical_fov_ * (-0.5 + i * 1.0 / (vertNumRays_ - 1)); + vertical_ray_angles_[i] = vertical_fov_ * (-0.5 + i * 1.0 / (vertNumRays_ - 1)); } } else { // custom distribution: std::vector vertAnglesStrs; - mrpt::system::tokenize( - vertical_ray_angles_str_, " \t\r\n", vertAnglesStrs); - ASSERT_EQUAL_( - vertAnglesStrs.size(), static_cast(vertNumRays_)); + mrpt::system::tokenize(vertical_ray_angles_str_, " \t\r\n", vertAnglesStrs); + ASSERT_EQUAL_(vertAnglesStrs.size(), static_cast(vertNumRays_)); std::set angs; for (const auto& s : vertAnglesStrs) angs.insert(std::stod(s)); ASSERT_EQUAL_(angs.size(), static_cast(vertNumRays_)); @@ -386,12 +366,10 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) const double vertFOVMax = vertical_ray_angles_.back(); const double vertFOVMin = std::abs(vertical_ray_angles_.front()); - const int FBO_NROWS_UP = - vertResolutionFactor_ * tan(vertFOVMax) * - sqrt(square(camModel.fx()) + square(camModel.cx())); - const int FBO_NROWS_DOWN = - vertResolutionFactor_ * tan(vertFOVMin) * - sqrt(square(camModel.fx()) + square(camModel.cx())); + const int FBO_NROWS_UP = vertResolutionFactor_ * tan(vertFOVMax) * + sqrt(square(camModel.fx()) + square(camModel.cx())); + const int FBO_NROWS_DOWN = vertResolutionFactor_ * tan(vertFOVMin) * + sqrt(square(camModel.fx()) + square(camModel.cx())); const int FBO_NROWS = FBO_NROWS_DOWN + FBO_NROWS_UP + 1; camModel.nrows = FBO_NROWS; @@ -447,10 +425,9 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) const double firstAngle = -aperture * 0.5; - const unsigned int numRenders = - std::ceil((aperture / camModel_hFOV) - 1e-3); - const auto numHorzRaysPerRender = mrpt::round( - horzNumRays_ * std::min(1.0, (camModel_hFOV / aperture))); + const unsigned int numRenders = std::ceil((aperture / camModel_hFOV) - 1e-3); + const auto numHorzRaysPerRender = + mrpt::round(horzNumRays_ * std::min(1.0, (camModel_hFOV / aperture))); ASSERT_(numHorzRaysPerRender > 0); @@ -473,14 +450,12 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) const double horzAng = (scanIsCW ? -1 : 1) * - (camModel_hFOV * 0.5 - - i * camModel_hFOV / (numHorzRaysPerRender - 1)); + (camModel_hFOV * 0.5 - i * camModel_hFOV / (numHorzRaysPerRender - 1)); const double cosHorzAng = std::cos(horzAng); const auto pixel_u = mrpt::saturate_val( - camModel.cx() - camModel.fx() * std::tan(horzAng), 0, - camModel.ncols - 1); + camModel.cx() - camModel.fx() * std::tan(horzAng), 0, camModel.ncols - 1); for (size_t j = 0; j < nRows; j++) { @@ -489,13 +464,10 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) const double vertAng = vertical_ray_angles_.at(j); const double cosVertAng = std::cos(vertAng); - const auto pixel_v = camModel.cy() - camModel.fy() * - std::tan(vertAng) / - cosHorzAng; + const auto pixel_v = camModel.cy() - camModel.fy() * std::tan(vertAng) / cosHorzAng; // out of the simulated camera (should not happen?) - if (pixel_v < 0 || pixel_v >= static_cast(camModel.nrows)) - continue; + if (pixel_v < 0 || pixel_v >= static_cast(camModel.nrows)) continue; entry.u = pixel_u; entry.v = pixel_v; @@ -537,23 +509,19 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) // Do the log->linear conversion ourselves for this sensor, // since only a few depth points are actually used: auto& depth_log2lin = depth_log2lin_t::Instance(); - const auto& depth_log2lin_lut = - depth_log2lin.lut_from_zn_zf(minRange_, maxRange_); + const auto& depth_log2lin_lut = depth_log2lin.lut_from_zn_zf(minRange_, maxRange_); #endif for (size_t renderIdx = 0; renderIdx < numRenders; renderIdx++) { const double thisRenderMidAngle = - firstAngle + (camModel_hFOV / 2.0 + camModel_hFOV * renderIdx) * - (scanIsCW ? 1 : -1); + firstAngle + (camModel_hFOV / 2.0 + camModel_hFOV * renderIdx) * (scanIsCW ? 1 : -1); const auto thisDepthSensorPoseWrtSensor = - mrpt::poses::CPose3D::FromYawPitchRoll( - thisRenderMidAngle, 0.0, 0.0) + + mrpt::poses::CPose3D::FromYawPitchRoll(thisRenderMidAngle, 0.0, 0.0) + fixedAxisConventionRot; - const auto thisDepthSensorPoseOnVeh = - curObs->sensorPose + thisDepthSensorPoseWrtSensor; + const auto thisDepthSensorPoseOnVeh = curObs->sensorPose + thisDepthSensorPoseWrtSensor; const auto thisDepthSensorPose = vehiclePose + thisDepthSensorPoseOnVeh; @@ -562,8 +530,8 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) // to make the camera to look forward: cam.setPose(thisDepthSensorPose); - auto tleRender = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.3Dlidar.renderSubScan"); + auto tleRender = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.3Dlidar.renderSubScan"); fbo_renderer_depth_->render_depth(world3DScene, depthImage); @@ -585,15 +553,14 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) } } - auto tleStore = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.3Dlidar.storeObs"); + auto tleStore = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.3Dlidar.storeObs"); // Convert depth into range and store into polar range images: for (int i = 0; i < numHorzRaysPerRender; i++) { const int iAbs = i + numHorzRaysPerRender * renderIdx; - if (iAbs >= rangeImage.cols()) - continue; // we don't need this image part + if (iAbs >= rangeImage.cols()) continue; // we don't need this image part for (unsigned int j = 0; j < nRows; j++) { @@ -607,8 +574,8 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) // Depth: float d = safeInterpolateRangeImage( - depthImage, maxDepthInterpolationStepVert_, - maxDepthInterpolationStepHorz_, FBO_NCOLS, FBO_NROWS, v, u + depthImage, maxDepthInterpolationStepVert_, maxDepthInterpolationStepHorz_, + FBO_NCOLS, FBO_NROWS, v, u #if MRPT_VERSION >= 0x270 , depth_log2lin_lut @@ -640,8 +607,7 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) const mrpt::math::TPoint3D pt_wrt_cam = { d * (u - camModel.cx()) / camModel.fx(), d * (v - camModel.cy()) / camModel.fy(), d}; - curPts.insertPoint( - thisDepthSensorPoseWrtSensor.composePoint(pt_wrt_cam)); + curPts.insertPoint(thisDepthSensorPoseWrtSensor.composePoint(pt_wrt_cam)); // Add "ring" field: #if defined(HAVE_POINTS_XYZIRT) @@ -664,8 +630,8 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) // Store generated obs: { - auto tle3 = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.3Dlidar.acqObsMtx"); + auto tle3 = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.3Dlidar.acqObsMtx"); std::lock_guard csl(last_scan_cs_); last_scan_ = std::move(curObs); @@ -675,8 +641,8 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) { auto lckHasTo = mrpt::lockHelper(has_to_render_mtx_); - auto tlePub = mrpt::system::CTimeLoggerEntry( - world_->getTimeLogger(), "sensor.3Dlidar.report"); + auto tlePub = + mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.3Dlidar.report"); SensorBase::reportNewObservation(last_scan_, *has_to_render_); diff --git a/modules/simulator/src/Sensors/SensorBase.cpp b/modules/simulator/src/Sensors/SensorBase.cpp index a0694df3..9494ac6b 100644 --- a/modules/simulator/src/Sensors/SensorBase.cpp +++ b/modules/simulator/src/Sensors/SensorBase.cpp @@ -73,8 +73,7 @@ void SensorBase::RegisterSensorFOVViz(const mrpt::opengl::CSetOfObjects::Ptr& o) auto lck = mrpt::lockHelper(gAllSensorVizMtx); gAllSensorsFOVViz->insert(o); } -void SensorBase::RegisterSensorOriginViz( - const mrpt::opengl::CSetOfObjects::Ptr& o) +void SensorBase::RegisterSensorOriginViz(const mrpt::opengl::CSetOfObjects::Ptr& o) { auto lck = mrpt::lockHelper(gAllSensorVizMtx); gAllSensorsOriginViz->insert(o); @@ -89,8 +88,7 @@ SensorBase::SensorBase(Simulable& vehicle) SensorBase::~SensorBase() = default; -SensorBase::Ptr SensorBase::factory( - Simulable& parent, const rapidxml::xml_node* root) +SensorBase::Ptr SensorBase::factory(Simulable& parent, const rapidxml::xml_node* root) { register_all_sensors(); @@ -117,8 +115,8 @@ SensorBase::Ptr SensorBase::factory( auto we = classFactory_sensors.create(sName, parent, root); if (!we) - throw runtime_error(mrpt::format( - "[SensorBase::factory] Unknown sensor type '%s'", root->name())); + throw runtime_error( + mrpt::format("[SensorBase::factory] Unknown sensor type '%s'", root->name())); // parse the optional visual model: we->parseVisual(*root); @@ -127,8 +125,7 @@ SensorBase::Ptr SensorBase::factory( } bool SensorBase::parseSensorPublish( - const rapidxml::xml_node* node, - const std::map& varValues) + const rapidxml::xml_node* node, const std::map& varValues) { MRPT_START @@ -158,8 +155,7 @@ bool SensorBase::parseSensorPublish( } void SensorBase::reportNewObservation( - const std::shared_ptr& obs, - const TSimulContext& context) + const std::shared_ptr& obs, const TSimulContext& context) { if (!obs) return; @@ -177,8 +173,7 @@ void SensorBase::reportNewObservation( std::vector serializedData; mrpt::serialization::ObjectToOctetVector(obs.get(), serializedData); - msg.set_mrptserializedobservation( - serializedData.data(), serializedData.size()); + msg.set_mrptserializedobservation(serializedData.data(), serializedData.size()); context.world->commsClient().publishTopic(publishTopic_, msg); } @@ -186,8 +181,7 @@ void SensorBase::reportNewObservation( } void SensorBase::reportNewObservation_lidar_2d( - const std::shared_ptr& obs, - const TSimulContext& context) + const std::shared_ptr& obs, const TSimulContext& context) { using namespace std::string_literals; @@ -228,8 +222,7 @@ void SensorBase::registerOnServer(mvsim::Client& c) #if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF) // Topic: - if (!publishTopic_.empty()) - c.advertiseTopic(publishTopic_); + if (!publishTopic_.empty()) c.advertiseTopic(publishTopic_); #endif } @@ -259,11 +252,9 @@ void SensorBase::make_sure_we_have_a_name(const std::string& prefix) if (!name_.empty()) return; size_t nextIdx = 0; - if (auto v = dynamic_cast(&vehicle_); v) - nextIdx = v->getSensors().size() + 1; + if (auto v = dynamic_cast(&vehicle_); v) nextIdx = v->getSensors().size() + 1; - name_ = mrpt::format( - "%s%u", prefix.c_str(), static_cast(nextIdx)); + name_ = mrpt::format("%s%u", prefix.c_str(), static_cast(nextIdx)); } bool SensorBase::should_simulate_sensor(const TSimulContext& context) @@ -271,19 +262,15 @@ bool SensorBase::should_simulate_sensor(const TSimulContext& context) // to fix edge cases with sensor period a multiple of simulation timestep: const double timeEpsilon = 1e-6; - if (context.simul_time < - sensor_last_timestamp_ + sensor_period_ - timeEpsilon) - return false; + if (context.simul_time < sensor_last_timestamp_ + sensor_period_ - timeEpsilon) return false; if ((context.simul_time - sensor_last_timestamp_) >= 2 * sensor_period_) { - std::cout - << "[mvsim::SensorBase] WARNING: " - "At least one sensor sample has been lost due to too coarse " - "discrete time steps. sensor_period=" - << sensor_period_ << " [s], (simul_time - sensor_last_timestamp)=" - << (context.simul_time - sensor_last_timestamp_) << " [s]." - << std::endl; + std::cout << "[mvsim::SensorBase] WARNING: " + "At least one sensor sample has been lost due to too coarse " + "discrete time steps. sensor_period=" + << sensor_period_ << " [s], (simul_time - sensor_last_timestamp)=" + << (context.simul_time - sensor_last_timestamp_) << " [s]." << std::endl; } sensor_last_timestamp_ = context.simul_time; diff --git a/modules/simulator/src/Shape2p5.cpp b/modules/simulator/src/Shape2p5.cpp index 8c6e6f19..eb8e1582 100644 --- a/modules/simulator/src/Shape2p5.cpp +++ b/modules/simulator/src/Shape2p5.cpp @@ -42,8 +42,7 @@ constexpr uint8_t CELL_VISITED = 0x40; double Shape2p5::volume() const { - return std::abs(mrpt::math::signedArea(getContour())) * - std::abs(zMin_ - zMax_); + return std::abs(mrpt::math::signedArea(getContour())) * std::abs(zMin_ - zMax_); } void Shape2p5::mergeWith(const Shape2p5& s) @@ -105,8 +104,7 @@ static auto glDebugTriangles = mrpt::opengl::CSetOfTriangles::Create(); #endif void Shape2p5::buildInit( - const mrpt::math::TPoint2Df& bbMin, const mrpt::math::TPoint2Df& bbMax, - int numCells) + const mrpt::math::TPoint2Df& bbMin, const mrpt::math::TPoint2Df& bbMax, int numCells) { contour_.reset(); // start from scratch @@ -211,8 +209,7 @@ void Shape2p5::computeShape() const static int cnt = 0; mrpt::opengl::COpenGLScene scene; scene.insert(glDebugTriangles); - scene.saveToFile( - mrpt::format("debug_shape2p5_triangles_%04i.3Dscene", cnt++)); + scene.saveToFile(mrpt::format("debug_shape2p5_triangles_%04i.3Dscene", cnt++)); } #endif } @@ -241,15 +238,14 @@ void Shape2p5::internalGridFilterSpurious() const if (*thisCell != CELL_OCCUPIED) continue; // it's occupied: // reset to unknown if no other neighbors is occupied: - bool anyNN = - (*grid_->cellByIndex(cx - 1, cy - 1) == CELL_OCCUPIED) || - (*grid_->cellByIndex(cx - 1, cy + 0) == CELL_OCCUPIED) || - (*grid_->cellByIndex(cx - 1, cy + 1) == CELL_OCCUPIED) || - (*grid_->cellByIndex(cx + 0, cy - 1) == CELL_OCCUPIED) || - (*grid_->cellByIndex(cx + 0, cy + 1) == CELL_OCCUPIED) || - (*grid_->cellByIndex(cx + 1, cy - 1) == CELL_OCCUPIED) || - (*grid_->cellByIndex(cx + 1, cy + 0) == CELL_OCCUPIED) || - (*grid_->cellByIndex(cx + 1, cy + 1) == CELL_OCCUPIED); + bool anyNN = (*grid_->cellByIndex(cx - 1, cy - 1) == CELL_OCCUPIED) || + (*grid_->cellByIndex(cx - 1, cy + 0) == CELL_OCCUPIED) || + (*grid_->cellByIndex(cx - 1, cy + 1) == CELL_OCCUPIED) || + (*grid_->cellByIndex(cx + 0, cy - 1) == CELL_OCCUPIED) || + (*grid_->cellByIndex(cx + 0, cy + 1) == CELL_OCCUPIED) || + (*grid_->cellByIndex(cx + 1, cy - 1) == CELL_OCCUPIED) || + (*grid_->cellByIndex(cx + 1, cy + 0) == CELL_OCCUPIED) || + (*grid_->cellByIndex(cx + 1, cy + 1) == CELL_OCCUPIED); if (!anyNN) *thisCell = CELL_UNDEFINED; } @@ -397,14 +393,10 @@ mrpt::math::TPolygon2D Shape2p5::internalGridContour() const if (*c != CELL_OCCUPIED) return false; // check 4 neighbors: - if (auto* cS = grid_->cellByIndex(cx, cy - 1); cS && *cS == CELL_FREE) - return true; - if (auto* cN = grid_->cellByIndex(cx, cy + 1); cN && *cN == CELL_FREE) - return true; - if (auto* cE = grid_->cellByIndex(cx + 1, cy); cE && *cE == CELL_FREE) - return true; - if (auto* cW = grid_->cellByIndex(cx - 1, cy); cW && *cW == CELL_FREE) - return true; + if (auto* cS = grid_->cellByIndex(cx, cy - 1); cS && *cS == CELL_FREE) return true; + if (auto* cN = grid_->cellByIndex(cx, cy + 1); cN && *cN == CELL_FREE) return true; + if (auto* cE = grid_->cellByIndex(cx + 1, cy); cE && *cE == CELL_FREE) return true; + if (auto* cW = grid_->cellByIndex(cx - 1, cy); cW && *cW == CELL_FREE) return true; return false; }; @@ -441,14 +433,10 @@ mrpt::math::TPolygon2D Shape2p5::internalGridContour() const } // check 4 neighbors: - if (auto* cS = grid_->cellByIndex(cx, cy - 1); cS && *cS == CELL_FREE) - return true; - if (auto* cN = grid_->cellByIndex(cx, cy + 1); cN && *cN == CELL_FREE) - return true; - if (auto* cE = grid_->cellByIndex(cx + 1, cy); cE && *cE == CELL_FREE) - return true; - if (auto* cW = grid_->cellByIndex(cx - 1, cy); cW && *cW == CELL_FREE) - return true; + if (auto* cS = grid_->cellByIndex(cx, cy - 1); cS && *cS == CELL_FREE) return true; + if (auto* cN = grid_->cellByIndex(cx, cy + 1); cN && *cN == CELL_FREE) return true; + if (auto* cE = grid_->cellByIndex(cx + 1, cy); cE && *cE == CELL_FREE) return true; + if (auto* cW = grid_->cellByIndex(cx - 1, cy); cW && *cW == CELL_FREE) return true; return false; }; @@ -486,8 +474,7 @@ mrpt::math::TPolygon2D Shape2p5::internalGridContour() const for (const auto& dir : dirs) { const int ix = dir.first, iy = dir.second; - const bool isBorder = - lambdaCellIsBorder(cx + ix, cy + iy, pass == 1); + const bool isBorder = lambdaCellIsBorder(cx + ix, cy + iy, pass == 1); if (isBorder) { @@ -509,14 +496,12 @@ mrpt::math::TPolygon2D Shape2p5::internalGridContour() const } void Shape2p5::debugSaveGridTo3DSceneFile( - const mrpt::math::TPolygon2D& rawGridContour, - const std::string& debugStr) const + const mrpt::math::TPolygon2D& rawGridContour, const std::string& debugStr) const { mrpt::opengl::COpenGLScene scene; auto glGrid = mrpt::opengl::CTexturedPlane::Create(); - glGrid->setPlaneCorners( - grid_->getXMin(), grid_->getXMax(), grid_->getYMin(), grid_->getYMax()); + glGrid->setPlaneCorners(grid_->getXMin(), grid_->getXMax(), grid_->getYMin(), grid_->getYMax()); mrpt::math::CMatrixDouble mat; grid_->getAsMatrix(mat); @@ -529,9 +514,8 @@ void Shape2p5::debugSaveGridTo3DSceneFile( scene.insert(mrpt::opengl::stock_objects::CornerXYZSimple()); scene.insert(glGrid); - auto lambdaRenderPoly = [&scene]( - const mrpt::math::TPolygon2D& p, - const mrpt::img::TColor& color, double z) + auto lambdaRenderPoly = + [&scene](const mrpt::math::TPolygon2D& p, const mrpt::img::TColor& color, double z) { auto glPts = mrpt::opengl::CPointCloud::Create(); auto glPoly = mrpt::opengl::CSetOfLines::Create(); @@ -544,8 +528,7 @@ void Shape2p5::debugSaveGridTo3DSceneFile( const size_t j1 = (j + 1) % N; const auto& p0 = p.at(j); const auto& p1 = p.at(j1); - glPoly->appendLine( - p0.x, p0.y, z + 1e-4 * j, p1.x, p1.y, z + 1e-4 * (j + 1)); + glPoly->appendLine(p0.x, p0.y, z + 1e-4 * j, p1.x, p1.y, z + 1e-4 * (j + 1)); glPts->insertPoint(p0.x, p0.y, z + 1e-4 * j); } scene.insert(glPoly); @@ -573,8 +556,7 @@ std::optional Shape2p5::lossOfRemovingVertex( const auto& pt_im1 = p[im1]; const auto& pt_ip1 = p[ip1]; const auto delta = pt_ip1 - pt_im1; - const size_t nSteps = - static_cast(ceil(delta.norm() / grid_->getResolution())); + const size_t nSteps = static_cast(ceil(delta.norm() / grid_->getResolution())); const auto d = delta * (1.0 / nSteps); for (size_t k = 0; k < nSteps; k++) { @@ -604,8 +586,7 @@ std::optional Shape2p5::lossOfRemovingVertex( return rc; } -mrpt::math::TPolygon2D Shape2p5::internalPrunePolygon( - const mrpt::math::TPolygon2D& poly) const +mrpt::math::TPolygon2D Shape2p5::internalPrunePolygon(const mrpt::math::TPolygon2D& poly) const { using namespace std::string_literals; @@ -624,8 +605,7 @@ mrpt::math::TPolygon2D Shape2p5::internalPrunePolygon( for (size_t i = 0; i < p.size(); i++) { - std::optional rc = - lossOfRemovingVertex(i, p, pass == 1); + std::optional rc = lossOfRemovingVertex(i, p, pass == 1); if (rc && (!best || rc->loss < best->loss)) best = *rc; } @@ -634,8 +614,7 @@ mrpt::math::TPolygon2D Shape2p5::internalPrunePolygon( p = best->next; #ifdef DEBUG_DUMP_ALL_TEMPORARY_GRIDS - debugSaveGridTo3DSceneFile( - p, mrpt::format("pass #%i loss=%f", pass, best->loss)); + debugSaveGridTo3DSceneFile(p, mrpt::format("pass #%i loss=%f", pass, best->loss)); #endif } } diff --git a/modules/simulator/src/Simulable.cpp b/modules/simulator/src/Simulable.cpp index 4b0bcda6..47ad2bd5 100644 --- a/modules/simulator/src/Simulable.cpp +++ b/modules/simulator/src/Simulable.cpp @@ -41,8 +41,7 @@ void Simulable::simul_pre_timestep( // const double tMax = mrpt::Clock::toDouble(poseSeq.rbegin()->first); poseSeq.interpolate( - mrpt::Clock::fromDouble(std::fmod(context.simul_time, tMax)), q, - interOk); + mrpt::Clock::fromDouble(std::fmod(context.simul_time, tMax)), q, interOk); if (interOk) this->setPose(initial_q_ + q); } @@ -75,8 +74,7 @@ void Simulable::simul_post_timestep(const TSimulContext& context) // world-element modifies them! (e.g. elevation map) // Update the GUI element **poses** only: - if (auto* vo = meAsVisualObject(); vo) - vo->guiUpdate(std::nullopt, std::nullopt); + if (auto* vo = meAsVisualObject(); vo) vo->guiUpdate(std::nullopt, std::nullopt); // Vel: const b2Vec2& vel = b2dBody_->GetLinearVelocity(); @@ -86,16 +84,14 @@ void Simulable::simul_post_timestep(const TSimulContext& context) dq_.omega = w; // Estimate acceleration from finite differences: - ddq_lin_ = - (q_.translation() - former_q_.translation()) * (1.0 / context.dt); + ddq_lin_ = (q_.translation() - former_q_.translation()) * (1.0 / context.dt); former_q_ = q_; // Instantaneous collision flag: isInCollision_ = false; if (b2ContactEdge* cl = b2dBody_->GetContactList(); cl != nullptr) { - for (auto contact = cl->contact; contact != nullptr; - contact = contact->GetNext()) + for (auto contact = cl->contact; contact != nullptr; contact = contact->GetNext()) { // We may store with which other bodies it's in collision? const auto shA = cl->contact->GetFixtureA()->GetShape(); @@ -107,10 +103,8 @@ void Simulable::simul_post_timestep(const TSimulContext& context) b2DistanceInput di; di.proxyA.Set(shA, 0 /*index for chains*/); di.proxyB.Set(shB, 0 /*index for chains*/); - di.transformA = - cl->contact->GetFixtureA()->GetBody()->GetTransform(); - di.transformB = - cl->contact->GetFixtureB()->GetBody()->GetTransform(); + di.transformA = cl->contact->GetFixtureA()->GetBody()->GetTransform(); + di.transformB = cl->contact->GetFixtureB()->GetBody()->GetTransform(); di.useRadii = true; b2SimplexCache dc; @@ -178,8 +172,7 @@ void Simulable::resetCollisionFlag() hadCollisionFlag_ = false; } -bool Simulable::parseSimulable( - const JointXMLnode<>& rootNode, const ParseSimulableParams& psp) +bool Simulable::parseSimulable(const JointXMLnode<>& rootNode, const ParseSimulableParams& psp) { MRPT_START @@ -192,32 +185,26 @@ bool Simulable::parseSimulable( if (const xml_node<>* nPose = rootNode.first_node("init_pose"); nPose) { mrpt::math::TPose3D p; - if (3 != ::sscanf( - mvsim::parse( - nPose->value(), - getSimulableWorldObject()->user_defined_variables()) - .c_str(), - "%lf %lf %lf", &p.x, &p.y, &p.yaw)) - THROW_EXCEPTION_FMT( - "Error parsing %s", nPose->value()); + if (3 != + ::sscanf( + mvsim::parse(nPose->value(), getSimulableWorldObject()->user_defined_variables()) + .c_str(), + "%lf %lf %lf", &p.x, &p.y, &p.yaw)) + THROW_EXCEPTION_FMT("Error parsing %s", nPose->value()); p.yaw *= M_PI / 180.0; // deg->rad this->setPose(p); initial_q_ = p; // save it for later usage in some animations, etc. } - else if (const xml_node<>* nPose3 = rootNode.first_node("init_pose3d"); - nPose3) + else if (const xml_node<>* nPose3 = rootNode.first_node("init_pose3d"); nPose3) { mrpt::math::TPose3D p; - if (6 != ::sscanf( - mvsim::parse( - nPose3->value(), - getSimulableWorldObject()->user_defined_variables()) - .c_str(), - "%lf %lf %lf %lf %lf %lf ", &p.x, &p.y, &p.z, &p.yaw, - &p.pitch, &p.roll)) - THROW_EXCEPTION_FMT( - "Error parsing %s", nPose3->value()); + if (6 != + ::sscanf( + mvsim::parse(nPose3->value(), getSimulableWorldObject()->user_defined_variables()) + .c_str(), + "%lf %lf %lf %lf %lf %lf ", &p.x, &p.y, &p.z, &p.yaw, &p.pitch, &p.roll)) + THROW_EXCEPTION_FMT("Error parsing %s", nPose3->value()); p.yaw *= M_PI / 180.0; // deg->rad p.pitch *= M_PI / 180.0; // deg->rad p.roll *= M_PI / 180.0; // deg->rad @@ -227,8 +214,7 @@ bool Simulable::parseSimulable( } else if (psp.init_pose_mandatory) { - THROW_EXCEPTION( - "Missing required XML node x y phi"); + THROW_EXCEPTION("Missing required XML node x y phi"); } // ------------------------------------- @@ -237,14 +223,12 @@ bool Simulable::parseSimulable( if (const xml_node<>* nInitVel = rootNode.first_node("init_vel"); nInitVel) { mrpt::math::TTwist2D dq; - if (3 != ::sscanf( - mvsim::parse( - nInitVel->value(), - getSimulableWorldObject()->user_defined_variables()) - .c_str(), - "%lf %lf %lf", &dq.vx, &dq.vy, &dq.omega)) - THROW_EXCEPTION_FMT( - "Error parsing %s", nInitVel->value()); + if (3 != + ::sscanf( + mvsim::parse(nInitVel->value(), getSimulableWorldObject()->user_defined_variables()) + .c_str(), + "%lf %lf %lf", &dq.vx, &dq.vy, &dq.omega)) + THROW_EXCEPTION_FMT("Error parsing %s", nInitVel->value()); dq.omega *= M_PI / 180.0; // deg->rad // Convert twist (velocity) from local -> global coords: @@ -261,11 +245,9 @@ bool Simulable::parseSimulable( params["publish_pose_topic"] = TParamEntry("%s", &publishPoseTopic_); params["publish_pose_period"] = TParamEntry("%lf", &publishPosePeriod_); - params["publish_relative_pose_topic"] = - TParamEntry("%s", &publishRelativePoseTopic_); + params["publish_relative_pose_topic"] = TParamEntry("%s", &publishRelativePoseTopic_); std::string listObjects; - params["publish_relative_pose_objects"] = - TParamEntry("%s", &listObjects); + params["publish_relative_pose_objects"] = TParamEntry("%s", &listObjects); auto varValues = simulable_parent_->user_defined_variables(); varValues["NAME"] = name_; @@ -286,14 +268,11 @@ bool Simulable::parseSimulable( if (!listObjects.empty()) { mrpt::system::tokenize( - mrpt::system::trim(listObjects), " ,", - publishRelativePoseOfOtherObjects_); + mrpt::system::trim(listObjects), " ,", publishRelativePoseOfOtherObjects_); } ASSERT_( - (publishRelativePoseOfOtherObjects_.empty() && - publishRelativePoseTopic_.empty()) || - (!publishRelativePoseOfOtherObjects_.empty() && - !publishRelativePoseTopic_.empty())); + (publishRelativePoseOfOtherObjects_.empty() && publishRelativePoseTopic_.empty()) || + (!publishRelativePoseOfOtherObjects_.empty() && !publishRelativePoseTopic_.empty())); } // end @@ -316,14 +295,13 @@ bool Simulable::parseSimulable( { mrpt::math::TPose3D p; double t = 0; - if (4 != ::sscanf( - mvsim::parse( - n->value(), getSimulableWorldObject() - ->user_defined_variables()) - .c_str(), - "%lf %lf %lf %lf", &t, &p.x, &p.y, &p.yaw)) - THROW_EXCEPTION_FMT( - "Error parsing :\n%s", n->value()); + if (4 != + ::sscanf( + mvsim::parse( + n->value(), getSimulableWorldObject()->user_defined_variables()) + .c_str(), + "%lf %lf %lf %lf", &t, &p.x, &p.y, &p.yaw)) + THROW_EXCEPTION_FMT("Error parsing :\n%s", n->value()); p.yaw *= M_PI / 180.0; // deg->rad poseSeq.insert(mrpt::Clock::fromDouble(t), p); @@ -332,15 +310,14 @@ bool Simulable::parseSimulable( { mrpt::math::TPose3D p; double t = 0; - if (7 != ::sscanf( - mvsim::parse( - n->value(), getSimulableWorldObject() - ->user_defined_variables()) - .c_str(), - "%lf %lf %lf %lf %lf %lf %lf", &t, &p.x, &p.y, - &p.z, &p.yaw, &p.pitch, &p.roll)) - THROW_EXCEPTION_FMT( - "Error parsing :\n%s", n->value()); + if (7 != + ::sscanf( + mvsim::parse( + n->value(), getSimulableWorldObject()->user_defined_variables()) + .c_str(), + "%lf %lf %lf %lf %lf %lf %lf", &t, &p.x, &p.y, &p.z, &p.yaw, &p.pitch, + &p.roll)) + THROW_EXCEPTION_FMT("Error parsing :\n%s", n->value()); p.yaw *= M_PI / 180.0; // deg->rad p.pitch *= M_PI / 180.0; // deg->rad p.roll *= M_PI / 180.0; // deg->rad @@ -400,14 +377,12 @@ void Simulable::internalHandlePublish(const TSimulContext& context) msg.set_relativetoobjectid(name_); // Note: getSimulableWorldObjectMtx() is already hold by my caller. - const auto& allObjects = - getSimulableWorldObject()->getListOfSimulableObjects(); + const auto& allObjects = getSimulableWorldObject()->getListOfSimulableObjects(); // detect other objects and publish their relative poses wrt me: for (const auto& otherId : publishRelativePoseOfOtherObjects_) { - if (auto itObj = allObjects.find(otherId); - itObj != allObjects.end()) + if (auto itObj = allObjects.find(otherId); itObj != allObjects.end()) { msg.set_objectid(otherId); @@ -425,12 +400,11 @@ void Simulable::internalHandlePublish(const TSimulContext& context) } else { - std::cerr - << "[WARNING] Trying to publish relative pose of '" - << otherId << "' wrt '" << name_ - << "' but could not find any object in the world with " - "the former name." - << std::endl; + std::cerr << "[WARNING] Trying to publish relative pose of '" << otherId + << "' wrt '" << name_ + << "' but could not find any object in the world with " + "the former name." + << std::endl; } } } @@ -448,8 +422,7 @@ void Simulable::registerOnServer(mvsim::Client& c) c.advertiseTopic(publishPoseTopic_); if (!publishRelativePoseTopic_.empty()) - c.advertiseTopic( - publishRelativePoseTopic_); + c.advertiseTopic(publishRelativePoseTopic_); #endif MRPT_END @@ -465,8 +438,7 @@ void Simulable::setPose(const mrpt::math::TPose3D& p, bool notifyChange) const me.q_ = p; // Update the GUI element poses only: - if (auto* vo = me.meAsVisualObject(); vo) - vo->guiUpdate(std::nullopt, std::nullopt); + if (auto* vo = me.meAsVisualObject(); vo) vo->guiUpdate(std::nullopt, std::nullopt); } if (notifyChange) const_cast(this)->notifySimulableSetPose(p); diff --git a/modules/simulator/src/VehicleBase.cpp b/modules/simulator/src/VehicleBase.cpp index 9e1e8886..5fd488d1 100644 --- a/modules/simulator/src/VehicleBase.cpp +++ b/modules/simulator/src/VehicleBase.cpp @@ -47,13 +47,10 @@ void register_all_veh_dynamics() done = true; REGISTER_VEHICLE_DYNAMICS("differential", DynamicsDifferential) - REGISTER_VEHICLE_DYNAMICS( - "differential_3_wheels", DynamicsDifferential_3_wheels) - REGISTER_VEHICLE_DYNAMICS( - "differential_4_wheels", DynamicsDifferential_4_wheels) + REGISTER_VEHICLE_DYNAMICS("differential_3_wheels", DynamicsDifferential_3_wheels) + REGISTER_VEHICLE_DYNAMICS("differential_4_wheels", DynamicsDifferential_4_wheels) REGISTER_VEHICLE_DYNAMICS("ackermann", DynamicsAckermann) - REGISTER_VEHICLE_DYNAMICS( - "ackermann_drivetrain", DynamicsAckermannDrivetrain) + REGISTER_VEHICLE_DYNAMICS("ackermann_drivetrain", DynamicsAckermannDrivetrain) } constexpr char VehicleBase::DL_TIMESTAMP[]; @@ -100,9 +97,7 @@ void VehicleBase::register_vehicle_class( const World& parent, const rapidxml::xml_node* xml_node) { // Sanity checks: - if (!xml_node) - throw runtime_error( - "[VehicleBase::register_vehicle_class] XML node is nullptr"); + if (!xml_node) throw runtime_error("[VehicleBase::register_vehicle_class] XML node is nullptr"); if (0 != strcmp(xml_node->name(), "vehicle:class")) throw runtime_error(mrpt::format( "[VehicleBase::register_vehicle_class] XML element is '%s' " @@ -115,22 +110,19 @@ void VehicleBase::register_vehicle_class( "NAME" /*sensor name*/, "PARENT_NAME" /*vehicle name*/}; // Parse XML to solve for includes: - veh_classes_registry.add( - xml_to_str_solving_includes(parent, xml_node, varsRetain)); + veh_classes_registry.add(xml_to_str_solving_includes(parent, xml_node, varsRetain)); } /** Class factory: Creates a vehicle from XML description of type * "...". */ -VehicleBase::Ptr VehicleBase::factory( - World* parent, const rapidxml::xml_node* root) +VehicleBase::Ptr VehicleBase::factory(World* parent, const rapidxml::xml_node* root) { register_all_veh_dynamics(); using namespace std; using namespace rapidxml; - if (!root) - throw runtime_error("[VehicleBase::factory] XML node is nullptr"); + if (!root) throw runtime_error("[VehicleBase::factory] XML node is nullptr"); if (0 != strcmp(root->name(), "vehicle")) throw runtime_error(mrpt::format( "[VehicleBase::factory] XML root element is '%s' ('vehicle' " @@ -151,9 +143,7 @@ VehicleBase::Ptr VehicleBase::factory( if (strcmp(n->name(), "include") != 0) continue; auto fileAttrb = n->first_attribute("file"); - ASSERTMSG_( - fileAttrb, - "XML tag '' must have a 'file=\"xxx\"' attribute)"); + ASSERTMSG_(fileAttrb, "XML tag '' must have a 'file=\"xxx\"' attribute)"); const std::string relFile = mvsim::parse(fileAttrb->value(), parent->user_defined_variables()); @@ -166,8 +156,7 @@ VehicleBase::Ptr VehicleBase::factory( // Inherit the user-defined variables from parent scope vars = parent->user_defined_variables(); // Plus new ones: - for (auto attr = n->first_attribute(); attr; - attr = attr->next_attribute()) + for (auto attr = n->first_attribute(); attr; attr = attr->next_attribute()) { if (strcmp(attr->name(), "file") == 0) continue; vars[attr->name()] = attr->value(); @@ -175,8 +164,7 @@ VehicleBase::Ptr VehicleBase::factory( // Delay the replacement of this variable (used in Sensors) until "veh" // is constructed and we actually have a name: - std::set varsRetain = { - "NAME" /*sensor name*/, "PARENT_NAME" /*vehicle name*/}; + std::set varsRetain = {"NAME" /*sensor name*/, "PARENT_NAME" /*vehicle name*/}; const auto [xml, nRoot] = readXmlAndGetRoot(absFile, vars, varsRetain); @@ -191,16 +179,13 @@ VehicleBase::Ptr VehicleBase::factory( // Always search in root. Also in the class root, if any: nodes.add(root); - if (const xml_attribute<>* veh_class = root->first_attribute("class"); - veh_class) + if (const xml_attribute<>* veh_class = root->first_attribute("class"); veh_class) { const string sClassName = veh_class->value(); - const rapidxml::xml_node* class_root = - veh_classes_registry.get(sClassName); + const rapidxml::xml_node* class_root = veh_classes_registry.get(sClassName); if (!class_root) throw runtime_error(mrpt::format( - "[VehicleBase::factory] Vehicle class '%s' undefined", - sClassName.c_str())); + "[VehicleBase::factory] Vehicle class '%s' undefined", sClassName.c_str())); nodes.add(class_root); } @@ -209,9 +194,7 @@ VehicleBase::Ptr VehicleBase::factory( // ------------------------------------------------- const xml_node<>* dyn_node = nodes.first_node("dynamics"); - if (!dyn_node) - throw runtime_error( - "[VehicleBase::factory] Missing XML node "); + if (!dyn_node) throw runtime_error("[VehicleBase::factory] Missing XML node "); const xml_attribute<>* dyn_class = dyn_node->first_attribute("class"); if (!dyn_class || !dyn_class->value()) @@ -219,12 +202,10 @@ VehicleBase::Ptr VehicleBase::factory( "[VehicleBase::factory] Missing mandatory attribute 'class' in " "node "); - VehicleBase::Ptr veh = - classFactory_vehicleDynamics.create(dyn_class->value(), parent); + VehicleBase::Ptr veh = classFactory_vehicleDynamics.create(dyn_class->value(), parent); if (!veh) throw runtime_error(mrpt::format( - "[VehicleBase::factory] Unknown vehicle dynamics class '%s'", - dyn_class->value())); + "[VehicleBase::factory] Unknown vehicle dynamics class '%s'", dyn_class->value())); // Initialize here all common params shared by any polymorphic class: // ------------------------------------------------- @@ -256,13 +237,9 @@ VehicleBase::Ptr VehicleBase::factory( veh->dynamics_load_params_from_xml(dyn_node); // Auto shape node from visual? - if (const rapidxml::xml_node* xml_chassis = - dyn_node->first_node("chassis"); - xml_chassis) + if (const rapidxml::xml_node* xml_chassis = dyn_node->first_node("chassis"); xml_chassis) { - if (const rapidxml::xml_node* sfv = - xml_chassis->first_node("shape_from_visual"); - sfv) + if (const rapidxml::xml_node* sfv = xml_chassis->first_node("shape_from_visual"); sfv) { const auto& bbVis = veh->collisionShape(); if (!bbVis.has_value()) @@ -289,8 +266,7 @@ VehicleBase::Ptr VehicleBase::factory( { // Update collision shape from shape loaded from XML: Shape2p5 cs; - cs.setShapeManual( - veh->chassis_poly_, veh->chassis_z_min_, veh->chassis_z_max_); + cs.setShapeManual(veh->chassis_poly_, veh->chassis_z_min_, veh->chassis_z_max_); veh->setCollisionShape(cs); } @@ -335,14 +311,12 @@ VehicleBase::Ptr VehicleBase::factory( if (!frict_node) { // Default: - veh->friction_ = - std::make_shared(*veh, nullptr /*default */); + veh->friction_ = std::make_shared(*veh, nullptr /*default */); } else { // Parse: - veh->friction_ = std::shared_ptr( - FrictionBase::factory(*veh, frict_node)); + veh->friction_ = std::shared_ptr(FrictionBase::factory(*veh, frict_node)); ASSERT_(veh->friction_); } } @@ -361,8 +335,7 @@ VehicleBase::Ptr VehicleBase::factory( return veh; } -VehicleBase::Ptr VehicleBase::factory( - World* parent, const std::string& xml_text) +VehicleBase::Ptr VehicleBase::factory(World* parent, const std::string& xml_text) { // Parse the string as if it was an XML file: std::stringstream s; @@ -376,11 +349,10 @@ VehicleBase::Ptr VehicleBase::factory( } catch (rapidxml::parse_error& e) { - unsigned int line = - static_cast(std::count(input_str, e.where(), '\n') + 1); + unsigned int line = static_cast(std::count(input_str, e.where(), '\n') + 1); throw std::runtime_error(mrpt::format( - "[VehicleBase::factory] XML parse error (Line %u): %s", - static_cast(line), e.what())); + "[VehicleBase::factory] XML parse error (Line %u): %s", static_cast(line), + e.what())); } return VehicleBase::factory(parent, xml.first_node()); } @@ -394,8 +366,7 @@ void VehicleBase::simul_pre_timestep(const TSimulContext& context) // configuration) for (size_t i = 0; i < fixture_wheels_.size(); i++) { - b2PolygonShape* wheelShape = - dynamic_cast(fixture_wheels_[i]->GetShape()); + b2PolygonShape* wheelShape = dynamic_cast(fixture_wheels_[i]->GetShape()); wheelShape->SetAsBox( wheels_info_[i].diameter * 0.5, wheels_info_[i].width * 0.5, b2Vec2(wheels_info_[i].x, wheels_info_[i].y), wheels_info_[i].yaw); @@ -432,8 +403,7 @@ void VehicleBase::simul_pre_timestep(const TSimulContext& context) fi.weight = weightPerWheel; fi.wheelCogLocalVel = wheelLocalVels[i]; - friction_->setLogger( - getLoggerPtr(LOGGER_WHEEL + std::to_string(i + 1))); + friction_->setLogger(getLoggerPtr(LOGGER_WHEEL + std::to_string(i + 1))); // eval friction (in the frame of the vehicle): const mrpt::math::TPoint2D F_r = friction_->evaluate_friction(fi); @@ -453,18 +423,14 @@ void VehicleBase::simul_pre_timestep(const TSimulContext& context) { loggers_[LOGGER_WHEEL + std::to_string(i + 1)]->updateColumn( DL_TIMESTAMP, context.simul_time); - loggers_[LOGGER_WHEEL + std::to_string(i + 1)]->updateColumn( - WL_TORQUE, fi.motorTorque); - loggers_[LOGGER_WHEEL + std::to_string(i + 1)]->updateColumn( - WL_WEIGHT, fi.weight); + loggers_[LOGGER_WHEEL + std::to_string(i + 1)]->updateColumn(WL_TORQUE, fi.motorTorque); + loggers_[LOGGER_WHEEL + std::to_string(i + 1)]->updateColumn(WL_WEIGHT, fi.weight); loggers_[LOGGER_WHEEL + std::to_string(i + 1)]->updateColumn( WL_VEL_X, fi.wheelCogLocalVel.x); loggers_[LOGGER_WHEEL + std::to_string(i + 1)]->updateColumn( WL_VEL_Y, fi.wheelCogLocalVel.y); - loggers_[LOGGER_WHEEL + std::to_string(i + 1)]->updateColumn( - WL_FRIC_X, F_r.x); - loggers_[LOGGER_WHEEL + std::to_string(i + 1)]->updateColumn( - WL_FRIC_Y, F_r.y); + loggers_[LOGGER_WHEEL + std::to_string(i + 1)]->updateColumn(WL_FRIC_X, F_r.x); + loggers_[LOGGER_WHEEL + std::to_string(i + 1)]->updateColumn(WL_FRIC_Y, F_r.y); } // save it for optional rendering: @@ -473,8 +439,7 @@ void VehicleBase::simul_pre_timestep(const TSimulContext& context) // [meters/N] const double forceScale = world_->guiOptions_.force_scale; - const mrpt::math::TPoint3D pt1( - wPt.x, wPt.y, chassis_z_max_ * 1.1 + getPose().z); + const mrpt::math::TPoint3D pt1(wPt.x, wPt.y, chassis_z_max_ * 1.1 + getPose().z); const mrpt::math::TPoint3D pt2 = pt1 + mrpt::math::TPoint3D(wForce.x, wForce.y, 0) * forceScale; @@ -522,9 +487,7 @@ void VehicleBase::simul_post_timestep(const TSimulContext& context) // lose 'double' accuracy): const double cur_abs_phi = std::abs(w.getPhi()); if (cur_abs_phi > 1e4) - w.setPhi( - ::fmod(cur_abs_phi, 2 * M_PI) * - (w.getPhi() < 0.0 ? -1.0 : 1.0)); + w.setPhi(::fmod(cur_abs_phi, 2 * M_PI) * (w.getPhi() < 0.0 ? -1.0 : 1.0)); } const auto q = getPose(); @@ -627,8 +590,7 @@ void VehicleBase::create_multibody_system(b2World& world) ASSERT_(nPts >= 3); ASSERT_LE_(nPts, (size_t)b2_maxPolygonVertices); std::vector pts(nPts); - for (size_t i = 0; i < nPts; i++) - pts[i] = b2Vec2(chassis_poly_[i].x, chassis_poly_[i].y); + for (size_t i = 0; i < nPts; i++) pts[i] = b2Vec2(chassis_poly_[i].x, chassis_poly_[i].y); b2PolygonShape chassisPoly; chassisPoly.Set(&pts[0], nPts); @@ -678,8 +640,7 @@ void VehicleBase::create_multibody_system(b2World& world) // Set the box density to be non-zero, so it will be dynamic. b2MassData mass; - wheelShape.ComputeMass( - &mass, 1); // Mass with density=1 => compute area + wheelShape.ComputeMass(&mass, 1); // Mass with density=1 => compute area fixtureDef.density = wheels_info_[i].mass / mass.mass; // Override the default friction. @@ -691,8 +652,7 @@ void VehicleBase::create_multibody_system(b2World& world) void VehicleBase::internalGuiUpdate( const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly) + const mrpt::optional_ref& physical, bool childrenOnly) { // 1st time call?? -> Create objects // ---------------------------------- @@ -750,15 +710,14 @@ void VehicleBase::internalGuiUpdate( for (size_t i = 0; i < nWs; i++) { const Wheel& w = getWheelInfo(i); - glWheelsPhysical_[i]->setPose(mrpt::math::TPose3D( - w.x, w.y, 0.5 * w.diameter, w.yaw, w.getPhi(), 0.0)); - glWheelsViz_[i]->setPose(mrpt::math::TPose3D( - w.x, w.y, 0.5 * w.diameter, w.yaw, w.getPhi(), 0.0)); + glWheelsPhysical_[i]->setPose( + mrpt::math::TPose3D(w.x, w.y, 0.5 * w.diameter, w.yaw, w.getPhi(), 0.0)); + glWheelsViz_[i]->setPose( + mrpt::math::TPose3D(w.x, w.y, 0.5 * w.diameter, w.yaw, w.getPhi(), 0.0)); if (!w.linked_yaw_object_name.empty()) { - auto glLinked = VisualObject::glCustomVisual_->getByName( - w.linked_yaw_object_name); + auto glLinked = VisualObject::glCustomVisual_->getByName(w.linked_yaw_object_name); if (!glLinked) { THROW_EXCEPTION_FMT( @@ -813,13 +772,11 @@ void VehicleBase::initLoggers() // loggers_[LOGGER_POSE]->addColumn(PL_DQ_X); // loggers_[LOGGER_POSE]->addColumn(PL_DQ_Y); // loggers_[LOGGER_POSE]->addColumn(PL_DQ_Z); - loggers_[LOGGER_POSE]->setFilepath( - log_path_ + "mvsim_" + name_ + LOGGER_POSE + ".log"); + loggers_[LOGGER_POSE]->setFilepath(log_path_ + "mvsim_" + name_ + LOGGER_POSE + ".log"); for (size_t i = 0; i < getNumWheels(); i++) { - loggers_[LOGGER_WHEEL + std::to_string(i + 1)] = - std::make_shared(); + loggers_[LOGGER_WHEEL + std::to_string(i + 1)] = std::make_shared(); // loggers_[LOGGER_WHEEL + std::to_string(i + // 1)]->addColumn(DL_TIMESTAMP); // loggers_[LOGGER_WHEEL + std::to_string(i + @@ -835,8 +792,7 @@ void VehicleBase::initLoggers() // loggers_[LOGGER_WHEEL + std::to_string(i + // 1)]->addColumn(WL_FRIC_Y); loggers_[LOGGER_WHEEL + std::to_string(i + 1)]->setFilepath( - log_path_ + "mvsim_" + name_ + LOGGER_WHEEL + - std::to_string(i + 1) + ".log"); + log_path_ + "mvsim_" + name_ + LOGGER_WHEEL + std::to_string(i + 1) + ".log"); } } @@ -853,8 +809,8 @@ void VehicleBase::apply_force( const mrpt::math::TVector2D& force, const mrpt::math::TPoint2D& applyPoint) { ASSERT_(b2dBody_); - const b2Vec2 wPt = b2dBody_->GetWorldPoint(b2Vec2( - applyPoint.x, applyPoint.y)); // Application point -> world coords + const b2Vec2 wPt = b2dBody_->GetWorldPoint( + b2Vec2(applyPoint.x, applyPoint.y)); // Application point -> world coords b2dBody_->ApplyForce(b2Vec2(force.x, force.y), wPt, true /*wake up*/); } diff --git a/modules/simulator/src/VehicleDynamics/VehicleAckermann.cpp b/modules/simulator/src/VehicleDynamics/VehicleAckermann.cpp index 36c080eb..79a36047 100644 --- a/modules/simulator/src/VehicleDynamics/VehicleAckermann.cpp +++ b/modules/simulator/src/VehicleDynamics/VehicleAckermann.cpp @@ -20,8 +20,7 @@ using namespace mvsim; using namespace std; // Ctor: -DynamicsAckermann::DynamicsAckermann(World* parent) - : VehicleBase(parent, 4 /*num wheels*/) +DynamicsAckermann::DynamicsAckermann(World* parent) : VehicleBase(parent, 4 /*num wheels*/) { chassis_mass_ = 500.0; chassis_z_min_ = 0.20; @@ -57,15 +56,12 @@ DynamicsAckermann::DynamicsAckermann(World* parent) } /** The derived-class part of load_params_from_xml() */ -void DynamicsAckermann::dynamics_load_params_from_xml( - const rapidxml::xml_node* xml_node) +void DynamicsAckermann::dynamics_load_params_from_xml(const rapidxml::xml_node* xml_node) { const std::map varValues = {{"NAME", name_}}; // - if (const rapidxml::xml_node* xml_chassis = - xml_node->first_node("chassis"); - xml_chassis) + if (const rapidxml::xml_node* xml_chassis = xml_node->first_node("chassis"); xml_chassis) { // Attribs: TParameterDefinitions attribs; @@ -75,16 +71,12 @@ void DynamicsAckermann::dynamics_load_params_from_xml( attribs["color"] = TParamEntry("%color", &this->chassis_color_); parse_xmlnode_attribs( - *xml_chassis, attribs, {}, - "[DynamicsAckermann::dynamics_load_params_from_xml]"); + *xml_chassis, attribs, {}, "[DynamicsAckermann::dynamics_load_params_from_xml]"); // Shape node (optional, fallback to default shape if none found) - if (const rapidxml::xml_node* xml_shape = - xml_chassis->first_node("shape"); - xml_shape) + if (const rapidxml::xml_node* xml_shape = xml_chassis->first_node("shape"); xml_shape) mvsim::parse_xmlnode_shape( - *xml_shape, chassis_poly_, - "[DynamicsAckermann::dynamics_load_params_from_xml]"); + *xml_shape, chassis_poly_, "[DynamicsAckermann::dynamics_load_params_from_xml]"); } // @@ -107,8 +99,7 @@ void DynamicsAckermann::dynamics_load_params_from_xml( else { world_->logFmt( - mrpt::system::LVL_WARN, - "No XML entry '%s' found: using defaults for wheel #%u", + mrpt::system::LVL_WARN, "No XML entry '%s' found: using defaults for wheel #%u", w_names[i], static_cast(i)); } } @@ -127,8 +118,7 @@ void DynamicsAckermann::dynamics_load_params_from_xml( ack_ps["max_steer_ang_deg"] = TParamEntry("%lf_deg", &max_steer_ang_); parse_xmlnode_children_as_param( - *xml_node, ack_ps, varValues, - "[DynamicsAckermann::dynamics_load_params_from_xml]"); + *xml_node, ack_ps, varValues, "[DynamicsAckermann::dynamics_load_params_from_xml]"); // Front-left: wheels_info_[WHEEL_FL].x = front_x; @@ -141,12 +131,10 @@ void DynamicsAckermann::dynamics_load_params_from_xml( // Vehicle controller: // ------------------------------------------------- { - const rapidxml::xml_node* xml_control = - xml_node->first_node("controller"); + const rapidxml::xml_node* xml_control = xml_node->first_node("controller"); if (xml_control) { - rapidxml::xml_attribute* control_class = - xml_control->first_attribute("class"); + rapidxml::xml_attribute* control_class = xml_control->first_attribute("class"); if (!control_class || !control_class->value()) throw runtime_error( "[DynamicsAckermann] Missing 'class' attribute in " @@ -156,8 +144,7 @@ void DynamicsAckermann::dynamics_load_params_from_xml( if (sCtrlClass == ControllerRawForces::class_name()) controller_ = std::make_shared(*this); else if (sCtrlClass == ControllerTwistFrontSteerPID::class_name()) - controller_ = - std::make_shared(*this); + controller_ = std::make_shared(*this); else if (sCtrlClass == ControllerFrontSteerPID::class_name()) controller_ = std::make_shared(*this); else @@ -170,13 +157,11 @@ void DynamicsAckermann::dynamics_load_params_from_xml( } } // Default controller: - if (!controller_) - controller_ = std::make_shared(*this); + if (!controller_) controller_ = std::make_shared(*this); } // See docs in base class: -std::vector DynamicsAckermann::invoke_motor_controllers( - const TSimulContext& context) +std::vector DynamicsAckermann::invoke_motor_controllers(const TSimulContext& context) { // Longitudinal forces at each wheel: std::vector out_torque_per_wheel; @@ -199,15 +184,13 @@ std::vector DynamicsAckermann::invoke_motor_controllers( // Ackermann formulas for inner&outer weels turning angles wrt the // equivalent (central) one: computeFrontWheelAngles( - co.steer_ang, wheels_info_[WHEEL_FL].yaw, - wheels_info_[WHEEL_FR].yaw); + co.steer_ang, wheels_info_[WHEEL_FL].yaw, wheels_info_[WHEEL_FR].yaw); } return out_torque_per_wheel; } void DynamicsAckermann::computeFrontWheelAngles( - const double desired_equiv_steer_ang, double& out_fl_ang, - double& out_fr_ang) const + const double desired_equiv_steer_ang, double& out_fl_ang, double& out_fr_ang) const { // EQ1: cot(d)+0.5*w/l = cot(do) // EQ2: cot(di)=cot(do)-w/l @@ -215,8 +198,7 @@ void DynamicsAckermann::computeFrontWheelAngles( const double l = wheels_info_[WHEEL_FL].x - wheels_info_[WHEEL_RL].x; ASSERT_(l > 0); const double w_l = w / l; - const double delta = - b2Clamp(std::abs(desired_equiv_steer_ang), 0.0, max_steer_ang_); + const double delta = b2Clamp(std::abs(desired_equiv_steer_ang), 0.0, max_steer_ang_); const bool delta_neg = (desired_equiv_steer_ang < 0); ASSERT_LT_(delta, 0.5 * M_PI - 0.01); @@ -224,10 +206,8 @@ void DynamicsAckermann::computeFrontWheelAngles( const double cot_di = cot_do - w_l; // delta>0: do->right, di->left wheel // delta<0: do->left , di->right wheel - (delta_neg ? out_fr_ang : out_fl_ang) = - atan(1.0 / cot_di) * (delta_neg ? -1.0 : 1.0); - (delta_neg ? out_fl_ang : out_fr_ang) = - atan(1.0 / cot_do) * (delta_neg ? -1.0 : 1.0); + (delta_neg ? out_fr_ang : out_fl_ang) = atan(1.0 / cot_di) * (delta_neg ? -1.0 : 1.0); + (delta_neg ? out_fl_ang : out_fr_ang) = atan(1.0 / cot_do) * (delta_neg ? -1.0 : 1.0); } // See docs in base class: diff --git a/modules/simulator/src/VehicleDynamics/VehicleAckermann_ControllerFrontSteerPID.cpp b/modules/simulator/src/VehicleDynamics/VehicleAckermann_ControllerFrontSteerPID.cpp index d93e6d08..74e872a4 100644 --- a/modules/simulator/src/VehicleDynamics/VehicleAckermann_ControllerFrontSteerPID.cpp +++ b/modules/simulator/src/VehicleDynamics/VehicleAckermann_ControllerFrontSteerPID.cpp @@ -14,8 +14,7 @@ using namespace mvsim; using namespace std; -DynamicsAckermann::ControllerFrontSteerPID::ControllerFrontSteerPID( - DynamicsAckermann& veh) +DynamicsAckermann::ControllerFrontSteerPID::ControllerFrontSteerPID(DynamicsAckermann& veh) : ControllerBase(veh), setpoint_lin_speed(0), setpoint_steer_ang(0), @@ -32,8 +31,7 @@ DynamicsAckermann::ControllerFrontSteerPID::ControllerFrontSteerPID( // See base class docs void DynamicsAckermann::ControllerFrontSteerPID::control_step( - const DynamicsAckermann::TControllerInput& ci, - DynamicsAckermann::TControllerOutput& co) + const DynamicsAckermann::TControllerInput& ci, DynamicsAckermann::TControllerOutput& co) { // Equivalent v/w velocities: const double v = setpoint_lin_speed; @@ -63,8 +61,7 @@ void DynamicsAckermann::ControllerFrontSteerPID::control_step( co.steer_ang = setpoint_steer_ang; // Mainly for the case of v=0 } -void DynamicsAckermann::ControllerFrontSteerPID::load_config( - const rapidxml::xml_node& node) +void DynamicsAckermann::ControllerFrontSteerPID::load_config(const rapidxml::xml_node& node) { TParameterDefinitions params; params["KP"] = TParamEntry("%lf", &KP); diff --git a/modules/simulator/src/VehicleDynamics/VehicleAckermann_ControllerRaw.cpp b/modules/simulator/src/VehicleDynamics/VehicleAckermann_ControllerRaw.cpp index d888b6c7..47dd7411 100644 --- a/modules/simulator/src/VehicleDynamics/VehicleAckermann_ControllerRaw.cpp +++ b/modules/simulator/src/VehicleDynamics/VehicleAckermann_ControllerRaw.cpp @@ -14,8 +14,7 @@ using namespace mvsim; using namespace std; -DynamicsAckermann::ControllerRawForces::ControllerRawForces( - DynamicsAckermann& veh) +DynamicsAckermann::ControllerRawForces::ControllerRawForces(DynamicsAckermann& veh) : ControllerBase(veh), setpoint_wheel_torque_l(0), setpoint_wheel_torque_r(0), @@ -33,8 +32,7 @@ void DynamicsAckermann::ControllerRawForces::control_step( co.steer_ang = this->setpoint_steer_ang; } -void DynamicsAckermann::ControllerRawForces::load_config( - const rapidxml::xml_node& node) +void DynamicsAckermann::ControllerRawForces::load_config(const rapidxml::xml_node& node) { TParameterDefinitions params; params["fl_torque"] = TParamEntry("%lf", &setpoint_wheel_torque_l); diff --git a/modules/simulator/src/VehicleDynamics/VehicleAckermann_ControllerTwistFrontSteerPID.cpp b/modules/simulator/src/VehicleDynamics/VehicleAckermann_ControllerTwistFrontSteerPID.cpp index 2e60dee0..b15a1019 100644 --- a/modules/simulator/src/VehicleDynamics/VehicleAckermann_ControllerTwistFrontSteerPID.cpp +++ b/modules/simulator/src/VehicleDynamics/VehicleAckermann_ControllerTwistFrontSteerPID.cpp @@ -25,8 +25,7 @@ DynamicsAckermann::ControllerTwistFrontSteerPID::ControllerTwistFrontSteerPID( max_torque(100.0) { // Get distance between wheels: - dist_fWheels_ = - veh_.wheels_info_[WHEEL_FL].y - veh_.wheels_info_[WHEEL_FR].y; + dist_fWheels_ = veh_.wheels_info_[WHEEL_FL].y - veh_.wheels_info_[WHEEL_FR].y; r2f_L_ = veh_.wheels_info_[WHEEL_FL].x - veh_.wheels_info_[WHEEL_RL].x; ASSERT_(dist_fWheels_ > 0.0); ASSERT_(r2f_L_ > 0.0); @@ -34,8 +33,7 @@ DynamicsAckermann::ControllerTwistFrontSteerPID::ControllerTwistFrontSteerPID( // See base class docs void DynamicsAckermann::ControllerTwistFrontSteerPID::control_step( - const DynamicsAckermann::TControllerInput& ci, - DynamicsAckermann::TControllerOutput& co) + const DynamicsAckermann::TControllerInput& ci, DynamicsAckermann::TControllerOutput& co) { // For each wheel: // 1) Compute desired velocity set-point (in m/s) @@ -55,9 +53,8 @@ void DynamicsAckermann::ControllerTwistFrontSteerPID::control_step( // Desired velocities for each wheel // In local vehicle frame: - const std::vector desired_wheel_vels = - veh_.getWheelsVelocityLocal( - mrpt::math::TTwist2D(setpoint_lin_speed, 0.0, setpoint_ang_speed)); + const std::vector desired_wheel_vels = veh_.getWheelsVelocityLocal( + mrpt::math::TTwist2D(setpoint_lin_speed, 0.0, setpoint_ang_speed)); ASSERT_(desired_wheel_vels.size() == 4); @@ -65,20 +62,17 @@ void DynamicsAckermann::ControllerTwistFrontSteerPID::control_step( // FL: double vel_fl, vel_fr; double desired_fl_steer_ang, desired_fr_steer_ang; - veh_.computeFrontWheelAngles( - co.steer_ang, desired_fl_steer_ang, desired_fr_steer_ang); + veh_.computeFrontWheelAngles(co.steer_ang, desired_fl_steer_ang, desired_fr_steer_ang); { const mrpt::poses::CPose2D wRotInv(0, 0, -desired_fl_steer_ang); mrpt::math::TPoint2D vel_w; - wRotInv.composePoint( - desired_wheel_vels[DynamicsAckermann::WHEEL_FL], vel_w); + wRotInv.composePoint(desired_wheel_vels[DynamicsAckermann::WHEEL_FL], vel_w); vel_fl = vel_w.x; } { const mrpt::poses::CPose2D wRotInv(0, 0, -desired_fr_steer_ang); mrpt::math::TPoint2D vel_w; - wRotInv.composePoint( - desired_wheel_vels[DynamicsAckermann::WHEEL_FR], vel_w); + wRotInv.composePoint(desired_wheel_vels[DynamicsAckermann::WHEEL_FR], vel_w); vel_fr = vel_w.x; } @@ -91,23 +85,19 @@ void DynamicsAckermann::ControllerTwistFrontSteerPID::control_step( veh_.getWheelsVelocityLocal(veh_.getVelocityLocalOdoEstimate()); ASSERT_(odo_wheel_vels.size() == 4); - const double actual_fl_steer_ang = - veh_.getWheelInfo(DynamicsAckermann::WHEEL_FL).yaw; - const double actual_fr_steer_ang = - veh_.getWheelInfo(DynamicsAckermann::WHEEL_FR).yaw; + const double actual_fl_steer_ang = veh_.getWheelInfo(DynamicsAckermann::WHEEL_FL).yaw; + const double actual_fr_steer_ang = veh_.getWheelInfo(DynamicsAckermann::WHEEL_FR).yaw; { const mrpt::poses::CPose2D wRotInv(0, 0, -actual_fl_steer_ang); mrpt::math::TPoint2D vel_w; - wRotInv.composePoint( - odo_wheel_vels[DynamicsAckermann::WHEEL_FL], vel_w); + wRotInv.composePoint(odo_wheel_vels[DynamicsAckermann::WHEEL_FL], vel_w); act_vel_fl = vel_w.x; } { const mrpt::poses::CPose2D wRotInv(0, 0, -actual_fr_steer_ang); mrpt::math::TPoint2D vel_w; - wRotInv.composePoint( - odo_wheel_vels[DynamicsAckermann::WHEEL_FR], vel_w); + wRotInv.composePoint(odo_wheel_vels[DynamicsAckermann::WHEEL_FR], vel_w); act_vel_fr = vel_w.x; } } diff --git a/modules/simulator/src/VehicleDynamics/VehicleAckermann_Drivetrain.cpp b/modules/simulator/src/VehicleDynamics/VehicleAckermann_Drivetrain.cpp index 761ddff7..f8827f78 100644 --- a/modules/simulator/src/VehicleDynamics/VehicleAckermann_Drivetrain.cpp +++ b/modules/simulator/src/VehicleDynamics/VehicleAckermann_Drivetrain.cpp @@ -73,8 +73,7 @@ void DynamicsAckermannDrivetrain::dynamics_load_params_from_xml( const rapidxml::xml_node* xml_node) { // - const rapidxml::xml_node* xml_chassis = - xml_node->first_node("chassis"); + const rapidxml::xml_node* xml_chassis = xml_node->first_node("chassis"); if (xml_chassis) { // Attribs: @@ -89,8 +88,7 @@ void DynamicsAckermannDrivetrain::dynamics_load_params_from_xml( "[DynamicsAckermannDrivetrain::dynamics_load_params_from_xml]"); // Shape node (optional, fallback to default shape if none found) - const rapidxml::xml_node* xml_shape = - xml_chassis->first_node("shape"); + const rapidxml::xml_node* xml_shape = xml_chassis->first_node("shape"); if (xml_shape) mvsim::parse_xmlnode_shape( *xml_shape, chassis_poly_, @@ -117,8 +115,7 @@ void DynamicsAckermannDrivetrain::dynamics_load_params_from_xml( else { world_->logFmt( - mrpt::system::LVL_WARN, - "No XML entry '%s' found: using defaults for wheel #%u", + mrpt::system::LVL_WARN, "No XML entry '%s' found: using defaults for wheel #%u", w_names[i], static_cast(i)); } } @@ -137,8 +134,7 @@ void DynamicsAckermannDrivetrain::dynamics_load_params_from_xml( ack_ps["max_steer_ang_deg"] = TParamEntry("%lf_deg", &max_steer_ang_); parse_xmlnode_children_as_param( - *xml_node, ack_ps, {}, - "[DynamicsAckermannDrivetrain::dynamics_load_params_from_xml]"); + *xml_node, ack_ps, {}, "[DynamicsAckermannDrivetrain::dynamics_load_params_from_xml]"); // Front-left: wheels_info_[WHEEL_FL].x = front_x; @@ -153,8 +149,7 @@ void DynamicsAckermannDrivetrain::dynamics_load_params_from_xml( const char* drive_names[6] = {"open_front", "open_rear", "open_4wd", "torsen_front", "torsen_rear", "torsen_4wd"}; - const rapidxml::xml_node* xml_drive = - xml_node->first_node("drivetrain"); + const rapidxml::xml_node* xml_drive = xml_node->first_node("drivetrain"); if (xml_drive) { TParameterDefinitions attribs; @@ -177,12 +172,9 @@ void DynamicsAckermannDrivetrain::dynamics_load_params_from_xml( TParameterDefinitions drive_params; drive_params["front_rear_split"] = TParamEntry("%lf", &FrontRearSplit_); drive_params["front_rear_bias"] = TParamEntry("%lf", &FrontRearBias_); - drive_params["front_left_right_split"] = - TParamEntry("%lf", &FrontLRSplit_); - drive_params["front_left_right_bias"] = - TParamEntry("%lf", &FrontLRBias_); - drive_params["rear_left_right_split"] = - TParamEntry("%lf", &RearLRSplit_); + drive_params["front_left_right_split"] = TParamEntry("%lf", &FrontLRSplit_); + drive_params["front_left_right_bias"] = TParamEntry("%lf", &FrontLRBias_); + drive_params["rear_left_right_split"] = TParamEntry("%lf", &RearLRSplit_); drive_params["rear_left_right_bias"] = TParamEntry("%lf", &RearLRBias_); parse_xmlnode_children_as_param( @@ -193,12 +185,10 @@ void DynamicsAckermannDrivetrain::dynamics_load_params_from_xml( // Vehicle controller: // ------------------------------------------------- { - const rapidxml::xml_node* xml_control = - xml_node->first_node("controller"); + const rapidxml::xml_node* xml_control = xml_node->first_node("controller"); if (xml_control) { - rapidxml::xml_attribute* control_class = - xml_control->first_attribute("class"); + rapidxml::xml_attribute* control_class = xml_control->first_attribute("class"); if (!control_class || !control_class->value()) throw runtime_error( "[DynamicsAckermannDrivetrain] Missing 'class' attribute " @@ -208,8 +198,7 @@ void DynamicsAckermannDrivetrain::dynamics_load_params_from_xml( if (sCtrlClass == ControllerRawForces::class_name()) controller_ = std::make_shared(*this); else if (sCtrlClass == ControllerTwistFrontSteerPID::class_name()) - controller_ = - std::make_shared(*this); + controller_ = std::make_shared(*this); else if (sCtrlClass == ControllerFrontSteerPID::class_name()) controller_ = std::make_shared(*this); else @@ -223,8 +212,7 @@ void DynamicsAckermannDrivetrain::dynamics_load_params_from_xml( } // Default controller: - if (!controller_) - controller_ = std::make_shared(*this); + if (!controller_) controller_ = std::make_shared(*this); } // See docs in base class: @@ -281,9 +269,8 @@ std::vector DynamicsAckermannDrivetrain::invoke_motor_controllers( torque_split_per_wheel[WHEEL_RR] = 0.0; computeDiffTorqueSplit( - wheels_info_[WHEEL_FL].getW(), - wheels_info_[WHEEL_FR].getW(), FrontLRBias_, FrontLRSplit_, - torque_split_per_wheel[WHEEL_FL], + wheels_info_[WHEEL_FL].getW(), wheels_info_[WHEEL_FR].getW(), FrontLRBias_, + FrontLRSplit_, torque_split_per_wheel[WHEEL_FL], torque_split_per_wheel[WHEEL_FR]); } break; @@ -294,9 +281,8 @@ std::vector DynamicsAckermannDrivetrain::invoke_motor_controllers( torque_split_per_wheel[WHEEL_FR] = 0.0; computeDiffTorqueSplit( - wheels_info_[WHEEL_RL].getW(), - wheels_info_[WHEEL_RR].getW(), RearLRBias_, RearLRSplit_, - torque_split_per_wheel[WHEEL_RL], + wheels_info_[WHEEL_RL].getW(), wheels_info_[WHEEL_RR].getW(), RearLRBias_, + RearLRSplit_, torque_split_per_wheel[WHEEL_RL], torque_split_per_wheel[WHEEL_RR]); } break; @@ -313,20 +299,17 @@ std::vector DynamicsAckermannDrivetrain::invoke_motor_controllers( double t_F = 0.0; double t_R = 0.0; // front-rear - computeDiffTorqueSplit( - w_F, w_R, FrontRearBias_, FrontRearSplit_, t_F, t_R); + computeDiffTorqueSplit(w_F, w_R, FrontRearBias_, FrontRearSplit_, t_F, t_R); double t_FL = 0.0; double t_FR = 0.0; // front left-right - computeDiffTorqueSplit( - w_FL, w_FR, FrontLRBias_, FrontLRSplit_, t_FL, t_FR); + computeDiffTorqueSplit(w_FL, w_FR, FrontLRBias_, FrontLRSplit_, t_FL, t_FR); double t_RL = 0.0; double t_RR = 0.0; // rear left-right - computeDiffTorqueSplit( - w_RL, w_RR, RearLRBias_, RearLRSplit_, t_RL, t_RR); + computeDiffTorqueSplit(w_RL, w_RR, RearLRBias_, RearLRSplit_, t_RL, t_RR); torque_split_per_wheel[WHEEL_FL] = t_F * t_FL; torque_split_per_wheel[WHEEL_FR] = t_F * t_FR; @@ -348,23 +331,20 @@ std::vector DynamicsAckermannDrivetrain::invoke_motor_controllers( ASSERT_(out_torque_per_wheel.size() == 4); for (int i = 0; i < 4; i++) { - out_torque_per_wheel[i] = - co.drive_torque * torque_split_per_wheel[i]; + out_torque_per_wheel[i] = co.drive_torque * torque_split_per_wheel[i]; } // Kinematically-driven steering wheels: // Ackermann formulas for inner&outer weels turning angles wrt the // equivalent (central) one: computeFrontWheelAngles( - co.steer_ang, wheels_info_[WHEEL_FL].yaw, - wheels_info_[WHEEL_FR].yaw); + co.steer_ang, wheels_info_[WHEEL_FL].yaw, wheels_info_[WHEEL_FR].yaw); } return out_torque_per_wheel; } void DynamicsAckermannDrivetrain::computeFrontWheelAngles( - const double desired_equiv_steer_ang, double& out_fl_ang, - double& out_fr_ang) const + const double desired_equiv_steer_ang, double& out_fl_ang, double& out_fr_ang) const { // EQ1: cot(d)+0.5*w/l = cot(do) // EQ2: cot(di)=cot(do)-w/l @@ -372,8 +352,7 @@ void DynamicsAckermannDrivetrain::computeFrontWheelAngles( const double l = wheels_info_[WHEEL_FL].x - wheels_info_[WHEEL_RL].x; ASSERT_(l > 0); const double w_l = w / l; - const double delta = - b2Clamp(std::abs(desired_equiv_steer_ang), 0.0, max_steer_ang_); + const double delta = b2Clamp(std::abs(desired_equiv_steer_ang), 0.0, max_steer_ang_); const bool delta_neg = (desired_equiv_steer_ang < 0); ASSERT_LT_(delta, 0.5 * M_PI - 0.01); @@ -381,15 +360,13 @@ void DynamicsAckermannDrivetrain::computeFrontWheelAngles( const double cot_di = cot_do - w_l; // delta>0: do->right, di->left wheel // delta<0: do->left , di->right wheel - (delta_neg ? out_fr_ang : out_fl_ang) = - atan(1.0 / cot_di) * (delta_neg ? -1.0 : 1.0); - (delta_neg ? out_fl_ang : out_fr_ang) = - atan(1.0 / cot_do) * (delta_neg ? -1.0 : 1.0); + (delta_neg ? out_fr_ang : out_fl_ang) = atan(1.0 / cot_di) * (delta_neg ? -1.0 : 1.0); + (delta_neg ? out_fl_ang : out_fr_ang) = atan(1.0 / cot_do) * (delta_neg ? -1.0 : 1.0); } void DynamicsAckermannDrivetrain::computeDiffTorqueSplit( - const double w1, const double w2, const double diffBias, - const double splitRatio, double& t1, double& t2) + const double w1, const double w2, const double diffBias, const double splitRatio, double& t1, + double& t2) { if (mrpt::signWithZero(w1) == 0.0 || mrpt::signWithZero(w2) == 0.0) { @@ -406,11 +383,10 @@ void DynamicsAckermannDrivetrain::computeDiffTorqueSplit( const double delta = omegaMax - diffBias * omegaMin; const double deltaTorque = (delta > 0) ? delta / omegaMax : 0.0f; - const double f1 = (w1Abs - w2Abs > 0) ? splitRatio * (1.0f - deltaTorque) - : splitRatio * (1.0f + deltaTorque); - const double f2 = (w1Abs - w2Abs > 0) - ? (1.0f - splitRatio) * (1.0f + deltaTorque) - : (1.0f - splitRatio) * (1.0f - deltaTorque); + const double f1 = + (w1Abs - w2Abs > 0) ? splitRatio * (1.0f - deltaTorque) : splitRatio * (1.0f + deltaTorque); + const double f2 = (w1Abs - w2Abs > 0) ? (1.0f - splitRatio) * (1.0f + deltaTorque) + : (1.0f - splitRatio) * (1.0f - deltaTorque); const double denom = 1.0f / (f1 + f2); t1 = f1 * denom; @@ -418,8 +394,7 @@ void DynamicsAckermannDrivetrain::computeDiffTorqueSplit( } // See docs in base class: -mrpt::math::TTwist2D DynamicsAckermannDrivetrain::getVelocityLocalOdoEstimate() - const +mrpt::math::TTwist2D DynamicsAckermannDrivetrain::getVelocityLocalOdoEstimate() const { mrpt::math::TTwist2D odo_vel; // Equations: diff --git a/modules/simulator/src/VehicleDynamics/VehicleDifferential.cpp b/modules/simulator/src/VehicleDynamics/VehicleDifferential.cpp index fc60ff35..1150214d 100644 --- a/modules/simulator/src/VehicleDynamics/VehicleDifferential.cpp +++ b/modules/simulator/src/VehicleDynamics/VehicleDifferential.cpp @@ -21,8 +21,7 @@ using namespace std; // Ctor: DynamicsDifferential::DynamicsDifferential( World* parent, const std::vector& cfgPerWheel) - : VehicleBase(parent, cfgPerWheel.size() /*num wheels*/), - configPerWheel_(cfgPerWheel) + : VehicleBase(parent, cfgPerWheel.size() /*num wheels*/), configPerWheel_(cfgPerWheel) { using namespace mrpt::math; @@ -46,14 +45,12 @@ DynamicsDifferential::DynamicsDifferential( } /** The derived-class part of load_params_from_xml() */ -void DynamicsDifferential::dynamics_load_params_from_xml( - const rapidxml::xml_node* xml_node) +void DynamicsDifferential::dynamics_load_params_from_xml(const rapidxml::xml_node* xml_node) { const std::map varValues = {{"NAME", name_}}; // - const rapidxml::xml_node* xml_chassis = - xml_node->first_node("chassis"); + const rapidxml::xml_node* xml_chassis = xml_node->first_node("chassis"); if (xml_chassis) { // Attribs: @@ -68,12 +65,10 @@ void DynamicsDifferential::dynamics_load_params_from_xml( "[DynamicsDifferential::dynamics_load_params_from_xml]"); // Shape node (optional, fallback to default shape if none found) - const rapidxml::xml_node* xml_shape = - xml_chassis->first_node("shape"); + const rapidxml::xml_node* xml_shape = xml_chassis->first_node("shape"); if (xml_shape) mvsim::parse_xmlnode_shape( - *xml_shape, chassis_poly_, - "[DynamicsDifferential::dynamics_load_params_from_xml]"); + *xml_shape, chassis_poly_, "[DynamicsDifferential::dynamics_load_params_from_xml]"); } // , @@ -86,8 +81,7 @@ void DynamicsDifferential::dynamics_load_params_from_xml( { const auto& cpw = configPerWheel_.at(i); - const rapidxml::xml_node* xml_wheel = - xml_node->first_node(cpw.name.c_str()); + const rapidxml::xml_node* xml_wheel = xml_node->first_node(cpw.name.c_str()); if (xml_wheel) wheels_info_[i].loadFromXML(xml_wheel); else @@ -100,12 +94,10 @@ void DynamicsDifferential::dynamics_load_params_from_xml( // Vehicle controller: // ------------------------------------------------- { - const rapidxml::xml_node* xml_control = - xml_node->first_node("controller"); + const rapidxml::xml_node* xml_control = xml_node->first_node("controller"); if (xml_control) { - rapidxml::xml_attribute* control_class = - xml_control->first_attribute("class"); + rapidxml::xml_attribute* control_class = xml_control->first_attribute("class"); if (!control_class || !control_class->value()) throw runtime_error( "[DynamicsDifferential] Missing 'class' attribute in " @@ -129,13 +121,11 @@ void DynamicsDifferential::dynamics_load_params_from_xml( } // Default controller: - if (!controller_) - controller_ = std::make_shared(*this); + if (!controller_) controller_ = std::make_shared(*this); } // See docs in base class: -std::vector DynamicsDifferential::invoke_motor_controllers( - const TSimulContext& context) +std::vector DynamicsDifferential::invoke_motor_controllers(const TSimulContext& context) { // Longitudinal forces at each wheel: std::vector otpw; @@ -174,8 +164,7 @@ std::vector DynamicsDifferential::invoke_motor_controllers( return otpw; } -void DynamicsDifferential::invoke_motor_controllers_post_step( - const TSimulContext& context) +void DynamicsDifferential::invoke_motor_controllers_post_step(const TSimulContext& context) { if (controller_) controller_->on_post_step(context); } diff --git a/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerRaw.cpp b/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerRaw.cpp index 17402a31..cb1599b3 100644 --- a/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerRaw.cpp +++ b/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerRaw.cpp @@ -66,6 +66,5 @@ void DynamicsDifferential::ControllerRawForces::teleop_interface( "w/s=incr/decr both torques.\n" "a/d=left/right. spacebar=stop.\n"; out.append_gui_lines += mrpt::format( - "setpoint: tl=%.03f tr=%.03f deg\n", setpoint_wheel_torque_l, - setpoint_wheel_torque_r); + "setpoint: tl=%.03f tr=%.03f deg\n", setpoint_wheel_torque_l, setpoint_wheel_torque_r); } diff --git a/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistIdeal.cpp b/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistIdeal.cpp index 60cba511..be52e105 100644 --- a/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistIdeal.cpp +++ b/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistIdeal.cpp @@ -11,8 +11,7 @@ using namespace mvsim; -DynamicsDifferential::ControllerTwistIdeal::ControllerTwistIdeal( - DynamicsDifferential& veh) +DynamicsDifferential::ControllerTwistIdeal::ControllerTwistIdeal(DynamicsDifferential& veh) : ControllerBase(veh) { // Get distance between wheels: @@ -113,6 +112,5 @@ void DynamicsDifferential::ControllerTwistIdeal::teleop_interface( } out.append_gui_lines += mrpt::format( - "setpoint: lin=%.03f ang=%.03f deg/s\n", setpoint_.vx, - 180.0 / M_PI * setpoint_.omega); + "setpoint: lin=%.03f ang=%.03f deg/s\n", setpoint_.vx, 180.0 / M_PI * setpoint_.omega); } diff --git a/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistPID.cpp b/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistPID.cpp index 1680294b..bf810bf8 100644 --- a/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistPID.cpp +++ b/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistPID.cpp @@ -14,8 +14,7 @@ using namespace mvsim; using namespace std; -DynamicsDifferential::ControllerTwistPID::ControllerTwistPID( - DynamicsDifferential& veh) +DynamicsDifferential::ControllerTwistPID::ControllerTwistPID(DynamicsDifferential& veh) : ControllerBase(veh) { // Get distance between wheels: @@ -27,8 +26,7 @@ DynamicsDifferential::ControllerTwistPID::ControllerTwistPID( // See base class docs void DynamicsDifferential::ControllerTwistPID::control_step( - const DynamicsDifferential::TControllerInput& ci, - DynamicsDifferential::TControllerOutput& co) + const DynamicsDifferential::TControllerInput& ci, DynamicsDifferential::TControllerOutput& co) { const auto sp = setpoint(); @@ -75,8 +73,7 @@ void DynamicsDifferential::ControllerTwistPID::control_step( } } -void DynamicsDifferential::ControllerTwistPID::load_config( - const rapidxml::xml_node& node) +void DynamicsDifferential::ControllerTwistPID::load_config(const rapidxml::xml_node& node) { TParameterDefinitions params; params["KP"] = TParamEntry("%lf", &KP); @@ -170,6 +167,5 @@ void DynamicsDifferential::ControllerTwistPID::teleop_interface( } out.append_gui_lines += mrpt::format( - "setpoint: lin=%.03f ang=%.03f deg/s\n", setpoint_.vx, - 180.0 / M_PI * setpoint_.omega); + "setpoint: lin=%.03f ang=%.03f deg/s\n", setpoint_.vx, 180.0 / M_PI * setpoint_.omega); } diff --git a/modules/simulator/src/VehicleDynamics/Vehicleackermann_Drivetrain_ControllerTwistFrontSteerPID.cpp b/modules/simulator/src/VehicleDynamics/Vehicleackermann_Drivetrain_ControllerTwistFrontSteerPID.cpp index 9da96b82..d4e49411 100644 --- a/modules/simulator/src/VehicleDynamics/Vehicleackermann_Drivetrain_ControllerTwistFrontSteerPID.cpp +++ b/modules/simulator/src/VehicleDynamics/Vehicleackermann_Drivetrain_ControllerTwistFrontSteerPID.cpp @@ -14,8 +14,8 @@ using namespace mvsim; using namespace std; -DynamicsAckermannDrivetrain::ControllerTwistFrontSteerPID:: - ControllerTwistFrontSteerPID(DynamicsAckermannDrivetrain& veh) +DynamicsAckermannDrivetrain::ControllerTwistFrontSteerPID::ControllerTwistFrontSteerPID( + DynamicsAckermannDrivetrain& veh) : ControllerBase(veh), setpoint_lin_speed(0), setpoint_ang_speed(0), @@ -25,8 +25,7 @@ DynamicsAckermannDrivetrain::ControllerTwistFrontSteerPID:: max_torque(400.0) { // Get distance between wheels: - dist_fWheels_ = - veh_.wheels_info_[WHEEL_FL].y - veh_.wheels_info_[WHEEL_FR].y; + dist_fWheels_ = veh_.wheels_info_[WHEEL_FL].y - veh_.wheels_info_[WHEEL_FR].y; r2f_L_ = veh_.wheels_info_[WHEEL_FL].x - veh_.wheels_info_[WHEEL_RL].x; ASSERT_(dist_fWheels_ > 0.0); @@ -77,8 +76,8 @@ void DynamicsAckermannDrivetrain::ControllerTwistFrontSteerPID::load_config( parse_xmlnode_children_as_param(node, params); } -void DynamicsAckermannDrivetrain::ControllerTwistFrontSteerPID:: - teleop_interface(const TeleopInput& in, TeleopOutput& out) +void DynamicsAckermannDrivetrain::ControllerTwistFrontSteerPID::teleop_interface( + const TeleopInput& in, TeleopOutput& out) { ControllerBase::teleop_interface(in, out); diff --git a/modules/simulator/src/VisualObject.cpp b/modules/simulator/src/VisualObject.cpp index 19c62f91..478e0399 100644 --- a/modules/simulator/src/VisualObject.cpp +++ b/modules/simulator/src/VisualObject.cpp @@ -30,8 +30,7 @@ static std::atomic_int32_t g_uniqueCustomVisualId = 0; double VisualObject::GeometryEpsilon = 1e-3; VisualObject::VisualObject( - World* parent, bool insertCustomVizIntoViz, - bool insertCustomVizIntoPhysical) + World* parent, bool insertCustomVizIntoViz, bool insertCustomVizIntoPhysical) : world_(parent), insertCustomVizIntoViz_(insertCustomVizIntoViz), insertCustomVizIntoPhysical_(insertCustomVizIntoPhysical) @@ -54,8 +53,7 @@ void VisualObject::guiUpdate( // If "viz" does not have a value, it's because we are already inside a // setPose() change event, so my caller already holds the mutex and we don't // need/can't acquire it again: - const auto objectPose = - viz.has_value() ? meSim->getPose() : meSim->getPoseNoLock(); + const auto objectPose = viz.has_value() ? meSim->getPose() : meSim->getPoseNoLock(); if (glCustomVisual_ && viz.has_value() && physical.has_value()) { @@ -71,8 +69,7 @@ void VisualObject::guiUpdate( // Add to the 3D scene: if (insertCustomVizIntoViz_) viz->get().insert(glCustomVisual_); - if (insertCustomVizIntoPhysical_) - physical->get().insert(glCustomVisual_); + if (insertCustomVizIntoPhysical_) physical->get().insert(glCustomVisual_); } // Update pose: @@ -158,8 +155,7 @@ bool VisualObject::parseVisual(const rapidxml::xml_node& rootNode) MRPT_TRY_START bool any = false; - for (auto n = rootNode.first_node("visual"); n; - n = n->next_sibling("visual")) + for (auto n = rootNode.first_node("visual"); n; n = n->next_sibling("visual")) { bool hasViz = implParseVisual(*n); any = any || hasViz; @@ -235,8 +231,7 @@ bool VisualObject::implParseVisual(const rapidxml::xml_node& visNode) // Add the 3D model as custom viz: addCustomVisualization( - glModel, mrpt::poses::CPose3D(modelPose), modelScale, objectName, - modelURI); + glModel, mrpt::poses::CPose3D(modelPose), modelScale, objectName, modelURI); return true; // yes, we have a custom viz model @@ -262,10 +257,9 @@ bool VisualObject::customVisualVisible() const } void VisualObject::addCustomVisualization( - const mrpt::opengl::CRenderizable::Ptr& glModel, - const mrpt::poses::CPose3D& modelPose, const float modelScale, - const std::string& modelName, const std::optional& modelURI, - const bool initialShowBoundingBox) + const mrpt::opengl::CRenderizable::Ptr& glModel, const mrpt::poses::CPose3D& modelPose, + const float modelScale, const std::string& modelName, + const std::optional& modelURI, const bool initialShowBoundingBox) { ASSERT_(glModel); @@ -288,8 +282,7 @@ void VisualObject::addCustomVisualization( #endif // Calculate its convex hull: - const auto shape = - chc.get(*glModel, zMin, zMax, modelPose, modelScale, modelURI); + const auto shape = chc.get(*glModel, zMin, zMax, modelPose, modelScale, modelURI); auto glGroup = mrpt::opengl::CSetOfObjects::Create(); diff --git a/modules/simulator/src/Wheel.cpp b/modules/simulator/src/Wheel.cpp index dff22b2e..89a0c970 100644 --- a/modules/simulator/src/Wheel.cpp +++ b/modules/simulator/src/Wheel.cpp @@ -24,8 +24,7 @@ using namespace std; Wheel::Wheel(World* world) : VisualObject(world) { recalcInertia(); } -void Wheel::getAs3DObject( - mrpt::opengl::CSetOfObjects& obj, bool isPhysicalScene) +void Wheel::getAs3DObject(mrpt::opengl::CSetOfObjects& obj, bool isPhysicalScene) { obj.clear(); @@ -35,11 +34,10 @@ void Wheel::getAs3DObject( } else { - auto gl_wheel = mrpt::opengl::CCylinder::Create( - 0.5 * diameter, 0.5 * diameter, this->width, 15); + auto gl_wheel = + mrpt::opengl::CCylinder::Create(0.5 * diameter, 0.5 * diameter, this->width, 15); gl_wheel->setColor_u8(color); - gl_wheel->setPose( - mrpt::poses::CPose3D(0, 0.5 * width, 0, 0, 0, mrpt::DEG2RAD(90))); + gl_wheel->setPose(mrpt::poses::CPose3D(0, 0.5 * width, 0, 0, 0, mrpt::DEG2RAD(90))); if (!isPhysicalScene) { @@ -48,8 +46,7 @@ void Wheel::getAs3DObject( gl_wheel_frame->insert(gl_wheel); { mrpt::opengl::CSetOfObjects::Ptr gl_xyz = - mrpt::opengl::stock_objects::CornerXYZSimple( - 0.9 * diameter, 2.0); + mrpt::opengl::stock_objects::CornerXYZSimple(0.9 * diameter, 2.0); #if MRPT_VERSION >= 0x270 gl_xyz->castShadows(false); #endif @@ -69,8 +66,7 @@ void Wheel::loadFromXML(const rapidxml::xml_node* xml_node) // // pos: - if (const auto attr = xml_node->first_attribute("pos"); - attr && attr->value()) + if (const auto attr = xml_node->first_attribute("pos"); attr && attr->value()) { const std::string sAttr = attr->value(); mrpt::math::TPose2D v = parseXYPHI(sAttr, true); @@ -101,8 +97,7 @@ void Wheel::recalcInertia() void Wheel::internalGuiUpdate( [[maybe_unused]] const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& - physical, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { // nothing to do, already done in getAs3DObject() diff --git a/modules/simulator/src/World.cpp b/modules/simulator/src/World.cpp index 70630d15..f889b0fd 100644 --- a/modules/simulator/src/World.cpp +++ b/modules/simulator/src/World.cpp @@ -23,10 +23,7 @@ using namespace mvsim; using namespace std; // Default ctor: inits empty world. -World::World() : mrpt::system::COutputLogger("mvsim::World") -{ - this->clear_all(); -} +World::World() : mrpt::system::COutputLogger("mvsim::World") { this->clear_all(); } // Dtor. World::~World() @@ -76,12 +73,11 @@ void World::internal_initialize() ASSERT_(worldVisual_); #if MRPT_VERSION >= 0x270 - worldVisual_->getViewport()->lightParameters().ambient = - lightOptions_.light_ambient; + worldVisual_->getViewport()->lightParameters().ambient = lightOptions_.light_ambient; #else worldVisual_->getViewport()->lightParameters().ambient = { - lightOptions_.light_ambient, lightOptions_.light_ambient, - lightOptions_.light_ambient, 1.0f}; + lightOptions_.light_ambient, lightOptions_.light_ambient, lightOptions_.light_ambient, + 1.0f}; #endif // Physical world light = visual world lights: worldPhysical_.getViewport()->lightParameters() = @@ -109,8 +105,7 @@ void World::run_simulation(double dt) const double t0 = mrpt::Clock::nowDouble(); // Define start of simulation time: - if (!simul_start_wallclock_time_.has_value()) - simul_start_wallclock_time_ = t0 - dt; + if (!simul_start_wallclock_time_.has_value()) simul_start_wallclock_time_ = t0 - dt; timlogger_.registerUserMeasure("run_simulation.dt", dt); @@ -131,8 +126,7 @@ void World::run_simulation(double dt) const double remainingTime = end_time - get_simul_time(); if (remainingTime <= 0) break; - internal_one_timestep( - remainingTime >= simulTimestep ? simulTimestep : remainingTime); + internal_one_timestep(remainingTime >= simulTimestep ? simulTimestep : remainingTime); // IMPORTANT: This must be inside the loop to allow breaking if we are // closing the app and simulatedTime is not ticking anymore. @@ -171,8 +165,7 @@ void World::internal_one_timestep(double dt) // 2) Run dynamics { - mrpt::system::CTimeLoggerEntry tle( - timlogger_, "timestep.1.dynamics_integrator"); + mrpt::system::CTimeLoggerEntry tle(timlogger_, "timestep.1.dynamics_integrator"); box2d_world_->Step(dt, b2dVelIters_, b2dPosIters_); @@ -186,8 +179,7 @@ void World::internal_one_timestep(double dt) // can be answered straight away without waiting for the main simulation // mutex: { - mrpt::system::CTimeLoggerEntry tle( - timlogger_, "timestep.3.save_dynstate"); + mrpt::system::CTimeLoggerEntry tle(timlogger_, "timestep.3.save_dynstate"); const auto lckPhys = mrpt::lockHelper(physical_objects_mtx()); const auto lckCopy = mrpt::lockHelper(copy_of_objects_dynstate_mtx_); @@ -202,16 +194,14 @@ void World::internal_one_timestep(double dt) copy_of_objects_dynstate_pose_[e.first] = e.second->getPose(); copy_of_objects_dynstate_twist_[e.first] = e.second->getTwist(); - if (e.second->hadCollision()) - copy_of_objects_had_collision_.insert(e.first); + if (e.second->hadCollision()) copy_of_objects_had_collision_.insert(e.first); } } { const auto lckCollis = mrpt::lockHelper(reset_collision_flags_mtx_); for (const auto& sId : reset_collision_flags_) { - if (auto itV = simulableObjects_.find(sId); - itV != simulableObjects_.end()) + if (auto itV = simulableObjects_.find(sId); itV != simulableObjects_.end()) itV->second->resetCollisionFlag(); } reset_collision_flags_.clear(); @@ -219,8 +209,7 @@ void World::internal_one_timestep(double dt) lckListObjs.unlock(); // for simulableObjects_ // 4) Wait for 3D sensors (OpenGL raytrace) to get executed on its thread: - mrpt::system::CTimeLoggerEntry tle4( - timlogger_, "timestep.4.wait_3D_sensors"); + mrpt::system::CTimeLoggerEntry tle4(timlogger_, "timestep.4.wait_3D_sensors"); if (pending_running_sensors_on_3D_scene()) { // Use a huge timeout here to avoid timing out in build farms / cloud @@ -268,8 +257,7 @@ std::string World::local_to_abs_path(const std::string& s_in) const // "X:\*", "/*" // ------------------- bool is_relative = true; - if (s.size() > 2 && s[1] == ':' && (s[2] == '/' || s[2] == '\\')) - is_relative = false; + if (s.size() > 2 && s[1] == ':' && (s[2] == '/' || s[2] == '\\')) is_relative = false; if (s.size() > 0 && (s[0] == '/' || s[0] == '\\')) is_relative = false; if (is_relative) ret = mrpt::system::pathJoin({basePath_, s}); @@ -330,8 +318,7 @@ void World::insertBlock(const Block::Ptr& block) simulableObjects_.insert( simulableObjects_.end(), - std::make_pair( - block->getName(), std::dynamic_pointer_cast(block))); + std::make_pair(block->getName(), std::dynamic_pointer_cast(block))); } double World::get_simul_timestep() const @@ -445,22 +432,19 @@ std::optional World::getJoystickState() const { auto lck = mrpt::lockHelper(gui_.gui_win->background_scene_mtx); auto& cam = gui_.gui_win->camera(); - cam.setAzimuthDegrees( - cam.getAzimuthDegrees() - js.axes[JOY_AXIS_AZIMUTH]); + cam.setAzimuthDegrees(cam.getAzimuthDegrees() - js.axes[JOY_AXIS_AZIMUTH]); } return js; } -void World::dispatchOnObservation( - const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs) +void World::dispatchOnObservation(const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs) { internalOnObservation(veh, obs); for (const auto& cb : callbacksOnObservation_) cb(veh, obs); } -void World::internalOnObservation( - const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs) +void World::internalOnObservation(const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs) { using namespace std::string_literals; @@ -472,19 +456,17 @@ void World::internalOnObservation( { for (const auto& v : vehicles_) { - const std::string fileName = mrpt::system::fileNameChangeExtension( - save_to_rawlog_, v.first + ".rawlog"s); + const std::string fileName = + mrpt::system::fileNameChangeExtension(save_to_rawlog_, v.first + ".rawlog"s); MRPT_LOG_INFO_STREAM("Creating dataset file: " << fileName); - rawlog_io_per_veh_[v.first] = - std::make_shared(fileName); + rawlog_io_per_veh_[v.first] = std::make_shared(fileName); } } // Store: - auto arch = - mrpt::serialization::archiveFrom(*rawlog_io_per_veh_.at(veh.getName())); + auto arch = mrpt::serialization::archiveFrom(*rawlog_io_per_veh_.at(veh.getName())); arch << *obs; } @@ -497,8 +479,7 @@ void World::internalPostSimulStepForRawlog() const double now = get_simul_time(); const double T_odom = 1.0 / rawlog_odometry_rate_; - if (rawlog_last_odom_time_.has_value() && - (now - *rawlog_last_odom_time_) < T_odom) + if (rawlog_last_odom_time_.has_value() && (now - *rawlog_last_odom_time_) < T_odom) { return; // not yet } @@ -566,7 +547,7 @@ void World::internalPostSimulStepForTrajectory() // timestamp x y z q_x q_y q_z q_w gt_io_per_veh_.at(vehName) << mrpt::format( - "%f %f %f %f %f %f %f %f\n", t, p.x(), p.y(), p.z(), p.quat().x(), - p.quat().y(), p.quat().z(), p.quat().w()); + "%f %f %f %f %f %f %f %f\n", t, p.x(), p.y(), p.z(), p.quat().x(), p.quat().y(), + p.quat().z(), p.quat().w()); } } diff --git a/modules/simulator/src/WorldElements/ElevationMap.cpp b/modules/simulator/src/WorldElements/ElevationMap.cpp index e9a1a982..510fdab7 100644 --- a/modules/simulator/src/WorldElements/ElevationMap.cpp +++ b/modules/simulator/src/WorldElements/ElevationMap.cpp @@ -58,11 +58,9 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node* root) params["texture_extension_x"] = TParamEntry("%f", &textureExtensionX_); params["texture_extension_y"] = TParamEntry("%f", &textureExtensionY_); - params["debug_show_contact_points"] = - TParamEntry("%bool", &debugShowContactPoints_); + params["debug_show_contact_points"] = TParamEntry("%bool", &debugShowContactPoints_); - parse_xmlnode_children_as_param( - *root, params, world_->user_defined_variables()); + parse_xmlnode_children_as_param(*root, params, world_->user_defined_variables()); // Load elevation data: mrpt::math::CMatrixFloat elevation_data; @@ -71,8 +69,7 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node* root) sElevationImgFile = world_->local_to_abs_path(sElevationImgFile); mrpt::img::CImage imgElev; - if (!imgElev.loadFromFile( - sElevationImgFile, 0 /*force load grayscale*/)) + if (!imgElev.loadFromFile(sElevationImgFile, 0 /*force load grayscale*/)) throw std::runtime_error(mrpt::format( "[ElevationMap] ERROR: Cannot read elevation image '%s'", sElevationImgFile.c_str())); @@ -87,8 +84,7 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node* root) mrpt::math::CMatrixFloat f = elevation_data; f -= vmin; f *= (img_max_z - img_min_z) / (vmax - vmin); - mrpt::math::CMatrixFloat m( - elevation_data.rows(), elevation_data.cols()); + mrpt::math::CMatrixFloat m(elevation_data.rows(), elevation_data.cols()); m.setConstant(img_min_z); f += m; elevation_data = std::move(f); @@ -107,8 +103,7 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node* root) if (!mesh_image.loadFromFile(sTextureImgFile)) throw std::runtime_error(mrpt::format( - "[ElevationMap] ERROR: Cannot read texture image '%s'", - sTextureImgFile.c_str())); + "[ElevationMap] ERROR: Cannot read texture image '%s'", sTextureImgFile.c_str())); has_mesh_image = true; } @@ -122,8 +117,7 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node* root) gl_mesh_->assignImageAndZ(mesh_image, elevation_data); #if MRPT_VERSION >= 0x270 - gl_mesh_->setMeshTextureExtension( - textureExtensionX_, textureExtensionY_); + gl_mesh_->setMeshTextureExtension(textureExtensionX_, textureExtensionY_); #endif } else @@ -139,17 +133,14 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node* root) const double LX = (elevation_data.rows() - 1) * resolution_; const double LY = (elevation_data.cols() - 1) * resolution_; - if (corner_min_x == std::numeric_limits::max()) - corner_min_x = -0.5 * LX; + if (corner_min_x == std::numeric_limits::max()) corner_min_x = -0.5 * LX; - if (corner_min_y == std::numeric_limits::max()) - corner_min_y = -0.5 * LY; + if (corner_min_y == std::numeric_limits::max()) corner_min_y = -0.5 * LY; // Important: the yMin/yMax in the next line are swapped to handle // the "+y" different direction in image and map coordinates, it is not // a bug: - gl_mesh_->setGridLimits( - corner_min_x, corner_min_x + LX, corner_min_y, corner_min_y + LY); + gl_mesh_->setGridLimits(corner_min_x, corner_min_x + LX, corner_min_y, corner_min_y + LY); gl_debugWheelsContactPoints_ = mrpt::opengl::CPointCloud::Create(); gl_debugWheelsContactPoints_->enableVariablePointSize(false); @@ -179,8 +170,7 @@ void ElevationMap::internalGuiUpdate( } } -void ElevationMap::simul_pre_timestep( - [[maybe_unused]] const TSimulContext& context) +void ElevationMap::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) { // For each vehicle: // 1) Compute its 3D pose according to the mesh tilt angle. @@ -234,8 +224,7 @@ void ElevationMap::simul_pre_timestep( corr.other_z = 0; #endif // Global frame - const mrpt::math::TPoint3D gPt = - cur_cpose.composePoint({wheel.x, wheel.y, 0.0}); + const mrpt::math::TPoint3D gPt = cur_cpose.composePoint({wheel.x, wheel.y, 0.0}); float z; if (!getElevationAt(gPt.x /*in*/, gPt.y /*in*/, z /*out*/)) { @@ -261,8 +250,7 @@ void ElevationMap::simul_pre_timestep( double transf_scale; mrpt::poses::CPose3DQuat tmpl; - mrpt::tfest::se3_l2( - corrs_, tmpl, transf_scale, true /*force scale unity*/); + mrpt::tfest::se3_l2(corrs_, tmpl, transf_scale, true /*force scale unity*/); optimalTf_ = mrpt::poses::CPose3D(tmpl); @@ -279,16 +267,14 @@ void ElevationMap::simul_pre_timestep( if (debugShowContactPoints_) { gl_debugWheelsContactPoints_->clear(); - for (const auto& c : corrs_) - gl_debugWheelsContactPoints_->insertPoint(c.global); + for (const auto& c : corrs_) gl_debugWheelsContactPoints_->insertPoint(c.global); } // compute "down" direction: { mrpt::poses::CPose3D rot_only; rot_only.setRotationMatrix(optimalTf_.getRotationMatrix()); - rot_only.inverseComposePoint( - .0, .0, -1.0, dir_down.x, dir_down.y, dir_down.z); + rot_only.inverseComposePoint(.0, .0, -1.0, dir_down.x, dir_down.y, dir_down.z); } // 2) Apply gravity force @@ -296,11 +282,9 @@ void ElevationMap::simul_pre_timestep( { // To chassis: const double chassis_weight = veh->getChassisMass() * gravity; - const mrpt::math::TPoint2D chassis_com = - veh->getChassisCenterOfMass(); + const mrpt::math::TPoint2D chassis_com = veh->getChassisCenterOfMass(); veh->apply_force( - {dir_down.x * chassis_weight, dir_down.y * chassis_weight}, - chassis_com); + {dir_down.x * chassis_weight, dir_down.y * chassis_weight}, chassis_com); // To wheels: for (size_t iW = 0; iW < nWheels; iW++) @@ -308,8 +292,7 @@ void ElevationMap::simul_pre_timestep( const Wheel& wheel = veh->getWheelInfo(iW); const double wheel_weight = wheel.mass * gravity; veh->apply_force( - {dir_down.x * wheel_weight, dir_down.y * wheel_weight}, - {wheel.x, wheel.y}); + {dir_down.x * wheel_weight, dir_down.y * wheel_weight}, {wheel.x, wheel.y}); } } @@ -334,10 +317,8 @@ static float calcz( (p3.y - p2.y) * (p1.x - p3.x); ASSERT_(det != 0.0f); - const float l1 = - ((p2.x - p3.x) * (y - p3.y) + (p3.y - p2.y) * (x - p3.x)) / det; - const float l2 = - ((p3.x - p1.x) * (y - p3.y) + (p1.y - p3.y) * (x - p3.x)) / det; + const float l1 = ((p2.x - p3.x) * (y - p3.y) + (p3.y - p2.y) * (x - p3.x)) / det; + const float l2 = ((p3.x - p1.x) * (y - p3.y) + (p1.y - p3.y) * (x - p3.x)) / det; const float l3 = 1.0f - l1 - l2; return l1 * p1.z + l2 * p2.z + l3 * p3.z; @@ -362,9 +343,7 @@ bool ElevationMap::getElevationAt(double x, double y, float& z) const const int cx00 = ::floor((x - x0) / sCellX); const int cy00 = ::floor((y - y0) / sCellY); - if (cx00 < 1 || cx00 >= int(nCellsX - 1) || cy00 < 1 || - cy00 >= int(nCellsY - 1)) - return false; + if (cx00 < 1 || cx00 >= int(nCellsX - 1) || cy00 < 1 || cy00 >= int(nCellsY - 1)) return false; // Linear interpolation: const float z00 = meshCacheZ_(cx00, cy00); diff --git a/modules/simulator/src/WorldElements/GroundGrid.cpp b/modules/simulator/src/WorldElements/GroundGrid.cpp index c0e02b66..e1ddd6bd 100644 --- a/modules/simulator/src/WorldElements/GroundGrid.cpp +++ b/modules/simulator/src/WorldElements/GroundGrid.cpp @@ -43,8 +43,7 @@ void GroundGrid::loadConfigFrom(const rapidxml::xml_node* root) TParameterDefinitions params; params["floating"] = TParamEntry("%bool", &is_floating_); - params["floating_focus"] = - TParamEntry("%s", &float_center_at_vehicle_name_); + params["floating_focus"] = TParamEntry("%s", &float_center_at_vehicle_name_); params["color"] = TParamEntry("%color", &color_); params["line_width"] = TParamEntry("%lf", &line_width_); @@ -54,8 +53,7 @@ void GroundGrid::loadConfigFrom(const rapidxml::xml_node* root) params["y_max"] = TParamEntry("%lf", &y_max_); params["interval"] = TParamEntry("%lf", &interval_); - parse_xmlnode_children_as_param( - *root, params, world_->user_defined_variables()); + parse_xmlnode_children_as_param(*root, params, world_->user_defined_variables()); // If a vehicle name is given, setting "is_floating=true" by the user is // optional: @@ -64,8 +62,7 @@ void GroundGrid::loadConfigFrom(const rapidxml::xml_node* root) void GroundGrid::internalGuiUpdate( const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& - physical, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { using namespace mrpt::math; @@ -89,8 +86,7 @@ void GroundGrid::internalGuiUpdate( // Centered at a vehicle: const World::VehicleList& vehs = world_->getListOfVehicles(); // Look for the vehicle by its name: - World::VehicleList::const_iterator it_veh = - vehs.find(float_center_at_vehicle_name_); + World::VehicleList::const_iterator it_veh = vehs.find(float_center_at_vehicle_name_); // not found -> error: if (!float_center_at_vehicle_name_.empty() && it_veh == vehs.end()) throw std::runtime_error(mrpt::format( @@ -112,11 +108,9 @@ void GroundGrid::internalGuiUpdate( // "Discretize" offset for a better visual impact: ASSERT_(interval_ > .0); - center_offset.x = interval_ * - ::floor(std::abs(center_offset.x) / interval_) * + center_offset.x = interval_ * ::floor(std::abs(center_offset.x) / interval_) * (center_offset.x < 0 ? -1. : 1.); - center_offset.y = interval_ * - ::floor(std::abs(center_offset.y) / interval_) * + center_offset.y = interval_ * ::floor(std::abs(center_offset.y) / interval_) * (center_offset.y < 0 ? -1. : 1.); gl_groundgrid_->setLocation(center_offset); } diff --git a/modules/simulator/src/WorldElements/HorizontalPlane.cpp b/modules/simulator/src/WorldElements/HorizontalPlane.cpp index 547e6e0a..74230fc0 100644 --- a/modules/simulator/src/WorldElements/HorizontalPlane.cpp +++ b/modules/simulator/src/WorldElements/HorizontalPlane.cpp @@ -22,8 +22,7 @@ using namespace rapidxml; using namespace mvsim; using namespace std; -HorizontalPlane::HorizontalPlane( - World* parent, const rapidxml::xml_node* root) +HorizontalPlane::HorizontalPlane(World* parent, const rapidxml::xml_node* root) : WorldElementBase(parent) { // Create opengl object: in this class, we'll store most state data directly @@ -63,8 +62,7 @@ void HorizontalPlane::loadConfigFrom(const rapidxml::xml_node* root) params["texture_size_x"] = TParamEntry("%lf", &textureSizeX_); params["texture_size_y"] = TParamEntry("%lf", &textureSizeY_); - parse_xmlnode_children_as_param( - *root, params, world_->user_defined_variables()); + parse_xmlnode_children_as_param(*root, params, world_->user_defined_variables()); } void HorizontalPlane::internalGuiUpdate( @@ -93,8 +91,7 @@ void HorizontalPlane::internalGuiUpdate( #if MRPT_VERSION >= 0x240 gl_plane_->cullFaces( - mrpt::typemeta::TEnumType::name2value( - cull_faces_)); + mrpt::typemeta::TEnumType::name2value(cull_faces_)); #endif glGroup_->insert(gl_plane_); viz->get().insert(glGroup_); @@ -103,8 +100,7 @@ void HorizontalPlane::internalGuiUpdate( // 1st call? (with texture) if (!gl_plane_text_ && !textureFileName_.empty() && viz && physical) { - const std::string localFileName = - world_->xmlPathToActualPath(textureFileName_); + const std::string localFileName = world_->xmlPathToActualPath(textureFileName_); ASSERT_FILE_EXISTS_(localFileName); mrpt::img::CImage texture; @@ -153,8 +149,7 @@ void HorizontalPlane::internalGuiUpdate( #if MRPT_VERSION >= 0x240 gl_plane_text_->cullFaces( - mrpt::typemeta::TEnumType::name2value( - cull_faces_)); + mrpt::typemeta::TEnumType::name2value(cull_faces_)); #endif glGroup_->insert(gl_plane_text_); diff --git a/modules/simulator/src/WorldElements/OccupancyGridMap.cpp b/modules/simulator/src/WorldElements/OccupancyGridMap.cpp index d32da2fc..dc8e705a 100644 --- a/modules/simulator/src/WorldElements/OccupancyGridMap.cpp +++ b/modules/simulator/src/WorldElements/OccupancyGridMap.cpp @@ -25,8 +25,7 @@ using namespace rapidxml; using namespace mvsim; using namespace std; -OccupancyGridMap::OccupancyGridMap( - World* parent, const rapidxml::xml_node* root) +OccupancyGridMap::OccupancyGridMap(World* parent, const rapidxml::xml_node* root) : WorldElementBase(parent), gui_uptodate_(false), show_grid_collision_points_(true), @@ -45,21 +44,17 @@ void OccupancyGridMap::doLoadConfigFrom(const rapidxml::xml_node* root) // FILENAME.{png,gridmap} xml_node<>* xml_file = root->first_node("file"); if (!xml_file || !xml_file->value()) - throw std::runtime_error( - "Error: XML entry not found inside gridmap node!"); + throw std::runtime_error("Error: XML entry not found inside gridmap node!"); const string sFile = world_->local_to_abs_path(xml_file->value()); - const string sFileExt = - mrpt::system::extractFileExtension(sFile, true /*ignore gz*/); + const string sFileExt = mrpt::system::extractFileExtension(sFile, true /*ignore gz*/); // ROS YAML map files: if (sFileExt == "yaml") { #if MRPT_VERSION >= 0x250 bool ok = grid_.loadFromROSMapServerYAML(sFile); - ASSERTMSG_( - ok, - mrpt::format("Error loading ROS map file: '%s'", sFile.c_str())); + ASSERTMSG_(ok, mrpt::format("Error loading ROS map file: '%s'", sFile.c_str())); #else THROW_EXCEPTION("Loading ROS YAML map files requires MRPT>=2.5.0"); #endif @@ -81,26 +76,21 @@ void OccupancyGridMap::doLoadConfigFrom(const rapidxml::xml_node* root) other_params["centerpixel_x"] = TParamEntry("%lf", &xcenterpixel); other_params["centerpixel_y"] = TParamEntry("%lf", &ycenterpixel); - parse_xmlnode_children_as_param( - *root, other_params, world_->user_defined_variables()); + parse_xmlnode_children_as_param(*root, other_params, world_->user_defined_variables()); - if (!grid_.loadFromBitmapFile( - sFile, resolution, {xcenterpixel, ycenterpixel})) - throw std::runtime_error(mrpt::format( - "[OccupancyGridMap] ERROR: File not found '%s'", - sFile.c_str())); + if (!grid_.loadFromBitmapFile(sFile, resolution, {xcenterpixel, ycenterpixel})) + throw std::runtime_error( + mrpt::format("[OccupancyGridMap] ERROR: File not found '%s'", sFile.c_str())); } { // Other general params: TParameterDefinitions ps; - ps["show_collisions"] = - TParamEntry("%bool", &show_grid_collision_points_); + ps["show_collisions"] = TParamEntry("%bool", &show_grid_collision_points_); ps["restitution"] = TParamEntry("%lf", &restitution_); ps["lateral_friction"] = TParamEntry("%lf", &lateral_friction_); - parse_xmlnode_children_as_param( - *root, ps, world_->user_defined_variables()); + parse_xmlnode_children_as_param(*root, ps, world_->user_defined_variables()); } } @@ -146,8 +136,8 @@ void OccupancyGridMap::internalGuiUpdate( { gl_objs = mrpt::opengl::CSetOfObjects::Create(); gl_objs->setName( - "OccupancyGridMap"s + this->getName() + ".obstacles["s + - std::to_string(i) + "]"s); + "OccupancyGridMap"s + this->getName() + ".obstacles["s + std::to_string(i) + + "]"s); viz->get().insert(gl_objs); } @@ -155,8 +145,7 @@ void OccupancyGridMap::internalGuiUpdate( // adquired from the caller) // proceed to replace the old with the new point cloud: gl_objs->clear(); - if (gl_obs_clouds_buffer_.size() > i) - gl_objs->insert(gl_obs_clouds_buffer_[i]); + if (gl_obs_clouds_buffer_.size() > i) gl_objs->insert(gl_obs_clouds_buffer_[i]); } gl_obs_clouds_buffer_.clear(); @@ -164,8 +153,7 @@ void OccupancyGridMap::internalGuiUpdate( } } -void OccupancyGridMap::simul_pre_timestep( - [[maybe_unused]] const TSimulContext& context) +void OccupancyGridMap::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) { // Make a list of objects subject to collide with the occupancy grid: // - Vehicles @@ -177,12 +165,11 @@ void OccupancyGridMap::simul_pre_timestep( obstacles_for_each_obj_.resize(nObjs); size_t obj_idx = 0; - for (World::VehicleList::const_iterator itVeh = lstVehs.begin(); - itVeh != lstVehs.end(); ++itVeh, ++obj_idx) + for (World::VehicleList::const_iterator itVeh = lstVehs.begin(); itVeh != lstVehs.end(); + ++itVeh, ++obj_idx) { TInfoPerCollidableobj& ipv = obstacles_for_each_obj_[obj_idx]; - ipv.max_obstacles_ranges = - itVeh->second->getMaxVehicleRadius() * 1.50f; + ipv.max_obstacles_ranges = itVeh->second->getMaxVehicleRadius() * 1.50f; const mrpt::math::TPose3D& pose = itVeh->second->getPose(); ipv.pose = mrpt::poses::CPose2D( pose.x, pose.y, @@ -191,8 +178,7 @@ void OccupancyGridMap::simul_pre_timestep( for (const auto& block : lstBlocks) { TInfoPerCollidableobj& ipv = obstacles_for_each_obj_[obj_idx]; - ipv.max_obstacles_ranges = - block.second->getMaxBlockRadius() * 1.50f; + ipv.max_obstacles_ranges = block.second->getMaxBlockRadius() * 1.50f; const mrpt::math::TPose3D& pose = block.second->getPose(); /* angle=0, no need to rotate everything to later rotate back again! */ @@ -225,8 +211,7 @@ void OccupancyGridMap::simul_pre_timestep( scan->aperture = 2.0 * M_PI; // 360 field of view scan->maxRange = veh_max_obstacles_ranges; - grid_.laserScanSimulator( - *scan, ipv.pose, occup_threshold, nRays, 0.0f /*noise*/); + grid_.laserScanSimulator(*scan, ipv.pose, occup_threshold, nRays, 0.0f /*noise*/); // Since we'll dilate obstacle points, let's give a bit more space // as compensation: @@ -247,13 +232,11 @@ void OccupancyGridMap::simul_pre_timestep( } // Force move the body to the vehicle origins (to use obstacles in // local coords): - ipv.collide_body->SetTransform( - b2Vec2(ipv.pose.x(), ipv.pose.y()), .0f); + ipv.collide_body->SetTransform(b2Vec2(ipv.pose.x(), ipv.pose.y()), .0f); // GL: // 1st usage? - mrpt::opengl::CPointCloud::Ptr& gl_pts = - gl_obs_clouds_buffer_[obj_idx]; + mrpt::opengl::CPointCloud::Ptr& gl_pts = gl_obs_clouds_buffer_[obj_idx]; if (show_grid_collision_points_) { gl_pts = mrpt::opengl::CPointCloud::Create(); @@ -278,14 +261,13 @@ void OccupancyGridMap::simul_pre_timestep( // Create fixtures at their place (or disable it if no obstacle has // been sensed): - const mrpt::obs::CSinCosLookUpTableFor2DScans::TSinCosValues& - sincos_tab = sincos_lut_.getSinCosForScan(*scan); + const mrpt::obs::CSinCosLookUpTableFor2DScans::TSinCosValues& sincos_tab = + sincos_lut_.getSinCosForScan(*scan); ipv.collide_fixtures.resize(nRays); for (size_t k = 0; k < nRays; k++) { if (!ipv.collide_fixtures[k].fixture) - ipv.collide_fixtures[k].fixture = - ipv.collide_body->CreateFixture(&fixtureDef); + ipv.collide_fixtures[k].fixture = ipv.collide_body->CreateFixture(&fixtureDef); if (!scan->getScanRangeValidity(k)) { @@ -299,14 +281,12 @@ void OccupancyGridMap::simul_pre_timestep( ipv.collide_fixtures[k].fixture->SetSensor(false); ipv.collide_fixtures[k].fixture->GetUserData().pointer = 0; - b2PolygonShape* poly = dynamic_cast( - ipv.collide_fixtures[k].fixture->GetShape()); + b2PolygonShape* poly = + dynamic_cast(ipv.collide_fixtures[k].fixture->GetShape()); ASSERT_(poly != nullptr); - const float llx = - sincos_tab.ccos[k] * scan->getScanRange(k); - const float lly = - sincos_tab.csin[k] * scan->getScanRange(k); + const float llx = sincos_tab.ccos[k] * scan->getScanRange(k); + const float lly = sincos_tab.csin[k] * scan->getScanRange(k); const float ggx = ipv.pose.x() + llx; const float ggy = ipv.pose.y() + lly; @@ -318,13 +298,11 @@ void OccupancyGridMap::simul_pre_timestep( const float ly = gy - ipv.pose.y(); poly->SetAsBox( - occCellSemiWidth, occCellSemiWidth, b2Vec2(lx, ly), - .0f /*angle*/); + occCellSemiWidth, occCellSemiWidth, b2Vec2(lx, ly), .0f /*angle*/); if (gl_pts && show_grid_collision_points_) { - gl_pts->mrpt::opengl::CPointCloud::insertPoint( - lx, ly, .0); + gl_pts->mrpt::opengl::CPointCloud::insertPoint(lx, ly, .0); } } } diff --git a/modules/simulator/src/WorldElements/PointCloud.cpp b/modules/simulator/src/WorldElements/PointCloud.cpp index 779f3f48..ab34a805 100644 --- a/modules/simulator/src/WorldElements/PointCloud.cpp +++ b/modules/simulator/src/WorldElements/PointCloud.cpp @@ -56,8 +56,7 @@ void PointCloud::doLoadConfigFrom(const rapidxml::xml_node* root) TParameterDefinitions ps; ps["points_size"] = TParamEntry("%lf", &render_points_size_); ps["pose_3d"] = TParamEntry("%pose3d", &pointcloud_pose_); - parse_xmlnode_children_as_param( - *root, ps, world_->user_defined_variables()); + parse_xmlnode_children_as_param(*root, ps, world_->user_defined_variables()); } points_->renderOptions.point_size = render_points_size_; @@ -90,7 +89,4 @@ void PointCloud::internalGuiUpdate( } } -void PointCloud::simul_pre_timestep( - [[maybe_unused]] const TSimulContext& context) -{ -} +void PointCloud::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) {} diff --git a/modules/simulator/src/WorldElements/SkyBox.cpp b/modules/simulator/src/WorldElements/SkyBox.cpp index ad3cdb75..4520ad3d 100644 --- a/modules/simulator/src/WorldElements/SkyBox.cpp +++ b/modules/simulator/src/WorldElements/SkyBox.cpp @@ -25,8 +25,7 @@ using namespace rapidxml; using namespace mvsim; using namespace std; -SkyBox::SkyBox(World* parent, const rapidxml::xml_node* root) - : WorldElementBase(parent) +SkyBox::SkyBox(World* parent, const rapidxml::xml_node* root) : WorldElementBase(parent) { // Create opengl object: in this class, we'll store most state data directly // in the mrpt::opengl object. @@ -44,12 +43,10 @@ void SkyBox::loadConfigFrom(const rapidxml::xml_node* root) TParameterDefinitions params; params["textures"] = TParamEntry("%s", &texturesPattern); - parse_xmlnode_children_as_param( - *root, params, world_->user_defined_variables()); + parse_xmlnode_children_as_param(*root, params, world_->user_defined_variables()); ASSERTMSG_( - !texturesPattern.empty(), - "Textures must be defined in a ... tag."); + !texturesPattern.empty(), "Textures must be defined in a ... tag."); ASSERTMSG_( texturesPattern.find("%s") != std::string::npos, @@ -60,12 +57,9 @@ void SkyBox::loadConfigFrom(const rapidxml::xml_node* root) using mrpt::opengl::CUBE_TEXTURE_FACE; const std::vector> faceImages = { - {CUBE_TEXTURE_FACE::FRONT, "Front"}, - {CUBE_TEXTURE_FACE::BACK, "Back"}, - {CUBE_TEXTURE_FACE::BOTTOM, "Down"}, - {CUBE_TEXTURE_FACE::TOP, "Up"}, - {CUBE_TEXTURE_FACE::LEFT, "Left"}, - {CUBE_TEXTURE_FACE::RIGHT, "Right"}, + {CUBE_TEXTURE_FACE::FRONT, "Front"}, {CUBE_TEXTURE_FACE::BACK, "Back"}, + {CUBE_TEXTURE_FACE::BOTTOM, "Down"}, {CUBE_TEXTURE_FACE::TOP, "Up"}, + {CUBE_TEXTURE_FACE::LEFT, "Left"}, {CUBE_TEXTURE_FACE::RIGHT, "Right"}, }; auto sb = mrpt::opengl::CSkyBox::Create(); @@ -82,9 +76,7 @@ void SkyBox::loadConfigFrom(const rapidxml::xml_node* root) glSkyBoxPrepared_ = sb; #else - std::cerr - << "[mvsim::SkyBox] Ignoring SkyBox since MRPT>=2.7.0 is not available." - << std::endl; + std::cerr << "[mvsim::SkyBox] Ignoring SkyBox since MRPT>=2.7.0 is not available." << std::endl; #endif } diff --git a/modules/simulator/src/WorldElements/VerticalPlane.cpp b/modules/simulator/src/WorldElements/VerticalPlane.cpp index 94cb03fc..94d9e6cb 100644 --- a/modules/simulator/src/WorldElements/VerticalPlane.cpp +++ b/modules/simulator/src/WorldElements/VerticalPlane.cpp @@ -22,8 +22,7 @@ using namespace rapidxml; using namespace mvsim; using namespace std; -VerticalPlane::VerticalPlane( - World* parent, const rapidxml::xml_node* root) +VerticalPlane::VerticalPlane(World* parent, const rapidxml::xml_node* root) : WorldElementBase(parent) { // Create opengl object: in this class, we'll store most state data directly @@ -64,8 +63,7 @@ void VerticalPlane::loadConfigFrom(const rapidxml::xml_node* root) params["texture_size_x"] = TParamEntry("%lf", &textureSizeX_); params["texture_size_y"] = TParamEntry("%lf", &textureSizeY_); - parse_xmlnode_children_as_param( - *root, params, world_->user_defined_variables()); + parse_xmlnode_children_as_param(*root, params, world_->user_defined_variables()); // Create box2d fixtures, to enable collision detection: // -------------------------------------------------------- @@ -138,8 +136,7 @@ void VerticalPlane::internalGuiUpdate( const float L = v01.norm(), H = height_; - const auto center = - (p0 + p1) * 0.5f + mrpt::math::TPoint3Df(0, 0, 0.5f * H); + const auto center = (p0 + p1) * 0.5f + mrpt::math::TPoint3Df(0, 0, 0.5f * H); gl_plane_ = mrpt::opengl::CTexturedPlane::Create(); gl_plane_->setPlaneCorners(-0.5 * L, 0.5 * L, -0.5 * H, 0.5 * H); @@ -162,8 +159,7 @@ void VerticalPlane::internalGuiUpdate( #if MRPT_VERSION >= 0x240 gl_plane_->cullFaces( - mrpt::typemeta::TEnumType::name2value( - cull_faces_)); + mrpt::typemeta::TEnumType::name2value(cull_faces_)); #endif glGroup_->insert(gl_plane_); viz->get().insert(glGroup_); @@ -172,8 +168,7 @@ void VerticalPlane::internalGuiUpdate( // 1st call? (with texture) if (!gl_plane_text_ && !textureFileName_.empty() && viz && physical) { - const std::string localFileName = - world_->xmlPathToActualPath(textureFileName_); + const std::string localFileName = world_->xmlPathToActualPath(textureFileName_); ASSERT_FILE_EXISTS_(localFileName); mrpt::img::CImage texture; @@ -230,8 +225,7 @@ void VerticalPlane::internalGuiUpdate( #if MRPT_VERSION >= 0x240 gl_plane_text_->cullFaces( - mrpt::typemeta::TEnumType::name2value( - cull_faces_)); + mrpt::typemeta::TEnumType::name2value(cull_faces_)); #endif glGroup_->insert(gl_plane_text_); diff --git a/modules/simulator/src/World_gui.cpp b/modules/simulator/src/World_gui.cpp index 50c1fca4..0e5d3b8d 100644 --- a/modules/simulator/src/World_gui.cpp +++ b/modules/simulator/src/World_gui.cpp @@ -34,15 +34,13 @@ using namespace std; void World::TGUI_Options::parse_from( const rapidxml::xml_node& node, mrpt::system::COutputLogger& logger) { - parse_xmlnode_children_as_param( - node, params, {}, "[World::TGUI_Options]", &logger); + parse_xmlnode_children_as_param(node, params, {}, "[World::TGUI_Options]", &logger); } void World::LightOptions::parse_from( const rapidxml::xml_node& node, mrpt::system::COutputLogger& logger) { - parse_xmlnode_children_as_param( - node, params, {}, "[World::LightOptions]", &logger); + parse_xmlnode_children_as_param(node, params, {}, "[World::LightOptions]", &logger); } //!< Return true if the GUI window is open, after a previous call to @@ -64,8 +62,8 @@ void World::GUI::prepare_control_window() gui_win->getSubWindowsUI()->setPosition({1, 1}); w->setPosition({1, 80}); - w->setLayout(new nanogui::BoxLayout( - nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 5)); + w->setLayout( + new nanogui::BoxLayout(nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 5)); w->add("Quit", ENTYPO_ICON_ARROW_BOLD_LEFT) ->setCallback( @@ -95,8 +93,7 @@ void World::GUI::prepare_control_window() }); w->add( - "Orthogonal view", - [&](bool b) { gui_win->camera().setCameraProjective(!b); }) + "Orthogonal view", [&](bool b) { gui_win->camera().setCameraProjective(!b); }) ->setChecked(parent_.guiOptions_.ortho); #if MRPT_VERSION >= 0x270 @@ -123,8 +120,7 @@ void World::GUI::prepare_control_window() { parent_.lightOptions_.light_azimuth = v; parent_.setLightDirectionFromAzimuthElevation( - parent_.lightOptions_.light_azimuth, - parent_.lightOptions_.light_elevation); + parent_.lightOptions_.light_azimuth, parent_.lightOptions_.light_elevation); }); } w->add("Light elevation:"); @@ -137,13 +133,11 @@ void World::GUI::prepare_control_window() { parent_.lightOptions_.light_elevation = v; parent_.setLightDirectionFromAzimuthElevation( - parent_.lightOptions_.light_azimuth, - parent_.lightOptions_.light_elevation); + parent_.lightOptions_.light_azimuth, parent_.lightOptions_.light_elevation); }); } - w->add( - "View forces", [&](bool b) { parent_.guiOptions_.show_forces = b; }) + w->add("View forces", [&](bool b) { parent_.guiOptions_.show_forces = b; }) ->setChecked(parent_.guiOptions_.show_forces); w->add( @@ -152,9 +146,8 @@ void World::GUI::prepare_control_window() { std::lock_guard lck(gui_win->background_scene_mtx); - auto glVizSensors = - std::dynamic_pointer_cast( - gui_win->background_scene->getByName("group_sensors_viz")); + auto glVizSensors = std::dynamic_pointer_cast( + gui_win->background_scene->getByName("group_sensors_viz")); ASSERT_(glVizSensors); glVizSensors->setVisibility(b); @@ -204,8 +197,7 @@ void World::GUI::prepare_status_window() #endif w->setPosition({5, 455}); - w->setLayout(new nanogui::BoxLayout( - nanogui::Orientation::Vertical, nanogui::Alignment::Fill)); + w->setLayout(new nanogui::BoxLayout(nanogui::Orientation::Vertical, nanogui::Alignment::Fill)); w->setFixedWidth(320); #if MRPT_VERSION < 0x211 @@ -216,8 +208,7 @@ void World::GUI::prepare_status_window() lbCpuUsage = w->add(" "); lbStatuses.resize(12); - for (size_t i = 0; i < lbStatuses.size(); i++) - lbStatuses[i] = w->add(" "); + for (size_t i = 0; i < lbStatuses.size(); i++) lbStatuses[i] = w->add(" "); } // Add editor window @@ -237,8 +228,8 @@ void World::GUI::prepare_editor_window() constexpr int slidersWidth = pnWidth - 80 - COORDS_LABEL_WIDTH; w->setPosition({1, 230}); - w->setLayout(new nanogui::BoxLayout( - nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 3, 3)); + w->setLayout( + new nanogui::BoxLayout(nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 3, 3)); w->setFixedWidth(pnWidth); #if MRPT_VERSION < 0x211 @@ -263,20 +254,17 @@ void World::GUI::prepare_editor_window() constexpr size_t NUM_TABS = 5; std::array tabs = { - tab->createTab("Vehicles"), tab->createTab("Sensors"), - tab->createTab("Blocks"), tab->createTab("Elements"), - tab->createTab("Misc.")}; + tab->createTab("Vehicles"), tab->createTab("Sensors"), tab->createTab("Blocks"), + tab->createTab("Elements"), tab->createTab("Misc.")}; tab->setActiveTab(0); for (auto t : tabs) t->setLayout(new nanogui::BoxLayout( - nanogui::Orientation::Vertical, nanogui::Alignment::Minimum, 3, - 3)); + nanogui::Orientation::Vertical, nanogui::Alignment::Minimum, 3, 3)); std::array vscrolls; - for (size_t i = 0; i < NUM_TABS; i++) - vscrolls[i] = tabs[i]->add(); + for (size_t i = 0; i < NUM_TABS; i++) vscrolls[i] = tabs[i]->add(); for (auto vs : vscrolls) vs->setFixedSize({pnWidth, pnHeight}); @@ -288,8 +276,8 @@ void World::GUI::prepare_editor_window() wrappers[i] = vscrolls[i]->add(); wrappers[i]->setFixedSize({pnWidth, pnHeight}); wrappers[i]->setLayout(new nanogui::GridLayout( - nanogui::Orientation::Horizontal, 1 /*columns */, - nanogui::Alignment::Minimum, 3, 3)); + nanogui::Orientation::Horizontal, 1 /*columns */, nanogui::Alignment::Minimum, 3, + 3)); } // Extend the list of world objects with the robot sensors: @@ -359,8 +347,7 @@ void World::GUI::prepare_editor_window() // deselect former one: if (gui_selectedObject.visual) gui_selectedObject.visual->showCollisionShape(false); - if (gui_selectedObject.cb) - gui_selectedObject.cb->setChecked(false); + if (gui_selectedObject.cb) gui_selectedObject.cb->setChecked(false); gui_selectedObject = InfoPerObject(); cb->setChecked(check); @@ -391,23 +378,18 @@ void World::GUI::prepare_editor_window() try { const std::string outFile = nanogui::file_dialog( - {{"3Dscene", "MRPT 3D scene file (*.3Dsceme)"}}, - true /*save*/); + {{"3Dscene", "MRPT 3D scene file (*.3Dsceme)"}}, true /*save*/); if (outFile.empty()) return; - auto lck = - mrpt::lockHelper(parent_.physical_objects_mtx()); + auto lck = mrpt::lockHelper(parent_.physical_objects_mtx()); parent_.worldPhysical_.saveToFile(outFile); - std::cout - << "[mvsim gui] Saved world scene to: " << outFile - << std::endl; + std::cout << "[mvsim gui] Saved world scene to: " << outFile << std::endl; } catch (const std::exception& e) { - std::cerr - << "[mvsim gui] Exception while saving 3D scene:\n" - << e.what() << std::endl; + std::cerr << "[mvsim gui] Exception while saving 3D scene:\n" + << e.what() << std::endl; } }); } @@ -440,8 +422,7 @@ void World::GUI::prepare_editor_window() { // Re-generate the other 6 sliders with this new scale: if (!gui_selectedObject.simulable || !onEntitySelected) return; - onEntitySelected( - gui_selectedObject.simulable->getRelativePose()); + onEntitySelected(gui_selectedObject.simulable->getRelativePose()); }); slCoord->setFixedWidth(slidersWidth - 30); btns_selectedOps.push_back(slCoord); @@ -491,12 +472,11 @@ void World::GUI::prepare_editor_window() // Now, we can define the lambda for filling in the current object pose in // the GUI controls: - onEntitySelected = [slidersCoords, slidersCoordScale, - slidersCoordScaleValue](const mrpt::math::TPose3D p) + onEntitySelected = + [slidersCoords, slidersCoordScale, slidersCoordScaleValue](const mrpt::math::TPose3D p) { ASSERT_(slidersCoordScale); - const double scale = - std::pow(10.0, mrpt::round(slidersCoordScale->value())); + const double scale = std::pow(10.0, mrpt::round(slidersCoordScale->value())); slidersCoordScaleValue->setCaption(mrpt::format("%.01e", scale)); @@ -504,15 +484,13 @@ void World::GUI::prepare_editor_window() for (int i = 0; i < 3; i++) { slidersCoords[i]->setRange( - {p[i] - scale * REPOSITION_SLIDER_RANGE, - p[i] + scale * REPOSITION_SLIDER_RANGE}); + {p[i] - scale * REPOSITION_SLIDER_RANGE, p[i] + scale * REPOSITION_SLIDER_RANGE}); slidersCoords[i]->setValue(p[i]); } // Angles: for (int i = 0; i < 3; i++) { - slidersCoords[i + 3]->setRange( - {p[i + 3] - scale * M_PI, p[i + 3] + scale * M_PI}); + slidersCoords[i + 3]->setRange({p[i + 3] - scale * M_PI, p[i + 3] + scale * M_PI}); slidersCoords[i + 3]->setValue(p[i + 3]); } @@ -522,8 +500,7 @@ void World::GUI::prepare_editor_window() onEntityMoved = [slidersCoordsValues](const mrpt::math::TPose3D p) { // Positions: - for (int i = 0; i < 3; i++) - slidersCoordsValues[i]->setCaption(mrpt::format("%.04f", p[i])); + for (int i = 0; i < 3; i++) slidersCoordsValues[i]->setCaption(mrpt::format("%.04f", p[i])); // Angles: for (int i = 0; i < 3; i++) slidersCoordsValues[i + 3]->setCaption( @@ -539,11 +516,9 @@ void World::GUI::prepare_editor_window() // if (!gui_selectedObject.simulable) return; - auto* formPose = - new nanogui::Window(gui_win.get(), "Enter new pose"); + auto* formPose = new nanogui::Window(gui_win.get(), "Enter new pose"); formPose->setLayout(new nanogui::GridLayout( - nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, - 5)); + nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 5)); nanogui::TextBox* lbs[6]; @@ -572,34 +547,31 @@ void World::GUI::prepare_editor_window() } const auto pos = gui_selectedObject.simulable->getRelativePose(); - for (int i = 0; i < 3; i++) - lbs[i]->setValue(std::to_string(pos[i])); + for (int i = 0; i < 3; i++) lbs[i]->setValue(std::to_string(pos[i])); - for (int i = 3; i < 6; i++) - lbs[i]->setValue(std::to_string(mrpt::RAD2DEG(pos[i]))); + for (int i = 3; i < 6; i++) lbs[i]->setValue(std::to_string(mrpt::RAD2DEG(pos[i]))); formPose->add(""); formPose->add(""); - formPose->add("Cancel")->setCallback( - [formPose]() { formPose->dispose(); }); + formPose->add("Cancel")->setCallback([formPose]() + { formPose->dispose(); }); formPose->add("Accept")->setCallback( [formPose, this, lbs]() { - const mrpt::math::TPose3D newPose = { - // X: - std::stod(lbs[0]->value()), - // Y: - std::stod(lbs[1]->value()), - // Z: - std::stod(lbs[2]->value()), - // Yaw - mrpt::DEG2RAD(std::stod(lbs[3]->value())), - // Pitch - mrpt::DEG2RAD(std::stod(lbs[4]->value())), - // Roll: - mrpt::DEG2RAD(std::stod(lbs[5]->value()))}; + const mrpt::math::TPose3D newPose = {// X: + std::stod(lbs[0]->value()), + // Y: + std::stod(lbs[1]->value()), + // Z: + std::stod(lbs[2]->value()), + // Yaw + mrpt::DEG2RAD(std::stod(lbs[3]->value())), + // Pitch + mrpt::DEG2RAD(std::stod(lbs[4]->value())), + // Roll: + mrpt::DEG2RAD(std::stod(lbs[5]->value()))}; gui_selectedObject.simulable->setRelativePose(newPose); onEntitySelected(newPose); @@ -619,8 +591,7 @@ void World::GUI::prepare_editor_window() #if MRPT_VERSION >= 0x231 gui_win->subwindowMinimize(subwinIdx); #else - if (auto btnMinimize = - dynamic_cast(w->buttonPanel()->children().at(0)); + if (auto btnMinimize = dynamic_cast(w->buttonPanel()->children().at(0)); btnMinimize) { btnMinimize->callback()(); // "push" button @@ -641,8 +612,8 @@ void World::internal_GUI_thread() mrpt::gui::CDisplayWindowGUI_Params cp; cp.maximized = guiOptions_.start_maximized; - gui_.gui_win = mrpt::gui::CDisplayWindowGUI::Create( - "mvsim", guiOptions_.win_w, guiOptions_.win_h, cp); + gui_.gui_win = + mrpt::gui::CDisplayWindowGUI::Create("mvsim", guiOptions_.win_w, guiOptions_.win_h, cp); // zmin / zmax of opengl viewport: worldVisual_->getViewport()->setViewportClipDistances( @@ -690,15 +661,13 @@ void World::internal_GUI_thread() const auto& lo = lightOptions_; - setLightDirectionFromAzimuthElevation( - lo.light_azimuth, lo.light_elevation); + setLightDirectionFromAzimuthElevation(lo.light_azimuth, lo.light_elevation); #if MRPT_VERSION >= 0x270 auto vv = worldVisual_->getViewport(); auto vp = worldPhysical_.getViewport(); - auto lambdaSetLightParams = - [&lo](const mrpt::opengl::COpenGLViewport::Ptr& v) + auto lambdaSetLightParams = [&lo](const mrpt::opengl::COpenGLViewport::Ptr& v) { // enable shadows and set the shadow map texture size: const int sms = lo.shadow_map_size; @@ -712,15 +681,12 @@ void World::internal_GUI_thread() vlp.color = colf; #if MRPT_VERSION >= 0x2A0 // New in mrpt>=2.10.0 - vlp.eyeDistance2lightShadowExtension = - lo.eye_distance_to_shadow_map_extension; + vlp.eyeDistance2lightShadowExtension = lo.eye_distance_to_shadow_map_extension; - vlp.minimum_shadow_map_extension_ratio = - lo.minimum_shadow_map_extension_ratio; + vlp.minimum_shadow_map_extension_ratio = lo.minimum_shadow_map_extension_ratio; #endif // light view frustrum near/far planes: - v->setLightShadowClipDistances( - lo.light_clip_plane_min, lo.light_clip_plane_max); + v->setLightShadowClipDistances(lo.light_clip_plane_min, lo.light_clip_plane_max); // Shadow bias should be proportional to clip range: #if MRPT_VERSION >= 0x281 @@ -753,8 +719,7 @@ void World::internal_GUI_thread() lastKeyEvent_.keycode = key; lastKeyEvent_.modifierShift = (modifiers & GLFW_MOD_SHIFT) != 0; - lastKeyEvent_.modifierCtrl = - (modifiers & GLFW_MOD_CONTROL) != 0; + lastKeyEvent_.modifierCtrl = (modifiers & GLFW_MOD_CONTROL) != 0; lastKeyEvent_.modifierSuper = (modifiers & GLFW_MOD_SUPER) != 0; lastKeyEvent_.modifierAlt = (modifiers & GLFW_MOD_ALT) != 0; @@ -802,23 +767,21 @@ void World::internal_GUI_thread() // Register observation callback: const auto lambdaOnObservation = - [this]( - const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs) + [this](const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs) { // obs->getDescriptionAsText(std::cout); - this->enqueue_task_to_run_in_gui_thread( - [this, obs, &veh]() { internal_gui_on_observation(veh, obs); }); + this->enqueue_task_to_run_in_gui_thread([this, obs, &veh]() + { internal_gui_on_observation(veh, obs); }); }; this->registerCallbackOnObservation(lambdaOnObservation); // ============= Mainloop ============= - const int refresh_ms = - std::max(1, mrpt::round(1000 / guiOptions_.refresh_fps)); + const int refresh_ms = std::max(1, mrpt::round(1000 / guiOptions_.refresh_fps)); MRPT_LOG_DEBUG_FMT( - "[World::internal_GUI_thread] Using GUI FPS=%i (T=%i ms)", - guiOptions_.refresh_fps, refresh_ms); + "[World::internal_GUI_thread] Using GUI FPS=%i (T=%i ms)", guiOptions_.refresh_fps, + refresh_ms); #if MRPT_VERSION >= 0x253 const int idleLoopTasks_ms = 10; @@ -844,8 +807,7 @@ void World::internal_GUI_thread() auto lckListObjs = mrpt::lockHelper(getListOfSimulableObjectsMtx()); - for (auto& obj : getListOfSimulableObjects()) - obj.second->freeOpenGLResources(); + for (auto& obj : getListOfSimulableObjects()) obj.second->freeOpenGLResources(); lckListObjs.unlock(); @@ -858,8 +820,7 @@ void World::internal_GUI_thread() } catch (const std::exception& e) { - MRPT_LOG_ERROR_STREAM( - "[internal_GUI_init] Exception: " << mrpt::exception_to_str(e)); + MRPT_LOG_ERROR_STREAM("[internal_GUI_init] Exception: " << mrpt::exception_to_str(e)); } gui_thread_running_ = false; } @@ -882,8 +843,7 @@ void World::GUI::handle_mouse_operations() vp->get3DRayForPixelCoord(mousePt.x(), mousePt.y(), ray); // Create a 3D plane, i.e. Z=0 - const auto ground_plane = - mrpt::math::TPlane::From3Points({0, 0, 0}, {1, 0, 0}, {0, 1, 0}); + const auto ground_plane = mrpt::math::TPlane::From3Points({0, 0, 0}, {1, 0, 0}, {0, 1, 0}); // Intersection of the line with the plane: mrpt::math::TObject3D inters; @@ -945,11 +905,9 @@ void World::internal_process_pending_gui_user_tasks() guiUserPendingTasksMtx_.unlock(); } -void World::internalRunSensorsOn3DScene( - mrpt::opengl::COpenGLScene& physicalObjects) +void World::internalRunSensorsOn3DScene(mrpt::opengl::COpenGLScene& physicalObjects) { - auto tle = mrpt::system::CTimeLoggerEntry( - timlogger_, "internalRunSensorsOn3DScene"); + auto tle = mrpt::system::CTimeLoggerEntry(timlogger_, "internalRunSensorsOn3DScene"); for (auto& v : vehicles_) for (auto& sensor : v.second->getSensors()) @@ -964,8 +922,7 @@ void World::internalUpdate3DSceneObjects( { // Update view of map elements // ----------------------------- - auto tle = - mrpt::system::CTimeLoggerEntry(timlogger_, "update_GUI.2.map-elements"); + auto tle = mrpt::system::CTimeLoggerEntry(timlogger_, "update_GUI.2.map-elements"); for (auto& e : worldElements_) e->guiUpdate(viz, physical); @@ -993,14 +950,12 @@ void World::internalUpdate3DSceneObjects( if (gui_.lbCpuUsage) { // 1st line: time - double cpu_usage_ratio = - std::max(1e-10, timlogger_.getMeanTime("run_simulation.cpu_dt")) / - std::max(1e-10, timlogger_.getMeanTime("run_simulation.dt")); + double cpu_usage_ratio = std::max(1e-10, timlogger_.getMeanTime("run_simulation.cpu_dt")) / + std::max(1e-10, timlogger_.getMeanTime("run_simulation.dt")); gui_.lbCpuUsage->setCaption(mrpt::format( "Time: %s (CPU usage: %.03f%%)", - mrpt::system::formatTimeInterval(get_simul_time()).c_str(), - cpu_usage_ratio * 100.0)); + mrpt::system::formatTimeInterval(get_simul_time()).c_str(), cpu_usage_ratio * 100.0)); // User supplied-lines: guiMsgLinesMtx_.lock(); @@ -1013,8 +968,7 @@ void World::internalUpdate3DSceneObjects( // split lines: std::vector lines; mrpt::system::tokenize(msg_lines, "\r\n", lines); - for (const auto& l : lines) - gui_.lbStatuses.at(nextStatusLine++)->setCaption(l); + for (const auto& l : lines) gui_.lbStatuses.at(nextStatusLine++)->setCaption(l); } gui_.lbStatuses.at(nextStatusLine++) ->setCaption(std::string("Mouse: ") + gui_.clickedPt.asString()); @@ -1026,8 +980,7 @@ void World::internalUpdate3DSceneObjects( // ----------------------- if (!guiOptions_.follow_vehicle.empty()) { - if (auto it = vehicles_.find(guiOptions_.follow_vehicle); - it != vehicles_.end()) + if (auto it = vehicles_.find(guiOptions_.follow_vehicle); it != vehicles_.end()) { const mrpt::poses::CPose2D pose = it->second->getCPose2D(); gui_.gui_win->camera().setCameraPointing(pose.x(), pose.y(), 0.0f); @@ -1061,8 +1014,7 @@ void World::update_GUI(TUpdateGUIParams* guiparams) const int MVSIM_OPEN_GUI_TIMEOUT_MS = mrpt::get_env("MVSIM_OPEN_GUI_TIMEOUT_MS", 3000); - for (int timeout = 0; timeout < MVSIM_OPEN_GUI_TIMEOUT_MS / 10; - timeout++) + for (int timeout = 0; timeout < MVSIM_OPEN_GUI_TIMEOUT_MS / 10; timeout++) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); if (gui_thread_running_) break; @@ -1114,23 +1066,18 @@ void World::internal_gui_on_observation( { if (!obs) return; - if (auto obs3D = - std::dynamic_pointer_cast(obs); - obs3D) + if (auto obs3D = std::dynamic_pointer_cast(obs); obs3D) { internal_gui_on_observation_3Dscan(veh, obs3D); } - else if (auto obsIm = - std::dynamic_pointer_cast(obs); - obsIm) + else if (auto obsIm = std::dynamic_pointer_cast(obs); obsIm) { internal_gui_on_observation_image(veh, obsIm); } } void World::internal_gui_on_observation_3Dscan( - const Simulable& veh, - const std::shared_ptr& obs) + const Simulable& veh, const std::shared_ptr& obs) { using namespace std::string_literals; @@ -1141,14 +1088,12 @@ void World::internal_gui_on_observation_3Dscan( if (obs->hasIntensityImage) { rgbImageWinSize = internal_gui_on_image( - veh.getName() + "/"s + obs->sensorLabel + "_rgb"s, - obs->intensityImage, 5); + veh.getName() + "/"s + obs->sensorLabel + "_rgb"s, obs->intensityImage, 5); } if (obs->hasRangeImage) { mrpt::math::CMatrixFloat d; - d = obs->rangeImage.asEigen().cast() * - (obs->rangeUnits / obs->maxRange); + d = obs->rangeImage.asEigen().cast() * (obs->rangeUnits / obs->maxRange); mrpt::img::CImage imDepth; imDepth.setFromMatrix(d, true /* in range [0,1] */); @@ -1160,8 +1105,7 @@ void World::internal_gui_on_observation_3Dscan( } void World::internal_gui_on_observation_image( - const Simulable& veh, - const std::shared_ptr& obs) + const Simulable& veh, const std::shared_ptr& obs) { using namespace std::string_literals; @@ -1169,8 +1113,8 @@ void World::internal_gui_on_observation_image( mrpt::math::TPoint2D rgbImageWinSize = {0, 0}; - rgbImageWinSize = internal_gui_on_image( - veh.getName() + "/"s + obs->sensorLabel + "_rgb"s, obs->image, 5); + rgbImageWinSize = + internal_gui_on_image(veh.getName() + "/"s + obs->sensorLabel + "_rgb"s, obs->image, 5); } mrpt::math::TPoint2D World::internal_gui_on_image( @@ -1181,8 +1125,7 @@ mrpt::math::TPoint2D World::internal_gui_on_image( // Once creation: if (!guiObsViz_.count(label)) { - auto& w = guiObsViz_[label] = - gui_.gui_win->createManagedSubWindow(label); + auto& w = guiObsViz_[label] = gui_.gui_win->createManagedSubWindow(label); w->setLayout(new nanogui::GridLayout( nanogui::Orientation::Vertical, 1, nanogui::Alignment::Fill, 2, 2)); @@ -1202,8 +1145,7 @@ mrpt::math::TPoint2D World::internal_gui_on_image( glControl->setFixedSize({winW, winH}); static std::map numGuiWindows; - w->setPosition( - {winPosX, 20 + (numGuiWindows[winPosX]++) * (winH + 10)}); + w->setPosition({winPosX, 20 + (numGuiWindows[winPosX]++) * (winH + 10)}); auto lck = mrpt::lockHelper(glControl->scene_mtx); @@ -1214,8 +1156,7 @@ mrpt::math::TPoint2D World::internal_gui_on_image( // Update from sensor data: auto& w = guiObsViz_[label]; - glControl = - dynamic_cast(w->children().at(1)); + glControl = dynamic_cast(w->children().at(1)); ASSERT_(glControl != nullptr); auto lck = mrpt::lockHelper(glControl->scene_mtx); @@ -1243,8 +1184,7 @@ void World::internalGraphicsLoopTasksForSimulation() { const auto lck = mrpt::lockHelper(guiUserObjectsMtx_); // replace list of smart pointers (fast): - if (guiUserObjectsPhysical_) - *glUserObjsPhysical_ = *guiUserObjectsPhysical_; + if (guiUserObjectsPhysical_) *glUserObjsPhysical_ = *guiUserObjectsPhysical_; if (guiUserObjectsViz_) *glUserObjsViz_ = *guiUserObjectsViz_; } } @@ -1258,12 +1198,10 @@ void World::internalGraphicsLoopTasksForSimulation() } } -void World::setLightDirectionFromAzimuthElevation( - const float azimuth, const float elevation) +void World::setLightDirectionFromAzimuthElevation(const float azimuth, const float elevation) { const mrpt::math::TPoint3Df dir = { - -cos(azimuth) * cos(elevation), -sin(azimuth) * cos(elevation), - -sin(elevation)}; + -cos(azimuth) * cos(elevation), -sin(azimuth) * cos(elevation), -sin(elevation)}; ASSERT_(worldVisual_); diff --git a/modules/simulator/src/World_load_xml.cpp b/modules/simulator/src/World_load_xml.cpp index a0f91328..460edc4a 100644 --- a/modules/simulator/src/World_load_xml.cpp +++ b/modules/simulator/src/World_load_xml.cpp @@ -32,8 +32,7 @@ void World::load_from_XML_file(const std::string& xmlFileNamePath) load_from_XML(fil_xml.data(), xmlFileNamePath.c_str()); } -void World::load_from_XML( - const std::string& xml_text, const std::string& fileNameForPath) +void World::load_from_XML(const std::string& xml_text, const std::string& fileNameForPath) { using namespace std; using namespace rapidxml; @@ -54,8 +53,7 @@ void World::load_from_XML( this->clear_all(); // Create anchor object for standalone (environment-attached) sensors: - DummyInvisibleBlock::Ptr standaloneSensorHost = - std::make_shared(this); + DummyInvisibleBlock::Ptr standaloneSensorHost = std::make_shared(this); simulableObjectsMtx_.lock(); simulableObjects_.emplace("__standaloneSensorHost", standaloneSensorHost); @@ -66,16 +64,15 @@ void World::load_from_XML( (void)xml; // unused if (0 != strcmp(root->name(), "mvsim_world")) - throw runtime_error(mrpt::format( - "XML root element is '%s' ('mvsim_world' expected)", root->name())); + throw runtime_error( + mrpt::format("XML root element is '%s' ('mvsim_world' expected)", root->name())); // Optional: format version attrib: const xml_attribute<>* attrb_version = root->first_attribute("version"); int version_major = 1, version_min = 0; if (attrb_version) { - int ret = sscanf( - attrb_version->value(), "%i.%i", &version_major, &version_min); + int ret = sscanf(attrb_version->value(), "%i.%i", &version_major, &version_min); if (ret != 2) throw runtime_error(mrpt::format( "Error parsing version attribute: '%s' ('%%i.%%i' " @@ -137,8 +134,7 @@ void World::internal_recursive_parse_XML(const XmlParserContext& ctx) userDefinedVariables_["MVSIM_CURRENT_FILE_DIRECTORY"] = basePath_; // Known tag parser? - if (auto itParser = xmlParsers_.find(node->name()); - itParser != xmlParsers_.end()) + if (auto itParser = xmlParsers_.find(node->name()); itParser != xmlParsers_.end()) { itParser->second(ctx); } @@ -167,8 +163,7 @@ void World::parse_tag_element(const XmlParserContext& ctx) worldElements_.emplace_back(e); auto lckListObjs = mrpt::lockHelper(getListOfSimulableObjectsMtx()); - simulableObjects_.emplace( - e->getName(), std::dynamic_pointer_cast(e)); + simulableObjects_.emplace(e->getName(), std::dynamic_pointer_cast(e)); } void World::parse_tag_vehicle(const XmlParserContext& ctx) @@ -185,8 +180,7 @@ void World::parse_tag_vehicle(const XmlParserContext& ctx) vehicles_.insert(VehicleList::value_type(veh->getName(), veh)); auto lckListObjs = mrpt::lockHelper(getListOfSimulableObjectsMtx()); - simulableObjects_.emplace( - veh->getName(), std::dynamic_pointer_cast(veh)); + simulableObjects_.emplace(veh->getName(), std::dynamic_pointer_cast(veh)); } void World::parse_tag_vehicle_class(const XmlParserContext& ctx) @@ -200,13 +194,11 @@ void World::parse_tag_sensor(const XmlParserContext& ctx) // top-level entries: auto lckListObjs = mrpt::lockHelper(getListOfSimulableObjectsMtx()); - DummyInvisibleBlock::Ptr standaloneSensorHost = - std::dynamic_pointer_cast( - simulableObjects_.find("__standaloneSensorHost")->second); + DummyInvisibleBlock::Ptr standaloneSensorHost = std::dynamic_pointer_cast( + simulableObjects_.find("__standaloneSensorHost")->second); ASSERT_(standaloneSensorHost); - SensorBase::Ptr sensor = - SensorBase::factory(*standaloneSensorHost, ctx.node); + SensorBase::Ptr sensor = SensorBase::factory(*standaloneSensorHost, ctx.node); standaloneSensorHost->add_sensor(sensor); } @@ -235,20 +227,14 @@ void World::parse_tag_lights(const XmlParserContext& ctx) lightOptions_.parse_from(*ctx.node, *this); } -void World::parse_tag_walls(const XmlParserContext& ctx) -{ - process_load_walls(*ctx.node); -} +void World::parse_tag_walls(const XmlParserContext& ctx) { process_load_walls(*ctx.node); } void World::parse_tag_include(const XmlParserContext& ctx) { auto fileAttrb = ctx.node->first_attribute("file"); - ASSERTMSG_( - fileAttrb, - "XML tag '' must have a 'file=\"xxx\"' attribute)"); + ASSERTMSG_(fileAttrb, "XML tag '' must have a 'file=\"xxx\"' attribute)"); - const std::string relFile = - mvsim::parse(fileAttrb->value(), user_defined_variables()); + const std::string relFile = mvsim::parse(fileAttrb->value(), user_defined_variables()); const auto absFile = this->local_to_abs_path(relFile); MRPT_LOG_DEBUG_STREAM("XML parser: including file: '" << absFile << "'"); @@ -257,8 +243,7 @@ void World::parse_tag_include(const XmlParserContext& ctx) // Inherit the user-defined variables from parent scope vars = user_defined_variables(); // Plus new variables as XML attributes, local only: - for (auto attr = ctx.node->first_attribute(); attr; - attr = attr->next_attribute()) + for (auto attr = ctx.node->first_attribute(); attr; attr = attr->next_attribute()) { if (strcmp(attr->name(), "file") == 0) continue; vars[attr->name()] = attr->value(); @@ -275,21 +260,15 @@ void World::parse_tag_include(const XmlParserContext& ctx) void World::parse_tag_variable(const XmlParserContext& ctx) { auto nameAttr = ctx.node->first_attribute("name"); - ASSERTMSG_( - nameAttr, - "XML tag '' must have a 'name=\"xxx\"' attribute)"); + ASSERTMSG_(nameAttr, "XML tag '' must have a 'name=\"xxx\"' attribute)"); const auto name = nameAttr->value(); auto valueAttr = ctx.node->first_attribute("value"); - ASSERTMSG_( - valueAttr, - "XML tag '' must have a 'value=\"xxx\"' attribute)"); + ASSERTMSG_(valueAttr, "XML tag '' must have a 'value=\"xxx\"' attribute)"); - const std::string finalValue = - mvsim::parse(valueAttr->value(), userDefinedVariables_); + const std::string finalValue = mvsim::parse(valueAttr->value(), userDefinedVariables_); - thread_local const bool MVSIM_VERBOSE_PARSE = - mrpt::get_env("MVSIM_VERBOSE_PARSE", false); + thread_local const bool MVSIM_VERBOSE_PARSE = mrpt::get_env("MVSIM_VERBOSE_PARSE", false); if (MVSIM_VERBOSE_PARSE) { @@ -305,13 +284,11 @@ void World::parse_tag_variable(const XmlParserContext& ctx) void World::parse_tag_for(const XmlParserContext& ctx) { auto varAttr = ctx.node->first_attribute("var"); - ASSERTMSG_( - varAttr, "XML tag '' must have a 'var=\"xxx\"' attribute)"); + ASSERTMSG_(varAttr, "XML tag '' must have a 'var=\"xxx\"' attribute)"); const auto varName = varAttr->value(); auto varFrom = ctx.node->first_attribute("from"); - ASSERTMSG_( - varFrom, "XML tag '' must have a 'from=\"xxx\"' attribute)"); + ASSERTMSG_(varFrom, "XML tag '' must have a 'from=\"xxx\"' attribute)"); const auto fromStr = mvsim::parse(varFrom->value(), userDefinedVariables_); auto varTo = ctx.node->first_attribute("to"); @@ -320,12 +297,10 @@ void World::parse_tag_for(const XmlParserContext& ctx) bool forBodyEmpty = true; - for (auto childNode = ctx.node->first_node(); childNode; - childNode = childNode->next_sibling()) + for (auto childNode = ctx.node->first_node(); childNode; childNode = childNode->next_sibling()) { forBodyEmpty = false; - for (int curVal = std::stoi(fromStr); curVal <= std::stoi(toStr); - curVal++) + for (int curVal = std::stoi(fromStr); curVal <= std::stoi(toStr); curVal++) { userDefinedVariables_[varName] = std::to_string(curVal); internal_recursive_parse_XML({childNode, basePath_}); @@ -346,8 +321,7 @@ void World::parse_tag_if(const XmlParserContext& ctx) bool isTrue = evaluate_tag_if(*ctx.node); if (!isTrue) return; - for (auto childNode = ctx.node->first_node(); childNode; - childNode = childNode->next_sibling()) + for (auto childNode = ctx.node->first_node(); childNode; childNode = childNode->next_sibling()) { internal_recursive_parse_XML({childNode, basePath_}); } @@ -356,8 +330,7 @@ void World::parse_tag_if(const XmlParserContext& ctx) bool World::evaluate_tag_if(const rapidxml::xml_node& node) const { auto varCond = node.first_attribute("condition"); - ASSERTMSG_( - varCond, "XML tag '' must have a 'condition=\"xxx\"' attribute)"); + ASSERTMSG_(varCond, "XML tag '' must have a 'condition=\"xxx\"' attribute)"); const auto str = mvsim::parse(varCond->value(), userDefinedVariables_); // is it "true"? @@ -366,10 +339,9 @@ bool World::evaluate_tag_if(const rapidxml::xml_node& node) const const long long ret = std::strtoll(str.c_str(), &retStr, 0 /*auto base*/); if (retStr != 0 && retStr != str.c_str()) intVal = ret; - bool isTrue = str == "y" || str == "Y" || str == "yes" || str == "Yes" || - str == "YES" || str == "true" || str == "True" || - str == "TRUE" || str == "on" || str == "ON" || str == "On" || - (intVal.has_value() && intVal.value() != 0); + bool isTrue = str == "y" || str == "Y" || str == "yes" || str == "Yes" || str == "YES" || + str == "true" || str == "True" || str == "TRUE" || str == "on" || str == "ON" || + str == "On" || (intVal.has_value() && intVal.value() != 0); return isTrue; } diff --git a/modules/simulator/src/World_services.cpp b/modules/simulator/src/World_services.cpp index 05253e16..56ce7fcf 100644 --- a/modules/simulator/src/World_services.cpp +++ b/modules/simulator/src/World_services.cpp @@ -28,8 +28,7 @@ using namespace mvsim; #if MVSIM_HAS_ZMQ && MVSIM_HAS_PROTOBUF -mvsim_msgs::SrvSetPoseAnswer World::srv_set_pose( - const mvsim_msgs::SrvSetPose& req) +mvsim_msgs::SrvSetPoseAnswer World::srv_set_pose(const mvsim_msgs::SrvSetPose& req) { mvsim_msgs::SrvSetPoseAnswer ans; ans.set_objectisincollision(false); @@ -43,9 +42,8 @@ mvsim_msgs::SrvSetPoseAnswer World::srv_set_pose( { auto p = mrpt::poses::CPose3D(itV->second->getPose()); p = p + mrpt::poses::CPose3D( - req.pose().x(), req.pose().y(), req.pose().z(), - req.pose().yaw(), req.pose().pitch(), - req.pose().roll()); + req.pose().x(), req.pose().y(), req.pose().z(), req.pose().yaw(), + req.pose().pitch(), req.pose().roll()); itV->second->setPose(p.asTPose()); auto* absPose = ans.mutable_objectglobalpose(); @@ -59,8 +57,8 @@ mvsim_msgs::SrvSetPoseAnswer World::srv_set_pose( else { itV->second->setPose( - {req.pose().x(), req.pose().y(), req.pose().z(), - req.pose().yaw(), req.pose().pitch(), req.pose().roll()}); + {req.pose().x(), req.pose().y(), req.pose().z(), req.pose().yaw(), + req.pose().pitch(), req.pose().roll()}); } ans.set_success(true); ans.set_objectisincollision(itV->second->hadCollision()); @@ -73,8 +71,7 @@ mvsim_msgs::SrvSetPoseAnswer World::srv_set_pose( return ans; } -mvsim_msgs::SrvGetPoseAnswer World::srv_get_pose( - const mvsim_msgs::SrvGetPose& req) +mvsim_msgs::SrvGetPoseAnswer World::srv_get_pose(const mvsim_msgs::SrvGetPose& req) { auto lckCopy = mrpt::lockHelper(copy_of_objects_dynstate_mtx_); @@ -104,8 +101,7 @@ mvsim_msgs::SrvGetPoseAnswer World::srv_get_pose( tw->set_wy(0); tw->set_wz(t.omega); - ans.set_objectisincollision( - copy_of_objects_had_collision_.count(sId) != 0); + ans.set_objectisincollision(copy_of_objects_had_collision_.count(sId) != 0); } else { @@ -150,14 +146,12 @@ mvsim_msgs::SrvSetControllerTwistAnswer World::srv_set_controller_twist( mvsim::ControllerBaseInterface* controller = veh->getControllerInterface(); if (!controller) { - ans.set_errormessage( - "objectId vehicle seems not to have any controller"); + ans.set_errormessage("objectId vehicle seems not to have any controller"); return ans; } const mrpt::math::TTwist2D t( - req.twistsetpoint().vx(), req.twistsetpoint().vy(), - req.twistsetpoint().wz()); + req.twistsetpoint().vx(), req.twistsetpoint().vy(), req.twistsetpoint().wz()); const bool ctrlAcceptTwist = controller->setTwistCommand(t); if (!ctrlAcceptTwist) @@ -189,22 +183,17 @@ void World::internal_advertiseServices() { #if MVSIM_HAS_ZMQ && MVSIM_HAS_PROTOBUF // global services: - client_ - .advertiseService( - "set_pose", [this](const auto& req) { return srv_set_pose(req); }); + client_.advertiseService( + "set_pose", [this](const auto& req) { return srv_set_pose(req); }); - client_ - .advertiseService( - "get_pose", [this](const auto& req) { return srv_get_pose(req); }); + client_.advertiseService( + "get_pose", [this](const auto& req) { return srv_get_pose(req); }); client_.advertiseService< - mvsim_msgs::SrvSetControllerTwist, - mvsim_msgs::SrvSetControllerTwistAnswer>( - "set_controller_twist", - [this](const auto& req) { return srv_set_controller_twist(req); }); + mvsim_msgs::SrvSetControllerTwist, mvsim_msgs::SrvSetControllerTwistAnswer>( + "set_controller_twist", [this](const auto& req) { return srv_set_controller_twist(req); }); - client_.advertiseService< - mvsim_msgs::SrvShutdown, mvsim_msgs::SrvShutdownAnswer>( + client_.advertiseService( "shutdown", [this](const auto& req) { return srv_shutdown(req); }); #endif diff --git a/modules/simulator/src/World_walls.cpp b/modules/simulator/src/World_walls.cpp index 7b0a1e0d..7d387417 100644 --- a/modules/simulator/src/World_walls.cpp +++ b/modules/simulator/src/World_walls.cpp @@ -29,9 +29,8 @@ struct WallProperties }; static Block::Ptr create_wall_segment( - World* parent, const mrpt::math::TPoint3D& rawEnd1, - const mrpt::math::TPoint3D& rawEnd2, const WallProperties& wp, - mrpt::maps::CSimplePointsMap& allPts) + World* parent, const mrpt::math::TPoint3D& rawEnd1, const mrpt::math::TPoint3D& rawEnd2, + const WallProperties& wp, mrpt::maps::CSimplePointsMap& allPts) { Block::Ptr b = std::make_shared(parent); @@ -40,8 +39,7 @@ static Block::Ptr create_wall_segment( b->setName(mrpt::format("wall_%04i", ++cnt)); } - float pt1Dist = std::numeric_limits::max(), - pt2Dist = std::numeric_limits::max(); + float pt1Dist = std::numeric_limits::max(), pt2Dist = std::numeric_limits::max(); if (!allPts.empty()) { @@ -168,12 +166,9 @@ void World::process_load_walls(const rapidxml::xml_node& node) // Load wall segments from "" tag. mrpt::math::TPolygon2D segments; - mvsim::parse_xmlnode_shape( - *xml_shape, segments, "[World::process_load_walls]"); + mvsim::parse_xmlnode_shape(*xml_shape, segments, "[World::process_load_walls]"); - MRPT_LOG_DEBUG_STREAM( - "Walls loaded from tag, " << segments.size() - << " segments."); + MRPT_LOG_DEBUG_STREAM("Walls loaded from tag, " << segments.size() << " segments."); // Transform them: tfPts.reserve(segments.size()); @@ -184,19 +179,16 @@ void World::process_load_walls(const rapidxml::xml_node& node) { // Load wall segments from external file. ASSERT_(!wallModelFileName.empty()); - const std::string localFileName = - xmlPathToActualPath(wallModelFileName); + const std::string localFileName = xmlPathToActualPath(wallModelFileName); ASSERT_FILE_EXISTS_(localFileName); - MRPT_LOG_DEBUG_STREAM( - "Loading walls definition model from: " << localFileName); + MRPT_LOG_DEBUG_STREAM("Loading walls definition model from: " << localFileName); auto glModel = mrpt::opengl::CAssimpModel::Create(); glModel->loadScene(localFileName); const auto& points = glModel->shaderWireframeVertexPointBuffer(); - MRPT_LOG_DEBUG_STREAM( - "Walls loaded from model file, " << points.size() << " segments."); + MRPT_LOG_DEBUG_STREAM("Walls loaded from model file, " << points.size() << " segments."); // Transform them: tfPts.reserve(points.size()); diff --git a/modules/simulator/src/XMLClassesRegistry.cpp b/modules/simulator/src/XMLClassesRegistry.cpp index f4462745..d3fb9bc6 100644 --- a/modules/simulator/src/XMLClassesRegistry.cpp +++ b/modules/simulator/src/XMLClassesRegistry.cpp @@ -17,8 +17,7 @@ using namespace mvsim; using namespace std; -const rapidxml::xml_node* XmlClassesRegistry::get( - const std::string& xml_node_class) const +const rapidxml::xml_node* XmlClassesRegistry::get(const std::string& xml_node_class) const { map::const_iterator it = classes_.find(xml_node_class); if (it == classes_.end()) @@ -40,15 +39,12 @@ void XmlClassesRegistry::add(const std::string& input_xml_node_class) // sanity checks: // e.g. "vehicle:class" - const rapidxml::xml_node<>* root_node = - xml->first_node(tagname_.c_str()); + const rapidxml::xml_node<>* root_node = xml->first_node(tagname_.c_str()); if (!root_node) - throw runtime_error(mrpt::format( - "[XmlClassesRegistry] Missing XML node <%s>", - tagname_.c_str())); + throw runtime_error( + mrpt::format("[XmlClassesRegistry] Missing XML node <%s>", tagname_.c_str())); - const rapidxml::xml_attribute<>* att_name = - root_node->first_attribute("name"); + const rapidxml::xml_attribute<>* att_name = root_node->first_attribute("name"); if (!att_name || !att_name->value()) throw runtime_error(mrpt::format( "[XmlClassesRegistry] Missing mandatory attribute " @@ -64,12 +60,11 @@ void XmlClassesRegistry::add(const std::string& input_xml_node_class) } catch (const rapidxml::parse_error& e) { - unsigned int line = - static_cast(std::count(input_str, e.where(), '\n') + 1); + unsigned int line = static_cast(std::count(input_str, e.where(), '\n') + 1); delete xml; throw std::runtime_error(mrpt::format( - "[XmlClassesRegistry] XML parse error (Line %u): %s", - static_cast(line), e.what())); + "[XmlClassesRegistry] XML parse error (Line %u): %s", static_cast(line), + e.what())); } catch (const std::exception&) { diff --git a/modules/simulator/src/XMLClassesRegistry.h b/modules/simulator/src/XMLClassesRegistry.h index 4e978c63..3fef62a0 100644 --- a/modules/simulator/src/XMLClassesRegistry.h +++ b/modules/simulator/src/XMLClassesRegistry.h @@ -38,14 +38,10 @@ class XmlClassesRegistry public: /** Define the xml tag, e.g. "vehicle:class" for "..." */ - XmlClassesRegistry(const std::string& xml_class_tag) - : tagname_(xml_class_tag) - { - } + XmlClassesRegistry(const std::string& xml_class_tag) : tagname_(xml_class_tag) {} /** Return an XML node with the class definition, or nullptr if not found */ - const rapidxml::xml_node* get( - const std::string& xml_node_class) const; + const rapidxml::xml_node* get(const std::string& xml_node_class) const; /** Register a new class, given its XML definition as a text block */ void add(const std::string& input_xml_node_class); diff --git a/modules/simulator/src/parse_utils.cpp b/modules/simulator/src/parse_utils.cpp index c050dbef..afb10d37 100644 --- a/modules/simulator/src/parse_utils.cpp +++ b/modules/simulator/src/parse_utils.cpp @@ -23,8 +23,7 @@ #include #include -thread_local const bool MVSIM_VERBOSE_PARSE = - mrpt::get_env("MVSIM_VERBOSE_PARSE", false); +thread_local const bool MVSIM_VERBOSE_PARSE = mrpt::get_env("MVSIM_VERBOSE_PARSE", false); using namespace mvsim; @@ -54,8 +53,7 @@ static std::string parseEnvVars(const std::string& text) else { THROW_EXCEPTION_FMT( - "parseEnvVars(): Undefined environment variable found: $env{%s}", - varname.c_str()); + "parseEnvVars(): Undefined environment variable found: $env{%s}", varname.c_str()); } return parseEnvVars(pre + varvalue + post.substr(post_end + 1)); @@ -63,8 +61,7 @@ static std::string parseEnvVars(const std::string& text) } static std::string parseVars( - const std::string& text, - const std::map& variableNamesValues) + const std::string& text, const std::map& variableNamesValues) { MRPT_TRY_START @@ -84,8 +81,7 @@ static std::string parseVars( const auto varname = post.substr(0, post_end); std::string varvalue; - if (const auto it = variableNamesValues.find(varname); - it != variableNamesValues.end()) + if (const auto it = variableNamesValues.find(varname); it != variableNamesValues.end()) { varvalue = it->second; } @@ -99,12 +95,11 @@ static std::string parseVars( } THROW_EXCEPTION_FMT( - "parseVars(): Undefined variable found: ${%s}. Known ones are: %s", - varname.c_str(), allKnown.c_str()); + "parseVars(): Undefined variable found: ${%s}. Known ones are: %s", varname.c_str(), + allKnown.c_str()); } - return parseVars( - pre + varvalue + post.substr(post_end + 1), variableNamesValues); + return parseVars(pre + varvalue + post.substr(post_end + 1), variableNamesValues); MRPT_TRY_END } @@ -134,9 +129,7 @@ static std::string parseCmdRuns(const std::string& text) int ret = mrpt::system::executeCommand(cmd, &cmdOut); if (ret != 0) { - THROW_EXCEPTION_FMT( - "Error (retval=%i) executing external command: `%s`", ret, - cmd.c_str()); + THROW_EXCEPTION_FMT("Error (retval=%i) executing external command: `%s`", ret, cmd.c_str()); } // Clear whitespaces: cmdOut = mrpt::system::trim(cmdOut); @@ -167,8 +160,7 @@ static double randn() // Examples: "$f{180/5}", "$f{ ${MAX_SPEED} * sin(deg2rad(45)) }" static std::string parseMathExpr( - const std::string& text, - const std::map& variableNamesValues) + const std::string& text, const std::map& variableNamesValues) { MRPT_TRY_START @@ -211,22 +203,19 @@ static std::string parseMathExpr( expr.compile(sExpr, numericVars); const double val = expr.eval(); - return parseCmdRuns( - pre + mrpt::format("%g", val) + post.substr(post_end + 1)); + return parseCmdRuns(pre + mrpt::format("%g", val) + post.substr(post_end + 1)); MRPT_TRY_END } std::string mvsim::parse( - const std::string& input, - const std::map& variableNamesValues) + const std::string& input, const std::map& variableNamesValues) { if (MVSIM_VERBOSE_PARSE) { std::cout << "[mvsim::parse] Input : '" << input << "' " << "with these variables: "; - for (const auto& kv : variableNamesValues) - std::cout << kv.first << ", "; + for (const auto& kv : variableNamesValues) std::cout << kv.first << ", "; std::cout << "\n"; } diff --git a/modules/simulator/src/parse_utils.h b/modules/simulator/src/parse_utils.h index 2115db41..01e5b9eb 100644 --- a/modules/simulator/src/parse_utils.h +++ b/modules/simulator/src/parse_utils.h @@ -21,7 +21,6 @@ namespace mvsim * */ std::string parse( - const std::string& input, - const std::map& variableNamesValues = {}); + const std::string& input, const std::map& variableNamesValues = {}); } // namespace mvsim diff --git a/modules/simulator/src/xml_utils.cpp b/modules/simulator/src/xml_utils.cpp index 2270f9d2..d4b75e21 100644 --- a/modules/simulator/src/xml_utils.cpp +++ b/modules/simulator/src/xml_utils.cpp @@ -67,8 +67,7 @@ void TParamEntry::parse( { bool& bool_val = *reinterpret_cast(val); - const std::string sStr = - mrpt::system::lowerCase(mrpt::system::trim(std::string(str))); + const std::string sStr = mrpt::system::lowerCase(mrpt::system::trim(std::string(str))); if (sStr == "1" || sStr == "true") bool_val = true; else if (sStr == "0" || sStr == "false") @@ -119,20 +118,17 @@ void TParamEntry::parse( // Sub-cases: if (!strcmp(frmt, "%pose2d")) { - mrpt::poses::CPose2D& pp = - *reinterpret_cast(val); + mrpt::poses::CPose2D& pp = *reinterpret_cast(val); pp = p; } else if (!strcmp(frmt, "%pose2d_ptr3d")) { - mrpt::poses::CPose3D& pp = - *reinterpret_cast(val); + mrpt::poses::CPose3D& pp = *reinterpret_cast(val); pp = mrpt::poses::CPose3D(p); } else - throw std::runtime_error(mrpt::format( - "%s Error: Unknown format specifier '%s'", functionNameContext, - frmt)); + throw std::runtime_error( + mrpt::format("%s Error: Unknown format specifier '%s'", functionNameContext, frmt)); } // %point3d else if (!strncmp(frmt, "%point3d", strlen("%point3d"))) @@ -141,11 +137,10 @@ void TParamEntry::parse( int ret = ::sscanf(str.c_str(), "%lf %lf %lf", &x, &y, &z); if (ret != 2 && ret != 3) throw std::runtime_error(mrpt::format( - "%s Error parsing '%s'='%s' (Expected format:'X Y [Z]')", - functionNameContext, varName.c_str(), str.c_str())); + "%s Error parsing '%s'='%s' (Expected format:'X Y [Z]')", functionNameContext, + varName.c_str(), str.c_str())); - mrpt::math::TPoint3D& pp = - *reinterpret_cast(val); + mrpt::math::TPoint3D& pp = *reinterpret_cast(val); pp.x = x; pp.y = y; @@ -156,8 +151,7 @@ void TParamEntry::parse( { double x, y, z, yawDeg, pitchDeg, rollDeg; int ret = ::sscanf( - str.c_str(), "%lf %lf %lf %lf %lf %lf", &x, &y, &z, &yawDeg, - &pitchDeg, &rollDeg); + str.c_str(), "%lf %lf %lf %lf %lf %lf", &x, &y, &z, &yawDeg, &pitchDeg, &rollDeg); if (ret != 6) throw std::runtime_error(mrpt::format( "%s Error parsing '%s'='%s' (Expected format:'X Y Z" @@ -171,8 +165,7 @@ void TParamEntry::parse( const mrpt::poses::CPose3D p(x, y, yaw); - mrpt::poses::CPose3D& pp = - *reinterpret_cast(val); + mrpt::poses::CPose3D& pp = *reinterpret_cast(val); pp = mrpt::poses::CPose3D(x, y, z, yaw, pitch, roll); } else @@ -187,35 +180,28 @@ void TParamEntry::parse( } void mvsim::parse_xmlnode_attribs( - const rapidxml::xml_node& xml_node, - const TParameterDefinitions& params, - const std::map& variableNamesValues, - const char* functionNameContext) + const rapidxml::xml_node& xml_node, const TParameterDefinitions& params, + const std::map& variableNamesValues, const char* functionNameContext) { for (const auto& param : params) { - if (auto attr = xml_node.first_attribute(param.first.c_str()); - attr && attr->value()) + if (auto attr = xml_node.first_attribute(param.first.c_str()); attr && attr->value()) { param.second.parse( - attr->value(), attr->name(), variableNamesValues, - functionNameContext); + attr->value(), attr->name(), variableNamesValues, functionNameContext); } } } bool mvsim::parse_xmlnode_as_param( - const rapidxml::xml_node& xml_node, - const TParameterDefinitions& params, - const std::map& variableNamesValues, - const char* functionNameContext) + const rapidxml::xml_node& xml_node, const TParameterDefinitions& params, + const std::map& variableNamesValues, const char* functionNameContext) { if (auto it_param = params.find(xml_node.name()); it_param != params.end()) { // parse parameter: it_param->second.parse( - xml_node.value(), xml_node.name(), variableNamesValues, - functionNameContext); + xml_node.value(), xml_node.name(), variableNamesValues, functionNameContext); return true; } else @@ -228,19 +214,18 @@ bool mvsim::parse_xmlnode_as_param( */ void mvsim::parse_xmlnode_children_as_param( const rapidxml::xml_node& root, const TParameterDefinitions& params, - const std::map& variableNamesValues, - const char* functionNameContext, mrpt::system::COutputLogger* logger) + const std::map& variableNamesValues, const char* functionNameContext, + mrpt::system::COutputLogger* logger) { rapidxml::xml_node<>* node = root.first_node(); while (node) { - bool recognized = parse_xmlnode_as_param( - *node, params, variableNamesValues, functionNameContext); + bool recognized = + parse_xmlnode_as_param(*node, params, variableNamesValues, functionNameContext); if (!recognized && logger) { logger->logFmt( - mrpt::system::LVL_WARN, "Unrecognized tag '<%s>' in %s", - node->name(), + mrpt::system::LVL_WARN, "Unrecognized tag '<%s>' in %s", node->name(), functionNameContext ? functionNameContext : "(none)"); } node = node->next_sibling(nullptr); // Move on to next node @@ -248,8 +233,7 @@ void mvsim::parse_xmlnode_children_as_param( } mrpt::math::TPose2D mvsim::parseXYPHI( - const std::string& sOrg, bool allow_missing_angle, - double default_angle_radians, + const std::string& sOrg, bool allow_missing_angle, double default_angle_radians, const std::map& variableNamesValues) { mrpt::math::TPose2D v; @@ -261,10 +245,8 @@ mrpt::math::TPose2D mvsim::parseXYPHI( // User provides numbers as degrees: v.phi = mrpt::DEG2RAD(v.phi); - if ((na != 3 && !allow_missing_angle) || - (na != 2 && na != 3 && allow_missing_angle)) - throw std::runtime_error( - mrpt::format("Malformed pose string: '%s'", s.c_str())); + if ((na != 3 && !allow_missing_angle) || (na != 2 && na != 3 && allow_missing_angle)) + throw std::runtime_error(mrpt::format("Malformed pose string: '%s'", s.c_str())); return v; } @@ -283,8 +265,8 @@ void mvsim::parse_xmlnode_shape( pt_node = pt_node->next_sibling("pt")) { if (!pt_node->value()) - throw std::runtime_error(mrpt::format( - "%s Error: node seems empty.", functionNameContext)); + throw std::runtime_error( + mrpt::format("%s Error: node seems empty.", functionNameContext)); mrpt::math::TPoint2D pt; const char* str_val = pt_node->value(); @@ -305,8 +287,7 @@ void mvsim::parse_xmlnode_shape( } std::tuple>, rapidxml::xml_node<>*> - mvsim::readXmlTextAndGetRoot( - const std::string& xmlData, const std::string& pathToFile) + mvsim::readXmlTextAndGetRoot(const std::string& xmlData, const std::string& pathToFile) { using namespace rapidxml; using namespace std::string_literals; @@ -320,24 +301,19 @@ std::tuple>, rapidxml::xml_node<>*> } catch (rapidxml::parse_error& e) { - unsigned int line = - static_cast(std::count(input_str, e.where(), '\n') + 1); - throw std::runtime_error(mrpt::format( - "XML parse error (Line %u): %s", static_cast(line), - e.what())); + unsigned int line = static_cast(std::count(input_str, e.where(), '\n') + 1); + throw std::runtime_error( + mrpt::format("XML parse error (Line %u): %s", static_cast(line), e.what())); } // Sanity check: xml_node<>* root = xml->first_node(); - ASSERTMSG_( - root, "XML parse error: No root node found (empty file '"s + - pathToFile + "'?)"s); + ASSERTMSG_(root, "XML parse error: No root node found (empty file '"s + pathToFile + "'?)"s); return {xml, root}; } std::tuple*> mvsim::readXmlAndGetRoot( - const std::string& pathToFile, - const std::map& variables, + const std::string& pathToFile, const std::map& variables, const std::set& varsRetain) { using namespace rapidxml; @@ -349,13 +325,11 @@ std::tuple*> mvsim::readXmlAndGetRoot( // Special variables: std::map childVariables = variables; - childVariables["MVSIM_CURRENT_FILE_DIRECTORY"] = - mrpt::system::toAbsolutePath( - mrpt::system::extractFileDirectory(pathToFile), - false /*canonical*/); + childVariables["MVSIM_CURRENT_FILE_DIRECTORY"] = mrpt::system::toAbsolutePath( + mrpt::system::extractFileDirectory(pathToFile), false /*canonical*/); - xmlDoc->documentData = mvsim::parse_variables( - mrpt::io::file_get_contents(pathToFile), childVariables, varsRetain); + xmlDoc->documentData = + mvsim::parse_variables(mrpt::io::file_get_contents(pathToFile), childVariables, varsRetain); auto [xml, root] = readXmlTextAndGetRoot(xmlDoc->documentData, pathToFile); @@ -365,8 +339,7 @@ std::tuple*> mvsim::readXmlAndGetRoot( } static std::string::size_type findClosing( - size_t pos, const std::string& s, const char searchEndChar, - const char otherStartChar) + size_t pos, const std::string& s, const char searchEndChar, const char otherStartChar) { int openEnvs = 1; for (; pos < s.size(); pos++) @@ -389,8 +362,7 @@ static std::string::size_type findClosing( } // "foo|bar" -> {"foo","bar"} -static std::tuple splitVerticalBar( - const std::string& s) +static std::tuple splitVerticalBar(const std::string& s) { const auto posBar = s.find("|"); if (posBar == std::string::npos) return {s, {}}; @@ -399,8 +371,7 @@ static std::tuple splitVerticalBar( } static std::string parseVars( - const std::string& text, - const std::map& variables, + const std::string& text, const std::map& variables, const std::set& varsRetain, const size_t searchStartPos = 0) { MRPT_TRY_START @@ -446,14 +417,11 @@ static std::string parseVars( } else { - THROW_EXCEPTION_FMT( - "mvsim::parseVars(): Undefined variable: ${%s}", - varname.c_str()); + THROW_EXCEPTION_FMT("mvsim::parseVars(): Undefined variable: ${%s}", varname.c_str()); } } - return parseVars( - pre + varvalue + post.substr(post_end + 1), variables, varsRetain); + return parseVars(pre + varvalue + post.substr(post_end + 1), variables, varsRetain); MRPT_TRY_END } @@ -463,8 +431,7 @@ std::string mvsim::parse_variables( { const auto ret = parseVars(in, variables, varsRetain); - thread_local const bool MVSIM_VERBOSE_PARSE = - mrpt::get_env("MVSIM_VERBOSE_PARSE", false); + thread_local const bool MVSIM_VERBOSE_PARSE = mrpt::get_env("MVSIM_VERBOSE_PARSE", false); if (MVSIM_VERBOSE_PARSE) { std::cout << "[parse_variables] Input:\n" << in << "\n"; @@ -478,16 +445,14 @@ std::string mvsim::parse_variables( } static void recursive_xml_to_str_solving_includes( - const World& parent, const rapidxml::xml_node* n, - const std::set& varsRetain, std::stringstream& ss) + const World& parent, const rapidxml::xml_node* n, const std::set& varsRetain, + std::stringstream& ss) { // TAG: if (strcmp(n->name(), "include") == 0) { auto fileAttrb = n->first_attribute("file"); - ASSERTMSG_( - fileAttrb, - "XML tag '' must have a 'file=\"xxx\"' attribute)"); + ASSERTMSG_(fileAttrb, "XML tag '' must have a 'file=\"xxx\"' attribute)"); const std::string relFile = mvsim::parse(fileAttrb->value(), parent.user_defined_variables()); @@ -500,8 +465,7 @@ static void recursive_xml_to_str_solving_includes( // Inherit the user-defined variables from parent scope vars = parent.user_defined_variables(); // Plus new ones: - for (auto attr = n->first_attribute(); attr; - attr = attr->next_attribute()) + for (auto attr = n->first_attribute(); attr; attr = attr->next_attribute()) { if (strcmp(attr->name(), "file") == 0) continue; vars[attr->name()] = attr->value(); @@ -519,11 +483,9 @@ static void recursive_xml_to_str_solving_includes( bool isTrue = parent.evaluate_tag_if(*n); if (!isTrue) return; - for (auto childNode = n->first_node(); childNode; - childNode = childNode->next_sibling()) + for (auto childNode = n->first_node(); childNode; childNode = childNode->next_sibling()) { - recursive_xml_to_str_solving_includes( - parent, childNode, varsRetain, ss); + recursive_xml_to_str_solving_includes(parent, childNode, varsRetain, ss); } } else diff --git a/modules/simulator/src/xml_utils.h b/modules/simulator/src/xml_utils.h index 5a996cc3..aa1ba947 100644 --- a/modules/simulator/src/xml_utils.h +++ b/modules/simulator/src/xml_utils.h @@ -39,13 +39,11 @@ struct XML_Doc_Data std::shared_ptr> doc; }; -std::tuple>, rapidxml::xml_node<>*> - readXmlTextAndGetRoot( - const std::string& xmlData, const std::string& pathToFile); +std::tuple>, rapidxml::xml_node<>*> readXmlTextAndGetRoot( + const std::string& xmlData, const std::string& pathToFile); std::tuple*> readXmlAndGetRoot( - const std::string& pathToFile, - const std::map& variables, + const std::string& pathToFile, const std::map& variables, const std::set& varsRetain = {}); /** @@ -60,8 +58,7 @@ std::string parse_variables( const std::set& varsRetain); void parse_xmlnode_attribs( - const rapidxml::xml_node& xml_node, - const TParameterDefinitions& params, + const rapidxml::xml_node& xml_node, const TParameterDefinitions& params, const std::map& variableNamesValues = {}, const char* functionNameContext = ""); @@ -70,24 +67,20 @@ void parse_xmlnode_attribs( * \sa parse_xmlnode_children_as_param */ bool parse_xmlnode_as_param( - const rapidxml::xml_node& xml_node, - const TParameterDefinitions& params, + const rapidxml::xml_node& xml_node, const TParameterDefinitions& params, const std::map& variableNamesValues = {}, const char* functionNameContext = ""); /** Call \a parse_xmlnode_as_param() for all children nodes of the given node. */ void parse_xmlnode_children_as_param( - const rapidxml::xml_node& xml_node, - const TParameterDefinitions& params, + const rapidxml::xml_node& xml_node, const TParameterDefinitions& params, const std::map& variableNamesValues = {}, - const char* functionNameContext = "", - mrpt::system::COutputLogger* logger = nullptr); + const char* functionNameContext = "", mrpt::system::COutputLogger* logger = nullptr); template void parse_xmlnodelist_children_as_param( - NODE_LIST& lst_nodes, const TParameterDefinitions& params, - const char* functionNameContext = "") + NODE_LIST& lst_nodes, const TParameterDefinitions& params, const char* functionNameContext = "") { for (auto& node : lst_nodes) parse_xmlnode_children_as_param(*node, params, functionNameContext); @@ -108,8 +101,7 @@ std::string xml_to_str_solving_includes( * exception upon malformed string. */ mrpt::math::TPose2D parseXYPHI( - const std::string& s, bool allow_missing_angle = false, - double default_angle_radians = 0.0, + const std::string& s, bool allow_missing_angle = false, double default_angle_radians = 0.0, const std::map& variableNamesValues = {}); /** Parses a X Y... XML node into a