diff --git a/README.md b/README.md index 425b29a..3bd507a 100644 --- a/README.md +++ b/README.md @@ -1,44 +1,3 @@ -# v-hsc-ros-bridge +# IMPORTANT -## 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 +This repository moved to https://github.com/ros-sports/hlvs_player! diff --git a/src/ros_bridge/CMakeLists.txt b/src/ros_bridge/CMakeLists.txt deleted file mode 100644 index 6f56878..0000000 --- a/src/ros_bridge/CMakeLists.txt +++ /dev/null @@ -1,62 +0,0 @@ -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( REQUIRED) - -add_executable(webots_controller src/webots_controller.cpp src/robot_client/robot_client.cpp ${ProtoSources} ${ProtoHeaders}) -target_include_directories(webots_controller PUBLIC -$ -$) -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 deleted file mode 100644 index 457e73f..0000000 --- a/src/ros_bridge/include/ros_bridge/robot_client/robot_client.hpp +++ /dev/null @@ -1,110 +0,0 @@ -#include -#include - -#include -#include -#include "messages.pb.h" -#include - -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 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 deleted file mode 100644 index 3f14602..0000000 --- a/src/ros_bridge/misc/messages.proto +++ /dev/null @@ -1,125 +0,0 @@ -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 deleted file mode 100644 index 8f867fb..0000000 --- a/src/ros_bridge/misc/myprotobuf.cmake +++ /dev/null @@ -1,55 +0,0 @@ -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 deleted file mode 100644 index 0360df1..0000000 --- a/src/ros_bridge/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - ros_bridge - 0.0.0 - A simple ROS 2 bridge for RoboCup Humanoid - soheil - Apache License 2.0 - - ament_cmake - - rclcpp - std_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/ros_bridge/resources/devices.json b/src/ros_bridge/resources/devices.json deleted file mode 100644 index 66f8c13..0000000 --- a/src/ros_bridge/resources/devices.json +++ /dev/null @@ -1,122 +0,0 @@ -{ - "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 deleted file mode 100644 index 3bf01e2..0000000 --- a/src/ros_bridge/src/robot_client/robot_client.cpp +++ /dev/null @@ -1,266 +0,0 @@ -#include "ros_bridge/robot_client/robot_client.hpp" - -#ifdef _WIN32 -#include -#define sleep(x) Sleep(x) -#else -#include -#include -#include -#include -#include -#include -#include -#endif - -#include -#include - -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 deleted file mode 100644 index 8b75351..0000000 --- a/src/ros_bridge/src/webots_controller.cpp +++ /dev/null @@ -1,165 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#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("host", "127.0.0.1"); - this->declare_parameter("port", 10001); - - // Publishers - clock_publisher_ = this->create_publisher("clock", 10); - image_publisher_ = this->create_publisher("image", 10); - sensor_publisher_ = this->create_publisher("sensor", 10); - gyro_publisher_ = this->create_publisher("gyro", 10); - - // Subscriptions - motor_command_subscription_ = this->create_subscription( - "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::SharedPtr clock_publisher_; - rclcpp::Publisher::SharedPtr image_publisher_; - rclcpp::Publisher::SharedPtr sensor_publisher_; - rclcpp::Publisher::SharedPtr gyro_publisher_; - rclcpp::Subscription::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()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file