Skip to content

Commit bba225c

Browse files
JPGC04JPGC04
committed
fixed wrapper, more efficient, reconnects automatically in main
1 parent 5952071 commit bba225c

File tree

3 files changed

+164
-100
lines changed

3 files changed

+164
-100
lines changed

‎pio_workspace/lib/micro_ros_wrapper/micro_ros_wrapper.cpp‎

Lines changed: 50 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1,47 +1,78 @@
11
#include "micro_ros_wrapper.h"
22

3-
#include <Arduino.h>
4-
5-
// MicroROSWrapper Implementation
6-
MicroROSWrapper::MicroROSWrapper(const char* node_name) {
3+
MicroROSWrapper::MicroROSWrapper() {
74
set_microros_transports();
5+
}
6+
7+
void MicroROSWrapper::init(const char* node_name) {
88
allocator_ = rcl_get_default_allocator();
99
RCCHECK(rclc_support_init(&support_, 0, NULL, &allocator_));
1010
RCCHECK(rclc_node_init_default(&node_, node_name, "", &support_));
11-
RCCHECK(rclc_executor_init(&executor_, &support_.context, 1, &allocator_));
11+
RCCHECK(rclc_executor_init(&executor_, &support_.context, MAX_HANDLE_COUNT, &allocator_));
12+
}
13+
14+
void MicroROSWrapper::spin() {
15+
RCCHECK(rclc_executor_spin_some(&executor_, RCL_MS_TO_NS(10)));
1216
}
1317

1418
rcl_publisher_t* MicroROSWrapper::createPublisher(const char* topic_name, const rosidl_message_type_support_t* type_support) {
15-
RCCHECK(rclc_publisher_init_default(&publishers_[publisher_count], &node_, type_support, topic_name));
16-
publisher_count++;
17-
return &publishers_[publisher_count - 1]; // Return a pointer to the publisher
19+
if (publisher_count_ == MAX_PUBLISHER_COUNT) {
20+
return nullptr;
21+
}
22+
RCCHECK(rclc_publisher_init_default(&publishers_[publisher_count_], &node_, type_support, topic_name));
23+
publisher_count_++;
24+
return &publishers_[publisher_count_ - 1];
1825
}
1926

27+
rcl_subscription_t* MicroROSWrapper::createSubscriber(const char* topic_name, const rosidl_message_type_support_t* type_support, void (*callback)(const void*)) {
28+
if (subscription_count_ == MAX_SUBSCRIPTION_COUNT) {
29+
return nullptr;
30+
}
31+
RCCHECK(rclc_subscription_init_default(&subscriptions_[subscription_count_], &node_, type_support, topic_name));
32+
RCCHECK(rclc_executor_add_subscription(&executor_, &subscriptions_[subscription_count_], nullptr, callback, ON_NEW_DATA));
33+
subscription_count_++;
34+
return &subscriptions_[subscription_count_ - 1];
35+
}
2036

2137
void MicroROSWrapper::publishData(const void* msg, rcl_publisher_t &publisher) {
2238
RCSOFTCHECK(rcl_publish(&publisher, msg, NULL));
2339
}
2440

25-
rcl_subscription_t* MicroROSWrapper::createSubscriber(const char* topic_name, const rosidl_message_type_support_t* type_support, void (*callback)(const void*)) {
26-
RCCHECK(rclc_subscription_init_default(&subscriptions_[subscription_count], &node_, type_support, topic_name));
27-
RCCHECK(rclc_executor_add_subscription(&executor_, &subscriptions_[subscription_count], nullptr, callback, ON_NEW_DATA));
28-
subscription_count++;
29-
return &subscriptions_[subscription_count - 1];
41+
bool MicroROSWrapper::pingAgent() {
42+
return RMW_RET_OK == rmw_uros_ping_agent(100, 1);
3043
}
3144

32-
void MicroROSWrapper::spin() {
33-
RCCHECK(rclc_executor_spin_some(&executor_, RCL_MS_TO_NS(100)));
45+
#pragma GCC diagnostic push
46+
#pragma GCC diagnostic ignored "-Wunused-result"
47+
void MicroROSWrapper::destroyMicroROS() {
48+
rmw_context_t * rmw_context = rcl_context_get_rmw_context(&support_.context);
49+
(void) rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);
50+
51+
for (int i = 0; i < publisher_count_; i++) {
52+
rcl_publisher_fini(&publishers_[i], &node_);
53+
}
54+
55+
publisher_count_ = 0;
56+
57+
for (int i = 0; i < subscription_count_; i++) {
58+
rcl_subscription_fini(&subscriptions_[i], &node_);
59+
}
60+
61+
subscription_count_ = 0;
62+
63+
rclc_executor_fini(&executor_);
64+
rcl_node_fini(&node_);
65+
rclc_support_fini(&support_);
3466
}
67+
#pragma GCC diagnostic pop
3568

36-
void MicroROSWrapper::errorLoop() {
69+
void MicroROSWrapper::errorLoop_() {
3770
int error = 0;
38-
39-
while (error < 20) {
71+
while (error < 10) {
4072
digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
4173
delay(100);
4274

4375
error++;
4476
}
45-
4677
digitalWrite(LED_BUILTIN, HIGH);
4778
}

‎pio_workspace/lib/micro_ros_wrapper/micro_ros_wrapper.h‎

Lines changed: 18 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -3,36 +3,46 @@
33

44
#include <micro_ros_arduino.h>
55

6+
#include <Arduino.h>
7+
68
#include <rcl/rcl.h>
79
#include <rcl/error_handling.h>
810
#include <rclc/rclc.h>
911
#include <rclc/executor.h>
1012

11-
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)) { errorLoop(); }}
13+
#define MAX_HANDLE_COUNT 101
14+
#define MAX_PUBLISHER_COUNT 100
15+
#define MAX_SUBSCRIPTION_COUNT 100
16+
17+
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)) { errorLoop_(); }}
1218
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)) {} }
1319

1420
class MicroROSWrapper {
1521
public:
16-
MicroROSWrapper(const char* node_name);
22+
MicroROSWrapper();
23+
void init(const char* node_name);
1724
void spin();
25+
1826
rcl_publisher_t* createPublisher(const char* topic_name, const rosidl_message_type_support_t* type_support);
1927
rcl_subscription_t* createSubscriber(const char* topic_name, const rosidl_message_type_support_t* type_support, void (*callback)(const void*));
20-
2128
void publishData(const void* msg, rcl_publisher_t &publisher);
2229

30+
bool pingAgent();
31+
void destroyMicroROS();
32+
2333
private:
2434
rclc_executor_t executor_;
2535
rclc_support_t support_;
2636
rcl_allocator_t allocator_;
2737
rcl_node_t node_;
2838

29-
rcl_publisher_t publishers_[50];
30-
rcl_subscription_t subscriptions_[50];
39+
rcl_publisher_t publishers_[MAX_PUBLISHER_COUNT];
40+
rcl_subscription_t subscriptions_[MAX_SUBSCRIPTION_COUNT];
3141

32-
int publisher_count = 0;
33-
int subscription_count = 0;
42+
int publisher_count_ = 0;
43+
int subscription_count_ = 0;
3444

35-
void errorLoop();
45+
void errorLoop_();
3646
};
3747

3848
#endif

‎pio_workspace/src/power_main.cpp‎

Lines changed: 96 additions & 73 deletions
Original file line numberDiff line numberDiff line change
@@ -13,100 +13,123 @@
1313

1414
#define LED_PIN 13
1515

16-
MicroROSWrapper micro_ros("power_node");
16+
#define ENABLE_VOLTAGE_SENSE false
17+
#define ENABLE_CURRENT_SENSE false
18+
19+
MicroROSWrapper micro_ros;
20+
ADCSensors adcSensors;
21+
TMP36 temperatureSensor(23, 3.3);
1722

1823
// Define the subscribers and publishers as rcl_publisher_t and rcl_subscription_t
1924
rcl_subscription_t* propulsion_microseconds_subscriber;
2025
rcl_publisher_t* power_thrusters_current_publisher;
2126
rcl_publisher_t* power_batteries_voltage_publisher;
22-
rcl_publisher_t* power_board_temprature_publisher;
23-
rcl_publisher_t* power_teensy_temprature_publisher;
27+
rcl_publisher_t* power_board_temperature_publisher;
28+
rcl_publisher_t* power_teensy_temperature_publisher;
2429

2530
std_msgs__msg__Int16MultiArray propulsion_microseconds_msg;
2631
std_msgs__msg__Float32MultiArray power_thrusters_current_msg;
2732
std_msgs__msg__Float32MultiArray power_batteries_voltage_msg;
2833
std_msgs__msg__Float32 power_board_temperature_msg;
2934
std_msgs__msg__Float32 power_teensy_temperature_msg;
3035

31-
TMP36 temperatureSensor(23, 3.3);
32-
ADCSensors adcSensors;
33-
3436
void propulsion_microseconds_callback(const void *msgin) {
35-
const std_msgs__msg__Int16MultiArray *propulsion_microseconds_msg = (const std_msgs__msg__Int16MultiArray *)msgin;
36-
// Ensure we don't exceed the size of the `microseconds` array
37-
for (size_t i = 0; i < 8 && i < propulsion_microseconds_msg->data.size; i++) {
38-
microseconds[i] = propulsion_microseconds_msg->data.data[i]; // Access the data correctly
39-
}
37+
const std_msgs__msg__Int16MultiArray *propulsion_microseconds_msg = (const std_msgs__msg__Int16MultiArray *)msgin;
38+
for (size_t i = 0; i < 8; i++) {
39+
microseconds[i] = propulsion_microseconds_msg->data.data[i]; // Access the data correctly
40+
}
4041
}
4142

42-
void power_setup() {
43-
pinMode(LED_PIN, OUTPUT);
44-
digitalWrite(LED_PIN, HIGH);
45-
46-
initThrusters();
47-
48-
// init all messages
49-
power_board_temperature_msg.data = 0.0;
50-
power_teensy_temperature_msg.data = 0.0;
51-
power_thrusters_current_msg.data.data = (float *)malloc(8 * sizeof(float));
52-
power_batteries_voltage_msg.data.data = (float *)malloc(2 * sizeof(float));
53-
propulsion_microseconds_msg.data.data = (int16_t *)malloc(8 * sizeof(int16_t));
54-
55-
power_thrusters_current_msg.data.size = 8;
56-
power_batteries_voltage_msg.data.size = 2;
57-
propulsion_microseconds_msg.data.size = 8;
58-
59-
temperatureSensor.begin();
60-
adcSensors.begin(true, true, &Wire1);
61-
62-
// Create subscribers and publishers using the MicroROSWrapper class
63-
propulsion_microseconds_subscriber = micro_ros.createSubscriber(
64-
"/propulsion/microseconds",
65-
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16MultiArray),
66-
propulsion_microseconds_callback
67-
);
68-
69-
power_thrusters_current_publisher = micro_ros.createPublisher(
70-
"/power/thrusters/current",
71-
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray)
72-
);
73-
74-
power_batteries_voltage_publisher = micro_ros.createPublisher(
75-
"/power/batteries/voltage",
76-
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray)
77-
);
78-
79-
power_board_temprature_publisher = micro_ros.createPublisher(
80-
"/power/board/temperature",
81-
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32)
82-
);
83-
84-
power_teensy_temprature_publisher = micro_ros.createPublisher(
85-
"/power/teensy/temperature",
86-
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32)
87-
);
43+
bool create_entities() {
44+
micro_ros.init("power_node");
45+
46+
// Create subscribers and publishers using the MicroROSWrapper class
47+
propulsion_microseconds_subscriber = micro_ros.createSubscriber(
48+
"/propulsion/microseconds",
49+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16MultiArray),
50+
propulsion_microseconds_callback
51+
);
52+
53+
power_thrusters_current_publisher = micro_ros.createPublisher(
54+
"/power/thrusters/current",
55+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray)
56+
);
57+
58+
power_batteries_voltage_publisher = micro_ros.createPublisher(
59+
"/power/batteries/voltage",
60+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray)
61+
);
62+
63+
power_board_temperature_publisher = micro_ros.createPublisher(
64+
"/power/board/temperature",
65+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32)
66+
);
67+
68+
power_teensy_temperature_publisher = micro_ros.createPublisher(
69+
"/power/teensy/temperature",
70+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32)
71+
);
72+
return true;
8873
}
8974

90-
void power_loop() {
91-
power_board_temperature_msg.data = temperatureSensor.readTemperature();
92-
micro_ros.publishData(&power_board_temperature_msg, *power_board_temprature_publisher); // Dereference pointer
75+
void publishAndSubscribe() {
76+
power_board_temperature_msg.data = temperatureSensor.readTemperature();
77+
micro_ros.publishData(&power_board_temperature_msg, *power_board_temperature_publisher); // Dereference pointer
9378

94-
power_teensy_temperature_msg.data = tempmonGetTemp();
95-
micro_ros.publishData(&power_teensy_temperature_msg, *power_teensy_temprature_publisher); // Dereference pointer
79+
power_teensy_temperature_msg.data = tempmonGetTemp();
80+
micro_ros.publishData(&power_teensy_temperature_msg, *power_teensy_temperature_publisher); // Dereference pointer
9681

97-
float* current_data = adcSensors.senseCurrent();
98-
for (size_t i = 0; i < 8; i++) {
99-
power_thrusters_current_msg.data.data[i] = current_data[i];
100-
}
101-
micro_ros.publishData(&power_thrusters_current_msg, *power_thrusters_current_publisher); // Dereference pointer
82+
float* current_data = adcSensors.senseCurrent();
83+
for (size_t i = 0; i < 8; i++) {
84+
power_thrusters_current_msg.data.data[i] = current_data[i];
85+
}
86+
micro_ros.publishData(&power_thrusters_current_msg, *power_thrusters_current_publisher); // Dereference pointer
10287

103-
float* voltage_data = adcSensors.senseVoltage();
104-
for (size_t i = 0; i < 2; i++) {
105-
power_batteries_voltage_msg.data.data[i] = voltage_data[i];
106-
}
107-
micro_ros.publishData(&power_batteries_voltage_msg, *power_batteries_voltage_publisher); // Dereference pointer
88+
float* voltage_data = adcSensors.senseVoltage();
89+
for (size_t i = 0; i < 2; i++) {
90+
power_batteries_voltage_msg.data.data[i] = voltage_data[i];
91+
}
92+
micro_ros.publishData(&power_batteries_voltage_msg, *power_batteries_voltage_publisher); // Dereference pointer
93+
}
10894

109-
updateThrusters(microseconds);
95+
void power_setup() {
96+
pinMode(LED_PIN, OUTPUT);
97+
digitalWrite(LED_PIN, HIGH);
98+
99+
initThrusters();
100+
101+
adcSensors.begin(ENABLE_VOLTAGE_SENSE, ENABLE_CURRENT_SENSE, &Wire1);
102+
temperatureSensor.begin();
103+
104+
// init all messages
105+
power_board_temperature_msg.data = 0.0;
106+
power_teensy_temperature_msg.data = 0.0;
107+
power_thrusters_current_msg.data.data = (float *)malloc(8 * sizeof(float));
108+
power_batteries_voltage_msg.data.data = (float *)malloc(2 * sizeof(float));
109+
propulsion_microseconds_msg.data.data = (int16_t *)malloc(8 * sizeof(int16_t));
110+
111+
power_thrusters_current_msg.data.size = 8;
112+
power_batteries_voltage_msg.data.size = 2;
113+
propulsion_microseconds_msg.data.size = 8;
114+
115+
create_entities();
116+
}
110117

118+
void power_loop() {
119+
if (micro_ros.pingAgent()) {
120+
publishAndSubscribe();
111121
micro_ros.spin();
122+
updateThrusters(microseconds);
123+
124+
} else {
125+
updateThrusters(offCommand);
126+
micro_ros.destroyMicroROS();
127+
128+
while (micro_ros.pingAgent()) {
129+
130+
}
131+
create_entities();
132+
}
133+
134+
delay(10);
112135
}

0 commit comments

Comments
 (0)