diff --git a/descartes_core/CMakeLists.txt b/descartes_core/CMakeLists.txt
index 0172621d..6baedd7f 100644
--- a/descartes_core/CMakeLists.txt
+++ b/descartes_core/CMakeLists.txt
@@ -5,7 +5,6 @@ add_compile_options(-std=c++11 -Wall -Wextra)
 
 find_package(catkin REQUIRED COMPONENTS
   cmake_modules
-  moveit_core
   roscpp
 )
 
@@ -23,7 +22,6 @@ catkin_package(
   LIBRARIES
     ${PROJECT_NAME}
   CATKIN_DEPENDS
-    moveit_core
     roscpp
   DEPENDS
     Boost
diff --git a/descartes_core/include/descartes_core/robot_model.h b/descartes_core/include/descartes_core/robot_model.h
index 497132e1..6f754dd3 100644
--- a/descartes_core/include/descartes_core/robot_model.h
+++ b/descartes_core/include/descartes_core/robot_model.h
@@ -23,6 +23,7 @@
 #include <moveit/kinematic_constraints/kinematic_constraint.h>
 #include "descartes_core/utils.h"
 
+
 namespace descartes_core
 {
 DESCARTES_CLASS_FORWARD(RobotModel);
@@ -150,6 +151,7 @@ class RobotModel
   }
 
   bool check_collisions_;
+
 };
 
 }  // descartes_core
diff --git a/descartes_core/package.xml b/descartes_core/package.xml
index 387c7acd..96a8997a 100644
--- a/descartes_core/package.xml
+++ b/descartes_core/package.xml
@@ -16,13 +16,11 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>boost</build_depend>
-  <build_depend>moveit_core</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>cmake_modules</build_depend>
   <build_depend>eigen</build_depend>
 
   <run_depend>boost</run_depend>
-  <run_depend>moveit_core</run_depend>
   <run_depend>roscpp</run_depend>
 
   <test_depend>rosunit</test_depend>
diff --git a/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h b/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
index eaa6965d..6afc198d 100644
--- a/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
+++ b/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
@@ -21,11 +21,13 @@
 
 #include "descartes_core/robot_model.h"
 #include "descartes_trajectory/cart_trajectory_pt.h"
-#include <moveit/planning_scene/planning_scene.h>
+
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <string>
 
+
 namespace descartes_moveit
 {
 /**
@@ -43,7 +45,7 @@ class MoveitStateAdapter : public descartes_core::RobotModel
   virtual bool initialize(const std::string &robot_description, const std::string &group_name,
                           const std::string &world_frame, const std::string &tcp_frame);
 
-  virtual bool initialize(robot_model::RobotModelConstPtr robot_model, const std::string &group_name,
+  virtual bool initialize(planning_scene_monitor::PlanningSceneMonitorPtr& psm, const std::string &group_name,
                           const std::string &world_frame, const std::string &tcp_frame);
 
   virtual bool getIK(const Eigen::Isometry3d &pose, const std::vector<double> &seed_state,
@@ -120,6 +122,11 @@ class MoveitStateAdapter : public descartes_core::RobotModel
    */
   bool isInLimits(const std::vector<double>& joint_pose) const;
 
+  /**
+   * @brief Updates the robot_state_ variable with the most recent joint states from the psm.
+   */
+  bool getCurrentRobotState() const;
+
   /**
    * Maximum joint velocities (rad/s) for each joint in the chain. Used for checking in
    * `isValidMove()`
@@ -128,12 +135,6 @@ class MoveitStateAdapter : public descartes_core::RobotModel
 
   mutable moveit::core::RobotStatePtr robot_state_;
 
-  planning_scene::PlanningScenePtr planning_scene_;
-
-  robot_model::RobotModelConstPtr robot_model_ptr_;
-
-  robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
-
   const moveit::core::JointModelGroup *joint_group_;
 
   /**
@@ -160,6 +161,11 @@ class MoveitStateAdapter : public descartes_core::RobotModel
    * @brief convenient transformation frame
    */
   descartes_core::Frame world_to_root_;
+
+  /**
+   * @brief Planning scene monitor (used to update internal planning scene)
+   */
+  mutable planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
 };
 
 }  // descartes_moveit
diff --git a/descartes_moveit/package.xml b/descartes_moveit/package.xml
index 610893b5..eaea0499 100644
--- a/descartes_moveit/package.xml
+++ b/descartes_moveit/package.xml
@@ -7,7 +7,7 @@
   <maintainer email="jnicho@swri.org">Jorge Nicho</maintainer>
   <maintainer email="jonathan.meyer@swri.org">Jonathan Meyer</maintainer>
   <maintainer email="sedwards@swri.org">Shaun Edwards</maintainer>
-  
+
   <license>Apache2</license>
 
   <author >Shaun Edwards</author>
diff --git a/descartes_moveit/src/ikfast_moveit_state_adapter.cpp b/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
index 2b623ab5..f5cdf526 100644
--- a/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
+++ b/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
@@ -21,8 +21,8 @@
 #include <eigen_conversions/eigen_msg.h>
 #include <ros/node_handle.h>
 
-const static std::string default_base_frame = "base_link";
-const static std::string default_tool_frame = "tool0";
+const static std::string DEFAULT_BASE_FRAME = "base_link";
+const static std::string DEFAULT_TOOL_FRAME = "tool0";
 
 // Compute the 'joint distance' between two poses
 static double distance(const std::vector<double>& a, const std::vector<double>& b)
@@ -140,8 +140,8 @@ bool descartes_moveit::IkFastMoveitStateAdapter::computeIKFastTransforms()
   // look up the IKFast base and tool frame
   ros::NodeHandle nh;
   std::string ikfast_base_frame, ikfast_tool_frame;
-  nh.param<std::string>("ikfast_base_frame", ikfast_base_frame, default_base_frame);
-  nh.param<std::string>("ikfast_tool_frame", ikfast_tool_frame, default_tool_frame);
+  nh.param<std::string>("ikfast_base_frame", ikfast_base_frame, DEFAULT_BASE_FRAME);
+  nh.param<std::string>("ikfast_tool_frame", ikfast_tool_frame, DEFAULT_TOOL_FRAME);
 
   if (!robot_state_->knowsFrameTransform(ikfast_base_frame))
   {
diff --git a/descartes_moveit/src/moveit_state_adapter.cpp b/descartes_moveit/src/moveit_state_adapter.cpp
index 6f6ad8ed..3526e5b3 100644
--- a/descartes_moveit/src/moveit_state_adapter.cpp
+++ b/descartes_moveit/src/moveit_state_adapter.cpp
@@ -23,7 +23,6 @@
 #include "descartes_core/pretty_print.hpp"
 #include "descartes_moveit/seed_search.h"
 
-
 #include <eigen_conversions/eigen_msg.h>
 #include <random_numbers/random_numbers.h>
 #include <ros/assert.h>
@@ -71,29 +70,25 @@ MoveitStateAdapter::MoveitStateAdapter() : world_to_root_(Eigen::Isometry3d::Ide
 
 bool MoveitStateAdapter::initialize(const std::string& robot_description, const std::string& group_name,
                                     const std::string& world_frame, const std::string& tcp_frame)
+
 {
   // Initialize MoveIt state objects
-  robot_model_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
-  auto model = robot_model_loader_->getModel();
-  if (!model)
-  {
-    CONSOLE_BRIDGE_logError("Failed to load robot model from robot description parameter: %s", robot_description.c_str());
-    return false;
-  }
-
-  return initialize(model, group_name, world_frame, tcp_frame);
+  planning_scene_monitor::PlanningSceneMonitorPtr psm(new planning_scene_monitor::PlanningSceneMonitor(robot_description));
+  return initialize(psm, group_name, world_frame, tcp_frame);
 }
 
-bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model, const std::string &group_name,
+bool MoveitStateAdapter::initialize(planning_scene_monitor::PlanningSceneMonitorPtr& psm, const std::string &group_name,
                                     const std::string &world_frame, const std::string &tcp_frame)
 {
-  robot_model_ptr_ = robot_model;
-  robot_state_.reset(new moveit::core::RobotState(robot_model_ptr_));
+  planning_scene_monitor_ = psm;
+  planning_scene_monitor_->startSceneMonitor();
+
+  robot_state_.reset(new moveit::core::RobotState(planning_scene_monitor_->getRobotModel()));
   robot_state_->setToDefaultValues();
-  planning_scene_.reset(new planning_scene::PlanningScene(robot_model));
-  joint_group_ = robot_model_ptr_->getJointModelGroup(group_name);
+  getCurrentRobotState();
+  joint_group_ = planning_scene_monitor_->getRobotModel()->getJointModelGroup(group_name);
 
-  // Assign robot frames
+  // Assign robot parameters
   group_name_ = group_name;
   tool_frame_ = tcp_frame;
   world_frame_ = world_frame;
@@ -135,10 +130,11 @@ bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model,
               " transformed to world frame '%s'",
               __FUNCTION__, world_frame_.c_str(), model_frame.c_str(), world_frame_.c_str());
 
-    Eigen::Isometry3d root_to_world = toIsometry(robot_state_->getFrameTransform(world_frame_));
+    Eigen::Isometry3d root_to_world = Eigen::Isometry3d(robot_state_->getFrameTransform(world_frame_));
     world_to_root_ = descartes_core::Frame(root_to_world.inverse());
   }
 
+  // Start the psm
   return true;
 }
 
@@ -154,14 +150,14 @@ bool MoveitStateAdapter::getIK(const Eigen::Isometry3d& pose, std::vector<double
   bool rtn = false;
 
   // transform to group base
-  Eigen::Isometry3d tool_pose = world_to_root_.frame * pose;
+  Eigen::Isometry3d tool_pose = Eigen::Isometry3d(world_to_root_.frame * pose);
 
   if (robot_state_->setFromIK(joint_group_, tool_pose, tool_frame_))
   {
     robot_state_->copyJointGroupPositions(group_name_, joint_pose);
     if (!isValid(joint_pose))
     {
-      ROS_DEBUG_STREAM("Robot joint pose is invalid");
+      CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.getIK: Robot joint pose is invalid");
     }
     else
     {
@@ -183,25 +179,26 @@ bool MoveitStateAdapter::getAllIK(const Eigen::Isometry3d& pose, std::vector<std
   // in the middle of a discretization step could be double counted.  In reality, we'd like solutions
   // to be further apart than this.
   double epsilon = 4 * joint_group_->getSolverInstance()->getSearchDiscretization();
-  CONSOLE_BRIDGE_logDebug("Utilizing an min. difference of %f between IK solutions", epsilon);
+  CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.getAllIK: Utilizing an min. difference of %f between IK solutions", epsilon);
   joint_poses.clear();
   for (size_t sample_iter = 0; sample_iter < seed_states_.size(); ++sample_iter)
   {
-    robot_state_->setJointGroupPositions(group_name_, seed_states_[sample_iter]);
     std::vector<double> joint_pose;
     if (getIK(pose, joint_pose))
     {
       if (joint_poses.empty())
       {
         std::stringstream msg;
-        msg << "Found *first* solution on " << sample_iter << " iteration, joint: " << joint_pose;
+        msg << "MoveitStateAdapter.getAllIK: " << "Found *first* solution on " << sample_iter
+            << " iteration, joint: " << joint_pose;
         CONSOLE_BRIDGE_logDebug(msg.str().c_str());
         joint_poses.push_back(joint_pose);
       }
       else
       {
         std::stringstream msg;
-        msg << "Found *potential* solution on " << sample_iter << " iteration, joint: " << joint_pose;
+        msg << "MoveitStateAdapter.getAllIK: " << "Found *potential* solution on " << sample_iter
+            << " iteration, joint: " << joint_pose;
         CONSOLE_BRIDGE_logDebug(msg.str().c_str());
 
         std::vector<std::vector<double> >::iterator joint_pose_it;
@@ -210,7 +207,7 @@ bool MoveitStateAdapter::getAllIK(const Eigen::Isometry3d& pose, std::vector<std
         {
           if (descartes_core::utils::equal(joint_pose, (*joint_pose_it), epsilon))
           {
-            CONSOLE_BRIDGE_logDebug("Found matching, potential solution is not new");
+            CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.getAllIK: Found matching, potential solution is not new");
             match_found = true;
             break;
           }
@@ -218,26 +215,28 @@ bool MoveitStateAdapter::getAllIK(const Eigen::Isometry3d& pose, std::vector<std
         if (!match_found)
         {
           std::stringstream msg;
-          msg << "Found *new* solution on " << sample_iter << " iteration, joint: " << joint_pose;
+          msg << "MoveitStateAdapter.getAllIK: " << "Found *new* solution on " << sample_iter
+              << " iteration, joint: " << joint_pose;
           CONSOLE_BRIDGE_logDebug(msg.str().c_str());
           joint_poses.push_back(joint_pose);
         }
       }
     }
+    robot_state_->setJointGroupPositions(group_name_, seed_states_[sample_iter]);
   }
 
-  CONSOLE_BRIDGE_logDebug("Found %lu joint solutions out of %lu iterations", static_cast<unsigned long>(joint_poses.size()),
-           static_cast<unsigned long>(seed_states_.size()));
+  CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.getAllIK: Found %lu joint solutions out of %lu iterations", static_cast<unsigned long>(joint_poses.size()),
+                          static_cast<unsigned long>(seed_states_.size()));
 
   if (joint_poses.empty())
   {
-    CONSOLE_BRIDGE_logError("Found 0 joint solutions out of %lu iterations", static_cast<unsigned long>(seed_states_.size()));
+    CONSOLE_BRIDGE_logError("MoveitStateAdapter.getAllIK: Found 0 joint solutions out of %lu iterations", static_cast<unsigned long>(seed_states_.size()));
     return false;
   }
   else
   {
-    CONSOLE_BRIDGE_logInform("Found %lu joint solutions out of %lu iterations", static_cast<unsigned long>(joint_poses.size()),
-              static_cast<unsigned long>(seed_states_.size()));
+    CONSOLE_BRIDGE_logInform("MoveitStateAdapter.getAllIK: Found %lu joint solutions out of %lu iterations", static_cast<unsigned long>(joint_poses.size()),
+                             static_cast<unsigned long>(seed_states_.size()));
     return true;
   }
 }
@@ -247,11 +246,23 @@ bool MoveitStateAdapter::isInCollision(const std::vector<double>& joint_pose) co
   bool in_collision = false;
   if (check_collisions_)
   {
-    moveit::core::RobotState state (robot_model_ptr_);
-    state.setToDefaultValues();
-    state.setJointGroupPositions(joint_group_, joint_pose);
-    in_collision = planning_scene_->isStateColliding(state, group_name_);
+    std::unique_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_));
+
+    // Check if we successfully got a locked planning scene
+    if (!(*ls))
+    {
+      CONSOLE_BRIDGE_logError("MoveitStateAdapter.isInCollision: Failed to secure a locked planning scene. Has the planing scene monitor been correctly initialized?");
+      return false;
+    }
+
+    // Seed with current state in case their are joints outside of the planning group
+    robot_state::RobotState robot_state_copy = (*ls)->getCurrentState();
+    robot_state_copy.setJointGroupPositions(group_name_, joint_pose);
+
+    // If the state is colliding return false
+    in_collision = (*ls)->isStateColliding(robot_state_copy);
   }
+
   return in_collision;
 }
 
@@ -263,24 +274,25 @@ bool MoveitStateAdapter::isInLimits(const std::vector<double> &joint_pose) const
 bool MoveitStateAdapter::getFK(const std::vector<double>& joint_pose, Eigen::Isometry3d& pose) const
 {
   bool rtn = false;
+  getCurrentRobotState();
   robot_state_->setJointGroupPositions(group_name_, joint_pose);
   if (isValid(joint_pose))
   {
     if (robot_state_->knowsFrameTransform(tool_frame_))
     {
-      pose = toIsometry(world_to_root_.frame * robot_state_->getFrameTransform(tool_frame_));
+      pose = Eigen::Isometry3d(world_to_root_.frame * robot_state_->getFrameTransform(tool_frame_));
       //pose.
       rtn = true;
     }
     else
     {
-      CONSOLE_BRIDGE_logError("Robot state does not recognize tool frame: %s", tool_frame_.c_str());
+      CONSOLE_BRIDGE_logError("MoveitStateAdapter.getFK: Robot state does not recognize tool frame: %s", tool_frame_.c_str());
       rtn = false;
     }
   }
   else
   {
-    CONSOLE_BRIDGE_logError("Invalid joint pose passed to get forward kinematics");
+    CONSOLE_BRIDGE_logError("MoveitStateAdapter.getFK: Invalid joint pose passed to get forward kinematics");
     rtn = false;
   }
 
@@ -298,7 +310,21 @@ bool MoveitStateAdapter::isValid(const std::vector<double>& joint_pose) const
     return false;
   }
 
-  return isInLimits(joint_pose) && !isInCollision(joint_pose);
+  // Satisfies joint positional bounds?
+  if (!isInLimits(joint_pose))
+  {
+    CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.isValid: Joint pose does not satisfy positional bounds");
+    return false;
+  }
+
+  // Is in collision (if collision is active)
+  if (isInCollision(joint_pose))
+  {
+    CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.isValid: Joint pose is in collision");
+    return false;
+  }
+
+  return true;
 }
 
 bool MoveitStateAdapter::isValid(const Eigen::Isometry3d& pose) const
@@ -335,10 +361,31 @@ std::vector<double> MoveitStateAdapter::getJointVelocityLimits() const
 
 void MoveitStateAdapter::setState(const moveit::core::RobotState& state)
 {
-  ROS_ASSERT_MSG(static_cast<bool>(robot_state_), "'robot_state_' member pointer is null. Have you called "
+  if(static_cast<bool>(robot_state_)){
+    CONSOLE_BRIDGE_logDebug("'robot_state_' member pointer is null. Have you called "
                                                   "initialize()?");
+  }
   *robot_state_ = state;
-  planning_scene_->setCurrentState(state);
+  robot_state_->update();
+  planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_)->setCurrentState(state);
+}
+
+bool MoveitStateAdapter::getCurrentRobotState() const
+{
+  planning_scene_monitor_->updateFrameTransforms();
+  std::unique_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_));
+
+  if (!(*ls))
+  {
+    CONSOLE_BRIDGE_logWarn("MoveitStateAdapter.getCurrentRobotState: Failed to get locked planning scene.");
+    return false;
+  }
+  else
+  {
+    *robot_state_ = (*ls)->getCurrentState();
+    robot_state_->update();
+    return true;
+  }
 }
 
 }  // descartes_moveit
diff --git a/descartes_moveit/src/seed_search.cpp b/descartes_moveit/src/seed_search.cpp
index 4e29df19..0c856624 100644
--- a/descartes_moveit/src/seed_search.cpp
+++ b/descartes_moveit/src/seed_search.cpp
@@ -30,7 +30,7 @@ bool doFK(moveit::core::RobotState& state, const moveit::core::JointModelGroup*
     return false;
   }
 
-  result = toIsometry(state.getFrameTransform(tool));
+  result = Eigen::Isometry3d(state.getFrameTransform(tool));
   return true;
 }
 
@@ -41,11 +41,10 @@ bool doFK(moveit::core::RobotState& state, const moveit::core::JointModelGroup*
 bool doIK(moveit::core::RobotState& state, const moveit::core::JointModelGroup* group, const std::string& group_name,
           const std::string& tool, const Eigen::Isometry3d& pose, const JointConfig& seed, JointConfig& result)
 {
-  const static int N_ATTEMPTS = 1;
   const static double IK_TIMEOUT = 0.01;
 
   state.setJointGroupPositions(group_name, seed);
-  if (!state.setFromIK(group, pose, tool, N_ATTEMPTS, IK_TIMEOUT))
+  if (!state.setFromIK(group, pose, tool, IK_TIMEOUT))
   {
     return false;
   }
diff --git a/descartes_trajectory/include/descartes_trajectory/cart_trajectory_pt.h b/descartes_trajectory/include/descartes_trajectory/cart_trajectory_pt.h
index 1ab6fae9..956c4c2e 100644
--- a/descartes_trajectory/include/descartes_trajectory/cart_trajectory_pt.h
+++ b/descartes_trajectory/include/descartes_trajectory/cart_trajectory_pt.h
@@ -102,10 +102,10 @@ struct ToleranceBase
     , y_lower(y_lower_lim)
     , z_lower(z_lower_lim)
   {
-    ROS_DEBUG_STREAM("Creating fully defined Tolerance(base type)");
-    ROS_DEBUG_STREAM("Initializing x tolerance (lower/upper)" << x_lower << "/" << x_upper);
-    ROS_DEBUG_STREAM("Initializing y tolerance (lower/upper)" << y_lower << "/" << y_upper);
-    ROS_DEBUG_STREAM("Initializing z tolerance (lower/upper)" << z_lower << "/" << z_upper);
+    ROS_DEBUG_STREAM_NAMED("ToleranceBase", "Creating fully defined Tolerance(base type)");
+    ROS_DEBUG_STREAM_NAMED("ToleranceBase", "Initializing x tolerance (lower/upper)" << x_lower << "/" << x_upper);
+    ROS_DEBUG_STREAM_NAMED("ToleranceBase", "Initializing y tolerance (lower/upper)" << y_lower << "/" << y_upper);
+    ROS_DEBUG_STREAM_NAMED("ToleranceBase", "Initializing z tolerance (lower/upper)" << z_lower << "/" << z_upper);
   }
 
   void clear()
@@ -129,7 +129,7 @@ struct PositionTolerance : public ToleranceBase
                     double z_upper_lim)
     : ToleranceBase(x_lower_lim, x_upper_lim, y_lower_lim, y_upper_lim, z_lower_lim, z_upper_lim)
   {
-    ROS_DEBUG_STREAM("Created fully defined Position Tolerance");
+    ROS_DEBUG_STREAM_NAMED("PositionTolerance", "Created fully defined Position Tolerance");
   }
 };
 
@@ -146,7 +146,7 @@ struct OrientationTolerance : public ToleranceBase
                        double z_lower_lim, double z_upper_lim)
     : ToleranceBase(x_lower_lim, x_upper_lim, y_lower_lim, y_upper_lim, z_lower_lim, z_upper_lim)
   {
-    ROS_DEBUG_STREAM("Created fully defined Orientation Tolerance");
+    ROS_DEBUG_STREAM_NAMED("OrientationTolerance","Created fully defined Orientation Tolerance");
   }
 };
 
diff --git a/descartes_trajectory/src/cart_trajectory_pt.cpp b/descartes_trajectory/src/cart_trajectory_pt.cpp
index 07df6820..c9d37363 100644
--- a/descartes_trajectory/src/cart_trajectory_pt.cpp
+++ b/descartes_trajectory/src/cart_trajectory_pt.cpp
@@ -83,7 +83,7 @@ EigenSTL::vector_Isometry3d uniform(const TolerancedFrame &frame, const double o
   // Estimating the number of samples base on tolerance zones and sampling increment.
   size_t est_num_samples = ntx * nty * ntz * nrx * nry * nrz;
 
-  ROS_DEBUG_STREAM("Estimated number of samples: " << est_num_samples << ", reserving space");
+  ROS_DEBUG_STREAM_NAMED("Descartes", "Estimated number of samples: " << est_num_samples << ", reserving space");
   rtn.reserve(est_num_samples);
 
   // TODO: The following for loops do not ensure that the rull range is sample (lower to upper)
@@ -124,9 +124,10 @@ EigenSTL::vector_Isometry3d uniform(const TolerancedFrame &frame, const double o
       }
     }
   }
-  ROS_DEBUG_STREAM("Uniform sampling of frame, utilizing orientation increment: "
-                   << orient_increment << ", and position increment: " << pos_increment << " resulted in " << rtn.size()
-                   << " samples");
+  ROS_DEBUG_STREAM_NAMED("Descartes",
+                         "Uniform sampling of frame, utilizing orientation increment: " << orient_increment <<
+                         ", and position increment: " << pos_increment <<
+                         " resulted in " << rtn.size() << " samples");
   return rtn;
 }
 
@@ -260,8 +261,8 @@ void CartTrajectoryPt::getCartesianPoses(const RobotModel &model, EigenSTL::vect
   }
   else
   {
-    ROS_DEBUG_STREAM("Get cartesian poses, sampled: " << all_poses.size() << ", with " << poses.size()
-                                                      << " valid(returned) poses");
+    ROS_DEBUG_STREAM_NAMED("Descartes", "Get cartesian poses, sampled: " << all_poses.size() << ", with " << poses.size()
+                                                                         << " valid(returned) poses");
   }
 }
 
@@ -397,8 +398,8 @@ void CartTrajectoryPt::getJointPoses(const RobotModel &model, std::vector<std::v
   }
   else
   {
-    ROS_DEBUG_STREAM("Get joint poses, sampled: " << poses.size() << ", with " << joint_poses.size()
-                                                  << " valid(returned) poses");
+    ROS_DEBUG_STREAM_NAMED("Descartes", "Get joint poses, sampled: " << poses.size() << ", with " << joint_poses.size()
+                                                                     << " valid(returned) poses");
   }
 }