Added new coursework, cleaned up structure

OSU Coursework/ROB 456 - Intelligent Robotics/Homework 3 - Part 1 - Occupancy Grids/src/hw3.py

@@ -0,0 +1,118 @@
+#!/usr/bin/env python
+
+import rospy
+import math
+import tf
+from tf.transformations import euler_from_quaternion
+import message_filters
+
+# The laser scan message
+from sensor_msgs.msg import LaserScan
+
+# 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()
+
+# global pi
+pi = math.pi
+
+# method to control the robot
+def callback(scan,odom):
+    # the odometry parameter should be global
+    global globalOdom
+    globalOdom = odom
+
+    # make a new twist message
+    command = Twist()
+
+    # Fill in the fields.  Field values are unspecified 
+    # until they are actually assigned. The Twist message 
+    # holds linear and angular velocities.
+    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
+    
+    # find current (x,y) position of robot based on odometry
+    currentX = globalOdom.pose.pose.position.x
+    currentY = globalOdom.pose.pose.position.y
+
+    # 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
+
+    # find laser scanner properties (min scan angle, max scan angle, scan angle increment)
+    maxAngle = scan.angle_max
+    minAngle = scan.angle_min
+    angleIncrement = scan.angle_increment
+
+    # find current laser angle, max scan length, distance array for all scans, and number of laser scans
+    currentLaserTheta = minAngle
+    maxScanLength = scan.range_max 
+    distanceArray = scan.ranges
+    numScans = len(distanceArray)
+   
+    # the code below (currently commented) shows how 
+    # you can print variables to the terminal (may 
+    # be useful for debugging)
+    #print 'x: {0}'.format(currentX)
+    #print 'y: {0}'.format(currentY)
+    #print 'theta: {0}'.format(currentAngle)
+
+    # for each laser scan
+    distThreshold = 2.0   # obstacle avoidance threshold
+    minScan = distanceArray[0]
+    velocity = 5.0
+    bearing = 0.0
+    for curScan in range(0, numScans):
+      if currentLaserTheta > -pi/2 and currentLaserTheta < 0 and distanceArray[curScan] < distThreshold:
+        bearing = 1.0 # turn left
+      elif currentLaserTheta >= 0 and currentLaserTheta < pi/2 and distanceArray[curScan] < distThreshold:
+        bearing = -1.0 # turn right
+      if distanceArray[curScan] < minScan:
+        minScan = distanceArray[curScan]
+      currentLaserTheta = currentLaserTheta + angleIncrement
+    
+    # set the robot motion
+    # commanded velocities
+    command.linear.x = velocity*min(1.0,minScan/distThreshold) # slow down if within threshold distance
+    command.angular.z = bearing
+    pub.publish(command)
+
+# main function call
+if __name__ == "__main__":
+    # Initialize the node
+    rospy.init_node('lab3', log_level=rospy.DEBUG)
+
+    # subscribe to laser scan message
+    sub = message_filters.Subscriber('base_scan', LaserScan)
+
+    # subscribe to odometry message    
+    sub2 = message_filters.Subscriber('odom', Odometry)
+
+    # synchronize laser scan and odometry data
+    ts = message_filters.TimeSynchronizer([sub, sub2], 10)
+    ts.registerCallback(callback)
+
+    # publish twist message
+    pub = rospy.Publisher('cmd_vel',Twist)
+
+    # Turn control over to ROS
+    rospy.spin()
+

OSU Coursework/ROB 456 - Intelligent Robotics/Homework 3 - Part 1 - Occupancy Grids/src/odom_error_printer.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+
+import rospy
+import math
+import tf
+from tf.transformations import euler_from_quaternion
+import message_filters
+
+# The odometry message
+from nav_msgs.msg import Odometry
+
+def callback(truth,odom):
+    # Calculate error between truth and odometry estimate
+    x_ODOM_error = truth.pose.pose.position.x - odom.pose.pose.position.x
+    y_ODOM_error = truth.pose.pose.position.y - odom.pose.pose.position.y
+    
+    xo = odom.pose.pose.orientation.x
+    yo = odom.pose.pose.orientation.y
+    zo = odom.pose.pose.orientation.z
+    wo = odom.pose.pose.orientation.w
+    
+    xt = truth.pose.pose.orientation.x
+    yt = truth.pose.pose.orientation.y
+    zt = truth.pose.pose.orientation.z
+    wt = truth.pose.pose.orientation.w
+
+    # find orientation of robot (Euler coordinates)
+    (ro, po, yo) = euler_from_quaternion([xo, yo, zo, wo])    
+    (rt, pt, yt) = euler_from_quaternion([xt, yt, zt, wt])  
+    t_ODOM_error = yt-yo  
+    
+    print "{0},{1},{2},{3}".format(rospy.get_time(),x_ODOM_error, y_ODOM_error, t_ODOM_error)
+
+
+# main function call
+if __name__ == "__main__":
+    # Initialize the node
+    rospy.init_node('odom_error_printer')
+
+    # subscribe to truth pose message
+    sub_truth = message_filters.Subscriber('base_pose_ground_truth', Odometry)
+
+    # subscribe to odometry message    
+    sub_odom = message_filters.Subscriber('odom', Odometry)
+
+    # synchronize truth pose and odometry data
+    ts = message_filters.TimeSynchronizer([sub_truth, sub_odom], 10)
+    ts.registerCallback(callback)
+
+    # Turn control over to ROS
+    rospy.spin()

OSU Coursework/ROB 456 - Intelligent Robotics/Homework 3 - Part 1 - Occupancy Grids/src/slam_error_printer.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python
+
+import rospy
+import math
+import tf
+from tf.transformations import euler_from_quaternion
+import message_filters
+
+# The odometry message
+from nav_msgs.msg import Odometry
+
+def callback(truth,odom):
+    # Calculate error between truth and SLAM estimate
+    x_ODOM_error = truth.pose.pose.position.x - odom.pose.pose.position.x
+    y_ODOM_error = truth.pose.pose.position.y - odom.pose.pose.position.y
+    
+    xo = odom.pose.pose.orientation.x
+    yo = odom.pose.pose.orientation.y
+    zo = odom.pose.pose.orientation.z
+    wo = odom.pose.pose.orientation.w
+    
+    xt = truth.pose.pose.orientation.x
+    yt = truth.pose.pose.orientation.y
+    zt = truth.pose.pose.orientation.z
+    wt = truth.pose.pose.orientation.w
+
+    # find orientation of robot (Euler coordinates)
+    (ro, po, yo) = euler_from_quaternion([xo, yo, zo, wo])    
+    (rt, pt, yt) = euler_from_quaternion([xt, yt, zt, wt])
+    t_ODOM_error = yt-yo
+    
+    # query SLAM solution
+    map_listener = tf.TransformListener()
+    try:
+      map_listener.waitForTransform("/map", "/odom", rospy.Time(), rospy.Duration(4.0))
+      (trans,rot) = map_listener.lookupTransform('/map', '/odom', rospy.Time(0))
+      x_SLAM_error = trans[0]-x_ODOM_error
+      y_SLAM_error = trans[1]-y_ODOM_error
+      t_SLAM_error = rot[2]-t_ODOM_error
+  
+      print "{0},{1},{2},{3}".format(rospy.get_time(), x_SLAM_error, y_SLAM_error, t_SLAM_error)
+
+    except(tf.LookupException, tf.ConnectivityException):
+      pass
+          	
+    
+
+# main function call
+if __name__ == "__main__":
+    # Initialize the node
+    rospy.init_node('slam_error_printer')
+
+    # subscribe to truth pose message
+    sub_truth = message_filters.Subscriber('base_pose_ground_truth', Odometry)
+
+    # subscribe to odometry message    
+    sub_odom = message_filters.Subscriber('odom', Odometry)
+
+    # synchronize truth pose and odometry data
+    ts = message_filters.TimeSynchronizer([sub_truth, sub_odom], 10)
+    ts.registerCallback(callback)
+
+    # Turn control over to ROS
+    rospy.spin()