Skip to content

Commit b1b8198

Browse files
committed
implementation of microros wrapper - to test #78
1 parent 6457f25 commit b1b8198

File tree

5 files changed

+143
-138
lines changed

5 files changed

+143
-138
lines changed
Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
#include "micro_ros_wrapper.h"
2+
3+
#include <Arduino.h>
4+
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+
20+
// MicroROSWrapper Implementation
21+
MicroROSWrapper::MicroROSWrapper(const char* node_name) {
22+
set_microros_transports();
23+
allocator_ = rcl_get_default_allocator();
24+
RCCHECK(rclc_support_init(&support_, 0, NULL, &allocator_));
25+
RCCHECK(rclc_node_init_default(&node_, node_name, "", &support_));
26+
RCCHECK(rclc_executor_init(&executor_, &support_.context, 1, &allocator_));
27+
}
28+
29+
Publisher MicroROSWrapper::createPublisher(const char* topic_name, const rosidl_message_type_support_t* type_support) {
30+
return Publisher(&node_, topic_name, type_support);
31+
}
32+
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);
35+
}
36+
37+
void MicroROSWrapper::spin() {
38+
RCCHECK(rclc_executor_spin_some(&executor_, RCL_MS_TO_NS(100)));
39+
}
40+
41+
void errorLoop() {
42+
int error = 0;
43+
44+
while (error < 20) {
45+
digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
46+
delay(100);
47+
48+
error++;
49+
}
50+
51+
digitalWrite(LED_BUILTIN, HIGH);
52+
}
Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
#ifndef MICRO_ROS_WRAPPER_H
2+
#define MICRO_ROS_WRAPPER_H
3+
4+
#include <micro_ros_arduino.h>
5+
6+
#include <rcl/rcl.h>
7+
#include <rcl/error_handling.h>
8+
#include <rclc/rclc.h>
9+
#include <rclc/executor.h>
10+
11+
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)) { errorLoop(); }}
12+
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)) {} }
13+
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+
33+
class MicroROSWrapper {
34+
public:
35+
MicroROSWrapper(const char* node_name);
36+
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*));
39+
40+
private:
41+
rclc_executor_t executor_;
42+
rclc_support_t support_;
43+
rcl_allocator_t allocator_;
44+
rcl_node_t node_;
45+
};
46+
47+
#endif

pio_workspace/lib/power_custom/power_micro_ros.cpp

Lines changed: 0 additions & 86 deletions
This file was deleted.

pio_workspace/lib/power_custom/power_micro_ros.h

Lines changed: 0 additions & 37 deletions
This file was deleted.

pio_workspace/src/power_main.cpp

Lines changed: 44 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -1,34 +1,63 @@
11
#include "power_main.h"
22

3-
#include "TMP36.h"
4-
#include "adc_sensors.h"
3+
#include <Arduino.h>
4+
55
#include "ThrusterControl.h"
6-
#include "power_micro_ros.h"
6+
#include "adc_sensors.h"
7+
#include "TMP36.h"
8+
#include "micro_ros_wrapper.h"
9+
10+
#include <std_msgs/msg/int16_multi_array.h>
11+
#include <std_msgs/msg/float32.h>
712

813
#define LED_PIN 13
9-
#define TEMP_SENSE 23
1014

11-
TMP36 temperature_sensor(TEMP_SENSE, 3.3);
15+
MicroROSWrapper micro_ros("power_node");
16+
17+
void propulsion_microseconds_callback(const void *msgin) {
18+
const std_msgs__msg__Int16MultiArray *propulsion_microseconds_msg = (const std_msgs__msg__Int16MultiArray *)msgin;
19+
// Ensure we don't exceed the size of the `microseconds` array
20+
for (size_t i = 0; i < 8 && i < propulsion_microseconds_msg->data.size; i++) {
21+
microseconds[i] = propulsion_microseconds_msg->data.data[i]; // Access the data correctly
22+
}
23+
}
24+
25+
Publisher power_board_temprature_publisher = micro_ros.createPublisher(
26+
"/power/board/temperature",
27+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32));
28+
29+
std_msgs__msg__Float32 power_board_temperature_msg;
30+
31+
Subscriber propulsion_microseconds_subscriber = micro_ros.createSubscriber(
32+
"/propulsion/microseconds",
33+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16MultiArray),
34+
propulsion_microseconds_callback);
35+
36+
std_msgs__msg__Int16MultiArray propulsion_microseconds_msg;
37+
38+
TMP36 temperatureSensor(23, 3.3);
1239

1340
void power_setup() {
1441
pinMode(LED_PIN, OUTPUT);
1542
digitalWrite(LED_PIN, HIGH);
1643

1744
initThrusters();
1845

19-
micro_ros_init();
46+
// init all messages
47+
power_board_temperature_msg.data = 0.0;
48+
49+
propulsion_microseconds_msg.data.data = (int16_t *)malloc(8 * sizeof(int16_t));
50+
propulsion_microseconds_msg.data.size = 8;
51+
propulsion_microseconds_msg.data.capacity = 8;
2052

21-
temperature_sensor.begin();
22-
23-
delay(2000);
53+
temperatureSensor.begin();
2454
}
2555

2656
void power_loop() {
27-
updateThrusters(microseconds);
57+
power_board_temperature_msg.data = temperatureSensor.readTemperature();
58+
power_board_temprature_publisher.publish(&power_board_temperature_msg);
2859

29-
power_board_temperature_publish(temperature_sensor.readTemperature());
60+
updateThrusters(microseconds);
3061

31-
spin_micro_ros();
32-
33-
delay(10);
34-
}
62+
micro_ros.spin();
63+
}

0 commit comments

Comments
 (0)