Note: This tutorial assumes that you have completed the previous tutorials: Tivaware and toolchain setup. |
Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Buttons publisher
Description: Buttons state publisher for the EK-TM4C123GXL board over CDC USB device.Tutorial Level: INTERMEDIATE
Next Tutorial: RGB subscriber
Contents
Introduction
This particular tutorial follows the same general steps as the previous two.
However on this project we will use a custom message to publish the user button state on the EK-TM4C123GXL.
Another change from the previous projects, is the use of the device USB port. When connected to the computer, the board will enumerate as a CDC usb device.
For this tutorial you must get the reference files which can be found on rosserial_tivac_tutorials.
Custom message generation
Let's create a new file message on the new directory msgs/Buttons.msg.
std_msgs/Bool sw1 std_msgs/Bool sw2
package.xml & CMakeLists.txt
On the package's package.xml and top level CMakeLists.txt we will add new dependencies and rules to generate our new message.
package.xml
Add new dependencies on package.xml:
<build_depend>message_generation</build_depend> <run_depend>message_runtime</run_depend>
CMakeLists.xml
Add the new package message_generation dependency to find_package:
(...) find_package(catkin REQUIRED COMPONENTS message_generation rosserial_client rosserial_tivac std_msgs ) add_message_files( FILES Buttons.msg ) generate_messages( DEPENDENCIES std_msgs ) catkin_package( CATKIN_DEPENDS message_runtime std_msgs ) (...)
As well as instructions to generate the new message files.
Also add the new subproject.
(...) # Custom message - Button publisher rosserial_configure_client( DIRECTORY buttons TOOLCHAIN_FILE ${ROSSERIAL_TIVAC_TOOLCHAIN} ) rosserial_add_client_target(buttons buttons.axf ALL) rosserial_add_client_target(buttons flash) rosserial_add_client_target(buttons size) (...)
Source code
Similarly to the previous tutorials, we'll create a publisher for the new Buttons message.
1 #include <stdbool.h>
2 #include <stdint.h>
3 // TivaC specific includes
4 extern "C"
5 {
6 #include <driverlib/sysctl.h>
7 #include <driverlib/gpio.h>
8 #include "buttons.h"
9 }
10 // ROS includes
11 #include <ros.h>
12 // Custom ROS message
13 #include "rosserial_tivac_tutorials/Buttons.h"
14
15 // ROS nodehandle
16 ros::NodeHandle nh;
17
18 rosserial_tivac_tutorials::Buttons button_msg;
19 ros::Publisher button_publisher("button_state", &button_msg);
20
21 int main(void)
22 {
23 // TivaC application specific code
24 MAP_FPUEnable();
25 MAP_FPULazyStackingEnable();
26 // TivaC system clock configuration. Set to 80MHz.
27 MAP_SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
28
29 uint8_t button_debounced_delta;
30 uint8_t button_raw_state;
31 ButtonsInit();
32
33 // ROS nodehandle initialization and topic registration
34 nh.initNode();
35 nh.advertise(button_publisher);
36
37 while (1)
38 {
39 uint8_t button_debounced_state = ButtonsPoll(&button_debounced_delta, &button_raw_state);
40 // Publish message to be transmitted.
41 button_msg.sw1.data = button_debounced_state & LEFT_BUTTON;
42 button_msg.sw2.data = button_debounced_state & RIGHT_BUTTON;
43 button_publisher.publish(&button_msg);
44
45 // Handle all communications and callbacks.
46 nh.spinOnce();
47
48 // Delay for a bit.
49 nh.getHardware()->delay(100);
50 }
51 }
Very few changes comparing to the "hello world" publisher.
We now include the header files corresponding to the Buttons message, and also instantiate the message and publisher for the topic with name /button_state.
MCU specific code to set up the buttons' pins.
37 while (1)
38 {
39 uint8_t button_debounced_state = ButtonsPoll(&button_debounced_delta, &button_raw_state);
40 // Publish message to be transmitted.
41 button_msg.sw1.data = button_debounced_state & LEFT_BUTTON;
42 button_msg.sw2.data = button_debounced_state & RIGHT_BUTTON;
43 button_publisher.publish(&button_msg);
44
45 // Handle all communications and callbacks.
46 nh.spinOnce();
47
48 // Delay for a bit.
49 nh.getHardware()->delay(100);
50 }
On the main loop, we read the button state using the function ButtonsPoll, and with the result we fill up the message before publishing.
The message is then queued when .publish is executed and transmitted when .spinOnce() is called.
Build & flash
catkin_make rosserial_tivac_tutorials_buttons_buttons.axf catkin_make rosserial_tivac_tutorials_buttons_flash
Test it up
Hold on! Don't forget to connect the device USB port to your computer!
You should be able to see the device under the name /dev/ttyACM#, depending on how many devices you have connected.
Run roscore and run serial_node.py from rosserial_python on the correct port /dev/ttyACM#.
roscore
rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0
rostopic echo /button_state
Press any of the two buttons, and watch the output of the topic change.