fixes

diff --git a/scripts/robo_explorer.py b/scripts/robo_explorer.py
index fd26738fb18b4f4ae90a18d66e04bae38f808100..6cf73e6802fc74176d8e8ec686ef9433ea8c8519 100755 (executable)
--- a/scripts/robo_explorer.py
+++ b/scripts/robo_explorer.py
@@ -13,30 +13,30 @@ from roboint.msg import Inputs
 class RoboExplorer:
        def __init__(self):
                rospy.init_node('robo_explorer')
-               
-               rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
-               rospy.Subscriber("ft/get_inputs", Inputs, self.inputsReceived)
-
-               self.pub_motor = rospy.Publisher("ft/set_motor", Motor)
-               self.pub_scan = rospy.Publisher("scan", LaserScan)
-               self.pub_odom = rospy.Publisher("odom", Odometry)
 
                self.wheel_dist = 0.188 # 18.8cm
-               self.wheel_size = 0.052*0.5 # 5.1cm gear ration=0.5
+               self.wheel_size = 0.052*0.5 # 5.2cm; gear ration=0.5
                self.speed = (0, 0)
                self.x = 0
                self.y = 0
                self.alpha = 0
-               self.last_in = [0, 0]
+               self.last_in = None
                self.tf_broadcaster = tf.TransformBroadcaster()
                self.last_time = rospy.Time.now()
 
+               self.pub_motor = rospy.Publisher("ft/set_motor", Motor)
+               self.pub_scan = rospy.Publisher("scan", LaserScan)
+               self.pub_odom = rospy.Publisher("odom", Odometry)
+               
+               rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
+               rospy.Subscriber("ft/get_inputs", Inputs, self.inputsReceived)
+
                rospy.spin()
 
        def inputsReceived(self, msg):
                current_time = rospy.Time.now()
 
-               self.tf_broadcaster.sendTransform((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), current_time, "map", "base_link");
+               self.tf_broadcaster.sendTransform((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), current_time, "odom", "map");
                self.send_odometry(msg, current_time)
                self.send_laser_scan(msg, current_time)
 
@@ -45,66 +45,67 @@ class RoboExplorer:
        def send_odometry(self, msg, current_time):
                dt = (current_time - self.last_time).to_sec();
                in_now = msg.input[1:3]
-               in_diff = [abs(a - b) for a, b in zip(in_now, self.last_in)] # get changed inputs
-               if self.speed[0] < 0:
-                       in_diff[0] = -in_diff[0]
-               if self.speed[1] < 0:
-                       in_diff[1] = -in_diff[1]
-
-               dist_dir = (in_diff[1] - in_diff[0])*self.wheel_size*pi/8 # steps_changed in different direction => m
-               delta_alpha = dist_dir/self.wheel_dist
-
-               dist = (in_diff[0] + in_diff[1])/2.0*self.wheel_size*pi/8 # steps_changed same direction => m
-
-               delta_x = cos(self.alpha + delta_alpha/2)*dist
-               delta_y = sin(self.alpha + delta_alpha/2)*dist
-
-               self.alpha += delta_alpha
-               if self.alpha > 2*pi:
-                       self.alpha -= 2*pi
-               elif self.alpha < -2*pi:
-                       self.alpha += 2*pi
-               self.x += delta_x
-               self.y += delta_y
+               if self.last_in is not None:
+                       in_diff = [abs(a - b) for a, b in zip(in_now, self.last_in)] # get changed inputs
+                       if self.speed[0] < 0:
+                               in_diff[0] = -in_diff[0]
+                       if self.speed[1] < 0:
+                               in_diff[1] = -in_diff[1]
+
+                       dist_dir = (in_diff[1] - in_diff[0])*self.wheel_size*pi/8 # steps_changed in different direction => m
+                       delta_alpha = dist_dir/self.wheel_dist
+
+                       dist = (in_diff[0] + in_diff[1])/2.0*self.wheel_size*pi/8 # steps_changed same direction => m
+
+                       delta_x = cos(self.alpha + delta_alpha/2)*dist
+                       delta_y = sin(self.alpha + delta_alpha/2)*dist
+
+                       self.alpha += delta_alpha
+                       if self.alpha > 2*pi:
+                               self.alpha -= 2*pi
+                       elif self.alpha < -2*pi:
+                               self.alpha += 2*pi
+                       self.x += delta_x
+                       self.y += delta_y
+
+                       # speeds
+                       vx = delta_x / dt
+                       vy = delta_y / dt
+                       valpha = delta_alpha / dt
+
+                       # since all odometry is 6DOF we'll need a quaternion created from yaw
+                       odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.alpha)
+
+                       # first, we'll publish the transform over tf
+                       self.tf_broadcaster.sendTransform((self.x, self.y, 0.0), odom_quat, current_time, "base_link", "odom");
+
+                       # next, we'll publish the odometry message over ROS
+                       odom = Odometry()
+                       odom.header.stamp = current_time
+                       odom.header.frame_id = "/odom"
+
+                       # set the position
+                       odom.pose.pose.position.x = self.x
+                       odom.pose.pose.position.y = self.y
+                       odom.pose.pose.position.z = 0.0
+                       odom.pose.pose.orientation.x = odom_quat[0]
+                       odom.pose.pose.orientation.y = odom_quat[1]
+                       odom.pose.pose.orientation.z = odom_quat[2]
+                       odom.pose.pose.orientation.w = odom_quat[3]
+
+                       # set the velocity
+                       odom.child_frame_id = "base_link";
+                       odom.twist.twist.linear.x = vx
+                       odom.twist.twist.linear.y = vy
+                       odom.twist.twist.angular.z = valpha
+
+                       # publish the message
+                       self.pub_odom.publish(odom)
                self.last_in = in_now
-
-               # speeds
-               vx = delta_x / dt
-               vy = delta_y / dt
-               valpha = delta_alpha / dt
-
-               # first, we'll publish the transform over tf
-               self.tf_broadcaster.sendTransform((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), current_time, "odom", "base_link");
-
-               # next, we'll publish the odometry message over ROS
-               odom = Odometry()
-               odom.header.stamp = current_time
-               odom.header.frame_id = "/odom"
-
-               # since all odometry is 6DOF we'll need a quaternion created from yaw
-               odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.alpha)
-
-               # set the position
-               odom.pose.pose.position.x = self.x
-               odom.pose.pose.position.y = self.y
-               odom.pose.pose.position.z = 0.0
-               odom.pose.pose.orientation.x = odom_quat[0]
-               odom.pose.pose.orientation.y = odom_quat[1]
-               odom.pose.pose.orientation.z = odom_quat[2]
-               odom.pose.pose.orientation.w = odom_quat[3]
-
-               # set the velocity
-               odom.child_frame_id = "base_link";
-               odom.twist.twist.linear.x = vx
-               odom.twist.twist.linear.y = vy
-               odom.twist.twist.angular.z = valpha
-
-               # publish the message
-               self.pub_odom.publish(odom)
                
        def send_laser_scan(self, msg, current_time):
                # first, we'll publish the transform over tf
-               self.tf_broadcaster.sendTransform((0.06, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), current_time, "base_link", "scan");
+               self.tf_broadcaster.sendTransform((0.06, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), current_time, "scan", "base_link");
 
                # actually ultra sonic range finder
                scan = LaserScan()

diff --git a/src/libft_adapter.cpp b/src/libft_adapter.cpp
index 9247894934281b491fd50bbc424125a765c32d1e..22c50ae064be63d86845949ee9652f7f90a520ec 100644 (file)
--- a/src/libft_adapter.cpp
+++ b/src/libft_adapter.cpp
@@ -93,7 +93,7 @@ int main(int argc, char **argv)
         * buffer up before throwing some away.
         */
        ros::Publisher chatter_pub = n.advertise<roboint::Inputs>("ft/get_inputs", 1000);
-       ros::Rate loop_rate(5);
+       ros::Rate loop_rate(100);
 
        while(ros::ok()) {
                roboint::Inputs msg;