import rospy
import tf
import struct
+import prctl
from i2c import *
from math import *
from geometry_msgs.msg import Twist
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:
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
(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)
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"
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)
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)
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__":