Use message, not hash for comparison in OMPL planner

robowflex_ompl/include/robowflex_ompl/ompl_interface.h

@@ -30,6 +30,15 @@ namespace robowflex
             \brief A const shared pointer wrapper for robowflex::OMPL::OMPLInterfacePlanner. */
 
         /** \brief A planner that directly uses \a MoveIt!'s OMPL planning interface.
+         *
+         * The underlying planning context used is created through
+         * refreshContext(). Any function that uses the context will call this
+         * function internally, using the provided scene and request. If the scene
+         * and request are the same as the previous call, the previous simple
+         * setup will be used in these functions. This is not supported on
+         * Kinetic. As a result, this planner can only be used in a single thread.
+         * This is to provide * access to the underlying OMPL representation so
+         * that it may be * modified by users through getLastSimpleSetup().
          */
         class OMPLInterfacePlanner : public Planner
         {
@@ -121,8 +130,8 @@ namespace robowflex
             bool hybridize_;    ///< Whether or not planner should hybridize solutions.
             bool interpolate_;  ///< Whether or not planner should interpolate solutions.
 
-            mutable ID::Key last_scene_id_{ID::getNullKey()};  ///< ID of last scene.
-            mutable std::string last_request_hash_;            ///< Hash of last request.
+            mutable moveit_msgs::PlanningScene previous_scene_;        // Previous scene.
+            mutable moveit_msgs::MotionPlanRequest previous_request_;  // Previous request.
 
             mutable ompl_interface::ModelBasedPlanningContextPtr context_;  ///< Last context.
             mutable ompl::geometric::SimpleSetupPtr ss_;  ///< Last OMPL simple setup used for

robowflex_ompl/src/ompl_interface.cpp

@@ -102,11 +102,16 @@ void OMPL::OMPLInterfacePlanner::refreshContext(const SceneConstPtr &scene,
                                                 const planning_interface::MotionPlanRequest &request,
                                                 bool force) const
 {
-    const auto &scene_id = scene->getKey();
-    const auto &request_hash = IO::getMessageMD5(request);
+    moveit_msgs::PlanningScene scene_msg = scene->getMessage();
 
-    bool same_scene = compareIDs(scene_id, last_scene_id_);
-    bool same_request = request_hash == last_request_hash_;
+    // Comparison not implemented on Kinetic. User must force refresh manually.
+#if ROBOWFLEX_AT_LEAST_MELODIC
+    bool same_scene = scene_msg == previous_scene_;
+    bool same_request = request == previous_request_;
+#else
+    bool same_scene = true;
+    bool same_request = true;
+#endif
 
     if (not force and ss_ and same_scene and same_request)
     {
@@ -129,8 +134,8 @@ void OMPL::OMPLInterfacePlanner::refreshContext(const SceneConstPtr &scene,
 
     ss_ = context_->getOMPLSimpleSetup();
 
-    last_scene_id_ = scene_id;
-    last_request_hash_ = request_hash;
+    previous_scene_ = scene_msg;
+    previous_request_ = request;
 
     RBX_INFO("Refreshed Context!");
 }