Skip to content

Commit 75d56aa

Browse files
qeyupBorjaOuterelo
qeyup
authored and
BorjaOuterelo
committed
Feature/demo fix (#9)
* Error fix * Update warning fix * ament_cppcheck and ament_uncrustify fix * Update C/RAD0_altitude_sensor/main.c Co-Authored-By: qeyup <42214121+qeyup@users.noreply.github.com> * Update C/RAD0_display/main.c Co-Authored-By: qeyup <42214121+qeyup@users.noreply.github.com>
1 parent 946b073 commit 75d56aa

16 files changed

Lines changed: 655 additions & 422 deletions

File tree

‎C/RAD0_actuator/main.c‎

Lines changed: 68 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -1,66 +1,91 @@
1+
// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima).
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
115
#include <rclc/rclc.h>
216
#include <std_msgs/msg/string.h>
317
#include <std_msgs/msg/int32.h>
418
#include <std_msgs/msg/u_int32.h>
519

620
#include <stdio.h>
721

8-
#define ASSERT(ptr) if (ptr == NULL) return -1;
22+
#define CUSTOM_ASSERT(ptr) if ((ptr) == NULL) {return -1;}
923

10-
static rclc_publisher_t* engine_pub;
24+
static rclc_publisher_t * engine_pub;
1125
static uint32_t engine_power = 100;
1226

13-
void engine_on_message(const void* msgin)
27+
void engine_on_message(const void * msgin)
1428
{
15-
16-
const std_msgs__msg__Int32* msg = (const std_msgs__msg__Int32*)msgin;
17-
18-
if (msg->data + engine_power > 0)
19-
{
20-
engine_power += msg->data;
21-
}
22-
else
23-
{
24-
engine_power = 0;
25-
}
26-
27-
// Publish new altitude
28-
std_msgs__msg__UInt32 msg_out;
29-
msg_out.data = engine_power;
30-
rclc_publish(engine_pub, (const void*)&msg_out);
29+
const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
30+
31+
if (msg->data + engine_power > 0) {
32+
engine_power += msg->data;
33+
} else {
34+
engine_power = 0;
35+
}
36+
37+
// Publish new altitude
38+
rclc_ret_t ret;
39+
std_msgs__msg__UInt32 msg_out;
40+
msg_out.data = engine_power;
41+
ret = rclc_publish(engine_pub, (const void *)&msg_out);
3142
}
3243

33-
int main(int argc, char* argv[])
44+
int main(int argc, char * argv[])
3445
{
35-
(void)argc;
36-
(void)argv;
46+
(void)argc;
47+
(void)argv;
48+
49+
rclc_node_t * node = NULL;
50+
rclc_subscription_t * engine_sub = NULL;
3751

38-
rclc_node_t* node = NULL;
39-
rclc_subscription_t* engine_sub = NULL;
40-
52+
rclc_ret_t ret;
4153

54+
ret = rclc_init(0, NULL);
55+
if (ret != RCL_RET_OK) {
56+
return -1;
57+
}
4258

43-
rclc_init(0, NULL);
44-
node = rclc_create_node("rad0_actuator_c", "");
45-
ASSERT(node);
46-
engine_sub = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "std_msgs_msg_Int32", engine_on_message, 1, false);
47-
ASSERT(engine_sub);
48-
engine_pub = rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, UInt32), "std_msgs_msg_UInt32", 1);
49-
ASSERT(engine_pub);
59+
node = rclc_create_node("rad0_actuator_c", "");
60+
CUSTOM_ASSERT(node);
61+
engine_sub = rclc_create_subscription(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg,
62+
Int32), "std_msgs_msg_Int32", engine_on_message, 1,
63+
false);
64+
CUSTOM_ASSERT(engine_sub);
65+
engine_pub = rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg,
66+
UInt32), "std_msgs_msg_UInt32",
67+
1);
68+
CUSTOM_ASSERT(engine_pub);
5069

51-
// Publish new altitude
52-
std_msgs__msg__UInt32 msg_out;
53-
msg_out.data = engine_power;
54-
rclc_publish(engine_pub, (const void*)&msg_out);
70+
// Publish new altitude
71+
std_msgs__msg__UInt32 msg_out;
72+
msg_out.data = engine_power;
73+
ret = rclc_publish(engine_pub, (const void *)&msg_out);
5574

56-
rclc_spin_node(node);
75+
rclc_spin_node(node);
5776

58-
//if (altitude_sub) rclc_destroy_subscription(altitude_sub);
59-
if (engine_sub) rclc_destroy_subscription(engine_sub);
60-
if (engine_pub) rclc_destroy_publisher(engine_pub);
61-
if (node) rclc_destroy_node(node);
77+
// if (altitude_sub) rclc_destroy_subscription(altitude_sub);
78+
if (engine_sub) {
79+
ret = rclc_destroy_subscription(engine_sub);
80+
}
81+
if (engine_pub) {
82+
ret = rclc_destroy_publisher(engine_pub);
83+
}
84+
if (node) {
85+
ret = rclc_destroy_node(node);
86+
}
6287

63-
printf("Actuator node closed.\n");
88+
printf("Actuator node closed.\n");
6489

65-
return 0;
90+
return 0;
6691
}

‎C/RAD0_altitude_sensor/main.c‎

Lines changed: 54 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,17 @@
1+
// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima).
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
115
#include <rclc/rclc.h>
216
#include <std_msgs/msg/float64.h>
317
#include <std_msgs/msg/int32.h>
@@ -6,50 +20,56 @@
620
#include <math.h>
721
#include <time.h>
822

9-
#define ASSERT(ptr) if (ptr == NULL) return -1;
23+
#define CUSTOM_ASSERT(ptr) if ((ptr) == NULL) {return -1;}
1024

1125
/**
12-
* @brief
13-
*
14-
* @param argc
15-
* @param argv
16-
* @return int
26+
* @brief
27+
*
28+
* @param argc
29+
* @param argv
30+
* @return int
1731
*/
18-
int main(int argc, char *argv[])
32+
int main(int argc, char * argv[])
1933
{
20-
(void)argc;
21-
(void)argv;
22-
rclc_init(0, NULL);
34+
(void)argc;
35+
(void)argv;
36+
37+
rclc_ret_t ret;
38+
39+
ret = rclc_init(0, NULL);
40+
if (ret != RCL_RET_OK) {
41+
return -1;
42+
}
43+
44+
rclc_node_t * node = NULL;
45+
rclc_publisher_t * publisher = NULL;
2346

24-
rclc_node_t* node = NULL;
25-
rclc_publisher_t* publisher = NULL;
47+
node = rclc_create_node("rad0_altitude_sensor_c", "");
48+
CUSTOM_ASSERT(node);
49+
publisher = rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg,
50+
Float64), "std_msgs_msg_Float64",
51+
1);
52+
CUSTOM_ASSERT(publisher);
2653

27-
node = rclc_create_node("rad0_altitude_sensor_c", "");
28-
ASSERT(node);
29-
publisher = rclc_create_publisher(node, RCLC_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64), "std_msgs_msg_Float64", 1);
30-
ASSERT(publisher);
3154

55+
double A = 0;
3256

33-
double A = 0;
3457

58+
while (rclc_ok()) {
59+
A += 0.0001;
3560

36-
while (rclc_ok())
37-
{
38-
A += 0.0001;
61+
// Publish new altitude
62+
std_msgs__msg__Float64 msg;
63+
msg.data = 500 * sin(A) + 950;
64+
ret = rclc_publish(publisher, (const void *)&msg);
3965

40-
// Publish new altitude
41-
std_msgs__msg__Float64 msg;
42-
msg.data = 500 * sin(A) + 950;
43-
rclc_publish(publisher, (const void*)&msg);
44-
66+
// Spin node
67+
rclc_spin_node_once(node, 0);
68+
}
4569

46-
// Spin node
47-
rclc_spin_node_once(node, 0);
48-
}
70+
if (publisher) {ret = rclc_destroy_publisher(publisher);}
71+
if (node) {ret = rclc_destroy_node(node);}
4972

50-
if (publisher) rclc_destroy_publisher(publisher);
51-
if (node) rclc_destroy_node(node);
52-
53-
printf("altitude sendor closed.\n");
54-
return 0;
55-
}
73+
printf("altitude sensor closed.\n");
74+
return 0;
75+
}

0 commit comments

Comments
 (0)