X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Fwt_node.py;h=a2a09ba1854a79694a98e4d1afbba89478bfde56;hp=72c04123a4428cf4bd4c7e362ff7a0ed6cc2c806;hb=3708a6b10b4b3b98e1c3422f545dba82948fec5a;hpb=860e5f83d540d60b46f4a0f242f007dfd1c0747d

diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index 72c0412..a2a09ba 100755
--- a/scripts/wt_node.py
+++ b/scripts/wt_node.py
@@ -39,15 +39,18 @@ 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_backward()
+				self.get_dist_left()
+				self.get_dist_right()
+			i+=1
 			rate.sleep()
 
 	def set_motor_handicap(self, front, aft): # percent
@@ -148,7 +151,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 +163,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)
@@ -228,12 +231,12 @@ class MoveBase:
 	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)
+			self.send_range(self.pub_range_fwd, "sonar_forward", Range.ULTRASOUND, dist, 0.04, 6, 30)
 
 	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)
+			self.send_range(self.pub_range_bwd, "sonar_backward", Range.ULTRASOUND, dist, 0.04, 6, 30)
 		
 
 if __name__ == "__main__":