diff --git a/tesseract_environment/include/tesseract_environment/environment.h b/tesseract_environment/include/tesseract_environment/environment.h index 7cdd0ad36af..3d5539080b7 100644 --- a/tesseract_environment/include/tesseract_environment/environment.h +++ b/tesseract_environment/include/tesseract_environment/environment.h @@ -192,7 +192,7 @@ class Environment * @param group_name The group name * @return A joint group */ - std::unique_ptr getJointGroup(const std::string& group_name) const; + std::shared_ptr getJointGroup(const std::string& group_name) const; /** * @brief Get a joint group given a vector of joint names @@ -200,8 +200,8 @@ class Environment * @param joint_names The joint names that make up the group * @return A joint group */ - std::unique_ptr getJointGroup(const std::string& name, - const std::vector& joint_names) const; + std::shared_ptr + getJointGroup(const std::string& name, const std::vector& joint_names) const; /** * @brief Get a kinematic group given group name and solver name @@ -210,8 +210,8 @@ class Environment * @param ik_solver_name The IK solver name * @return A kinematics group */ - std::unique_ptr getKinematicGroup(const std::string& group_name, - const std::string& ik_solver_name = "") const; + std::shared_ptr + getKinematicGroup(const std::string& group_name, const std::string& ik_solver_name = "") const; /** * @brief Find tool center point provided in the manipulator info diff --git a/tesseract_environment/src/environment.cpp b/tesseract_environment/src/environment.cpp index bbf055b124e..ec2522baa0f 100644 --- a/tesseract_environment/src/environment.cpp +++ b/tesseract_environment/src/environment.cpp @@ -248,7 +248,7 @@ struct Environment::Implementation * @details This will cleared when environment changes * @note This is intentionally not serialized it will auto updated */ - mutable std::unordered_map> joint_group_cache{}; + mutable std::unordered_map> joint_group_cache{}; mutable std::shared_mutex joint_group_cache_mutex; /** @@ -256,7 +256,7 @@ struct Environment::Implementation * @details This will cleared when environment changes * @note This is intentionally not serialized it will auto updated */ - mutable std::map, std::unique_ptr> + mutable std::map, std::shared_ptr> kinematic_group_cache{}; mutable std::shared_mutex kinematic_group_cache_mutex; @@ -295,13 +295,13 @@ struct Environment::Implementation std::vector getGroupJointNames(const std::string& group_name) const; - std::unique_ptr getJointGroup(const std::string& group_name) const; + std::shared_ptr getJointGroup(const std::string& group_name) const; - std::unique_ptr getJointGroup(const std::string& name, - const std::vector& joint_names) const; + std::shared_ptr + getJointGroup(const std::string& name, const std::vector& joint_names) const; - std::unique_ptr getKinematicGroup(const std::string& group_name, - std::string ik_solver_name) const; + std::shared_ptr getKinematicGroup(const std::string& group_name, + std::string ik_solver_name) const; Eigen::Isometry3d findTCPOffset(const tesseract_common::ManipulatorInfo& manip_info) const; @@ -478,13 +478,8 @@ std::unique_ptr Environment::Implementation::clone( cloned_env->collision_margin_data = collision_margin_data; // Copy cache - cloned_env->joint_group_cache.reserve(joint_group_cache.size()); - for (const auto& c : joint_group_cache) - cloned_env->joint_group_cache[c.first] = (std::make_unique(*c.second)); - - for (const auto& c : kinematic_group_cache) - cloned_env->kinematic_group_cache[c.first] = (std::make_unique(*c.second)); - + cloned_env->joint_group_cache = joint_group_cache; + cloned_env->kinematic_group_cache = kinematic_group_cache; cloned_env->group_joint_names_cache = group_joint_names_cache; // NOLINTNEXTLINE @@ -781,7 +776,7 @@ std::vector Environment::Implementation::getGroupJointNames(const s throw std::runtime_error("Environment, failed to get group '" + group_name + "' joint names!"); } -std::unique_ptr +std::shared_ptr Environment::Implementation::getJointGroup(const std::string& group_name) const { std::unique_lock cache_lock(joint_group_cache_mutex); @@ -789,25 +784,25 @@ Environment::Implementation::getJointGroup(const std::string& group_name) const if (it != joint_group_cache.end()) { CONSOLE_BRIDGE_logDebug("Environment, getJointGroup(%s) cache hit!", group_name.c_str()); - return std::make_unique(*it->second); + return it->second; } CONSOLE_BRIDGE_logDebug("Environment, getJointGroup(%s) cache miss!", group_name.c_str()); // Store copy in cache and return std::vector joint_names = getGroupJointNames(group_name); - tesseract_kinematics::JointGroup::UPtr jg = getJointGroup(group_name, joint_names); - joint_group_cache[group_name] = std::make_unique(*jg); + tesseract_kinematics::JointGroup::ConstPtr jg = getJointGroup(group_name, joint_names); + joint_group_cache[group_name] = jg; return jg; } -std::unique_ptr +std::shared_ptr Environment::Implementation::getJointGroup(const std::string& name, const std::vector& joint_names) const { - return std::make_unique(name, joint_names, *scene_graph, current_state); + return std::make_shared(name, joint_names, *scene_graph, current_state); } -std::unique_ptr +std::shared_ptr Environment::Implementation::getKinematicGroup(const std::string& group_name, std::string ik_solver_name) const { std::unique_lock cache_lock(kinematic_group_cache_mutex); @@ -817,7 +812,7 @@ Environment::Implementation::getKinematicGroup(const std::string& group_name, st { CONSOLE_BRIDGE_logDebug( "Environment, getKinematicGroup(%s, %s) cache hit!", group_name.c_str(), ik_solver_name.c_str()); - return std::make_unique(*it->second); + return it->second; } CONSOLE_BRIDGE_logDebug( @@ -835,10 +830,10 @@ Environment::Implementation::getKinematicGroup(const std::string& group_name, st return nullptr; // Store copy in cache and return - auto kg = std::make_unique( + auto kg = std::make_shared( group_name, joint_names, std::move(inv_kin), *scene_graph, current_state); - kinematic_group_cache[key] = std::make_unique(*kg); + kinematic_group_cache[key] = kg; #if !defined(NDEBUG) && TESSERACT_ENABLE_TESTING if (!tesseract_kinematics::checkKinematics(*kg)) @@ -2318,20 +2313,20 @@ std::vector Environment::getGroupJointNames(const std::string& grou return std::as_const(*impl_).getGroupJointNames(group_name); } -std::unique_ptr Environment::getJointGroup(const std::string& group_name) const +std::shared_ptr Environment::getJointGroup(const std::string& group_name) const { std::shared_lock lock(mutex_); return std::as_const(*impl_).getJointGroup(group_name); } -std::unique_ptr +std::shared_ptr Environment::getJointGroup(const std::string& name, const std::vector& joint_names) const { std::shared_lock lock(mutex_); return std::as_const(*impl_).getJointGroup(name, joint_names); } -std::unique_ptr +std::shared_ptr Environment::getKinematicGroup(const std::string& group_name, const std::string& ik_solver_name) const { std::shared_lock lock(mutex_);