import roslib; roslib.load_manifest('roboint')
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
def __init__(self):
rospy.init_node('robo_explorer')
- self.speed = (0, 0)
self.x = 0
self.y = 0
self.alpha = 0
# fake laser scan with ultra sonic range finder
self.enable_ultrasonic_laser = bool(rospy.get_param('~ultrasonic_laser', "True"))
- # Distance between both wheels in meter (18.8cm)
- self.wheel_dist = float(rospy.get_param('~wheel_dist', "0.188"))
- # Size of wheel Diameter in meter (5.1cm) * gear ratio (0.5) = 2.55cm
- self.wheel_size = float(rospy.get_param('~wheel_size', "0.0255"))
+ # Distance between both wheels in meter (18.55cm)
+ self.wheel_dist = float(rospy.get_param('~wheel_dist', "0.1855"))
+ # Size of wheel Diameter in meter (5.15cm) * gear ratio (0.5) = 2.575cm
+ self.wheel_size = float(rospy.get_param('~wheel_size', "0.02575"))
# Speed to PWM equation gradiant (The m in pwm = speed*m+b)
self.speed_gradiant = float(rospy.get_param('~speed_gradiant', "64.3"))
# Speed to PWM equation constant (The b in pwm = speed*m+b)
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
# fix in_diff from actual motor direction
- if self.speed[0] < 0:
+ if msg.output[1] > 0: # left reverse
in_diff[0] = -in_diff[0]
- elif self.speed[0] == 0:
+ elif msg.output[0] == 0 and msg.output[1] == 0: # left stop
in_diff[0] = 0
- if self.speed[1] < 0:
+ if msg.output[3] > 0: # right reverse
in_diff[1] = -in_diff[1]
- elif self.speed[1] == 0:
+ elif msg.output[2] == 0 and msg.output[3] == 0: # right stop
in_diff[1] = 0
dist_dir = (in_diff[1] - in_diff[0])*self.wheel_size*pi/8 # steps_changed in different direction => m
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
+ 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
# 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
#print "Speed wanted: %.2f m/s %.2f rad/s, set: %d %d" % (trans, rot, round(speed_l), round(speed_r))
outmsg = Motor()
- outmsg.num = 1
+ outmsg.num = 0
outmsg.speed = round(speed_l)
self.pub_motor.publish(outmsg)
outmsg = Motor()
- outmsg.num = 2
+ outmsg.num = 1
outmsg.speed = round(speed_r)
self.pub_motor.publish(outmsg)
- self.speed = (speed_l, speed_r)
-
if __name__ == '__main__':
RoboExplorer()