Added new coursework, cleaned up structure

OSU Coursework/ROB 456 - Intelligent Robotics/Homework 2 - Obstacle Avoidance/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rob456_hw2)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  stage_ros
+  geometry_msgs
+  roscpp
+  rospy
+)
+
+catkin_package()
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)

OSU Coursework/ROB 456 - Intelligent Robotics/Homework 2 - Obstacle Avoidance/launch/hw2.launch

@@ -0,0 +1,7 @@
+<launch>
+  <node pkg="stage_ros" type="stageros" name="simulator" args="$(find rob456_hw2)/worlds/manyDots.world"/>
+  <node pkg="rob456_hw2" type="hw2.py" name="hw2" output="screen">
+	  <param name="goalX" value="20" />
+    <param name="goalY" value="-20" />
+  </node>
+</launch>

OSU Coursework/ROB 456 - Intelligent Robotics/Homework 2 - Obstacle Avoidance/package.xml

@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<package>
+  <name>rob456_hw2</name>
+  <version>0.0.0</version>
+  <description>ROB456 HW2</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="rob456@todo.todo">rob456</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/pioneer_delivery</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>stage_ros</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>stage_ros</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

OSU Coursework/ROB 456 - Intelligent Robotics/Homework 2 - Obstacle Avoidance/src/hw2.py

@@ -0,0 +1,181 @@
+#!/usr/bin/env python
+
+import rospy
+import math
+import tf
+from tf.transformations import euler_from_quaternion
+import message_filters
+from pprint import pprint
+import time
+# 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 - this may come in handy
+pi = math.pi
+
+
+def angle_between_points((x_1, y_1), (x_2, y_2)):
+    return math.atan2(y_2 - y_1, x_2 - x_1)
+
+
+# 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
+
+    # get goal x and y locations from the launch file
+    goalX = rospy.get_param('hw2/goalX', 0.0)
+    goalY = rospy.get_param('hw2/goalY', 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)
+
+    # ######### My Modifications ##########
+    # Get the angle from the robot to the goal position
+    desired_angle = angle_between_points((currentX, currentY), (goalX, goalY))
+
+    # Precision variables
+    angle_offset = 0.05  # Handles how much deviance from our desired angle we can be
+    goal_precision = 0.1  # Sets how close to the goal is determined to be the actual goal
+    precision = 0.02  # Sets how forcefully we try to maintain a path
+
+    min_theta = desired_angle - angle_offset  # Sets the min theta to be within from settings above
+    max_theta = desired_angle + angle_offset  # Same but for max theta
+
+    abs_min_theta = abs(currentAngle - min_theta)  # Get the absolute value of how far off we are, in regards to limits
+    abs_max_theta = abs(currentAngle - max_theta)  # Same as previous
+
+    # If we've reached the goal, within our goal precision, stop the rover from moving
+    if abs(goalY - currentY) < goal_precision and abs(goalX - currentX) < goal_precision:
+        return
+
+    # Otherwise, make the rover move forward always
+    command.linear.x = 0.35
+
+    # Check if our current angle is within our precision for direction
+    # This is what sets the rover heading towards the goal
+    if abs_min_theta > precision and abs_max_theta > precision:
+        # If we're not, turn left or right depending on which limit we're closer to
+        if abs_max_theta > abs_min_theta:
+            command.angular.z = 0.25  # Counter-clockwise
+        else:
+            command.angular.z = -0.25  # Clockwise
+
+    # Sets up obstacle avoidance
+    num_scans = 400  # Set the number of samples around the center-forward of the rover's vision
+    threshold = 1.0  # Set a threshold for how close objects are allowed to be
+    results = ["Good" for _ in range(num_scans)]  # Set up an array for the number of scans, all set to good
+    middle_point = numScans / 2  # Get the middle point for the scan information we have
+
+    # Get our limits for upper and lower scan boundaries based on the scan inputs and num_scans
+    lower_range = middle_point - (num_scans / 2)
+    upper_range = middle_point + (num_scans / 2)
+
+    position = 0  # Variable to keep track of the results array position
+
+    # Go through our scans in the range, and mark any that are out of bounds.
+    for current_scan in range(lower_range, upper_range):
+        if distanceArray[current_scan] < threshold:
+            results[position] = "Bad"
+        position += 1
+
+    # Split the results up into measurements from the left vs right side of the rover, counting how many sensor
+    # readings on each side were too close to an object
+    right_count = results[: (len(results) / 2) - 10].count("Bad")
+    left_count = results[(len(results) / 2) + 10:].count("Bad")
+
+    # Debug statements while testing
+    # print time.time()
+    # print "Left: %s Right: %s" % (left_count, right_count)
+
+    # Threshold for how many positions on one side of the rover need to be too close to an object before avoiding
+    count_thresh = 20
+
+    # How forcefully to turn the rover one direction or another when it is trying to avoid obstacles
+    increment = 0.8
+
+    # If either side of the rover has too many counts for an object being too closer
+    if left_count > count_thresh or right_count > count_thresh:
+        # Turn the rover left or right, depending on which side has more counts than the other
+        if left_count and right_count:
+            if left_count > right_count or right_count == left_count:
+                command.angular.z += -increment
+            else:
+                command.angular.z += increment
+        elif left_count:
+            command.angular.z += -increment
+        elif right_count:
+            command.angular.z += increment
+
+    # Send the commands
+    pub.publish(command)
+
+# main function call
+if __name__ == "__main__":
+    # Initialize the node
+    rospy.init_node('lab2', 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, queue_size=10)
+
+    # Turn control over to ROS
+    rospy.spin()
+

OSU Coursework/ROB 456 - Intelligent Robotics/Homework 2 - Obstacle Avoidance/worlds/manyDots.world

@@ -0,0 +1,71 @@
+define block model
+(
+  size [0.5 0.5 0.5]
+  gui_nose 0
+)
+
+define topurg ranger
+(
+	sensor( 			
+    range [ 0.0  30.0 ]
+    fov 270.25
+   samples 1081
+  )
+
+  # generic model properties
+  color "black"
+  size [ 0.05 0.05 0.1 ]
+)
+
+define erratic position
+(
+  size [0.35 0.35 0.25]
+  origin [-0.05 0 0 0]
+  gui_nose 0
+  drive "diff"
+  topurg(pose [ 0.050 0.000 0 0.000 ])
+  odom_error [0.00 0.00 0.00 0.00 0.00 0.00]
+)
+
+define floorplan model
+(
+  # sombre, sensible, artistic
+  color "gray30"
+
+  # most maps will need a bounding box
+  boundary 1
+
+  gui_nose 0
+  gui_grid 0
+
+  gui_outline 0
+  gripper_return 0
+  fiducial_return 0
+  laser_return 1
+)
+
+# set the resolution of the underlying raytrace model in meters
+resolution 0.005
+
+interval_sim 100  # simulation timestep in milliseconds
+
+
+window
+( 
+  size [ 745.000 448.000 ] 
+
+  rotate [ 0.000 0.000 ]
+  scale 5
+)
+
+# load an environment bitmap
+floorplan
+( 
+  name "manyDots"
+  bitmap "manyDots.pgm"
+  size [54.0 58.7 0.5]
+  pose [ 0 0.0 0 90.000 ]
+)
+
+# throw in a robot
+erratic( pose [ -10.000 10.000 0 90.000 ] name "era" color "blue" localizaion "gps" localization_origin [0 0 0 0])