عربة شاحنة ROS. الجزء 6. المسافات مع ترميز عجلة ، خريطة غرفة ، ليدار

 #!/usr/bin/env python """ diff_tf.py - follows the output of a wheel encoder and creates tf and odometry messages. some code borrowed from the arbotix diff_controller script A good reference: http://rossum.sourceforge.net/papers/DiffSteer/ Copyright (C) 2012 Jon Stephan. """ import rospy #import roslib #roslib.load_manifest('differential_drive') from math import sin, cos, pi from geometry_msgs.msg import Quaternion from geometry_msgs.msg import Twist from geometry_msgs.msg import Vector3 from nav_msgs.msg import Odometry import tf from tf.broadcaster import TransformBroadcaster from std_msgs.msg import Int16, Int32, Int64, UInt32 ############################################################################# class DiffTf: ############################################################################# ############################################################################# def __init__(self): ############################################################################# rospy.init_node("diff_tf") self.nodename = rospy.get_name() rospy.loginfo("-I- %s started" % self.nodename) #### parameters ####### #Wheel radius : 0.0325 # wheel circum = 2* 3.14 * 0.0325 = 0.2041 # One rotation encoder ticks : 8 ticks # For 1 meter: 8 * ( 1 / 0.2041) = 39 ticks self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform self.ticks_meter = float(rospy.get_param('ticks_meter', 190)) # The number of wheel encoder ticks per meter of travel self.base_width = float(rospy.get_param('~base_width', 0.11)) # The wheel base width in meters self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # basefootprint /the name of the base frame of the robot self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame self.encoder_min = rospy.get_param('encoder_min', -2147483648) self.encoder_max = rospy.get_param('encoder_max', 2147483648) self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min ) self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min ) self.t_delta = rospy.Duration(1.0/self.rate) self.t_next = rospy.Time.now() + self.t_delta # internal data self.enc_left = None # wheel encoder readings self.enc_right = None self.left = 0 # actual values coming back from robot self.right = 0 self.lmult = 0 self.rmult = 0 self.prev_lencoder = 0 self.prev_rencoder = 0 self.x = 0 # position in xy plane self.y = 0 self.th = 0 self.dx = 0 # speeds in x/rotation self.dr = 0 self.yaw = 0.01 self.pitch = 0.01 self.roll = 0.01 self.then = rospy.Time.now() self.quaternion_1 = Quaternion() # subscriptions rospy.Subscriber("wheel_ticks_left", UInt32, self.lwheelCallback) rospy.Subscriber("wheel_ticks_right", UInt32, self.rwheelCallback) #rospy.Subscriber("imu_data", Vector3, self.imu_value_update) self.odomPub = rospy.Publisher("odom", Odometry,queue_size=10) self.odomBroadcaster = TransformBroadcaster() ############################################################################# def spin(self): ############################################################################# r = rospy.Rate(self.rate) while not rospy.is_shutdown(): self.update() r.sleep() ############################################################################# def update(self): ############################################################################# now = rospy.Time.now() if now > self.t_next: elapsed = now - self.then self.then = now elapsed = elapsed.to_sec() # calculate odometry if self.enc_left == None: d_left = 0 d_right = 0 else: d_left = (self.left - self.enc_left) / self.ticks_meter d_right = (self.right - self.enc_right) / self.ticks_meter self.enc_left = self.left self.enc_right = self.right # distance traveled is the average of the two wheels d = ( d_left + d_right ) / 2 # this approximation works (in radians) for small angles th = ( d_right - d_left ) / self.base_width # calculate velocities self.dx = d / elapsed self.dr = th / elapsed if (d != 0): # calculate distance traveled in x and y x = cos( th ) * d y = -sin( th ) * d # calculate the final position of the robot self.x = self.x + ( cos( self.th ) * x - sin( self.th ) * y ) self.y = self.y + ( sin( self.th ) * x + cos( self.th ) * y ) if( th != 0): self.th = self.th + th # publish the odom information quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin( self.th / 2 ) quaternion.w = cos( self.th / 2 ) ''' try: quaternion.z = self.quaternion_1[2] quaternion.w = self.quaternion_1[3] except: quaternion.z = sin( self.th / 2 ) quaternion.w = cos( self.th / 2 ) pass ''' self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rospy.Time.now(), self.base_frame_id, self.odom_frame_id ) odom = Odometry() odom.header.stamp = now odom.header.frame_id = self.odom_frame_id odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.child_frame_id = self.base_frame_id odom.twist.twist.linear.x = self.dx odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = self.dr self.odomPub.publish(odom) def imu_value_update(self, imu_data): orient = Vector3() orient = imu_data self.yaw = orient.x self.pitch = orient.y self.roll = orient.z try: self.quaternion_1 = tf.transformations.quaternion_from_euler(self.yaw, self.pitch, self.roll) #print self.quaternion_1[0] #print self.quaternion_1[1] #print self.quaternion_1[2] #print self.quaternion_1[3] except: rospy.logwarn("Unable to get quaternion values") pass ############################################################################# def lwheelCallback(self, msg): ############################################################################# enc = msg.data if (enc < self.encoder_low_wrap and self.prev_lencoder > self.encoder_high_wrap): self.lmult = self.lmult + 1 if (enc > self.encoder_high_wrap and self.prev_lencoder < self.encoder_low_wrap): self.lmult = self.lmult - 1 self.left = 1.0 * (enc + self.lmult * (self.encoder_max - self.encoder_min)) self.prev_lencoder = enc ############################################################################# def rwheelCallback(self, msg): ############################################################################# enc = msg.data if(enc < self.encoder_low_wrap and self.prev_rencoder > self.encoder_high_wrap): self.rmult = self.rmult + 1 if(enc > self.encoder_high_wrap and self.prev_rencoder < self.encoder_low_wrap): self.rmult = self.rmult - 1 self.right = 1.0 * (enc + self.rmult * (self.encoder_max - self.encoder_min)) self.prev_rencoder = enc ############################################################################# ############################################################################# if __name__ == '__main__': """ main """ diffTf = DiffTf() diffTf.spin() 

  #### parameters ####### #Wheel radius : 0.0325 # wheel circum = 2* 3.14 * 0.0325 = 0.2041 # One rotation encoder ticks : 8 ticks # For 1 meter: 8 * ( 1 / 0.2041) = 39 ticks self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform self.ticks_meter = float(rospy.get_param('ticks_meter', 190)) # The number of wheel encoder ticks per meter of travel self.base_width = float(rospy.get_param('~base_width', 0.11)) # The wheel base width in meters 

 <?xml version="1.0"?> <launch> <node pkg="tf" type="static_transform_publisher" name="laser_link" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 50" /> <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen"> <!-- Frame names --> <param name="map_frame" value="map" /> <param name="odom_frame" value="base_link" /> <!-- Map size / start point --> <param name="map_resolution" value="0.050"/> <param name="map_size" value="1024"/> <param name="map_start_x" value="0.5"/> //  <param name="map_start_y" value="0.5" /> <param name="map_multi_res_levels" value="2" /> <!-- Map update parameters --> <param name="update_factor_free" value="0.4"/> <param name="update_factor_occupied" value="0.9" /> <param name="map_update_distance_thresh" value="0.4"/> <param name="map_update_angle_thresh" value="0.06" /> <param name="laser_z_min_value" value="-1.0" /> <param name="laser_z_max_value" value="1.0" /> <!-- Advertising config --> <param name="advertise_map_service" value="true"/> <param name="scan_subscriber_queue_size" value="5"/> <param name="scan_topic" value="scan"/> </node> </launch>