Ask Your Question
0

Implimenting odometry on a real tutlebot

asked 2016-09-26 17:03:31 -0500

arpit901 gravatar image

updated 2016-09-27 10:39:01 -0500

I wrote the following code to navigate my robot in a square using odometry. The objective for the code is use 4 goal positions(the 4 corners of a square) and navigating to it, by first making sure it is lined up(using odometry and goal positions) and then move using a linear twist if it is lined up. The process repeats 4 times to navigate a square.

In simulation(rviz) the code works and i can see the robot trying to implement the algorithm. However, on a real turtlebot, the code is being implimented but the robot barely or hardly moves from its position.

I have checked that the kobuki base works, by implementing a simple code.. Any suggestions or ideas.I really need urgent help and am open to any suggestions you may have.

import roslib
import rospy
import math
import random
import tf
from tf.transformations import euler_from_quaternion
import message_filters #A set of message filters which take in messages and may output those messages at a later time, based on the conditions that filter needs met.
# The odometry message
from nav_msgs.msg import Odometry
# the velocity command message
from geometry_msgs.msg import Twist

# instantiate global variables "globalOdom"
globalOdom = Odometry()
goalX = None
goalY = None
success_percent = 0

arr_goalX = [0, -5, 0]
arr_goalY = [5, 0, -5]

# method to control the robot
def callback(odom):
    # the odometry parameter should be global
    global globalOdom
    globalOdom = odom
    at_goal = False

    global goalX, goalY, success_percent, arr_goalX, arr_goalY

    # make a new twist message
    command = Twist()

    #log the position to a file
    X_file = open('X_data.txt','a')
    Y_file = open('Y_data.txt','a')

    for index in range(1):
        # Fill in the fields.  Field values are unspecified 
        # until they are actually assigned. The Twist message 
        # holds linear and angular velocities.
        print "Success Percent = ", success_percent
        command.linear.x = 0.0
        command.linear.y = 0.0
        command.linear.z = 0.0
        command.angular.x = 0.0
        command.angular.y = 0.0
        command.angular.z = 0.0
        # get goal x and y locations from the launch file
        # get parameter 'lab2/goalX' with a default value of 0
        # find current (x,y) position of robot based on odometry
        currentX = globalOdom.pose.pose.position.x
        currentY = globalOdom.pose.pose.position.y
        print type(currentX)
        if goalX is None:
            goalX = currentX + 5
        if goalY is None:
            goalY = currentY
        print "GoalX = ", goalX, "\tGoalY = ", goalY

        if abs(currentX - goalX) < 0.1 and abs(currentY - goalY) < 0.1:
            print "At goal!!!!"
            if success_percent == 3:
                rospy.signal_shutdown("Goal Reached.")
            else:
                goalX = goalX + arr_goalX[success_percent]
                goalY = goalY + arr_goalY[success_percent]
                success_percent = success_percent + 1

        # find current orientation of robot based on odometry (quaternion coordinates)
        xOr = globalOdom.pose.pose.orientation.x
        yOr = globalOdom.pose.pose.orientation.y
        zOr = globalOdom.pose.pose.orientation.z
        wOr = globalOdom.pose.pose.orientation.w

        # find orientation of robot (Euler coordinates)
        (roll, pitch, yaw) = euler_from_quaternion([xOr, yOr, zOr, wOr])

        # find currentAngle of robot (equivalent to yaw)
        # now that you have yaw, the robot's pose is completely defined by (currentX, currentY, currentAngle)
        currentAngle = yaw

        d_x = goalX - currentX
        d_y = goalY ...
(more)
edit retag flag offensive close merge delete

Comments

Please provide a explanation of the real fault, i.e. what do you expect to happen, what does actually happen.

Just stating "it does not work" will not let us provide any help...

mgruhler gravatar imagemgruhler ( 2016-09-27 01:26:54 -0500 )edit

I have edited the description. Sorry for being so broad and vague.

arpit901 gravatar imagearpit901 ( 2016-09-27 10:39:36 -0500 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2016-09-27 18:30:07 -0500

Henrique gravatar image

Maybe you should verify if the movebase package is not using all the processiong of the cpu. If the computer is overloaded, the movebase stop sending the cmd_vel as it should. Verify if you see any warning related to controling process losing the frequency. Or verify the frequency the cmd_vel is being published with rostopic hz topic_name.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2016-09-26 17:03:31 -0500

Seen: 134 times

Last updated: Sep 27 '16