fix velocity message in robo_explorer

diff --git a/move_base.launch b/move_base.launch
index a0ea7f8773a445b0c017ce232f37054a6ba99c62..5f6200710ea0a90b06b4dbd9285d366207037a72 100644 (file)
--- 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 33114931a7ec6f9b5ae29b434b9f743cedbed303..3a302cc40df33d504aa146d0a6da7b954a892de8 100755 (executable)
--- 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