Restarting level

AirSim/AirLib/include/api/RpcLibClientBase.hpp

@@ -28,6 +28,7 @@ class RpcLibClientBase {
 
     void confirmConnection(double timeout);
     void reset();
+    void restart();
 
     ConnectionState getConnectionState();
     bool ping();

AirSim/AirLib/include/api/WorldSimApiBase.hpp

@@ -28,6 +28,8 @@ class WorldSimApiBase {
 
     virtual bool isPaused() const = 0;
     virtual void reset() = 0;
+    virtual void restart() = 0;
+
     virtual void pause(bool is_paused) = 0;
     virtual void continueForTime(double seconds) = 0;
 

AirSim/AirLib/src/api/RpcLibClientBase.cpp

@@ -111,6 +111,11 @@ void RpcLibClientBase::reset()
     pimpl_->client.call("reset");
 }
 
+void RpcLibClientBase::restart()
+{
+    pimpl_->client.call("restart");
+}
+
 void RpcLibClientBase::confirmConnection(double timeout)
 {
     ClockBase* clock = ClockFactory::get();

AirSim/AirLib/src/api/RpcLibServerBase.cpp

@@ -184,6 +184,11 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         resetInProgress = false;
     });
 
+    pimpl_->server.bind("restart", [&]() -> void {
+        auto* sim_world_api = getWorldSimApi();
+        sim_world_api->restart();
+        });
+
     pimpl_->server.bind("simPrintLogMessage", [&](const std::string& message, const std::string& message_param, unsigned char severity) -> void {
         getWorldSimApi()->printLogMessage(message, message_param, severity);
     });

UE4Project/Plugins/AirSim/Source/SimMode/SimModeBase.cpp

@@ -45,6 +45,12 @@ ASimModeBase::ASimModeBase()
     refereeBP_class_ = refereeBP_class.Succeeded() ? refereeBP_class.Class : nullptr;
 }
 
+void ASimModeBase::restart() {
+    APlayerController* player_controller = this->GetWorld()->GetFirstPlayerController();
+    player_controller->RestartLevel();
+}
+
+
 void ASimModeBase::BeginPlay()
 {
     Super::BeginPlay();

UE4Project/Plugins/AirSim/Source/SimMode/SimModeBase.h

@@ -49,6 +49,8 @@ class AIRSIM_API ASimModeBase : public AActor
     virtual void reset();
     virtual ECameraDirectorMode getInitialViewMode() const;
 
+    virtual void restart();
+
     virtual bool isPaused() const;
     virtual void pause(bool is_paused);
     virtual void continueForTime(double seconds);

UE4Project/Plugins/AirSim/Source/WorldSimApi.cpp

@@ -23,6 +23,16 @@ void WorldSimApi::reset()
         }, true);
 }
 
+void WorldSimApi::restart()
+{
+    //APlayerController::RestartLevel()
+    //simmode_->EndPlay(EEndPlayReason::Quit);
+    //simmode_->restart();
+    UAirBlueprintLib::RunCommandOnGameThread([this]() {
+        simmode_->restart();
+        }, true);
+}
+
 void WorldSimApi::pause(bool is_paused)
 {
     simmode_->pause(is_paused);

UE4Project/Plugins/AirSim/Source/WorldSimApi.h

@@ -18,6 +18,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase {
 
     virtual bool isPaused() const override;
     virtual void reset() override;
+    virtual void restart();
     virtual void pause(bool is_paused) override;
     virtual void continueForTime(double seconds) override;
 

python/fsds/client.py

@@ -25,6 +25,19 @@ def reset(self):
         """
         self.client.call('reset')
 
+    def restart(self):
+        """
+        Reset the vehicle AND THE CONES to their original starting state
+        """
+        try:
+            self.client.call('restart')
+        except Exception as e:
+            print(e)
+
+        time.sleep(0.1)  # VERY IMPORTANT!
+
+        self.__init__(self.ip, self.port, self.timeout_value)
+
     def ping(self):
         """
         If connection is established then this call will return true otherwise it will be blocked until timeout

ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h

@@ -49,6 +49,7 @@ STRICT_MODE_OFF //todo what does this do?
 #include <fstream>
 #include <curl/curl.h>
 #include <ros/transport_hints.h>
+#include <std_srvs/Trigger.h>
 // #include "nodelet/nodelet.h"
 #define printVariableNameAndValue(x) std::cout << "The name of variable **" << (#x) << "** and the value of variable is => " << x << "\n"
 
@@ -139,6 +140,7 @@ class AirsimROSWrapper
 
     /// ROS service callbacks
     bool reset_srv_cb(fs_msgs::Reset::Request& request, fs_msgs::Reset::Response& response);
+    bool restart_airsim_service(std_srvs::TriggerRequest& request, std_srvs::TriggerResponse& response);
 
     // methods which parse setting json ang generate ros pubsubsrv
     void create_ros_pubs_from_settings_json();
@@ -209,6 +211,7 @@ class AirsimROSWrapper
 
     std::vector<ros::Publisher> lidar_pub_vec_;
 
+    ros::ServiceServer restart_service_;
 
     /// ROS publishers
     ros::Publisher clock_pub_;

ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp

@@ -137,6 +137,7 @@ void AirsimROSWrapper::initialize_ros()
     if(!competition_mode_) {
         odom_update_timer_ = nh_private_.createTimer(ros::Duration(update_odom_every_n_sec), &AirsimROSWrapper::odom_cb, this);
 		extra_info_timer_ = nh_private_.createTimer(ros::Duration(1), &AirsimROSWrapper::extra_info_cb, this);
+        restart_service_ = nh_private_.advertiseService("restart_level", &AirsimROSWrapper::restart_airsim_service, this);
     }
 
     gps_update_timer_ = nh_private_.createTimer(ros::Duration(update_gps_every_n_sec), &AirsimROSWrapper::gps_timer_cb, this);
@@ -280,6 +281,39 @@ bool AirsimROSWrapper::reset_srv_cb(fs_msgs::Reset::Request& request, fs_msgs::R
     return true; //todo
 }
 
+bool AirsimROSWrapper::restart_airsim_service(std_srvs::TriggerRequest& request, std_srvs::TriggerResponse& response){
+    // Stop all timers
+    odom_update_timer_.stop();
+    gps_update_timer_.stop();
+    imu_update_timer_.stop();
+    gss_update_timer_.stop();
+    statistics_timer_.stop();
+    go_signal_timer_.stop();
+    statictf_timer_.stop();
+	extra_info_timer_.stop();
+    for(auto timer: airsim_lidar_update_timers_){
+        timer.stop();
+    }
+
+    airsim_client_.restart();
+    airsim_client_lidar_.restart();
+
+    initialize_airsim(10);
+
+    odom_update_timer_.start();
+    gps_update_timer_.start();
+    imu_update_timer_.start();
+    gss_update_timer_.start();
+    statistics_timer_.start();
+    go_signal_timer_.start();
+    statictf_timer_.start();
+	extra_info_timer_.start();
+    for(auto & timer: airsim_lidar_update_timers_){
+        timer.start();
+    }
+
+}
+
 tf2::Quaternion AirsimROSWrapper::get_tf2_quat(const msr::airlib::Quaternionr& airlib_quat) const
 {
     return tf2::Quaternion(airlib_quat.x(), airlib_quat.y(), airlib_quat.z(), airlib_quat.w());
@@ -448,6 +482,11 @@ void AirsimROSWrapper::odom_cb(const ros::TimerEvent& event)
         std::string msg = e.get_error().as<std::string>();
         ROS_ERROR_STREAM("Exception raised by the API while getting car state:" << std::endl << msg);
     }
+    catch (rpc::timeout& e){
+        std::cout << "Timeout for simGetImage, probably restarting" << std::endl;
+        std::cout << "Trying to reconnect" << std::endl;
+        airsim_client_.confirmConnection(10.0f);
+    }
 }
 
 void AirsimROSWrapper::gps_timer_cb(const ros::TimerEvent& event)

ros/src/fsds_ros_bridge/src/fsds_ros_bridge_camera.cpp

@@ -29,6 +29,7 @@ std::string camera_name = "";
 double framerate = 0.0;
 std::string host_ip = "localhost";
 bool depthcamera = false;
+double timeout_sec = 10.0;
 
 ros::Time make_ts(uint64_t unreal_ts)
 {
@@ -52,6 +53,10 @@ std::vector<ImageResponse> getImage(ImageRequest req) {
         std::string msg = e.get_error().as<std::string>();
         std::cout << "Exception raised by the API while getting image:" << std::endl
                 << msg << std::endl;
+    } catch (rpc::timeout & e) {
+        std::cout << "Timeout for simGetImage, probably restarting" << std::endl;
+        std::cout << "Trying to reconnect" << std::endl;
+        airsim_api->confirmConnection(timeout_sec);
     }
     return img_responses;
 }
@@ -162,7 +167,6 @@ int main(int argc, char ** argv)
     msr::airlib::CarRpcLibClient client(host_ip, RpcLibPort, 5);
     airsim_api = &client;
 
-    double timeout_sec = 10.0;
     nh.getParam("timeout", timeout_sec);
 
     try {