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_backward()
+ self.get_dist_left()
+ self.get_dist_right()
+ i+=1
rate.sleep()
def set_motor_handicap(self, front, aft): # percent
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)