## For instruction on writing tutorials ## http://www.ros.org/wiki/WritingTutorials #################################### ##FILL ME IN #################################### ## for a custom note with links: ## note = ## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links ## note.0=[[rosserial_tivac/Tutorials/TivaWare Setup|Tivaware and toolchain setup]] ## descriptive title for the tutorial ## title = RGB subscriber ## multi-line description to be displayed in search ## description = EK-TM4C123GXL board over CDC USB device subscribes to ColorRGB msg type and change LED color. ## the next tutorial description (optional) ## next = ## links to next tutorial (optional) ## next.0.link= [[rosserial_tivac/Tutorials/RGB srv|RGB service]] ## next.1.link= ## what level user is this tutorial for ## level= IntermediateCategory ## keywords = #################################### <> <> ##startpage == Introduction == Having created several publishers on the previous tutorials, its about time we implement a subscriber. Our application will subscribe to the topic `/led` and change the color accordingly to the data on the ColorRGBA message. == Source code explained == {{{ #!cplusplus block=srccode #include #include // TivaC specific includes extern "C" { #include #include #include } // ROS includes #include #include uint32_t current_color[3] = {0xFFFE, 0xFFFE, 0xFFFE}; float current_intensity = 1.f; void color_cb(const std_msgs::ColorRGBA& msg) { current_color[0] = msg.r * 0xFFFE; current_color[1] = msg.g * 0xFFFE; current_color[2] = msg.b * 0xFFFE; current_intensity = msg.a; RGBSet(current_color, current_intensity); } // ROS nodehandle and subscriber ros::NodeHandle nh; ros::Subscriber rgb_sub("led", &color_cb); int main(void) { // TivaC application specific code MAP_FPUEnable(); MAP_FPULazyStackingEnable(); // TivaC system clock configuration. Set to 80MHz. MAP_SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN); // Setup RGB driver RGBInit(0); RGBSet(current_color, current_intensity); // ROS nodehandle initialization and topic registration nh.initNode(); nh.subscribe(rgb_sub); bool nh_prev_state = false; while (1) { // If subscribed, enable RGB driver if (nh.connected() && !nh_prev_state) { RGBEnable(); nh_prev_state = true; } if (!nh.connected() && nh_prev_state) { RGBDisable(); nh_prev_state = false; } // Handle all communications and callbacks. nh.spinOnce(); // Delay for a bit. nh.getHardware()->delay(1000); } } }}} Again, use the project `rgb_led` from `rosserial_tivac_tutorials package as reference to follow the tutorial. <> As always, the node is initiated and all the publishers and subscribers must be registered. <> Here we instantiate a new subscriber on the topic `/led` of message type `ColorRGBA`, handled by `color_cb` function. <> The callback function will be executed for each new received message. It takes color and intensity values in the range [0 1] and applies the color to the board LED using the board specific library. <> Then execution is trapped inside a loop, continuously waiting for new messages on the topic. However the LED will only be enabled when a valid node connection is present. == Build and flash == By now you're an expert at this. Build and flash the binary file into your board. {{{ catkin_make catkin_make rosserial_tivac_tutorials_rgb_led_flash }}} == Test it! == Don't forget to connect the device USB port to your computer! Run `roscore` and `run serial_node.py` from `rosserial_python` on the correct port `/dev/ttyACM#`. {{{ roscore }}} When you successfully connect using `serial_node the LED will light up. {{{ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 }}} On a new terminal window, publish a new color. {{{ rostopic pub /led std_msgs/ColorRGBA "r: 0.0 g: 1.0 b: 0.0 a: 1.0" }}} It should change to the brightest green. ##endpage ## AUTOGENERATED DO NOT DELETE ## TutorialCategory ## FILL IN THE STACK TUTORIAL CATEGORY HERE