Added restore_physx_physics function

LibCarla/source/carla/client/Vehicle.cpp

@@ -144,6 +144,10 @@ namespace client {
         BaseJSONPath);
   }
 
+  void Vehicle::RestorePhysXPhysics() {
+    GetEpisode().Lock()->RestorePhysXPhysics(*this);
+  }
+
   rpc::VehicleFailureState Vehicle::GetFailureState() const {
     return GetEpisode().Lock()->GetActorSnapshot(*this).state.vehicle_data.failure_state;
   }

LibCarla/source/carla/client/Vehicle.h

@@ -137,6 +137,8 @@ namespace client {
         std::string TireJSON = "",
         std::string BaseJSONPath = "");
 
+    void RestorePhysXPhysics();
+
     /// Returns the failure state of the vehicle
     rpc::VehicleFailureState GetFailureState() const;
 

LibCarla/source/carla/client/detail/Client.cpp

@@ -528,6 +528,10 @@ namespace detail {
         BaseJSONPath);
   }
 
+  void Client::RestorePhysXPhysics(rpc::ActorId vehicle) {
+    _pimpl->AsyncCall("restore_physx_physics", vehicle);
+  }
+
   void Client::ApplyControlToWalker(rpc::ActorId walker, const rpc::WalkerControl &control) {
     _pimpl->AsyncCall("apply_control_to_walker", walker, control);
   }

LibCarla/source/carla/client/detail/Client.h

@@ -331,6 +331,8 @@ namespace detail {
         std::string TireJSON,
         std::string BaseJSONPath);
 
+    void RestorePhysXPhysics(rpc::ActorId vehicle);
+
     void ApplyControlToWalker(
         rpc::ActorId walker,
         const rpc::WalkerControl &control);

LibCarla/source/carla/client/detail/Simulator.h

@@ -617,6 +617,10 @@ namespace detail {
           BaseJSONPath);
     }
 
+    void RestorePhysXPhysics(Vehicle &vehicle) {
+      _client.RestorePhysXPhysics(vehicle.GetId());
+    }
+
     /// @}
     // =========================================================================
     /// @name Operations with the recorder

PythonAPI/carla/source/libcarla/Actor.cpp

@@ -203,6 +203,7 @@ void export_actor() {
       .def("enable_carsim", &cc::Vehicle::EnableCarSim, (arg("simfile_path") = ""))
       .def("use_carsim_road", &cc::Vehicle::UseCarSimRoad, (arg("enabled")))
       .def("enable_chrono_physics", &cc::Vehicle::EnableChronoPhysics, (arg("max_substeps")=30, arg("max_substep_delta_time")=0.002, arg("vehicle_json")="", arg("powetrain_json")="", arg("tire_json")="", arg("base_json_path")=""))
+      .def("restore_physx_physics", &cc::Vehicle::RestorePhysXPhysics)
       .def("get_failure_state", &cc::Vehicle::GetFailureState)
       .def(self_ns::str(self_ns::self))
   ;

PythonAPI/examples/manual_control_chrono.py

@@ -324,6 +324,7 @@ class KeyboardControl(object):
     def __init__(self, world, start_in_autopilot):
         self._carsim_enabled = False
         self._carsim_road = False
+        self._chrono_enabled = False
         self._autopilot_enabled = start_in_autopilot
         if isinstance(world.player, carla.Vehicle):
             self._control = carla.VehicleControl()
@@ -417,14 +418,24 @@ class KeyboardControl(object):
                     world.camera_manager.set_sensor(current_index)
                 elif event.key == K_k and (pygame.key.get_mods() & KMOD_CTRL):
                     print("k pressed")
+                    if not self._carsim_enabled:
+                        self._carsim_enabled = True
                         world.player.enable_carsim()
+                    else:
+                        self._carsim_enabled = False
+                        world.player.restore_physx_physics()
                 elif event.key == K_o and (pygame.key.get_mods() & KMOD_CTRL):
                     print("o pressed")
+                    if not self._chrono_enabled:
+                        self._chrono_enabled = True
                         vehicle_json = "sedan/vehicle/Sedan_Vehicle.json"
                         powertrain_json = "sedan/powertrain/Sedan_SimpleMapPowertrain.json"
                         tire_json = "sedan/tire/Sedan_TMeasyTire.json"
-                    base_path = "~/carla/Build/chrono-install/share/chrono/data/vehicle/"
+                        base_path = "/home/adas/carla/Build/chrono-install/share/chrono/data/vehicle/"
                         world.player.enable_chrono_physics(5000, 0.002, vehicle_json, powertrain_json, tire_json, base_path)
+                    else:
+                        self._chrono_enabled = False
+                        world.player.restore_physx_physics()
                 elif event.key == K_j and (pygame.key.get_mods() & KMOD_CTRL):
                     self._carsim_road = not self._carsim_road
                     world.player.use_carsim_road(self._carsim_road)

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp

@@ -1055,6 +1055,28 @@ ECarlaServerResponse FVehicleActor::EnableChronoPhysics(
   return ECarlaServerResponse::Success;
 }
 
+ECarlaServerResponse FVehicleActor::RestorePhysXPhysics()
+{
+  if (IsDormant())
+  {
+  }
+  else
+  {
+    auto Vehicle = Cast<ACarlaWheeledVehicle>(GetActor());
+    if (Vehicle == nullptr)
+    {
+      return ECarlaServerResponse::NotAVehicle;
+    }
+    UBaseCarlaMovementComponent* MovementComponent = 
+        Vehicle->GetCarlaMovementComponent<UBaseCarlaMovementComponent>();
+    if(MovementComponent)
+    {
+      MovementComponent->DisableSpecialPhysics();
+    }
+  }
+  return ECarlaServerResponse::Success;
+}
+
 // FSensorActor functions ---------------------
 
 // FtrafficSignActor functions ---------------------

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h

@@ -343,6 +343,11 @@ public:
     return ECarlaServerResponse::ActorTypeMismatch;
   }
 
+  virtual ECarlaServerResponse RestorePhysXPhysics()
+  {
+    return ECarlaServerResponse::ActorTypeMismatch;
+  }
+
   // Traffic light functions
 
   virtual ECarlaServerResponse SetTrafficLightState(const ETrafficLightState&)
@@ -532,6 +537,8 @@ public:
       uint64_t MaxSubsteps, float MaxSubstepDeltaTime,
       const FString& VehicleJSON, const FString& PowertrainJSON,
       const FString& TireJSON, const FString& BaseJSONPath) final;
+
+  virtual ECarlaServerResponse RestorePhysXPhysics();
 };
 
 class FSensorActor : public FCarlaActor

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp

@@ -2319,6 +2319,30 @@ BIND_SYNC(is_sensor_enabled_for_ros) << [this](carla::streaming::detail::stream_
     return R<void>::Success();
   };
 
+  BIND_SYNC(restore_physx_physics) << [this](
+      cr::ActorId ActorId) -> R<void>
+  {
+    REQUIRE_CARLA_EPISODE();
+    FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
+    if (!CarlaActor)
+    {
+      return RespondError(
+          "restore_physx_physics",
+          ECarlaServerResponse::ActorNotFound,
+          " Actor Id: " + FString::FromInt(ActorId));
+    }
+    ECarlaServerResponse Response =
+        CarlaActor->RestorePhysXPhysics();
+    if (Response != ECarlaServerResponse::Success)
+    {
+      return RespondError(
+          "restore_physx_physics",
+          Response,
+          " Actor Id: " + FString::FromInt(ActorId));
+    }
+    return R<void>::Success();
+  };
+
   // ~~ Traffic lights ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
   BIND_SYNC(set_traffic_light_state) << [this](

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/BaseCarlaMovementComponent.h

@@ -35,6 +35,8 @@ public:
 
   virtual float GetVehicleForwardSpeed() const;
 
+  virtual void DisableSpecialPhysics() {};
+
 protected:
 
   void DisableUE4VehiclePhysics();

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/CarSimManagerComponent.cpp

@@ -103,10 +103,7 @@ void UCarSimManagerComponent::ProcessControl(FVehicleControl &Control)
   #endif
 }
 
-void UCarSimManagerComponent::OnCarSimHit(AActor *Actor,
+void UCarSimManagerComponent::DisableCarSimPhysics()
-    AActor *OtherActor,
-    FVector NormalImpulse,
-    const FHitResult &Hit)
 {
   #ifdef WITH_CARSIM
   // finish Carsim simulation
@@ -130,6 +127,19 @@ void UCarSimManagerComponent::OnCarSimHit(AActor *Actor,
   #endif
 }
 
+void UCarSimManagerComponent::DisableSpecialPhysics()
+{
+  DisableCarSimPhysics();
+}
+
+void UCarSimManagerComponent::OnCarSimHit(AActor *Actor,
+    AActor *OtherActor,
+    FVector NormalImpulse,
+    const FHitResult &Hit)
+{
+  DisableCarSimPhysics();
+}
+
 
 void UCarSimManagerComponent::OnCarSimOverlap(UPrimitiveComponent* OverlappedComponent,
     AActor* OtherActor,
@@ -142,26 +152,7 @@ void UCarSimManagerComponent::OnCarSimOverlap(UPrimitiveComponent* OverlappedCom
       ECollisionChannel::ECC_WorldDynamic) ==
       ECollisionResponse::ECR_Block)
   {
-    #ifdef WITH_CARSIM
+    DisableCarSimPhysics();
-    // finish Carsim simulation
-    UDefaultMovementComponent::CreateDefaultMovementComponent(CarlaVehicle);
-    CarlaVehicle->GetMesh()->SetPhysicsLinearVelocity(FVector(0,0,0), false, "Vehicle_Base");
-    CarlaVehicle->GetVehicleMovementComponent()->SetComponentTickEnabled(true);
-    CarlaVehicle->GetVehicleMovementComponent()->Activate();
-    CarlaVehicle->GetMesh()->PhysicsTransformUpdateMode = EPhysicsTransformUpdateMode::SimulationUpatesComponentTransform;
-    auto * Bone = CarlaVehicle->GetMesh()->GetBodyInstance(NAME_None);
-    if (Bone)
-    {
-      Bone->SetInstanceSimulatePhysics(true);
-    }
-    else
-    {
-      carla::log_warning("No bone with name");
-    }
-    CarlaVehicle->GetMesh()->SetCollisionResponseToChannel(ECollisionChannel::ECC_WorldStatic, ECollisionResponse::ECR_Block);
-    CarlaVehicle->GetMesh()->SetCollisionProfileName("Vehicle");    
-    CarlaVehicle->RestoreVehiclePhysicsControl();
-    #endif
   }
 }
 

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/CarSimManagerComponent.h

@@ -52,6 +52,10 @@ public:
 
   float GetVehicleForwardSpeed() const override;
 
+  void DisableCarSimPhysics();
+
+  virtual void DisableSpecialPhysics() override;
+
 private:
 
   // On car mesh hit, only works when carsim is enabled

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/ChronoMovementComponent.cpp

@@ -348,6 +348,11 @@ void UChronoMovementComponent::EndPlay(const EEndPlayReason::Type EndPlayReason)
 }
 #endif
 
+void UChronoMovementComponent::DisableSpecialPhysics()
+{
+  DisableChronoPhysics();
+}
+
 void UChronoMovementComponent::DisableChronoPhysics()
 {
   this->SetComponentTickEnabled(false);
@@ -358,7 +363,6 @@ void UChronoMovementComponent::DisableChronoPhysics()
   CarlaVehicle->GetMesh()->SetCollisionResponseToChannel(
       ECollisionChannel::ECC_WorldStatic, ECollisionResponse::ECR_Block);
   UDefaultMovementComponent::CreateDefaultMovementComponent(CarlaVehicle);
-  carla::log_warning("Chrono physics does not support collisions yet, reverting to default PhysX physics.");
 }
 
 void UChronoMovementComponent::OnVehicleHit(AActor *Actor,
@@ -366,6 +370,7 @@ void UChronoMovementComponent::OnVehicleHit(AActor *Actor,
     FVector NormalImpulse,
     const FHitResult &Hit)
 {
+  carla::log_warning("Chrono physics does not support collisions yet, reverting to default PhysX physics.");
   DisableChronoPhysics();
 }
 
@@ -383,6 +388,7 @@ void UChronoMovementComponent::OnVehicleOverlap(
       ECollisionChannel::ECC_WorldDynamic) ==
       ECollisionResponse::ECR_Block)
   {
+    carla::log_warning("Chrono physics does not support collisions yet, reverting to default PhysX physics.");
     DisableChronoPhysics();
   }
 }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/ChronoMovementComponent.h

@@ -101,6 +101,8 @@ public:
   virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override;
   #endif
 
+  virtual void DisableSpecialPhysics() override;
+
 private:
 
   void DisableChronoPhysics();