diff --git a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h index 7924eb280..8ac2116b7 100644 --- a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h +++ b/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 ID::Key last_scene_id_{ID::getNullKey()}; ///< ID of last 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 diff --git a/robowflex_ompl/src/ompl_interface.cpp b/robowflex_ompl/src/ompl_interface.cpp index f80d0f66e..9b716aec5 100644 --- a/robowflex_ompl/src/ompl_interface.cpp +++ b/robowflex_ompl/src/ompl_interface.cpp @@ -103,10 +103,15 @@ void OMPL::OMPLInterfacePlanner::refreshContext(const SceneConstPtr &scene, bool force) const { const auto &scene_id = scene->getKey(); - const auto &request_hash = IO::getMessageMD5(request); - 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_id == last_scene_id_; + 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) { @@ -130,7 +135,7 @@ void OMPL::OMPLInterfacePlanner::refreshContext(const SceneConstPtr &scene, ss_ = context_->getOMPLSimpleSetup(); last_scene_id_ = scene_id; - last_request_hash_ = request_hash; + previous_request_ = request; RBX_INFO("Refreshed Context!"); }