sonar sensors: set fov to 40° as stated in http://picaxe.hobbizine.com/srf05.html

[ros_wild_thumper.git] / scripts / wt_node.py

diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index 72c04123a4428cf4bd4c7e362ff7a0ed6cc2c806..dcae6740c02b7fa35e07eb7416f75293cc0819eb 100755 (executable)
--- a/scripts/wt_node.py
+++ b/scripts/wt_node.py
@@ -4,6 +4,7 @@
 import rospy
 import tf
 import struct
+import prctl
 from i2c import *
 from math import *
 from geometry_msgs.msg import Twist
@@ -16,7 +17,8 @@ WHEEL_DIST = 0.248
 class MoveBase:
        def __init__(self):
                rospy.init_node('wild_thumper')
-               rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
+               prctl.set_name("wild_thumper")
+               rospy.Subscriber("cmd_vel_out", Twist, self.cmdVelReceived)
                rospy.Subscriber("imu", Imu, self.imuReceived)
                enable_odom_tf = rospy.get_param("~enable_odom_tf", True)
                if enable_odom_tf:
@@ -39,15 +41,19 @@ class MoveBase:
                rate = rospy.Rate(20.0)
                reset_val = self.get_reset()
                rospy.loginfo("Reset Status: 0x%x" % reset_val)
+               i = 0
                while not rospy.is_shutdown():
                        #print struct.unpack(">B", i2c_read_reg(0x50, 0xA2, 1))[0] # count test
                        self.get_tle_err()
                        self.get_odom()
                        self.get_voltage()
-                       self.get_dist_forward()
-                       self.get_dist_backward()
-                       self.get_dist_left()
-                       self.get_dist_right()
+                       if i % 2:
+                               self.get_dist_forward()
+                               self.get_dist_left()
+                       else:
+                               self.get_dist_backward()
+                               self.get_dist_right()
+                       i+=1
                        rate.sleep()
 
        def set_motor_handicap(self, front, aft): # percent
@@ -59,10 +65,10 @@ class MoveBase:
                (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(msg.orientation.__getstate__())
                if pitch > 30*pi/180:
                        val = (100.0/65)*abs(pitch)*180/pi
-                       self.set_motor_handicap(0, int(val))
+                       self.set_motor_handicap(int(val), 0)
                elif pitch < -30*pi/180:
                        val = (100.0/65)*abs(pitch)*180/pi
-                       self.set_motor_handicap(int(val), 0)
+                       self.set_motor_handicap(0, int(val))
                else:
                        self.set_motor_handicap(0, 0)
 
@@ -148,7 +154,7 @@ class MoveBase:
                odom.pose.covariance[14] = 1e6 # z
                odom.pose.covariance[21] = 1e6 # rotation about X axis
                odom.pose.covariance[28] = 1e6 # rotation about Y axis
-               odom.pose.covariance[35] = 0.1 # rotation about Z axis
+               odom.pose.covariance[35] = 0.03 # rotation about Z axis
 
                # set the velocity
                odom.child_frame_id = "base_footprint"
@@ -160,7 +166,7 @@ class MoveBase:
                odom.twist.covariance[14] = 1e6 # z
                odom.twist.covariance[21] = 1e6 # rotation about X axis
                odom.twist.covariance[28] = 1e6 # rotation about Y axis
-               odom.twist.covariance[35] = 0.1 # rotation about Z axis
+               odom.twist.covariance[35] = 0.03 # rotation about Z axis
 
                # publish the message
                self.pub_odom.publish(odom)
@@ -190,13 +196,17 @@ class MoveBase:
                val = struct.unpack(">H", s)[0]
                return val
        
-       def get_dist_srf(self, num):
+       def start_dist_srf(self, num):
                dev = i2c(0x52)
                s = struct.pack("B", num)
                dev.write(s)
                dev.close()
 
-               sleep(50e-3)
+       def read_dist_srf(self, num):
+               dev = i2c(0x52)
+               s = struct.pack("B", num)
+               dev.write(s)
+               dev.close()
 
                dev = i2c(0x52)
                s = dev.read(2)
@@ -218,22 +228,24 @@ class MoveBase:
        def get_dist_left(self):
                if self.pub_range_left.get_num_connections() > 0:
                        dist = 30.553/(self.get_dist_ir(0x1) - -67.534)
-                       self.send_range(self.pub_range_left, "ir_left", Range.INFRARED, dist, 0.04, 0.3, 5)
+                       self.send_range(self.pub_range_left, "ir_left", Range.INFRARED, dist, 0.04, 0.3, 1)
 
        def get_dist_right(self):
                if self.pub_range_right.get_num_connections() > 0:
                        dist = 17.4/(self.get_dist_ir(0x3) - 69)
-                       self.send_range(self.pub_range_right, "ir_right", Range.INFRARED, dist, 0.04, 0.3, 5)
+                       self.send_range(self.pub_range_right, "ir_right", Range.INFRARED, dist, 0.04, 0.3, 1)
 
        def get_dist_forward(self):
                if self.pub_range_fwd.get_num_connections() > 0:
-                       dist = self.get_dist_srf(0x5)
-                       self.send_range(self.pub_range_fwd, "sonar_forward", Range.ULTRASOUND, dist, 0.04, 6, 60)
+                       dist = self.read_dist_srf(0x15)
+                       self.send_range(self.pub_range_fwd, "sonar_forward", Range.ULTRASOUND, dist, 0.04, 6, 40)
+                       self.start_dist_srf(0x5) # get next value
 
        def get_dist_backward(self):
                if self.pub_range_bwd.get_num_connections() > 0:
-                       dist = self.get_dist_srf(0x7)
-                       self.send_range(self.pub_range_bwd, "sonar_backward", Range.ULTRASOUND, dist, 0.04, 6, 60)
+                       dist = self.read_dist_srf(0x17)
+                       self.send_range(self.pub_range_bwd, "sonar_backward", Range.ULTRASOUND, dist, 0.04, 6, 40)
+                       self.start_dist_srf(0x7) # get next value
                
 
 if __name__ == "__main__":