Skip to content

Commit 608f2f9

Browse files
JPGC04JPGC04
committed
simplfiied wrapper, stores array of pub and sub, uses pointers in main
1 parent 8be7360 commit 608f2f9

File tree

3 files changed

+75
-86
lines changed

3 files changed

+75
-86
lines changed

‎pio_workspace/lib/micro_ros_wrapper/micro_ros_wrapper.cpp‎

Lines changed: 15 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -2,21 +2,6 @@
22

33
#include <Arduino.h>
44

5-
// Publisher Implementation
6-
Publisher::Publisher(rcl_node_t* node, const char* topic_name, const rosidl_message_type_support_t* type_support) {
7-
RCCHECK(rclc_publisher_init_default(&publisher_, node, type_support, topic_name));
8-
}
9-
10-
void Publisher::publish(const void* msg) {
11-
RCSOFTCHECK(rcl_publish(&publisher_, msg, NULL));
12-
}
13-
14-
// Subscriber Implementation
15-
Subscriber::Subscriber(rcl_node_t* node, const char* topic_name, const rosidl_message_type_support_t* type_support, rclc_executor_t* executor, void (*callback)(const void*)) {
16-
RCCHECK(rclc_subscription_init_default(&subscriber_, node, type_support, topic_name));
17-
RCCHECK(rclc_executor_add_subscription(executor, &subscriber_, nullptr, callback, ON_NEW_DATA));
18-
}
19-
205
// MicroROSWrapper Implementation
216
MicroROSWrapper::MicroROSWrapper(const char* node_name) {
227
set_microros_transports();
@@ -26,19 +11,29 @@ MicroROSWrapper::MicroROSWrapper(const char* node_name) {
2611
RCCHECK(rclc_executor_init(&executor_, &support_.context, 1, &allocator_));
2712
}
2813

29-
Publisher MicroROSWrapper::createPublisher(const char* topic_name, const rosidl_message_type_support_t* type_support) {
30-
return Publisher(&node_, topic_name, type_support);
14+
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
18+
}
19+
20+
21+
void MicroROSWrapper::publishData(const void* msg, rcl_publisher_t &publisher) {
22+
RCSOFTCHECK(rcl_publish(&publisher, msg, NULL));
3123
}
3224

33-
Subscriber MicroROSWrapper::createSubscriber(const char* topic_name, const rosidl_message_type_support_t* type_support, void (*callback)(const void*)) {
34-
return Subscriber(&node_, topic_name, type_support, &executor_, callback);
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];
3530
}
3631

3732
void MicroROSWrapper::spin() {
3833
RCCHECK(rclc_executor_spin_some(&executor_, RCL_MS_TO_NS(100)));
3934
}
4035

41-
void errorLoop() {
36+
void MicroROSWrapper::errorLoop() {
4237
int error = 0;
4338

4439
while (error < 20) {

‎pio_workspace/lib/micro_ros_wrapper/micro_ros_wrapper.h‎

Lines changed: 12 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -11,37 +11,28 @@
1111
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)) { errorLoop(); }}
1212
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)) {} }
1313

14-
void errorLoop();
15-
16-
class Publisher {
17-
public:
18-
Publisher(rcl_node_t* node, const char* topic_name, const rosidl_message_type_support_t* type_support);
19-
void publish(const void* msg);
20-
21-
private:
22-
rcl_publisher_t publisher_;
23-
};
24-
25-
class Subscriber {
26-
public:
27-
Subscriber(rcl_node_t* node, const char* topic_name, const rosidl_message_type_support_t* type_support, rclc_executor_t* executor, void (*callback)(const void*));
28-
29-
private:
30-
rcl_subscription_t subscriber_;
31-
};
32-
3314
class MicroROSWrapper {
3415
public:
3516
MicroROSWrapper(const char* node_name);
3617
void spin();
37-
Publisher createPublisher(const char* topic_name, const rosidl_message_type_support_t* type_support);
38-
Subscriber createSubscriber(const char* topic_name, const rosidl_message_type_support_t* type_support, void (*callback)(const void*));
18+
rcl_publisher_t* createPublisher(const char* topic_name, const rosidl_message_type_support_t* type_support);
19+
rcl_subscription_t* createSubscriber(const char* topic_name, const rosidl_message_type_support_t* type_support, void (*callback)(const void*));
20+
21+
void publishData(const void* msg, rcl_publisher_t &publisher);
3922

4023
private:
4124
rclc_executor_t executor_;
4225
rclc_support_t support_;
4326
rcl_allocator_t allocator_;
4427
rcl_node_t node_;
28+
29+
rcl_publisher_t publishers_[50];
30+
rcl_subscription_t subscriptions_[50];
31+
32+
int publisher_count = 0;
33+
int subscription_count = 0;
34+
35+
void errorLoop();
4536
};
4637

4738
#endif

‎pio_workspace/src/power_main.cpp‎

Lines changed: 48 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -15,48 +15,30 @@
1515

1616
MicroROSWrapper micro_ros("power_node");
1717

18-
void propulsion_microseconds_callback(const void *msgin) {
19-
const std_msgs__msg__Int16MultiArray *propulsion_microseconds_msg = (const std_msgs__msg__Int16MultiArray *)msgin;
20-
// Ensure we don't exceed the size of the `microseconds` array
21-
for (size_t i = 0; i < 8 && i < propulsion_microseconds_msg->data.size; i++) {
22-
microseconds[i] = propulsion_microseconds_msg->data.data[i]; // Access the data correctly
23-
}
24-
}
18+
// Define the subscribers and publishers as rcl_publisher_t and rcl_subscription_t
19+
rcl_subscription_t* propulsion_microseconds_subscriber;
20+
rcl_publisher_t* power_thrusters_current_publisher;
21+
rcl_publisher_t* power_batteries_voltage_publisher;
22+
rcl_publisher_t* power_board_temprature_publisher;
23+
rcl_publisher_t* power_teensy_temprature_publisher;
2524

26-
Subscriber propulsion_microseconds_subscriber = micro_ros.createSubscriber(
27-
"/propulsion/microseconds",
28-
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16MultiArray),
29-
propulsion_microseconds_callback);
30-
3125
std_msgs__msg__Int16MultiArray propulsion_microseconds_msg;
32-
33-
Publisher power_thrusters_current_publisher = micro_ros.createPublisher(
34-
"/power/thrusters/current",
35-
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray));
36-
3726
std_msgs__msg__Float32MultiArray power_thrusters_current_msg;
38-
39-
Publisher power_batteries_voltage_publisher = micro_ros.createPublisher(
40-
"/power/batteries/voltage",
41-
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray));
42-
4327
std_msgs__msg__Float32MultiArray power_batteries_voltage_msg;
44-
45-
Publisher power_board_temprature_publisher = micro_ros.createPublisher(
46-
"/power/board/temperature",
47-
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32));
48-
4928
std_msgs__msg__Float32 power_board_temperature_msg;
50-
51-
Publisher power_teensy_temprature_publisher = micro_ros.createPublisher(
52-
"/power/teensy/temperature",
53-
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32));
54-
5529
std_msgs__msg__Float32 power_teensy_temperature_msg;
5630

5731
TMP36 temperatureSensor(23, 3.3);
5832
ADCSensors adcSensors;
5933

34+
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+
}
40+
}
41+
6042
void power_setup() {
6143
pinMode(LED_PIN, OUTPUT);
6244
digitalWrite(LED_PIN, HIGH);
@@ -65,43 +47,64 @@ void power_setup() {
6547

6648
// init all messages
6749
power_board_temperature_msg.data = 0.0;
68-
6950
power_teensy_temperature_msg.data = 0.0;
70-
7151
power_thrusters_current_msg.data.data = (float *)malloc(8 * sizeof(float));
72-
propulsion_microseconds_msg.data.size = 8;
73-
propulsion_microseconds_msg.data.capacity = 8;
74-
7552
power_batteries_voltage_msg.data.data = (float *)malloc(2 * sizeof(float));
76-
power_batteries_voltage_msg.data.size = 2;
77-
power_batteries_voltage_msg.data.capacity = 2;
78-
7953
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;
8057
propulsion_microseconds_msg.data.size = 8;
81-
propulsion_microseconds_msg.data.capacity = 8;
8258

8359
temperatureSensor.begin();
8460
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+
);
8588
}
8689

8790
void power_loop() {
8891
power_board_temperature_msg.data = temperatureSensor.readTemperature();
89-
power_board_temprature_publisher.publish(&power_board_temperature_msg);
92+
micro_ros.publishData(&power_board_temperature_msg, *power_board_temprature_publisher); // Dereference pointer
9093

9194
power_teensy_temperature_msg.data = tempmonGetTemp();
92-
power_teensy_temprature_publisher.publish(&power_teensy_temperature_msg);
95+
micro_ros.publishData(&power_teensy_temperature_msg, *power_teensy_temprature_publisher); // Dereference pointer
9396

9497
float* current_data = adcSensors.senseCurrent();
9598
for (size_t i = 0; i < 8; i++) {
9699
power_thrusters_current_msg.data.data[i] = current_data[i];
97100
}
98-
power_thrusters_current_publisher.publish(&power_thrusters_current_msg);
101+
micro_ros.publishData(&power_thrusters_current_msg, *power_thrusters_current_publisher); // Dereference pointer
99102

100103
float* voltage_data = adcSensors.senseVoltage();
101104
for (size_t i = 0; i < 2; i++) {
102105
power_batteries_voltage_msg.data.data[i] = voltage_data[i];
103106
}
104-
power_batteries_voltage_publisher.publish(&power_batteries_voltage_msg);
107+
micro_ros.publishData(&power_batteries_voltage_msg, *power_batteries_voltage_publisher); // Dereference pointer
105108

106109
updateThrusters(microseconds);
107110

0 commit comments

Comments
 (0)