98 lines
2.7 KiB
C++
98 lines
2.7 KiB
C++
#include "led_controller.hpp"
|
|
#include "devices.hpp"
|
|
#include "rcl_util.hpp"
|
|
|
|
#include <zephyr/drivers/gpio.h>
|
|
#include <rcl/rcl.h>
|
|
#include <rclc/rclc.h>
|
|
#include <rclc/executor.h>
|
|
#include <std_msgs/msg/bool.h>
|
|
|
|
std_msgs__msg__Bool led_msg;
|
|
rcl_node_t LedController::node;
|
|
rcl_subscription_t LedController::led0_subscription;
|
|
/*
|
|
rcl_subscription_t LedController::led1_subscription;
|
|
rcl_subscription_t LedController::led2_subscription;
|
|
*/
|
|
|
|
void led0_callback(const void *msg){
|
|
gpio_pin_configure_dt(&led0, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
|
|
}
|
|
|
|
|
|
/*
|
|
void led1_callback(const void *msg){
|
|
gpio_pin_configure_dt(&led1, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
|
|
}
|
|
|
|
|
|
void led2_callback(const void *msg){
|
|
gpio_pin_configure_dt(&led2, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
|
|
}
|
|
*/
|
|
|
|
|
|
rcl_ret_t LedController::setup(rclc_support_t* support, rclc_executor_t* executor) {
|
|
RCL_RETURN_ON_ERROR(rclc_node_init_default(
|
|
&LedController::node,
|
|
"led_controller",
|
|
"",
|
|
support));
|
|
|
|
RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
|
|
&LedController::led0_subscription,
|
|
&LedController::node,
|
|
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
|
|
"/nucleo/led0"));
|
|
|
|
/*
|
|
RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
|
|
&LedController::led1_subscription,
|
|
&LedController::node,
|
|
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
|
|
"/nucleo/led1"));
|
|
|
|
RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
|
|
&LedController::led2_subscription,
|
|
&LedController::node,
|
|
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
|
|
"/nucleo/led2"));
|
|
*/
|
|
|
|
RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
|
|
executor,
|
|
&LedController::led0_subscription,
|
|
&led_msg,
|
|
led0_callback,
|
|
ON_NEW_DATA
|
|
));
|
|
|
|
/*
|
|
RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
|
|
executor,
|
|
&LedController::led1_subscription,
|
|
&led_msg,
|
|
led1_callback,
|
|
ON_NEW_DATA
|
|
));
|
|
|
|
RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
|
|
executor,
|
|
&LedController::led2_subscription,
|
|
&led_msg,
|
|
led2_callback,
|
|
ON_NEW_DATA
|
|
));
|
|
*/
|
|
|
|
return RCL_RET_OK;
|
|
}
|
|
|
|
|
|
void LedController::kill() {
|
|
rcl_subscription_fini(&LedController::led0_subscription, &LedController::node);
|
|
//rcl_subscription_fini(&LedController::led1_subscription, &LedController::node);
|
|
//rcl_subscription_fini(&LedController::led2_subscription, &LedController::node);
|
|
rcl_node_fini(&LedController::node);
|
|
} |