Go to the documentation of this file.00001
00002
00003
00004 import rospy, math
00005 from geometry_msgs.msg import Twist
00006 import subprocess
00007
00008
00009 def get_motor_freq():
00010 swfile = "/dev/rtmotoren0"
00011 lfile = "/dev/rtmotor_raw_l0"
00012 rfile = "/dev/rtmotor_raw_r0"
00013 vel = Twist()
00014 sound_count = 0
00015 while not rospy.is_shutdown():
00016 try:
00017 with open(swfile, "r") as f:
00018 motor_power_status = f.readline().rstrip()
00019 if motor_power_status == "0":
00020 sound_count = 0
00021 if motor_power_status == "1" and sound_count == 0:
00022 subprocess.call("aplay $(rospack find raspimouse_control)/misc/ms_sound.wav", shell=True)
00023 sound_count = 1
00024 if motor_power_status == "1":
00025 with open(lfile, "r") as lf, \
00026 open(rfile, "r") as rf:
00027 lhz_str = lf.readline().rstrip()
00028 rhz_str = rf.readline().rstrip()
00029 if len(lhz_str) == 0:
00030 lhz = 0
00031 else:
00032 try:
00033 lhz = int(lhz_str)
00034 except:
00035 lhz = 0
00036 if len(rhz_str) == 0:
00037 rhz = 0
00038 else:
00039 try:
00040 rhz = int(rhz_str)
00041 except:
00042 rhz = 0
00043 vel.linear.x = (lhz + rhz) * 9 * math.pi / 160000.0
00044 vel.angular.z = (rhz - lhz) * math.pi / 800.0
00045 print(vel)
00046 pub.publish(vel)
00047
00048 except rospy.ROSInterruptException:
00049 pass
00050
00051
00052 if __name__ == "__main__":
00053 rospy.init_node("virtual_motors")
00054 pub = rospy.Publisher(rospy.get_namespace() + "diff_drive_controller/cmd_vel", Twist, queue_size=10)
00055 get_motor_freq()
00056 rospy.spin()