Ask Your Question
0

How to use publish and use multiple sonar range_msg data (using rosserial) on a single topic?

asked 2019-01-24 11:45:47 -0600

geistmate gravatar image

updated 2019-01-24 12:17:23 -0600

jayess gravatar image

Hi,

I am learning ROS by developing a differential drive collision avoidance robot. (Specifically the Parallax ARLO Robot). I have written a code that can publish one ping sensor's range value to a node, and then have the Arduino subscribe to that node and read the data and execute it's if loop based on that data.

My question is, how do I publish multiple range messages to a topic and decide which ultrasonic sensor value should be used when subscribing to that topic?

Below is the current working code. What I'm unsure how to figure out is: in the callback function, the distance (this is the front sensor) is equal to range_msg.range. So what if I have multiple range messages, and don't know how to define which distance belongs to each sensor?

For example,

distance_left = range_msg.range
distance_right = range_msg.range

and so on is all I know how to do as of now. I know how to publish multiple messages to that range_msg but I my understanding of subscribing is still very weak. Thank you very much for your help, and I am very new to this ros community so please let me know if there are ways I can improve my question.

#include <ArloRobot.h>      // Include ARLO Library
#include <SoftwareSerial.h> // Include SoftwareSerial Library
#include <Servo.h>
#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/Range.h>

// Create Arlo and Serial Objects
ArloRobot Arlo;                                       // Arlo Object
SoftwareSerial ArloSerial(12, 13);                    // Serial in I/O 12, 
Servo servo;

ros::NodeHandle  nh;

sensor_msgs::Range range_msg;

long duration;
int distance;

/**** Motor Callback Function ****/
void motor_cb( const sensor_msgs::Range& range_msg){
  distance = range_msg.range; 

  if (distance < 30) {
    Arlo.writeMotorPower(0,0);
  }
  else {
    Arlo.writeMotorPower(30,30);
  }
}

ros::Publisher pub_range1("/ultrasound1", &range_msg);
ros::Publisher pub_range2("/ultrasound2", &range_msg);
ros::Subscriber<sensor_msgs::Range> sub_range("/ultrasound1", motor_cb);

char frameid[] = "/ultrasound";

void setup()
{
  servo.attach(9);
  nh.initNode();
  nh.advertise(pub_range1); // Front Sensor
  nh.advertise(pub_range2); // Back Sensor
  nh.subscribe(sub_range);

  ArloSerial.begin(19200);
  Arlo.begin(ArloSerial);

  Arlo.writeMotorPower(60, 60);
  delay(3000);
  Arlo.writeMotorPower(0,0);
}



long range_time;

void loop()
{
  if ( millis() >= range_time ){
    range_msg.range = getRange_Ultrasound(14);
    pub_range1.publish(&range_msg);
    pub_range2.publish(&range_msg); 

    range_time =  millis() + 50;
  }

  nh.spinOnce();
}


/**** Get Ultrasonic Ranges ****/
float getRange_Ultrasound(int pin_num)
{
  pinMode(pin_num, OUTPUT);
  digitalWrite(pin_num, LOW);
  delayMicroseconds(2);
  digitalWrite(pin_num, HIGH);
  delayMicroseconds(10);
  digitalWrite(pin_num, LOW);

  pinMode(pin_num, INPUT);
  duration = pulseIn(pin_num, HIGH);

  return duration/58;
}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-01-24 13:20:59 -0600

gvdhoorn gravatar image

tl;dr: use the frame_id field in the header of the Range message to identify individual sensors.

If you're feeling fancy, you could then even use tf2 to transform incoming messages to figure out the exact range relative to some shared origin.


To "encode" the location of the range sensor, you'd set the frame_id field in the header field of the Range message.

Based on information from your URDF (or static transform publishers), you would then be able to transform incoming range information to the appropriate location (essentially: by interpreting it as coming from a specific sensor on your robot) and by doing so would know the relative distance between all sensors and some other link of your robot.

The first thing to do is to set the frame_id field in your loop() body for the two Range messages you publish(). See the IR Ranger Tutorial tutorial for an example.

With that done, you'd ideally use the tf2 library for the transformations, but seeing as you're running this on an Arduino, I'm not sure that is a good idea / available.

If you know the locations of your sensors relative to some shared origin (perhaps the base_link of your robot?), you could do the transform manually on the Arduino side. Alternatively, you could put the Range subscriber on the PC side, and command the motors from there. On the PC, TF is definitely available.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-01-24 11:45:47 -0600

Seen: 407 times

Last updated: Jan 24