diff --git a/C/RAD0_actuator/main.c b/C/RAD0_actuator/main.c index 6c72992..e06529b 100644 --- a/C/RAD0_actuator/main.c +++ b/C/RAD0_actuator/main.c @@ -1,3 +1,17 @@ +// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include @@ -5,62 +19,73 @@ #include -#define ASSERT(ptr) if (ptr == NULL) return -1; +#define CUSTOM_ASSERT(ptr) if ((ptr) == NULL) {return -1;} -static rclc_publisher_t* engine_pub; +static rclc_publisher_t * engine_pub; static uint32_t engine_power = 100; -void engine_on_message(const void* msgin) +void engine_on_message(const void * msgin) { - - const std_msgs__msg__Int32* msg = (const std_msgs__msg__Int32*)msgin; - - if (msg->data + engine_power > 0) - { - engine_power += msg->data; - } - else - { - engine_power = 0; - } - - // Publish new altitude - std_msgs__msg__UInt32 msg_out; - msg_out.data = engine_power; - rclc_publish(engine_pub, (const void*)&msg_out); + const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin; + + if (msg->data + engine_power > 0) { + engine_power += msg->data; + } else { + engine_power = 0; + } + + // Publish new altitude + rclc_ret_t ret; + std_msgs__msg__UInt32 msg_out; + msg_out.data = engine_power; + ret = rclc_publish(engine_pub, (const void *)&msg_out); } -int main(int argc, char* argv[]) +int main(int argc, char * argv[]) { - (void)argc; - (void)argv; + (void)argc; + (void)argv; + + rclc_node_t * node = NULL; + rclc_subscription_t * engine_sub = NULL; - rclc_node_t* node = NULL; - rclc_subscription_t* engine_sub = NULL; - + rclc_ret_t ret; + ret = rclc_init(0, NULL); + if (ret != RCL_RET_OK) { + return -1; + } - rclc_init(0, NULL); - node = rclc_create_node("rad0_actuator_c", ""); - ASSERT(node); - engine_sub = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "std_msgs_msg_Int32", engine_on_message, 1, false); - ASSERT(engine_sub); - engine_pub = rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, UInt32), "std_msgs_msg_UInt32", 1); - ASSERT(engine_pub); + node = rclc_create_node("rad0_actuator_c", ""); + CUSTOM_ASSERT(node); + engine_sub = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, + Int32), "std_msgs_msg_Int32", engine_on_message, 1, + false); + CUSTOM_ASSERT(engine_sub); + engine_pub = rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, + UInt32), "std_msgs_msg_UInt32", + 1); + CUSTOM_ASSERT(engine_pub); - // Publish new altitude - std_msgs__msg__UInt32 msg_out; - msg_out.data = engine_power; - rclc_publish(engine_pub, (const void*)&msg_out); + // Publish new altitude + std_msgs__msg__UInt32 msg_out; + msg_out.data = engine_power; + ret = rclc_publish(engine_pub, (const void *)&msg_out); - rclc_spin_node(node); + rclc_spin_node(node); - //if (altitude_sub) rclc_destroy_subscription(altitude_sub); - if (engine_sub) rclc_destroy_subscription(engine_sub); - if (engine_pub) rclc_destroy_publisher(engine_pub); - if (node) rclc_destroy_node(node); + // if (altitude_sub) rclc_destroy_subscription(altitude_sub); + if (engine_sub) { + ret = rclc_destroy_subscription(engine_sub); + } + if (engine_pub) { + ret = rclc_destroy_publisher(engine_pub); + } + if (node) { + ret = rclc_destroy_node(node); + } - printf("Actuator node closed.\n"); + printf("Actuator node closed.\n"); - return 0; + return 0; } diff --git a/C/RAD0_altitude_sensor/main.c b/C/RAD0_altitude_sensor/main.c index 67332eb..db2e64d 100644 --- a/C/RAD0_altitude_sensor/main.c +++ b/C/RAD0_altitude_sensor/main.c @@ -1,3 +1,17 @@ +// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include @@ -6,50 +20,56 @@ #include #include -#define ASSERT(ptr) if (ptr == NULL) return -1; +#define CUSTOM_ASSERT(ptr) if ((ptr) == NULL) {return -1;} /** - * @brief - * - * @param argc - * @param argv - * @return int + * @brief + * + * @param argc + * @param argv + * @return int */ -int main(int argc, char *argv[]) +int main(int argc, char * argv[]) { - (void)argc; - (void)argv; - rclc_init(0, NULL); + (void)argc; + (void)argv; + + rclc_ret_t ret; + + ret = rclc_init(0, NULL); + if (ret != RCL_RET_OK) { + return -1; + } + + rclc_node_t * node = NULL; + rclc_publisher_t * publisher = NULL; - rclc_node_t* node = NULL; - rclc_publisher_t* publisher = NULL; + node = rclc_create_node("rad0_altitude_sensor_c", ""); + CUSTOM_ASSERT(node); + publisher = rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, + Float64), "std_msgs_msg_Float64", + 1); + CUSTOM_ASSERT(publisher); - node = rclc_create_node("rad0_altitude_sensor_c", ""); - ASSERT(node); - publisher = rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64), "std_msgs_msg_Float64", 1); - ASSERT(publisher); + double A = 0; - double A = 0; + while (rclc_ok()) { + A += 0.0001; - while (rclc_ok()) - { - A += 0.0001; + // Publish new altitude + std_msgs__msg__Float64 msg; + msg.data = 500 * sin(A) + 950; + ret = rclc_publish(publisher, (const void *)&msg); - // Publish new altitude - std_msgs__msg__Float64 msg; - msg.data = 500 * sin(A) + 950; - rclc_publish(publisher, (const void*)&msg); - + // Spin node + rclc_spin_node_once(node, 0); + } - // Spin node - rclc_spin_node_once(node, 0); - } + if (publisher) {ret = rclc_destroy_publisher(publisher);} + if (node) {ret = rclc_destroy_node(node);} - if (publisher) rclc_destroy_publisher(publisher); - if (node) rclc_destroy_node(node); - - printf("altitude sendor closed.\n"); - return 0; -} \ No newline at end of file + printf("altitude sensor closed.\n"); + return 0; +} diff --git a/C/RAD0_display/main.c b/C/RAD0_display/main.c index e8fe3cb..64b8572 100644 --- a/C/RAD0_display/main.c +++ b/C/RAD0_display/main.c @@ -1,3 +1,17 @@ +// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include @@ -6,7 +20,7 @@ #include -#define ASSERT(ptr) if (ptr == NULL) return -1; +#define CUSTOM_ASSERT(ptr) if ((ptr) == NULL) {return -1;} static float altitude; static uint32_t engine_power; @@ -14,91 +28,104 @@ static unsigned it; static char Alert[200]; /** - * @brief Update screen - * + * @brief Update screen + * */ void UpdateDisplay() { - printf("\r[received messages: %7u] [Altitude: %8.2f] [Engine power: %8u] [Status: %10s]", it++, altitude, engine_power, Alert); + printf("\r[received messages: %7u] [Altitude: %8.2f] [Engine power: %8u] [Status: %10s]", + it++, altitude, engine_power, Alert); } /** * @brief new alert callback - * - * @param msgin + * + * @param msgin */ -void on_alert_message(const void* msgin) +void on_alert_message(const void * msgin) { - const std_msgs__msg__String* msg = (const std_msgs__msg__String*)msgin; - strcpy(Alert, msg->data.data); - - UpdateDisplay(); + const std_msgs__msg__String * msg = (const std_msgs__msg__String *)msgin; + strcpy(Alert, msg->data.data); // NOLINT + + UpdateDisplay(); } /** - * @brief new altitude calback - * - * @param msgin + * @brief new altitude callback + * + * @param msgin */ -void on_altitude_message(const void* msgin) +void on_altitude_message(const void * msgin) { - std_msgs__msg__Float64* msg = (std_msgs__msg__Float64*)msgin; + std_msgs__msg__Float64 * msg = (std_msgs__msg__Float64 *)msgin; - altitude = msg->data; - UpdateDisplay(); + altitude = msg->data; + UpdateDisplay(); } /** - * @brief - * - * @param msgin + * @brief + * + * @param msgin */ -void on_power_message(const void* msgin) +void on_power_message(const void * msgin) { - std_msgs__msg__UInt32* msg = (std_msgs__msg__UInt32*)msgin; + std_msgs__msg__UInt32 * msg = (std_msgs__msg__UInt32 *)msgin; - engine_power = msg->data; - UpdateDisplay(); + engine_power = msg->data; + UpdateDisplay(); } /** * @brief Main - * - * @param argc - * @param argv - * @return int + * + * @param argc + * @param argv + * @return int */ -int main(int argc, char *argv[]) +int main(int argc, char * argv[]) { - (void)argc; - (void)argv; - - rclc_node_t* node = NULL; - rclc_subscription_t* alert_subscription = NULL; - rclc_subscription_t* altitude_subscription = NULL; - rclc_subscription_t* power_subscription = NULL; - - - rclc_init(0, NULL); - - node = rclc_create_node("rad0_display_c", ""); - ASSERT(node); - alert_subscription = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), "std_msgs_msg_String", on_alert_message, 1, false); - ASSERT(alert_subscription); - altitude_subscription = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64), "std_msgs_msg_Float64", on_altitude_message, 1, false); - ASSERT(altitude_subscription); - power_subscription = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, UInt32), "std_msgs_msg_UInt32", on_power_message, 1, false); - ASSERT(power_subscription); - - rclc_spin_node(node); - - if (alert_subscription) rclc_destroy_subscription(alert_subscription); - if (altitude_subscription) rclc_destroy_subscription(altitude_subscription); - if (node) rclc_destroy_node(node); - - printf("Display node closed."); - -} \ No newline at end of file + (void)argc; + (void)argv; + + rclc_node_t * node = NULL; + rclc_subscription_t * alert_subscription = NULL; + rclc_subscription_t * altitude_subscription = NULL; + rclc_subscription_t * power_subscription = NULL; + + rclc_ret_t ret; + + ret = rclc_init(0, NULL); + if (ret != RCL_RET_OK) { + return -1; + } + + node = rclc_create_node("rad0_display_c", ""); + CUSTOM_ASSERT(node); + alert_subscription = + rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, + String), "std_msgs_msg_String", on_alert_message, 1, + false); + CUSTOM_ASSERT(alert_subscription); + altitude_subscription = + rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, + Float64), "std_msgs_msg_Float64", on_altitude_message, 1, + false); + CUSTOM_ASSERT(altitude_subscription); + power_subscription = + rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, + UInt32), "std_msgs_msg_UInt32", on_power_message, 1, + false); + CUSTOM_ASSERT(power_subscription); + + rclc_spin_node(node); + + if (alert_subscription) {ret = rclc_destroy_subscription(alert_subscription);} + if (altitude_subscription) {ret = rclc_destroy_subscription(altitude_subscription);} + if (node) {ret = rclc_destroy_node(node);} + + printf("Display node closed."); +} diff --git a/C/complex_msg_publisher/main.c b/C/complex_msg_publisher/main.c index 1c49b36..2659e73 100644 --- a/C/complex_msg_publisher/main.c +++ b/C/complex_msg_publisher/main.c @@ -1,96 +1,119 @@ +// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include -int main(int argc, char* argv[]) +int main(int argc, char * argv[]) { - (void)argc; - (void)argv; - rclc_init(0, NULL); - rclc_node_t* node = rclc_create_node("complex_msg_publisher_c", ""); - if (node == NULL) - { - return -1; - } - rclc_publisher_t* publisher = - rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(complex_msgs, msg, NestedMsgTest), "complex_msgs_msg_NestedMsgTest", 1); - if (publisher == NULL) - { - return -1; - } + (void)argc; + (void)argv; + + rclc_ret_t ret; - complex_msgs__msg__NestedMsgTest msg; - char Buff1[30]; - msg.data14.data1.data = Buff1; - msg.data14.data1.size = 0; - msg.data14.data1.capacity = sizeof(Buff1); + ret = rclc_init(0, NULL); + if (ret != RCL_RET_OK) { + return -1; + } + rclc_node_t * node = rclc_create_node("complex_msg_publisher_c", ""); + if (node == NULL) { + return -1; + } + rclc_publisher_t * publisher = + rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(complex_msgs, msg, + NestedMsgTest), "complex_msgs_msg_NestedMsgTest", + 1); + if (publisher == NULL) { + return -1; + } - char Buff2[30]; - msg.data14.data2.data = Buff2; - msg.data14.data2.size = 0; - msg.data14.data2.capacity = sizeof(Buff1); - - char Buff3[30]; - msg.data14.data3.data = Buff3; - msg.data14.data3.size = 0; - msg.data14.data3.capacity = sizeof(Buff1); - - char Buff4[30]; - msg.data14.data4.data = Buff4; - msg.data14.data4.size = 0; - msg.data14.data4.capacity = sizeof(Buff1); - - + complex_msgs__msg__NestedMsgTest msg; + char Buff1[30]; + msg.data14.data1.data = Buff1; + msg.data14.data1.size = 0; + msg.data14.data1.capacity = sizeof(Buff1); - int num = 0; - while (rclc_ok()) - { - msg.data1 = (bool)(num & 0x01); - msg.data2 = (uint8_t)num; - msg.data3 = (signed char)num; - msg.data4 = (float)num; - msg.data5 = (double)num; - msg.data6 = (int8_t)num; - msg.data7 = (uint8_t)num; - msg.data8 = (int16_t)num; - msg.data9 = (uint16_t)num; - msg.data10 = (int32_t)num; - msg.data11 = (uint32_t)num; - msg.data12 = (int64_t)num; - msg.data13 = (uint64_t)num; - msg.data14.data1.size = snprintf(msg.data14.data1.data, msg.data14.data1.capacity, "Msg A - %i", num); - msg.data14.data2.size = snprintf(msg.data14.data2.data, msg.data14.data2.capacity, "Msg B - %i", num); - msg.data14.data3.size = snprintf(msg.data14.data3.data, msg.data14.data3.capacity, "Msg C - %i", num); - msg.data14.data4.size = snprintf(msg.data14.data4.data, msg.data14.data4.capacity, "Msg D - %i", num); - num++; + char Buff2[30]; + msg.data14.data2.data = Buff2; + msg.data14.data2.size = 0; + msg.data14.data2.capacity = sizeof(Buff1); + char Buff3[30]; + msg.data14.data3.data = Buff3; + msg.data14.data3.size = 0; + msg.data14.data3.capacity = sizeof(Buff1); - printf("I send:\n"); - printf("\tBool: %u\n", msg.data1); - printf("\tuint8_t: %u\n", msg.data2); - printf("\tsigned char: %u\n", msg.data3); - printf("\tfloat: %f\n", msg.data4); - printf("\tdouble: %lf\n", msg.data5); - printf("\tint8_t: %i\n", msg.data6); - printf("\tuint8_t: %u\n", msg.data7); - printf("\tint16_t: %i\n", msg.data8); - printf("\tuint16_t: %u\n", msg.data9); - printf("\tint32_t: %i\n", msg.data10); - printf("\tuint32_t: %u\n", msg.data11); - printf("\tint64_t: %li\n", msg.data12); - printf("\tuint64_t: %lu\n", msg.data13); + char Buff4[30]; + msg.data14.data4.data = Buff4; + msg.data14.data4.size = 0; + msg.data14.data4.capacity = sizeof(Buff1); - printf("\tstring 1: %s\n", msg.data14.data1.data); - printf("\tstring 2: %s\n", msg.data14.data2.data); - printf("\tstring 3: %s\n", msg.data14.data3.data); - printf("\tstring 4: %s\n", msg.data14.data4.data); - printf("\n\n"); - rclc_publish(publisher, (const void*)&msg); - rclc_spin_node_once(node, 500); + int num = 0; + while (rclc_ok()) { + msg.data1 = (bool)(num & 0x01); + msg.data2 = (uint8_t)num; + msg.data3 = (signed char)num; + msg.data4 = (float)num; + msg.data5 = (double)num; + msg.data6 = (int8_t)num; + msg.data7 = (uint8_t)num; + msg.data8 = (int16_t)num; + msg.data9 = (uint16_t)num; + msg.data10 = (int32_t)num; + msg.data11 = (uint32_t)num; + msg.data12 = (int64_t)num; + msg.data13 = (uint64_t)num; + msg.data14.data1.size = snprintf(msg.data14.data1.data, msg.data14.data1.capacity, "Msg A - %i", + num); + msg.data14.data2.size = snprintf(msg.data14.data2.data, msg.data14.data2.capacity, "Msg B - %i", + num); + msg.data14.data3.size = snprintf(msg.data14.data3.data, msg.data14.data3.capacity, "Msg C - %i", + num); + msg.data14.data4.size = snprintf(msg.data14.data4.data, msg.data14.data4.capacity, "Msg D - %i", + num); + num++; + + ret = rclc_publish(publisher, (const void *)&msg); + if (ret == RCL_RET_OK) { + printf("I send:\n"); + printf("\tBool: %u\n", msg.data1); + printf("\tuint8_t: %u\n", msg.data2); + printf("\tsigned char: %u\n", msg.data3); + printf("\tfloat: %f\n", msg.data4); + printf("\tdouble: %lf\n", msg.data5); + printf("\tint8_t: %i\n", msg.data6); + printf("\tuint8_t: %u\n", msg.data7); + printf("\tint16_t: %i\n", msg.data8); + printf("\tuint16_t: %u\n", msg.data9); + printf("\tint32_t: %i\n", msg.data10); + printf("\tuint32_t: %u\n", msg.data11); + printf("\tint64_t: %li\n", msg.data12); + printf("\tuint64_t: %lu\n", msg.data13); + + printf("\tstring 1: %s\n", msg.data14.data1.data); + printf("\tstring 2: %s\n", msg.data14.data2.data); + printf("\tstring 3: %s\n", msg.data14.data3.data); + printf("\tstring 4: %s\n", msg.data14.data4.data); + printf("\n\n"); } - rclc_destroy_publisher(publisher); - rclc_destroy_node(node); - return 0; + + rclc_spin_node_once(node, 500); + } + ret = rclc_destroy_publisher(publisher); + ret = rclc_destroy_node(node); + return 0; } diff --git a/C/complex_msg_subscriber/main.c b/C/complex_msg_subscriber/main.c index 4dc17bf..07e2e76 100644 --- a/C/complex_msg_subscriber/main.c +++ b/C/complex_msg_subscriber/main.c @@ -1,48 +1,71 @@ +// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include -#define ASSERT(ptr) if (ptr == NULL) return -1; +#define CUSTOM_ASSERT(ptr) if ((ptr) == NULL) {return -1;} -void on_message(const void* msgin) +void on_message(const void * msgin) { - const complex_msgs__msg__NestedMsgTest* msg = (const complex_msgs__msg__NestedMsgTest*)msgin; - printf("I heard:\n"); - printf("\tBool: %u\n", msg->data1); - printf("\tuint8_t: %u\n", msg->data2); - printf("\tsigned char: %u\n", msg->data3); - printf("\tfloat: %f\n", msg->data4); - printf("\tdouble: %lf\n", msg->data5); - printf("\tint8_t: %i\n", msg->data6); - printf("\tuint8_t: %u\n", msg->data7); - printf("\tint16_t: %i\n", msg->data8); - printf("\tuint16_t: %u\n", msg->data9); - printf("\tint32_t: %i\n", msg->data10); - printf("\tuint32_t: %u\n", msg->data11); - printf("\tint64_t: %li\n", msg->data12); - printf("\tuint64_t: %lu\n", msg->data13); - - printf("\tstring 1: %s\n", msg->data14.data1.data); - printf("\tstring 2: %s\n", msg->data14.data2.data); - printf("\tstring 3: %s\n", msg->data14.data3.data); - printf("\tstring 4: %s\n", msg->data14.data4.data); - printf("\n\n"); + const complex_msgs__msg__NestedMsgTest * msg = (const complex_msgs__msg__NestedMsgTest *)msgin; + printf("I heard:\n"); + printf("\tBool: %u\n", msg->data1); + printf("\tuint8_t: %u\n", msg->data2); + printf("\tsigned char: %u\n", msg->data3); + printf("\tfloat: %f\n", msg->data4); + printf("\tdouble: %lf\n", msg->data5); + printf("\tint8_t: %i\n", msg->data6); + printf("\tuint8_t: %u\n", msg->data7); + printf("\tint16_t: %i\n", msg->data8); + printf("\tuint16_t: %u\n", msg->data9); + printf("\tint32_t: %i\n", msg->data10); + printf("\tuint32_t: %u\n", msg->data11); + printf("\tint64_t: %li\n", msg->data12); + printf("\tuint64_t: %lu\n", msg->data13); + + printf("\tstring 1: %s\n", msg->data14.data1.data); + printf("\tstring 2: %s\n", msg->data14.data2.data); + printf("\tstring 3: %s\n", msg->data14.data3.data); + printf("\tstring 4: %s\n", msg->data14.data4.data); + printf("\n\n"); } -int main(int argc, char* argv[]) +int main(int argc, char * argv[]) { - (void)argc; - (void)argv; - rclc_init(0, NULL); - rclc_node_t* node = rclc_create_node("complex_msg_subscriber_c", ""); - ASSERT(node); - rclc_subscription_t* sub = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(complex_msgs, msg, NestedMsgTest), "complex_msgs_msg_NestedMsgTest", on_message, 1, false); - ASSERT(sub); - - rclc_spin_node(node); - - rclc_destroy_subscription(sub); - rclc_destroy_node(node); - return 0; + (void)argc; + (void)argv; + + rclc_ret_t ret; + + ret = rclc_init(0, NULL); + if (ret != RCL_RET_OK) { + return -1; + } + rclc_node_t * node = rclc_create_node("complex_msg_subscriber_c", ""); + CUSTOM_ASSERT(node); + rclc_subscription_t * sub = + rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(complex_msgs, msg, + NestedMsgTest), "complex_msgs_msg_NestedMsgTest", on_message, 1, + false); + CUSTOM_ASSERT(sub); + + rclc_spin_node(node); + + ret = rclc_destroy_subscription(sub); + ret = rclc_destroy_node(node); + return 0; } diff --git a/C/int32_publisher/main.c b/C/int32_publisher/main.c index 13462f2..53a8472 100644 --- a/C/int32_publisher/main.c +++ b/C/int32_publisher/main.c @@ -1,29 +1,52 @@ +// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include -#define ASSERT(ptr) if (ptr == NULL) return -1; +#define CUSTOM_ASSERT(ptr) if ((ptr) == NULL) {return -1;} -int main(int argc, char *argv[]) +int main(int argc, char * argv[]) { - (void)argc; - (void)argv; - rclc_init(0, NULL); - rclc_node_t* node = rclc_create_node("int32_publisher_c", ""); - ASSERT(node); - rclc_publisher_t* publisher = rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "std_msgs_msg_Int32", 1); - ASSERT(publisher); - std_msgs__msg__Int32 msg; - msg.data = -1; + (void)argc; + (void)argv; + + rclc_ret_t ret; + + ret = rclc_init(0, NULL); + if (ret != RCL_RET_OK) { + return -1; + } + rclc_node_t * node = rclc_create_node("int32_publisher_c", ""); + CUSTOM_ASSERT(node); + rclc_publisher_t * publisher = + rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, + Int32), "std_msgs_msg_Int32", 1); + CUSTOM_ASSERT(publisher); + std_msgs__msg__Int32 msg; + msg.data = -1; - while (rclc_ok()) - { - printf("Sending: '%i'\n", msg.data++); - rclc_publish(publisher, (const void*)&msg); - rclc_spin_node_once(node, 500); + while (rclc_ok()) { + ret = rclc_publish(publisher, (const void *)&msg); + if (ret == RCL_RET_OK) { + printf("Sending: '%i'\n", msg.data++); } - rclc_destroy_publisher(publisher); - rclc_destroy_node(node); - return 0; -} \ No newline at end of file + rclc_spin_node_once(node, 500); + } + ret = rclc_destroy_publisher(publisher); + ret = rclc_destroy_node(node); + return 0; +} diff --git a/C/int32_subscriber/main.c b/C/int32_subscriber/main.c index 621abc7..2c81d38 100644 --- a/C/int32_subscriber/main.c +++ b/C/int32_subscriber/main.c @@ -1,30 +1,52 @@ +// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include -#define ASSERT(ptr) if (ptr == NULL) return -1; +#define CUSTOM_ASSERT(ptr) if ((ptr) == NULL) {return -1;} -void on_message(const void* msgin) +void on_message(const void * msgin) { - const std_msgs__msg__Int32* msg = (const std_msgs__msg__Int32*)msgin; - printf("I heard: [%i]\n", msg->data); + const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin; + printf("I heard: [%i]\n", msg->data); } -int main(int argc, char* argv[]) +int main(int argc, char * argv[]) { - (void)argc; - (void)argv; - rclc_init(0, NULL); - rclc_node_t* node = rclc_create_node("int32_subscriber_c", ""); - ASSERT(node); - rclc_subscription_t* sub = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), - "std_msgs_msg_Int32", on_message, 1, false); - ASSERT(sub); - - rclc_spin_node(node); - - rclc_destroy_subscription(sub); - rclc_destroy_node(node); - return 0; + (void)argc; + (void)argv; + + rclc_ret_t ret; + + ret = rclc_init(0, NULL); + if (ret != RCL_RET_OK) { + return -1; + } + + rclc_node_t * node = rclc_create_node("int32_subscriber_c", ""); + CUSTOM_ASSERT(node); + rclc_subscription_t * sub = + rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), + "std_msgs_msg_Int32", on_message, 1, false); + CUSTOM_ASSERT(sub); + + rclc_spin_node(node); + + ret = rclc_destroy_subscription(sub); + ret = rclc_destroy_node(node); + return 0; } diff --git a/C/string_publisher/main.c b/C/string_publisher/main.c index f61a7eb..13e4f8f 100644 --- a/C/string_publisher/main.c +++ b/C/string_publisher/main.c @@ -1,39 +1,64 @@ +// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include -#define ASSERT(ptr) if (ptr == NULL) return -1; +#define CUSTOM_ASSERT(ptr) if ((ptr) == NULL) {return -1;} -int main(int argc, char* argv[]) +int main(int argc, char * argv[]) { - (void)argc; - (void)argv; - rclc_init(0, NULL); - rclc_node_t* node = rclc_create_node("string_publisher_c", ""); - ASSERT(node); - rclc_publisher_t* publisher = - rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), "std_msgs_msg_String", 1); - ASSERT(publisher); - - std_msgs__msg__String msg; - char buff[128] = {0}; - msg.data.data = buff; - msg.data.capacity = sizeof(buff); - msg.data.size = 0; - - int num = 0; - while (rclc_ok()) - { - msg.data.size = snprintf(msg.data.data, msg.data.capacity, "Hello World %i", num++); - if (msg.data.size > msg.data.capacity) - msg.data.size = 0; - - printf("Sending: '%s'\n", msg.data.data); - rclc_publish(publisher, (const void*)&msg); - rclc_spin_node_once(node, 500); + (void)argc; + (void)argv; + + rclc_ret_t ret; + + ret = rclc_init(0, NULL); + if (ret != RCL_RET_OK) { + return -1; + } + + rclc_node_t * node = rclc_create_node("string_publisher_c", ""); + CUSTOM_ASSERT(node); + rclc_publisher_t * publisher = + rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, + String), "std_msgs_msg_String", 1); + CUSTOM_ASSERT(publisher); + + std_msgs__msg__String msg; + char buff[128] = {0}; + msg.data.data = buff; + msg.data.capacity = sizeof(buff); + msg.data.size = 0; + + int num = 0; + while (rclc_ok()) { + msg.data.size = snprintf(msg.data.data, msg.data.capacity, "Hello World %i", num++); + if (msg.data.size > msg.data.capacity) { + msg.data.size = 0; + } + + ret = rclc_publish(publisher, (const void *)&msg); + if (ret == RCL_RET_OK) { + printf("Sending: '%s'\n", msg.data.data); } - rclc_destroy_publisher(publisher); - rclc_destroy_node(node); - return 0; + + rclc_spin_node_once(node, 500); + } + ret = rclc_destroy_publisher(publisher); + ret = rclc_destroy_node(node); + return 0; } diff --git a/C/string_subscriber/main.c b/C/string_subscriber/main.c index 9309f3e..7123c32 100644 --- a/C/string_subscriber/main.c +++ b/C/string_subscriber/main.c @@ -1,30 +1,52 @@ +// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include -#define ASSERT(ptr) if (ptr == NULL) return -1; +#define CUSTOM_ASSERT(ptr) if ((ptr) == NULL) {return -1;} -void on_message(const void* msgin) +void on_message(const void * msgin) { - const std_msgs__msg__String* msg = (const std_msgs__msg__String*)msgin; - printf("I heard: [%s]\n", msg->data.data); + const std_msgs__msg__String * msg = (const std_msgs__msg__String *)msgin; + printf("I heard: [%s]\n", msg->data.data); } -int main(int argc, char* argv[]) +int main(int argc, char * argv[]) { - (void)argc; - (void)argv; - rclc_init(0, NULL); - rclc_node_t* node = rclc_create_node("string_subscriber_c", ""); - ASSERT(node); - rclc_subscription_t* sub = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), - "std_msgs_msg_String", on_message, 1, false); - ASSERT(sub); - - rclc_spin_node(node); - - rclc_destroy_subscription(sub); - rclc_destroy_node(node); - return 0; + (void)argc; + (void)argv; + + rclc_ret_t ret; + + ret = rclc_init(0, NULL); + if (ret != RCL_RET_OK) { + return -1; + } + + rclc_node_t * node = rclc_create_node("string_subscriber_c", ""); + CUSTOM_ASSERT(node); + rclc_subscription_t * sub = + rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), + "std_msgs_msg_String", on_message, 1, false); + CUSTOM_ASSERT(sub); + + rclc_spin_node(node); + + ret = rclc_destroy_subscription(sub); + ret = rclc_destroy_node(node); + return 0; } diff --git a/Cpp/RAD0_control/main.cpp b/Cpp/RAD0_control/main.cpp index 74ab558..f12487d 100644 --- a/Cpp/RAD0_control/main.cpp +++ b/Cpp/RAD0_control/main.cpp @@ -12,21 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/float64.hpp" -#include "std_msgs/msg/int32.hpp" -#include "std_msgs/msg/string.hpp" +#include +#include +#include +#include + using std::placeholders::_1; class ExternalNode : public rclcpp::Node { public: - ExternalNode() : Node("rad0_control_cpp") + ExternalNode() + : Node("rad0_control_cpp") { engine_power_publisher_ = this->create_publisher("std_msgs_msg_Int32"); warn_publisher_ = this->create_publisher("std_msgs_msg_String"); - - altitude_subscription_ = this->create_subscription("std_msgs_msg_Float64", std::bind(&ExternalNode::topic_callback, this, _1)); + + altitude_subscription_ = this->create_subscription( + "std_msgs_msg_Float64", std::bind(&ExternalNode::topic_callback, this, _1)); } private: @@ -37,40 +40,34 @@ class ExternalNode : public rclcpp::Node void topic_callback(const std_msgs::msg::Float64::SharedPtr msg) { // Warning - if (msg->data <= 500) - { - { - auto message = std_msgs::msg::String(); - message.data = "Failure!!"; - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()) - this->warn_publisher_->publish(message); - } - - { - auto message = std_msgs::msg::Int32(); - message.data = 10; - this->engine_power_publisher_->publish(message); - } + if (msg->data <= 500) { + { + auto message = std_msgs::msg::String(); + message.data = "Failure!!"; + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + this->warn_publisher_->publish(message); + } - } + { + auto message = std_msgs::msg::Int32(); + message.data = 10; + this->engine_power_publisher_->publish(message); + } + // Failure + } else if (msg->data <= 1000) { + { + auto message = std_msgs::msg::String(); + message.data = "Warning!!"; + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + this->warn_publisher_->publish(message); + } - // Failure - else if (msg->data <= 1000) - { - { - auto message = std_msgs::msg::String(); - message.data = "Warning!!"; - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()) - this->warn_publisher_->publish(message); - } - - { - auto message = std_msgs::msg::Int32(); - message.data = 5; - this->engine_power_publisher_->publish(message); - } - - } + { + auto message = std_msgs::msg::Int32(); + message.data = 5; + this->engine_power_publisher_->publish(message); + } + } } }; diff --git a/Cpp/complex_msg_publisher/main.cpp b/Cpp/complex_msg_publisher/main.cpp index 484f3c4..9c7af8e 100644 --- a/Cpp/complex_msg_publisher/main.cpp +++ b/Cpp/complex_msg_publisher/main.cpp @@ -12,18 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + +#include #include #include -#include -#include "rclcpp/rclcpp.hpp" + using namespace std::chrono_literals; class complex_msg_publisher_cpp_node : public rclcpp::Node { public: - complex_msg_publisher_cpp_node() : Node("complex_msg_publisher_cpp") + complex_msg_publisher_cpp_node() + : Node("complex_msg_publisher_cpp") { - publisher_ = this->create_publisher("complex_msgs_msg_NestedMsgTest"); + publisher_ = this->create_publisher( + "complex_msgs_msg_NestedMsgTest"); timer_ = this->create_wall_timer( 500ms, std::bind(&complex_msg_publisher_cpp_node::timer_callback, this)); } @@ -31,25 +36,25 @@ class complex_msg_publisher_cpp_node : public rclcpp::Node private: void timer_callback() { - msg.data1 = (bool)(num & 0x01); - msg.data2 = (uint8_t)num; - msg.data3 = (signed char)num; - msg.data4 = (float)num; - msg.data5 = (double)num; - msg.data6 = (int8_t)num; - msg.data7 = (uint8_t)num; - msg.data8 = (int16_t)num; - msg.data9 = (uint16_t)num; - msg.data10 = (int32_t)num; - msg.data11 = (uint32_t)num; - msg.data12 = (int64_t)num; - msg.data13 = (uint64_t)num; + msg.data1 = static_cast(num & 0x01); + msg.data2 = static_cast(num); + msg.data3 = static_cast(num); + msg.data4 = static_cast(num); + msg.data5 = static_cast(num); + msg.data6 = static_cast(num); + msg.data7 = static_cast(num); + msg.data8 = static_cast(num); + msg.data9 = static_cast(num); + msg.data10 = static_cast(num); + msg.data11 = static_cast(num); + msg.data12 = static_cast(num); + msg.data13 = static_cast(num); msg.data14.data1 = "Msg A - " + std::to_string(num); msg.data14.data2 = "Msg B - " + std::to_string(num); msg.data14.data3 = "Msg C - " + std::to_string(num); msg.data14.data4 = "Msg D - " + std::to_string(num); num++; - + publisher_->publish(msg); std::cout << "I send:" << std::endl; diff --git a/Cpp/complex_msg_subscriber/main.cpp b/Cpp/complex_msg_subscriber/main.cpp index 28bd114..c95efa9 100644 --- a/Cpp/complex_msg_subscriber/main.cpp +++ b/Cpp/complex_msg_subscriber/main.cpp @@ -12,17 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include -#include "rclcpp/rclcpp.hpp" +#include + +#include +#include + using std::placeholders::_1; class complex_msg_subscriber_cpp_node : public rclcpp::Node { public: - complex_msg_subscriber_cpp_node() : Node("complex_msg_subscriber_cpp") - { - subscription_ = this->create_subscription("complex_msgs_msg_NestedMsgTest", + complex_msg_subscriber_cpp_node() + : Node("complex_msg_subscriber_cpp") + { + subscription_ = this->create_subscription( + "complex_msgs_msg_NestedMsgTest", std::bind(&complex_msg_subscriber_cpp_node::topic_callback, this, _1)); } diff --git a/Cpp/int32_publisher/main.cpp b/Cpp/int32_publisher/main.cpp index a3c104e..4c09eb3 100644 --- a/Cpp/int32_publisher/main.cpp +++ b/Cpp/int32_publisher/main.cpp @@ -12,16 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + +#include #include #include -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/int32.hpp" + + using namespace std::chrono_literals; class int32_publisher_node : public rclcpp::Node { public: - int32_publisher_node() : Node("int32_publisher_cpp") + int32_publisher_node() + : Node("int32_publisher_cpp") { publisher_ = this->create_publisher("std_msgs_msg_Int32"); timer_ = this->create_wall_timer( diff --git a/Cpp/int32_subscriber/main.cpp b/Cpp/int32_subscriber/main.cpp index 1dcba12..f05e352 100644 --- a/Cpp/int32_subscriber/main.cpp +++ b/Cpp/int32_subscriber/main.cpp @@ -12,19 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + +#include #include -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/int32.hpp" using std::placeholders::_1; class int32_subscriber_node : public rclcpp::Node { public: - int32_subscriber_node() : Node("int32_subscriber_cpp") - { - subscription_ = this->create_subscription("std_msgs_msg_Int32", - std::bind(&int32_subscriber_node::topic_callback, this, _1)); + int32_subscriber_node() + : Node("int32_subscriber_cpp") + { + subscription_ = this->create_subscription("std_msgs_msg_Int32", + std::bind(&int32_subscriber_node::topic_callback, this, _1)); } private: diff --git a/Cpp/string_publisher/main.cpp b/Cpp/string_publisher/main.cpp index a696ce2..03ff35e 100644 --- a/Cpp/string_publisher/main.cpp +++ b/Cpp/string_publisher/main.cpp @@ -12,16 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + +#include #include #include -#include -#include "rclcpp/rclcpp.hpp" + using namespace std::chrono_literals; class string_publisher_cpp_node : public rclcpp::Node { public: - string_publisher_cpp_node() : Node("string_publisher_cpp") + string_publisher_cpp_node() + : Node("string_publisher_cpp") { publisher_ = this->create_publisher("std_msgs_msg_String"); timer_ = this->create_wall_timer( diff --git a/Cpp/string_subscriber/main.cpp b/Cpp/string_subscriber/main.cpp index 74eca5c..0f6846e 100644 --- a/Cpp/string_subscriber/main.cpp +++ b/Cpp/string_subscriber/main.cpp @@ -12,18 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include -#include "rclcpp/rclcpp.hpp" +#include + +#include +#include + using std::placeholders::_1; class string_subscriber_cpp_node : public rclcpp::Node { public: - string_subscriber_cpp_node() : Node("string_subscriber_cpp") - { - subscription_ = this->create_subscription("std_msgs_msg_String", - std::bind(&string_subscriber_cpp_node::topic_callback, this, _1)); + string_subscriber_cpp_node() + : Node("string_subscriber_cpp") + { + subscription_ = this->create_subscription("std_msgs_msg_String", + std::bind(&string_subscriber_cpp_node::topic_callback, this, _1)); } private: