diff --git a/softwareComponents/simplesimClientLib/include/simplesim_client.hpp b/softwareComponents/simplesimClientLib/include/simplesim_client.hpp index d2f87f2b7..5a42529ff 100644 --- a/softwareComponents/simplesimClientLib/include/simplesim_client.hpp +++ b/softwareComponents/simplesimClientLib/include/simplesim_client.hpp @@ -94,10 +94,10 @@ class SimplesimClient : public QMainWindow { // Can be called from any thread void onConfigurationUpdate( - std::shared_ptr< const rofi::configuration::RofiWorld > newConfiguration ) + std::shared_ptr< const rofi::configuration::RofiWorld > newConfiguration, bool collision ) { assert( newConfiguration ); - assert( newConfiguration->isValid( rofi::configuration::SimpleCollision() ) ); + assert( !collision || newConfiguration->isValid( rofi::configuration::SimpleCollision() ) ); _currentConfiguration.replace( std::move( newConfiguration ) ); } diff --git a/softwareComponents/simplesimLib/include/simplesim/module_states.hpp b/softwareComponents/simplesimLib/include/simplesim/module_states.hpp index d24cd3a34..d9e6f7a9a 100644 --- a/softwareComponents/simplesimLib/include/simplesim/module_states.hpp +++ b/softwareComponents/simplesimLib/include/simplesim/module_states.hpp @@ -251,7 +251,7 @@ class ModuleStates { using RofiWorldConfigurationPtr = std::shared_ptr< const rofi::configuration::RofiWorld >; - explicit ModuleStates( RofiWorldConfigurationPtr rofiworldConfiguration, bool verbose ) + explicit ModuleStates( RofiWorldConfigurationPtr rofiworldConfiguration, bool verbose, bool collision ) : _physicalModulesConfiguration( rofiworldConfiguration ? std::move( rofiworldConfiguration ) @@ -262,9 +262,9 @@ class ModuleStates { return initInnerStatesFromConfiguration( *configPtr, verbose ); } ) ) { - assert( _physicalModulesConfiguration.visit( []( const auto & configuration ) { + assert( _physicalModulesConfiguration.visit( [ collision ]( const auto & configuration ) { assert( configuration ); - return configuration->isValid( rofi::configuration::SimpleCollision() ); + return !collision || configuration->isValid( rofi::configuration::SimpleCollision() ); } ) ); } @@ -312,9 +312,9 @@ class ModuleStates { template < std::invocable< RofiResp > Callback > auto updateToNextIteration( std::chrono::duration< float > simStepTime, - Callback onRespCallback ) -> RofiWorldConfigurationPtr + Callback onRespCallback, bool collision ) -> RofiWorldConfigurationPtr { - auto [ newConfiguration, updateEvents ] = computeNextIteration( simStepTime ); + auto [ newConfiguration, updateEvents ] = computeNextIteration( simStepTime, collision ); assert( newConfiguration ); auto oldConfiguration = _physicalModulesConfiguration.visit( @@ -359,7 +359,7 @@ class ModuleStates { } private: - auto computeNextIteration( std::chrono::duration< float > simStepTime ) const + auto computeNextIteration( std::chrono::duration< float > simStepTime, bool collision ) const -> std::pair< RofiWorldConfigurationPtr, detail::ConfigurationUpdateEvents >; static auto initInnerStatesFromConfiguration( diff --git a/softwareComponents/simplesimLib/include/simplesim/simplesim.hpp b/softwareComponents/simplesimLib/include/simplesim/simplesim.hpp index 37967c7f0..dd68fbc43 100644 --- a/softwareComponents/simplesimLib/include/simplesim/simplesim.hpp +++ b/softwareComponents/simplesimLib/include/simplesim/simplesim.hpp @@ -88,10 +88,11 @@ class Simplesim { Simplesim( std::shared_ptr< const rofi::configuration::RofiWorld > worldConfiguration, PacketFilter::FilterFunction packetFilter, - bool verbose ) + bool verbose, bool collision ) : _simulation( std::make_shared< Simulation >( std::move( worldConfiguration ), std::move( packetFilter ), - verbose ) ) + verbose, + collision ) ) , _communication( std::make_shared< Communication >( _simulation->commandHandler(), verbose ) ) { @@ -104,7 +105,7 @@ class Simplesim { return _communication; } - void run( OnConfigurationUpdate onConfigurationUpdate, std::stop_token stopToken = {} ); + void run( OnConfigurationUpdate onConfigurationUpdate, bool collision, std::stop_token stopToken = {} ); // Can be called from any thread [[nodiscard]] ServerSettings onSettingsCmd( const msgs::SettingsCmd & settingsCmd ); diff --git a/softwareComponents/simplesimLib/include/simplesim/simulation.hpp b/softwareComponents/simplesimLib/include/simplesim/simulation.hpp index 4972781ae..8a51b1e75 100644 --- a/softwareComponents/simplesimLib/include/simplesim/simulation.hpp +++ b/softwareComponents/simplesimLib/include/simplesim/simulation.hpp @@ -29,9 +29,9 @@ class Simulation { explicit Simulation( std::shared_ptr< const rofi::configuration::RofiWorld > rofiworldConfiguration, PacketFilter::FilterFunction packetFilter, - bool verbose ) + bool verbose, bool collision ) : _moduleStates( - std::make_shared< ModuleStates >( std::move( rofiworldConfiguration ), verbose ) ) + std::make_shared< ModuleStates >( std::move( rofiworldConfiguration ), verbose, collision ) ) , _commandHandler( std::make_shared< CommandHandler >( this->_moduleStates, std::move( packetFilter ) ) ) { @@ -42,7 +42,7 @@ class Simulation { // Moves each rofi module based on the inner state // Returns the responses that happen inside RoFIs std::pair< std::vector< RofiResp >, std::shared_ptr< const rofi::configuration::RofiWorld > > - simulateOneIteration( std::chrono::milliseconds duration ) + simulateOneIteration( std::chrono::milliseconds duration, bool collision ) { assert( _commandHandler ); assert( _moduleStates ); @@ -52,7 +52,7 @@ class Simulation { auto new_configuration = _moduleStates->updateToNextIteration( duration, [ &responses ]( RofiResp resp ) { responses.push_back( std::move( resp ) ); - } ); + }, collision ); assert( new_configuration ); _commandHandler->advanceTime( duration, [ &responses ]( auto resp ) { diff --git a/softwareComponents/simplesimLib/src/module_states.cpp b/softwareComponents/simplesimLib/src/module_states.cpp index 5996668ee..9b68f562e 100644 --- a/softwareComponents/simplesimLib/src/module_states.cpp +++ b/softwareComponents/simplesimLib/src/module_states.cpp @@ -318,7 +318,7 @@ auto updateConnectorStates( RofiWorld & configuration, return connectorUpdateEvents; } -auto ModuleStates::computeNextIteration( std::chrono::duration< float > simStepTime ) const +auto ModuleStates::computeNextIteration( std::chrono::duration< float > simStepTime, bool collision ) const -> std::pair< RofiWorldConfigurationPtr, detail::ConfigurationUpdateEvents > { using CUE = detail::ConfigurationUpdateEvents; @@ -336,7 +336,7 @@ auto ModuleStates::computeNextIteration( std::chrono::duration< float > simStepT // Workaround for a bug in configuration (not setting the prepared flag properly) newConfiguration->prepare().get_or_throw_as< std::logic_error >(); - if ( auto ok = newConfiguration->validate( SimpleCollision{} ); !ok ) { + if ( auto ok = newConfiguration->validate( SimpleCollision{} ); collision && !ok ) { std::cerr << "Error after joint update: '" << ok.assume_error() << "'\n"; throw std::runtime_error( std::move( ok ).assume_error() ); } @@ -346,7 +346,7 @@ auto ModuleStates::computeNextIteration( std::chrono::duration< float > simStepT connectorUpdateEvents.connectorsToFinalizePosition ); updateEvents.connectionsChanged = std::move( connectorUpdateEvents.connectionsChanged ); - if ( auto ok = newConfiguration->validate( SimpleCollision{} ); !ok ) { + if ( auto ok = newConfiguration->validate( SimpleCollision{} ); collision && !ok ) { std::cerr << "Error after connector update: '" << ok.assume_error() << "'\n"; throw std::runtime_error( std::move( ok ).assume_error() ); } diff --git a/softwareComponents/simplesimLib/src/simplesim.cpp b/softwareComponents/simplesimLib/src/simplesim.cpp index 04b0354d2..12e9060ed 100644 --- a/softwareComponents/simplesimLib/src/simplesim.cpp +++ b/softwareComponents/simplesimLib/src/simplesim.cpp @@ -3,7 +3,7 @@ namespace rofi::simplesim { -void Simplesim::run( Simplesim::OnConfigurationUpdate onConfigurationUpdate, +void Simplesim::run( Simplesim::OnConfigurationUpdate onConfigurationUpdate, bool collision, std::stop_token stopToken ) { assert( onConfigurationUpdate && "Provide on configuration update callback" ); @@ -33,7 +33,7 @@ void Simplesim::run( Simplesim::OnConfigurationUpdate onConfigurationUpdate, } auto [ responses, - newConfiguration ] = simulation.simulateOneIteration( settings.getSimStepTime() ); + newConfiguration ] = simulation.simulateOneIteration( settings.getSimStepTime(), collision ); lastConfiguration = std::move( newConfiguration ); assert( onConfigurationUpdate ); diff --git a/softwareComponents/simplesimServerLib/include/simplesim_server.hpp b/softwareComponents/simplesimServerLib/include/simplesim_server.hpp index f577c336e..307bce83b 100644 --- a/softwareComponents/simplesimServerLib/include/simplesim_server.hpp +++ b/softwareComponents/simplesimServerLib/include/simplesim_server.hpp @@ -34,6 +34,8 @@ class SimplesimServerOpts { .desc( "Python packet filter file" ); cli.opt( &verbose, "v verbose" ).desc( "Run simulator in verbose mode" ); + + cli.opt( &collision, "c collision" ).desc( "Crash on collision"); } auto readInputWorldFile() const -> atoms::Result< rofi::configuration::RofiWorld > @@ -63,6 +65,7 @@ class SimplesimServerOpts { std::optional< std::filesystem::path > pyPacketFilterFile = {}; bool verbose = {}; + bool collision = {}; }; } // namespace rofi::simplesim diff --git a/tools/simplesim/main.cpp b/tools/simplesim/main.cpp index dcdace563..34b8f8a80 100644 --- a/tools/simplesim/main.cpp +++ b/tools/simplesim/main.cpp @@ -55,7 +55,8 @@ int main( int argc, char * argv[] ) return packetFilter.filter( std::move( packet ) ); } : simplesim::PacketFilter::FilterFunction{}, - opts.verbose ); + opts.verbose, + opts.collision ); // Setup client auto client = simplesim::SimplesimClient(); @@ -68,17 +69,18 @@ int main( int argc, char * argv[] ) std::cout << "Starting simplesim server..." << std::endl; // Run server - auto serverThread = std::jthread( [ &server, &client ]( std::stop_token stopToken ) { + auto serverThread = std::jthread( [ &server, &client, collision = opts.collision ]( std::stop_token stopToken ) { server.run( - [ &client ]( std::shared_ptr< const configuration::RofiWorld > newRofiWorld ) { - client.onConfigurationUpdate( std::move( newRofiWorld ) ); - }, + [ &client, collision ]( std::shared_ptr< const configuration::RofiWorld > newRofiWorld ) { + client.onConfigurationUpdate( std::move( newRofiWorld ), collision ); + }, + collision, stopToken ); } ); // Run client std::cout << "Adding configuration to the client" << std::endl; - client.onConfigurationUpdate( std::move( *inputWorld ) ); + client.onConfigurationUpdate( std::move( *inputWorld ), opts.collision ); std::cout << "Starting simplesim client..." << std::endl; client.run(); diff --git a/tools/simplesimClient/main.cpp b/tools/simplesimClient/main.cpp index 9d2ea3108..88310dfb1 100644 --- a/tools/simplesimClient/main.cpp +++ b/tools/simplesimClient/main.cpp @@ -64,7 +64,8 @@ class SimplesimMsgSubscriber { return; } - _client.onConfigurationUpdate( std::move( rofiworld ) ); + // There should not be a collision when loading a configuration from a file + _client.onConfigurationUpdate( std::move( rofiworld ), true ); } void onSettingsResp( const SettingsStateMsgPtr & msgPtr ) diff --git a/tools/simplesimServer/main.cpp b/tools/simplesimServer/main.cpp index a5bf01d10..bd1e501aa 100644 --- a/tools/simplesimServer/main.cpp +++ b/tools/simplesimServer/main.cpp @@ -96,7 +96,8 @@ int main( int argc, char * argv[] ) return packetFilter.filter( std::move( packet ) ); } : simplesim::PacketFilter::FilterFunction{}, - opts.verbose ); + opts.verbose, + opts.collision ); // Listen for settings cmds auto settingsCmdSub = SettingsCmdSubscriber( server ); @@ -115,5 +116,6 @@ int main( int argc, char * argv[] ) auto message = google::protobuf::StringValue(); message.set_value( configuration::serialization::toJSON( *newRofiWorld ).dump() ); configurationPub->Publish( message, true ); - } ); + }, + opts.collision ); }