arpit901's profile - activity

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 ...

I am new to ROS and am following tutorials.Everything has worked perfectly fine till the turtlebot bringup. However when I am running a simple python code to draw a square(see below) import rospy from geometry_msgs.msg import Twist from math import radians

class DrawASquare():

def __init__(self):
    # initiliaze
    rospy.init_node('drawasquare', anonymous=False)

    # What to do you ctrl + c    
    rospy.on_shutdown(self.shutdown)

    self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
# 5 HZ
    r = rospy.Rate(5);
# create two different Twist() variables.  One for moving forward.  One for turning 45 degrees.

    # let's go forward at 0.2 m/s
    move_cmd = Twist()
    move_cmd.linear.x = 0.2
# by default angular.z is 0 so setting this isn't required

    #let's turn at 45 deg/s
    turn_cmd = Twist()
    turn_cmd.linear.x = 0
    turn_cmd.angular.z = radians(45); #45 deg/s in radians/s // 1.5708 radians(45)

#two keep drawing squares.  Go forward for 2 seconds (10 x 5 HZ) then turn for 2 second
count = 0
    while (not (rospy.is_shutdown()) and count < 5):
        rospy.loginfo("Going Straight")
        for x in range(0,20):
           self.cmd_vel.publish(move_cmd)
           r.sleep()  
    # turn 90 degrees
    rospy.loginfo("Turning")
        for x in range(0,10): # time has non linear transform . Angle= rate * angle per second
           self.cmd_vel.publish(turn_cmd)
           r.sleep() 
    count = count + 1
    if(count % 4==0): 
           rospy.loginfo("TurtleBot should be close to the original starting position (but it's probably way off)")


def shutdown(self):
    # stop turtlebot
    rospy.loginfo("Stop Drawing")
    self.cmd_vel.publish(Twist())
    rospy.sleep(1)

if __name__ == '__main__': try: DrawASquare() except: rospy.loginfo("node terminated.")

the terminal is giving me the output correct output

[INFO] [WallTime: 1464972316.780542] Going Straight [INFO] [WallTime: 1464972320.780868] Turning [INFO] [WallTime: 1464972323.180919] Going Straight ^C[INFO] [WallTime: 1464972326.380775] Stop Drawing [INFO] [WallTime: 1464972328.126076] Turning ^Cturtlebot@turtlebot1:~/turtlebot$ python draw_a_square.py [INFO] [WallTime: 1464974923.646459] Going Straight [INFO] [WallTime: 1464974927.646840] Turning [INFO] [WallTime: 1464974930.046803] Going Straight [INFO] [WallTime: 1464974934.046856] Turning [INFO] [WallTime: 1464974936.446891] Going Straight [INFO] [WallTime: 1464974940.446869] Turning [INFO] [WallTime: 1464974942.846905] Going Straight [INFO] [WallTime: 1464974946.846854] Turning [INFO] [WallTime: 1464974949.246856] TurtleBot should be close to the original starting position (but it's probably way off) [INFO] [WallTime: 1464974949.247515] Going Straight [INFO] [WallTime: 1464974953.246847] Turning [INFO] [WallTime: 1464974955.647318] Stop Drawing turtlebot@turtlebot1:~/turtlebot$ python draw_a_square.py [INFO] [WallTime: 1464974985.363926] Going Straight [INFO] [WallTime: 1464974989.364275] Turning [INFO] [WallTime: 1464974991.764251] Going Straight [INFO] [WallTime: 1464974995.764257] Turning [INFO] [WallTime: 1464974998.164311] Going Straight [INFO] [WallTime: 1464975002.164276] Turning [INFO] [WallTime: 1464975004.564348] Going Straight [INFO] [WallTime: 1464975008.564305] Turning [INFO] [WallTime: 1464975010.964148] TurtleBot should be close to the original starting position (but it's probably way off) [INFO] [WallTime: 1464975010.964686] Going Straight [INFO] [WallTime: 1464975014.964331] Turning [INFO] [WallTime: 1464975017.364785] Stop Drawing turtlebot@turtlebot1:~/turtlebot$ python draw_a_square.py [INFO] [WallTime: 1464975280.145368] Going Straight [INFO] [WallTime: 1464975284.145799] Turning [INFO ... (more)