Reconnection to existing Webots environment now possible

The listening socket now stays open for clients to connect to. On reconnect, the world can be re-initialized. The exact reconnection behavior can be set in the start message sent by AERA.

controllers/AERA_controller_base/aera_controller.cpp

@@ -9,6 +9,8 @@ AERAController::AERAController()
   receive_queue_ = std::make_shared<tcp_io_device::SafeQueue>(100);
   send_queue_ = std::make_shared<tcp_io_device::SafeQueue>(1);
   tcp_connection_ = std::make_shared<tcp_io_device::TCPConnection>(receive_queue_, send_queue_, 8);
+
+  reconnection_type_ = tcp_io_device::StartMessage_ReconnectionType_RE_INIT;
 }
 
 AERAController::~AERAController()
@@ -60,7 +62,8 @@ std::map<std::string, std::map<std::string, tcp_io_device::MetaData>> AERAContro
 int AERAController::startConnection()
 {
   std::cout << "Establishing TCP connection" << std::endl;
-  int err = tcp_connection_->establishConnection("127.0.0.1", "8080");
+  int err = tcp_connection_->listenAndAwaitConnection("8080");
+  // int err = tcp_connection_->establishConnection("127.0.0.1", "8080");
   if (err != 0) {
     std::cout << "ERROR: Could not establish a connection. Shutting down..." << std::endl;
     return -1;
@@ -151,6 +154,22 @@ void AERAController::handleStartMsg(std::unique_ptr<tcp_io_device::TCPMessage> m
   diagnostic_mode_ = msg->startmessage().diagnosticmode();
 }
 
+void AERAController::handleReconnect() {
+  switch (reconnection_type_)
+  {
+  case tcp_io_device::StartMessage_ReconnectionType_RE_INIT:
+    robot_->simulationReset();
+    init();
+    sendSetupMessage(objects_meta_data_, commands_meta_data_);
+    break;
+  case tcp_io_device::StartMessage_ReconnectionType_RE_SETUP:
+    sendSetupMessage(objects_meta_data_, commands_meta_data_);
+    break;
+  default:
+    break;
+  }
+}
+
 template<typename T>
 tcp_io_device::MsgData AERAController::createMsgData(tcp_io_device::MetaData meta_data, std::vector<T> data) {
   return tcp_io_device::MsgData::createNewMsgData(meta_data, data);

controllers/AERA_controller_base/aera_controller.h

@@ -23,6 +23,8 @@ class AERAController
 
   virtual int start() = 0;
 
+  virtual void init() = 0;
+
   std::map<std::string, std::map<std::string, tcp_io_device::MetaData> > setup(std::string settings_file_name);
 
   int startConnection();
@@ -44,6 +46,8 @@ class AERAController
   std::shared_ptr<tcp_io_device::SafeQueue> send_queue_;
   std::shared_ptr<tcp_io_device::TCPConnection> tcp_connection_;
 
+  tcp_io_device::StartMessage_ReconnectionType reconnection_type_;
+
   bool aera_started_ = false;
   bool diagnostic_mode_ = false;
 
@@ -57,6 +61,8 @@ class AERAController
 
   virtual void run() = 0;
 
+  virtual void handleReconnect();
+
   static std::vector<tcp_io_device::MsgData> dataMsgToMsgData(std::unique_ptr<tcp_io_device::TCPMessage> msg) {
     std::vector<tcp_io_device::MsgData> out_vec;
     auto variables = msg->datamessage().variables();

controllers/hand_grab_sphere_UR_3D_controller/hand_grab_sphere_UR_3D_controller.cpp

@@ -49,17 +49,6 @@ UR3eController::UR3eController() : AERAController() {
 
   tip_camera_ = new TipCamera(robot_->getCamera("tip_camera"));
 
-  for (size_t i = 0; i < NUMBER_OF_ARM_MOTORS; ++i) {
-    arm_motors_[i]->setVelocity(1.0);
-  }
-
-  for (size_t i = 0; i < NUMBER_OF_ARM_MOTORS; ++i) {
-    arm_sensors_[i]->enable(robot_time_step_);
-  }
-  for (size_t i = 0; i < NUMBER_OF_HAND_MOTORS; ++i) {
-    hand_sensors_[i]->enable(robot_time_step_);
-  }
-
   gps_sensor_->enable(robot_time_step_);
 
   inertial_unit_->enable(robot_time_step_);
@@ -146,6 +135,18 @@ int UR3eController::start() {
 void UR3eController::init() {
   robot_->step(robot_time_step_);
   running_time_steps_ += robot_time_step_;
+
+  for (size_t i = 0; i < NUMBER_OF_ARM_MOTORS; ++i) {
+    arm_motors_[i]->setVelocity(1.0);
+  }
+
+  for (size_t i = 0; i < NUMBER_OF_ARM_MOTORS; ++i) {
+    arm_sensors_[i]->enable(robot_time_step_);
+  }
+  for (size_t i = 0; i < NUMBER_OF_HAND_MOTORS; ++i) {
+    hand_sensors_[i]->enable(robot_time_step_);
+  }
+
   for (size_t i = 0; i < NUMBER_OF_ARM_MOTORS; ++i)
   {
     arm_motors_[i]->setPosition(0.0 + motor_offsets_[i]);
@@ -212,15 +213,21 @@ void UR3eController::run() {
       pending_msg = std::move(msg);
     }
     if (pending_msg && (!diagnostic_mode_ || aera_us == receive_deadline)) {
-      if (pending_msg->messagetype() == tcp_io_device::TCPMessage_Type_DATA) {
+      switch (pending_msg->messagetype())
+      {
+      case tcp_io_device::TCPMessage_Type_DATA:
         handleDataMsg(dataMsgToMsgData(std::move(pending_msg)));
         std::cout << "HandleDataMsg called" << std::endl;
         robot_->step(1);
-      }
-      else {
+        break;
+      case tcp_io_device::TCPMessage_Type_RECONNECT:
+        std::cout << "Reconnect occured, resetting environment and sending new setup message" << std::endl;
+        handleReconnect();
+      default:
         std::cout << "Received message with unexpected type "
           << tcp_io_device::TCPConnection::type_to_name_map_[pending_msg->messagetype()]
           << ". Ignoring the message..." << std::endl;
+        break;
       }
       pending_msg = NULL;
     }