From a2710325f79db1a91c089bec2cd684b25b044494 Mon Sep 17 00:00:00 2001 From: Zak Kingston Date: Fri, 29 Oct 2021 14:22:29 -0500 Subject: [PATCH 1/3] Use message, not hash for comparison in OMPL planner --- .../include/robowflex_ompl/ompl_interface.h | 4 ++-- robowflex_ompl/src/ompl_interface.cpp | 11 +++++------ 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h index 7924eb280..aa6a5d7c5 100644 --- a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h +++ b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h @@ -121,8 +121,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 diff --git a/robowflex_ompl/src/ompl_interface.cpp b/robowflex_ompl/src/ompl_interface.cpp index f80d0f66e..98dff8db3 100644 --- a/robowflex_ompl/src/ompl_interface.cpp +++ b/robowflex_ompl/src/ompl_interface.cpp @@ -102,11 +102,10 @@ 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_; + bool same_scene = scene_msg == previous_scene_; + bool same_request = request == previous_request_; if (not force and ss_ and same_scene and same_request) { @@ -129,8 +128,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!"); } From 6d6fce73035db8d5b984d41242d0543dcd32177b Mon Sep 17 00:00:00 2001 From: Zak Kingston Date: Mon, 1 Nov 2021 13:44:30 -0500 Subject: [PATCH 2/3] Removed comparison on Kinetic and added documentation --- robowflex_ompl/include/robowflex_ompl/ompl_interface.h | 9 +++++++++ robowflex_ompl/src/ompl_interface.cpp | 6 ++++++ 2 files changed, 15 insertions(+) diff --git a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h index aa6a5d7c5..4cd3bfa8e 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 { diff --git a/robowflex_ompl/src/ompl_interface.cpp b/robowflex_ompl/src/ompl_interface.cpp index 98dff8db3..b7990535b 100644 --- a/robowflex_ompl/src/ompl_interface.cpp +++ b/robowflex_ompl/src/ompl_interface.cpp @@ -104,8 +104,14 @@ void OMPL::OMPLInterfacePlanner::refreshContext(const SceneConstPtr &scene, { moveit_msgs::PlanningScene scene_msg = scene->getMessage(); + // 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) { From 5d1e39f6f1c954d2fca6dc3f735fa22b8d0e47f0 Mon Sep 17 00:00:00 2001 From: Zachary Kingston Date: Tue, 2 Nov 2021 13:32:25 -0500 Subject: [PATCH 3/3] revert back to using Scene ID key instead of message comparison --- robowflex_ompl/include/robowflex_ompl/ompl_interface.h | 4 ++-- robowflex_ompl/src/ompl_interface.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h index 4cd3bfa8e..8ac2116b7 100644 --- a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h +++ b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h @@ -130,8 +130,8 @@ namespace robowflex bool hybridize_; ///< Whether or not planner should hybridize solutions. bool interpolate_; ///< Whether or not planner should interpolate solutions. - mutable moveit_msgs::PlanningScene previous_scene_; // Previous scene. - mutable moveit_msgs::MotionPlanRequest previous_request_; // Previous 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 b7990535b..9b716aec5 100644 --- a/robowflex_ompl/src/ompl_interface.cpp +++ b/robowflex_ompl/src/ompl_interface.cpp @@ -102,11 +102,11 @@ void OMPL::OMPLInterfacePlanner::refreshContext(const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request, bool force) const { - moveit_msgs::PlanningScene scene_msg = scene->getMessage(); + const auto &scene_id = scene->getKey(); // Comparison not implemented on Kinetic. User must force refresh manually. #if ROBOWFLEX_AT_LEAST_MELODIC - bool same_scene = scene_msg == previous_scene_; + bool same_scene = scene_id == last_scene_id_; bool same_request = request == previous_request_; #else bool same_scene = true; @@ -134,7 +134,7 @@ void OMPL::OMPLInterfacePlanner::refreshContext(const SceneConstPtr &scene, ss_ = context_->getOMPLSimpleSetup(); - previous_scene_ = scene_msg; + last_scene_id_ = scene_id; previous_request_ = request; RBX_INFO("Refreshed Context!");