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 // Include ARLO Library
#include // Include SoftwareSerial Library
#include
#include
#include
#include
// 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 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;
}
↧