From 416ac5d02482b1f9ca9534f1eccf067a585ecf86 Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Wed, 18 Sep 2013 20:48:29 +0200
Subject: [PATCH] fix velocity message in robo_explorer

---
 move_base.launch         | 1 +
 scripts/robo_explorer.py | 6 +++---
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/move_base.launch b/move_base.launch
index a0ea7f8..5f62007 100644
--- a/move_base.launch
+++ b/move_base.launch
@@ -9,6 +9,7 @@
 	<include file="$(find amcl)/examples/amcl_diff.launch" />
 
 	<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+		<param name="controller_frequency" value="4.0" />
 		<rosparam file="$(find roboint)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
 		<rosparam file="$(find roboint)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
 		<rosparam file="$(find roboint)/config/global_costmap_params.yaml" command="load" />
diff --git a/scripts/robo_explorer.py b/scripts/robo_explorer.py
index 3311493..3a302cc 100755
--- a/scripts/robo_explorer.py
+++ b/scripts/robo_explorer.py
@@ -4,7 +4,7 @@ import rospy
 import tf
 import tf.broadcaster
 import tf.transformations
-from math import sin, cos, pi
+from math import *
 from geometry_msgs.msg import Twist, TransformStamped, Point32
 from sensor_msgs.msg import LaserScan
 from nav_msgs.msg import Odometry
@@ -92,8 +92,8 @@ class RoboExplorer:
 	def send_odometry(self, msg, current_time):
 		# speeds
 		dt = (current_time - self.last_time).to_sec()
-		vx = (self.x - self.x_last) / dt
-		vy = (self.y - self.y_last) / dt
+		vx = sqrt((self.x - self.x_last)**2 + (self.y - self.y_last)**2) / dt
+		vy = 0.0
 		valpha = (self.alpha - self.alpha_last) / dt
 		self.x_last = self.x
 		self.y_last = self.y
-- 
2.39.5