Added CloneFollower, removed ApplyFilter

README.md

 ## Lab 0
-This project contains skeleton code for lab0. The first portion of the lab requires the student to record a bag file of the robot driving around while being tele-operated. Then a script is written to play back the bag file such that the robot autonomously follows (approximately) the path that was recorded in the bag. The second portion requires the student to apply an edge and gaussian filters to images coming from a topic. See the [assignment spec](docs/lab0_spec.pdf) for more details about the assignment.
+This project contains skeleton code for lab0. The first portion of the lab requires the student to publish poses for a car clone that follows the original car around. The second portion requires the student to record a bag file of the robot driving around while being tele-operated. Then a script is written to play back the bag file such that the robot autonomously follows (approximately) the path that was recorded in the bag. See the [assignment spec](docs/lab0_spec.pdf) for more details about the assignment.
 
 ### Installation
 The following packages are required:
 1. ackermann_msgs: **sudo apt-get install ros-kinetic-ackermann_msgs**
-  
-2. cv_bridge: **sudo apt-get install ros-kinetic-cv-bridge**
  
-3. numpy: **sudo apt-get install python-numpy**
-  
-4. scipy: **sudo apt-get install python-scipy**
+2. numpy: **sudo apt-get install python-numpy**
 
 Once these packages have been installed, clone this package, place it in a catkin workspace, and compile using catkin_make (or whichever build mechanism you prefer).
   
 ### Usage
-After implementing BagFollower.py and BagFollower.launch, the following command will launch the script for the first portion of the assignment: **roslaunch lab0 BagFollower.launch**
 
-After implementing ApplyFilter.py and ApplyFilter.launch, the following command will launch the script for the second portion of the assignment: **roslaunch lab0 ApplyFilter.launch**
+After implementing CloneFollower.py and CloneFollower.launch, the following command will launch the script for the first portion of the assignment: **roslaunch lab0 CloneFollower.launch**
+
+After implementing BagFollower.py and BagFollower.launch, the following command will launch the script for the second portion of the assignment: **roslaunch lab0 BagFollower.launch**

data/filter1.csv

--1,0,1

data/filter2.csv

-0.1111,0.1111,0.1111
-0.1111,0.1112,0.1111
-0.1111,0.1111,0.1111

launch/ApplyFilter.launch

-<launch>
-	<!-- Launch an ApplyFilter node and pass any necessary parameters -->
-
-</launch>

launch/CloneFollower.launch

+<launch>
+
+    <!--
+      follow_offset: The x offset between the car and the clone
+      force_in_bounds: Whether or not bounds for the clone should be checked
+    -->
+
+   <!--
+	Launch a clone_follower node here
+   -->
+
+</launch>

src/ApplyFilter.py

-#!/usr/bin/env python
-
-import rospy
-import numpy as np
-from scipy import signal
-from sensor_msgs.msg import Image
-from cv_bridge import CvBridge, CvBridgeError
-
-# Applies a filter to images received on a specified topic and publishes the filtered image
-class Filter:
-
-  # Initializes the filter
-	def __init__(self, filter_path, sub_topic, pub_topic, fast_convolve=False):
-		# Load the filter from csv
-		# Create the publisher and subscriber
-		# Create a CvBridge object for converting sensor_msgs/Image into numpy arrays (and vice-versa)
-		#		http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython
-		# Use the 'self' parameter to initialize as necessary
-		pass
-
-  # Callback for when an image is received. Applies the filter to that image
-	def apply_filter_cb(self, msg):
-		# Apply the filter to the incoming image and publish the result
-		# If the image has multiple channels, apply the filter to each channel independent of the other channels
-		pass
-		
-if __name__ == '__main__':
-	filter_path = None    # The path to the csv file containing the filter
-	sub_topic = None      # The image topic to be subscribed to
-	pub_topic = None      # The topic to publish filtered images to
-	fast_convolve = False # Whether or not the nested for loop or Scipy's convolve method should be used for applying the filter
-
-	rospy.init_node('apply_filter', anonymous=True)
-	
-	# Populate params with values passed by launch file
-
-	# Create a Filter object and pass it the loaded parameters
-
-	rospy.spin()
-	

src/CloneFollower.py

+#!/usr/bin/env python
+
+import rospy
+import numpy as np
+import tf
+from geometry_msgs.msg import PoseStamped
+import Utils # <----------- LOOK AT THESE FUNCTIONS ***************************
+
+SUB_TOPIC = '/sim_car_pose/pose' # The topic that provides the simulated car pose
+PUB_TOPIC = '/clone_follower_pose/pose' # The topic that you should publish to
+MAP_TOPIC = 'static_map' # The service topic that will provide the map
+WORLD_FRAME = 'map' # The name of the world frame
+ROBOT_FRAME = 'sim_pose' # The name of the robot's frame
+
+# Follows the simulated robot around
+class CloneFollower:
+
+  '''
+  Initializes a CloneFollower object
+  In:
+    follow_offset: The required x offset between the robot and its clone follower
+    force_in_bounds: Whether the clone should toggle between following in front
+                     and behind when it goes out of bounds of the map
+  '''
+  def __init__(self, follow_offset, force_in_bounds):
+    # YOUR CODE HERE 
+    self.follow_offset = # Store the input params in self
+    self.force_in_bounds = # Store the input params in self
+    self.listener = # Instantiate a transform listener
+    self.map_img, self.map_info = # Get and store the map
+                                  # for bounds checking
+    
+    # Setup publisher that publishes to PUB_TOPIC
+    self.pub = 
+    
+    # Setup subscriber that subscribes to SUB_TOPIC and uses the self.update_pose
+    # callback
+    self.sub = 
+    
+  '''
+  Given the translation and rotation between the robot and map, computes the pose
+  of the clone
+  (This function is optional)
+  In:
+    trans: The translation between the robot and map
+    rot: The rotation between the robot and map
+  Out:
+    The pose of the clone
+  '''
+  def compute_follow_pose(self, trans, rot):
+    # YOUR CODE HERE
+    
+  '''
+  Callback that runs each time a sim pose is received. Should publish an updated
+  pose of the clone.
+  In:
+    msg: The pose of the simulated car. Should be a geometry_msgs/PoseStamped
+  '''  
+  def update_pose(self, msg):
+    # YOUR CODE HERE
+  
+    # Get the transform between the simulated car and the map
+    try:
+      # Note that lookup_transform returns a tuple of the form (trans, rot).
+      # Trans is a 3 element tuple of the form [x,y,z] where z is zero
+      # Rot is a 4 element tuple of the form [x,y,z,w] (which represents a quaternion)
+      # If you plan to do matrix operations with these, make sure that you
+      # convert them to numpy arrays with the correct dimensions!
+    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+      # Print/handle exception to help debug
+      return
+      
+    # Compute the pose of the clone
+    # Note: If 'rot' contains the quaternion returned by lookupTransform, you 
+    # can do the following to get the corresponding rotation matrix:
+    #     roll, pitch, yaw = tf.transformations.euler_from_quaternion(rot) 
+    #     rot_mat = Utils.rotation_matrix(yaw)
+    
+    # Check bounds if required
+    if self.force_in_bounds:
+      pass
+      
+    # Setup the out going PoseStamped message
+
+    # Publish the clone's pose
+      
+    
+if __name__ == '__main__':
+  follow_offset = 1.0 # The offset between the robot and clone
+  force_in_bounds = False # Whether or not map bounds should be enforced
+  
+  rospy.init_node('clone_follower', anonymous=True) # Initialize the node
+  
+  # Populate params with values passed by launch file
+  follow_offset = # YOUR CODE HERE
+  force_in_bounds = # YOUR CODE HERE
+  
+  cf = CloneFollower(follow_offset, force_in_bounds) # Create a clone follower
+  rospy.spin() # Spin
+  

src/Utils.py

+#!/usr/bin/env python
+
+import rospy
+import numpy as np
+
+from std_msgs.msg import Header
+from geometry_msgs.msg import Quaternion 
+from nav_msgs.srv import GetMap
+import tf.transformations
+import tf
+import matplotlib.pyplot as plt
+
+''' 
+Convert an angle in radians into a quaternion message.
+In:
+    angle: The yaw angle in radians
+Out:
+    The Quaternion message
+'''
+def angle_to_quaternion(angle):
+    return Quaternion(*tf.transformations.quaternion_from_euler(0, 0, angle))
+
+''' 
+Convert a quaternion message into an angle in radians.
+In:
+  q: The quaternion message
+Out:
+  The yaw angle
+'''
+def quaternion_to_angle(q):
+    x, y, z, w = q.x, q.y, q.z, q.w
+    roll, pitch, yaw = tf.transformations.euler_from_quaternion((x, y, z, w))
+    return yaw
+
+''' 
+Returns a rotation matrix that applies the passed angle (in radians)
+In:
+  theta: The desired rotation angle
+Out:
+  The corresponding rotation matrix
+'''
+def rotation_matrix(theta):
+    c, s = np.cos(theta), np.sin(theta)
+    return np.matrix([[c, -s], [s, c]])
+
+''' Get the map from the map server
+In:
+  map_topic: The service topic that will provide the map
+Out:
+  map_img: A numpy array with dimensions (map_info.height, map_info.width). 
+           A zero at a particular location indicates that the location is impermissible
+           A one at a particular location indicates that the location is permissible
+  map_info: Info about the map, see
+            http://docs.ros.org/kinetic/api/nav_msgs/html/msg/MapMetaData.html 
+            for more info    
+'''
+def get_map(map_topic):
+  rospy.wait_for_service(map_topic)
+  map_msg = rospy.ServiceProxy(map_topic, GetMap)().map
+  array_255 = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width))
+  map_img = np.zeros_like(array_255, dtype=bool)
+  map_img[array_255==0] = 1 
+  
+  return map_img, map_msg.info
+  
+''' 
+Convert a pose in the world to a pixel location in the map image 
+In:
+  pose: The pose in the world to be converted. Should be a list or tuple of the
+        form [x,y,theta]
+  map_info: Info about the map (returned by get_map)
+Out:
+  The corresponding pose in the pixel map - has the form [x,y,theta]
+  where x and y are integers
+'''    
+def world_to_map(pose, map_info):
+    scale = map_info.resolution
+    angle = -quaternion_to_angle(map_info.origin.orientation)
+    config = [0.0,0.0,0.0]
+    # translation
+    config[0] = (1.0/float(scale))*(pose[0] - map_info.origin.position.x)
+    config[1] = (1.0/float(scale))*(pose[1] - map_info.origin.position.y)
+    config[2] = pose[2]
+   
+
+    # rotation
+    c, s = np.cos(angle), np.sin(angle)
+    # we need to store the x coordinates since they will be overwritten
+    temp = np.copy(config[0])
+    config[0] = int(c*config[0] - s*config[1])
+    config[1] = int(s*temp       + c*config[1])
+    config[2] += angle
+    
+    return config
+      
+''' 
+Convert a pixel location in the map to a pose in the world
+In: 
+  pose: The pixel pose in the map. Should be a list or tuple of the form [x,y,theta]
+  map_info: Info about the map (returned by get_map)
+Out:
+  The corresponding pose in the world - has the form [x,y,theta]
+'''
+def map_to_world(pose,map_info):
+    scale = map_info.resolution
+    angle = quaternion_to_angle(map_info.origin.orientation)
+
+    # rotate
+    config = np.array([pose[0],map_info.height-pose[1],pose[2]])
+    # rotation
+    c, s = np.cos(angle), np.sin(angle)
+    # we need to store the x coordinates since they will be overwritten
+    temp = np.copy(config[0])
+    config[0] = c*config[0] - s*config[1]
+    config[1] = s*temp       + c*config[1]
+
+    # scale
+    config[:2] *= float(scale)
+
+    # translate
+    config[0] += map_info.origin.position.x
+    config[1] += map_info.origin.position.y
+    config[2] += angle
+
+    return config