diff --git a/exercises/5.0/src/myworkcell_core/CMakeLists.txt b/exercises/5.0/src/myworkcell_core/CMakeLists.txt
index 4793d23c7..39210cf5e 100644
--- a/exercises/5.0/src/myworkcell_core/CMakeLists.txt
+++ b/exercises/5.0/src/myworkcell_core/CMakeLists.txt
@@ -1,8 +1,8 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(myworkcell_core)
 
-## Add support for C++11, supported in ROS Kinetic and newer
-add_definitions(-std=c++11)
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -41,10 +41,10 @@ find_package(catkin REQUIRED COMPONENTS
 ##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
 ## * In the file package.xml:
 ##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
 ##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
 ##     but can be declared for certainty nonetheless:
-##     * add a run_depend tag for "message_runtime"
+##     * add a exec_depend tag for "message_runtime"
 ## * In this file (CMakeLists.txt):
 ##   * add "message_generation" and every package in MSG_DEP_SET to
 ##     find_package(catkin REQUIRED COMPONENTS ...)
@@ -79,7 +79,7 @@ add_service_files(
 ## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
-  geometry_msgs
+  geometry_msgs  # Or other packages containing msgs
   trajectory_msgs
 )
 
@@ -90,7 +90,7 @@ generate_messages(
 ## To declare and build dynamic reconfigure parameters within this
 ## package, follow these steps:
 ## * In the file package.xml:
-##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
 ## * In this file (CMakeLists.txt):
 ##   * add "dynamic_reconfigure" to
 ##     find_package(catkin REQUIRED COMPONENTS ...)
@@ -108,26 +108,26 @@ generate_messages(
 ###################################
 ## The catkin_package macro generates cmake config files for your package
 ## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if you package contains header files
+## INCLUDE_DIRS: uncomment this if your package contains header files
 ## LIBRARIES: libraries you create in this project that dependent projects also need
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES myworkcell_core
- CATKIN_DEPENDS
-   roscpp
-   fake_ar_publisher
-   message_runtime
-   geometry_msgs
-   tf
-   moveit_ros_planning_interface
-   ur5_demo_descartes
-   descartes_trajectory
-   descartes_planner
-   descartes_utilities
-   trajectory_msgs
-   tf_conversions
+   CATKIN_DEPENDS
+     roscpp
+     fake_ar_publisher
+     message_runtime
+     geometry_msgs
+     tf
+     moveit_ros_planning_interface
+     ur5_demo_descartes
+     descartes_trajectory
+     descartes_planner
+     descartes_utilities
+     trajectory_msgs
+     tf_conversions
 #  DEPENDS system_lib
 )
 
@@ -137,8 +137,8 @@ catkin_package(
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-# include_directories(include)
 include_directories(
+# include
   ${catkin_INCLUDE_DIRS}
 )
 
@@ -182,7 +182,6 @@ target_link_libraries(descartes_node ${catkin_LIBRARIES})
 target_link_libraries(adv_descartes_node ${catkin_LIBRARIES})
 target_link_libraries(adv_myworkcell_node ${catkin_LIBRARIES})
 
-
 #############
 ## Install ##
 #############
diff --git a/exercises/5.0/src/myworkcell_core/package.xml b/exercises/5.0/src/myworkcell_core/package.xml
index 9dc42a2e2..dae520abf 100644
--- a/exercises/5.0/src/myworkcell_core/package.xml
+++ b/exercises/5.0/src/myworkcell_core/package.xml
@@ -1,37 +1,70 @@
 <?xml version="1.0"?>
-<package>
+<package format="2">
   <name>myworkcell_core</name>
   <version>0.0.0</version>
   <description>The myworkcell_core package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="ros-industrial@todo.todo">ros-industrial</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>TODO</license>
 
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/myworkcell_core</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, 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 depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roscpp</build_depend>
-  <run_depend>roscpp</run_depend>
-  <build_depend>fake_ar_publisher</build_depend>
-  <run_depend>fake_ar_publisher</run_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <depend>fake_ar_publisher</depend>
   <build_depend>message_generation</build_depend>
-  <run_depend>message_runtime</run_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <run_depend>geometry_msgs</run_depend>
-  <build_depend>tf</build_depend>
-  <run_depend>tf</run_depend>
-  <build_depend>moveit_ros_planning_interface</build_depend>
-  <run_depend>moveit_ros_planning_interface</run_depend>
-  <build_depend>ur5_demo_descartes</build_depend>
-  <run_depend>ur5_demo_descartes</run_depend>
-  <build_depend>descartes_trajectory</build_depend>
-  <run_depend>descartes_trajectory</run_depend>
-  <build_depend>descartes_planner</build_depend>
-  <run_depend>descartes_planner</run_depend>
-  <build_depend>descartes_utilities</build_depend>
-  <run_depend>descartes_utilities</run_depend>
-  <build_depend>trajectory_msgs</build_depend>
-  <run_depend>trajectory_msgs</run_depend>
-  <build_depend>tf_conversions</build_depend>
-  <run_depend>tf_conversions</run_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>ur5_demo_descartes</depend>
+  <depend>descartes_trajectory</depend>
+  <depend>descartes_planner</depend>
+  <depend>descartes_utilities</depend>
+  <depend>trajectory_msgs</depend>
+  <depend>tf_conversions</depend>
+
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
diff --git a/exercises/5.0/src/myworkcell_core/src/adv_descartes_node.cpp b/exercises/5.0/src/myworkcell_core/src/adv_descartes_node.cpp
index aa0b59116..c8116fb20 100644
--- a/exercises/5.0/src/myworkcell_core/src/adv_descartes_node.cpp
+++ b/exercises/5.0/src/myworkcell_core/src/adv_descartes_node.cpp
@@ -25,7 +25,7 @@ std::vector<double> getCurrentJointState(const std::string& topic)
 EigenSTL::vector_Affine3d makeLine(const Eigen::Vector3d& start, const Eigen::Vector3d& stop, double ds)
 {
   EigenSTL::vector_Affine3d line;
-  
+
   const Eigen::Vector3d travel = stop - start;
   const int steps = std::floor(travel.norm() / ds);
 
@@ -61,7 +61,7 @@ class CartesianPlanner
   {
     // Create a robot model
     model_ = boost::make_shared<ur5_demo_descartes::UR5RobotModel>();
-    
+
     // Define the relevant "frames"
     const std::string robot_description = "robot_description";
     const std::string group_name = "puzzle";
@@ -127,7 +127,8 @@ class CartesianPlanner
     EigenSTL::vector_Affine3d path;
     std::ifstream indata;
 
-    std::string filename = ros::package::getPath("myworkcell_core") + "/config/puzzle_bent.csv";
+    std::string packagePath = ros::package::getPath("myworkcell_core");
+    std::string filename = packagePath + "/config/puzzle_bent.csv";
 
     indata.open(filename);
 
@@ -232,8 +233,8 @@ class CartesianPlanner
     {
       auto p = point;
       TolerancedFrame tool_pt(p);
-      tool_pt.orientation_tolerance.z_lower -= M_PI;
-      tool_pt.orientation_tolerance.z_upper += M_PI;
+      tool_pt.orientation_tolerance.z_lower = -M_PI;
+      tool_pt.orientation_tolerance.z_upper = +M_PI;
 
       boost::shared_ptr<CartTrajectoryPt> pt(new CartTrajectoryPt(wobj_base, wobj_pt, tool_base, tool_pt, 0, M_PI/20.0));
       descartes_path.push_back(pt);
diff --git a/exercises/5.0/src/myworkcell_core/src/descartes_node.cpp b/exercises/5.0/src/myworkcell_core/src/descartes_node.cpp
index a69f16af3..8a3f39a25 100644
--- a/exercises/5.0/src/myworkcell_core/src/descartes_node.cpp
+++ b/exercises/5.0/src/myworkcell_core/src/descartes_node.cpp
@@ -117,21 +117,21 @@ class CartesianPlanner
 
     // We assume that our path is centered at (0, 0, 0), so let's define the
     // corners of the AR marker
-    const double side_length = 0.25; // All units are in meters (M)
+    const double side_length = 0.08; // All units are in meters (M)
     const double half_side = side_length / 2.0;
     const double step_size = 0.02;
 
-    Eigen::Vector3d top_left (half_side, half_side, 0);
-    Eigen::Vector3d bot_left (-half_side, half_side, 0);
-    Eigen::Vector3d bot_right (-half_side, -half_side, 0);
-    Eigen::Vector3d top_right (half_side, -half_side, 0);
+    Eigen::Vector3d top_left (-half_side, half_side, 0);
+    Eigen::Vector3d bot_left (-half_side, -half_side, 0);
+    Eigen::Vector3d bot_right (half_side, -half_side, 0);
+    Eigen::Vector3d top_right (half_side, half_side, 0);
 
     // Descartes requires you to guide it in how dense the points should be,
     // so you have to do your own "discretization".
     // NOTE that the makeLine function will create a sequence of points inclusive
     // of the start and exclusive of finish point, i.e. line = [start, stop)
     
-    // Create cartesian path from line-segments
+    // TODO: Add the rest of the cartesian path
     auto segment1 = makeLine(top_left, bot_left, step_size);
     auto segment2 = makeLine(bot_left, bot_right, step_size);
     auto segment3 = makeLine(bot_right, top_right, step_size);
@@ -156,7 +156,7 @@ class CartesianPlanner
 
     for (auto& point : path)
     {
-      // Create a Descartes "cartesian" point with some kind of constraints
+      // TODO: make a Descartes "cartesian" point with some kind of constraints
       descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(ref * point);
       descartes_path.push_back(pt);
     }
diff --git a/exercises/5.0/src/myworkcell_core/src/myworkcell_node.cpp b/exercises/5.0/src/myworkcell_core/src/myworkcell_node.cpp
index ad258ede7..50300867b 100644
--- a/exercises/5.0/src/myworkcell_core/src/myworkcell_node.cpp
+++ b/exercises/5.0/src/myworkcell_core/src/myworkcell_node.cpp
@@ -18,7 +18,6 @@ class ScanNPlan
   void start(const std::string& base_frame)
   {
     ROS_INFO("Attempting to localize part");
-
     // Localize the part
     myworkcell_core::LocalizePart srv;
     srv.request.base_frame = base_frame;
@@ -32,9 +31,10 @@ class ScanNPlan
     ROS_INFO_STREAM("part localized: " << srv.response);
 
     geometry_msgs::Pose move_target = srv.response.pose;
+    moveit::planning_interface::MoveGroupInterface move_group("manipulator");
 
     // Plan for robot to move to part
-    moveit::planning_interface::MoveGroupInterface move_group("manipulator");
+    move_group.setPoseReferenceFrame(base_frame);
     move_group.setPoseTarget(move_target);
     move_group.move();
 
@@ -67,8 +67,9 @@ int main(int argc, char **argv)
 {
   ros::init(argc, argv, "myworkcell_node");
   ros::NodeHandle nh;
-  ros::NodeHandle private_node_handle("~");
-  ros::AsyncSpinner async_spinner (1);
+  ros::NodeHandle private_node_handle ("~");
+  ros::AsyncSpinner async_spinner(1);
+  async_spinner.start();
 
   ROS_INFO("ScanNPlan node has been initialized");
 
@@ -76,12 +77,9 @@ int main(int argc, char **argv)
   private_node_handle.param<std::string>("base_frame", base_frame, "world"); // parameter name, string object reference, default value
 
   ScanNPlan app(nh);
-  ros::Duration(.5).sleep();  // wait for the class to initialize
 
-  async_spinner.start();
+  ros::Duration(.5).sleep();  // wait for the class to initialize
   app.start(base_frame);
 
-  ROS_INFO("ScanNPlan node has been initialized");
-
   ros::waitForShutdown();
 }
diff --git a/exercises/5.0/src/myworkcell_core/src/vision_node.cpp b/exercises/5.0/src/myworkcell_core/src/vision_node.cpp
index abcff5f5e..b779917ba 100644
--- a/exercises/5.0/src/myworkcell_core/src/vision_node.cpp
+++ b/exercises/5.0/src/myworkcell_core/src/vision_node.cpp
@@ -9,59 +9,59 @@
 class Localizer
 {
 public:
-    Localizer(ros::NodeHandle& nh)
-    {
-        ar_sub_ = nh.subscribe<fake_ar_publisher::ARMarker>("ar_pose_marker", 1,
-        &Localizer::visionCallback, this);
+  Localizer(ros::NodeHandle& nh)
+  {
+      ar_sub_ = nh.subscribe<fake_ar_publisher::ARMarker>("ar_pose_marker", 1,
+      &Localizer::visionCallback, this);
 
-        server_ = nh.advertiseService("localize_part", &Localizer::localizePart, this);
-    }
+      server_ = nh.advertiseService("localize_part", &Localizer::localizePart, this);
+  }
 
-    void visionCallback(const fake_ar_publisher::ARMarkerConstPtr& msg)
-    {
-        last_msg_ = msg;
-        //ROS_INFO_STREAM(last_msg_->pose.pose);
-    }
+  void visionCallback(const fake_ar_publisher::ARMarkerConstPtr& msg)
+  {
+      last_msg_ = msg;
+      //ROS_INFO_STREAM(last_msg_->pose.pose);
+  }
 
-    bool localizePart(myworkcell_core::LocalizePart::Request& req,
-                      myworkcell_core::LocalizePart::Response& res)
-    {
-      // Read last message
-      fake_ar_publisher::ARMarkerConstPtr p = last_msg_;
-      if (!p) return false;
+  bool localizePart(myworkcell_core::LocalizePart::Request& req,
+                    myworkcell_core::LocalizePart::Response& res)
+  {
+    // Read last message
+    fake_ar_publisher::ARMarkerConstPtr p = last_msg_;
+    if (!p) return false;
 
-      tf::Transform cam_to_target;
-      tf::poseMsgToTF(p->pose.pose, cam_to_target);
+    tf::Transform cam_to_target;
+    tf::poseMsgToTF(p->pose.pose, cam_to_target);
 
-      tf::StampedTransform req_to_cam;
-      listener_.lookupTransform(req.base_frame, p->header.frame_id, ros::Time(0), req_to_cam);
+    tf::StampedTransform req_to_cam;
+    listener_.lookupTransform(req.base_frame, p->header.frame_id, ros::Time(0), req_to_cam);
 
-      tf::Transform req_to_target;
-      req_to_target = req_to_cam * cam_to_target;
+    tf::Transform req_to_target;
+    req_to_target = req_to_cam * cam_to_target;
 
-      tf::poseTFToMsg(req_to_target, res.pose);
-      return true;
-    }
+    tf::poseTFToMsg(req_to_target, res.pose);
+    return true;
+  }
 
-    ros::Subscriber ar_sub_;
-    fake_ar_publisher::ARMarkerConstPtr last_msg_;
-    ros::ServiceServer server_;
-    tf::TransformListener listener_;
+  ros::Subscriber ar_sub_;
+  fake_ar_publisher::ARMarkerConstPtr last_msg_;
+  ros::ServiceServer server_;
+  tf::TransformListener listener_;
 };
 
 int main(int argc, char* argv[])
 {
-    // This must be called before anything else ROS-related
-    ros::init(argc, argv, "vision_node");
+  // This must be called before anything else ROS-related
+  ros::init(argc, argv, "vision_node");
 
-    // Create a ROS node handle
-    ros::NodeHandle nh;
+  // Create a ROS node handle
+  ros::NodeHandle nh;
 
-    // The Localizer class provides this node's ROS interfaces
-    Localizer localizer(nh);
+  // The Localizer class provides this node's ROS interfaces
+  Localizer localizer(nh);
 
-    ROS_INFO("Vision node starting");
+  ROS_INFO("Vision node starting");
 
-    // Don't exit the program.
-    ros::spin();
+  // Don't exit the program.
+  ros::spin();
 }
diff --git a/exercises/5.0/src/myworkcell_moveit_config/.setup_assistant b/exercises/5.0/src/myworkcell_moveit_config/.setup_assistant
index 372315205..2f37a29d2 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/.setup_assistant
+++ b/exercises/5.0/src/myworkcell_moveit_config/.setup_assistant
@@ -2,6 +2,7 @@ moveit_setup_assistant_config:
   URDF:
     package: myworkcell_support
     relative_path: urdf/workcell.xacro
+    xacro_args: "--inorder "
   SRDF:
     relative_path: config/myworkcell.srdf
   CONFIG:
diff --git a/exercises/5.0/src/myworkcell_moveit_config/config/controllers.yaml b/exercises/5.0/src/myworkcell_moveit_config/config/controllers.yaml
index 6e8ef19f7..a2479ff01 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/config/controllers.yaml
+++ b/exercises/5.0/src/myworkcell_moveit_config/config/controllers.yaml
@@ -2,4 +2,4 @@ controller_list:
   - name: ""
     action_ns: joint_trajectory_action
     type: FollowJointTrajectory
-    joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 
\ No newline at end of file
+    joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
diff --git a/exercises/5.0/src/myworkcell_moveit_config/config/fake_controllers.yaml b/exercises/5.0/src/myworkcell_moveit_config/config/fake_controllers.yaml
index a7300cf48..470ad88c3 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/config/fake_controllers.yaml
+++ b/exercises/5.0/src/myworkcell_moveit_config/config/fake_controllers.yaml
@@ -1,5 +1,13 @@
 controller_list:
   - name: fake_manipulator_controller
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+  - name: fake_puzzle_controller
     joints:
       - shoulder_pan_joint
       - shoulder_lift_joint
diff --git a/exercises/5.0/src/myworkcell_moveit_config/config/joint_names.yaml b/exercises/5.0/src/myworkcell_moveit_config/config/joint_names.yaml
index dce3623f7..11bd6762f 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/config/joint_names.yaml
+++ b/exercises/5.0/src/myworkcell_moveit_config/config/joint_names.yaml
@@ -1 +1 @@
-controller_joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 
\ No newline at end of file
+controller_joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
diff --git a/exercises/5.0/src/myworkcell_moveit_config/config/kinematics.yaml b/exercises/5.0/src/myworkcell_moveit_config/config/kinematics.yaml
index 969ba1425..5e9d57ce7 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/config/kinematics.yaml
+++ b/exercises/5.0/src/myworkcell_moveit_config/config/kinematics.yaml
@@ -1,4 +1,9 @@
 manipulator:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.005
+  kinematics_solver_attempts: 3
+puzzle:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
   kinematics_solver_timeout: 0.005
diff --git a/exercises/5.0/src/myworkcell_moveit_config/config/myworkcell.srdf b/exercises/5.0/src/myworkcell_moveit_config/config/myworkcell.srdf
index 3ee2945c6..ba7d13f1b 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/config/myworkcell.srdf
+++ b/exercises/5.0/src/myworkcell_moveit_config/config/myworkcell.srdf
@@ -12,7 +12,6 @@
     <group name="manipulator">
         <chain base_link="base_link" tip_link="tool0" />
     </group>
-
     <group name="puzzle">
         <chain base_link="base_link" tip_link="part" />
     </group>
@@ -39,8 +38,6 @@
     <disable_collisions link1="base_link" link2="grinder_frame" reason="Never" />
     <disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent" />
     <disable_collisions link1="base_link" link2="table" reason="Adjacent" />
-    <disable_collisions link1="base_link" link2="upper_arm_link" reason="Never" />
-    <disable_collisions link1="base_link" link2="wrist_1_link" reason="Never" />
     <disable_collisions link1="ee_base" link2="ee_link" reason="Never" />
     <disable_collisions link1="ee_base" link2="part" reason="Adjacent" />
     <disable_collisions link1="ee_base" link2="wrist_1_link" reason="Never" />
@@ -54,14 +51,11 @@
     <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent" />
     <disable_collisions link1="grinder_frame" link2="shoulder_link" reason="Never" />
     <disable_collisions link1="grinder_frame" link2="table" reason="Adjacent" />
-    <disable_collisions link1="grinder_frame" link2="upper_arm_link" reason="Never" />
     <disable_collisions link1="part" link2="wrist_1_link" reason="Never" />
     <disable_collisions link1="part" link2="wrist_2_link" reason="Never" />
     <disable_collisions link1="part" link2="wrist_3_link" reason="Never" />
-    <disable_collisions link1="shoulder_link" link2="table" reason="Never" />
+    <disable_collisions link1="shoulder_link" link2="table" reason="Default" />
     <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent" />
-    <disable_collisions link1="shoulder_link" link2="wrist_1_link" reason="Never" />
-    <disable_collisions link1="shoulder_link" link2="wrist_2_link" reason="Never" />
     <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent" />
     <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never" />
     <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent" />
diff --git a/exercises/5.0/src/myworkcell_moveit_config/config/ompl_planning.yaml b/exercises/5.0/src/myworkcell_moveit_config/config/ompl_planning.yaml
index 4dc5746f8..5a93de273 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/config/ompl_planning.yaml
+++ b/exercises/5.0/src/myworkcell_moveit_config/config/ompl_planning.yaml
@@ -121,6 +121,7 @@ planner_configs:
     dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
     max_failures: 5000  # maximum consecutive failure limit. default: 5000
 manipulator:
+  default_planner_config: NonekConfigDefault
   planner_configs:
     - SBLkConfigDefault
     - ESTkConfigDefault
@@ -146,4 +147,32 @@ manipulator:
     - SPARSkConfigDefault
     - SPARStwokConfigDefault
   projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
-  longest_valid_segment_fraction: 0.005
\ No newline at end of file
+  longest_valid_segment_fraction: 0.005
+puzzle:
+  default_planner_config: NonekConfigDefault
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
+    - FMTkConfigDefault
+    - BFMTkConfigDefault
+    - PDSTkConfigDefault
+    - STRIDEkConfigDefault
+    - BiTRRTkConfigDefault
+    - LBTRRTkConfigDefault
+    - BiESTkConfigDefault
+    - ProjESTkConfigDefault
+    - LazyPRMkConfigDefault
+    - LazyPRMstarkConfigDefault
+    - SPARSkConfigDefault
+    - SPARStwokConfigDefault
+  projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
+  longest_valid_segment_fraction: 0.005
diff --git a/exercises/5.0/src/myworkcell_moveit_config/launch/demo.launch b/exercises/5.0/src/myworkcell_moveit_config/launch/demo.launch
index 0fbfd476a..ff5aebe5b 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/launch/demo.launch
+++ b/exercises/5.0/src/myworkcell_moveit_config/launch/demo.launch
@@ -8,6 +8,16 @@
   <!-- By default, we are not in debug mode -->
   <arg name="debug" default="false" />
 
+  <!--
+  By default, hide joint_state_publisher's GUI
+
+  MoveIt!'s "demo" mode replaces the real robot driver with the joint_state_publisher.
+  The latter one maintains and publishes the current joint configuration of the simulated robot.
+  It also provides a GUI to move the simulated robot around "manually".
+  This corresponds to moving around the real robot without the use of MoveIt.
+  -->
+  <arg name="use_gui" default="false" />
+
   <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
   <include file="$(find myworkcell_moveit_config)/launch/planning_context.launch">
     <arg name="load_robot_description" value="true"/>
@@ -18,8 +28,8 @@
 
   <!-- We do not have a robot connected, so publish fake joint states -->
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="/use_gui" value="false"/>
-    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
+    <param name="use_gui" value="$(arg use_gui)"/>
+    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
   </node>
 
   <!-- Given the published joint states, publish tf for the robot links -->
diff --git a/exercises/5.0/src/myworkcell_moveit_config/launch/joystick_control.launch b/exercises/5.0/src/myworkcell_moveit_config/launch/joystick_control.launch
index f74173527..9411f6e60 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/launch/joystick_control.launch
+++ b/exercises/5.0/src/myworkcell_moveit_config/launch/joystick_control.launch
@@ -13,5 +13,5 @@
 
   <!-- Launch python interface -->
   <node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>
-        
+
 </launch>
diff --git a/exercises/5.0/src/myworkcell_moveit_config/launch/moveit_rviz.launch b/exercises/5.0/src/myworkcell_moveit_config/launch/moveit_rviz.launch
index e70fabf89..222f1ab53 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/launch/moveit_rviz.launch
+++ b/exercises/5.0/src/myworkcell_moveit_config/launch/moveit_rviz.launch
@@ -7,7 +7,7 @@
   <arg name="config" default="false" />
   <arg unless="$(arg config)" name="command_args" value="" />
   <arg     if="$(arg config)" name="command_args" value="-d $(find myworkcell_moveit_config)/launch/moveit.rviz" />
-  
+
   <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
 	args="$(arg command_args)" output="screen">
     <rosparam command="load" file="$(find myworkcell_moveit_config)/config/kinematics.yaml"/>
diff --git a/exercises/5.0/src/myworkcell_moveit_config/launch/myworkcell_planning_execution.launch b/exercises/5.0/src/myworkcell_moveit_config/launch/myworkcell_planning_execution.launch
index cd8466f0c..524adb168 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/launch/myworkcell_planning_execution.launch
+++ b/exercises/5.0/src/myworkcell_moveit_config/launch/myworkcell_planning_execution.launch
@@ -1,17 +1,4 @@
-<?xml version="1.0" ?><launch>
-  <!-- The planning and execution components of MoveIt! configured to run -->
-  <!-- using the ROS-Industrial interface. -->
-
-  <!-- the "sim" argument controls whether we connect to a Simulated or Real robot -->
-  <!--  - if sim=false, a robot_ip argument is required -->
-  <arg name="sim" default="true" />
-  <arg name="robot_ip" unless="$(arg sim)" />
-
-  <arg name="prefix" default="" />
-  <arg name="min_payload" default="0.0" />
-  <arg name="max_payload" default="5.0" />
-
-
+<launch>
   <!-- The planning and execution components of MoveIt! configured to run -->
   <!-- using the ROS-Industrial interface. -->
 
@@ -22,39 +9,37 @@
        - and uncomment the following line: -->
   <rosparam command="load" file="$(find myworkcell_moveit_config)/config/joint_names.yaml"/>
 
+  <!-- the "sim" argument controls whether we connect to a Simulated or Real robot -->
+  <!--  - if sim=false, a robot_ip argument is required -->
+  <arg name="sim" default="true" />
+  <arg name="robot_ip" unless="$(arg sim)" />
+
   <!-- load the robot_description parameter before launching ROS-I nodes -->
   <include file="$(find myworkcell_moveit_config)/launch/planning_context.launch" >
-    <arg name="load_robot_description" value="true" />
+   <arg name="load_robot_description" value="true" />
   </include>
 
   <!-- run the robot simulator and action interface nodes -->
   <group if="$(arg sim)">
     <include file="$(find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
-
-    <!-- publish the robot state (tf transforms) -->
-    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
   </group>
 
   <!-- run the "real robot" interface nodes -->
+  <!--   - this typically includes: robot_state, motion_interface, and joint_trajectory_action nodes -->
+  <!--   - replace these calls with appropriate robot-specific calls or launch files -->
   <group unless="$(arg sim)">
-    <remap from="/follow_joint_trajectory/status" to="/joint_trajectory_action/status" />
-    <remap from="/follow_joint_trajectory/feedback" to="/joint_trajectory_action/feedback" />
-    <remap from="/follow_joint_trajectory/result" to="/joint_trajectory_action/result" />
-    <remap from="/follow_joint_trajectory/goal" to="/joint_trajectory_action/goal" />
-    <remap from="/follow_joint_trajectory/cancel" to="/joint_trajectory_action/cancel" />
-
-    <include file="$(find ur_modern_driver)/launch/ur_common.launch">
-      <arg name="prefix" value="$(arg prefix)" />
-      <arg name="robot_ip" value="$(arg robot_ip)" />
-      <arg name="min_payload" value="$(arg min_payload)" />
-      <arg name="max_payload" value="$(arg max_payload)" />
-    </include>
+    <include file="$(find ur_bringup)/launch/ur5_bringup.launch" />
   </group>
 
+  <!-- publish the robot state (tf transforms) -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
 
   <include file="$(find myworkcell_moveit_config)/launch/move_group.launch">
     <arg name="publish_monitored_planning_scene" value="true" />
   </include>
 
-  <rosparam command="load" file="$(find myworkcell_moveit_config)/config/joint_names.yaml"/>
+  <include file="$(find myworkcell_moveit_config)/launch/moveit_rviz.launch">
+    <arg name="config" value="true"/>
+  </include>
+
 </launch>
diff --git a/exercises/5.0/src/myworkcell_moveit_config/launch/ompl_planning_pipeline.launch.xml b/exercises/5.0/src/myworkcell_moveit_config/launch/ompl_planning_pipeline.launch.xml
index f3dc28191..6a23df4c7 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/launch/ompl_planning_pipeline.launch.xml
+++ b/exercises/5.0/src/myworkcell_moveit_config/launch/ompl_planning_pipeline.launch.xml
@@ -3,7 +3,7 @@
   <!-- OMPL Plugin for MoveIt! -->
   <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
 
-  <!-- The request adapters (plugins) used when planning with OMPL. 
+  <!-- The request adapters (plugins) used when planning with OMPL.
        ORDER MATTERS -->
   <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
 				       default_planner_request_adapters/FixWorkspaceBounds
diff --git a/exercises/5.0/src/myworkcell_moveit_config/launch/planning_context.launch b/exercises/5.0/src/myworkcell_moveit_config/launch/planning_context.launch
index a97db329a..1ed54b26c 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/launch/planning_context.launch
+++ b/exercises/5.0/src/myworkcell_moveit_config/launch/planning_context.launch
@@ -6,11 +6,11 @@
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="$(find xacro)/xacro --inorder '$(find myworkcell_support)/urdf/workcell.xacro'"/>
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro --inorder  '$(find myworkcell_support)/urdf/workcell.xacro'"/>
 
   <!-- The semantic description that corresponds to the URDF -->
   <param name="$(arg robot_description)_semantic" textfile="$(find myworkcell_moveit_config)/config/myworkcell.srdf" />
-  
+
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
     <rosparam command="load" file="$(find myworkcell_moveit_config)/config/joint_limits.yaml"/>
@@ -20,5 +20,5 @@
   <group ns="$(arg robot_description)_kinematics">
     <rosparam command="load" file="$(find myworkcell_moveit_config)/config/kinematics.yaml"/>
   </group>
-  
+
 </launch>
diff --git a/exercises/5.0/src/myworkcell_moveit_config/launch/planning_pipeline.launch.xml b/exercises/5.0/src/myworkcell_moveit_config/launch/planning_pipeline.launch.xml
index 8f70792dd..9996548af 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/launch/planning_pipeline.launch.xml
+++ b/exercises/5.0/src/myworkcell_moveit_config/launch/planning_pipeline.launch.xml
@@ -1,7 +1,7 @@
 <launch>
 
-  <!-- This file makes it easy to include different planning pipelines; 
-       It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->  
+  <!-- This file makes it easy to include different planning pipelines;
+       It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->
 
   <arg name="pipeline" default="ompl" />
 
diff --git a/exercises/5.0/src/myworkcell_moveit_config/launch/run_benchmark_ompl.launch b/exercises/5.0/src/myworkcell_moveit_config/launch/run_benchmark_ompl.launch
index 07efe1263..09ef8af4f 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/launch/run_benchmark_ompl.launch
+++ b/exercises/5.0/src/myworkcell_moveit_config/launch/run_benchmark_ompl.launch
@@ -11,7 +11,7 @@
   <!-- Start the database -->
   <include file="$(find myworkcell_moveit_config)/launch/warehouse.launch">
     <arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>
-  </include>  
+  </include>
 
   <!-- Start Benchmark Executable -->
   <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
diff --git a/exercises/5.0/src/myworkcell_moveit_config/launch/sensor_manager.launch.xml b/exercises/5.0/src/myworkcell_moveit_config/launch/sensor_manager.launch.xml
index e4bbdc326..02b00a51a 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/launch/sensor_manager.launch.xml
+++ b/exercises/5.0/src/myworkcell_moveit_config/launch/sensor_manager.launch.xml
@@ -1,6 +1,6 @@
 <launch>
 
-  <!-- This file makes it easy to include the settings for sensor managers -->  
+  <!-- This file makes it easy to include the settings for sensor managers -->
 
   <!-- Params for the octomap monitor -->
   <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
@@ -10,5 +10,5 @@
   <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
   <arg name="moveit_sensor_manager" default="myworkcell" />
   <include file="$(find myworkcell_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
-  
+
 </launch>
diff --git a/exercises/5.0/src/myworkcell_moveit_config/launch/setup_assistant.launch b/exercises/5.0/src/myworkcell_moveit_config/launch/setup_assistant.launch
index 85d597262..112b0f1e9 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/launch/setup_assistant.launch
+++ b/exercises/5.0/src/myworkcell_moveit_config/launch/setup_assistant.launch
@@ -7,7 +7,7 @@
   <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
 
   <!-- Run -->
-  <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant" 
+  <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
 	args="--config_pkg=myworkcell_moveit_config"
 	launch-prefix="$(arg launch_prefix)"
         output="screen" />
diff --git a/exercises/5.0/src/myworkcell_moveit_config/launch/trajectory_execution.launch.xml b/exercises/5.0/src/myworkcell_moveit_config/launch/trajectory_execution.launch.xml
index 430ce2f2c..a40b25f9f 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/launch/trajectory_execution.launch.xml
+++ b/exercises/5.0/src/myworkcell_moveit_config/launch/trajectory_execution.launch.xml
@@ -1,6 +1,6 @@
 <launch>
 
-  <!-- This file makes it easy to include the settings for trajectory execution  -->  
+  <!-- This file makes it easy to include the settings for trajectory execution  -->
 
   <!-- Flag indicating whether MoveIt! is allowed to load/unload  or switch controllers -->
   <arg name="moveit_manage_controllers" default="true"/>
@@ -12,9 +12,9 @@
   <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
   <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
   <param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->
-  
+
   <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
   <arg name="moveit_controller_manager" default="myworkcell" />
   <include file="$(find myworkcell_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />
-  
+
 </launch>
diff --git a/exercises/5.0/src/myworkcell_moveit_config/launch/warehouse.launch b/exercises/5.0/src/myworkcell_moveit_config/launch/warehouse.launch
index ee814adcb..4d163e27c 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/launch/warehouse.launch
+++ b/exercises/5.0/src/myworkcell_moveit_config/launch/warehouse.launch
@@ -1,9 +1,9 @@
 <launch>
-  
+
   <!-- The path to the database must be specified -->
   <arg name="moveit_warehouse_database_path" />
 
-  <!-- Load warehouse parameters -->  
+  <!-- Load warehouse parameters -->
   <include file="$(find myworkcell_moveit_config)/launch/warehouse_settings.launch.xml" />
 
   <!-- Run the DB server -->
diff --git a/exercises/5.0/src/myworkcell_moveit_config/launch/warehouse_settings.launch.xml b/exercises/5.0/src/myworkcell_moveit_config/launch/warehouse_settings.launch.xml
index 967e0ff4d..e473b083b 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/launch/warehouse_settings.launch.xml
+++ b/exercises/5.0/src/myworkcell_moveit_config/launch/warehouse_settings.launch.xml
@@ -1,12 +1,12 @@
 <launch>
   <!-- Set the parameters for the warehouse and run the mongodb server. -->
 
-  <!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->  
-  <arg name="moveit_warehouse_port" default="33829" /> 
+  <!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->
+  <arg name="moveit_warehouse_port" default="33829" />
 
   <!-- The default DB host for moveit -->
-  <arg name="moveit_warehouse_host" default="localhost" /> 
-  
+  <arg name="moveit_warehouse_host" default="localhost" />
+
   <!-- Set parameters for the warehouse -->
   <param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>
   <param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>
diff --git a/exercises/5.0/src/myworkcell_moveit_config/package.xml b/exercises/5.0/src/myworkcell_moveit_config/package.xml
index 11538d364..f9573d757 100644
--- a/exercises/5.0/src/myworkcell_moveit_config/package.xml
+++ b/exercises/5.0/src/myworkcell_moveit_config/package.xml
@@ -17,6 +17,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <run_depend>moveit_ros_move_group</run_depend>
+  <run_depend>moveit_fake_controller_manager</run_depend>
   <run_depend>moveit_kinematics</run_depend>
   <run_depend>moveit_planners_ompl</run_depend>
   <run_depend>moveit_ros_visualization</run_depend>
diff --git a/exercises/5.0/src/myworkcell_support/CMakeLists.txt b/exercises/5.0/src/myworkcell_support/CMakeLists.txt
index 42505d9f3..b94eb41af 100644
--- a/exercises/5.0/src/myworkcell_support/CMakeLists.txt
+++ b/exercises/5.0/src/myworkcell_support/CMakeLists.txt
@@ -1,8 +1,8 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(myworkcell_support)
 
-## Add support for C++11, supported in ROS Kinetic and newer
-# add_definitions(-std=c++11)
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -30,10 +30,10 @@ find_package(catkin REQUIRED COMPONENTS
 ##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
 ## * In the file package.xml:
 ##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
 ##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
 ##     but can be declared for certainty nonetheless:
-##     * add a run_depend tag for "message_runtime"
+##     * add a exec_depend tag for "message_runtime"
 ## * In this file (CMakeLists.txt):
 ##   * add "message_generation" and every package in MSG_DEP_SET to
 ##     find_package(catkin REQUIRED COMPONENTS ...)
@@ -78,7 +78,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## To declare and build dynamic reconfigure parameters within this
 ## package, follow these steps:
 ## * In the file package.xml:
-##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
 ## * In this file (CMakeLists.txt):
 ##   * add "dynamic_reconfigure" to
 ##     find_package(catkin REQUIRED COMPONENTS ...)
@@ -96,7 +96,7 @@ find_package(catkin REQUIRED COMPONENTS
 ###################################
 ## The catkin_package macro generates cmake config files for your package
 ## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if you package contains header files
+## INCLUDE_DIRS: uncomment this if your package contains header files
 ## LIBRARIES: libraries you create in this project that dependent projects also need
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
@@ -113,8 +113,8 @@ catkin_package(
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-# include_directories(include)
 include_directories(
+# include
   ${catkin_INCLUDE_DIRS}
 )
 
diff --git a/exercises/5.0/src/myworkcell_support/launch/setup.launch b/exercises/5.0/src/myworkcell_support/launch/setup.launch
index df2c2a03a..eccc254ad 100644
--- a/exercises/5.0/src/myworkcell_support/launch/setup.launch
+++ b/exercises/5.0/src/myworkcell_support/launch/setup.launch
@@ -1,23 +1,8 @@
 <launch>
-	<arg name="sim_robot" default="true"/>
-	<arg name="robot_ip" if="$(arg sim_robot)"/>
-
-  <arg name="adv" default="0" />
-  <include file="$(find myworkcell_moveit_config)/launch/myworkcell_planning_execution.launch">
-		<arg name="sim" value="$(arg sim_robot)"/>
-		<arg name="robot_ip" value="$(arg robot_ip)" unless="$(arg sim_robot)"  />
-	</include>
+  <arg name="adv" default="0" doc="enable Advanced Descartes example"/>
+  <include file="$(find myworkcell_moveit_config)/launch/myworkcell_planning_execution.launch"/>
   <node name="fake_ar_publisher" pkg="fake_ar_publisher" type="fake_ar_publisher_node" />
   <node name="vision_node" type="vision_node" pkg="myworkcell_core" output="screen"/>
   <node name="descartes_node" type="descartes_node" pkg="myworkcell_core" output="screen" unless="$(arg adv)"/>
   <node name="adv_descartes_node" type="adv_descartes_node" pkg="myworkcell_core" output="screen" if="$(arg adv)"/>
-
-  <!-- Run Rviz and load the default config to see the state of the move_group node -->
-  <include file="$(find myworkcell_moveit_config)/launch/moveit_rviz.launch">
-    <arg name="config" value="true"/>
-    <arg name="debug" value="false"/>
-  </include>
-
-
-	
 </launch>
diff --git a/exercises/5.0/src/myworkcell_support/launch/workcell.launch b/exercises/5.0/src/myworkcell_support/launch/workcell.launch
index f11ad2bf5..717abe47e 100644
--- a/exercises/5.0/src/myworkcell_support/launch/workcell.launch
+++ b/exercises/5.0/src/myworkcell_support/launch/workcell.launch
@@ -1,6 +1,6 @@
 <launch>
-  <node name="fake_ar_publisher" pkg="fake_ar_publisher" type="fake_ar_publisher_node" output="screen"/>
-  <node name="vision_node" pkg="myworkcell_core" type="vision_node" output="screen"/>
+  <node name="fake_ar_publisher" pkg="fake_ar_publisher" type="fake_ar_publisher_node" output="screen" />
+  <node name="vision_node" pkg="myworkcell_core" type="vision_node" output="screen" />
   <node name="myworkcell_node" pkg="myworkcell_core" type="myworkcell_node" output="screen">
     <param name="base_frame" value="world"/>
   </node>
diff --git a/exercises/5.0/src/myworkcell_support/package.xml b/exercises/5.0/src/myworkcell_support/package.xml
index e37dc8ee6..d6ec3de83 100644
--- a/exercises/5.0/src/myworkcell_support/package.xml
+++ b/exercises/5.0/src/myworkcell_support/package.xml
@@ -1,10 +1,10 @@
 <?xml version="1.0"?>
-<package>
+<package format="2">
   <name>myworkcell_support</name>
   <version>0.0.0</version>
   <description>The myworkcell_support package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="ros-industrial@todo.todo">ros-industrial</maintainer>
@@ -28,20 +28,31 @@
   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
 
 
-  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- The *depend tags are used to specify dependencies -->
   <!-- Dependencies can be catkin packages or system dependencies -->
   <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
   <!-- Use build_depend for packages you need at compile time: -->
   <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_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 exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>myworkcell_core</build_depend>
-  <run_depend>myworkcell_core</run_depend>
+  <build_export_depend>myworkcell_core</build_export_depend>
+  <exec_depend>myworkcell_core</exec_depend>
+
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
diff --git a/exercises/5.0/src/myworkcell_support/urdf/workcell.xacro b/exercises/5.0/src/myworkcell_support/urdf/workcell.xacro
index dfb952acb..623454123 100644
--- a/exercises/5.0/src/myworkcell_support/urdf/workcell.xacro
+++ b/exercises/5.0/src/myworkcell_support/urdf/workcell.xacro
@@ -1,10 +1,8 @@
 <?xml version="1.0" ?>
 <robot name="myworkcell" xmlns:xacro="http://ros.org/wiki/xacro">
   <xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />
-
-  <!-- This contains the macro "ses_part_macro" that places the clip wrt the tool tip -->
-  <include filename="$(find myworkcell_support)/urdf/puzzle_mount.xacro" />
-  <include filename="$(find myworkcell_support)/urdf/grinder.xacro" />
+  <xacro:include filename="$(find myworkcell_support)/urdf/grinder.xacro" />
+  <xacro:include filename="$(find myworkcell_support)/urdf/puzzle_mount.xacro" />
 
   <link name="world"/>
 
@@ -23,7 +21,6 @@
 
   <link name="camera_frame"/>
 
-
   <xacro:ur5_robot prefix="" joint_limited="true"/>
 
   <joint name="world_to_table" type="fixed">
@@ -32,12 +29,6 @@
     <origin xyz="0 0 0.5" rpy="0 0 0"/>
   </joint>
 
-  <joint name="world_to_grinder" type="fixed">
-    <parent link="world"/>
-    <child link="grinder_frame"/>
-    <origin xyz="0.0 -0.4 0.6" rpy="0 3.14159 0"/>
-  </joint>
-
   <joint name="world_to_camera" type="fixed">
     <parent link="world"/>
     <child link="camera_frame"/>
@@ -50,7 +41,13 @@
     <origin xyz="0 0 0" rpy="0 0 0"/>
   </joint>
 
-  <joint name="robot_tool" type="fixed">
+  <joint name="world_to_grinder" type="fixed">
+    <parent link="world"/>
+    <child link="grinder_frame"/>
+    <origin xyz="0 -0.4 0.6" rpy="0 3.14159 0"/>
+  </joint>
+
+  <joint name="tool_to_puzzle" type="fixed">
     <parent link="tool0"/>
     <child link="ee_mount"/>
     <origin xyz="0 0 0" rpy="1.5708 0 0"/>
diff --git a/exercises/5.0/src/ur10_demo_descartes/CMakeLists.txt b/exercises/5.0/src/ur10_demo_descartes/CMakeLists.txt
index e12fdaeb7..fda897516 100644
--- a/exercises/5.0/src/ur10_demo_descartes/CMakeLists.txt
+++ b/exercises/5.0/src/ur10_demo_descartes/CMakeLists.txt
@@ -7,7 +7,7 @@ find_package(catkin REQUIRED descartes_core descartes_moveit pluginlib moveit_co
 find_package(rosconsole_bridge REQUIRED)
 find_package(LAPACK REQUIRED)
 find_package(Boost REQUIRED)
-find_package(Eigen REQUIRED)
+find_package(Eigen3 REQUIRED)
 
 ###################################
 ## catkin specific configuration ##
@@ -32,7 +32,7 @@ catkin_package(
 include_directories(include
   ${catkin_INCLUDE_DIRS}
   ${Boost_INCLUDE_DIRS}
-  ${Eigen_INCLUDE_DIRS}
+  ${Eigen3_INCLUDE_DIRS}
 )
 
 add_library(${PROJECT_NAME}
@@ -42,7 +42,7 @@ add_library(${PROJECT_NAME}
   src/plugin_init.cpp)
 
 target_link_libraries(${PROJECT_NAME}
-  ${catkin_LIBRARIES} ${Eigen_LIBRARIES}  ${Boost_LIBRARIES} ${LAPACK_LIBRARIES})
+  ${catkin_LIBRARIES} ${Eigen3_LIBRARIES}  ${Boost_LIBRARIES} ${LAPACK_LIBRARIES})
 
 #############
 ## Install ##
diff --git a/exercises/5.0/src/ur5_demo_descartes/CMakeLists.txt b/exercises/5.0/src/ur5_demo_descartes/CMakeLists.txt
index cea5ffdec..a1e8c6a37 100644
--- a/exercises/5.0/src/ur5_demo_descartes/CMakeLists.txt
+++ b/exercises/5.0/src/ur5_demo_descartes/CMakeLists.txt
@@ -7,7 +7,7 @@ find_package(catkin REQUIRED descartes_core descartes_moveit pluginlib moveit_co
 find_package(rosconsole_bridge REQUIRED)
 find_package(LAPACK REQUIRED)
 find_package(Boost REQUIRED)
-find_package(Eigen REQUIRED)
+find_package(Eigen3 REQUIRED)
 
 ###################################
 ## catkin specific configuration ##
@@ -32,7 +32,7 @@ catkin_package(
 include_directories(include
   ${catkin_INCLUDE_DIRS}
   ${Boost_INCLUDE_DIRS}
-  ${Eigen_INCLUDE_DIRS}
+  ${Eigen3_INCLUDE_DIRS}
 )
 
 add_library(${PROJECT_NAME}
@@ -42,7 +42,7 @@ add_library(${PROJECT_NAME}
   src/plugin_init.cpp)
 
 target_link_libraries(${PROJECT_NAME}
-  ${catkin_LIBRARIES} ${Eigen_LIBRARIES}  ${Boost_LIBRARIES} ${LAPACK_LIBRARIES})
+  ${catkin_LIBRARIES} ${Eigen3_LIBRARIES}  ${Boost_LIBRARIES} ${LAPACK_LIBRARIES})
 
 #############
 ## Install ##
diff --git a/exercises/5.2/src/lesson_perception/CMakeLists.txt b/exercises/5.1/solution_ws/src/lesson_perception/CMakeLists.txt
similarity index 100%
rename from exercises/5.2/src/lesson_perception/CMakeLists.txt
rename to exercises/5.1/solution_ws/src/lesson_perception/CMakeLists.txt
diff --git a/exercises/5.2/src/lesson_perception/launch/processing_node.launch b/exercises/5.1/solution_ws/src/lesson_perception/launch/processing_node.launch
similarity index 87%
rename from exercises/5.2/src/lesson_perception/launch/processing_node.launch
rename to exercises/5.1/solution_ws/src/lesson_perception/launch/processing_node.launch
index 7629a911c..ea7675ff4 100644
--- a/exercises/5.2/src/lesson_perception/launch/processing_node.launch
+++ b/exercises/5.1/solution_ws/src/lesson_perception/launch/processing_node.launch
@@ -1,7 +1,5 @@
-<?xml version="1.0" ?>
 <launch>
-
-  <node name="processing_node" pkg="lesson_perception" type="perception_node" output="screen">  
+  <node name="processing_node" pkg="lesson_perception" type="perception_node" output="screen">
     <rosparam>
       cloud_topic: "kinect/depth_registered/points"
       world_frame: "world_frame"
@@ -15,7 +13,7 @@
       z_filter_max: 2.5 <!-- m -->
       plane_max_iterations: 100
       plane_distance_threshold: 0.03
-      cluster_tolerance: 0.02
+      cluster_tolerance: 0.01
       cluster_min_size: 250
       cluster_max_size: 500000
     </rosparam>
diff --git a/exercises/5.2/src/lesson_perception/package.xml b/exercises/5.1/solution_ws/src/lesson_perception/package.xml
similarity index 100%
rename from exercises/5.2/src/lesson_perception/package.xml
rename to exercises/5.1/solution_ws/src/lesson_perception/package.xml
diff --git a/exercises/5.1/solution_ws/src/lesson_perception/src/perception_node.cpp b/exercises/5.1/solution_ws/src/lesson_perception/src/perception_node.cpp
new file mode 100644
index 000000000..254554656
--- /dev/null
+++ b/exercises/5.1/solution_ws/src/lesson_perception/src/perception_node.cpp
@@ -0,0 +1,324 @@
+#include <ros/ros.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <sensor_msgs/PointCloud2.h> //hydro
+
+// PCL specific includes
+#include <pcl_conversions/pcl_conversions.h> //hydro
+#include "pcl_ros/transforms.h"
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/filters/crop_box.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <tf_conversions/tf_eigen.h>
+#include <pcl/segmentation/extract_polygonal_prism_data.h>
+
+int main(int argc, char *argv[])
+{
+  /*
+   * INITIALIZE ROS NODE
+   */
+  ros::init(argc, argv, "perception_node");
+  ros::NodeHandle nh;
+  ros::NodeHandle priv_nh_("~");
+
+  /*
+   * SET UP PARAMETERS (COULD BE INPUT FROM LAUNCH FILE/TERMINAL)
+   */
+  std::string cloud_topic, world_frame, camera_frame;
+  float voxel_leaf_size;
+  float x_filter_min, x_filter_max, y_filter_min, y_filter_max, z_filter_min, z_filter_max;
+  int plane_max_iter;
+  float plane_dist_thresh;
+  float cluster_tol;
+  int cluster_min_size;
+  int cluster_max_size;
+  cloud_topic = priv_nh_.param<std::string>("cloud_topic", "kinect/depth_registered/points");
+  world_frame = priv_nh_.param<std::string>("world_frame", "kinect_link");
+  camera_frame = priv_nh_.param<std::string>("camera_frame", "kinect_link");
+  voxel_leaf_size = priv_nh_.param<float>("voxel_leaf_size", 0.002);
+  x_filter_min = priv_nh_.param<float>("x_filter_min", -2.5);
+  x_filter_max = priv_nh_.param<float>("x_filter_max",  2.5);
+  y_filter_min = priv_nh_.param<float>("y_filter_min", -2.5);
+  y_filter_max = priv_nh_.param<float>("y_filter_max",  2.5);
+  z_filter_min = priv_nh_.param<float>("z_filter_min", -2.5);
+  z_filter_max = priv_nh_.param<float>("z_filter_max",  2.5);
+  plane_max_iter = priv_nh_.param<int>("plane_max_iterations", 50);
+  plane_dist_thresh = priv_nh_.param<float>("plane_distance_threshold", 0.05);
+  cluster_tol = priv_nh_.param<float>("cluster_tolerance", 0.01);
+  cluster_min_size = priv_nh_.param<int>("cluster_min_size", 100);
+  cluster_max_size = priv_nh_.param<int>("cluster_max_size", 50000);
+
+
+
+  /*
+   * SETUP PUBLISHERS
+   */
+  ros::Publisher object_pub, cluster_pub, pose_pub;
+  object_pub = nh.advertise<sensor_msgs::PointCloud2>("object_cluster", 1);
+  cluster_pub = nh.advertise<sensor_msgs::PointCloud2>("primary_cluster", 1);
+
+ while (ros::ok())
+ {
+  /*
+   * LISTEN FOR POINTCLOUD
+   */
+  std::string topic = nh.resolveName(cloud_topic);
+  ROS_INFO_STREAM("Cloud service called; waiting for a PointCloud2 on topic "<< topic);
+  sensor_msgs::PointCloud2::ConstPtr recent_cloud =
+               ros::topic::waitForMessage<sensor_msgs::PointCloud2>(topic, nh);
+
+  /*
+   * TRANSFORM POINTCLOUD FROM CAMERA FRAME TO WORLD FRAME
+   */
+  tf::TransformListener listener;
+  tf::StampedTransform stransform;
+  try
+  {
+    listener.waitForTransform(world_frame, recent_cloud->header.frame_id,  ros::Time::now(), ros::Duration(6.0));
+    listener.lookupTransform(world_frame, recent_cloud->header.frame_id,  ros::Time(0), stransform);
+  }
+  catch (tf::TransformException ex)
+  {
+    ROS_ERROR("%s",ex.what());
+  }
+  sensor_msgs::PointCloud2 transformed_cloud;
+//  sensor_msgs::PointCloud2::ConstPtr recent_cloud =
+//               ros::topic::waitForMessage<sensor_msgs::PointCloud2>(topic, nh);
+  pcl_ros::transformPointCloud(world_frame, stransform, *recent_cloud, transformed_cloud);
+
+  /*
+   * CONVERT POINTCLOUD ROS->PCL
+   */
+  pcl::PointCloud<pcl::PointXYZ> cloud;
+  pcl::fromROSMsg (transformed_cloud, cloud);
+
+  /* ========================================
+   * Fill Code: VOXEL GRID
+   * ========================================*/
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ> (cloud));
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxel_filtered (new pcl::PointCloud<pcl::PointXYZ> ());
+  pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
+  voxel_filter.setInputCloud (cloud_ptr);
+  voxel_filter.setLeafSize (voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
+  voxel_filter.filter (*cloud_voxel_filtered);
+
+  //ROS_INFO_STREAM("Original cloud  had " << cloud_ptr->size() << " points");
+  //ROS_INFO_STREAM("Downsampled cloud  with " << cloud_voxel_filtered->size() << " points");
+
+  /* ========================================
+   * Fill Code: PASSTHROUGH FILTER(S)
+   * ========================================*/
+  pcl::PointCloud<pcl::PointXYZ> xf_cloud, yf_cloud, zf_cloud;
+  pcl::PassThrough<pcl::PointXYZ> pass_x;
+  pass_x.setInputCloud(cloud_voxel_filtered);
+  pass_x.setFilterFieldName("x");
+  pass_x.setFilterLimits(x_filter_min, x_filter_max);
+  pass_x.filter(xf_cloud);
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr xf_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>(xf_cloud));
+  pcl::PassThrough<pcl::PointXYZ> pass_y;
+  pass_y.setInputCloud(xf_cloud_ptr);
+  pass_y.setFilterFieldName("y");
+  pass_y.setFilterLimits(y_filter_min, y_filter_max);
+  pass_y.filter(yf_cloud);
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr yf_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>(yf_cloud));
+  pcl::PassThrough<pcl::PointXYZ> pass_z;
+  pass_z.setInputCloud(yf_cloud_ptr);
+  pass_z.setFilterFieldName("z");
+  pass_z.setFilterLimits(z_filter_min, z_filter_max);
+  pass_z.filter(zf_cloud);
+
+  /* ========================================
+   * Fill Code: CROPBOX (OPTIONAL)
+   * ========================================*/
+  pcl::PointCloud<pcl::PointXYZ> xyz_filtered_cloud;
+  pcl::CropBox<pcl::PointXYZ> crop;
+  crop.setInputCloud(cloud_voxel_filtered);
+  Eigen::Vector4f min_point = Eigen::Vector4f(x_filter_min, y_filter_min, z_filter_min, 0);
+  Eigen::Vector4f max_point = Eigen::Vector4f(x_filter_max, y_filter_max, z_filter_max, 0);
+  crop.setMin(min_point);
+  crop.setMax(max_point);
+  crop.filter(xyz_filtered_cloud);
+
+  /* ========================================
+   * Fill Code: PLANE SEGEMENTATION
+   * ========================================*/
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZ>(xyz_filtered_cloud));
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
+  // Create the segmentation object for the planar model and set all the parameters
+  pcl::SACSegmentation<pcl::PointXYZ> seg;
+  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
+  seg.setOptimizeCoefficients (true);
+  seg.setModelType (pcl::SACMODEL_PLANE);
+  seg.setMethodType (pcl::SAC_RANSAC);
+  seg.setMaxIterations (plane_max_iter);
+  seg.setDistanceThreshold (plane_dist_thresh);
+  // Segment the largest planar component from the cropped cloud
+  seg.setInputCloud (cropped_cloud);
+  seg.segment (*inliers, *coefficients);
+  if (inliers->indices.size () == 0)
+  {
+    ROS_WARN_STREAM ("Could not estimate a planar model for the given dataset.") ;
+    //break;
+  }
+
+  // Extract the planar inliers from the input cloud
+  pcl::ExtractIndices<pcl::PointXYZ> extract;
+  extract.setInputCloud (cropped_cloud);
+  extract.setIndices(inliers);
+  extract.setNegative (false);
+
+  // Get the points associated with the planar surface
+  extract.filter (*cloud_plane);
+  ROS_INFO_STREAM("PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." );
+
+  // Remove the planar inliers, extract the rest
+  extract.setNegative (true);
+  extract.filter (*cloud_f);
+
+  /* ========================================
+   * Fill Code: PUBLISH PLANE MARKER (OPTIONAL)
+   * ========================================*/
+
+
+  /* ========================================
+   * Fill Code: EUCLIDEAN CLUSTER EXTRACTION (OPTIONAL/RECOMMENDED)
+   * ========================================*/
+  // Creating the KdTree object for the search method of the extraction
+  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
+  *cloud_filtered = *cloud_f;
+  tree->setInputCloud (cloud_filtered);
+
+  std::vector<pcl::PointIndices> cluster_indices;
+  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+  ec.setClusterTolerance (cluster_tol);
+  ec.setMinClusterSize (cluster_min_size);
+  ec.setMaxClusterSize (cluster_max_size);
+  ec.setSearchMethod (tree);
+  ec.setInputCloud (cloud_filtered);
+  ec.extract (cluster_indices);
+
+  std::vector<sensor_msgs::PointCloud2::Ptr> pc2_clusters;
+  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > clusters;
+  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
+  {
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
+    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
+      cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
+    cloud_cluster->width = cloud_cluster->points.size ();
+    cloud_cluster->height = 1;
+    cloud_cluster->is_dense = true;
+    std::cout << "Cluster has " << cloud_cluster->points.size() << " points.\n";
+    clusters.push_back(cloud_cluster);
+    sensor_msgs::PointCloud2::Ptr tempROSMsg(new sensor_msgs::PointCloud2);
+    pcl::toROSMsg(*cloud_cluster, *tempROSMsg);
+    pc2_clusters.push_back(tempROSMsg);
+  }
+
+  /* ========================================
+   * Fill Code: STATISTICAL OUTLIER REMOVAL (OPTIONAL)
+   * ========================================*/
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud_ptr= clusters.at(0);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
+  sor.setInputCloud (cluster_cloud_ptr);
+  sor.setMeanK (50);
+  sor.setStddevMulThresh (1.0);
+  sor.filter (*sor_cloud_filtered);
+
+
+  /* ========================================
+   * Fill Code: PUBLISH OTHER MARKERS (OPTIONAL)
+   * ========================================*/
+
+
+  /* ========================================
+  * BROADCAST TRANSFORM (OPTIONAL)
+  * ========================================*/
+  static tf::TransformBroadcaster br;
+  tf::Transform part_transform;
+
+  //Here in the tf::Vector3(x,y,z) x,y, and z should be calculated based on the pointcloud filtering results
+  part_transform.setOrigin( tf::Vector3(sor_cloud_filtered->at(1).x, sor_cloud_filtered->at(1).y, sor_cloud_filtered->at(1).z) );
+  tf::Quaternion q;
+  q.setRPY(0, 0, 0);
+  part_transform.setRotation(q);
+  br.sendTransform(tf::StampedTransform(part_transform, ros::Time::now(), world_frame, "part"));
+
+  /* ========================================
+   * Fill Code: POLYGONAL SEGMENTATION (OPTIONAL)
+   * ========================================*/
+  pcl::PointCloud<pcl::PointXYZ>::Ptr sensor_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>(cloud));
+  pcl::PointCloud<pcl::PointXYZ>::Ptr prism_filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr pick_surface_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism;
+  pcl::ExtractIndices<pcl::PointXYZ> extract_ind;
+  prism.setInputCloud(sensor_cloud_ptr);
+  pcl::PointIndices::Ptr pt_inliers (new pcl::PointIndices());
+  // create prism surface
+  double box_length=0.25;
+  double box_width=0.25;
+  pick_surface_cloud_ptr->width = 5;
+  pick_surface_cloud_ptr->height = 1;
+  pick_surface_cloud_ptr->points.resize(5);
+
+  pick_surface_cloud_ptr->points[0].x = 0.5f*box_length;
+  pick_surface_cloud_ptr->points[0].y = 0.5f*box_width;
+  pick_surface_cloud_ptr->points[0].z = 0;
+
+  pick_surface_cloud_ptr->points[1].x = -0.5f*box_length;
+  pick_surface_cloud_ptr->points[1].y = 0.5f*box_width;
+  pick_surface_cloud_ptr->points[1].z = 0;
+
+  pick_surface_cloud_ptr->points[2].x = -0.5f*box_length;
+  pick_surface_cloud_ptr->points[2].y = -0.5f*box_width;
+  pick_surface_cloud_ptr->points[2].z = 0;
+
+  pick_surface_cloud_ptr->points[3].x = 0.5f*box_length;
+  pick_surface_cloud_ptr->points[3].y = -0.5f*box_width;
+  pick_surface_cloud_ptr->points[3].z = 0;
+
+  pick_surface_cloud_ptr->points[4].x = 0.5f*box_length;
+  pick_surface_cloud_ptr->points[4].y = 0.5f*box_width;
+  pick_surface_cloud_ptr->points[4].z = 0;
+
+  Eigen::Affine3d eigen3d;
+  tf::transformTFToEigen(part_transform,eigen3d);
+  pcl::transformPointCloud(*pick_surface_cloud_ptr,*pick_surface_cloud_ptr,Eigen::Affine3f(eigen3d));
+
+  prism.setInputPlanarHull( pick_surface_cloud_ptr);
+  prism.setHeightLimits(-10,10);
+
+  prism.segment(*pt_inliers);
+
+  extract_ind.setInputCloud(sensor_cloud_ptr);
+  extract_ind.setIndices(pt_inliers);
+
+  extract_ind.setNegative(true);
+  extract_ind.filter(*prism_filtered_cloud);
+
+  /* ========================================
+   * CONVERT POINTCLOUD PCL->ROS
+   * PUBLISH CLOUD
+   * Fill Code: UPDATE AS NECESSARY
+   * ========================================*/
+  sensor_msgs::PointCloud2::Ptr pc2_cloud (new sensor_msgs::PointCloud2);
+  pcl::toROSMsg(*prism_filtered_cloud, *pc2_cloud);
+  pc2_cloud->header.frame_id=world_frame;
+  pc2_cloud->header.stamp=ros::Time::now();
+  object_pub.publish(pc2_cloud);
+  }
+  return 0;
+}
diff --git a/exercises/perception_ws/src/lesson_perception/CMakeLists.txt b/exercises/5.1/template_ws/src/lesson_perception/CMakeLists.txt
similarity index 99%
rename from exercises/perception_ws/src/lesson_perception/CMakeLists.txt
rename to exercises/5.1/template_ws/src/lesson_perception/CMakeLists.txt
index 3080c2db1..c79433ce5 100644
--- a/exercises/perception_ws/src/lesson_perception/CMakeLists.txt
+++ b/exercises/5.1/template_ws/src/lesson_perception/CMakeLists.txt
@@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
   pcl_ros
   pcl_conversions
   visualization_msgs
+  tf_conversions
 )
 find_package(PCL REQUIRED)
 
diff --git a/exercises/perception_ws/src/lesson_perception/package.xml b/exercises/5.1/template_ws/src/lesson_perception/package.xml
similarity index 96%
rename from exercises/perception_ws/src/lesson_perception/package.xml
rename to exercises/5.1/template_ws/src/lesson_perception/package.xml
index 802b35f41..c1e60d1f3 100644
--- a/exercises/perception_ws/src/lesson_perception/package.xml
+++ b/exercises/5.1/template_ws/src/lesson_perception/package.xml
@@ -47,6 +47,7 @@
   <build_depend>pcl_msgs</build_depend>
   <build_depend>pcl_ros</build_depend>
   <build_depend>visualization_msgs</build_depend>
+  <build_depend>tf_conversions</build_depend>
   
   <run_depend>roscpp</run_depend>
   <run_depend>sensor_msgs</run_depend>
@@ -55,6 +56,7 @@
   <run_depend>pcl_msgs</run_depend>
   <run_depend>pcl_ros</run_depend>
   <run_depend>visualization_msgs</run_depend>
+  <run_depend>tf_conversions</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
diff --git a/exercises/perception_ws/src/lesson_perception/src/perception_node.cpp b/exercises/5.1/template_ws/src/lesson_perception/src/perception_node.cpp
similarity index 97%
rename from exercises/perception_ws/src/lesson_perception/src/perception_node.cpp
rename to exercises/5.1/template_ws/src/lesson_perception/src/perception_node.cpp
index 71db5285b..4a7022883 100644
--- a/exercises/perception_ws/src/lesson_perception/src/perception_node.cpp
+++ b/exercises/5.1/template_ws/src/lesson_perception/src/perception_node.cpp
@@ -15,8 +15,10 @@
 //#include <pcl/segmentation/sac_segmentation.h>
 //#include <pcl/filters/extract_indices.h>
 //#include <pcl/segmentation/extract_clusters.h>
-//#include <pcl/kdtree/kdtree_flann.h>
+//#include <pcl/filters/crop_box.h>
 //#include <pcl/filters/statistical_outlier_removal.h>
+//#include <tf_conversions/tf_eigen.h>
+//#include <pcl/segmentation/extract_polygonal_prism_data.h>
 
 int main(int argc, char *argv[])
 {
@@ -118,17 +120,6 @@ int main(int argc, char *argv[])
    * ========================================*/
 
 
-  /* ========================================
-   * CONVERT POINTCLOUD PCL->ROS
-   * PUBLISH CLOUD
-   * Fill Code: UPDATE AS NECESSARY
-   * ========================================*/
-  sensor_msgs::PointCloud2::Ptr pc2_cloud (new sensor_msgs::PointCloud2);
-  //pcl::toROSMsg(*cloud_ptr, *pc2_cloud);
-  pc2_cloud->header.frame_id=world_frame;
-  pc2_cloud->header.stamp=ros::Time::now();
-  object_pub.publish(pc2_cloud);
-
   /* ========================================
    * Fill Code: PUBLISH OTHER MARKERS (OPTIONAL)
    * ========================================*/
@@ -144,6 +135,16 @@ int main(int argc, char *argv[])
    * ========================================*/
 
 
+  /* ========================================
+   * CONVERT POINTCLOUD PCL->ROS
+   * PUBLISH CLOUD
+   * Fill Code: UPDATE AS NECESSARY
+   * ========================================*/
+  sensor_msgs::PointCloud2::Ptr pc2_cloud (new sensor_msgs::PointCloud2);
+  //pcl::toROSMsg(*cloud_ptr, *pc2_cloud);
+  pc2_cloud->header.frame_id=world_frame;
+  pc2_cloud->header.stamp=ros::Time::now();
+  object_pub.publish(pc2_cloud);
   }
   return 0;
 }
diff --git a/exercises/5.2/src/lesson_perception/launch/lesson_perception.rviz b/exercises/5.2/src/lesson_perception/launch/lesson_perception.rviz
deleted file mode 100644
index 18f5bc106..000000000
--- a/exercises/5.2/src/lesson_perception/launch/lesson_perception.rviz
+++ /dev/null
@@ -1,237 +0,0 @@
-Panels:
-  - Class: rviz/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-        - /PointCloud23
-      Splitter Ratio: 0.5
-    Tree Height: 503
-  - Class: rviz/Selection
-    Name: Selection
-  - Class: rviz/Tool Properties
-    Expanded:
-      - /2D Pose Estimate1
-      - /2D Nav Goal1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.588679
-  - Class: rviz/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: rviz/Time
-    Experimental: false
-    Name: Time
-    SyncMode: 0
-    SyncSource: PointCloud2
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.03
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: RGB8
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.01
-      Style: Points
-      Topic: /kinect/depth_registered/points
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.01
-      Style: Flat Squares
-      Topic: /object_cluster
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Class: rviz/TF
-      Enabled: true
-      Frame Timeout: 15
-      Frames:
-        All Enabled: true
-        kinect_depth_frame:
-          Value: true
-        kinect_depth_optical_frame:
-          Value: true
-        kinect_link:
-          Value: true
-        kinect_rgb_frame:
-          Value: true
-        kinect_rgb_optical_frame:
-          Value: true
-        part:
-          Value: true
-        ur5_stand:
-          Value: true
-        world_frame:
-          Value: true
-      Marker Scale: 1
-      Name: TF
-      Show Arrows: true
-      Show Axes: true
-      Show Names: true
-      Tree:
-        world_frame:
-          part:
-            {}
-          ur5_stand:
-            kinect_link:
-              kinect_depth_frame:
-                kinect_depth_optical_frame:
-                  {}
-              kinect_rgb_frame:
-                kinect_rgb_optical_frame:
-                  {}
-      Update Interval: 0
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.01
-      Style: Flat Squares
-      Topic: /scene_minus_box
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: world_frame
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz/Interact
-      Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-    - Class: rviz/FocusCamera
-    - Class: rviz/Measure
-    - Class: rviz/SetInitialPose
-      Topic: /initialpose
-    - Class: rviz/SetGoal
-      Topic: /move_base_simple/goal
-    - Class: rviz/PublishPoint
-      Single click: true
-      Topic: /clicked_point
-  Value: true
-  Views:
-    Current:
-      Class: rviz/Orbit
-      Distance: 4.79058
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.06
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: -0.174017
-        Y: 0.0337533
-        Z: -0.323589
-      Name: Current View
-      Near Clip Distance: 0.01
-      Pitch: 0.929796
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 3.0354
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 784
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000013c00000286fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000286000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000286fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000286000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000036e0000028600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 1200
-  X: 378
-  Y: 86
diff --git a/exercises/5.2/src/lesson_perception/src/perception_node.cpp b/exercises/5.2/src/lesson_perception/src/perception_node.cpp
deleted file mode 100644
index 39c5ef6e5..000000000
--- a/exercises/5.2/src/lesson_perception/src/perception_node.cpp
+++ /dev/null
@@ -1,371 +0,0 @@
-#include <ros/ros.h>
-#include <tf/transform_datatypes.h>
-#include <tf/transform_listener.h>
-#include <tf/transform_broadcaster.h>
-#include <sensor_msgs/PointCloud2.h> //hydro
-
-// PCL specific includes
-#include <pcl_conversions/pcl_conversions.h> //hydro
-#include "pcl_ros/transforms.h"
-
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/filters/passthrough.h>
-#include <pcl/sample_consensus/method_types.h>
-#include <pcl/sample_consensus/model_types.h>
-#include <pcl/segmentation/sac_segmentation.h>
-#include <pcl/filters/extract_indices.h>
-#include <pcl/segmentation/extract_clusters.h>
-#include <pcl/kdtree/kdtree_flann.h>
-
-#include <visualization_msgs/Marker.h>
-#include <pcl/filters/crop_box.h>
-#include <pcl/filters/statistical_outlier_removal.h>
-
-//polygonal segmentation
-#include <pcl/segmentation/extract_polygonal_prism_data.h>
-#include <tf_conversions/tf_eigen.h>
-#include <pcl/surface/convex_hull.h>
-
-int main(int argc, char *argv[])
-{
-  /*
-   * INITIALIZE ROS NODE
-   */
-  ros::init(argc, argv, "perception_node");
-  ros::NodeHandle nh;
-  ros::NodeHandle priv_nh_("~");
-
-  /*
-   * SET UP PARAMETERS (COULD BE INPUT FROM LAUNCH FILE/TERMINAL)
-   */
-  std::string cloud_topic, world_frame, camera_frame;
-  double voxel_leaf_size;
-  double x_filter_min, x_filter_max, y_filter_min, y_filter_max, z_filter_min, z_filter_max;
-  double plane_max_iter, plane_dist_thresh;
-  double cluster_tol;
-  int cluster_min_size, cluster_max_size;
-  /*world_frame="world_frame";
-  camera_frame="kinect_link";
-  cloud_topic="camera/depth_registered/points";
-  voxel_leaf_size=0.001f;
-  x_filter_min=-2.5;
-  x_filter_max=2.5;
-  y_filter_min=-2.5;
-  y_filter_max=2.5;
-  z_filter_min=-2.5;
-  z_filter_max=1.0;
-  plane_max_iter=50;
-  plane_dist_thresh=0.05;
-  cluster_tol=0.02;
-  cluster_min_size=100;
-  cluster_max_size=50000;*/
-  priv_nh_.getParam("cloud_topic", cloud_topic);
-  priv_nh_.getParam("world_frame", world_frame);
-  priv_nh_.getParam("camera_frame", camera_frame);
-  priv_nh_.getParam("voxel_leaf_size", voxel_leaf_size);
-  priv_nh_.getParam("x_filter_min", x_filter_min);
-  priv_nh_.getParam("x_filter_max", x_filter_max);
-  priv_nh_.getParam("y_filter_min", y_filter_min);
-  priv_nh_.getParam("y_filter_max", y_filter_max);
-  priv_nh_.getParam("z_filter_min", z_filter_min);
-  priv_nh_.getParam("z_filter_max", z_filter_max);
-  priv_nh_.getParamCached("plane_max_iterations", plane_max_iter);
-  priv_nh_.getParamCached("plane_distance_threshold", plane_dist_thresh);
-  priv_nh_.getParam("cluster_tolerance", cluster_tol);
-  priv_nh_.getParam("cluster_min_size", cluster_min_size);
-  priv_nh_.getParam("cluster_max_size", cluster_max_size);
-
-  /*
-   * SETUP PUBLISHERS
-   */
-  ros::Publisher object_pub, cluster_pub, pose_pub, vis_pub, scene_wo_box;
-  object_pub = nh.advertise<sensor_msgs::PointCloud2>("object_cluster", 1);
-  cluster_pub = nh.advertise<sensor_msgs::PointCloud2>("primary_cluster", 1);
-  scene_wo_box = nh.advertise<sensor_msgs::PointCloud2>("scene_minus_box", 1);
-  vis_pub  = nh.advertise<visualization_msgs::Marker>( "visualization_marker", 1);
-
-  while (ros::ok())
-  {
-    /*
-    * LISTEN FOR POINTCLOUD
-    */
-    std::string topic = nh.resolveName(cloud_topic);
-    ROS_INFO_STREAM("Cloud service called; waiting for a PointCloud2 on topic "<< topic);
-    sensor_msgs::PointCloud2::ConstPtr recent_cloud =
-                ros::topic::waitForMessage<sensor_msgs::PointCloud2>(topic, nh);
-
-    /*
-    * TRANSFORM POINTCLOUD FROM CAMERA FRAME TO WORLD FRAME
-    */
-    tf::TransformListener listener;
-    tf::StampedTransform stransform;
-    try
-    {
-      listener.waitForTransform(world_frame, recent_cloud->header.frame_id,  ros::Time::now(), ros::Duration(6.0));
-      listener.lookupTransform(world_frame, recent_cloud->header.frame_id,  ros::Time(0), stransform);
-    }
-    catch (tf::TransformException ex)
-    {
-      ROS_ERROR("%s",ex.what());
-    }
-    sensor_msgs::PointCloud2 transformed_cloud;
-    //  sensor_msgs::PointCloud2::ConstPtr recent_cloud =
-    //               ros::topic::waitForMessage<sensor_msgs::PointCloud2>(topic, nh);
-    pcl_ros::transformPointCloud(world_frame, stransform, *recent_cloud, transformed_cloud);
-
-    /*
-    * CONVERT POINTCLOUD ROS->PCL
-    */
-    pcl::PointCloud<pcl::PointXYZ> cloud;
-    pcl::fromROSMsg (transformed_cloud, cloud);
-
-
-    //MAKE TIMERS FOR PROCESS (OPTIONAL)
-    ros::Time start_init = ros::Time::now();
-
-    /* ========================================
-     * Fill Code: VOXEL GRID
-     * ========================================*/
-    //input clout must be a pointer, so we make a new cloud_ptr from the cloud object
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>(cloud));
-    //output cloud - set up as pointer to ease transition into further processing
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxel_filtered (new pcl::PointCloud<pcl::PointXYZ> ());
-    //create an instance of the pcl VoxelGrid
-    pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
-    voxel_filter.setInputCloud (cloud_ptr);
-    voxel_filter.setLeafSize (float(voxel_leaf_size), float(voxel_leaf_size), float(voxel_leaf_size));
-    voxel_filter.filter (*cloud_voxel_filtered);
-
-    ROS_INFO_STREAM("Original cloud  had " << cloud_voxel_filtered->size() << " points");
-    //ROS_INFO_STREAM("Downsampled cloud  with " << cloud_voxel_filtered->size() << " points");
-
-    /* ========================================
-     * Fill Code: PASSTHROUGH FILTER(S)
-     * ======================*/
-    //step 1- filter in x
-    pcl::PointCloud<pcl::PointXYZ> xf_cloud, yf_cloud, zf_cloud;
-    pcl::PassThrough<pcl::PointXYZ> pass_x;
-    pass_x.setInputCloud(cloud_voxel_filtered);
-    pass_x.setFilterFieldName("x");
-    pass_x.setFilterLimits(x_filter_min,x_filter_max);
-    pass_x.filter(xf_cloud);
-
-    //pass to filter in y
-    pcl::PointCloud<pcl::PointXYZ>::Ptr xf_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>(xf_cloud));
-    pcl::PassThrough<pcl::PointXYZ> pass_y;
-    pass_y.setInputCloud(xf_cloud_ptr);
-    pass_y.setFilterFieldName("y");
-    pass_y.setFilterLimits(y_filter_min, y_filter_max);
-    pass_y.filter(yf_cloud);
-
-    //pass to filter in z
-    pcl::PointCloud<pcl::PointXYZ>::Ptr yf_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>(yf_cloud));
-    pcl::PassThrough<pcl::PointXYZ> pass_z;
-    pass_z.setInputCloud(yf_cloud_ptr);
-    pass_z.setFilterFieldName("z");
-    pass_z.setFilterLimits(z_filter_min, z_filter_max);
-    pass_z.filter(zf_cloud);
-
-    /* ========================================
-    * Fill Code: CROPBOX (OPTIONAL)
-     * Instead of three passthrough filters, the cropbox filter can be used
-     * The user should choose one or the other method
-     * ========================================*/
-    pcl::PointCloud<pcl::PointXYZ> xyz_filtered_cloud;
-    pcl::CropBox<pcl::PointXYZ> crop;
-    crop.setInputCloud(cloud_voxel_filtered);
-    Eigen::Vector4f min_point = Eigen::Vector4f(x_filter_min, y_filter_min, z_filter_min, 0);
-    Eigen::Vector4f max_point = Eigen::Vector4f(x_filter_max, y_filter_max, z_filter_max, 0);
-    crop.setMin(min_point);
-    crop.setMax(max_point);
-    crop.filter(xyz_filtered_cloud);
-
-    /* ========================================
-     * Fill Code: PLANE SEGEMENTATION
-     * ========================================*/
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZ>(zf_cloud)); //this passes in either passthrough or crop filtered cloud.
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
-    // Create the segmentation object for the planar model and set all the parameters
-    pcl::SACSegmentation<pcl::PointXYZ> seg;
-    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
-    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
-    seg.setOptimizeCoefficients (true);
-    seg.setModelType (pcl::SACMODEL_PLANE);
-    seg.setMethodType (pcl::SAC_RANSAC);
-    seg.setMaxIterations (plane_max_iter);
-    seg.setDistanceThreshold (plane_dist_thresh);
-    // Segment the largest planar component from the cropped cloud
-    seg.setInputCloud (cropped_cloud);
-    seg.segment (*inliers, *coefficients);
-    if (inliers->indices.size () == 0)
-    {
-      ROS_WARN_STREAM ("Could not estimate a planar model for the given dataset.") ;
-      //break;
-    }
-
-    // Extract the planar inliers from the input cloud
-    pcl::ExtractIndices<pcl::PointXYZ> extract;
-    extract.setInputCloud (cropped_cloud);
-    extract.setIndices(inliers);
-    extract.setNegative (false);
-
-    // Get the points associated with the planar surface
-    extract.filter (*cloud_plane);
-    ROS_INFO_STREAM("PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." );
-
-    /* ========================================
-     * Fill Code: PUBLISH PLANE MARKER (OPTIONAL)
-     * ========================================*/
-    visualization_msgs::Marker table_marker;
-
-
-    // Remove the planar inliers, extract the rest
-    extract.setNegative (true);
-    extract.filter (*cloud_f);
-
-    /* ========================================
-     * Fill Code: EUCLIDEAN CLUSTER EXTRACTION (OPTIONAL/RECOMMENDED)
-     * ========================================*/
-    // Creating the KdTree object for the search method of the extraction
-    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
-    *cloud_filtered = *cloud_f;
-    tree->setInputCloud (cloud_filtered);
-
-    std::vector<pcl::PointIndices> cluster_indices;
-    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
-    ec.setClusterTolerance (cluster_tol); // 2cm
-    ec.setMinClusterSize (cluster_min_size);
-    ec.setMaxClusterSize (cluster_max_size);
-    ec.setSearchMethod (tree);
-    ec.setInputCloud (cloud_filtered);
-    ec.extract (cluster_indices);
-
-    std::vector<sensor_msgs::PointCloud2::Ptr> pc2_clusters;
-    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > clusters;
-    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
-    {
-      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
-      for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
-        cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
-      cloud_cluster->width = cloud_cluster->points.size ();
-      cloud_cluster->height = 1;
-      cloud_cluster->is_dense = true;
-      std::cout << "Cluster has " << cloud_cluster->points.size() << " points.\n";
-      clusters.push_back(cloud_cluster);
-      sensor_msgs::PointCloud2::Ptr tempROSMsg(new sensor_msgs::PointCloud2);
-      pcl::toROSMsg(*cloud_cluster, *tempROSMsg);
-      pc2_clusters.push_back(tempROSMsg);
-    }
-    pc2_clusters.at(0)->header.frame_id=world_frame;
-    pc2_clusters.at(0)->header.stamp=ros::Time::now();
-    cluster_pub.publish(pc2_clusters.at(0));
-
-    /* ========================================
-     * Fill Code: STATISTICAL OUTLIER REMOVAL (OPTIONAL)
-     * ========================================*/
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud_ptr= clusters.at(0);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
-    sor.setInputCloud (cluster_cloud_ptr);
-    sor.setMeanK (50);
-    sor.setStddevMulThresh (1.0);
-    sor.filter (*sor_cloud_filtered);
-
-    //OPTIONAL TIMERS
-    ros::Time finish_process = ros::Time::now();
-    ros::Duration total_process = finish_process - start_init;
-    ROS_INFO_STREAM("Point Cloud processing took "<< total_process<<" s");
-
-    /* ========================================
-     * CONVERT POINTCLOUD PCL->ROS
-     * PUBLISH CLOUD
-     * Fill Code: UPDATE AS NECESSARY
-     * ========================================*/
-    sensor_msgs::PointCloud2::Ptr pc2_cloud (new sensor_msgs::PointCloud2);
-    pcl::toROSMsg(*sor_cloud_filtered, *pc2_cloud);
-    pc2_cloud->header.frame_id=world_frame;
-    pc2_cloud->header.stamp=ros::Time::now();
-    object_pub.publish(*pc2_cloud);
-
-    /* ========================================
-     * Fill Code: PUBLISH OTHER MARKERS (OPTIONAL)
-     * ========================================*/
-
-
-    /* ========================================
-    * BROADCAST TRANSFORM (OPTIONAL)
-    * ========================================*/
-    static tf::TransformBroadcaster br;
-    tf::Transform part_transform;
-    //Here in the tf::Vector3(x,y,z) x,y, and z should be calculated based on the pointcloud filtering results
-    part_transform.setOrigin( tf::Vector3(sor_cloud_filtered->at(1).x, sor_cloud_filtered->at(1).y, sor_cloud_filtered->at(1).z) );
-    tf::Quaternion q;
-    q.setRPY(0, 0, 0);
-    part_transform.setRotation(q);
-    br.sendTransform(tf::StampedTransform(part_transform, ros::Time::now(), world_frame, "part"));
-
-  /* ========================================
-   * Fill Code: POLYGONAL SEGMENTATION (OPTIONAL)
-   * ========================================*/
-    pcl::PointCloud<pcl::PointXYZ>::Ptr sensor_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>(cloud));
-    pcl::PointCloud<pcl::PointXYZ>::Ptr prism_filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr pick_surface_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-
-    pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism;
-    pcl::ExtractIndices<pcl::PointXYZ> extract_ind;
-    pcl::PointIndices::Ptr pt_inliers (new pcl::PointIndices());
-
-    prism.setInputCloud(sensor_cloud_ptr);
-
-    // create prism surface
-    double box_length=0.25;
-    double box_width=0.25;
-    pick_surface_cloud_ptr->width = 5;
-    pick_surface_cloud_ptr->height = 1;
-    pick_surface_cloud_ptr->points.resize(5);
-
-    pick_surface_cloud_ptr->points[0].x = 0.5f*box_length;
-    pick_surface_cloud_ptr->points[0].y = 0.5f*box_width;
-    pick_surface_cloud_ptr->points[0].z = 0;
-
-    pick_surface_cloud_ptr->points[1].x = -0.5f*box_length;
-    pick_surface_cloud_ptr->points[1].y = 0.5f*box_width;
-    pick_surface_cloud_ptr->points[1].z = 0;
-
-    pick_surface_cloud_ptr->points[2].x = -0.5f*box_length;
-    pick_surface_cloud_ptr->points[2].y = -0.5f*box_width;
-    pick_surface_cloud_ptr->points[2].z = 0;
-
-    pick_surface_cloud_ptr->points[3].x = 0.5f*box_length;
-    pick_surface_cloud_ptr->points[3].y = -0.5f*box_width;
-    pick_surface_cloud_ptr->points[3].z = 0;
-
-    pick_surface_cloud_ptr->points[4].x = 0.5f*box_length;
-    pick_surface_cloud_ptr->points[4].y = 0.5f*box_width;
-    pick_surface_cloud_ptr->points[4].z = 0;
-
-    Eigen::Affine3d eigen3d;
-    tf::transformTFToEigen(part_transform,eigen3d);
-    pcl::transformPointCloud(*pick_surface_cloud_ptr,*pick_surface_cloud_ptr,Eigen::Affine3f(eigen3d));
-
-    prism.setInputPlanarHull( pick_surface_cloud_ptr);
-    prism.setHeightLimits(-10,10);
-    prism.segment(*pt_inliers);
-
-    //extracting remaining points
-    extract_ind.setInputCloud(sensor_cloud_ptr);
-    extract_ind.setIndices(pt_inliers);
-    extract_ind.setNegative(true);
-    extract_ind.filter(*prism_filtered_cloud);;
-
-    //convert and publish the cloud without box
-    sensor_msgs::PointCloud2::Ptr scene_minus_box_cloud (new sensor_msgs::PointCloud2);
-    pcl::toROSMsg(*prism_filtered_cloud, *scene_minus_box_cloud); //into ros, from
-    scene_minus_box_cloud->header.frame_id=world_frame;
-    scene_minus_box_cloud->header.stamp=ros::Time::now();
-    scene_wo_box.publish(scene_minus_box_cloud);
-  }
-  return 0;
-}
diff --git a/exercises/5.1/src/myworkcell_moveit_config/.setup_assistant b/exercises/5.2/src/myworkcell_moveit_config/.setup_assistant
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/.setup_assistant
rename to exercises/5.2/src/myworkcell_moveit_config/.setup_assistant
diff --git a/exercises/5.1/src/myworkcell_moveit_config/CMakeLists.txt b/exercises/5.2/src/myworkcell_moveit_config/CMakeLists.txt
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/CMakeLists.txt
rename to exercises/5.2/src/myworkcell_moveit_config/CMakeLists.txt
diff --git a/exercises/5.1/src/myworkcell_moveit_config/config/fake_controllers.yaml b/exercises/5.2/src/myworkcell_moveit_config/config/fake_controllers.yaml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/config/fake_controllers.yaml
rename to exercises/5.2/src/myworkcell_moveit_config/config/fake_controllers.yaml
diff --git a/exercises/5.1/src/myworkcell_moveit_config/config/joint_limits.yaml b/exercises/5.2/src/myworkcell_moveit_config/config/joint_limits.yaml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/config/joint_limits.yaml
rename to exercises/5.2/src/myworkcell_moveit_config/config/joint_limits.yaml
diff --git a/exercises/5.1/src/myworkcell_moveit_config/config/kinematics.yaml b/exercises/5.2/src/myworkcell_moveit_config/config/kinematics.yaml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/config/kinematics.yaml
rename to exercises/5.2/src/myworkcell_moveit_config/config/kinematics.yaml
diff --git a/exercises/5.1/src/myworkcell_moveit_config/config/myworkcell.srdf b/exercises/5.2/src/myworkcell_moveit_config/config/myworkcell.srdf
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/config/myworkcell.srdf
rename to exercises/5.2/src/myworkcell_moveit_config/config/myworkcell.srdf
diff --git a/exercises/5.1/src/myworkcell_moveit_config/config/ompl_planning.yaml b/exercises/5.2/src/myworkcell_moveit_config/config/ompl_planning.yaml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/config/ompl_planning.yaml
rename to exercises/5.2/src/myworkcell_moveit_config/config/ompl_planning.yaml
diff --git a/exercises/5.1/src/myworkcell_moveit_config/config/stomp_config.yaml b/exercises/5.2/src/myworkcell_moveit_config/config/stomp_config.yaml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/config/stomp_config.yaml
rename to exercises/5.2/src/myworkcell_moveit_config/config/stomp_config.yaml
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/default_warehouse_db.launch b/exercises/5.2/src/myworkcell_moveit_config/launch/default_warehouse_db.launch
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/default_warehouse_db.launch
rename to exercises/5.2/src/myworkcell_moveit_config/launch/default_warehouse_db.launch
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/demo.launch b/exercises/5.2/src/myworkcell_moveit_config/launch/demo.launch
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/demo.launch
rename to exercises/5.2/src/myworkcell_moveit_config/launch/demo.launch
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/exercises/5.2/src/myworkcell_moveit_config/launch/fake_moveit_controller_manager.launch.xml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/fake_moveit_controller_manager.launch.xml
rename to exercises/5.2/src/myworkcell_moveit_config/launch/fake_moveit_controller_manager.launch.xml
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/joystick_control.launch b/exercises/5.2/src/myworkcell_moveit_config/launch/joystick_control.launch
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/joystick_control.launch
rename to exercises/5.2/src/myworkcell_moveit_config/launch/joystick_control.launch
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/move_group.launch b/exercises/5.2/src/myworkcell_moveit_config/launch/move_group.launch
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/move_group.launch
rename to exercises/5.2/src/myworkcell_moveit_config/launch/move_group.launch
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/moveit.rviz b/exercises/5.2/src/myworkcell_moveit_config/launch/moveit.rviz
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/moveit.rviz
rename to exercises/5.2/src/myworkcell_moveit_config/launch/moveit.rviz
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/moveit_rviz.launch b/exercises/5.2/src/myworkcell_moveit_config/launch/moveit_rviz.launch
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/moveit_rviz.launch
rename to exercises/5.2/src/myworkcell_moveit_config/launch/moveit_rviz.launch
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/myworkcell_moveit_controller_manager.launch.xml b/exercises/5.2/src/myworkcell_moveit_config/launch/myworkcell_moveit_controller_manager.launch.xml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/myworkcell_moveit_controller_manager.launch.xml
rename to exercises/5.2/src/myworkcell_moveit_config/launch/myworkcell_moveit_controller_manager.launch.xml
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/myworkcell_moveit_sensor_manager.launch.xml b/exercises/5.2/src/myworkcell_moveit_config/launch/myworkcell_moveit_sensor_manager.launch.xml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/myworkcell_moveit_sensor_manager.launch.xml
rename to exercises/5.2/src/myworkcell_moveit_config/launch/myworkcell_moveit_sensor_manager.launch.xml
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/ompl_planning_pipeline.launch.xml b/exercises/5.2/src/myworkcell_moveit_config/launch/ompl_planning_pipeline.launch.xml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/ompl_planning_pipeline.launch.xml
rename to exercises/5.2/src/myworkcell_moveit_config/launch/ompl_planning_pipeline.launch.xml
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/planning_context.launch b/exercises/5.2/src/myworkcell_moveit_config/launch/planning_context.launch
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/planning_context.launch
rename to exercises/5.2/src/myworkcell_moveit_config/launch/planning_context.launch
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/planning_pipeline.launch.xml b/exercises/5.2/src/myworkcell_moveit_config/launch/planning_pipeline.launch.xml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/planning_pipeline.launch.xml
rename to exercises/5.2/src/myworkcell_moveit_config/launch/planning_pipeline.launch.xml
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/run_benchmark_ompl.launch b/exercises/5.2/src/myworkcell_moveit_config/launch/run_benchmark_ompl.launch
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/run_benchmark_ompl.launch
rename to exercises/5.2/src/myworkcell_moveit_config/launch/run_benchmark_ompl.launch
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/sensor_manager.launch.xml b/exercises/5.2/src/myworkcell_moveit_config/launch/sensor_manager.launch.xml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/sensor_manager.launch.xml
rename to exercises/5.2/src/myworkcell_moveit_config/launch/sensor_manager.launch.xml
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/setup_assistant.launch b/exercises/5.2/src/myworkcell_moveit_config/launch/setup_assistant.launch
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/setup_assistant.launch
rename to exercises/5.2/src/myworkcell_moveit_config/launch/setup_assistant.launch
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/stomp_planning_pipeline.launch.xml b/exercises/5.2/src/myworkcell_moveit_config/launch/stomp_planning_pipeline.launch.xml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/stomp_planning_pipeline.launch.xml
rename to exercises/5.2/src/myworkcell_moveit_config/launch/stomp_planning_pipeline.launch.xml
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/trajectory_execution.launch.xml b/exercises/5.2/src/myworkcell_moveit_config/launch/trajectory_execution.launch.xml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/trajectory_execution.launch.xml
rename to exercises/5.2/src/myworkcell_moveit_config/launch/trajectory_execution.launch.xml
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/warehouse.launch b/exercises/5.2/src/myworkcell_moveit_config/launch/warehouse.launch
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/warehouse.launch
rename to exercises/5.2/src/myworkcell_moveit_config/launch/warehouse.launch
diff --git a/exercises/5.1/src/myworkcell_moveit_config/launch/warehouse_settings.launch.xml b/exercises/5.2/src/myworkcell_moveit_config/launch/warehouse_settings.launch.xml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/launch/warehouse_settings.launch.xml
rename to exercises/5.2/src/myworkcell_moveit_config/launch/warehouse_settings.launch.xml
diff --git a/exercises/5.1/src/myworkcell_moveit_config/package.xml b/exercises/5.2/src/myworkcell_moveit_config/package.xml
similarity index 100%
rename from exercises/5.1/src/myworkcell_moveit_config/package.xml
rename to exercises/5.2/src/myworkcell_moveit_config/package.xml
diff --git a/exercises/5.1/src/myworkcell_support/CMakeLists.txt b/exercises/5.2/src/myworkcell_support/CMakeLists.txt
similarity index 100%
rename from exercises/5.1/src/myworkcell_support/CMakeLists.txt
rename to exercises/5.2/src/myworkcell_support/CMakeLists.txt
diff --git a/exercises/5.1/src/myworkcell_support/launch/view_urdf.launch b/exercises/5.2/src/myworkcell_support/launch/view_urdf.launch
similarity index 100%
rename from exercises/5.1/src/myworkcell_support/launch/view_urdf.launch
rename to exercises/5.2/src/myworkcell_support/launch/view_urdf.launch
diff --git a/exercises/5.1/src/myworkcell_support/package.xml b/exercises/5.2/src/myworkcell_support/package.xml
similarity index 100%
rename from exercises/5.1/src/myworkcell_support/package.xml
rename to exercises/5.2/src/myworkcell_support/package.xml
diff --git a/exercises/5.1/src/myworkcell_support/rviz/myworkcell.rviz b/exercises/5.2/src/myworkcell_support/rviz/myworkcell.rviz
similarity index 100%
rename from exercises/5.1/src/myworkcell_support/rviz/myworkcell.rviz
rename to exercises/5.2/src/myworkcell_support/rviz/myworkcell.rviz
diff --git a/exercises/5.1/src/myworkcell_support/urdf/myworkcell.xacro b/exercises/5.2/src/myworkcell_support/urdf/myworkcell.xacro
similarity index 100%
rename from exercises/5.1/src/myworkcell_support/urdf/myworkcell.xacro
rename to exercises/5.2/src/myworkcell_support/urdf/myworkcell.xacro
diff --git a/exercises/5.4/detect_pump/CMakeLists.txt b/exercises/5.4/detect_pump/CMakeLists.txt
new file mode 100644
index 000000000..1923a3d96
--- /dev/null
+++ b/exercises/5.4/detect_pump/CMakeLists.txt
@@ -0,0 +1,198 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(detect_pump)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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
+  cv_bridge
+  rospy
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES detect_pump
+#  CATKIN_DEPENDS cv_bridge rospy
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/detect_pump.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/detect_pump_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_detect_pump.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/exercises/5.4/detect_pump/nodes/detect_pump.py b/exercises/5.4/detect_pump/nodes/detect_pump.py
new file mode 100755
index 000000000..ec800a24f
--- /dev/null
+++ b/exercises/5.4/detect_pump/nodes/detect_pump.py
@@ -0,0 +1,133 @@
+#!/usr/bin/env python
+import rospy
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+import cv2
+import math
+import numpy as np
+
+# known pump geometry
+#  - units are pixels (of half-size image)
+PUMP_DIAMETER = 360
+PISTON_DIAMETER = 90
+PISTON_COUNT = 7
+
+def showImage(img):
+    cv2.imshow('image', img)
+    cv2.waitKey(1)
+
+def plotCircles(img, circles, color):
+    if circles is None: return
+
+    for (x,y,r) in circles[0]:
+        cv2.circle(img, (int(x),int(y)), int(r), color, 2)
+
+def ptDist(p1, p2):
+    dx=p2[0]-p1[0]; dy=p2[1]-p1[1]
+    return math.sqrt( dx*dx + dy*dy )
+
+def ptMean(p1, p2):
+    return ((int(p1[0]+p2[0])/2, int(p1[1]+p2[1])/2))
+
+def rect2centerline(rect):
+    p0=rect[0]; p1=rect[1]; p2=rect[2]; p3=rect[3];
+    width=ptDist(p0,p1); height=ptDist(p1,p2);
+
+    # centerline lies along longest median
+    if (height > width):
+        cl = ( ptMean(p0,p1), ptMean(p2,p3) )
+    else:
+        cl = ( ptMean(p1,p2), ptMean(p3,p0) )
+
+    return cl
+
+def ptLineDist(pt, line):
+    x0=pt[0]; x1=line[0][0]; x2=line[1][0];
+    y0=pt[1]; y1=line[0][1]; y2=line[1][1];
+    return abs((x2-x1)*(y1-y0)-(x1-x0)*(y2-y1))/(math.sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)))
+
+def findAngle(p1, p2, p3):
+    p1=np.array(p1); p2=np.array(p2); p3=np.array(p3);
+    v1=p1-p2; v2=p3-p2;
+    return math.atan2(-v1[0]*v2[1]+v1[1]*v2[0],v1[0]*v2[0]+v1[1]*v2[1]) * 180/3.14159
+
+def process_image(msg):
+    try:
+        # convert sensor_msgs/Image to OpenCV Image
+        bridge = CvBridge()
+        orig = bridge.imgmsg_to_cv2(msg, "bgr8")
+        drawImg = orig
+
+        # resize image (half-size) for easier processing
+        resized = cv2.resize(orig, None, fx=0.5, fy=0.5)
+        drawImg = resized
+
+        # convert to single-channel image
+        gray = cv2.cvtColor(resized, cv2.COLOR_BGR2GRAY)
+        drawImg = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
+
+        # threshold grayscale to binary (black & white) image
+        threshVal = 150
+        ret,thresh = cv2.threshold(gray, threshVal, 255, cv2.THRESH_BINARY)
+        drawImg = cv2.cvtColor(thresh, cv2.COLOR_GRAY2BGR)
+
+        # detect outer pump circle
+        pumpRadiusRange = ( PUMP_DIAMETER/2-2, PUMP_DIAMETER/2+2)
+        pumpCircles = cv2.HoughCircles(thresh, cv2.HOUGH_GRADIENT, 1, PUMP_DIAMETER, param2=7, minRadius=pumpRadiusRange[0], maxRadius=pumpRadiusRange[1])
+        plotCircles(drawImg, pumpCircles, (255,0,0))
+        if (pumpCircles is None):
+            raise Exception("No pump circles found!")
+        elif len(pumpCircles[0])<>1:
+            raise Exception("Wrong # of pump circles: found {} expected {}".format(len(pumpCircles[0]),1))
+        else:
+            pumpCircle = pumpCircles[0][0]
+
+        # detect blobs inside pump body
+        pistonArea = 3.14159 * PISTON_DIAMETER**2 / 4
+        blobParams = cv2.SimpleBlobDetector_Params()
+        blobParams.filterByArea = True;
+        blobParams.minArea = 0.80 * pistonArea;
+        blobParams.maxArea = 1.20 * pistonArea;
+        blobDetector = cv2.SimpleBlobDetector_create(blobParams)
+        blobs = blobDetector.detect(thresh)
+        drawImg = cv2.drawKeypoints(drawImg, blobs, (), (0,255,0), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
+        if len(blobs) <> PISTON_COUNT:
+            raise Exception("Wring # of pistons: found {} expected {}".format(len(blobs), PISTON_COUNT))
+        pistonCenters = [(int(b.pt[0]),int(b.pt[1])) for b in blobs]
+
+        # determine primary axis, using largest contour
+        im2, contours, h = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+        maxC = max(contours, key=lambda c: cv2.contourArea(c))
+        boundRect = cv2.minAreaRect(maxC)
+        centerline = rect2centerline(cv2.boxPoints(boundRect))
+        cv2.line(drawImg, centerline[0], centerline[1], (0,0,255))
+
+        # find closest piston to primary axis
+        closestPiston = min( pistonCenters, key=lambda ctr: ptLineDist(ctr, centerline))
+        cv2.circle(drawImg, closestPiston, 5, (255,255,0), -1)
+
+        # calculate pump angle
+        p1 = (orig.shape[1], pumpCircle[1])
+        p2 = (pumpCircle[0], pumpCircle[1])
+        p3 = (closestPiston[0], closestPiston[1])
+        angle = findAngle(p1, p2, p3)
+        print "Found pump angle: {}".format(angle)
+
+    except Exception as err:
+        print err
+
+    # show results
+    showImage(drawImg)
+
+def start_node():
+    rospy.init_node('detect_pump')
+    rospy.loginfo('detect_pump node started')
+
+    rospy.Subscriber("image", Image, process_image)
+    rospy.spin()
+
+if __name__ == '__main__':
+    try:
+        start_node()
+    except rospy.ROSInterruptException:
+        pass
diff --git a/exercises/5.4/detect_pump/nodes/image_pub.py b/exercises/5.4/detect_pump/nodes/image_pub.py
new file mode 100755
index 000000000..62a6086c1
--- /dev/null
+++ b/exercises/5.4/detect_pump/nodes/image_pub.py
@@ -0,0 +1,39 @@
+#!/usr/bin/env python
+import rospy
+import sys
+import cv2
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image
+
+def rotateImg(img, angle):
+    rows,cols,ch = img.shape
+    M = cv2.getRotationMatrix2D((cols/2,rows/2),angle,1)
+    return cv2.warpAffine(img,M,(cols,rows))
+
+def start_node(filename):
+    rospy.init_node('image_pub')
+    rospy.loginfo('image_pub node started')
+
+    img = cv2.imread(filename)
+    # cv2.imshow("image", img)
+    # cv2.waitKey(2000)
+
+    bridge = CvBridge()
+    imgMsg = bridge.cv2_to_imgmsg(img, "bgr8")
+
+    pub = rospy.Publisher('image', Image, queue_size=10)
+
+    angle = 0
+    while not rospy.is_shutdown():
+        rotImg = rotateImg(img, angle)
+        imgMsg = bridge.cv2_to_imgmsg(rotImg, "bgr8")
+
+        pub.publish(imgMsg)
+        angle = (angle + 10) % 360
+        rospy.Rate(1.0).sleep()  # 1 Hz
+
+if __name__ == '__main__':
+    try:
+        start_node( rospy.myargv(argv=sys.argv)[1] )
+    except rospy.ROSInterruptException:
+        pass
diff --git a/exercises/5.4/detect_pump/package.xml b/exercises/5.4/detect_pump/package.xml
new file mode 100644
index 000000000..9deecf41b
--- /dev/null
+++ b/exercises/5.4/detect_pump/package.xml
@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>detect_pump</name>
+  <version>0.0.0</version>
+  <description>The detect_pump package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="ros-industrial@todo.todo">ros-industrial</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>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/detect_pump</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, 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 depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_export_depend>cv_bridge</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>rospy</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/exercises/5.4/pump.jpg b/exercises/5.4/pump.jpg
new file mode 100644
index 000000000..d896570f1
Binary files /dev/null and b/exercises/5.4/pump.jpg differ
diff --git a/exercises/6.1/src/myworkcell_core/src/test/utest.cpp b/exercises/6.1/src/myworkcell_core/src/test/utest.cpp
index 0e8cbed62..193469459 100644
--- a/exercises/6.1/src/myworkcell_core/src/test/utest.cpp
+++ b/exercises/6.1/src/myworkcell_core/src/test/utest.cpp
@@ -4,8 +4,9 @@
 
 fake_ar_publisher::ARMarkerConstPtr test_msg_;
 
-TEST(TestSuite, myworkcell_core_framework){
-    ASSERT_TRUE(true);
+TEST(TestSuite, myworkcell_core_framework)
+{
+  ASSERT_TRUE(true);
 }
 
 void testCallback(const fake_ar_publisher::ARMarkerConstPtr &msg)
@@ -13,7 +14,8 @@ void testCallback(const fake_ar_publisher::ARMarkerConstPtr &msg)
   test_msg_ = msg;
 }
 
-TEST(TestSuite, myworkcell_core_fake_ar_pub_ref_frame){
+TEST(TestSuite, myworkcell_core_fake_ar_pub_ref_frame)
+{
     ros::NodeHandle nh;
     ros::Subscriber sub = nh.subscribe("/ar_pose_marker", 1, &testCallback);
 
@@ -22,14 +24,15 @@ TEST(TestSuite, myworkcell_core_fake_ar_pub_ref_frame){
     EXPECT_EQ(test_msg_->header.frame_id, "camera_frame");
 }
 
-int main(int argc, char **argv){
-    testing::InitGoogleTest(&argc, argv);
-    ros::init(argc, argv, "MyWorkcellCoreTest");
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "MyWorkcellCoreTest");
 
-    ros::AsyncSpinner spinner(1);
-    spinner.start();
-    int ret = RUN_ALL_TESTS();
-    spinner.stop();
-    ros::shutdown();
-    return ret;
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
 }
diff --git a/gh_pages/_source/session5/Advanced-Descartes-Path-Planning.md b/gh_pages/_source/session5/Advanced-Descartes-Path-Planning.md
index 3a94bd304..98c006841 100644
--- a/gh_pages/_source/session5/Advanced-Descartes-Path-Planning.md
+++ b/gh_pages/_source/session5/Advanced-Descartes-Path-Planning.md
@@ -27,18 +27,18 @@ In this exercise, you will add two new nodes, two xacro, and config file to your
 ## Scan-N-Plan Application: Guidance
 In the interest of time, we've included several files:
  1. The first is a template node `adv_descartes_node.cpp` where most of the exercise is spent creating the complicated trajectory for deburring a complicated part.
- 1. The second node `adv_myworkcell_node.cpp` which is a duplicate of the `myworkcell_node.cpp` where it calls a service within the `adv_descartes_node.cpp`.
+ 1. The second node, `adv_myworkcell_node.cpp`, is a duplicate of the `myworkcell_node.cpp` that has been updated to call the `adv_plan_path` service provided by `adv_descartes_node.cpp`.
  1. The config file `puzzle_bent.csv` which contains the path relative to the part coordinate system.
  1. The two xacro files `puzzle_mount.xacro` and `grinder.xacro` which are used to update the urdf/xacro `workcell.xacro` file.
 
 Left to you are the details of:
  1. Updating the workcell.xacro file to include the two new xacro files.
- 1. Define a new move group in your moveit_config package called "puzzle".
+ 1. Updating the moveit_config package to define a new Planning Group for this exercise, including the new end-effector links.
  1. Defining a series of Cartesian poses that comprise a robot “path”.
  1. Translating those paths into something Descartes can understand.
 
 ### Setup workspace
- 1. If have not went through sessions 1-4, copy the src directory from exercise 4.1. Otherwise move to the next step.
+ 1. This exercise uses the same workspace from the Basic Training course.  If you don't have this workspace (completed through Exercise 4.1), copy the completed reference code and pull in other required dependencies as shown below. Otherwise move to the next step.
 
     ```bash
     mkdir ~/catkin_ws
@@ -68,7 +68,7 @@ Left to you are the details of:
  1. Copy over the `adv_myworkcell_node.cpp` into your core package's src/ folder.
 
     ```bash
-    cp ~/industrial_training/exercises/5.0/src/myworkcell_core/src/adv_myworkcell_node.cpp myworkcell_core/src/adv_myworkcell_node.cpp
+    cp ~/industrial_training/exercises/5.0/src/myworkcell_core/src/adv_myworkcell_node.cpp myworkcell_core/src/
     ```
 
  1. Create rules in the `myworkcell_core` package's `CMakeLists.txt` to build a new node called `adv_myworkcell_node`.  As in previous exercises, add these lines near similar lines in the template file (not as a block as shown below).
@@ -82,41 +82,55 @@ Left to you are the details of:
  1. Copy over the necessesary config file:
     ``` bash
     mkdir ~/catkin_ws/src/myworkcell_core/config
-    cp ~/industrial_training/exercises/5.0/src/myworkcell_core/config/puzzle_bent.csv myworkcell_core/config/puzzle_bent.csv
-    cp ~/industrial_training/exercises/5.0/src/myworkcell_support/urdf/grinder.xacro myworkcell_support/urdf/grinder.xacro
-    cp ~/industrial_training/exercises/5.0/src/myworkcell_support/urdf/puzzle_mount.xacro myworkcell_support/urdf/puzzle_mount.xacro
+    cp ~/industrial_training/exercises/5.0/src/myworkcell_core/config/puzzle_bent.csv myworkcell_core/config/
+    cp ~/industrial_training/exercises/5.0/src/myworkcell_support/urdf/grinder.xacro myworkcell_support/urdf/
+    cp ~/industrial_training/exercises/5.0/src/myworkcell_support/urdf/puzzle_mount.xacro myworkcell_support/urdf/
     mkdir ~/catkin_ws/src/myworkcell_support/meshes
     cp ~/industrial_training/exercises/5.0/src/myworkcell_support/meshes/* myworkcell_support/meshes/ 
     ```
 
+ 1. Add new package dependencies:
+    * Add `tf_conversions` to `CMakeLists.txt` (2 places) and `package.xml` (1 place)
+
 ### Update your workcell.xacro file.
- 1. Add two include tags for grinder.xacro and puzzle_mount.xacro.
- 1. Attach grinder to the **world** link with the origin: 
+ 1. Add two `<include>` tags for the new `grinder.xacro` and `puzzle_mount.xacro` files.
+ 1. Attach the grinder to the **world** link with the following offset: 
     ``` xml
     <origin xyz="0.0 -0.4 0.6" rpy="0 3.14159 0"/>
     ```
- 1. Attach the puzzle mount the robot link **tool0** with the origin:
+    * Look in the `grinder.xacro` file to locate the appropriate `<child_link>` name.
+    * Copy one of the other `<joint>` tag definitions and modify as appropriate.
+ 1. Attach the puzzle mount to the robot's **tool0** frame with the following offset:
     ``` xml
     <origin xyz="0 0 0" rpy="1.5708 0 0"/>
     ```
- 1. Launch the demo.launch file within your moveit_config package to verify the workcell. There should be a grinder sticking out of the table and a puzzle piece shaped part attached to the robot.
+    * Look in the `puzzle_mount.xacro` file to locate the appropriate `<child_link>` name.  You may need to study its various `<link>` and `<joint>` definitions to find the root link of this part.
+    * The `tool0` frame is standardized across most ROS-I URDFs to be the robot's end-effector mounting flange.
+
+ 1. Launch the demo.launch file within your moveit_config package to verify the workcell. There should be a grinder sticking out of the table and a puzzle-shaped part attached to the robot.
     ``` bash
     roslaunch myworkcell_moveit_config demo.launch
     ```
 
-### Add new move group to your moveit_config package.
- 1. Run moveit setup assistant and add a new move group with the kinematic chain **base_link -> part**. Note: Since you added geometry you should also regenerate the allowed collision matrix.
+### Add new planning group to your moveit_config package.
+ 1. Re-run the MoveIt! Setup Assistant and create a new Planning Group named **puzzle**.  Define the kinematic chain to extend from the **base_link** to the new **part** link.
     ``` bash
-    roslaunch moveit_setup_assistant setup_assistant.launch 
+    roslaunch myworkcell_moveit_config setup_assistant.launch 
     ```
+    * _Note: Since you added geometry, you should also regenerate the allowed collision matrix._
 
 ### Complete Advanced Descartes Node
- 1. First, the function `EigenSTL::vector_Affine3d makePuzzleToolPoses()` needs to be completed. The file path for **puzzle_bent.csv** is need. Don't not provide the full path, please use the ros tool `ros::package::getPath()` for getting the path of a package.
- 1. Next, the function `std::vector<descartes_core::TrajectoryPtPtr>
-  makeDescartesTrajectory(const EigenSTL::vector_Affine3d& path)` needs to be completed. The transform between **world** and **grinder_frame** needs to be found. Also Each point needs to have a tolerance set for the z-axis to +/- PI;
+ 1. First, the function `makePuzzleToolPoses()` needs to be completed. The file path for **puzzle_bent.csv** is needed. For portability, don't hardcode the full path. Please use the ROS tool `ros::package::getPath()` to retrieve the root path of the relevant package.
+    * reference [getPath()](http://docs.ros.org/kinetic/api/roslib/html/c++/namespaceros_1_1package.html#ae9470dd201aa4e66abb833e710d812a4) API
+ 1. Next, the function `makeDescartesTrajectory()` needs to be completed. The transform between **world** and **grinder_frame** needs to be found. Also Each point needs to have the orientation tolerance set for the z-axis to +/- PI;
+    * reference [lookupTransform()](http://docs.ros.org/kinetic/api/tf/html/c++/classtf_1_1Transformer.html#ac01a9f8709a828c427f1a5faa0ced42b) API
+    * reference [tf::conversions](http://docs.ros.org/kinetic/api/tf_conversions/html/c++/tf__eigen_8h.html) namespace
+    * reference [TolerancedFrame](https://github.com/ros-industrial-consortium/descartes/blob/kinetic-devel/descartes_trajectory/include/descartes_trajectory/cart_trajectory_pt.h#L156) definition
+    * reference [OrientationTolerance](https://github.com/ros-industrial-consortium/descartes/blob/kinetic-devel/descartes_trajectory/include/descartes_trajectory/cart_trajectory_pt.h#L139) definition
 
 ### Update the setup.launch file.
- 1. Update the file to take an argument **adv** so that either the basic or advanced descartes node can be launched.
+ 1. Update the file to take a boolean argument named **adv** so that either the basic or advanced descartes node can be launched.  Use `<if>` and `<unless>` modifiers to control which node is launched.
+    * reference [roslaunch XML](http://wiki.ros.org/roslaunch/XML) wiki
 
 ### Test Full Application
 
@@ -127,53 +141,7 @@ Left to you are the details of:
     rosrun myworkcell_core adv_myworkcell_node
     ```
 
-    It's difficult to see what's happening with the rviz planning-loop always running.  Disable this loop animation in rviz (Displays -> Planned Path -> Loop Animation), then rerun `adv_myworkcell_node`.
-
-### Solutions
- * 5.2 Update your workcell.xacro file.
-   ``` xml
-   <include filename="$(find myworkcell_support)/urdf/puzzle_mount.xacro" />
-   <include filename="$(find myworkcell_support)/urdf/grinder.xacro" />   
-   
-   <joint name="world_to_grinder" type="fixed">
-    <parent link="world"/>
-    <child link="grinder_frame"/>
-    <origin xyz="0.0 -0.4 0.6" rpy="0 3.14159 0"/>
-   </joint> 
+    * Descartes can take **several minutes** to plan this complex path, so be patient.
+    * It's difficult to see what's happening with the rviz planning-loop animation always running.  Disable this loop animation in rviz (Displays -> Planned Path -> Loop Animation) before running `adv_myworkcell_node`.
+
 
-   <joint name="robot_tool" type="fixed">
-    <parent link="tool0"/>
-    <child link="ee_mount"/>
-    <origin xyz="0 0 0" rpy="1.5708 0 0"/>
-   </joint>
-   ```
- * 5.4 Complete Advanced Descartes Node
-
-   **Step 1:**
-   ``` c++
-   std::string filename = ros::package::getPath("myworkcell_core") + "/config/puzzle_bent.csv";
-   ```
-
-   **Step 2:**
-   ``` c++
-   listener_.lookupTransform("world", "grinder_frame", ros::Time(0), grinder_frame);
-   tf::transformTFToEigen(grinder_frame, gf);
-   ```
-
-   **Step 3:**
-   ``` c++
-   tool_pt.orientation_tolerance.z_lower -= M_PI;
-   tool_pt.orientation_tolerance.z_upper += M_PI;
-   ```
-
- * 5.5 Update the setup.launch file.
-   ``` xml
-   <launch>
-     <arg name="adv" default="0" />
-     <include file="$(find myworkcell_moveit_config)/launch/myworkcell_planning_execution.launch"/>
-     <node name="fake_ar_publisher" pkg="fake_ar_publisher" type="fake_ar_publisher_node" />
-     <node name="vision_node" type="vision_node" pkg="myworkcell_core" output="screen"/>
-     <node name="descartes_node" type="descartes_node" pkg="myworkcell_core" output="screen" unless="$(arg adv)"/>
-     <node name="adv_descartes_node" type="adv_descartes_node" pkg="myworkcell_core" output="screen" if="$(arg adv)"/>
-   </launch>
-   ```
diff --git a/gh_pages/_source/session5/Building-a-Perception-Pipeline.rst b/gh_pages/_source/session5/Building-a-Perception-Pipeline.rst
index 0c33bbedc..1e761094a 100644
--- a/gh_pages/_source/session5/Building-a-Perception-Pipeline.rst
+++ b/gh_pages/_source/session5/Building-a-Perception-Pipeline.rst
@@ -4,7 +4,7 @@ In this exercise, we will fill in the appropriate pieces of code to build a perc
 
 Prepare New Workspace:
 ----------------------
-We will create a new catkin workspace, since this exercise does not overlap with the previous PlanNScan exercises.
+We will create a new catkin workspace, since this exercise does not overlap with the previous exercises.
 
 #. Disable automatic sourcing of your previous catkin workspace:
 
@@ -25,7 +25,7 @@ We will create a new catkin workspace, since this exercise does not overlap with
 
    .. code-block:: bash
 
-      cp -r ~/industrial_training/exercises/perception_ws ~
+      cp -r ~/industrial_training/exercises/5.1/template_ws ~/perception_ws
       cd ~/perception_ws/
 
  #. Initialize and Build this new workspace
@@ -41,14 +41,15 @@ We will create a new catkin workspace, since this exercise does not overlap with
 
       source ~/perception_ws/devel/setup.bash
 
-#. Download the :download:`PointCloud file </_downloads/table.pcd>` and place the file in your home directory (~).
-
+#. Copy the PointCloud file from prior Exercise 4.2 to your home directory (~):
 
+    .. code-block:: bash
 
+      cp ~/industrial_training/exercises/4.2/table.pcd ~
 
 #. Import the new workspace into your QTCreator IDE:
 
-   * In QTCreator: File -> New Project -> Import -> Import ROS Workspace -> ~/perception_ws
+   * In QTCreator: `File -> New File or Project -> Other Project -> ROS Workspace -> ~/perception_ws`
 
 Intro (Review Existing Code)
 ----------------------------
@@ -96,6 +97,8 @@ The task of filling in the middle section containing the perception algorithms i
 
 Implement Voxel Filter
 ^^^^^^^^^^^^^^^^^^^^^^
+#. Uncomment the `voxel_grid` include header, near the top of the file.
+
 #. Change code:
     
    The first step in most point cloud processing pipelines is the voxel filter. This filter not only helps to downsample your points, but also eliminates any NAN values so that any further filtering or processing is done on real values. See  `PCL Voxel Filter Tutorial <http://pointclouds.org/documentation/tutorials/voxel_grid.php#voxelgrid>`_ for hints, otherwise you can copy the below code snippet.
@@ -173,6 +176,8 @@ Viewing Results
 
 Implement Pass-through Filters
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+#. As before, uncomment the PassThrough filter include-header near the top of the file.
+
 #. Change code:
 
    The next set of useful filtering to get the region of interest, is a series of pass-through filters. These filters crop your point cloud down to a volume of space (if you use x y and z filter). At this point you should apply a series of pass-through filters, one for each the x, y, and z directions. See `PCL Pass-Through Filter Tutorial <http://pointclouds.org/documentation/tutorials/passthrough.php#passthrough>`_ for hints, or use code below.
@@ -297,6 +302,7 @@ Plane Segmentation
 #. Find the ``pcl::toROSMsg`` call where the ``pc2_cloud`` is populated.  This is the point cloud that is published to RViz display.  Replace the current cloud (``zf_cloud``) with the plane-fit outliers result (``*cloud_f``).
 
 #. Compile and run, as in previous steps.
+    Did you forget to uncomment the new headers used in this step?
 
 #. Evaluate Results
 
@@ -389,7 +395,7 @@ Create a CropBox Filter
        * The user should choose one or the other method
        * ========================================*/
 
-   This CropBox filter should replace your passthrough filters, you may delete or comment the passthrough filters. There is not PCL tutorial to guide you, only the PCL documentation at the link above. The general setup will be the same (set the output, declare instance of filter, set input, set parameters, and filter).
+   This CropBox filter should replace your passthrough filters, you may delete or comment the passthrough filters. There is no PCL tutorial to guide you, only the PCL documentation at the link above. The general setup will be the same (set the output, declare instance of filter, set input, set parameters, and filter).
 
    Set the output cloud:
 
@@ -413,8 +419,8 @@ Create a CropBox Filter
 
    .. code-block:: c++
 
-      Eigen::Vector4f min_point = Eigen::Vector4f(x_filter_min, y_filter_min, z_filter_min, 0);
-      Eigen::Vector4f max_point = Eigen::Vector4f(x_filter_max, y_filter_max, z_filter_max, 0);
+      Eigen::Vector4f min_point = Eigen::Vector4f(-1.0, -1.0, -1.0, 0);
+      Eigen::Vector4f max_point = Eigen::Vector4f(1.0, 1.0, 1.0, 0);
       crop.setMin(min_point);
       crop.setMax(max_point);
 
@@ -431,77 +437,11 @@ Create a CropBox Filter
       pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZ>(xyz_filtered_cloud));
 
 
-#. Update Publisher within perception_node.cpp, find section
-
-   .. code-block:: c++
-
-     /* ========================================
-      * CONVERT POINTCLOUD PCL->ROS
-      * PUBLISH CLOUD
-      * Fill Code: UPDATE AS NECESSARY
-      * ========================================*/
-
-   Change the "toROSMsg" line to convert from your newly processed cloud into a ROS sensor_msgs::PointCloud2.
-
-   Change:
-
-   .. code-block:: c++
+#. Find the ``pcl::toROSMsg`` call where the ``pc2_cloud`` is populated.  This is the point cloud that is published to RViz display.  Replace the current cloud with the new filtered results (``xyz_filtered_cloud``).
 
-      sensor_msgs::PointCloud2::Ptr pc2_cloud (new sensor_msgs::PointCloud2);
-      pcl::toROSMsg(zf_cloud, *pc2_cloud);
-
-   to:
-
-   .. code-block:: c++
+#. Compile and run, as in previous steps
 
-      sensor_msgs::PointCloud2::Ptr pc2_cloud (new sensor_msgs::PointCloud2);
-      pcl::toROSMsg(xyz_filtered_cloud, *pc2_cloud);
-
-#. Compile
-
-   .. code-block:: bash
-
-      catkin build
-
-
-   .. Note:: If you have the time/patience, I would suggest creating a ros publisher for each type of filter. It is often useful to view the results of multiple filters at once in Rviz and just toggle different clouds.
-
-Viewing Result
-""""""""""""""
-#. Open multiple terminals
-
-   Either open three more tabs within your terminal *CTRL-SHIFT-T* or open three more windows *CTRL-SHIFT-N*. These terminals will run a roscore, the pcl_ros, and Rviz. Below, Terminal 1 corresponds to the terminal you have been working out of.
-
-   In terminal 4:
-
-   .. code-block:: bash
-
-      roscore
-
-   In terminal 3:
-
-   .. code-block:: bash
-
-      cd ~
-      rosrun pcl_ros pcd_to_pointcloud table.pcd 0.1 _frame_id:=kinect_link cloud_pcd:=kinect/depth_registered/points
-
-   In terminal 2:
-
-   .. code-block:: bash
-
-      rosrun rviz rviz
-
-   In terminal 1:
-
-   .. code-block:: bash
-
-      rosrun lesson_perception perception_node
-
-#. View results
-
-   Within Rviz, add a *PointCloud2* and subscribe to the topic "object_cluster". What you see will be the results of the voxel filter overlaid on the original point cloud.
-
-   The following image of the CropBox filter in use will closely resemble the Plane Segmentation filter image.
+    The following image of the CropBox filter in use will closely resemble the Plane Segmentation filter image.
 
    .. image:: /_static/xyz_filtered_cloud.png
 
@@ -542,7 +482,7 @@ Create a Statistical Outlier Removal
 
       sor.setInputCloud (cluster_cloud_ptr);
 
-   Set parameters - looking at documentation, CropBox takes an Eigen Vector4f as inputs for max and min values:
+   Set parameters - looking at documentation, S.O.R. uses the number of neighbors to inspect and the standard-deviation threshold to use for outlier rejection:
 
    .. code-block:: c++
 
@@ -555,75 +495,9 @@ Create a Statistical Outlier Removal
 
       sor.filter (*sor_cloud_filtered);
 
-#. Update Publisher within perception_node.cpp, find section
-
-   .. code-block:: c++
-
-      /* ========================================
-       * CONVERT POINTCLOUD PCL->ROS
-       * PUBLISH CLOUD
-       * Fill Code: UPDATE AS NECESSARY
-       * ========================================*/
-
-   Change the "toROSMsg" line to convert from your newly processed cloud into a ROS sensor_msgs::PointCloud2.
-
-   Change:
-
-   .. code-block:: c++
-
-      sensor_msgs::PointCloud2::Ptr pc2_cloud (new sensor_msgs::PointCloud2);
-      pcl::toROSMsg(*cloud_f, *pc2_cloud);
-
-   to:
-
-   .. code-block:: c++
+#. Find the ``pcl::toROSMsg`` call where the ``pc2_cloud`` is populated.  Replace the current cloud with the new filtered results (``*sor_cloud_filtered``).
 
-      sensor_msgs::PointCloud2::Ptr pc2_cloud (new sensor_msgs::PointCloud2);
-      pcl::toROSMsg(*sor_cloud_filtered, *pc2_cloud);
-
-#. Compile
-
-   .. code-block:: bash
-
-      catkin build
-
-
-   .. Note:: If you have the time/patience, I would suggest creating a ros publisher for each type of filter. It is often useful to view the results of multiple filters at once in Rviz and just toggle different clouds.
-
-Viewing Result
-""""""""""""""
-#. Open multiple terminals
-
-   Either open three more tabs within your terminal *CTRL-SHIFT-T* or open three more windows *CTRL-SHIFT-N*. These terminals will run a roscore, the pcl_ros, and Rviz. Below, Terminal 1 corresponds to the terminal you have been working out of.
-
-   In terminal 4:
-
-   .. code-block:: bash
-
-      roscore
-
-   In terminal 3:
-
-   .. code-block:: bash
-
-      cd ~
-      rosrun pcl_ros pcd_to_pointcloud table.pcd 0.1 _frame_id:=kinect_link cloud_pcd:=kinect/depth_registered/points
-
-   In terminal 2:
-
-   .. code-block:: bash
-
-      rosrun rviz rviz
-
-   In terminal 1:
-
-   .. code-block:: bash
-
-      rosrun lesson_perception perception_node
-
-#. View results
-
-   Within Rviz, add a *PointCloud2* and subscribe to the topic "object_cluster". What you see will be the results of the voxel filter overlaid on the original point cloud.
+#. Compile and run, as in previous steps
 
    .. image:: /_static/sor_cloud_filtered.png
 
@@ -631,12 +505,10 @@ Viewing Result
 Create a Broadcast Transform
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-While this is not a filter method, it is directly related to the final project (the capstone project during the training class), so if you have the time, you should implement this to get a better understanding of how the demo works. 
+While this is not a filter method, it demonstrates how to publish the results of a processing pipeline for other nodes to use.  Often, the goal of a processing pipeline is to generate a measurement, location, or some other message for other nodes to use.  This sub-task broadcasts a TF transform to define the location of the largest box on the table.  This transform could be used by other nodes to identify the position/orientation of the box for grasping.
 
 #. Change/Insert code
 
-   Transforms are used to convey relations between two frames of reference or coordinate systems. In our demo, the AR tag detection software pipeline broadcasts a transform based on the position and orientation of the AR tag. A separate node then listens for that transform in order to identify the position/orientation of the box for grasping.
-
    Within perception_node.cpp, find section
 
    .. code-block:: c++
@@ -664,55 +536,16 @@ While this is not a filter method, it is directly related to the final project (
 
       br.sendTransform(tf::StampedTransform(part_transform, ros::Time::now(), world_frame, "part"));
 
-#. Compile
-
-   .. code-block:: bash
-
-      catkin build
-
-Viewing Result
-""""""""""""""
-
-#. Open multiple terminals
-
-   Either open three more tabs within your terminal *CTRL-SHIFT-T* or open three more windows *CTRL-SHIFT-N*. These terminals will run a roscore, the pcl_ros, and Rviz. Below, Terminal 1 corresponds to the terminal you have been working out of.
-
-   In terminal 4:
-
-   .. code-block:: bash
-
-      roscore
-
-   In terminal 3:
-   .. code-block:: bash
-
-      cd ~
-      rosrun pcl_ros pcd_to_pointcloud table.pcd 0.1 _frame_id:=kinect_link cloud_pcd:=kinect/depth_registered/points
-
-   In terminal 2:
-
-   .. code-block:: bash
-
-      rosrun rviz rviz
-
-   In terminal 1:
-
-   .. code-block:: bash
-
-      rosrun lesson_perception perception_node
-
- 2. View results
-
-    Within Rviz, add a *PointCloud2* and subscribe to the topic "object_cluster". What you see will be the results of the voxel filter overlaid on the original point cloud. There is no difference in the point cloud from the last image given in the statistical outlier removal. 
+#. Compile and Run as usual.  In this case, add a TF display to Rviz and observe the new "part" transform located at the top of the box.
 
 Create a Polygonal Segmentation
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-This method was included primarily to have something that ties in directly to the demo that you will be programming in session 5. This demo uses AR tag perception, but also collision avoidance. The collision avoidance uses a pointcloud to actively determine where the obstacles are. However, becuase the part itself is within the scene, we must subtract the points that comprise the part in order to remove these points from becoming a collision object (which will then make a grasp impossible due to the object colliding with itself).
+When using sensor data for collision detection, it is sometimes necessary to exclude "known" objects from the scene to avoid interference from these objects.  MoveIt! contains methods for masking out a robot's own geometry as a "Self Collision" filtering feature.  This example shows how to do something similar using PCL's Polygonal Segmentation filtering.
 
 #. Change code
 
-   This method is similar to the plane segmentation from Sub-Task 3, but instead of segmenting out a plane, you can segment and remove a prism. Documentation on the PCL Polygonal Segmentation can be found `here <http://docs.pointclouds.org/1.7.0/classpcl_1_1_convex_hull.html>`__ and `here <http://docs.pointclouds.org/trunk/classpcl_1_1_extract_polygonal_prism_data.html>`__. The goal in using this filter for this demo is to remove the points that correspond to the object of interest (because the collision/path planning requires it). So this particular filter is applied to the entire point cloud, but only after we know the position/orientation of the box.
+   This method is similar to the plane segmentation from Sub-Task 3, but instead of segmenting out a plane, you can segment and remove a prism. Documentation on the PCL Polygonal Segmentation can be found `here <http://docs.pointclouds.org/1.7.0/classpcl_1_1_convex_hull.html>`__ and `here <http://docs.pointclouds.org/trunk/classpcl_1_1_extract_polygonal_prism_data.html>`__. The goal in this sub-task is to remove the points that correspond to a known object (e.g. the box we detected earlier). This particular filter is applied to the entire point cloud (original sensor data), but only after we've already completed the processing steps to identify the position/orientation of the box.
 
    Within perception_node.cpp, add ``#include <tf_conversions/tf_eigen.h>`` and find section
 
@@ -749,7 +582,7 @@ This method was included primarily to have something that ties in directly to th
       prism.setInputCloud(sensor_cloud_ptr);
       pcl::PointIndices::Ptr pt_inliers (new pcl::PointIndices());
 
-   Set parameters - looking at documentation, ExtractPolygonalPrismData takes a polygon pointcloud as input.
+   Set parameters - looking at documentation, ExtractPolygonalPrismData uses a pointcloud defining the polygon vertices as its input.
 
    .. code-block:: c++
 
@@ -802,7 +635,7 @@ This method was included primarily to have something that ties in directly to th
       extract_ind.setInputCloud(sensor_cloud_ptr);
       extract_ind.setIndices(pt_inliers);
 
-   Set parameters - looking at documentation, ExtractPolygonalPrismData takes a polygon pointcloud as input:
+   This time, we invert the index extraction, so that we remove points inside the filter and keep points outside the filter.
 
    .. code-block:: c++
 
@@ -814,88 +647,19 @@ This method was included primarily to have something that ties in directly to th
 
       extract_ind.filter(*prism_filtered_cloud);
 
-#. Update Publisher within perception_node.cpp, find section
-
-   .. code-block:: c++
-
-      /* ========================================
-       * CONVERT POINTCLOUD PCL->ROS
-       * PUBLISH CLOUD
-       * Fill Code: UPDATE AS NECESSARY
-       * ========================================*/
-
-   Change the "toROSMsg" line to convert from your newly processed cloud into a ROS sensor_msgs::PointCloud2. Hint: If following the PCL tutorial, you will have a vector of sensor_msgs::PointCloud2; you can just publish the first one.
-
-    Change:
-
-    .. code-block:: c++
-
-       sensor_msgs::PointCloud2::Ptr pc2_cloud (new sensor_msgs::PointCloud2);
-       pcl::toROSMsg(*sor_cloud_filtered, *pc2_cloud);
-
-    to:
-
-    .. code-block:: c++
-
-       sensor_msgs::PointCloud2::Ptr pc2_cloud (new sensor_msgs::PointCloud2);
-       pcl::toROSMsg(*prism_filtered_cloud, *pc2_cloud);
-
-
-    .. Note:: *If you did not create your own publisher* to use for the Polygonal Segmentation filter, it will be necessary to move ``CONVERT POINTCLOUD PCL->ROS`` below ``extract_ind.filter(*prism_filtered_cloud);``.
-
-#. Compile
-
-   .. code-block:: bash
-
-      catkin build
-
-
-   .. Note:: If you have the time/patience, I would suggest creating a ros publisher for each type of filter. It is often useful to view the results of multiple filters at once in Rviz and just toggle different clouds.
-
-Viewing Result
-""""""""""""""
-
-#. Open multiple terminals
-
-   Either open three more tabs within your terminal *CTRL-SHIFT-T* or open three more windows *CTRL-SHIFT-N*. These terminals will run a roscore, the pcl_ros, and Rviz. Below, Terminal 1 corresponds to the terminal you have been working out of.
-
-   In terminal 4:
-
-   .. code-block:: bash
-
-      roscore
-
-   In terminal 3:
-
-   .. code-block:: bash
-
-      cd ~
-      rosrun pcl_ros pcd_to_pointcloud table.pcd 0.1 _frame_id:=kinect_link cloud_pcd:=kinect/depth_registered/points
+#. Find the ``pcl::toROSMsg`` call where the ``pc2_cloud`` is populated.  This is the point cloud that is published to RViz display.  Replace the current cloud with the new filtered results (``*prism_filtered_cloud``).
 
-   In terminal 2:
-
-   .. code-block:: bash
-
-      rosrun rviz rviz
-
-   In terminal 1:
-
-   .. code-block:: bash
-
-      rosrun lesson_perception perception_node
-
- 2. View results
-
-    Within Rviz, add a *PointCloud2* and subscribe to the topic "object_cluster". What you see will be the results of the voxel filter overlaid on the original point cloud.
+#. Compile and run as before.
 
     .. image:: /_static/prism_filtered_cloud.png
 
-   .. Note:: Notice the pointer is pointing to empty area of the table. That is the goal of using the filter this way.
+   .. Note:: Notice that the target box has been removed from the point cloud display.
 
 Write a launch file
 ^^^^^^^^^^^^^^^^^^^
 
-While this is not a filter method, it is useful when using PCL or other perception methods because of the number of parameters used in the different methods.  
+While this is not a filter method, it is useful when using PCL or other perception methods because of the number of parameters used in the different methods.
+
 #. Change/Insert code
 
    If you are really awesome and read the Task 1 write-up thoroughly, you will note that it was suggested that you put your parameters in one place.
@@ -912,10 +676,10 @@ While this is not a filter method, it is useful when using PCL or other percepti
 
    .. code-block:: yaml
 
-      world_frame="camera_depth_optical_frame";
+      world_frame="kinect_link";
       camera_frame="kinect_link";
-      cloud_topic="camera/depth_registered/points";
-      voxel_leaf_size=0.001f;
+      cloud_topic="kinect/depth_registered/points";
+      voxel_leaf_size=0.002f;
       x_filter_min=-2.5;
       x_filter_max=2.5;
       y_filter_min=-2.5;
@@ -929,37 +693,29 @@ While this is not a filter method, it is useful when using PCL or other percepti
       cluster_max_size=50000;
 
 
-   If you took this step, you will be in great shape to convert what you have into something that can be input from a launch file, or yaml file. You will want to use the "getParam" method as described in this `tutorial <http://wiki.ros.org/roscpp_tutorials/Tutorials/Parameters>`_. Get params from ros parameter server/launch file:
+   If you took this step, you will be in great shape to convert what you have into something that can be input from a launch file, or yaml file. You could use the "getParam" method as described in this `tutorial <http://wiki.ros.org/roscpp_tutorials/Tutorials/Parameters>`_. But a better choice might be to use the `param <http://docs.ros.org/kinetic/api/roscpp/html/classros_1_1NodeHandle.html#aa9b23d4206216ed13b5833fb1a090f1a>`_ method, which returns a default value if the parameter is not found on the parameter server.  Get params from ros parameter server/launch file, replacing your previous hardcoded values (but leave the variable declarations!)
 
    .. code-block:: c++
 
-      priv_nh_.getParam("cloud_topic", cloud_topic);
-      priv_nh_.getParam("world_frame", world_frame);
-      priv_nh_.getParam("camera_frame", camera_frame);
-      priv_nh_.getParam("voxel_leaf_size", voxel_leaf_size);
-      priv_nh_.getParam("x_filter_min", x_filter_min);
-      priv_nh_.getParam("x_filter_max", x_filter_max);
-      priv_nh_.getParam("y_filter_min", y_filter_min);
-      priv_nh_.getParam("y_filter_max", y_filter_max);
-      priv_nh_.getParam("z_filter_min", z_filter_min);
-      priv_nh_.getParam("z_filter_max", z_filter_max);
-      priv_nh_.getParamCached("plane_max_iterations", plane_max_iter);
-      priv_nh_.getParamCached("plane_distance_threshold", plane_dist_thresh);
-      priv_nh_.getParam("cluster_tolerance", cluster_tol);
-      priv_nh_.getParam("cluster_min_size", cluster_min_size);
-      priv_nh_.getParam("cluster_max_size", cluster_max_size);
-
-   Once you've done this, you can either delete or comment out your hard-coded values, but leave the declaration of those variables!
-
-#. Compile
-
-   .. code-block:: c++
-
-      catkin build
+      cloud_topic = priv_nh_.param<std::string>("cloud_topic", "kinect/depth_registered/points");
+      world_frame = priv_nh_.param<std::string>("world_frame", "kinect_link");
+      camera_frame = priv_nh_.param<std::string>("camera_frame", "kinect_link");
+      voxel_leaf_size = param<float>("voxel_leaf_size", 0.002);
+      x_filter_min = priv_nh_.param<float>("x_filter_min", -2.5);
+      x_filter_max = priv_nh_.param<float>("x_filter_max",  2.5);
+      y_filter_min = priv_nh_.param<float>("y_filter_min", -2.5);
+      y_filter_max = priv_nh_.param<float>("y_filter_max",  2.5);
+      z_filter_min = priv_nh_.param<float>("z_filter_min", -2.5);
+      z_filter_max = priv_nh_.param<float>("z_filter_max",  2.5);
+      plane_max_iter = priv_nh_.param<int>("plane_max_iterations", 50);
+      plane_dist_thresh = priv_nh_.param<float>("plane_distance_threshold", 0.05);
+      cluster_tol = priv_nh_.param<float>("cluster_tolerance", 0.01);
+      cluster_min_size = priv_nh_.param<int>("cluster_min_size", 100);
+      cluster_max_size = priv_nh_.param<int>("cluster_max_size", 50000);
 
 #. Write launch file.
 
-   Using gedit or some other text editor, make a new file (processing_node.launch) and put the following in it.
+   Using gedit or some other text editor, make a new file (''lesson_perception/launch/processing_node.launch'') and put the following in it.
 
    .. code-block:: xml
 
@@ -985,37 +741,12 @@ While this is not a filter method, it is useful when using PCL or other percepti
         </node>
       </launch>
 
-Viewing Results
-"""""""""""""""
-
-#. Open multiple terminals
-
-   Either open three more tabs within your terminal *CTRL-SHIFT-T* or open three more windows *CTRL-SHIFT-N*. These terminals will run a roscore, the pcl_ros, and Rviz. Below, Terminal 1 corresponds to the terminal you have been working out of.
-
-   In terminal 4:
-
-   .. code-block:: bash
-
-      roscore
-
-   In terminal 3:
+#. Compile as usual...
 
-   .. code-block:: bash
-
-      cd ~
-      rosrun pcl_ros pcd_to_pointcloud table.pcd 0.1 _frame_id:=kinect_link cloud_pcd:=kinect/depth_registered/points
+But this time, run the new launch file that was created instead of using rosrun to start the processing node.
 
-   In terminal 2:
-
-   .. code-block:: bash
-
-      rosrun rviz rviz -d `rospack find lesson_perception`/launch/lesson_perception.rviz
-
-   In terminal 1:
-
-   .. code-block:: bash
+The results should look similar to previous runs.  However, now you can edit these configuration parameters much easier!  No recompile step is required; just edit the launch-file values and relaunch the node.  In a real application, you could take this approach one step further and implement dynamic_reconfigure support in your node.  That would allow you to see the results of parameter changes in RViz in real-time!
 
-      roslaunch lesson_perception processing_node.launch
 
    When you are satisfied with the results, go to each terminal and *CTRL-C*.
 
diff --git a/gh_pages/_source/session5/OpenCV-in-Python.md b/gh_pages/_source/session5/OpenCV-in-Python.md
new file mode 100644
index 000000000..8088165b5
--- /dev/null
+++ b/gh_pages/_source/session5/OpenCV-in-Python.md
@@ -0,0 +1,516 @@
+# OpenCV Image Processing (Python)
+In this exercise, we will gain familiarity with both OpenCV and Python, through a simple 2D image-processing application.
+
+## Motivation
+OpenCV is a mature, stable library for 2D image processing, used in a wide variety of applications.  Much of ROS makes use of 3D sensors and point-cloud data, but there are still many applications that use traditional 2D cameras and image processing.
+
+This tutorial uses python to build the image-processing pipeline.  Python is a good choice for this application, due to its ease of rapid prototyping and existing bindings to the OpenCV library.
+
+## Further Information and Resources
+* [OpenCV Website](https://opencv.org/)
+* [OpenCV API](https://docs.opencv.org/3.0-beta/modules/refman.html)
+* [OpenCV Python Tutorials](https://docs.opencv.org/3.4.2/d6/d00/tutorial_py_root.html)
+* [ROS cv_bridge package (Python)](http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython)
+* [Writing a Publisher and Subscriber (Python)](http://wiki.ros.org/rospy_tutorials/Tutorials/WritingPublisherSubscriber)
+* [sensor_msgs/Image](http://docs.ros.org/kinetic/api/sensor_msgs/html/msg/Image.html)
+
+
+## Problem Statement
+In this exercise, you will create a new node to determine the angular pose of a pump housing using the OpenCV image processing library.  The pump's orientation is computed using a series of processing steps to extract and compare geometry features:
+
+  1. Resize the image (to speed up processing)
+  1. Threshold the image (convert to black & white)
+  1. Locate the pump's outer housing (circle-finding)
+  1. Locate the piston sleeve locations (blob detection)
+  1. Estimate primary axis using bounding box
+  1. Determine orientation using piston sleeve locations
+  1. Calculate the axis orientation relative to a reference (horizontal) axis
+  
+![pump images](https://aeswiki.datasys.swri.edu/rositraining/indigo/Exercises/2A.2?action=AttachFile&do=get&target=pump_images.png
+)
+
+## Implementation
+
+### Create package
+This exercise uses a single package that can be placed in any catkin workspace. The examples below will use the `~/catkin_ws` workspace from earlier exercises.
+
+ 1. Create a new `detect_pump` package to contain the new python nodes we'll be making:
+
+    ```bash
+    cd ~/catkin_ws/src
+    catkin create pkg detect_pump --catkin-deps rospy cv_bridge
+    ```
+    * all ROS packages depend on `rospy`
+    * we'll use `cv_bridge` to convert between ROS's standard Image message and OpenCV's Image object
+    * `cv_bridge` also automatically brings in dependencies on the relevant OpenCV modules
+
+ 1. Create a python module for this package:
+
+    ```bash
+    cd detect_pump
+    mkdir nodes
+    ```
+
+    * For a simple package such as this, the [Python Style Guide](http://docs.ros.org/kinetic/api/catkin/html/howto/format2/installing_python.html) recommends this simplified package structure.
+    * More complex packages (e.g. with exportable modules, msg/srv defintions, etc.) should us a more complex package structure, with an `__init__.py` and `setup.py`.
+        * reference [Installing Python Scripts](http://docs.ros.org/kinetic/api/catkin/html/howto/format2/installing_python.html)
+        * reference [Handling setup.py](http://docs.ros.org/api/catkin/html/user_guide/setup_dot_py.html)
+
+### Create an Image Publisher
+The first node will read in an image from a file and publish it as a ROS [Image](http://docs.ros.org/kinetic/api/sensor_msgs/html/msg/Image.html) message on the `image` topic.
+
+ * Note: ROS already contains an `image_publisher` package/node that performs this function, but we will duplicate it here to learn about ROS Publishers in Python.
+
+ 1. Create a new python script for our image-publisher node (`nodes/image_pub.py`).  Fill in the following template for a skeleton ROS python node:
+
+    ```python
+    #!/usr/bin/env python
+    import rospy
+
+    def start_node():
+        rospy.init_node('image_pub')
+        rospy.loginfo('image_pub node started')
+
+    if __name__ == '__main__':
+        try:
+            start_node()
+        except rospy.ROSInterruptException:
+            pass
+    ```
+
+ 1. Allow execution of the new script file:
+
+    ```bash
+    chmod u+x nodes/image_pub.py
+    ```
+
+ 1. Test run the image publisher:
+
+    ```bash
+    roscore
+    rosrun detect_pump image_pump.py
+    ```
+
+    * You should see the "node started" message
+
+ 1. Read the image file to publish, using the filename provided on the command line
+
+    1. Import the `sys` and `cv2` (OpenCV) modules:
+
+       ```python
+       import sys
+       import cv2
+       ```
+
+    1. Pass the command-line argument into the `start_node` function:
+
+       ```python
+       def start_node(filename):
+       ...
+       start_node( rospy.myargv(argv=sys.argv)[1] )
+       ```
+       * Note the use of `rospy.myargv()` to strip out any ROS-specific command-line arguments.
+
+    1. In the `start_node` function, call the OpenCV [imread](https://docs.opencv.org/3.0-beta/modules/imgcodecs/doc/reading_and_writing_images.html#imread) function to read the image.  Then use [imshow](https://docs.opencv.org/3.0-beta/modules/highgui/doc/user_interface.html#imshow) to display it:
+
+       ```python
+       img = cv2.imread(filename)
+       cv2.imshow("image", img)
+       cv2.waitKey(2000)
+       ```
+
+    1. Run the node, with the specified image file:
+
+       ```bash
+       rosrun detect_pump image_pub.py ~/industrial_training/exercises/5.4/pump.jpg
+       ```
+       * You should see the image displayed
+       * Comment out the `imshow`/`waitKey` lines, as we won't need those any more
+       * Note that you don't need to run `catkin build` after editing the python file, since no compile step is needed.
+
+ 1. Convert the image from OpenCV Image object to ROS Image message:
+
+    1. Import the `CvBridge` and `Image` (ROS message) modules:
+
+       ```python
+       from cv_bridge import CvBridge
+       from sensor_msgs.msg import Image
+       ```
+
+    1. Add a call to the CvBridge [cv2_to_imgmsg](https://docs.ros.org/api/cv_bridge/html/python/) method:
+
+       ```python
+       bridge = CvBridge()
+       imgMsg = bridge.cv2_to_imgmsg(img, "bgr8")
+       ```
+
+1. Create a ROS publisher to continually publish the Image message on the `image` topic.  Use a loop with a 1 Hz throttle to publish the message.
+
+    ```python
+    pub = rospy.Publisher('image', Image, queue_size=10)
+    while not rospy.is_shutdown():
+        pub.publish(imgMsg)
+        rospy.Rate(1.0).sleep()  # 1 Hz
+    ```
+
+1. Run the node and inspect the newly-published image message
+    1. Run the node (as before):
+
+       ```bash
+       rosrun detect_pump image_pub.py ~/industrial_training/exercises/5.4/pump.jpg
+       ```
+
+    1. Inspect the message topic using command-line tools:
+
+       ```bash
+       rostopic list
+       rostopic hz /image
+       rosnode info /image_pub
+       ```
+
+    1. Inspect the published image using the standalone [image_view](http://wiki.ros.org/image_view#image_view.2BAC8-diamondback.image_view) node
+
+       ```bash
+       rosrun image_view image_view
+       ```
+
+### Create the Detect_Pump Image-Processing Node
+The next node will subscribe to the `image` topic and execute a series of processing steps to identify the pump's orientation relative to the horizontal image axis.
+
+ 1. As before, create a basic ROS python node (`detect_pump.py`) and set its executable permissions:
+
+    ```python
+    #!/usr/bin/env python
+    import rospy
+
+    # known pump geometry
+    #  - units are pixels (of half-size image)
+    PUMP_DIAMETER = 360
+    PISTON_DIAMETER = 90
+    PISTON_COUNT = 7
+
+    def start_node():
+        rospy.init_node('detect_pump')
+        rospy.loginfo('detect_pump node started')
+
+    if __name__ == '__main__':
+        try:
+            start_node()
+        except rospy.ROSInterruptException:
+            pass
+    ```
+
+    ```bash
+    chmod u+x nodes/detect_pump.py
+    ```
+
+    * Note that we don't have to edit `CMakeLists` to create new build rules for each script, since python does not need to be compiled.
+
+ 1. Add a ROS subscriber to the `image` topic, to provide the source for images to process.
+
+    1. Import the `Image` message header
+
+       ```python
+       from sensor_msgs.msg import Image
+       ```
+
+    1. Above the `start_node` function, create an empty callback (`process_image`) that will be called when a new Image message is received:
+
+       ```python
+       def process_image(msg):
+	   try:
+	       pass
+           except Exception as err:
+               print err
+       ```
+       * The try/except error handling will allow our code to continue running, even if there are errors during the processing pipeline.
+
+    1. In the `start_node` function, create a ROS Subscriber object:
+       * subscribe to the `image` topic, monitoring messages of type `Image`
+       * register the callback function we defined above
+
+       ```python
+       rospy.Subscriber("image", Image, process_image)
+       rospy.spin()
+       ```
+
+       * reference: [rospy.Subscriber](http://docs.ros.org/kinetic/api/rospy/html/rospy.topics.Subscriber-class.html)
+       * reference: [rospy.spin](http://docs.ros.org/kinetic/api/rospy/html/rospy-module.html#spin)
+
+    1. Run the new node and verify that it is subscribing to the topic as expected:
+
+       ```bash
+       rosrun detect_pump detect_pump.py
+       rosnode info /detect_pump
+       rqt_graph
+       ```
+
+ 1. Convert the incoming `Image` message to an OpenCV `Image` object and display it
+ As before, we'll use the `CvBridge` module to do the conversion.
+
+    1. Import the `CvBridge` modules:
+
+       ```python
+       from cv_bridge import CvBridge
+       ```
+
+    1. In the `process_image` callback, add a call to the CvBridge [imgmsg_to_cv2](https://docs.ros.org/api/cv_bridge/html/python/) method:
+
+       ```python
+       # convert sensor_msgs/Image to OpenCV Image
+       bridge = CvBridge()
+       orig = bridge.imgmsg_to_cv2(msg, "bgr8")
+       ```
+       * This code (and all other image-processing code) should go inside the `try` block, to ensure that processing errors don't crash the node.
+       * This should replace the placeholder `pass` command placed in the `try` block earlier
+
+    1. Use the OpenCV `imshow` method to display the images received.  We'll create a pattern that can be re-used to show the result of each image-processing step.
+
+       1. Import the OpenCV `cv2` module:
+
+          ```python
+          import cv2
+          ```
+
+       1. Add a display helper function above the `process_image` callback:
+
+          ```python
+          def showImage(img):
+              cv2.imshow('image', img)
+              cv2.waitKey(1)
+          ```
+
+       1. Copy the received image to a new "drawImg" variable:
+
+          ```python
+          drawImg = orig
+          ```
+
+       1. **Below** the `except` block (outside its scope; at `process_image` scope, display the `drawImg` variable:
+
+          ```python
+          # show results
+          showImage(drawImg)
+          ```
+
+    1. Run the node and see the received image displayed.
+
+ 1. The first step in the image-processing pipeline is to resize the image, to speed up future processing steps.  Add the following code inside the `try` block, then rerun the node.
+
+    ```python
+    # resize image (half-size) for easier processing
+    resized = cv2.resize(orig, None, fx=0.5, fy=0.5)
+    drawImg = resized
+    ```
+    * you should see a smaller image being displayed
+    * reference: [resize()](https://docs.opencv.org/3.0-beta/modules/imgproc/doc/geometric_transformations.html#resize)
+
+ 1. Next, convert the image from color to grayscale.  Run the node to check for errors, but the image will still look the same as previously.
+
+    ```python
+    # convert to single-channel image
+    gray = cv2.cvtColor(resized, cv2.COLOR_BGR2GRAY)
+    drawImg = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
+    ```
+    * Even though the original image looks gray, the JPG file, Image message, and `orig` OpenCV image are all 3-channel color images.
+    * Many OpenCV functions operate on individual image channels.  Converting an image that appears gray to a "true" 1-channel grayscale image can help avoid confusion further on.
+    * We convert back to a color image for `drawImg` so that we can draw colored overlays on top of the image to display the results of later processing steps.
+    * reference: [cvtColor()](https://docs.opencv.org/3.0-beta/modules/imgproc/doc/miscellaneous_transformations.html#cvtcolor)
+
+ 1. Apply a thresholding operation to turn the grayscale image into a binary image.  Run the node and see the thresholded image.
+
+    ```python
+    # threshold grayscale to binary (black & white) image
+    threshVal = 75
+    ret,thresh = cv2.threshold(gray, threshVal, 255, cv2.THRESH_BINARY)
+    drawImg = cv2.cvtColor(thresh, cv2.COLOR_GRAY2BGR) 
+    ```
+
+    You should experiment with the `threshVal` paramter to find a value that works best for this image.  Valid values for this parameter lie between [0-255], to match the grayscale pixel intensity range.  Find a value that clearly highlights the pump face geometry.  I found that a value of `150` seemed good to me.
+
+    * reference [threshold](https://docs.opencv.org/3.0-beta/modules/imgproc/doc/miscellaneous_transformations.html#threshold)
+
+ 1. Detect the outer pump-housing circle.
+
+     This is not actually used to detect the pump angle, but serves as a good example of feature detection.  In a more complex scene, you could use OpenCV's Region Of Interest (ROI) feature to limit further processing to only features inside this pump housing circle.
+
+    1. Use the `HoughCircles` method to detect a pump housing of known size:
+
+       ```python
+       # detect outer pump circle
+       pumpRadiusRange = ( PUMP_DIAMETER/2-2, PUMP_DIAMETER/2+2)
+       pumpCircles = cv2.HoughCircles(thresh, cv2.HOUGH_GRADIENT, 1, PUMP_DIAMETER, param2=2, minRadius=pumpRadiusRange[0], maxRadius=pumpRadiusRange[1])
+       ```
+
+       * reference: [HoughCircles](https://docs.opencv.org/3.0-beta/modules/imgproc/doc/feature_detection.html#houghcircles)
+
+    1. Add a function to display all detected circles (above the `process_image` callback):
+
+       ```python
+       def plotCircles(img, circles, color):
+           if circles is None: return
+
+           for (x,y,r) in circles[0]:
+               cv2.circle(img, (int(x),int(y)), int(r), color, 2)
+       ```
+
+    1. Below the circle-detect, call the display function and check for the expected # of circles (1)
+
+       ```python
+       plotCircles(drawImg, pumpCircles, (255,0,0))
+       if (pumpCircles is None):
+           raise Exception("No pump circles found!")
+       elif len(pumpCircles[0])<>1:
+           raise Exception("Wrong # of pump circles: found {} expected {}".format(len(pumpCircles[0]),1))
+       else:
+           pumpCircle = pumpCircles[0][0]
+       ```
+
+    1. Run the node and see the detected circles.
+
+       * Experiment with adjusting the `param2` input to `HoughCircles` to find a value that seems to work well.  This parameter represents the sensitivity of the detector; lower values detect more circles, but also will return more false-positives.
+       * Tru removing the `min/maxRadius` parameters or reducing the minimum distance between circles (4th parameter) to see what other circles are detected.
+       * I found that a value of `param2=7` seemed to work well
+
+ 1. Detect the piston sleeves, using blob detection.
+
+    Blob detection analyses the image to identify connected regions (blobs) of similar color.  Filtering of the resulting blob features on size, shape, or other characteristics can help identify features of interest.  We will be using OpenCV's [SimpleBlobDetector](https://docs.opencv.org/3.2.0/d0/d7a/classcv_1_1SimpleBlobDetector.html).
+
+    1. Add the following code to run blob detection on the binary image:
+
+       ```python
+       # detect blobs inside pump body
+       pistonArea = 3.14159 * PISTON_DIAMETER**2 / 4
+       blobParams = cv2.SimpleBlobDetector_Params()
+       blobParams.filterByArea = True;
+       blobParams.minArea = 0.80 * pistonArea;
+       blobParams.maxArea = 1.20 * pistonArea;
+       blobDetector = cv2.SimpleBlobDetector_create(blobParams)
+       blobs = blobDetector.detect(thresh)
+       ```
+       * Note the use of an Area filter to select blobs within 20% of the expected piston-sleeve area.
+       * By default, the blob detector is configured to detect black blobs on a white background. so no additional color filtering is required.
+
+    1. Below the blob detection, call the OpenCV blob display function and check for the expected # of piston sleeves (7):
+
+       ```python
+       drawImg = cv2.drawKeypoints(drawImg, blobs, (), (0,255,0), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
+       if len(blobs) <> PISTON_COUNT:
+           raise Exception("Wring # of pistons: found {} expected {}".format(len(blobs), PISTON_COUNT))
+       pistonCenters = [(int(b.pt[0]),int(b.pt[1])) for b in blobs]
+       ```
+
+    1. Run the node and see if all piston sleeves were properly identified
+
+ 1. Detect the primary axis of the pump body.
+
+    This axis is used to identify the key piston sleeve feature.  We'll reduce the image to contours (outlines), then find the largest one, fit a rectangular box (rotated for best-fit), and identify the major axis of that box.
+
+    1. Calculate image contours and select the one with the largest area:
+
+       ```python
+       # determine primary axis, using largest contour
+       im2, contours, h = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+       maxC = max(contours, key=lambda c: cv2.contourArea(c))
+       ```
+
+    1. Fit a bounding box to the largest contour:
+
+       ```python
+       boundRect = cv2.minAreaRect(maxC)
+       ```
+
+    1. Copy these 3 helper functions to calculate the endpoints of the rectangle's major axis (above the `process_image` callback):
+
+       ```python
+       import math
+       ...
+
+       def ptDist(p1, p2):
+           dx=p2[0]-p1[0]; dy=p2[1]-p1[1]
+           return math.sqrt( dx*dx + dy*dy )
+
+       def ptMean(p1, p2):
+           return ((int(p1[0]+p2[0])/2, int(p1[1]+p2[1])/2))
+
+       def rect2centerline(rect):
+           p0=rect[0]; p1=rect[1]; p2=rect[2]; p3=rect[3];
+           width=ptDist(p0,p1); height=ptDist(p1,p2);
+
+           # centerline lies along longest median
+           if (height > width):
+               cl = ( ptMean(p0,p1), ptMean(p2,p3) )
+           else:
+               cl = ( ptMean(p1,p2), ptMean(p3,p0) )
+
+           return cl
+       ```
+
+    1. Call the `rect2centerline` function from above, with the bounding rectangle calculated earlier.  Draw the centerline on top of our display image.
+
+       ```python
+       centerline = rect2centerline(cv2.boxPoints(boundRect))
+       cv2.line(drawImg, centerline[0], centerline[1], (0,0,255))
+       ```
+
+ 1. The final step is to identify the key piston sleeve (closest to centerline) and use position to calculate the pump angle.
+
+    1. Add a helper function to calculate the distance between a point and the centerline:
+
+       ```python
+       def ptLineDist(pt, line):
+           x0=pt[0]; x1=line[0][0]; x2=line[1][0];
+           y0=pt[1]; y1=line[0][1]; y2=line[1][1];
+           return abs((x2-x1)*(y1-y0)-(x1-x0)*(y2-y1))/(math.sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)))
+       ```
+
+    1. Call the `ptLineDist` function to find which piston blob is closest to the centerline.  Update the drawImg to show which blob was identified.
+
+       ```python
+       # find closest piston to primary axis
+       closestPiston = min( pistonCenters, key=lambda ctr: ptLineDist(ctr, centerline))
+       cv2.circle(drawImg, closestPiston, 5, (255,255,0), -1)
+       ```
+
+    1. Calculate the angle between the 3 key points: piston sleeve centerpoint, pump center, and an arbitrary point along the horizontal axis (our reference "zero" position).
+
+        1. Add a helper function `findAngle` to calculate the angle between 3 points:
+
+           ```python
+           import numpy as np
+
+           def findAngle(p1, p2, p3):
+               p1=np.array(p1); p2=np.array(p2); p3=np.array(p3);
+               v1=p1-p2; v2=p3-p2;
+               return math.atan2(-v1[0]*v2[1]+v1[1]*v2[0],v1[0]*v2[0]+v1[1]*v2[1]) * 180/3.14159
+           ```
+
+        1. Call the `findAngle` function with the appropriate 3 keypoints:
+
+           ```python
+           # calculate pump angle
+           p1 = (orig.shape[1], pumpCircle[1])
+           p2 = (pumpCircle[0], pumpCircle[1])
+           p3 = (closestPiston[0], closestPiston[1])
+           angle = findAngle(p1, p2, p3)
+           print "Found pump angle: {}".format(angle)
+           ```
+
+ 1. You're done!  Run the node as before.  The reported pump angle should be near 24 degrees.
+
+### Challenge Exercises
+For a greater challenge, try the following suggestions to modify the operation of this image-processing example:
+
+ 1. Modify the `image_pub` node to rotate the image by 10 degrees between each publishing step.  The following code can be used to rotate an image:
+
+    ```python
+    def rotateImg(img, angle):
+        rows,cols,ch = img.shape
+        M = cv2.getRotationMatrix2D((cols/2,rows/2),angle,1)
+        return cv2.warpAffine(img,M,(cols,rows))
+    ```
+
+ 1. Change the `detect_pump` node to provide a **service** that performs the image detection.  Define a custom service type that takes an input image and outputs the pump angle.  Create a new application node that subscribes to the `image` topic and calls the `detect_pump` service.
+
+ 1. Try using `HoughCircles` instead of `BlobDetector` to locate the piston sleeves.
+
diff --git a/gh_pages/_source/session6/Unit-Testing.rst b/gh_pages/_source/session6/Unit-Testing.rst
index d4ab9b266..57850ebd6 100644
--- a/gh_pages/_source/session6/Unit-Testing.rst
+++ b/gh_pages/_source/session6/Unit-Testing.rst
@@ -11,7 +11,7 @@ The ROS Scan-N-Plan application is complete and documented.  Now we want to test
 Information and Resources
 -------------------------
 
-`Google Test <https://github.com/google/googletest>`__: C++ XUnit test framework
+`Google Test <https://github.com/google/googletest/blob/master/googletest/docs/primer.md>`__: C++ XUnit test framework
 
 `rostest <http://wiki.ros.org/rostest>`__: ROS wrapper for XUnit test framework
 
@@ -73,11 +73,11 @@ Create the unit test frame work
 
    .. code-block:: cmake
 
-	       if(CATKIN_ENABLE_TESTING)
-		      find_package(rostest REQUIRED)
-		      add_rostest_gtest(utest_node test/utest_launch.test src/test/utest.cpp)
-		      target_link_libraries(utest_node ${catkin_LIBRARIES})
-	       endif()
+            if(CATKIN_ENABLE_TESTING)
+              find_package(rostest REQUIRED)
+              add_rostest_gtest(utest_node test/utest_launch.test src/test/utest.cpp)
+              target_link_libraries(utest_node ${catkin_LIBRARIES})
+            endif()
 
 #. Create a test folder under myworkcell_core
 
@@ -101,14 +101,13 @@ Create the unit test frame work
                 <test test-name="unit_test_node" pkg="myworkcell_core" type="utest_node"/>
             </launch>
 
-#. Test the framework
+#. Build and test the framework
 
    .. code-block:: bash
 
-	       catkin build
-	       catkin run_tests
+	       catkin run_tests myworkcell_core
 
-   The console output should show:
+   The console output should show (buried in the midst of many build messages):
 
    .. code-block:: bash
 
@@ -124,6 +123,8 @@ Create the unit test frame work
 
    This means our framework is functional and now we can add usefull unit tests.
 
+   .. Note:: You can also run tests directly from the command line, using the launch file we made above: `rostest myworkcell_core utest_launch.test`.  Note that test files are not built using the regular `catkin build` command, so use `catkin run_tests myworkcell_core` instead.
+
 Add stock publisher tests
 ^^^^^^^^^^^^^^^^^^^^^^^^^
 
@@ -149,7 +150,7 @@ Add stock publisher tests
 
    .. code-block:: xml
 
-            rostest myworkcell_core utest_launch.test
+            catkin run_tests myworkcell_core
 
 You should see:
 
@@ -193,12 +194,28 @@ Write specific unit tests
                 EXPECT_EQ(test_msg_->header.frame_id, "camera_frame");
             }
 
+#. Add some node-initialization boilerplate to the main() function, since our unit tests interact with a running ROS system.  Replace the current main() function with the new code below:
+
+   .. code-block:: c++
+
+            int main(int argc, char **argv)
+            {
+              testing::InitGoogleTest(&argc, argv);
+              ros::init(argc, argv, "MyWorkcellCoreTest");
+
+              ros::AsyncSpinner spinner(1);
+              spinner.start();
+              int ret = RUN_ALL_TESTS();
+              spinner.stop();
+              ros::shutdown();
+              return ret;
+            }
+
 #. Run the test:
 
    .. code-block:: bash
 
-	       catkin build
-	       rostest myworkcell_core utest_launch.test
+	       catkin run_tests myworkcell_core
 
 #. view the results of the test:
 
diff --git a/gh_pages/index.rst b/gh_pages/index.rst
index 30e16829c..e41a996a1 100644
--- a/gh_pages/index.rst
+++ b/gh_pages/index.rst
@@ -121,6 +121,7 @@ Session 5 - Path Planning and Building a Perception Pipeline
    Exercise 5.1 - Building a Perception Pipeline <_source/session5/Building-a-Perception-Pipeline.md>
    Exercise 5.2 - Introduction to STOMP <_source/session5/Introduction-to-STOMP.md>
    Exercise 5.3 - Simple PCL Interface for Python <_source/session5/Simple-PCL-Interface-for-Python.rst>
+   Exercise 5.4 - OpenCV Image Processing (Python) <_source/session5/OpenCV-in-Python.md>
 
 Session 6 - Documentation, Unit Tests, ROS Utilities and Debugging ROS
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~