ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

To solve your issue:

#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/Range.h>
#include <std_msgs/Empty.h>

 ros::NodeHandle  nh;

 void messageCb( const sensor_msgs::Range& range_msg)
 {
 if (range_msg.range >= 700) {                    // NOTE: I changed this
 digitalWrite(13, HIGH-digitalRead(13));   // blink the led
 }

 else {
 digitalWrite(13, LOW-digitalRead(13));
 }
 }
 ros::Subscriber<sensor_msgs::Range> sub("range_data", &messageCb );


 sensor_msgs::Range range_msg;
 ros::Publisher pub_range( "range_data", &range_msg);

 const int analog_pin = 0;
 unsigned long range_timer;

 float getRange(int pin_num){
int sample;

sample = analogRead(pin_num);

 return (sample);
 }

 char frameid[] = "/ir_ranger";

 void setup()
 { 
 pinMode(13, OUTPUT);
 nh.initNode();
 nh.subscribe(sub);
 nh.initNode();
 nh.advertise(pub_range);
 range_timer =  millis();                    // NOTE: I added this 
}

  void loop()
 {
 // NOTE: I changed this to really throttle your publisher with the timeout
if ( range_timer  + 50 <  millis() )
{
  range_msg.range = getRange(analog_pin);
  range_msg.header.stamp = nh.now();
  pub_range.publish(&range_msg);
  range_timer =  millis();
}

 nh.spinOnce();
 }

Note that you do not really "need" to include the publisher and subscribe to your own publisher (the node essentially subscribes to itself). You could also put the logic in the messageCb directly into your loop. However, this still should be valid and its of course nice for debugging purposes because you can subscribe the range msgs outside your device....