from bitathome_remote_control.msg import Follow
import socket
import traceback
import math
port = 23333 # server port
links = 1 # max link
if __name__ == '__main__':
follow_pub = rospy.Publisher('Kinect/Follow_point', Follow, queue_size=10)
rospy.init_node('netPublisher', anonymous=True)
rate = rospy.Rate(30)
rospy.loginfo("Initializing socket...")
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(('', port))
rospy.loginfo("Ready!")
while not rospy.is_shutdown():
try:
buf,(r_ip, r_port) = sock.recvfrom(1024)
print(buf, r_ip, r_port)
buf = buf.split(" ")
ret = Follow()
ret.s = buf[0]
follow_pub.publish(ret)
except Exception,ex:
rospy.logerr("Exception!")
print(ex)
traceback.print_exc()
continue
if styleData.s == "stop":
ser(0, 0, 0)
elif styleData.s == "go":
ser(300, 0, 0)
elif styleData.s == "goRight":
ser(200, 0, 0 - 150)
elif styleData.s == "goLeft":
ser(200, 0, 150)
elif styleData.s == "right":
ser(0, 0, 0 - 200)
elif styleData.s == "left":
ser(0, 0, 200)
elif styleData.s == "back":
ser(-200, 0, 0)
rospy.sleep(0.5)
if __name__ == "__main__":
rospy.init_node("kinect_move")
ser = rospy.ServiceProxy("/hc_motor_cmd/vector_speed", VectorSpeed)
scanData = list()
styleData = Follow()
scan_pub = rospy.Subscriber("/scan", LaserScan, run1)
point_pub = rospy.Subscriber("/Kinect/Follow_point", Follow, run2)
follow_pub()
rospy.spin()
