0
$\begingroup$

Rosanswers logo

I am using an analog sensor to control the LED. I have modified two example codes (IR range sensor and blink tutorials) in rosserial/Arduino/tutorials to build the control system. the code i did was shown below. How can i get the publisher value and use it in my "If" statement. my objective is to glow the LED when analog sensor output is greater than 700

#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 >= 700) {
 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);
 }

  void loop()
 {

 range_msg.range = getRange(analog_pin);
 range_msg.header.stamp = nh.now();
 pub_range.publish(&range_msg);
 range_timer =  millis() + 50;

 nh.spinOnce();
 }

Originally posted by Kishore Kumar on ROS Answers with karma: 173 on 2015-01-31

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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


Originally posted by Wolf with karma: 7555 on 2015-01-31

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by Kishore Kumar on 2015-01-31:
Thank you wolf, the code is doing great but i am getting this error "[WARN] [WallTime: 1422707498.360818] Serial Port read returned short (expected 77 bytes, received 19 instead) " and some time my LED is glowing when my sensor value is less than threshold

Comment by Wolf on 2015-02-01:
I think it's a connection problem --> new issue. Maybe due to the connection problem some of your messages get lost so the LED keeps glowing.... Plz open a new thread and provide some more info (operating system, device, rosserial version etc.)

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.