ROS Arduino.6. Reading distance with ultrasonic sensor

ultrasonic sensor ေလးကိုပဲ servo motor ေပၚတင္ျပီး distance ဖတ္ထားတယ္ေပါ့. လြယ္တယ္ဆိုေပမယ္. arduino ကေန topic ေတြကို ဘယ္လို subscribe လုပ္မယ္. ဘယ္လို publish လုပ္မယ္ဆိုတာကိုေတာ့ ေကာင္းေကာင္းနားလည္သြားေစနိုင္ပါတယ္။

#include <ros.h>
#include <Servo.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Float32.h>

#define trigPin 52
#define echoPin 53

ros::NodeHandle nh;
std_msgs::Float32 sonar;

Servo servo;

void servo_cb(const std_msgs::UInt16& cmd_msg)
{
  servo.write(cmd_msg.data);
}

ros::Publisher sonar_pub("sonar",&sonar);
ros::Subscriber<std_msgs::UInt16> sub_servo("sh",servo_cb);

void setup()
{
  nh.initNode();
  nh.subscribe(sub_servo);
  nh.advertise(sonar_pub);
  pinMode(trigPin,OUTPUT);
  pinMode(echoPin,INPUT);
  servo.attach(9);
}

void loop()
{
  float duration,distance;
  digitalWrite(trigPin,LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin,HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin,LOW);
  duration=pulseIn(echoPin,HIGH);
  distance=(duration/2)/29.1;
  distance=distance/100;
  sonar.data=distance;
  sonar_pub.publish(&sonar);
  nh.spinOnce();
  nh.spinOnce();
  delay(10);
}

arduino ကေန sh ဆုိတဲ့ topic ကို subscribe လုပ္ေနပါတယ္။ ရလာတဲ့ data ကိုေတာ့ servo motor ကို control လုပ္တာေပါ့. သူကေန sonar ဆုိတဲ့ topic ကို ျပန္ျပီး publish လုပ္ေပးပါတယ္။ဒါကေတာ့ ဖတ္လို႕ရတဲ့ distance ေပါ့.. သူ႕အတြက္ gui ေလးကိုေတာ့ ေအာက္ပါအတိုင္း အလြယ္ေလးေရးထားလိုက္ပါတယ္။

from Tkinter import *
import rospy
from std_msgs.msg import UInt16
from std_msgs.msg import Float32
from std_msgs.msg import String

master = Tk()
v= StringVar()

def sonar_sub(data):
    global v
    distance = data.data
    distance = round(distance,2)
    v.set(distance)

rospy.init_node('servo_gui')
h = rospy.Publisher("sh",UInt16,queue_size=1)
sub = rospy.Subscriber("sonar",Float32,sonar_sub)

def send(val):
    h.publish(int(val))

TDist = Label(master, font=('times', 20, 'bold'), text="Distance")
TDist.pack()

T = Label(master,font=('times', 20, 'bold'), textvariable=v)
T.pack()

w2 = Scale(master, from_=0, to=180, orient=HORIZONTAL, command=send)
w2.set(0)
w2.pack()

mainloop()