simplesim: optional collision

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 ) );
     }
 

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(

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 );

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 ) {

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() );
     }

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 );

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

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();

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 )

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 );
 }