From b51cd75baaf8f97f23596ff2b2bab3bd5092737c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nicol=C3=B2=20Boscolo?= Date: Mon, 27 Apr 2015 14:01:51 +0200 Subject: [PATCH] Added sample file and minor architecture fixes --- .gitignore | 1 + include/cros.h | 14 + samples/.gitignore | 4 - samples/CMakeLists.txt | 7 +- samples/api-test.c | 367 ++++++++++++++++++ .../bin/rosdb/gripping_robot/CloseGripper.srv | 3 + .../rosdb/gripping_robot/GripperJoints.msg | 1 + .../rosdb/gripping_robot/GripperStatus.msg | 4 + samples/bin/rosdb/gripping_robot/MoveArm.srv | 5 + .../bin/rosdb/gripping_robot/OpenGripper.srv | 3 + .../bin/rosdb/gripping_robot/Reconfigure.srv | 4 + .../bin/rosdb/gripping_robot/RestPosition.srv | 2 + samples/bin/rosdb/gripping_robot/Transfer.srv | 2 + samples/bin/rosdb/rosgraph_msgs/Log.msg | 19 + samples/raw-api-test.c | 6 +- src/cros_node.c | 3 +- 16 files changed, 435 insertions(+), 10 deletions(-) create mode 100644 include/cros.h delete mode 100644 samples/.gitignore create mode 100644 samples/api-test.c create mode 100644 samples/bin/rosdb/gripping_robot/CloseGripper.srv create mode 100644 samples/bin/rosdb/gripping_robot/GripperJoints.msg create mode 100644 samples/bin/rosdb/gripping_robot/GripperStatus.msg create mode 100644 samples/bin/rosdb/gripping_robot/MoveArm.srv create mode 100644 samples/bin/rosdb/gripping_robot/OpenGripper.srv create mode 100644 samples/bin/rosdb/gripping_robot/Reconfigure.srv create mode 100644 samples/bin/rosdb/gripping_robot/RestPosition.srv create mode 100644 samples/bin/rosdb/gripping_robot/Transfer.srv create mode 100644 samples/bin/rosdb/rosgraph_msgs/Log.msg diff --git a/.gitignore b/.gitignore index 80986d3..3fadaee 100644 --- a/.gitignore +++ b/.gitignore @@ -12,4 +12,5 @@ build/ debug/ release/ docs/ +gmon.out *~ diff --git a/include/cros.h b/include/cros.h new file mode 100644 index 0000000..33617c9 --- /dev/null +++ b/include/cros.h @@ -0,0 +1,14 @@ +/* + * cros.h + * + * Created on: 27/apr/2015 + * Author: nico + */ + +#ifndef INCLUDE_CROS_H_ +#define INCLUDE_CROS_H_ + +#include "cros_api.h" +#include "cros_log.h" + +#endif /* INCLUDE_CROS_H_ */ diff --git a/samples/.gitignore b/samples/.gitignore deleted file mode 100644 index ff8e24c..0000000 --- a/samples/.gitignore +++ /dev/null @@ -1,4 +0,0 @@ -# Ignore binaries -* -!*.* -!*/ diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index cf7fb08..b4d8cd7 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -1,7 +1,10 @@ -set(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}) +set(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin) add_executable (raw-api-test raw-api-test.c) target_link_libraries(raw-api-test cros) add_executable(parameters-test parameters-test.c) -target_link_libraries(parameters-test cros) \ No newline at end of file +target_link_libraries(parameters-test cros) + +add_executable(api-test api-test.c) +target_link_libraries(api-test cros) diff --git a/samples/api-test.c b/samples/api-test.c new file mode 100644 index 0000000..2a9a8b2 --- /dev/null +++ b/samples/api-test.c @@ -0,0 +1,367 @@ +#include +#include +#include +#include +#include +#include + +#include + +#define SUBSCRIBER + +// TODO Signal handler + +static void printHelp( char *cmd_name ) +{ + printf("Usage: %s [OPTION] ... \n", cmd_name); + printf("Options:\n"); + printf("\t-name Set the node name (default: /test_node)\n"); + printf("\t-host Set the node host (default: 127.0.0.1)\n"); + printf("\t-chost Set the roscore host (default: 127.0.0.1)\n"); + printf("\t-cport Set the roscore port (default: 11311)\n"); + printf("\t-h Print this help\n"); +} + +static uint64_t start_timer = 0; +CrosNode *node; + +static CallbackResponse gripperstatus_sub_callback(cRosMessage *message, void* data_context) +{ + cRosMessageField *PowerStatus_field = cRosMessageGetField(message, "PowerStatus"); + cRosMessageField *ClampStatus_field = cRosMessageGetField(message, "ClampStatus"); + cRosMessageField *AlarmCodes_field = cRosMessageGetField(message, "AlarmCodes"); + cRosMessageField *Configuration_field = cRosMessageGetField(message, "Configuration"); + + uint32_t PowerStatus = PowerStatus_field->data.as_uint32; + uint32_t NAlarmCodes = AlarmCodes_field->size; + uint32_t Configuration = Configuration_field->data.as_uint64; + uint32_t ClampStatus[4]; + ClampStatus[0] = *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 0); + ClampStatus[1] = *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 1); + ClampStatus[2] = *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 2); + ClampStatus[3] = *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 3); + + uint32_t *AlarmCodes = (uint32_t *)malloc(NAlarmCodes*sizeof(uint32_t)); + if (AlarmCodes == NULL) + exit(1); + + int it = 0; + for (; it < NAlarmCodes; it++) + { + AlarmCodes[it] = *cRosMessageFieldArrayAtUInt32(AlarmCodes_field, it); + } + + printf("PowerStatus: %d\n", PowerStatus); + printf("ClampStatus: [%d %d %d %d]\n", ClampStatus[0], ClampStatus[1], + ClampStatus[2], ClampStatus[3]); + printf("AlarmCodes: ["); + int first = 1; + + for (it = 0; it < NAlarmCodes; it++) + { + if (first) + first = 0; + else + printf(" "); + + printf("%d", AlarmCodes[it]); + } + printf("]\n"); + printf("Configuration: %d\n", Configuration); + fflush(stdout); + + free(AlarmCodes); + + return 0; +} + +static CallbackResponse gripperjoints_sub_callback(cRosMessage *message, void* context) +{ + cRosMessageField *Position_field = cRosMessageGetField(message, "Position"); + double Position[9]; + Position[0] = *cRosMessageFieldArrayAtFloat64(Position_field, 0); + Position[1] = *cRosMessageFieldArrayAtFloat64(Position_field, 1); + Position[2] = *cRosMessageFieldArrayAtFloat64(Position_field, 2); + Position[3] = *cRosMessageFieldArrayAtFloat64(Position_field, 3); + Position[4] = *cRosMessageFieldArrayAtFloat64(Position_field, 4); + Position[5] = *cRosMessageFieldArrayAtFloat64(Position_field, 5); + Position[6] = *cRosMessageFieldArrayAtFloat64(Position_field, 6); + Position[7] = *cRosMessageFieldArrayAtFloat64(Position_field, 7); + Position[8] = *cRosMessageFieldArrayAtFloat64(Position_field, 8); + + printf("Position: [%f %f %f %f %f %f %f %f %f]\n", Position[0], Position[1], Position[2], + Position[3], Position[4], Position[5], Position[6], Position[7], Position[8]); + + fflush(stdout); + return 0; +} + +static CallbackResponse callback_pub_gripperstatus(cRosMessage *message, void* data_context) +{ + cRosMessageField *PowerStatus_field = cRosMessageGetField(message, "PowerStatus"); + cRosMessageField *ClampStatus_field = cRosMessageGetField(message, "ClampStatus"); + cRosMessageField *AlarmCodes_field = cRosMessageGetField(message, "AlarmCodes"); + cRosMessageField *Configuration_field = cRosMessageGetField(message, "Configuration"); + + PowerStatus_field->data.as_uint32 = 1; + + *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 0) = 0; + *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 1) = 1; + *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 2) = 2; + *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 3) = 3; + + cRosMessageFieldArrayClear(AlarmCodes_field); + cRosMessageFieldArrayPushBackUInt32(AlarmCodes_field, 1); + cRosMessageFieldArrayPushBackUInt32(AlarmCodes_field, 8); + + Configuration_field->data.as_uint32 = 3; + + return 0; +} + +static CallbackResponse callback_pub_gripperjoints(cRosMessage *message, void* context) +{ + cRosMessageField *Position_field = cRosMessageGetField(message, "Position"); + double Position[9]; + + *cRosMessageFieldArrayAtFloat64(Position_field, 0) = 8; + *cRosMessageFieldArrayAtFloat64(Position_field, 1) = 7; + *cRosMessageFieldArrayAtFloat64(Position_field, 2) = 6; + *cRosMessageFieldArrayAtFloat64(Position_field, 3) = 5; + *cRosMessageFieldArrayAtFloat64(Position_field, 4) = 4; + *cRosMessageFieldArrayAtFloat64(Position_field, 5) = 3; + *cRosMessageFieldArrayAtFloat64(Position_field, 6) = 2; + *cRosMessageFieldArrayAtFloat64(Position_field, 7) = 1; + *cRosMessageFieldArrayAtFloat64(Position_field, 8) = 0; + + return 0; +} + +/* + * Service callbacks definition + */ + +static CallbackResponse callback_srv_close_gripper(cRosMessage *request, cRosMessage *response, void* context) +{ + srand (time(NULL)); + uint8_t response_val = rand() % 2; + + cRosMessageField *closed_field = cRosMessageGetField(response, "closed"); + closed_field->data.as_uint8 = response_val; + + printf("Service close_clamps. Arguments: none . Response %d \n", response_val); + + fflush(stdout); + return 0; +} + +static CallbackResponse callback_srv_open_gripper(cRosMessage *request, cRosMessage *response, void* context) +{ + srand (time(NULL)); + uint8_t response_val = rand() % 2; + + cRosMessageField *opened_field = cRosMessageGetField(response, "opened"); + opened_field->data.as_uint8 = response_val; + + printf("Service open_clamps. Arguments: none . Response %d \n", response_val); + fflush(stdout); + + return 0; +} + +static CallbackResponse callback_srv_rest(cRosMessage *request, cRosMessage *response, void* context) +{ + srand (time(NULL)); + uint8_t response_val = rand() % 2; + + cRosMessageField *parked_field = cRosMessageGetField(response, "parked"); + parked_field->data.as_uint8 = response_val; + + printf("Service park. Arguments: none . Response %d \n", response_val); + fflush(stdout); + + return 0; +} + +static CallbackResponse callback_srv_reconfigure(cRosMessage *request, cRosMessage *response, void* context) +{ + cRosMessageField *id_field = cRosMessageGetField(request, "id"); + int64_t reconfigure_id; + reconfigure_id = id_field->data.as_int64; + + srand (time(NULL)); + uint8_t response_val = rand() % 2; + + cRosMessageField *reconfigured_field = cRosMessageGetField(response, "reconfigured"); + reconfigured_field->data.as_uint8 = response_val; + + printf("Service reconfigure. Arguments: %lld. Response %d \n", reconfigure_id, response_val); + fflush(stdout); + + return 0; +} + +static CallbackResponse callback_srv_move_arm(cRosMessage *request, cRosMessage *response, void* context) +{ + cRosMessageField *arm_field = cRosMessageGetField(request, "arm"); + cRosMessageField *rad_field = cRosMessageGetField(request, "rad"); + cRosMessageField *velocity_field = cRosMessageGetField(request, "velocity"); + + int32_t arm = arm_field->data.as_int32; + int32_t rad = arm_field->data.as_int32; + int32_t velocity = arm_field->data.as_int32; + + srand (time(NULL)); + uint8_t response_val = rand() % 2; + + cRosMessageField *positioned_field = cRosMessageGetField(response, "positioned"); + positioned_field->data.as_uint8 = response_val; + + printf("Service reconfigure. Arguments: {arm: %d, rad: %d, velocity: %d} . Response %d \n", arm, rad, velocity, response_val); + fflush(stdout); + + return 0; +} + +static CallbackResponse callback_srv_transfer(cRosMessage *request, cRosMessage *response, void* context) +{ + srand (time(NULL)); + uint8_t response_val = rand() % 2; + + cRosMessageField *transfered_field = cRosMessageGetField(response, "transfered"); + transfered_field->data.as_uint8 = response_val; + + printf("Service transfer. Arguments: none . Response %d \n", response_val); + fflush(stdout); + + return 0; +} + +int main(int argc, char **argv) +{ + char *default_node_name = "/Gripper", + *node_name = default_node_name; + char *default_host = "127.0.0.1", + *node_host = default_host, + *roscore_host = default_host; + unsigned short roscore_port = 11311; + + if(argc == 2) + { + printHelp( argv[0] ); + return EXIT_SUCCESS; + } + + srand (time(NULL)); + + int i = 1; + for( ; i < argc - 1; i+=2) + { + if( strcmp(argv[i],"-name") == 0) + node_name = argv[i+1]; + else if( strcmp(argv[i],"-host") == 0) + node_host = argv[i+1]; + else if( strcmp(argv[i],"-chost") == 0) + roscore_host = argv[i+1]; + else if( strcmp(argv[i],"-cport") == 0) + { + int i_port = atoi(argv[i+1]); + if( i_port < 0 || i_port > USHRT_MAX ) + { + fprintf(stderr,"Invalid port %d\n",i_port); + exit(EXIT_FAILURE); + } + roscore_port = (unsigned short)i_port; + } + else + { + fprintf(stderr,"Invalid option %s\n",argv[i]); + exit(EXIT_FAILURE); + } + } + + printf("Running node \"%s\" with host : %s, roscore host : %s and roscore port : %d\n", + node_name, node_host, roscore_host, roscore_port ); + printf("To set a different node/host/port, take a look at the options: "); + printf("%s -h\n", argv[0]); + + char path[1024]; + getcwd(path, sizeof(path)); + strncat(path, "/rosdb", sizeof(path)); + node = cRosNodeCreate(node_name, node_host, roscore_host, roscore_port, path, NULL); + + int rc; + ROS_INFO(node, "cROS Node (version %.2f) created!\n", 0.9); + +#ifdef SUBSCRIBER + + rc = cRosApiRegisterSubscriber(node, "/gripperstatus", "gripping_robot/GripperStatus", + gripperstatus_sub_callback, NULL, NULL); + if (rc == -1) + return EXIT_FAILURE; + + rc = cRosApiRegisterSubscriber(node, "/gripperjoints", "gripping_robot/GripperJoints", + gripperjoints_sub_callback, NULL, NULL); + if (rc == -1) + return EXIT_FAILURE; + +#endif + +/* + +Publisher Example + +#ifdef PUBLISHER + + rc = cRosApiRegisterPublisher(node, "/gripperstatus","gripping_robot/GripperStatus",1000, + callback_pub_gripperstatus, NULL, NULL); + if (rc == -1) + return EXIT_FAILURE; + + rc = cRosApiRegisterPublisher(node,"/gripperjoints","gripping_robot/GripperJoints", 1000, + callback_pub_gripperjoints, NULL, NULL); + if (rc == -1) + return EXIT_FAILURE; + +#endif + +*/ + + rc = cRosApiRegisterServiceProvider(node,"/close_clamps","gripping_robot/CloseGripper", + callback_srv_close_gripper, NULL, NULL); + if (rc == -1) + return EXIT_FAILURE; + + rc = cRosApiRegisterServiceProvider(node,"/open_clamps","gripping_robot/OpenGripper", + callback_srv_open_gripper, NULL, NULL); + if (rc == -1) + return EXIT_FAILURE; + + rc = cRosApiRegisterServiceProvider(node,"/park_configuration","gripping_robot/RestPosition", + callback_srv_rest, NULL, NULL); + if (rc == -1) + return EXIT_FAILURE; + + rc = cRosApiRegisterServiceProvider(node,"/reconfigure","gripping_robot/Reconfigure", + callback_srv_reconfigure, NULL, NULL); + if (rc == -1) + return EXIT_FAILURE; + + rc = cRosApiRegisterServiceProvider(node,"/transfer","gripping_robot/Transfer", + callback_srv_transfer, NULL, NULL); + if (rc == -1) + return EXIT_FAILURE; + + rc = cRosApiRegisterServiceProvider(node,"/move_arm","gripping_robot/MoveArm", + callback_srv_move_arm, NULL, NULL); + if (rc == -1) + return EXIT_FAILURE; + + unsigned char exit = 0; + + cRosNodeStart( node, &exit ); + + cRosNodeDestroy( node ); + + return EXIT_SUCCESS; +} diff --git a/samples/bin/rosdb/gripping_robot/CloseGripper.srv b/samples/bin/rosdb/gripping_robot/CloseGripper.srv new file mode 100644 index 0000000..6045b07 --- /dev/null +++ b/samples/bin/rosdb/gripping_robot/CloseGripper.srv @@ -0,0 +1,3 @@ +--- +bool closed + diff --git a/samples/bin/rosdb/gripping_robot/GripperJoints.msg b/samples/bin/rosdb/gripping_robot/GripperJoints.msg new file mode 100644 index 0000000..1bc7c0b --- /dev/null +++ b/samples/bin/rosdb/gripping_robot/GripperJoints.msg @@ -0,0 +1 @@ +float64[9] Position diff --git a/samples/bin/rosdb/gripping_robot/GripperStatus.msg b/samples/bin/rosdb/gripping_robot/GripperStatus.msg new file mode 100644 index 0000000..95c32b0 --- /dev/null +++ b/samples/bin/rosdb/gripping_robot/GripperStatus.msg @@ -0,0 +1,4 @@ +uint32 PowerStatus +uint32[4] ClampStatus +uint32[] AlarmCodes +uint32 Configuration diff --git a/samples/bin/rosdb/gripping_robot/MoveArm.srv b/samples/bin/rosdb/gripping_robot/MoveArm.srv new file mode 100644 index 0000000..5e9e3f6 --- /dev/null +++ b/samples/bin/rosdb/gripping_robot/MoveArm.srv @@ -0,0 +1,5 @@ +int32 arm +int32 rad +int32 velocity +--- +bool positioned diff --git a/samples/bin/rosdb/gripping_robot/OpenGripper.srv b/samples/bin/rosdb/gripping_robot/OpenGripper.srv new file mode 100644 index 0000000..0807861 --- /dev/null +++ b/samples/bin/rosdb/gripping_robot/OpenGripper.srv @@ -0,0 +1,3 @@ +--- +bool opened + diff --git a/samples/bin/rosdb/gripping_robot/Reconfigure.srv b/samples/bin/rosdb/gripping_robot/Reconfigure.srv new file mode 100644 index 0000000..897ceaf --- /dev/null +++ b/samples/bin/rosdb/gripping_robot/Reconfigure.srv @@ -0,0 +1,4 @@ +int64 id +--- +bool reconfigured + diff --git a/samples/bin/rosdb/gripping_robot/RestPosition.srv b/samples/bin/rosdb/gripping_robot/RestPosition.srv new file mode 100644 index 0000000..988d5c0 --- /dev/null +++ b/samples/bin/rosdb/gripping_robot/RestPosition.srv @@ -0,0 +1,2 @@ +--- +bool parked diff --git a/samples/bin/rosdb/gripping_robot/Transfer.srv b/samples/bin/rosdb/gripping_robot/Transfer.srv new file mode 100644 index 0000000..979b249 --- /dev/null +++ b/samples/bin/rosdb/gripping_robot/Transfer.srv @@ -0,0 +1,2 @@ +--- +bool transfered diff --git a/samples/bin/rosdb/rosgraph_msgs/Log.msg b/samples/bin/rosdb/rosgraph_msgs/Log.msg new file mode 100644 index 0000000..9a9597c --- /dev/null +++ b/samples/bin/rosdb/rosgraph_msgs/Log.msg @@ -0,0 +1,19 @@ +## +## Severity level constants +## +byte DEBUG=1 #debug level +byte INFO=2 #general level +byte WARN=4 #warning level +byte ERROR=8 #error level +byte FATAL=16 #fatal/critical level +## +## Fields +## +Header header +byte level +string name # name of the node +string msg # message +string file # file the message came from +string function # function the message came from +uint32 line # line the message came from +string[] topics # topic names that the node publishes diff --git a/samples/raw-api-test.c b/samples/raw-api-test.c index f146bc2..c946f59 100755 --- a/samples/raw-api-test.c +++ b/samples/raw-api-test.c @@ -287,7 +287,7 @@ static CallbackResponse int32_sub_callback(DynBuffer *buffer, void* data_context static CallbackResponse int64_sub_callback(DynBuffer *buffer, void* data_context) { - printf("Read: %li\n", *(int64_t *)buffer->data); + printf("Read: %lli\n", *(int64_t *)buffer->data); fflush(stdout); return 0; } @@ -322,7 +322,7 @@ static CallbackResponse uint32_sub_callback(DynBuffer *buffer, void* data_contex static CallbackResponse uint64_sub_callback(DynBuffer *buffer, void* data_context) { - printf("Read: %lu\n", *(uint64_t *)buffer->data); + printf("Read: %llu\n", *(uint64_t *)buffer->data); fflush(stdout); return 0; } @@ -377,7 +377,7 @@ static CallbackResponse counter_callback(DynBuffer *buffer, void* data_context) static CallbackResponse clock_callback(DynBuffer *buffer, void* data_context) { uint64_t cur_timer = cRosClockGetTimeMs(); - snprintf(message_buffer_clock, 100, "Time elapsed since start of node execution : %ld msec", + snprintf(message_buffer_clock, 100, "Time elapsed since start of node execution : %lld msec", cur_timer - start_timer ); size_t len = strlen(message_buffer_clock); dynBufferPushBackUInt32( buffer, (uint32_t)len ); diff --git a/src/cros_node.c b/src/cros_node.c index 3445f5e..bfa2ed1 100644 --- a/src/cros_node.c +++ b/src/cros_node.c @@ -16,6 +16,7 @@ #include "cros_defs.h" #include "cros_node_api.h" #include "cros_tcpros.h" +#include "cros_log.h" static void initPublisherNode(PublisherNode *node); static void initSubscriberNode(SubscriberNode *node); @@ -2212,7 +2213,7 @@ void cRosNodeDoEventsLoop ( CrosNode *n ) { if (errno == EINTR) { - PRINT_INFO(n, "cRosNodeDoEventsLoop() : select() returned EINTR\n"); + PRINT_INFO("cRosNodeDoEventsLoop() : select() returned EINTR\n"); } else {