(!) 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.

Push Button

Description: Monitor a push button and publish its state in ROS

Tutorial Level: BEGINNER

Next Tutorial: Servo Controller

  Show EOL distros: 

This tutorial describes one of the simplest and most common pieces of hardware you might want to integrate into your ROS system: a push button. A button and be an input device to command your robot to do its next task, a sensor to detect of a box is opened, or an important safety feature to stop a motor from moving past a joints limits.

In this tutorial, we will write a rosserial_mbed node that will monitor the state of a normally off button. It will publish a boolean message to the topic "pushed" everytime the button's state changes.

Hardware

For this tutorial, you need an MBED board and some sort of normally off momentary switch or push button.

For those of you who are used to hardware design, you will notice that there is no pull-up resistor on switch input. This is because the mbed has built in pullup resistors. Pin 7 is pulled high until the button is pressed and connected to ground.

Code

The code for this program is below. To use the code, copy it directly into a fresh sketch in the mbed IDE.

   1 /*
   2  * Button Example for Rosserial
   3  */
   4 
   5 #include "mbed.h"
   6 #include <ros.h>
   7 #include <std_msgs/Bool.h>
   8 
   9 #ifdef TARGET_LPC1768
  10 PinName button = p20;
  11 #elif defined(TARGET_KL25Z)
  12 PinName button = D10;
  13 #elif defined(TARGET_NUCLEO_F401RE)
  14 PinName button = USER_BUTTON;
  15 #else
  16 #error "You need to specify a pin for the button"
  17 #endif
  18 
  19 ros::NodeHandle nh;
  20 
  21 std_msgs::Bool pushed_msg;
  22 ros::Publisher pub_button("pushed", &pushed_msg);
  23 
  24 DigitalIn button_pin(button);
  25 DigitalOut led_pin(LED1);
  26 
  27 bool last_reading;
  28 long last_debounce_time=0;
  29 long debounce_delay=50;
  30 bool published = true;
  31 
  32 Timer t;
  33 int main() {
  34     t.start();
  35     nh.initNode();
  36     nh.advertise(pub_button);
  37 
  38     //Enable the pullup resistor on the button
  39     button_pin.mode(PullUp);
  40 
  41     //The button is a normally button
  42     last_reading = ! button_pin;
  43 
  44     while (1) {
  45         bool reading = ! button_pin;
  46 
  47         if (last_reading!= reading) {
  48             last_debounce_time = t.read_ms();
  49             published = false;
  50         }
  51 
  52         //if the button value has not changed for the debounce delay, we know its stable
  53         if ( !published && (t.read_ms() - last_debounce_time)  > debounce_delay) {
  54             led_pin = reading;
  55             pushed_msg.data = reading;
  56             pub_button.publish(&pushed_msg);
  57             published = true;
  58         }
  59 
  60         last_reading = reading;
  61 
  62         nh.spinOnce();
  63     }
  64 }

This code is a typical example of how rosserial_mbed is used. The rosserial objects are globally declared.

   9 #ifdef TARGET_LPC1768
  10 PinName button = p20;
  11 #elif defined(TARGET_KL25Z)
  12 PinName button = D10;
  13 #elif defined(TARGET_NUCLEO_F401RE)
  14 PinName button = USER_BUTTON;
  15 #else
  16 #error "You need to specify a pin for the button"
  17 #endif
  18 

Some preprocessor directives for defining the GPIOs used by the program, depending on which MBED board are you using

Monitoring the Button

This sensor should not spam messages about button's state but should publish an update only when the button is pressed or released. To monitor this change we need to remember the button's last state and know how long this state has been valid. Then once the state has changed, the sensor should debounce the button. It should wait "long enough" so we know that the button value has settled and is not oscillating around. This is because the voltage at the input pin will bounce around (as shown in the diagram below) as the mechanical contacts bounce back and forth during contact and release.

debounce.png

  27 bool last_reading;
  28 long last_debounce_time=0;
  29 long debounce_delay=50;
  30 bool published = true;

Debouncing variables, this approach is used for avoiding false lectures from the push buttons.

  32 Timer t;

Creates a timer "t"

  39     button_pin.mode(PullUp);

Set pin mode as PullUp, which means it connects a weak resistor in between the pin and VCC, so you don't need to connect a resistor from outside.

  45         bool reading = ! button_pin;
  46 
  47         if (last_reading!= reading) {
  48             last_debounce_time = t.read_ms();
  49             published = false;
  50         }

Stores the actual state of the input on a variable and compares it with the last reading before that and reset the published variable to false.

  53         if ( !published && (t.read_ms() - last_debounce_time)  > debounce_delay) {
  54             led_pin = reading;
  55             pushed_msg.data = reading;
  56             pub_button.publish(&pushed_msg);
  57             published = true;
  58         }

Finally the code reads the button at every loop, checks to see if the button state has changed, and then checks to see if the button state is stable and has been published.

  60         last_reading = reading;

Updates the value of the last_reading variable with the actual reading value.

Testing

Startup roscore

roscore

In a new terminal window, run the rosserial_python serial_node.py. Make sure to use the correct serial port.

rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0

rosrun rosserial_python serial_node.py /dev/ttyUSB0

Now watch the button's value on the pushed topic

rostopic echo pushed

Wiki: rosserial_mbed/Tutorials/Push Button (last edited 2015-11-20 17:21:40 by AlexisPojomovsky)