diff --git a/README.md b/README.md
index b959aea..425b29a 100644
--- a/README.md
+++ b/README.md
@@ -1 +1,44 @@
 # v-hsc-ros-bridge
+
+## Introduction
+This repository provides a ROS package named `ros_bridge` that can be used to communicate with the Webots server while using the Player/Client API.
+
+It requests and receives sensor data (specified in the `src/ros_bridge/resources/devices.json`) over specific topics and subscribes to actuator command messages(only position control available at the moment). Node `webots_controller` is responsible for these operations.
+
+This node is currently available only for ROS 2.
+
+## Usage
+
+Clone the repository:
+
+`git clone https://github.com/robocup-hl-tc/v-hsc-ros-bridge.git`
+
+To access to the ROS2 commands in a bash, source ROS with this command:
+
+`source /opt/ros/$ROS_DISTRO/setup.bash`
+
+(replace $ROS_DISTRO with your ROS distribution, e.g. foxy)
+
+Move to the repository:
+
+`cd v-hsc-ros-bridge`
+
+You can install the dependencies using:
+
+`rosdep install -i --from-path src --rosdistro foxy -y`
+
+build the package with this command:
+
+`colcon build`
+
+In the root of the repository, source your overlay using:
+
+`. install/local_setup.bash`
+
+Run the node using this command:
+
+`ros2 run ros_bridge webots_controller --ros-args -p host:="127.0.0.1" -p port:=10001`
+
+(you can replace host and port with your preferred ones)
+
+Done, now you can access sensor data and publish commands to be performed on the robot.
\ No newline at end of file
diff --git a/src/ros_bridge/CMakeLists.txt b/src/ros_bridge/CMakeLists.txt
new file mode 100644
index 0000000..6f56878
--- /dev/null
+++ b/src/ros_bridge/CMakeLists.txt
@@ -0,0 +1,62 @@
+cmake_minimum_required(VERSION 3.5)
+project(ros_bridge)
+find_package(Protobuf REQUIRED)
+
+include(${CMAKE_CURRENT_SOURCE_DIR}/misc/myprotobuf.cmake)
+file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/misc/*.proto")
+my_protobuf_generate_cpp(generated_files/myproject ProtoSources ProtoHeaders ${ProtoFiles})
+include_directories(${CMAKE_CURRENT_BINARY_DIR}/generated_files/myproject)
+include_directories(${Protobuf_INCLUDE_DIRS})
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosgraph_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(OpenCV REQUIRED)
+include_directories(${OpenCV_INCLUDE_DIRS})
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(JSONCPP jsoncpp)
+include_directories(${JSONCPP_INCLUDE_DIRS})
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+add_executable(webots_controller src/webots_controller.cpp src/robot_client/robot_client.cpp ${ProtoSources} ${ProtoHeaders})
+target_include_directories(webots_controller PUBLIC
+$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+$<INSTALL_INTERFACE:include>)
+ament_target_dependencies(webots_controller rclcpp std_msgs rosgraph_msgs cv_bridge image_transport Protobuf OpenCV JSONCPP)
+
+install(TARGETS webots_controller
+  EXPORT export_${PROJECT_NAME}
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/src/ros_bridge/include/ros_bridge/robot_client/robot_client.hpp b/src/ros_bridge/include/ros_bridge/robot_client/robot_client.hpp
new file mode 100644
index 0000000..457e73f
--- /dev/null
+++ b/src/ros_bridge/include/ros_bridge/robot_client/robot_client.hpp
@@ -0,0 +1,110 @@
+#include <deque>
+#include <memory>
+
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+#include "messages.pb.h"
+#include <opencv2/highgui.hpp>
+
+class RobotClient {
+public:
+  RobotClient(const std::string &host, int port, int verbosity = 3);
+
+  /**
+   * Close socket if opened and free all resources associated to the current connection
+   * Returns true on success.
+   */
+  bool connectClient();
+
+  /**
+   * Close socket if opened and free all resources associated to the current connection
+   */
+  void disconnectClient();
+
+  /**
+   * Send the provided message to the simulator.
+   * On failure, the client is closed and a runtime_error is thrown afterwards.
+   */
+  void sendRequest(const ActuatorRequests &actuator_request);
+
+  /**
+   * Returns next sensor message received, or an empty pointer on failure. This call is blocking.
+   * On failure, the client is closed and a runtime_error is thrown afterwards.
+   */
+  SensorMeasurements receive();
+
+  /**
+   * Returns true if the client is connected and no error has been detected
+   */
+  bool isOk();
+
+  static ActuatorRequests buildRequestMessage(const std::string &path);
+
+private:
+  /**
+   * Host address
+   */
+  std::string host;
+
+  /**
+   * The destination port to establish connection
+   */
+  int port;
+
+  /**
+   * The file descriptor for the socket: -1 if connection is not established
+   */
+  int socket_fd;
+
+  /**
+   * 0: Silent mode, no error messages, even when the connection breaks
+   * 1: Only print error messages
+   * 2: Print messages on successful connection and warnings + errors from the simulator
+   * 3: Print statistics over messages received
+   * 4: Print all messages received
+   */
+  int verbosity;
+
+  struct MessageProperty {
+    uint32_t simulated_time;  // [ms]
+    uint64_t real_time;       // [ms]
+    uint32_t msg_size;        // number of bytes in the message
+  };
+  std::deque<MessageProperty> msg_history;
+
+  uint64_t history_total_size;
+  uint64_t client_start;
+  uint64_t last_history_print;
+
+  /**
+   * The period (in seconds) used to estimated bandwidth and real_time factor.
+   */
+  static float history_period;
+
+  /**
+   * The maximal size of messages that can be received, if the announced size of a message is larger than this, the connection
+   * will be considered as out of sync and the connection will automatically be closed.
+   */
+  static int max_answer_size;
+
+  /**
+   * The number of connection attempts before giving up
+   */
+  static int max_attempts;
+
+  /**
+   * The number of seconds to wait between two connection attempts
+   */
+  static int wait_time_sec;
+
+  /**
+   * Throws logic_error if the connection has not been started.
+   * In case an error occurs during reception, the connection is ended and a runtime_error is thrown
+   */
+  void receiveData(char *buffer, int bytes);
+
+  /**
+   * Update the message history with a message
+   */
+  void updateHistory(const SensorMeasurements &sensors);
+};
diff --git a/src/ros_bridge/misc/messages.proto b/src/ros_bridge/misc/messages.proto
new file mode 100644
index 0000000..3f14602
--- /dev/null
+++ b/src/ros_bridge/misc/messages.proto
@@ -0,0 +1,125 @@
+syntax = "proto3";
+
+message Vector3 {
+  double X = 1;
+  double Y = 2;
+  double Z = 3;
+}
+
+message PositionSensorMeasurement {
+  string name = 1;
+  double value = 2;
+}
+
+message AccelerometerMeasurement {
+  string name = 1;
+  Vector3 value = 2;  // [m/sˆ2], x-axis, y-axis, z-axis
+}
+
+message GyroMeasurement {
+  string name = 1;
+  Vector3 value = 2;  // [rad/s], x-axis, y-axis, z-axis
+}
+
+message BumperMeasurement {
+  string name = 1;
+  bool value = 2;
+}
+
+message ForceMeasurement {
+  string name = 1;
+  double value = 2;  // [N]
+}
+
+message Force3DMeasurement {
+  string name = 1;
+  Vector3 value = 2;  // [N], x-axis, y-axis, z-axis
+}
+
+message Force6DMeasurement {  // not yet implemented
+  string name = 1;
+  Vector3 force = 2;   // [N], x-axis, y-axis, z-axis
+  Vector3 torque = 3;  // [N.m], x-axis, y-axis, z-axis
+}
+
+message CameraMeasurement {
+  string name = 1;
+  uint32 width = 2;
+  uint32 height = 3;
+  sint32 quality = 4;  // -1 = raw image, 100 = no compression, 0 = high compression
+  bytes image = 5;     // RAW or JPEG encoded data (note: JPEG is not yet implemented)
+}
+
+message Message {
+  enum MessageType {
+    ERROR_MESSAGE = 0;
+    WARNING_MESSAGE = 1;
+  }
+  MessageType message_type = 1;
+  string text = 2;
+}
+
+message SensorMeasurements {
+  uint32 time = 1;  // time stamp at which the measurements were performed expressed in [ms]
+  uint64 real_time = 2; // real unix time stamp at which the measurements were performed in [ms]
+  repeated Message messages = 3;
+  repeated AccelerometerMeasurement accelerometers = 4;
+  repeated BumperMeasurement bumpers = 5;
+  repeated CameraMeasurement cameras = 6;
+  repeated ForceMeasurement forces = 7;
+  repeated Force3DMeasurement force3ds = 8;
+  repeated Force6DMeasurement force6ds = 9;
+  repeated GyroMeasurement gyros = 10;
+  repeated PositionSensorMeasurement position_sensors = 11;
+}
+
+message MotorPosition {
+  string name = 1;
+  double position = 2;  // linear or angular target position expressed in [m] or [rad]
+}
+
+message MotorVelocity {
+  string name = 1;
+  double velocity = 2;  // linear or angular target velocity expressed in [m/s] or [rad/s]
+}
+
+message MotorForce {
+  string name = 1;
+  double force = 2;  // target force expressed in [N]
+}
+
+message MotorTorque {
+  string name = 1;
+  double torque = 2;  // target torque expressed in [N.m]
+}
+
+message MotorPID {
+  string name = 1;
+  Vector3 PID = 2;  // PID controller values
+}
+
+message SensorTimeStep { // apply to all sensors: Accelerometer, Camera, Gyro, PositionSensor and TouchSensor
+  string name = 1;
+  uint32 timeStep = 2;  // time between two measurements expressed in [ms], disabled if 0
+}
+
+message CameraQuality {
+  string name = 1;
+  sint32 quality = 2;  // JPEG quality
+}
+
+message CameraExposure {
+  string name = 1;
+  double exposure = 2;  // photometric exposure of the scene in joule per square meter [J/m^2]
+}
+
+message ActuatorRequests {
+  repeated MotorPosition motor_positions = 1;
+  repeated MotorVelocity motor_velocities = 2;
+  repeated MotorForce motor_forces = 3;
+  repeated MotorTorque motor_torques = 4;
+  repeated MotorPID motor_pids = 5;
+  repeated SensorTimeStep sensor_time_steps = 6;
+  repeated CameraQuality camera_qualities = 7;
+  repeated CameraExposure camera_exposures = 8;
+}
diff --git a/src/ros_bridge/misc/myprotobuf.cmake b/src/ros_bridge/misc/myprotobuf.cmake
new file mode 100644
index 0000000..8f867fb
--- /dev/null
+++ b/src/ros_bridge/misc/myprotobuf.cmake
@@ -0,0 +1,55 @@
+function(MY_PROTOBUF_GENERATE_CPP PATH SRCS HDRS)
+  if(NOT ARGN)
+    message(SEND_ERROR "Error: PROTOBUF_GENERATE_CPP() called without any proto files")
+    return()
+  endif()
+
+  if(PROTOBUF_GENERATE_CPP_APPEND_PATH)
+    # Create an include path for each file specified
+    foreach(FIL ${ARGN})
+      get_filename_component(ABS_FIL ${FIL} ABSOLUTE)
+      get_filename_component(ABS_PATH ${ABS_FIL} PATH)
+      list(FIND _protobuf_include_path ${ABS_PATH} _contains_already)
+      if(${_contains_already} EQUAL -1)
+          list(APPEND _protobuf_include_path -I ${ABS_PATH})
+      endif()
+    endforeach()
+  else()
+    set(_protobuf_include_path -I ${CMAKE_CURRENT_SOURCE_DIR})
+  endif()
+
+  if(DEFINED PROTOBUF_IMPORT_DIRS)
+    foreach(DIR ${PROTOBUF_IMPORT_DIRS})
+      get_filename_component(ABS_PATH ${DIR} ABSOLUTE)
+      list(FIND _protobuf_include_path ${ABS_PATH} _contains_already)
+      if(${_contains_already} EQUAL -1)
+          list(APPEND _protobuf_include_path -I ${ABS_PATH})
+      endif()
+    endforeach()
+  endif()
+
+  set(${SRCS})
+  set(${HDRS})
+  foreach(FIL ${ARGN})
+    get_filename_component(ABS_FIL ${FIL} ABSOLUTE)
+    get_filename_component(FIL_WE ${FIL} NAME_WE)
+
+    list(APPEND ${SRCS} "${CMAKE_CURRENT_BINARY_DIR}/${PATH}/${FIL_WE}.pb.cc")
+    list(APPEND ${HDRS} "${CMAKE_CURRENT_BINARY_DIR}/${PATH}/${FIL_WE}.pb.h")
+
+    execute_process(COMMAND ${CMAKE_COMMAND} -E make_directory ${CMAKE_CURRENT_BINARY_DIR}/${PATH})
+
+    add_custom_command(
+      OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/${PATH}/${FIL_WE}.pb.cc"
+             "${CMAKE_CURRENT_BINARY_DIR}/${PATH}/${FIL_WE}.pb.h"
+      COMMAND  ${PROTOBUF_PROTOC_EXECUTABLE}
+      ARGS --cpp_out  ${CMAKE_CURRENT_BINARY_DIR}/${PATH} ${_protobuf_include_path} ${ABS_FIL}
+      DEPENDS ${ABS_FIL}
+      COMMENT "Running C++ protocol buffer compiler on ${FIL}"
+      VERBATIM )
+  endforeach()
+
+  set_source_files_properties(${${SRCS}} ${${HDRS}} PROPERTIES GENERATED TRUE)
+  set(${SRCS} ${${SRCS}} PARENT_SCOPE)
+  set(${HDRS} ${${HDRS}} PARENT_SCOPE)
+endfunction()
\ No newline at end of file
diff --git a/src/ros_bridge/package.xml b/src/ros_bridge/package.xml
new file mode 100644
index 0000000..0360df1
--- /dev/null
+++ b/src/ros_bridge/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ros_bridge</name>
+  <version>0.0.0</version>
+  <description>A simple ROS 2 bridge for RoboCup Humanoid</description>
+  <maintainer email="email@email.com">soheil</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/ros_bridge/resources/devices.json b/src/ros_bridge/resources/devices.json
new file mode 100644
index 0000000..66f8c13
--- /dev/null
+++ b/src/ros_bridge/resources/devices.json
@@ -0,0 +1,122 @@
+{
+    "joints" : [
+        {
+            "motor_name" : "Neck",
+            "sensor_name" : "NeckS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "Head",
+            "sensor_name" : "HeadS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "ShoulderL",
+            "sensor_name" : "ShoulderLS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "ArmUpperL",
+            "sensor_name" : "ArmUpperLS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "ArmLowerL",
+            "sensor_name" : "ArmLowerLS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "PelvYL",
+            "sensor_name" : "PelvYLS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "PelvL",
+            "sensor_name" : "PelvLS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "LegUpperL",
+            "sensor_name" : "LegUpperLS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "LegLowerL",
+            "sensor_name" : "LegLowerLS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "AnkleL",
+            "sensor_name" : "AnkleLS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "FootL",
+            "sensor_name" : "FootLS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "PelvYR",
+            "sensor_name" : "PelvYRS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "PelvR",
+            "sensor_name" : "PelvRS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "LegUpperR",
+            "sensor_name" : "LegUpperRS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "LegLowerR",
+            "sensor_name" : "LegLowerRS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "AnkleR",
+            "sensor_name" : "AnkleRS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "FootR",
+            "sensor_name" : "FootRS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "ShoulderR",
+            "sensor_name" : "ShoulderRS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "ArmUpperR",
+            "sensor_name" : "ArmUpperRS",
+            "time_step" : 8
+        },
+        {
+            "motor_name" : "ArmLowerR",
+            "sensor_name" : "ArmLowerRS",
+            "time_step" : 8
+        }
+    ],
+    "cameras" : [
+        {
+            "name" : "Camera",
+            "time_step" : 16
+        }
+    ],
+    "gyros" : [
+        {
+            "name" : "Gyro",
+            "time_step" : 8
+        }
+    ],
+    "accelerometers" : [
+        {
+            "name" : "Accelerometer",
+            "time_step" : 8
+        }
+    ]
+}
diff --git a/src/ros_bridge/src/robot_client/robot_client.cpp b/src/ros_bridge/src/robot_client/robot_client.cpp
new file mode 100644
index 0000000..3bf01e2
--- /dev/null
+++ b/src/ros_bridge/src/robot_client/robot_client.cpp
@@ -0,0 +1,266 @@
+#include "ros_bridge/robot_client/robot_client.hpp"
+
+#ifdef _WIN32
+#include <winsock.h>
+#define sleep(x) Sleep(x)
+#else
+#include <arpa/inet.h>
+#include <netdb.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
+#endif
+
+#include <cmath>
+#include <stdexcept>
+
+float RobotClient::history_period = 5;
+int RobotClient::max_answer_size = 1920 * 1080 * 3 + 1000;  // Adding some margin for other data than image
+int RobotClient::max_attempts = 20;
+int RobotClient::wait_time_sec = 1;
+
+static void close_socket(int fd) {
+#ifdef _WIN32
+  closesocket(fd);
+#else
+  close(fd);
+#endif
+}
+
+static char *read_file(const char *filename) {
+  char *buffer = NULL;
+  FILE *fp = fopen(filename, "r");
+  if (!fp)
+    return NULL;
+  if (fseek(fp, 0L, SEEK_END) == 0) {
+    const long size = ftell(fp);
+    assert(size != -1);
+    buffer = (char *)malloc(sizeof(char) * (size + 1));
+    fseek(fp, 0L, SEEK_SET);
+    const size_t len = fread(buffer, sizeof(char), size, fp);
+    buffer[len] = '\0';
+  }
+  fclose(fp);
+  return buffer;
+}
+
+void printMessages(const SensorMeasurements &sensor_measurements) {
+  for (int i = 0; i < sensor_measurements.messages_size(); i++) {
+    const Message &msg = sensor_measurements.messages(i);
+    std::string prefix = Message_MessageType_Name(msg.message_type());
+    printf("%s: %s\n", prefix.c_str(), msg.text().c_str());
+  }
+}
+
+RobotClient::RobotClient(const std::string &host, int port, int verbosity) :
+  host(host),
+  port(port),
+  socket_fd(-1),
+  verbosity(verbosity),
+  history_total_size(0),
+  client_start(0),
+  last_history_print(0) {
+}
+
+bool RobotClient::connectClient() {
+  int return_code;
+  struct hostent *server = gethostbyname(host.c_str());
+  struct sockaddr_in address;
+  memset(&address, 0, sizeof(struct sockaddr_in));
+  address.sin_family = AF_INET;
+  address.sin_port = htons(port);
+  if (server)
+    memcpy((char *)&address.sin_addr.s_addr, (char *)server->h_addr, server->h_length);
+  else {
+    fprintf(stderr, "Cannot resolve server name: %s\n", host.c_str());
+    return false;
+  }
+#ifdef _WIN32
+  WSADATA info;
+  return_code = WSAStartup(MAKEWORD(2, 2), &info);  // Winsock 2.2
+  if (return_code != 0) {
+    if (verbosity > 0)
+      fprintf(stderr, "Cannot initialize Winsock\n");
+    return false;
+  }
+#endif
+  socket_fd = socket(AF_INET, SOCK_STREAM, 0);
+  if (socket_fd == -1) {
+    if (verbosity > 0)
+      fprintf(stderr, "Cannot create socket\n");
+    return false;
+  }
+  int attempt = 1;
+  bool connected = false;
+  while (attempt <= max_attempts) {
+    return_code = connect(socket_fd, (struct sockaddr *)&address, sizeof(struct sockaddr));
+    if (return_code == 0) {
+      connected = true;
+      break;
+    }
+    fprintf(stderr, "Failed to connect to %s:%d (attempt %d / %d)\n", host.c_str(), port, attempt, max_attempts);
+    attempt++;
+    sleep(wait_time_sec);
+  }
+  if (!connected) {
+    if (verbosity > 0) {
+      fprintf(stderr, "Failed to connect after %d attempts. Giving up on connection\n", attempt);
+    }
+    disconnectClient();
+    return false;
+  }
+  // Receiving the 'welcome message'
+  char answer[8];
+  receiveData(answer, 8);
+  if (verbosity >= 4)
+    printf("Welcome message: %s\n", answer);
+  if (strncmp(answer, "Welcome", 8) != 0) {
+    if (verbosity > 0) {
+      if (strncmp(answer, "Refused", 8) == 0)
+        fprintf(stderr, "Connection to %s:%d refused: your IP address is not allowed in the game.json configuration file.\n",
+                host.c_str(), port);
+      else
+        fprintf(stderr, "Received unknown answer from server: %s\n", answer);
+    }
+    disconnectClient();
+    return false;
+  }
+  if (verbosity >= 2)
+    printf("Connected to %s:%d\n", host.c_str(), port);
+  return true;
+}
+
+void RobotClient::disconnectClient() {
+  if (socket_fd == -1 && verbosity > 0) {
+    fprintf(stderr, "RobotClient is already disconnected\n");
+    return;
+  }
+  close_socket(socket_fd);
+  socket_fd = -1;
+}
+
+void RobotClient::sendRequest(const ActuatorRequests &actuator_request) {
+  if (socket_fd == -1)
+    throw std::logic_error("RobotClient is not connected");
+  const uint32_t size = htonl(actuator_request.ByteSizeLong());
+#ifndef _WIN32
+  // This doesn't work on Windows, we should implement SocketOutputStream to make it work efficiently on Windows
+  // See https://stackoverflow.com/questions/23280457/c-google-protocol-buffers-open-http-socket
+  if (send(socket_fd, (char *)(&size), sizeof(uint32_t), 0) == -1) {
+    std::string error_msg = "Failed to send message of size: " + std::to_string(size) + " errno: " + std::to_string(errno);
+    if (errno == ECONNRESET)
+      error_msg = "Simulator interrupted the connection";
+    disconnectClient();
+    throw std::runtime_error(error_msg);
+  }
+  google::protobuf::io::ZeroCopyOutputStream *zeroCopyStream = new google::protobuf::io::FileOutputStream(socket_fd);
+  actuator_request.SerializeToZeroCopyStream(zeroCopyStream);
+  delete zeroCopyStream;
+#else  // here we make a useless malloc, copy and free
+  char *output = (char *)malloc(sizeof(int) + size);
+  uint32_t *content_size = (uint32_t *)output;
+  *content_size = size;
+  uint32_t total_size = sizeof(uint32_t) + *content_size;
+  actuator_request.SerializeToArray(&output[sizeof(uint32_t)], *content_size);
+  if (send(socket_fd, output, total_size, 0) == -1) {
+    std::string error_msg =
+      "Failed to send message of size: " + std::to_string(total_size) + " errno: " + std::to_string(errno);
+    free(output);
+    disconnectClient();
+    throw std::runtime_error(error_msg);
+  }
+  free(output);
+#endif
+}
+
+SensorMeasurements RobotClient::receive() {
+  uint32_t content_size_network;
+  receiveData((char *)&content_size_network, sizeof(uint32_t));
+  const int answer_size = ntohl(content_size_network);
+  if (answer_size > max_answer_size || answer_size == 0) {
+    disconnectClient();
+    throw std::logic_error("Unexpected size for the answer: " + std::to_string(answer_size) + " (probably out of sync)");
+  }
+  SensorMeasurements sensor_measurements;
+  char *buffer = (char *)malloc(answer_size);
+  receiveData(buffer, answer_size);
+  sensor_measurements.ParseFromArray(buffer, answer_size);
+  free(buffer);
+  // History is only updated when printing it
+  if (verbosity >= 2)
+    printMessages(sensor_measurements);
+  if (verbosity >= 3)
+    updateHistory(sensor_measurements);
+  if (verbosity >= 4) {
+    std::string printout;
+    google::protobuf::TextFormat::PrintToString(sensor_measurements, &printout);
+    std::cout << printout << std::endl;
+  }
+  return sensor_measurements;
+}
+
+bool RobotClient::isOk() {
+  return socket_fd != -1;
+}
+
+ActuatorRequests RobotClient::buildRequestMessage(const std::string &path) {
+  const char *message = read_file(path.c_str());
+  if (message == nullptr)
+    throw std::runtime_error("File '" + path + "' does not exist");
+  ActuatorRequests actuator_request;
+  google::protobuf::TextFormat::ParseFromString(message, &actuator_request);
+  return actuator_request;
+}
+
+void RobotClient::receiveData(char *buffer, int bytes) {
+  if (socket_fd == -1)
+    throw std::logic_error("RobotClient is not connected");
+  int received = 0;
+  while (received < bytes) {
+    int n = recv(socket_fd, buffer + received, bytes - received, 0);
+    if (n == -1) {
+      std::string error_msg = "Failed to send message of size: " + std::to_string(bytes) + " errno: " + std::to_string(errno);
+      if (errno == ECONNRESET)
+        error_msg = "Simulator interrupted the connection";
+      disconnectClient();
+      throw std::runtime_error(error_msg);
+    }
+    received += n;
+  }
+}
+
+void RobotClient::updateHistory(const SensorMeasurements &sensors) {
+  uint32_t msg_size = sensors.ByteSizeLong();
+  MessageProperty new_msg;
+  new_msg.simulated_time = sensors.time();
+  new_msg.real_time = sensors.real_time();
+  new_msg.msg_size = msg_size;
+  history_total_size += msg_size;
+  if (client_start == 0) {
+    client_start = new_msg.real_time;
+    last_history_print = new_msg.real_time;
+  }
+  // Adding new message + removing old ones
+  msg_history.push_front(new_msg);
+  while (msg_history.front().real_time - msg_history.back().real_time > history_period * 1000) {
+    history_total_size -= msg_history.back().msg_size;
+    msg_history.pop_back();
+  }
+  // Printing at fixed frequency
+  if (new_msg.real_time - last_history_print > history_period * 1000) {
+    if (msg_history.size() == 1) {
+      printf("Cannot compute stats on 1 message\n");
+    } else {
+      const MessageProperty &old_msg = msg_history.back();
+      double real_time_elapsed = new_msg.real_time - old_msg.real_time;
+      double simulated_time_elapsed = new_msg.simulated_time - old_msg.simulated_time;
+      double real_time_factor = simulated_time_elapsed / real_time_elapsed;
+      double bandwidth = history_total_size / (real_time_elapsed / 1000) / std::pow(2, 20);
+      printf("[%08.3f|%08.3f] real_time_factor: %f, bandwidth: %f MB/s\n", (new_msg.real_time - client_start) / 1000.0,
+             new_msg.simulated_time / 1000.0, real_time_factor, bandwidth);
+    }
+    last_history_print = new_msg.real_time;
+  }
+}
diff --git a/src/ros_bridge/src/webots_controller.cpp b/src/ros_bridge/src/webots_controller.cpp
new file mode 100644
index 0000000..8b75351
--- /dev/null
+++ b/src/ros_bridge/src/webots_controller.cpp
@@ -0,0 +1,165 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <json/value.h>
+#include <jsoncpp/json/json.h>
+#include <fstream>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/time.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <rosgraph_msgs/msg/clock.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+#include "ros_bridge/robot_client/robot_client.hpp"
+
+using std::placeholders::_1;
+using namespace std::chrono_literals;
+
+class WebotsController : public rclcpp::Node {
+public:
+  WebotsController()
+      : Node("webots_controller") {
+
+    // Parameters
+    this->declare_parameter<std::string>("host", "127.0.0.1");
+    this->declare_parameter<int>("port", 10001);
+
+    // Publishers
+    clock_publisher_ = this->create_publisher<rosgraph_msgs::msg::Clock>("clock", 10);
+    image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>("image", 10);
+    sensor_publisher_ = this->create_publisher<sensor_msgs::msg::JointState>("sensor", 10);
+    gyro_publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("gyro", 10);
+
+    // Subscriptions
+    motor_command_subscription_ = this->create_subscription<sensor_msgs::msg::JointState>(
+        "command", 10, std::bind(&WebotsController::command_callback, this, _1));
+
+    // Timer and its callback
+    timer_ = this->create_wall_timer(
+        8ms, std::bind(&WebotsController::timer_callback, this));
+
+    // Client construction and connecting
+    this->get_parameter("host", host_);
+    this->get_parameter("port", port_);
+    client = new RobotClient(host_, port_, 3);
+    client->connectClient();
+
+    // Enable devices
+    ActuatorRequests request;
+    Json::Value devices;
+    std::ifstream json_file("src/ros_bridge/resources/devices.json");
+    json_file >> devices;
+
+    // Cameras
+    for (unsigned int i = 0; i < devices["cameras"].size(); i++) {
+      SensorTimeStep *camera_sensor = request.add_sensor_time_steps();
+      camera_sensor->set_name(devices["cameras"][i]["name"].asString());
+      camera_sensor->set_timestep(devices["cameras"][i]["time_step"].asDouble());
+    }
+
+    // Position sensors
+    for (unsigned int i = 0; i < devices["joints"].size(); i++) {
+      SensorTimeStep *sensor = request.add_sensor_time_steps();
+      sensor->set_name(devices["joints"][i]["sensor_name"].asString());
+      sensor->set_timestep(devices["joints"][i]["time_step"].asDouble());
+    }
+
+    // Gyros
+    for (unsigned int i = 0; i < devices["gyros"].size(); i++) {
+      SensorTimeStep *sensor = request.add_sensor_time_steps();
+      sensor->set_name(devices["gyros"][i]["name"].asString());
+      sensor->set_timestep(devices["gyros"][i]["time_step"].asDouble());
+    }
+
+    // Accelerometers
+    for (unsigned int i = 0; i < devices["accelerometers"].size(); i++) {
+      SensorTimeStep *sensor = request.add_sensor_time_steps();
+      sensor->set_name(devices["accelerometers"][i]["name"].asString());
+      sensor->set_timestep(devices["accelerometers"][i]["time_step"].asDouble());
+    }
+
+    client->sendRequest(request);
+    SensorMeasurements sensors = client->receive();
+  }
+
+private:
+  void timer_callback() {
+    if (client->isOk()) {
+      try {
+        ActuatorRequests request;
+        client->sendRequest(request);
+        SensorMeasurements sensors = client->receive();
+        auto clk = rosgraph_msgs::msg::Clock();
+        clk.clock = rclcpp::Time(sensors.time());
+        clock_publisher_->publish(clk);
+        publishImage(sensors);
+        publishSensors(sensors);
+      }
+      catch (const std::runtime_error &exc) {
+        std::cerr << "Runtime error: " << exc.what() << std::endl;
+      }
+    }
+  }
+
+  void publishImage(const SensorMeasurements &sensors) {
+    for (int i = 0; i < sensors.cameras_size(); i++) {
+      const CameraMeasurement &sensor_data = sensors.cameras(i);
+      if (sensor_data.quality() != -1) {
+        throw std::runtime_error("Encoded images are not supported in this client");
+      }
+
+      cv::Mat img(sensor_data.height(), sensor_data.width(), CV_8UC3, (void *)sensor_data.image().c_str());
+
+      auto imgmsg = sensor_msgs::msg::Image();
+      cv_bridge::CvImage img_bridge;
+      img_bridge = cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::BGR8, img);
+      img_bridge.toImageMsg(imgmsg);
+      image_publisher_->publish(imgmsg);
+    }
+  }
+
+  void publishSensors(const SensorMeasurements &sensors) {
+    auto jointmsg = sensor_msgs::msg::JointState();
+    for (int i = 0; i < sensors.position_sensors_size(); i++) {
+      jointmsg.name.push_back(sensors.position_sensors(i).name());
+      jointmsg.position.push_back(sensors.position_sensors(i).value());
+    }
+    sensor_publisher_->publish(jointmsg);
+  }
+
+  void command_callback(const sensor_msgs::msg::JointState::SharedPtr msg) const {
+
+    ActuatorRequests request;
+
+    for (unsigned int i = 0; i < msg->name.size(); i++) {
+      MotorPosition *sensor = request.add_motor_positions();
+      sensor->set_name(msg->name[i]);
+      sensor->set_position(msg->position[i]);
+    }
+    client->sendRequest(request);
+    SensorMeasurements sensors = client->receive();
+  }
+
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr sensor_publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr gyro_publisher_;
+  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr motor_command_subscription_;
+  RobotClient *client;
+  std::string host_;
+  int port_;
+};
+
+int main(int argc, char *argv[]) {
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<WebotsController>());
+  rclcpp::shutdown();
+  return 0;
+}
\ No newline at end of file