X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_roboint.git;a=blobdiff_plain;f=scripts%2Frobo_explorer.py;fp=scripts%2Frobo_explorer.py;h=de755c10c38abbb52fb0261ccd2ed5689282e415;hp=3a302cc40df33d504aa146d0a6da7b954a892de8;hb=839da311672219243b7f0bc3291b331f92a395c8;hpb=8940b6be5868ba9d85e83198722fb18562df1989

diff --git a/scripts/robo_explorer.py b/scripts/robo_explorer.py
index 3a302cc..de755c1 100755
--- a/scripts/robo_explorer.py
+++ b/scripts/robo_explorer.py
@@ -2,8 +2,6 @@
 import roslib; roslib.load_manifest('roboint')
 import rospy
 import tf
-import tf.broadcaster
-import tf.transformations
 from math import *
 from geometry_msgs.msg import Twist, TransformStamped, Point32
 from sensor_msgs.msg import LaserScan
@@ -93,7 +91,9 @@ class RoboExplorer:
 		# speeds
 		dt = (current_time - self.last_time).to_sec()
 		vx = sqrt((self.x - self.x_last)**2 + (self.y - self.y_last)**2) / dt
-		vy = 0.0
+		if (msg.output[0]-msg.output[1] + msg.output[2]-msg.output[3]) < 0:
+			# moving backward
+			vx*=-1
 		valpha = (self.alpha - self.alpha_last) / dt
 		self.x_last = self.x
 		self.y_last = self.y
@@ -122,7 +122,7 @@ class RoboExplorer:
 		# set the velocity
 		odom.child_frame_id = "base_link"
 		odom.twist.twist.linear.x = vx
-		odom.twist.twist.linear.y = vy
+		odom.twist.twist.linear.y = 0.0
 		odom.twist.twist.angular.z = valpha
 
 		# publish the message