blink.c

#include <stdio.h>

#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rcl/error_handling.h>
#include <std_msgs/msg/int32.h>
#include <rclc/executor.h>
#include <rmw_microros/rmw_microros.h>

#include "pico/stdlib.h"
#include "pico_uart_transports.h"


#include "boards/pico_w.h"    	// pico w or wh
#include "pico/cyw43_arch.h"	// pico w or wh


//const uint LED_PIN = 25; 		// Step 1 : this is the number of the GPIO port that the onboard LED in mounted (for classic pico) 

rcl_subscription_t subscriber;	// Step 2 : we include the rcl component of the rcl library to create a subscriber object
std_msgs__msg__Int32 msg; 		// Step 3 : we include the std_msgs_msg to define the message type our subscriber listens to





void subscription_callback(const void * msgin){		// Step 9 : define the subscriber callback function
	const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;

	if (msg->data == 0){
		cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, false);	// Set pin LOW (= LED IS OFF) for pico w or wh
		// gpio_put(LED_PIN, 0);  // Set pin LOW (= LED IS OFF) for classic pico
		sleep_ms(1000); 
	}
	else {
		cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true);	// Set pin HIGH (= LED IS ON) for pico w or wh 
		// gpio_put(LED_PIN, 1);  // Set pin HIGH (= LED IS ON) for classic pico
		sleep_ms(1000);
	}

}




int main()
{

	// -----------SETTING UP THE PICO CCONNECTION-----------


    rmw_uros_set_custom_transport(      // Step 4 : this function is used to determine the protocol by which the pico works
		true,                           // in terms of connectivity. For example it defines whether the micro ros app will connect to                            
		NULL,                           // the micro ros agent through USB or Wifi if the pico aupports it. These are defined in the  
		pico_serial_transport_open,     // pico_uart_transport.c of the precompiled pico library. We include this file to have access
		pico_serial_transport_close,    // to the functions and also, we include the <rmw_microros/rmw_microros.h> to set them.
		pico_serial_transport_write,
		pico_serial_transport_read
	);


   
    const int timeout_ms = 1000;		// Step 5 : Wait for agent successful ping for 2 minutes (arbitrary)
    const uint8_t attempts = 120;

	rcl_ret_t ret = rmw_uros_ping_agent(timeout_ms, attempts);	// Step 6 : we create a return code value so we can check it afterwwards. In case of 
																// a success code is returned, the program has connected with the ROS2 agent, else the 
    if (ret != RCL_RET_OK)										// connecttion has failed. For that, we include the error handling component of the rcl
    {															// library
        // Unreachable agent, exiting program
        return ret;
    }



	// -----------PROGRAM LOGIC-------------

    cyw43_arch_init();                 		// Step 7 : we initializee the wifi driver ffor theleed pin (pico w or wh)
	//gpio_init(LED_PIN);					// Step 7 : we initialize the GPIO function on the Pin. This way we can access the led with  gpio 	
    //gpio_set_dir(LED_PIN, GPIO_OUT);    	// funcions. Then, we set the pin as output (classic pico)

	// Step 8 : we will now create the ssubscriber node using micro ros
    
	rcl_allocator_t allocator;					// Allocate the memory needed for the node
	allocator = rcl_get_default_allocator();	// Initialize
    
	rclc_support_t support;						// Create the support object (include the rclc component of  the rclc library) 
	rclc_support_init(&support, 0, NULL, &allocator);	// Initialize
	
	rcl_node_t node;							// Create the node object
	rclc_node_init_default(&node, "pico_node", "", &support);	// Initialize

	rclc_subscription_init_default(								//Initilize a subscriber node which is subscribed to the "pico blink subscriber" topic
		&subscriber,
		&node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
		"pico_blink_subscriber");								// names of topics must not have spaces!!!

	
	rclc_executor_t executor;		// Create an executor object (include the executor component of the rclc library)
	rclc_executor_init(&executor, &support.context, 1, &allocator);			// Initilize
	rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA); // Here we point the executor to run the subscipption callback function. We define it outside of main()



	//  Step 10 : Create a while loop in order for the subscriber node to constantly run

	while (true)
	{
		rclc_executor_spin(&executor);
	}
	
	rcl_subscription_fini(&subscriber, &node);	// When (or if) the publissher stops running, its a good practice that the memory allocated be freed
	
    return 0;
}