Skip to content

Commit

Permalink
Profile Dictionary Update (#255)
Browse files Browse the repository at this point in the history
  • Loading branch information
@marip8
marip8 authored Nov 4, 2022
1 parent 46df97e commit 2ac8431
Show file tree
Hide file tree
Showing 51 changed files with 477 additions and 646 deletions.
10 changes: 5 additions & 5 deletions tesseract_command_language/include/tesseract_command_language/composite_instruction.h
View file
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 @@ -83,10 +83,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 @@ -349,7 +349,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
18 changes: 9 additions & 9 deletions tesseract_command_language/include/tesseract_command_language/move_instruction.h
View file
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
42 changes: 21 additions & 21 deletions tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h
View file
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 @@ -132,11 +132,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 @@ -184,16 +184,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 @@ -247,22 +247,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 @@ -312,16 +312,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
83 changes: 45 additions & 38 deletions tesseract_command_language/include/tesseract_command_language/profile_dictionary.h
View file
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
10 changes: 10 additions & 0 deletions tesseract_command_language/include/tesseract_command_language/types.h
View file
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
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,13 +69,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
View file
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 2ac8431

Please sign in to comment.