How do I constantly make my robot the centre of the map

asked 2020-02-02 04:15:53 -0500

rumin gravatar image

updated 2020-02-02 10:44:56 -0500

gvdhoorn gravatar image

I am currently using Ubuntu 16.04 and turtlebot3. How do i ensure that the TurtleBot is always at the centre of the map image? My current code only allows the map to be rotated in the forward orientation, but doesnt make the turtlebot at the centre of the map image.

Below is my current code:

occ_bins = [-1, 0, 100, 101]

rotated = Image.fromarray(np.array(np.zeros((1,1))))

def callback(msg, tfBuffer):

    global rotated

    occdata = np.array([msg.data])
    occ_counts = np.histogram(occdata,occ_bins)
    total_bins = msg.info.width * msg.info.height
    rospy.loginfo('Width: %i Height: %i',msg.info.width,msg.info.height)
    rospy.loginfo('Unmapped: %i Unoccupied: %i Occupied: %i Total: %i', occ_counts[0][0], occ_counts[0][1], occ_counts[0][2], total_bins)

    trans = tfBuffer.lookup_transform('base_link', 'map', rospy.Time(0))
    rospy.loginfo(['Trans: ' + str(trans.transform.translation)])
    rospy.loginfo(['Rot: ' + str(trans.transform.rotation)])
    orientation_list = [trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w]
    (roll, pitch, yaw) = euler_from_quaternion(orientation_list)
    rospy.loginfo(['Yaw: R: ' + str(yaw) + ' D: ' + str(np.degrees(yaw))])
    oc2 = occdata[0]+ 1
    oc3 = (oc2>1).choose(oc2,2)
    odata = np.uint8(oc3.reshape(msg.info.width,msg.info.height,order='F'))
    img = Image.fromarray(odata)
    rotated = img.rotate(np.degrees(yaw)+180)
    plt.imshow(rotated,cmap='gray')
    plt.draw_all()
    plt.pause(0.00000000001)
edit retag flag offensive close merge delete