First version

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.

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

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

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