Melodic collision minimum

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

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

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>

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

descartes_moveit/package.xml

@@ -7,7 +7,7 @@
   <maintainer email="[email protected]">Jorge Nicho</maintainer>
   <maintainer email="[email protected]">Jonathan Meyer</maintainer>
   <maintainer email="[email protected]">Shaun Edwards</maintainer>
-  
+
   <license>Apache2</license>
 
   <author >Shaun Edwards</author>

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))
   {