Skip to content

Commit

Permalink
Profile Dictionary Update (#255)
Browse files Browse the repository at this point in the history
  • Loading branch information
marip8 authored and Levi-Armstrong committed Jan 13, 2023
1 parent 6c006bf commit fafbd46
Show file tree
Hide file tree
Showing 51 changed files with 476 additions and 645 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_command_language/poly/instruction_poly.h>
#include <tesseract_command_language/poly/move_instruction_poly.h>
#include <tesseract_command_language/constants.h>
#include <tesseract_command_language/profile_dictionary.h>
#include <tesseract_command_language/types.h>
#include <tesseract_common/manipulator_info.h>

namespace tesseract_planning
Expand Down Expand Up @@ -115,10 +115,10 @@ class CompositeInstruction
void print(const std::string& prefix = "") const;

void setProfile(const std::string& profile);
const std::string& getProfile() const;
const std::string& getProfile(const std::string& ns = "") const;

void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides);
ProfileDictionary::ConstPtr getProfileOverrides() const;
void setProfileOverrides(ProfileOverrides profile_overrides);
ProfileOverrides getProfileOverrides() const;

void setManipulatorInfo(tesseract_common::ManipulatorInfo info);
const tesseract_common::ManipulatorInfo& getManipulatorInfo() const;
Expand Down Expand Up @@ -384,7 +384,7 @@ class CompositeInstruction
std::string profile_{ DEFAULT_PROFILE_KEY };

/** @brief Dictionary of profiles that will override named profiles for a specific task*/
ProfileDictionary::ConstPtr profile_overrides_;
ProfileOverrides profile_overrides_;

/** @brief The order of the composite instruction */
CompositeInstructionOrder order_{ CompositeInstructionOrder::ORDERED };
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_command_language/poly/move_instruction_poly.h>
#include <tesseract_command_language/poly/waypoint_poly.h>
#include <tesseract_command_language/constants.h>
#include <tesseract_command_language/profile_dictionary.h>
#include <tesseract_command_language/types.h>
#include <tesseract_common/manipulator_info.h>

namespace tesseract_planning
Expand Down Expand Up @@ -127,16 +127,16 @@ class MoveInstruction
tesseract_common::ManipulatorInfo& getManipulatorInfo();

void setProfile(const std::string& profile);
const std::string& getProfile() const;
const std::string& getProfile(const std::string& ns = "") const;

void setPathProfile(const std::string& profile);
const std::string& getPathProfile() const;
const std::string& getPathProfile(const std::string& ns = "") const;

void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides);
ProfileDictionary::ConstPtr getProfileOverrides() const;
void setProfileOverrides(ProfileOverrides profile_overrides);
ProfileOverrides getProfileOverrides() const;

void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides);
ProfileDictionary::ConstPtr getPathProfileOverrides() const;
void setPathProfileOverrides(ProfileOverrides profile_overrides);
ProfileOverrides getPathProfileOverrides() const;

const std::string& getDescription() const;

Expand Down Expand Up @@ -171,10 +171,10 @@ class MoveInstruction
std::string path_profile_;

/** @brief Dictionary of profiles that will override named profiles for a specific task*/
ProfileDictionary::ConstPtr profile_overrides_;
ProfileOverrides profile_overrides_;

/** @brief Dictionary of path profiles that will override named profiles for a specific task*/
ProfileDictionary::ConstPtr path_profile_overrides_;
ProfileOverrides path_profile_overrides_;

/** @brief The assigned waypoint (Cartesian, Joint or State) */
WaypointPoly waypoint_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_command_language/poly/joint_waypoint_poly.h>
#include <tesseract_command_language/poly/state_waypoint_poly.h>
#include <tesseract_command_language/poly/waypoint_poly.h>
#include <tesseract_command_language/profile_dictionary.h>
#include <tesseract_command_language/types.h>
#include <tesseract_common/manipulator_info.h>
#include <tesseract_common/serialization.h>
#include <tesseract_common/type_erasure.h>
Expand Down Expand Up @@ -131,11 +131,11 @@ struct MoveInstructionConcept // NOLINT
const std::string& path_profile_const = c.getPathProfile();
UNUSED(path_profile_const);

c.setProfileOverrides(nullptr);
c.setProfileOverrides(ProfileOverrides{});
auto profile_overrides = c.getProfileOverrides();
UNUSED(profile_overrides);

c.setPathProfileOverrides(nullptr);
c.setPathProfileOverrides(ProfileOverrides{});
auto path_profile_overrides = c.getPathProfileOverrides();
UNUSED(path_profile_overrides);

Expand Down Expand Up @@ -183,16 +183,16 @@ struct MoveInstructionInterface : tesseract_common::TypeErasureInterface
virtual tesseract_common::ManipulatorInfo& getManipulatorInfo() = 0;

virtual void setProfile(const std::string& profile) = 0;
virtual const std::string& getProfile() const = 0;
virtual const std::string& getProfile(const std::string& ns = "") const = 0;

virtual void setPathProfile(const std::string& profile) = 0;
virtual const std::string& getPathProfile() const = 0;
virtual const std::string& getPathProfile(const std::string& ns = "") const = 0;

virtual void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) = 0;
virtual ProfileDictionary::ConstPtr getProfileOverrides() const = 0;
virtual void setProfileOverrides(ProfileOverrides profile_overrides) = 0;
virtual ProfileOverrides getProfileOverrides() const = 0;

virtual void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) = 0;
virtual ProfileDictionary::ConstPtr getPathProfileOverrides() const = 0;
virtual void setPathProfileOverrides(ProfileOverrides profile_overrides) = 0;
virtual ProfileOverrides getPathProfileOverrides() const = 0;

virtual void setMoveType(MoveInstructionType move_type) = 0;
virtual MoveInstructionType getMoveType() const = 0;
Expand Down Expand Up @@ -246,22 +246,22 @@ struct MoveInstructionInstance : tesseract_common::TypeErasureInstance<T, MoveIn
tesseract_common::ManipulatorInfo& getManipulatorInfo() final { return this->get().getManipulatorInfo(); }

void setProfile(const std::string& profile) final { this->get().setProfile(profile); }
const std::string& getProfile() const final { return this->get().getProfile(); }
const std::string& getProfile(const std::string& ns = "") const final { return this->get().getProfile(ns); }

void setPathProfile(const std::string& profile) final { this->get().setPathProfile(profile); }
const std::string& getPathProfile() const final { return this->get().getPathProfile(); }
const std::string& getPathProfile(const std::string& ns = "") const final { return this->get().getPathProfile(ns); }

void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) final
void setProfileOverrides(ProfileOverrides profile_overrides) final
{
this->get().setProfileOverrides(profile_overrides);
}
ProfileDictionary::ConstPtr getProfileOverrides() const final { return this->get().getProfileOverrides(); }
ProfileOverrides getProfileOverrides() const final { return this->get().getProfileOverrides(); }

void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) final
void setPathProfileOverrides(ProfileOverrides profile_overrides) final
{
this->get().setPathProfileOverrides(profile_overrides);
}
ProfileDictionary::ConstPtr getPathProfileOverrides() const final { return this->get().getPathProfileOverrides(); }
ProfileOverrides getPathProfileOverrides() const final { return this->get().getPathProfileOverrides(); }

void setMoveType(MoveInstructionType move_type) final { this->get().setMoveType(move_type); }
MoveInstructionType getMoveType() const final { return this->get().getMoveType(); }
Expand Down Expand Up @@ -311,16 +311,16 @@ struct MoveInstructionPoly : MoveInstructionPolyBase
tesseract_common::ManipulatorInfo& getManipulatorInfo();

void setProfile(const std::string& profile);
const std::string& getProfile() const;
const std::string& getProfile(const std::string& ns = "") const;

void setPathProfile(const std::string& profile);
const std::string& getPathProfile() const;
const std::string& getPathProfile(const std::string& ns = "") const;

void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides);
ProfileDictionary::ConstPtr getProfileOverrides() const;
void setProfileOverrides(ProfileOverrides profile_overrides);
ProfileOverrides getProfileOverrides() const;

void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides);
ProfileDictionary::ConstPtr getPathProfileOverrides() const;
void setPathProfileOverrides(ProfileOverrides profile_overrides);
ProfileOverrides getPathProfileOverrides() const;

void setMoveType(MoveInstructionType move_type);
MoveInstructionType getMoveType() const;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,19 +35,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <memory>
#include <mutex>
#include <shared_mutex>
#include <sstream>
#include <boost/core/demangle.hpp>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

namespace tesseract_planning
{
/**
* This used to store specific profile mapping with the request
*
* For example say you have a profile named Raster in your command language. Say you have multiple Raster profiles
* for descartes {Raster, Raster1, Raster2}. This allows you to remap the meaning of Raster in the command language to
* say Raster2 for the specific planner Descartes by Map<Descartes, Map<Raster, Raster1>>.
*/
using ProfileRemapping = std::unordered_map<std::string, std::unordered_map<std::string, std::string>>;

/**
* @brief This class is used to store profiles for motion planning and process planning
* @details This is a thread safe class
Expand Down Expand Up @@ -156,31 +149,6 @@ class ProfileDictionary
}
}

/**
* @brief Check if a profile exists
* @details If profile entry does not exist it also returns false
* @return True if profile exists, otherwise false
*/
template <typename ProfileType>
bool hasProfile(const std::string& ns, const std::string& profile_name) const
{
std::shared_lock lock(mutex_);
auto it = profiles_.find(ns);
if (it == profiles_.end())
return false;

auto it2 = it->second.find(std::type_index(typeid(ProfileType)));
if (it2 != it->second.end())
{
const auto& profile_map =
std::any_cast<const std::unordered_map<std::string, std::shared_ptr<const ProfileType>>&>(it2->second);
auto it3 = profile_map.find(profile_name);
if (it3 != profile_map.end())
return true;
}
return false;
}

/**
* @brief Get a profile by name
* @details Check if the profile exist before calling this function, if missing an exception is thrown
Expand All @@ -191,11 +159,50 @@ class ProfileDictionary
std::shared_ptr<const ProfileType> getProfile(const std::string& ns, const std::string& profile_name) const
{
std::shared_lock lock(mutex_);
const auto& it = profiles_.at(ns);
const auto& it2 = it.at(std::type_index(typeid(ProfileType)));

const std::unordered_map<std::type_index, std::any>* it = nullptr;
try
{
it = &(profiles_.at(ns));
}
catch (const std::exception&)
{
std::stringstream ss;
ss << "Failed to find entries for namespace '" << ns << "'. Available namespaces are: ";
for (auto it = profiles_.begin(); it != profiles_.end(); ++it)
{
ss << "'" << it->first << "', ";
}
std::throw_with_nested(std::runtime_error(ss.str()));
}

const std::any* it2 = nullptr;
try
{
it2 = &(it->at(std::type_index(typeid(ProfileType))));
}
catch (const std::exception&)
{
std::stringstream ss;
ss << "No entries for profile base class type '" << boost::core::demangle(typeid(ProfileType).name()) << "'";
std::throw_with_nested(std::runtime_error(ss.str()));
}

const auto& profile_map =
std::any_cast<const std::unordered_map<std::string, std::shared_ptr<const ProfileType>>&>(it2);
return profile_map.at(profile_name);
std::any_cast<const std::unordered_map<std::string, std::shared_ptr<const ProfileType>>&>(*it2);
try
{
return profile_map.at(profile_name);
}
catch (const std::exception&)
{
std::stringstream ss;
ss << "No entries for profile '" << profile_name << "' in namespace '" << ns << "'. Available profiles are: ";
for (auto it = profile_map.begin(); it != profile_map.end(); ++it)
ss << "'" << it->first << "', ";

std::throw_with_nested(std::runtime_error(ss.str()));
}
}

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,16 @@
namespace tesseract_planning
{
using ManipulatorInfo = tesseract_common::ManipulatorInfo;

/**
* @brief Map that associates for override profile names (values) with specified task namespaces (keys)
* @details For example, a profile "default" might have the following override profiles names for various specific task
* namespaces
* - ["ompl", "custom_profile_1"]
* - ["time_parameterization", "custom_profile_2"]
*/
using ProfileOverrides = std::unordered_map<std::string, std::string>;

} // namespace tesseract_planning

#endif // TESSERACT_COMMAND_LANGUAGE_TYPES_H
12 changes: 9 additions & 3 deletions tesseract_command_language/src/composite_instruction.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -72,13 +72,19 @@ void CompositeInstruction::setProfile(const std::string& profile)
{
profile_ = (profile.empty()) ? DEFAULT_PROFILE_KEY : profile;
}
const std::string& CompositeInstruction::getProfile() const { return profile_; }
const std::string& CompositeInstruction::getProfile(const std::string& ns) const
{
if (ns.empty() || (profile_overrides_.find(ns) == profile_overrides_.end()))
return profile_;

return profile_overrides_.at(ns);
}

void CompositeInstruction::setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides)
void CompositeInstruction::setProfileOverrides(ProfileOverrides profile_overrides)
{
profile_overrides_ = std::move(profile_overrides);
}
ProfileDictionary::ConstPtr CompositeInstruction::getProfileOverrides() const { return profile_overrides_; }
ProfileOverrides CompositeInstruction::getProfileOverrides() const { return profile_overrides_; }

void CompositeInstruction::setManipulatorInfo(tesseract_common::ManipulatorInfo info)
{
Expand Down
24 changes: 18 additions & 6 deletions tesseract_command_language/src/move_instruction.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -148,22 +148,34 @@ const tesseract_common::ManipulatorInfo& MoveInstruction::getManipulatorInfo() c
tesseract_common::ManipulatorInfo& MoveInstruction::getManipulatorInfo() { return manipulator_info_; }

void MoveInstruction::setProfile(const std::string& profile) { profile_ = profile; }
const std::string& MoveInstruction::getProfile() const { return profile_; }
const std::string& MoveInstruction::getProfile(const std::string& ns) const
{
if (ns.empty() || (profile_overrides_.find(ns) == profile_overrides_.end()))
return profile_;

return profile_overrides_.at(ns);
}

void MoveInstruction::setPathProfile(const std::string& profile) { path_profile_ = profile; }
const std::string& MoveInstruction::getPathProfile() const { return path_profile_; }
const std::string& MoveInstruction::getPathProfile(const std::string& ns) const
{
if (ns.empty() || (path_profile_overrides_.find(ns) == path_profile_overrides_.end()))
return path_profile_;

return path_profile_overrides_.at(ns);
}

void MoveInstruction::setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides)
void MoveInstruction::setProfileOverrides(ProfileOverrides profile_overrides)
{
profile_overrides_ = std::move(profile_overrides);
}
ProfileDictionary::ConstPtr MoveInstruction::getProfileOverrides() const { return profile_overrides_; }
ProfileOverrides MoveInstruction::getProfileOverrides() const { return profile_overrides_; }

void MoveInstruction::setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides)
void MoveInstruction::setPathProfileOverrides(ProfileOverrides profile_overrides)
{
path_profile_overrides_ = std::move(profile_overrides);
}
ProfileDictionary::ConstPtr MoveInstruction::getPathProfileOverrides() const { return path_profile_overrides_; }
ProfileOverrides MoveInstruction::getPathProfileOverrides() const { return path_profile_overrides_; }

const std::string& MoveInstruction::getDescription() const { return description_; }

Expand Down
Loading

0 comments on commit fafbd46

Please sign in to comment.