In Class Exercise -- Where am I?

[cmurray3]

Here's some code to get us started on the process of determining where our Turtlebot is located. We'll add functionality together in class.

#!/usr/bin/env python3

import rospy, math
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion

def rad2deg(rad):
    return rad*(180/math.pi)

def deg2rad(deg):
    return deg*(math.pi/180)    

def odom_callback(msg):
    print(msg)

rospy.init_node('where_am_i')

odom_sub = rospy.Subscriber('odom', Odometry, odom_callback)

rospy.spin()

[cmurray3]

Updated code:

#!/usr/bin/env python3

import rospy, math
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion

def rad2deg(rad):
    return rad*(180/math.pi)

def deg2rad(deg):
    return deg*(math.pi/180)    

def odom_callback(msg):
    # print(msg)
    (roll, pitch, yaw) = euler_from_quaternion([msg.pose.pose.orientation.x,
                                                msg.pose.pose.orientation.y, 
                                                msg.pose.pose.orientation.z, 
                                                msg.pose.pose.orientation.w]) 

    print("x: ", msg.pose.pose.position.x, "y: ", msg.pose.pose.position.y, "yaw: ", rad2deg(yaw))

rospy.init_node('where_am_i')

odom_sub = rospy.Subscriber('odom', Odometry, odom_callback)

# Make the robot rotate 45 degrees

rospy.spin()

[cmurray3]

In the wanderbot package, replace the existing CMakeLists.txt and package.xml files with the ones in the attached .zip archive.

new_cmakelists_and_packagexml.zip

[cmurray3]

Updated version of where_am_i.py (from March 14, 2023 class):

#!/usr/bin/env python3

import rospy, math
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Twist
from wanderbot.msg import target

yaw = 0.0

def rad2deg(rad):
    return rad*(180/math.pi)

def deg2rad(deg):
    return deg*(math.pi/180)    

def odom_callback(msg):
    # print(msg)
    global yaw
    (roll, pitch, yaw) = euler_from_quaternion([msg.pose.pose.orientation.x,
                                                msg.pose.pose.orientation.y, 
                                                msg.pose.pose.orientation.z, 
                                                msg.pose.pose.orientation.w]) 

    print("x: ", msg.pose.pose.position.x, "y: ", msg.pose.pose.position.y, "yaw: ", rad2deg(yaw))

def target_callback(msg):
    '''
    float32 degrees
    bool    isGlobal

    rostopic pub target wanderbot/target '{degrees: 90, isGlobal: True}'
    '''
    print(msg.degrees, msg.isGlobal)

rospy.init_node('where_am_i')

odom_sub   = rospy.Subscriber('odom', Odometry, odom_callback)
target_sub = rospy.Subscriber('target', target, target_callback)

cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)

# Make the robot rotate 45 degrees
targetDegrees = 45 
targetEpsilon =  0.4  # degrees   

# Specify the maximum allowable angular velocity
maxAngularZ = deg2rad(40)   # [rad/s]

# rospy.spin()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
    radianDelta = deg2rad(targetDegrees) - yaw   # [radians]

    twist = Twist()
    if (abs(radianDelta) > deg2rad(targetEpsilon)):     
        # twist.angular.z = deg2rad(0)    # [rad/s]
        twist.angular.z = min(maxAngularZ, radianDelta)

    cmd_vel_pub.publish(twist)

    print('======== twist.angular.z = ', twist.angular.z)

    rate.sleep()

[cmurray3]

This is target.msg, it goes in the msg folder in the wanderbot package:

float32 degrees
bool    isGlobal

[cmurray3]

The version of where_am_i.py that we worked on today is pasted below.

• Note 1: After class I switched the linear.x value back to +1.
• Note 2: This code "works" (in the sense that, eventually, your robot will end up at the desired target). See if you can fine-tune the logic so the robot converges more smoothly to the target.

#!/usr/bin/env python3

import rospy, math
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Twist
from wanderbot.msg import target

yaw = 0.0
targetDegrees = 0.0
x = 0.0
y = 0.0

def rad2deg(rad):
    return rad*(180/math.pi)

def deg2rad(deg):
    return deg*(math.pi/180)    

def odom_callback(msg):
    # print(msg)
    global yaw, x, y
    (roll, pitch, yaw) = euler_from_quaternion([msg.pose.pose.orientation.x,
                                                msg.pose.pose.orientation.y, 
                                                msg.pose.pose.orientation.z, 
                                                msg.pose.pose.orientation.w]) 

    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    # print("x: ", x, "y: ", y, "yaw: ", rad2deg(yaw))

def target_callback(msg):
    '''
    float32 degrees
    bool    isGlobal

    rostopic pub target wanderbot/target '{degrees: 90, isGlobal: True}'
    '''
    global targetDegrees

    print(msg.degrees, msg.isGlobal)
    targetDegrees = msg.degrees

rospy.init_node('where_am_i')

odom_sub   = rospy.Subscriber('odom', Odometry, odom_callback)
target_sub = rospy.Subscriber('target', target, target_callback)

cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)

# Make the robot rotate 45 degrees
targetDegrees = 45 
targetEpsilon =  0.4  # degrees   

# Set a target location (pose.position)
targetX = 4.0 # [m] in x-direction from the origin (0,0)
targetY = 4.0 # [m] in y-direction from the origin 

# Specify the maximum allowable angular velocity
maxAngularZ = deg2rad(40)   # [rad/s]

targetEpsilonDistance = 0.1  # [m]

# rospy.spin()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
    # what is the reqd heading to face the target?
    reqdHdg = math.atan2(targetY - y, targetX - x)
    radianDelta = reqdHdg - yaw   # [radians]

    twist = Twist()
    if (abs(radianDelta) > deg2rad(targetEpsilon)):     
        # twist.angular.z = deg2rad(0)    # [rad/s]
        twist.angular.z = min(maxAngularZ, radianDelta)
        print("ROTATE x = ", x, "y = ", y, "yaw = ", rad2deg(yaw), "reqdHdg = ", rad2deg(reqdHdg), "dist = ", math.sqrt((targetX - x)**2 + (targetY - y)**2))

    elif (math.sqrt((targetX - x)**2 + (targetY - y)**2) > targetEpsilonDistance):
        twist.linear.x = 1  # [m/s]
        print("MOVE   x = ", x, "y = ", y, "yaw = ", rad2deg(yaw), "reqdHdg = ", rad2deg(reqdHdg), "dist = ", math.sqrt((targetX - x)**2 + (targetY - y)**2))

    cmd_vel_pub.publish(twist)

    '''
    radianDelta = deg2rad(targetDegrees) - yaw   # [radians]

    twist = Twist()
    # Save this for later -- rotation only
    if (abs(radianDelta) > deg2rad(targetEpsilon)):     
        # twist.angular.z = deg2rad(0)    # [rad/s]
        twist.angular.z = min(maxAngularZ, radianDelta)
    ''' 
    # cmd_vel_pub.publish(twist)

    # print('======== twist.angular.z = ', twist.angular.z)

    rate.sleep()

[cmurray3]

Python

Latest version of where_am_i.py:

#!/usr/bin/env python3

import rospy, math
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Twist
from wanderbot.msg import target

def rad2deg(rad):
    return rad*(180/math.pi)

def deg2rad(deg):
    return deg*(math.pi/180)    

# FIXME - Do we need to initialize these here?
# Are these variables even used?
yaw = 0.0
x = 0.0
y = 0.0

# Make the robot rotate 45 degrees
useYaw = False
targetDegrees = 45 
targetEpsilon =  0.4  # degrees   

# Set a target location (pose.position)
useXY  = False
targetX = 4.0 # [m] in x-direction from the origin (0,0)
targetY = 4.0 # [m] in y-direction from the origin 
targetEpsilonDistance = 0.1  # [m]

# Specify the maximum allowable angular velocity
maxAngularZ = deg2rad(40)   # [rad/s]

def odom_callback(msg):
    # print(msg)
    global yaw, x, y
    (roll, pitch, yaw) = euler_from_quaternion([msg.pose.pose.orientation.x,
                                                msg.pose.pose.orientation.y, 
                                                msg.pose.pose.orientation.z, 
                                                msg.pose.pose.orientation.w]) 

    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    # print("x: ", x, "y: ", y, "yaw: ", rad2deg(yaw))

def target_callback(msg):
    '''
    float32 targetYaw   # [degrees] desired rotation of our robot
    bool    isGlobal    # We're not using this at the moment.  Assuming only global reference frame
    float32 targetX     # [m] (targetX, targetY) is the desired ending location
    float32 targetY     # [m] 
    bool    useYaw      # If True, apply the targetYaw value
    bool    useXY       # If True, apply the targetX and targetY values 

    rostopic pub target wanderbot/target '{targetYaw: 90, isGlobal: True}'
    '''
    global targetDegrees, targetX, targetY, useYaw, useXY

    print(msg.targetYaw, msg.isGlobal)
    targetDegrees = msg.targetYaw
    targetX = msg.targetX
    targetY = msg.targetY
    useYaw  = msg.useYaw
    useXY   = msg.useXY

rospy.init_node('where_am_i')

odom_sub   = rospy.Subscriber('odom', Odometry, odom_callback)
target_sub = rospy.Subscriber('target', target, target_callback)

cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)

# rospy.spin()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
    twist = Twist()

    '''
    We will apply either useXY **or** useYaw.
    If our target msg includes useYaw=True **and** useXY=True,
    we'll only worry about the XY part.
    '''
    if (useXY == True):
        # code to control our robot to go to an (x,y) target
        # what is the reqd heading to face the target?
        reqdHdg = math.atan2(targetY - y, targetX - x)
        radianDelta = reqdHdg - yaw   # [radians]

        if (abs(radianDelta) > deg2rad(targetEpsilon)):     
            # twist.angular.z = deg2rad(0)    # [rad/s]
            twist.angular.z = min(maxAngularZ, radianDelta)
            print("ROTATE x = ", x, "y = ", y, "yaw = ", rad2deg(yaw), "reqdHdg = ", rad2deg(reqdHdg), "dist = ", math.sqrt((targetX - x)**2 + (targetY - y)**2))

        elif (math.sqrt((targetX - x)**2 + (targetY - y)**2) > targetEpsilonDistance):
            twist.linear.x = 1  # [m/s]
            print("MOVE   x = ", x, "y = ", y, "yaw = ", rad2deg(yaw), "reqdHdg = ", rad2deg(reqdHdg), "dist = ", math.sqrt((targetX - x)**2 + (targetY - y)**2))

    elif (useYaw == True):
        # code to control our robot to rotate to a desired yaw
        radianDelta = deg2rad(targetDegrees) - yaw   # [radians]

        # rotation only
        if (abs(radianDelta) > deg2rad(targetEpsilon)):     
            # twist.angular.z = deg2rad(0)    # [rad/s]
            twist.angular.z = min(maxAngularZ, radianDelta)

    cmd_vel_pub.publish(twist)

    rate.sleep()

message

Latest version of target.msg:

float32 targetYaw   # [degrees] desired rotation of our robot
bool    isGlobal    # We're not using this at the moment.  Assuming only global reference frame
float32 targetX     # [m] (targetX, targetY) is the desired ending location
float32 targetY     # [m] 
bool    useYaw      # If True, apply the targetYaw value
bool    useXY       # If True, apply the targetX and targetY values

[noahkust]

I ran the code provided above and my turtlebot would neither "ROTATE" or "MOVE" and no messages appeared in the terminal. I did ctrl + c and I got the following messages. I am curious why this happened and how I might go about fixing this issue, if anyone has any insight that would be greatly appreciated.

student@vm:~/catkin_ws/src/wanderbot/scripts$ rosrun wanderbot where_am_i.py ^CTraceback (most recent call last): File "/home/student/catkin_ws/src/wanderbot/scripts/where_am_i.py", line 126, in rate.sleep() File "/opt/ros/noetic/lib/python3/dist-packages/rospy/timer.py", line 103, in sleep sleep(self._remaining(curr_time)) File "/opt/ros/noetic/lib/python3/dist-packages/rospy/timer.py", line 165, in sleep raise rospy.exceptions.ROSInterruptException("ROS shutdown request") rospy.exceptions.ROSInterruptException: ROS shutdown request

[cmurray3]

Hi, Noah, In a 3rd terminal, are you publishing a "target" message? If not, the robot has no goal.

Now that we have updated/expanded target.msg, a command to publish a target message would look like:

rostopic pub target wanderbot/target '{targetX: 4.0, targetY: 3.4, useXY: True}'

(the 1st terminal is for roslaunch, the 2nd is for running your python code)