From 0c795cea3cd41027c8c75fc84d73793450dd8fab Mon Sep 17 00:00:00 2001 From: Soheil Date: Fri, 21 May 2021 03:53:35 +0430 Subject: [PATCH 01/14] [FEATURE] Add simple node that has a client and connects to the player controller. --- src/ros_bridge/CMakeLists.txt | 55 ++++ .../ros_bridge/robot_client/robot_client.hpp | 106 +++++++ src/ros_bridge/misc/messages.proto | 125 ++++++++ src/ros_bridge/misc/myprotobuf.cmake | 55 ++++ src/ros_bridge/package.xml | 21 ++ .../src/robot_client/robot_client.cpp | 268 ++++++++++++++++++ src/ros_bridge/src/webots_controller.cpp | 44 +++ 7 files changed, 674 insertions(+) create mode 100644 src/ros_bridge/CMakeLists.txt create mode 100644 src/ros_bridge/include/ros_bridge/robot_client/robot_client.hpp create mode 100644 src/ros_bridge/misc/messages.proto create mode 100644 src/ros_bridge/misc/myprotobuf.cmake create mode 100644 src/ros_bridge/package.xml create mode 100644 src/ros_bridge/src/robot_client/robot_client.cpp create mode 100644 src/ros_bridge/src/webots_controller.cpp diff --git a/src/ros_bridge/CMakeLists.txt b/src/ros_bridge/CMakeLists.txt new file mode 100644 index 0000000..4b4051f --- /dev/null +++ b/src/ros_bridge/CMakeLists.txt @@ -0,0 +1,55 @@ +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) +# 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 Protobuf) + +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..7632715 --- /dev/null +++ b/src/ros_bridge/include/ros_bridge/robot_client/robot_client.hpp @@ -0,0 +1,106 @@ +#include +#include +#include "messages.pb.h" + +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 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 @@ + + + + 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/src/robot_client/robot_client.cpp b/src/ros_bridge/src/robot_client/robot_client.cpp new file mode 100644 index 0000000..3ac1bb5 --- /dev/null +++ b/src/ros_bridge/src/robot_client/robot_client.cpp @@ -0,0 +1,268 @@ +#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 +#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 new file mode 100644 index 0000000..136542f --- /dev/null +++ b/src/ros_bridge/src/webots_controller.cpp @@ -0,0 +1,44 @@ +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include "rosgraph_msgs/msg/clock.hpp" + +#include "ros_bridge/robot_client/robot_client.hpp" + +using namespace std::chrono_literals; + +class WebotsController : public rclcpp::Node +{ + public: + WebotsController() + : Node("webots_controller") + { + publisher_ = this->create_publisher("topic", 10); + clock_publisher_ = this->create_publisher("clock", 10); + timer_ = this->create_wall_timer( + 0ms, std::bind(&WebotsController::timer_callback, this)); + client = new RobotClient(0, 0, 0); + } + + private: + void timer_callback() + { + } + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr clock_publisher_; + RobotClient* client; +}; + +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 From e6ed4097e325a20848df55f6846365156004469e Mon Sep 17 00:00:00 2001 From: Soheil Date: Mon, 24 May 2021 08:52:44 +0430 Subject: [PATCH 02/14] [#REFACTOR] Connect to a static robot for test. --- src/ros_bridge/src/webots_controller.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/ros_bridge/src/webots_controller.cpp b/src/ros_bridge/src/webots_controller.cpp index 136542f..d904af8 100644 --- a/src/ros_bridge/src/webots_controller.cpp +++ b/src/ros_bridge/src/webots_controller.cpp @@ -15,13 +15,13 @@ class WebotsController : public rclcpp::Node { public: WebotsController() - : Node("webots_controller") - { + : Node("webots_controller") { publisher_ = this->create_publisher("topic", 10); clock_publisher_ = this->create_publisher("clock", 10); timer_ = this->create_wall_timer( - 0ms, std::bind(&WebotsController::timer_callback, this)); - client = new RobotClient(0, 0, 0); + 8ms, std::bind(&WebotsController::timer_callback, this)); + client = new RobotClient("127.0.0.1", 10001, 3); + client->connectClient(); } private: @@ -34,8 +34,7 @@ class WebotsController : public rclcpp::Node RobotClient* client; }; -int main(int argc, char * argv[]) -{ +int main(int argc, char * argv[]) { GOOGLE_PROTOBUF_VERIFY_VERSION; rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); From 43e8c9eb54247c4ea7f0cba6782a2d70421679b0 Mon Sep 17 00:00:00 2001 From: Soheil Date: Mon, 24 May 2021 10:09:47 +0430 Subject: [PATCH 03/14] [#FEATURE] Request, receive, and publish image. --- src/ros_bridge/CMakeLists.txt | 6 ++- .../ros_bridge/robot_client/robot_client.hpp | 4 ++ .../src/robot_client/robot_client.cpp | 2 - src/ros_bridge/src/webots_controller.cpp | 40 +++++++++++++++++++ 4 files changed, 49 insertions(+), 3 deletions(-) diff --git a/src/ros_bridge/CMakeLists.txt b/src/ros_bridge/CMakeLists.txt index 4b4051f..7fdb55b 100644 --- a/src/ros_bridge/CMakeLists.txt +++ b/src/ros_bridge/CMakeLists.txt @@ -27,6 +27,10 @@ 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}) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) @@ -35,7 +39,7 @@ add_executable(webots_controller src/webots_controller.cpp src/robot_client/robo target_include_directories(webots_controller PUBLIC $ $) -ament_target_dependencies(webots_controller rclcpp std_msgs rosgraph_msgs Protobuf) +ament_target_dependencies(webots_controller rclcpp std_msgs rosgraph_msgs cv_bridge image_transport Protobuf OpenCV) install(TARGETS webots_controller EXPORT export_${PROJECT_NAME} 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 index 7632715..457e73f 100644 --- a/src/ros_bridge/include/ros_bridge/robot_client/robot_client.hpp +++ b/src/ros_bridge/include/ros_bridge/robot_client/robot_client.hpp @@ -1,6 +1,10 @@ #include #include + +#include +#include #include "messages.pb.h" +#include class RobotClient { public: diff --git a/src/ros_bridge/src/robot_client/robot_client.cpp b/src/ros_bridge/src/robot_client/robot_client.cpp index 3ac1bb5..3bf01e2 100644 --- a/src/ros_bridge/src/robot_client/robot_client.cpp +++ b/src/ros_bridge/src/robot_client/robot_client.cpp @@ -13,8 +13,6 @@ #include #endif -#include -#include #include #include diff --git a/src/ros_bridge/src/webots_controller.cpp b/src/ros_bridge/src/webots_controller.cpp index d904af8..ed3e47e 100644 --- a/src/ros_bridge/src/webots_controller.cpp +++ b/src/ros_bridge/src/webots_controller.cpp @@ -6,6 +6,8 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" #include "rosgraph_msgs/msg/clock.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "cv_bridge/cv_bridge.h" #include "ros_bridge/robot_client/robot_client.hpp" @@ -18,19 +20,57 @@ class WebotsController : public rclcpp::Node : Node("webots_controller") { publisher_ = this->create_publisher("topic", 10); clock_publisher_ = this->create_publisher("clock", 10); + image_publisher_ = this->create_publisher("image", 10); timer_ = this->create_wall_timer( 8ms, std::bind(&WebotsController::timer_callback, this)); client = new RobotClient("127.0.0.1", 10001, 3); client->connectClient(); + + // Enable camera + ActuatorRequests request; + SensorTimeStep *camera_sensor = request.add_sensor_time_steps(); + camera_sensor->set_name("Camera"); + camera_sensor->set_timestep(16); + client->sendRequest(request); + SensorMeasurements sensors = client->receive(); } private: void timer_callback() { + if (client->isOk()) { + try { + ActuatorRequests request; + client->sendRequest(request); + SensorMeasurements sensors = client->receive(); + publishImage(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); + } + } + rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr publisher_; rclcpp::Publisher::SharedPtr clock_publisher_; + rclcpp::Publisher::SharedPtr image_publisher_; RobotClient* client; }; From e8f99dd9bd867553e97a0e6e4eced96de269376c Mon Sep 17 00:00:00 2001 From: Soheil Date: Tue, 25 May 2021 05:11:43 +0430 Subject: [PATCH 04/14] [#FEATURE] Publish motor position sensors data. --- src/ros_bridge/CMakeLists.txt | 5 +- src/ros_bridge/src/motors.json | 82 ++++++++++++++++++++++++ src/ros_bridge/src/webots_controller.cpp | 39 +++++++++-- 3 files changed, 121 insertions(+), 5 deletions(-) create mode 100644 src/ros_bridge/src/motors.json diff --git a/src/ros_bridge/CMakeLists.txt b/src/ros_bridge/CMakeLists.txt index 7fdb55b..6f56878 100644 --- a/src/ros_bridge/CMakeLists.txt +++ b/src/ros_bridge/CMakeLists.txt @@ -31,6 +31,9 @@ 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) @@ -39,7 +42,7 @@ add_executable(webots_controller src/webots_controller.cpp src/robot_client/robo target_include_directories(webots_controller PUBLIC $ $) -ament_target_dependencies(webots_controller rclcpp std_msgs rosgraph_msgs cv_bridge image_transport Protobuf OpenCV) +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} diff --git a/src/ros_bridge/src/motors.json b/src/ros_bridge/src/motors.json new file mode 100644 index 0000000..a8af51b --- /dev/null +++ b/src/ros_bridge/src/motors.json @@ -0,0 +1,82 @@ +[ + { + "name" : "Neck", + "time_step" : 8 + }, + { + "name" : "Head", + "time_step" : 8 + }, + { + "name" : "ShoulderL", + "time_step" : 8 + }, + { + "name" : "ArmUpperL", + "time_step" : 8 + }, + { + "name" : "ArmLowerL", + "time_step" : 8 + }, + { + "name" : "PelvYL", + "time_step" : 8 + }, + { + "name" : "PelvL", + "time_step" : 8 + }, + { + "name" : "LegUpperL", + "time_step" : 8 + }, + { + "name" : "LegLowerL", + "time_step" : 8 + }, + { + "name" : "AnkleL", + "time_step" : 8 + }, + { + "name" : "FootL", + "time_step" : 8 + }, + { + "name" : "PelvYR", + "time_step" : 8 + }, + { + "name" : "PelvR", + "time_step" : 8 + }, + { + "name" : "LegUpperR", + "time_step" : 8 + }, + { + "name" : "LegLowerR", + "time_step" : 8 + }, + { + "name" : "AnkleR", + "time_step" : 8 + }, + { + "name" : "FootR", + "time_step" : 8 + }, + { + "name" : "ShoulderR", + "time_step" : 8 + }, + { + "name" : "ArmUpperR", + "time_step" : 8 + }, + { + "name" : "ArmLowerR", + "time_step" : 8 + } +] diff --git a/src/ros_bridge/src/webots_controller.cpp b/src/ros_bridge/src/webots_controller.cpp index ed3e47e..70deabc 100644 --- a/src/ros_bridge/src/webots_controller.cpp +++ b/src/ros_bridge/src/webots_controller.cpp @@ -7,10 +7,15 @@ #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 "cv_bridge/cv_bridge.h" #include "ros_bridge/robot_client/robot_client.hpp" +#include +#include +#include + using namespace std::chrono_literals; class WebotsController : public rclcpp::Node @@ -18,19 +23,35 @@ class WebotsController : public rclcpp::Node public: WebotsController() : Node("webots_controller") { - publisher_ = this->create_publisher("topic", 10); clock_publisher_ = this->create_publisher("clock", 10); image_publisher_ = this->create_publisher("image", 10); + sensor_publisher_ = this->create_publisher("sensor", 10); + timer_ = this->create_wall_timer( 8ms, std::bind(&WebotsController::timer_callback, this)); client = new RobotClient("127.0.0.1", 10001, 3); client->connectClient(); - // Enable camera ActuatorRequests request; + + // Enable camera SensorTimeStep *camera_sensor = request.add_sensor_time_steps(); camera_sensor->set_name("Camera"); camera_sensor->set_timestep(16); + + Json::Value motors_json; + std::ifstream json_file("src/ros_bridge/src/motors.json"); + json_file >> motors_json; + + // Enable sensors + for (unsigned int i=0; i < motors_json.size(); i++) { + // std::cout<< motors_json[i]["name"].asString() << std::endl; + // std::cout<< motors_json[i]["time_step"].asDouble() << std::endl; + SensorTimeStep *sensor = request.add_sensor_time_steps(); + sensor->set_name(motors_json[i]["name"].asString() + "S"); + sensor->set_timestep(motors_json[i]["time_step"].asDouble()); + } + client->sendRequest(request); SensorMeasurements sensors = client->receive(); } @@ -44,6 +65,7 @@ class WebotsController : public rclcpp::Node client->sendRequest(request); SensorMeasurements sensors = client->receive(); publishImage(sensors); + publishSensors(sensors); } catch (const std::runtime_error &exc) { std::cerr << "Runtime error: " << exc.what() << std::endl; } @@ -56,7 +78,7 @@ class WebotsController : public rclcpp::Node 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(); @@ -67,10 +89,19 @@ class WebotsController : public rclcpp::Node } } + 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); + } + rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; rclcpp::Publisher::SharedPtr clock_publisher_; rclcpp::Publisher::SharedPtr image_publisher_; + rclcpp::Publisher::SharedPtr sensor_publisher_; RobotClient* client; }; From 34ca5f893987fa1f97d147e29cf7026f3ef8f5d1 Mon Sep 17 00:00:00 2001 From: Soheil Date: Tue, 25 May 2021 06:20:54 +0430 Subject: [PATCH 05/14] [#FEATURE] Publish time. --- src/ros_bridge/src/webots_controller.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/ros_bridge/src/webots_controller.cpp b/src/ros_bridge/src/webots_controller.cpp index 70deabc..66cfb6e 100644 --- a/src/ros_bridge/src/webots_controller.cpp +++ b/src/ros_bridge/src/webots_controller.cpp @@ -4,6 +4,7 @@ #include #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" @@ -64,6 +65,10 @@ class WebotsController : public rclcpp::Node ActuatorRequests request; client->sendRequest(request); SensorMeasurements sensors = client->receive(); + auto clk = rosgraph_msgs::msg::Clock(); + clk.clock = rclcpp::Time(sensors.time()); + // std::cout<< clk.clock.seconds() << std::endl; + clock_publisher_->publish(clk); publishImage(sensors); publishSensors(sensors); } catch (const std::runtime_error &exc) { From f87f060585b12639930c5824dac364711f0a2635 Mon Sep 17 00:00:00 2001 From: Soheil Date: Tue, 25 May 2021 08:25:23 +0430 Subject: [PATCH 06/14] [#FEATURE] Subscribing to motor command topic. --- src/ros_bridge/src/webots_controller.cpp | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/ros_bridge/src/webots_controller.cpp b/src/ros_bridge/src/webots_controller.cpp index 66cfb6e..4009f84 100644 --- a/src/ros_bridge/src/webots_controller.cpp +++ b/src/ros_bridge/src/webots_controller.cpp @@ -17,6 +17,7 @@ #include #include +using std::placeholders::_1; using namespace std::chrono_literals; class WebotsController : public rclcpp::Node @@ -28,6 +29,9 @@ class WebotsController : public rclcpp::Node image_publisher_ = this->create_publisher("image", 10); sensor_publisher_ = this->create_publisher("sensor", 10); + motor_command_subscription_ = this->create_subscription( + "command", 10, std::bind(&WebotsController::command_callback, this, _1)); + timer_ = this->create_wall_timer( 8ms, std::bind(&WebotsController::timer_callback, this)); client = new RobotClient("127.0.0.1", 10001, 3); @@ -67,7 +71,7 @@ class WebotsController : public rclcpp::Node SensorMeasurements sensors = client->receive(); auto clk = rosgraph_msgs::msg::Clock(); clk.clock = rclcpp::Time(sensors.time()); - // std::cout<< clk.clock.seconds() << std::endl; + // std::cout<< clk.clock.nanosec << std::endl; clock_publisher_->publish(clk); publishImage(sensors); publishSensors(sensors); @@ -103,10 +107,24 @@ class WebotsController : public rclcpp::Node 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::Subscription::SharedPtr motor_command_subscription_; RobotClient* client; }; From 20bd04f8fe5907c20111f42c114ceb6682c5c76f Mon Sep 17 00:00:00 2001 From: Soheil Date: Mon, 31 May 2021 10:29:29 +0430 Subject: [PATCH 07/14] [#REFACTOR] Change includes from angle brackets to quotes. --- src/ros_bridge/src/webots_controller.cpp | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/src/ros_bridge/src/webots_controller.cpp b/src/ros_bridge/src/webots_controller.cpp index 4009f84..0b19601 100644 --- a/src/ros_bridge/src/webots_controller.cpp +++ b/src/ros_bridge/src/webots_controller.cpp @@ -2,20 +2,20 @@ #include #include #include +#include +#include +#include -#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 "cv_bridge/cv_bridge.h" +#include +#include +#include +#include +#include +#include +#include #include "ros_bridge/robot_client/robot_client.hpp" -#include -#include -#include using std::placeholders::_1; using namespace std::chrono_literals; @@ -50,8 +50,6 @@ class WebotsController : public rclcpp::Node // Enable sensors for (unsigned int i=0; i < motors_json.size(); i++) { - // std::cout<< motors_json[i]["name"].asString() << std::endl; - // std::cout<< motors_json[i]["time_step"].asDouble() << std::endl; SensorTimeStep *sensor = request.add_sensor_time_steps(); sensor->set_name(motors_json[i]["name"].asString() + "S"); sensor->set_timestep(motors_json[i]["time_step"].asDouble()); @@ -71,7 +69,6 @@ class WebotsController : public rclcpp::Node SensorMeasurements sensors = client->receive(); auto clk = rosgraph_msgs::msg::Clock(); clk.clock = rclcpp::Time(sensors.time()); - // std::cout<< clk.clock.nanosec << std::endl; clock_publisher_->publish(clk); publishImage(sensors); publishSensors(sensors); From 36ed248940cd271371de13aaa6d461f852eb8c72 Mon Sep 17 00:00:00 2001 From: Soheil Date: Mon, 31 May 2021 10:33:28 +0430 Subject: [PATCH 08/14] [#REFACTOR] Move motors.json to resources. --- src/ros_bridge/{src => resources}/motors.json | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/ros_bridge/{src => resources}/motors.json (100%) diff --git a/src/ros_bridge/src/motors.json b/src/ros_bridge/resources/motors.json similarity index 100% rename from src/ros_bridge/src/motors.json rename to src/ros_bridge/resources/motors.json From cb80eb979ab037921316f880cbc3e6f430fd0bf8 Mon Sep 17 00:00:00 2001 From: Soheil Date: Mon, 31 May 2021 10:39:56 +0430 Subject: [PATCH 09/14] [#REFACTOR] Rename motors.json to joints.json --- src/ros_bridge/resources/{motors.json => joints.json} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/ros_bridge/resources/{motors.json => joints.json} (100%) diff --git a/src/ros_bridge/resources/motors.json b/src/ros_bridge/resources/joints.json similarity index 100% rename from src/ros_bridge/resources/motors.json rename to src/ros_bridge/resources/joints.json From 3f03ba8850f23c2313138dbce6f17f63b0468e65 Mon Sep 17 00:00:00 2001 From: Soheil Date: Mon, 31 May 2021 18:41:18 +0430 Subject: [PATCH 10/14] [#REFACTOR] Add gyros, acelometers, cameras and sensors enabling, and reading port and host as parameters. --- src/ros_bridge/resources/devices.json | 122 +++++++++++++++++++++++ src/ros_bridge/resources/joints.json | 82 --------------- src/ros_bridge/src/webots_controller.cpp | 57 ++++++++--- 3 files changed, 167 insertions(+), 94 deletions(-) create mode 100644 src/ros_bridge/resources/devices.json delete mode 100644 src/ros_bridge/resources/joints.json 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/resources/joints.json b/src/ros_bridge/resources/joints.json deleted file mode 100644 index a8af51b..0000000 --- a/src/ros_bridge/resources/joints.json +++ /dev/null @@ -1,82 +0,0 @@ -[ - { - "name" : "Neck", - "time_step" : 8 - }, - { - "name" : "Head", - "time_step" : 8 - }, - { - "name" : "ShoulderL", - "time_step" : 8 - }, - { - "name" : "ArmUpperL", - "time_step" : 8 - }, - { - "name" : "ArmLowerL", - "time_step" : 8 - }, - { - "name" : "PelvYL", - "time_step" : 8 - }, - { - "name" : "PelvL", - "time_step" : 8 - }, - { - "name" : "LegUpperL", - "time_step" : 8 - }, - { - "name" : "LegLowerL", - "time_step" : 8 - }, - { - "name" : "AnkleL", - "time_step" : 8 - }, - { - "name" : "FootL", - "time_step" : 8 - }, - { - "name" : "PelvYR", - "time_step" : 8 - }, - { - "name" : "PelvR", - "time_step" : 8 - }, - { - "name" : "LegUpperR", - "time_step" : 8 - }, - { - "name" : "LegLowerR", - "time_step" : 8 - }, - { - "name" : "AnkleR", - "time_step" : 8 - }, - { - "name" : "FootR", - "time_step" : 8 - }, - { - "name" : "ShoulderR", - "time_step" : 8 - }, - { - "name" : "ArmUpperR", - "time_step" : 8 - }, - { - "name" : "ArmLowerR", - "time_step" : 8 - } -] diff --git a/src/ros_bridge/src/webots_controller.cpp b/src/ros_bridge/src/webots_controller.cpp index 0b19601..41f3a61 100644 --- a/src/ros_bridge/src/webots_controller.cpp +++ b/src/ros_bridge/src/webots_controller.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include "ros_bridge/robot_client/robot_client.hpp" @@ -25,34 +26,63 @@ class WebotsController : public rclcpp::Node public: WebotsController() : Node("webots_controller") { + + // Parameters + this->declare_parameter("host", "127.0.0.1"); + this->declare_parameter("port", -1); + + // 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("127.0.0.1", 10001, 3); client->connectClient(); + // Enable devices ActuatorRequests request; + Json::Value devices; + std::ifstream json_file("src/ros_bridge/resources/devices.json"); + json_file >> devices; - // Enable camera - SensorTimeStep *camera_sensor = request.add_sensor_time_steps(); - camera_sensor->set_name("Camera"); - camera_sensor->set_timestep(16); - - Json::Value motors_json; - std::ifstream json_file("src/ros_bridge/src/motors.json"); - json_file >> motors_json; + // 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()); + } - // Enable sensors - for (unsigned int i=0; i < motors_json.size(); i++) { + // Accelerometers + for (unsigned int i=0; i < devices["accelerometers"].size(); i++) { SensorTimeStep *sensor = request.add_sensor_time_steps(); - sensor->set_name(motors_json[i]["name"].asString() + "S"); - sensor->set_timestep(motors_json[i]["time_step"].asDouble()); + sensor->set_name(devices["accelerometers"][i]["name"].asString()); + sensor->set_timestep(devices["accelerometers"][i]["time_step"].asDouble()); } client->sendRequest(request); @@ -121,8 +151,11 @@ class WebotsController : public rclcpp::Node 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[]) { From 713a542880cb1b6bcfc2475ff67b5354da586f48 Mon Sep 17 00:00:00 2001 From: Soheil Date: Fri, 4 Jun 2021 11:41:49 +0430 Subject: [PATCH 11/14] Format webots_controller.cpp --- src/ros_bridge/src/webots_controller.cpp | 242 +++++++++++------------ 1 file changed, 120 insertions(+), 122 deletions(-) diff --git a/src/ros_bridge/src/webots_controller.cpp b/src/ros_bridge/src/webots_controller.cpp index 41f3a61..9cc4905 100644 --- a/src/ros_bridge/src/webots_controller.cpp +++ b/src/ros_bridge/src/webots_controller.cpp @@ -17,148 +17,146 @@ #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", -1); - - // 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("127.0.0.1", 10001, 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()); - } +class WebotsController : public rclcpp::Node { +public: + WebotsController() + : Node("webots_controller") { + + // Parameters + this->declare_parameter("host", "127.0.0.1"); + this->declare_parameter("port", -1); + + // 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("127.0.0.1", 10001, 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()); + } - // 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()); - } + // 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()); + } - // 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()); - } + // 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()); + } - client->sendRequest(request); - SensorMeasurements sensors = client->receive(); + // 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()); } - 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; - } + 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"); - } + 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()); + 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); - } + 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 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(); - } + void command_callback(const sensor_msgs::msg::JointState::SharedPtr msg) const { - 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_; + 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[]) { +int main(int argc, char *argv[]) { GOOGLE_PROTOBUF_VERIFY_VERSION; rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); From d3d399544c8d482727267931a18cccede5b5d3b2 Mon Sep 17 00:00:00 2001 From: Soheil Date: Fri, 4 Jun 2021 11:44:17 +0430 Subject: [PATCH 12/14] [#REFACTOR] Connect to the server using port and ip parameters. --- src/ros_bridge/src/webots_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ros_bridge/src/webots_controller.cpp b/src/ros_bridge/src/webots_controller.cpp index 9cc4905..8b75351 100644 --- a/src/ros_bridge/src/webots_controller.cpp +++ b/src/ros_bridge/src/webots_controller.cpp @@ -27,7 +27,7 @@ class WebotsController : public rclcpp::Node { // Parameters this->declare_parameter("host", "127.0.0.1"); - this->declare_parameter("port", -1); + this->declare_parameter("port", 10001); // Publishers clock_publisher_ = this->create_publisher("clock", 10); @@ -46,7 +46,7 @@ class WebotsController : public rclcpp::Node { // Client construction and connecting this->get_parameter("host", host_); this->get_parameter("port", port_); - client = new RobotClient("127.0.0.1", 10001, 3); + client = new RobotClient(host_, port_, 3); client->connectClient(); // Enable devices From 055b5565d541b11b9b9973bd6affb611428da6d2 Mon Sep 17 00:00:00 2001 From: Soheil Date: Sat, 5 Jun 2021 14:00:23 +0430 Subject: [PATCH 13/14] Update README. --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index b959aea..17746ab 100644 --- a/README.md +++ b/README.md @@ -1 +1,4 @@ # v-hsc-ros-bridge +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 (specifed int the `src/ros_bridge/resources/devices.json`) over specific topics and subscribes to actuator command messages. Node `webots_controller` is responsible for these operations. \ No newline at end of file From 4b9636e999e5a901e4d6221567be083b66a4e8f5 Mon Sep 17 00:00:00 2001 From: Soheil Date: Sun, 6 Jun 2021 00:54:42 +0430 Subject: [PATCH 14/14] Update README --- README.md | 42 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 17746ab..425b29a 100644 --- a/README.md +++ b/README.md @@ -1,4 +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 (specifed int the `src/ros_bridge/resources/devices.json`) over specific topics and subscribes to actuator command messages. Node `webots_controller` is responsible for these operations. \ No newline at end of file +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