Skip to content

Commit

Permalink
Sync submodule (#740)
Browse files Browse the repository at this point in the history 
Co-authored-by: ad-daniel <[email protected]>
  • Loading branch information
@github-actions
github-actions[bot] and ad-daniel authored May 10, 2023
1 parent 2343f0f commit d12d24d
Show file tree
Hide file tree
Showing 3 changed files with 90 additions and 42 deletions.
125 changes: 86 additions & 39 deletions webots_ros2_driver/webots/src/controller/c/robot.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1104,7 +1104,7 @@ static char *encode_robot_name(const char *robot_name) {
return encoded_name;
}

static char *compute_socket_filename() {
static char *compute_socket_filename(char *error_buffer) {
char *robot_name = encode_robot_name(wbu_system_getenv("WEBOTS_ROBOT_NAME"));
const char *WEBOTS_INSTANCE_PATH = wbu_system_webots_instance_path(true);
char *socket_filename;
Expand Down Expand Up @@ -1159,7 +1159,8 @@ static char *compute_socket_filename() {
#endif
DIR *dr = opendir(WEBOTS_TMP_DIR);
if (dr == NULL) {
fprintf(stderr, "Error: cannot open directory %s\n", WEBOTS_TMP_DIR);
snprintf(error_buffer, ERROR_BUFFER_SIZE, "Cannot open directory %s", WEBOTS_TMP_DIR);
free(WEBOTS_TMP_DIR);
return NULL;
}
struct stat filestat;
Expand Down Expand Up @@ -1196,8 +1197,11 @@ static char *compute_socket_filename() {
}
closedir(dr);
free(WEBOTS_TMP_DIR);
if (number == -1)
// No Webots instance has been started
if (number == -1) {
snprintf(error_buffer, ERROR_BUFFER_SIZE, "Cannot find any instance of Webots");
return NULL;
}
if (WEBOTS_CONTROLLER_URL && WEBOTS_CONTROLLER_URL[0]) { // only the robot name was provided in WEBOTS_CONTROLLER_URL
const int length = 19 + strlen(WEBOTS_CONTROLLER_URL);
char *tmp = malloc(length);
Expand All @@ -1221,36 +1225,69 @@ static char *compute_socket_filename() {
exit(EXIT_FAILURE);
}
#ifdef _WIN32
int length = strlen(TMP_DIR) + 24; // TMP_DIR + "/webots-12345678901/ipc"
const int webots_instance_path_length = strlen(TMP_DIR) + 20; // TMP_DIR + "/webots-12345678901"
const int ipc_path_length = webots_instance_path_length + 4; // TMP_DIR + "/webots-12345678901/ipc"
#else
int length = strlen(TMP_DIR) + strlen(username) + 25; // TMP_DIR + '/webots/' + username + '/12345678901/ipc'
const int webots_instance_path_length =
strlen(TMP_DIR) + strlen(username) + 21; // TMP_DIR + '/webots/' + username + '/12345678901'
const int ipc_path_length = webots_instance_path_length + 4; // TMP_DIR + '/webots/' + username + '/12345678901/ipc'
#endif
char *folder = malloc(length);
char *webots_instance_folder = malloc(webots_instance_path_length);
char *ipc_folder = malloc(ipc_path_length);
#ifdef _WIN32
snprintf(folder, length, "%s/webots-%d/ipc", TMP_DIR, number);
snprintf(webots_instance_folder, webots_instance_path_length, "%s/webots-%d", TMP_DIR, number);
snprintf(ipc_folder, ipc_path_length, "%s/ipc", webots_instance_folder);
#else
snprintf(folder, length, "%s/webots/%s/%d/ipc", TMP_DIR, username, number);
snprintf(webots_instance_folder, webots_instance_path_length, "%s/webots/%s/%d", TMP_DIR, username, number);
snprintf(ipc_folder, ipc_path_length, "%s/ipc", webots_instance_folder);
#endif

// check if the Webots instance has been started
DIR *dr = opendir(webots_instance_folder);
if (dr == NULL) {
snprintf(error_buffer, ERROR_BUFFER_SIZE, "Cannot connect to Webots instance");
free(webots_instance_folder);
free(ipc_folder);
return NULL;
}
closedir(dr);
// check if Webots is currently loading
char *loading_file_path = malloc(strlen(webots_instance_folder) + 9); // webots_instance_folder + '/loading'
snprintf(loading_file_path, strlen(webots_instance_folder) + 9, "%s/loading", webots_instance_folder);
FILE *loading_file = fopen(loading_file_path, "r");
if (loading_file) {
fclose(loading_file);
snprintf(error_buffer, ERROR_BUFFER_SIZE, "The Webots simulation world is not yet ready");
free(webots_instance_folder);
free(ipc_folder);
free(loading_file_path);
return NULL;
}
free(webots_instance_folder);
free(loading_file_path);

free(robot_name);
char *sub_string = strstr(&WEBOTS_CONTROLLER_URL[6], "/");
robot_name = encode_robot_name(sub_string ? sub_string + 1 : NULL);
if (robot_name) {
#ifndef _WIN32
// socket file name is like: folder + robot_name + "/extern"
length += strlen(robot_name) + 8;
const int length = ipc_path_length + strlen(robot_name) + 8;
socket_filename = malloc(length);
snprintf(socket_filename, length, "%s/%s/extern", folder, robot_name);
snprintf(socket_filename, length, "%s/%s/extern", ipc_folder, robot_name);
#else
// socket file name is like: "\\.\\pipe\webots-XXX-robot_name"
length = 28 + strlen(robot_name);
const int length = 28 + strlen(robot_name);
socket_filename = malloc(length);
snprintf(socket_filename, length, "\\\\.\\pipe\\webots-%d-%s", number, robot_name);
#endif
free(robot_name);
} else { // check if a single extern robot is present in the ipc folder
DIR *dr = opendir(folder);
dr = opendir(ipc_folder);
if (dr == NULL) { // the ipc folder was not yet created
free(folder);
free(ipc_folder);
snprintf(error_buffer, ERROR_BUFFER_SIZE,
"The Webots simulation has not yet started or there is no robot in the simulation");
return NULL;
}
char **filenames = NULL;
Expand All @@ -1261,9 +1298,9 @@ static char *compute_socket_filename() {
continue;
bool found = false;
// search the robot folder for a file named "extern"
const int l = length + strlen(de->d_name) + 1;
const int l = ipc_path_length + strlen(de->d_name) + 1;
char *subfolder = malloc(l);
snprintf(subfolder, l, "%s/%s", folder, de->d_name);
snprintf(subfolder, l, "%s/%s", ipc_folder, de->d_name);
DIR *d = opendir(subfolder);
free(subfolder);
if (d) {
Expand All @@ -1279,26 +1316,30 @@ static char *compute_socket_filename() {
continue;
char **r = realloc(filenames, (count + 1) * sizeof(char *));
if (!r) {
fprintf(stderr, "Cannot allocate memory for listing \"%s\" folder.\n", folder);
fprintf(stderr, "Cannot allocate memory for listing \"%s\" folder.\n", ipc_folder);
exit(EXIT_FAILURE);
}
filenames = r;
filenames[count++] = strdup(de->d_name);
}
closedir(dr);
if (count == 0)
if (count == 0) {
snprintf(error_buffer, ERROR_BUFFER_SIZE,
"The Webots simulation has not yet started or there is no robot with an <extern> controller");
socket_filename = NULL;
else if (count > 1) { // more than one extern controller in the current instance of Webots
fprintf(stderr, "Webots instance %d has several extern controller robots.\n", number);
fprintf(stderr, "Please set the WEBOTS_CONTROLLER_URL environment variable to select one among:\n");
} else if (count > 1) { // more than one extern controller in the current instance of Webots
fprintf(stderr,
"No robot name provided, Webots instance %d should have exactly one robot set with an <extern> controller.\n",
number);
fprintf(stderr, "Available robots with <extern> controllers are:\n");
for (int i = 0; i < count; i++)
fprintf(stderr, "ipc://%d/%s\n", number, filenames[i]);
fprintf(stderr, " * %s\n", filenames[i]);
exit(EXIT_FAILURE);
} else {
#ifndef _WIN32
const int l = length + strlen(filenames[0] + 1) + 8; // folder + robot_name + "/extern"
const int l = ipc_path_length + strlen(filenames[0] + 1) + 8; // folder + robot_name + "/extern"
socket_filename = malloc(l);
snprintf(socket_filename, l, "%s/%s/extern", folder, filenames[0]);
snprintf(socket_filename, l, "%s/%s/extern", ipc_folder, filenames[0]);
#else
const int l = 28 + strlen(filenames[0] + 1); // "\\.\pipe\webots-XXXX" + robot_name
socket_filename = malloc(l);
Expand All @@ -1309,7 +1350,7 @@ static char *compute_socket_filename() {
free(filenames[i]);
free(filenames);
}
free(folder);
free(ipc_folder);
free(WEBOTS_CONTROLLER_URL);
return socket_filename;
}
Expand Down Expand Up @@ -1385,46 +1426,52 @@ int wb_robot_init() { // API initialization
int retry = 0;
while (true) {
bool success;
bool is_simulation_loading;
const char *WEBOTS_CONTROLLER_URL = wbu_system_getenv("WEBOTS_CONTROLLER_URL");
const char *WEBOTS_ROBOT_NAME = wbu_system_getenv("WEBOTS_ROBOT_NAME");
const char *WEBOTS_TMP_PATH = wbu_system_webots_instance_path(true);
char *error_message = malloc(ERROR_BUFFER_SIZE);
char *socket_filename = NULL;
if ((WEBOTS_CONTROLLER_URL != NULL) &&
!(WEBOTS_ROBOT_NAME && WEBOTS_ROBOT_NAME[0] && WEBOTS_TMP_PATH && WEBOTS_TMP_PATH[0]) &&
strncmp(WEBOTS_CONTROLLER_URL, "tcp://", 6) == 0) { // TCP URL given and not an intern controller
char *host, *robot_name;
int port = -1;
compute_remote_info(&host, &port, &robot_name);
char *error_message = malloc(ERROR_BUFFER_SIZE);
success = scheduler_init_remote(host, port, robot_name, error_message);
is_simulation_loading = strncmp(error_message, "The Webots simulation world is not ready yet", 44) == 0;
if (success) {
free(host);
free(robot_name);
free(error_message);
break;
} else {
if (retry % 5 == 0 && retry != 50)
fprintf(stderr, "%s, retrying for another %d seconds...\n", error_message, 50 - retry);
}

free(host);
free(robot_name);
free(error_message);
} else { // Intern or IPC extern controller
char *socket_filename = compute_socket_filename();
socket_filename = compute_socket_filename(error_message);
success = socket_filename ? scheduler_init_local(socket_filename) : false;
is_simulation_loading = strncmp(error_message, "The Webots simulation world is not ready yet", 44) == 0;
if (success) {
free(socket_filename);
free(error_message);
break;
}
if (retry % 5 == 0 && retry != 50) {
if (socket_filename)
fprintf(stderr, "Cannot connect to Webots instance on socket \"%s\", retrying for another %d seconds...\n",
socket_filename, 50 - retry);
else
fprintf(stderr, "Cannot connect to Webots instance, retrying for another %d seconds...\n", 50 - retry);
}
free(socket_filename);
}
if (retry % 5 == 0 && retry != 50) {
if (is_simulation_loading) {
retry -= 5;
fprintf(stderr, "%s, pending until loading is done...\n", error_message);
} else if (socket_filename) {
free(socket_filename);
fprintf(
stderr,
"The specified robot is not in the list of robots with <extern> controllers, retrying for another %d seconds...\n",
50 - retry);
} else
fprintf(stderr, "%s, retrying for another %d seconds...\n", error_message, 50 - retry);
}
free(error_message);
if (retry++ > 50) {
fprintf(stderr, "Giving up...\n");
exit(EXIT_FAILURE);
Expand Down
5 changes: 3 additions & 2 deletions webots_ros2_driver/webots/src/controller/c/scheduler.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,8 +73,9 @@ int scheduler_init_remote(const char *host, int port, const char *robot_name, ch
tcp_client_receive(scheduler_client, acknowledge_message, 10); // wait for ack message from Webots
if (strncmp(acknowledge_message, "FAILED", 6) == 0) {
snprintf(buffer, ERROR_BUFFER_SIZE, "%s",
robot_name == NULL ? "Exactly one robot should be set with an <extern> controller in the Webots simulation" :
"The specified robot is not in the list of robots with <extern> controllers");
robot_name == NULL ?
"No robot name provided, exactly one robot should be set with an <extern> controller in the Webots simulation" :
"The specified robot is not in the list of robots with <extern> controllers");
return false;
} else if (strncmp(acknowledge_message, "PROCESSING", 10) == 0) {
snprintf(buffer, ERROR_BUFFER_SIZE, "The Webots simulation world is not ready yet");
Expand Down
2 changes: 1 addition & 1 deletion webots_ros2_driver/webots/src/controller/c/tcp_client.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@

#include "tcp_client.h"

const int ERROR_BUFFER_SIZE = 128;
const int ERROR_BUFFER_SIZE = 256;

int tcp_client_new(const char *host, int port, char *buffer) {
const int fd = tcp_client_open(buffer);
Expand Down

0 comments on commit d12d24d

Please sign in to comment.