ROS2 JETSON NANO - TU PRIMER NODO PYTHON MICROROS

[@IN3DMEX2.0]

#include #include #include #include #include #include #include rcl_subscription_t LED_subscriber; std_msgs__msg__Int8 LED_msg; rclc_executor_t executor; rclc_support_t support; rcl_allocator_t allocator; rcl_node_t node; #define LED_PIN 2 #define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}} #define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}} void error_loop(){ while(1){ delay(100); } } void LED_subscription_callback(const void * msgin) { const std_msgs__msg__Int8 * msg = (const std_msgs__msg__Int8 *)msgin; digitalWrite(LED_PIN, msg->data == 1 ? HIGH : LOW); } void setup() { //Nombre de su red, Contraseña de su red, IP del JETSON NANO, 8888 set_microros_wifi_transports("XXX", "XXXXXX", "192.168.XX.XXX", 8888); pinMode(LED_PIN, OUTPUT); digitalWrite(LED_PIN, LOW); allocator = rcl_get_default_allocator(); // Create init_options RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); // Create node RCCHECK(rclc_node_init_default(&node, "lineturtle_esp32", "", &support)); // Create subscriber RCCHECK(rclc_subscription_init_default( &LED_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int8), "control_led")); // Create executor RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator)); RCCHECK(rclc_executor_add_subscription(&executor, &LED_subscriber, &LED_msg, &LED_subscription_callback, ON_NEW_DATA)); } void loop() { delay(100); RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100))); }

[@IN3DMEX2.0]

import rclpy from rclpy.node import Node from std_msgs.msg import Int8 import sys, select, termios, tty class LedController(Node): def __init__(self): super().__init__('led_controller') self.publisher_ = self.create_publisher(Int8, 'control_led', 10) self.led_state = 0 self.get_logger().info('Presiona "p" para encender el LED y "a" para apagarlo') def publish_led_state(self, state): msg = Int8() msg.data = state self.publisher_.publish(msg) self.get_logger().info('LED State: "%s"' % msg.data) def get_key(): tty.setraw(sys.stdin.fileno()) select.select([sys.stdin], [], [], 0) key = sys.stdin.read(1) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, termios.tcgetattr(sys.stdin)) return key def main(args=None): rclpy.init(args=args) led_controller = LedController() try: while rclpy.ok(): key = get_key() if key == 'p': led_controller.publish_led_state(1) elif key == 'a': led_controller.publish_led_state(0) elif key == '\x03': # Ctrl+C para salir break except Exception as e: print(e) finally: led_controller.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()