(!) 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: CMake

  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_arduino node that will monitor the state of a normally off button on pin 7. It will publish a boolean message to the topic "pushed" everytime the button's state changes.

Hardware

For this tutorial, you need an Arduino and some sort of normally off momentary switch or push button. This microswitch from Sparkfun is a good choice.

button_hardware.png

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 Arduino 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 Arduino IDE.

   1 /* 
   2  * Button Example for Rosserial
   3  */
   4 
   5 #include <ros.h>
   6 #include <std_msgs/Bool.h>
   7 
   8 
   9 ros::NodeHandle nh;
  10 
  11 std_msgs::Bool pushed_msg;
  12 ros::Publisher pub_button("pushed", &pushed_msg);
  13 
  14 const int button_pin = 7;
  15 const int led_pin = 13;
  16 
  17 bool last_reading;
  18 long last_debounce_time=0;
  19 long debounce_delay=50;
  20 bool published = true;
  21 
  22 void setup()
  23 {
  24   nh.initNode();
  25   nh.advertise(pub_button);
  26   
  27   //initialize an LED output pin 
  28   //and a input pin for our push button
  29   pinMode(led_pin, OUTPUT);
  30   pinMode(button_pin, INPUT);
  31   
  32   //Enable the pullup resistor on the button
  33   digitalWrite(button_pin, HIGH);
  34   
  35   //The button is a normally button
  36   last_reading = ! digitalRead(button_pin);
  37  
  38 }
  39 
  40 void loop()
  41 {
  42   
  43   bool reading = ! digitalRead(button_pin);
  44   
  45   if (last_reading!= reading){
  46       last_debounce_time = millis();
  47       published = false;
  48   }
  49   
  50   //if the button value has not changed during the debounce delay
  51   // we know it is stable
  52   if ( !published && (millis() - last_debounce_time)  > debounce_delay) {
  53     digitalWrite(led_pin, reading);
  54     pushed_msg.data = reading;
  55     pub_button.publish(&pushed_msg);
  56     published = true;
  57   }
  58 
  59   last_reading = reading;
  60   
  61   nh.spinOnce();
  62 }

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

   9 ros::NodeHandle nh;
  10 
  11 std_msgs::Bool pushed_msg;
  12 ros::Publisher pub_button("pushed", &pushed_msg);

Initialized in the setup() function

  24   nh.initNode();
  25   nh.advertise(pub_button);

and then publishing is performed in the "void loop()" when the button's state changes.

  55     pub_button.publish(&pushed_msg);

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

In the code, this debouncing is performed is a few places. First, there are global variables storing the button's last value, the time that reading was taken, the delay period for the debouncing, and if it was published.

  17 bool last_reading;
  18 long last_debounce_time=0;
  19 long debounce_delay=50;
  20 bool published = true;

Next, in the void setup(), we initialize the arduino pins for io and pullup resistors. We also initialize our state variables.

  27   //initialize an LED output pin 
  28   //and a input pin for our push button
  29   pinMode(led_pin, OUTPUT);
  30   pinMode(button_pin, INPUT);
  31   
  32   //Enable the pullup resistor on the button
  33   digitalWrite(button_pin, HIGH);
  34   
  35   //The button is a normally button
  36   last_reading = ! digitalRead(button_pin);

Finally, in void loop(), 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.

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_arduino/Tutorials/Push Button (last edited 2014-09-22 06:11:42 by AustinHendrix)