Skip to content

Commit

Permalink
Modify the concept of profile overrides in Composite and Move Instruc…
Browse files Browse the repository at this point in the history 
…tion
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Dec 8, 2024
1 parent eeb1b60 commit 8729b17
Show file tree
Hide file tree
Showing 50 changed files with 354 additions and 2,283 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 @@ -37,13 +37,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_command_language/poly/instruction_poly.h>
#include <tesseract_command_language/constants.h>
#include <tesseract_command_language/types.h>
#include <tesseract_common/manipulator_info.h>
#include <tesseract_common/any_poly.h>

namespace tesseract_planning
{
class CompositeInstruction;
class ProfileDictionary;
struct MoveInstructionPoly;

/**
Expand Down Expand Up @@ -134,10 +134,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(std::shared_ptr<const ProfileDictionary> profile_overrides);
std::shared_ptr<const ProfileDictionary> getProfileOverrides() const;
void setProfileOverrides(ProfileOverrides profile_overrides);
const ProfileOverrides& getProfileOverrides() const;

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

/** @brief Dictionary of profiles that will override named profiles for a specific task*/
std::shared_ptr<const ProfileDictionary> profile_overrides_;
ProfileOverrides profile_overrides_;

/** @brief The order of the composite instruction */
CompositeInstructionOrder order_{ CompositeInstructionOrder::ORDERED };
Expand Down
17 changes: 9 additions & 8 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,6 +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/types.h>
#include <tesseract_common/manipulator_info.h>

namespace tesseract_planning
Expand Down Expand Up @@ -139,16 +140,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(std::shared_ptr<const ProfileDictionary> profile_overrides);
std::shared_ptr<const ProfileDictionary> getProfileOverrides() const;
void setProfileOverrides(ProfileOverrides profile_overrides);
const ProfileOverrides& getProfileOverrides() const;

void setPathProfileOverrides(std::shared_ptr<const ProfileDictionary> profile_overrides);
std::shared_ptr<const ProfileDictionary> getPathProfileOverrides() const;
void setPathProfileOverrides(ProfileOverrides profile_overrides);
const ProfileOverrides& getPathProfileOverrides() const;

const std::string& getDescription() const;

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

/** @brief Dictionary of profiles that will override named profiles for a specific task*/
std::shared_ptr<const ProfileDictionary> profile_overrides_;
ProfileOverrides profile_overrides_;

/** @brief Dictionary of path profiles that will override named profiles for a specific task*/
std::shared_ptr<const ProfileDictionary> path_profile_overrides_;
ProfileOverrides path_profile_overrides_;

/** @brief The assigned waypoint (Cartesian, Joint or State) */
WaypointPoly waypoint_;
Expand Down
51 changes: 23 additions & 28 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,6 +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/types.h>
#include <tesseract_common/manipulator_info.h>
#include <tesseract_common/type_erasure.h>
#include <tesseract_common/fwd.h>
Expand Down Expand Up @@ -127,11 +128,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 @@ -180,16 +181,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(std::shared_ptr<const ProfileDictionary> profile_overrides) = 0;
virtual std::shared_ptr<const ProfileDictionary> getProfileOverrides() const = 0;
virtual void setProfileOverrides(ProfileOverrides profile_overrides) = 0;
virtual const ProfileOverrides& getProfileOverrides() const = 0;

virtual void setPathProfileOverrides(std::shared_ptr<const ProfileDictionary> profile_overrides) = 0;
virtual std::shared_ptr<const ProfileDictionary> getPathProfileOverrides() const = 0;
virtual void setPathProfileOverrides(ProfileOverrides profile_overrides) = 0;
virtual const ProfileOverrides& getPathProfileOverrides() const = 0;

virtual void setMoveType(MoveInstructionType move_type) = 0;
virtual MoveInstructionType getMoveType() const = 0;
Expand Down Expand Up @@ -244,28 +245,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(std::shared_ptr<const ProfileDictionary> profile_overrides) final
void setProfileOverrides(ProfileOverrides profile_overrides) final
{
this->get().setProfileOverrides(profile_overrides);
}
std::shared_ptr<const ProfileDictionary> getProfileOverrides() const final
{
return this->get().getProfileOverrides();
this->get().setProfileOverrides(std::move(profile_overrides));
}
const ProfileOverrides& getProfileOverrides() const final { return this->get().getProfileOverrides(); }

void setPathProfileOverrides(std::shared_ptr<const ProfileDictionary> profile_overrides) final
{
this->get().setPathProfileOverrides(profile_overrides);
}
std::shared_ptr<const ProfileDictionary> getPathProfileOverrides() const final
void setPathProfileOverrides(ProfileOverrides profile_overrides) final
{
return this->get().getPathProfileOverrides();
this->get().setPathProfileOverrides(std::move(profile_overrides));
}
const 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 @@ -321,16 +316,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(std::shared_ptr<const ProfileDictionary> profile_overrides);
std::shared_ptr<const ProfileDictionary> getProfileOverrides() const;
void setProfileOverrides(ProfileOverrides profile_overrides);
const ProfileOverrides& getProfileOverrides() const;

void setPathProfileOverrides(std::shared_ptr<const ProfileDictionary> profile_overrides);
std::shared_ptr<const ProfileDictionary> getPathProfileOverrides() const;
void setPathProfileOverrides(ProfileOverrides profile_overrides);
const ProfileOverrides& getPathProfileOverrides() const;

void setMoveType(MoveInstructionType move_type);
MoveInstructionType getMoveType() const;
Expand Down
12 changes: 3 additions & 9 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 @@ -38,15 +38,6 @@ 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 used by various tasks
* @details This is a thread safe class
Expand All @@ -65,6 +56,9 @@ class ProfileDictionary
* @param profile The profile to add
*/
void addProfile(const std::string& ns, const std::string& profile_name, const Profile::ConstPtr& profile);
void addProfile(const std::string& ns,
const std::vector<std::string>& profile_names,
const Profile::ConstPtr& profile);

/**
* @brief Check if a profile exists
Expand Down
75 changes: 45 additions & 30 deletions ...and_language/include/tesseract_command_language/test_suite/move_instruction_poly_unit.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,8 +103,8 @@ void runMoveInstructionConstructorTest()
EXPECT_EQ(instr.getMoveType(), MoveInstructionType::CIRCULAR);
EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY);
EXPECT_EQ(instr.getPathProfile(), DEFAULT_PROFILE_KEY);
EXPECT_TRUE(instr.getProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getProfileOverrides().empty());
EXPECT_TRUE(instr.getPathProfileOverrides().empty());
EXPECT_FALSE(instr.getDescription().empty());
EXPECT_FALSE(instr.getUUID().is_nil());
EXPECT_TRUE(instr.getParentUUID().is_nil());
Expand Down Expand Up @@ -133,8 +133,8 @@ void runMoveInstructionConstructorTest()
EXPECT_EQ(instr.getMoveType(), MoveInstructionType::CIRCULAR);
EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY);
EXPECT_EQ(instr.getPathProfile(), DEFAULT_PROFILE_KEY);
EXPECT_TRUE(instr.getProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getProfileOverrides().empty());
EXPECT_TRUE(instr.getPathProfileOverrides().empty());
EXPECT_FALSE(instr.getDescription().empty());
EXPECT_FALSE(instr.getUUID().is_nil());
EXPECT_TRUE(instr.getParentUUID().is_nil());
Expand Down Expand Up @@ -163,8 +163,8 @@ void runMoveInstructionConstructorTest()
EXPECT_EQ(instr.getMoveType(), MoveInstructionType::FREESPACE);
EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY);
EXPECT_TRUE(instr.getPathProfile().empty());
EXPECT_TRUE(instr.getProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getProfileOverrides().empty());
EXPECT_TRUE(instr.getPathProfileOverrides().empty());
EXPECT_FALSE(instr.getDescription().empty());
EXPECT_FALSE(instr.getUUID().is_nil());
EXPECT_TRUE(instr.getParentUUID().is_nil());
Expand Down Expand Up @@ -193,8 +193,8 @@ void runMoveInstructionConstructorTest()
EXPECT_EQ(instr.getMoveType(), MoveInstructionType::LINEAR);
EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY);
EXPECT_EQ(instr.getPathProfile(), DEFAULT_PROFILE_KEY);
EXPECT_TRUE(instr.getProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getProfileOverrides().empty());
EXPECT_TRUE(instr.getPathProfileOverrides().empty());
EXPECT_FALSE(instr.getDescription().empty());
EXPECT_FALSE(instr.getUUID().is_nil());
EXPECT_TRUE(instr.getParentUUID().is_nil());
Expand Down Expand Up @@ -224,8 +224,8 @@ void runMoveInstructionConstructorTest()
EXPECT_EQ(instr.getMoveType(), MoveInstructionType::CIRCULAR);
EXPECT_EQ(instr.getProfile(), "TEST_PROFILE");
EXPECT_EQ(instr.getPathProfile(), "TEST_PROFILE");
EXPECT_TRUE(instr.getProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getProfileOverrides().empty());
EXPECT_TRUE(instr.getPathProfileOverrides().empty());
EXPECT_FALSE(instr.getDescription().empty());
EXPECT_FALSE(instr.getUUID().is_nil());
EXPECT_TRUE(instr.getParentUUID().is_nil());
Expand Down Expand Up @@ -254,8 +254,8 @@ void runMoveInstructionConstructorTest()
EXPECT_EQ(instr.getMoveType(), MoveInstructionType::FREESPACE);
EXPECT_EQ(instr.getProfile(), "TEST_PROFILE");
EXPECT_TRUE(instr.getPathProfile().empty());
EXPECT_TRUE(instr.getProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getProfileOverrides().empty());
EXPECT_TRUE(instr.getPathProfileOverrides().empty());
EXPECT_FALSE(instr.getDescription().empty());
EXPECT_FALSE(instr.getUUID().is_nil());
EXPECT_TRUE(instr.getParentUUID().is_nil());
Expand Down Expand Up @@ -284,8 +284,8 @@ void runMoveInstructionConstructorTest()
EXPECT_EQ(instr.getMoveType(), MoveInstructionType::FREESPACE);
EXPECT_EQ(instr.getProfile(), "TEST_PROFILE");
EXPECT_TRUE(instr.getPathProfile().empty());
EXPECT_TRUE(instr.getProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getProfileOverrides().empty());
EXPECT_TRUE(instr.getPathProfileOverrides().empty());
EXPECT_FALSE(instr.getDescription().empty());
EXPECT_FALSE(instr.getUUID().is_nil());
EXPECT_TRUE(instr.getParentUUID().is_nil());
Expand Down Expand Up @@ -314,8 +314,8 @@ void runMoveInstructionConstructorTest()
EXPECT_EQ(instr.getMoveType(), MoveInstructionType::LINEAR);
EXPECT_EQ(instr.getProfile(), "TEST_PROFILE");
EXPECT_EQ(instr.getPathProfile(), "TEST_PROFILE");
EXPECT_TRUE(instr.getProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getProfileOverrides().empty());
EXPECT_TRUE(instr.getPathProfileOverrides().empty());
EXPECT_FALSE(instr.getDescription().empty());
EXPECT_FALSE(instr.getUUID().is_nil());
EXPECT_TRUE(instr.getParentUUID().is_nil());
Expand Down Expand Up @@ -345,8 +345,8 @@ void runMoveInstructionConstructorTest()
EXPECT_EQ(instr.getMoveType(), MoveInstructionType::CIRCULAR);
EXPECT_EQ(instr.getProfile(), "TEST_PROFILE");
EXPECT_EQ(instr.getPathProfile(), "TEST_PATH_PROFILE");
EXPECT_TRUE(instr.getProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getProfileOverrides().empty());
EXPECT_TRUE(instr.getPathProfileOverrides().empty());
EXPECT_FALSE(instr.getDescription().empty());
EXPECT_FALSE(instr.getUUID().is_nil());
EXPECT_TRUE(instr.getParentUUID().is_nil());
Expand Down Expand Up @@ -375,8 +375,8 @@ void runMoveInstructionConstructorTest()
EXPECT_EQ(instr.getMoveType(), MoveInstructionType::FREESPACE);
EXPECT_EQ(instr.getProfile(), "TEST_PROFILE");
EXPECT_EQ(instr.getPathProfile(), "TEST_PATH_PROFILE");
EXPECT_TRUE(instr.getProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getProfileOverrides().empty());
EXPECT_TRUE(instr.getPathProfileOverrides().empty());
EXPECT_FALSE(instr.getDescription().empty());
EXPECT_FALSE(instr.getUUID().is_nil());
EXPECT_TRUE(instr.getParentUUID().is_nil());
Expand Down Expand Up @@ -406,8 +406,8 @@ void runMoveInstructionConstructorTest()
EXPECT_EQ(instr.getMoveType(), MoveInstructionType::LINEAR);
EXPECT_EQ(instr.getProfile(), "TEST_PROFILE");
EXPECT_EQ(instr.getPathProfile(), "TEST_PATH_PROFILE");
EXPECT_TRUE(instr.getProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getProfileOverrides().empty());
EXPECT_TRUE(instr.getPathProfileOverrides().empty());
EXPECT_FALSE(instr.getDescription().empty());
EXPECT_FALSE(instr.getUUID().is_nil());
EXPECT_TRUE(instr.getParentUUID().is_nil());
Expand Down Expand Up @@ -436,8 +436,8 @@ void runMoveInstructionConstructorTest()
EXPECT_EQ(instr.getMoveType(), MoveInstructionType::LINEAR);
EXPECT_EQ(instr.getProfile(), "TEST_PROFILE");
EXPECT_EQ(instr.getPathProfile(), "TEST_PATH_PROFILE");
EXPECT_TRUE(instr.getProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr);
EXPECT_TRUE(instr.getProfileOverrides().empty());
EXPECT_TRUE(instr.getPathProfileOverrides().empty());
EXPECT_FALSE(instr.getDescription().empty());
EXPECT_FALSE(instr.getUUID().is_nil());
EXPECT_TRUE(instr.getParentUUID().is_nil());
Expand Down Expand Up @@ -496,13 +496,28 @@ void runMoveInstructionSettersTest()
instr.setPathProfile("TEST_PATH_PROFILE");
EXPECT_EQ(instr.getPathProfile(), "TEST_PATH_PROFILE");

auto profiles = std::make_shared<ProfileDictionary>();
instr.setProfileOverrides(profiles);
EXPECT_TRUE(instr.getProfileOverrides() == profiles);
// Create arbitrary profile overrides under arbitrary namespaces
const std::string ns1 = "ns1";
const std::string ns1_profile = "profile1";
const std::string ns2 = "ns2";
const std::string ns2_profile = "profile2";
{
ProfileOverrides overrides;
overrides[ns1] = ns1_profile;
overrides[ns2] = ns2_profile;
instr.setProfileOverrides(overrides);
instr.setPathProfileOverrides(overrides);
}

// Profile Overrides
EXPECT_EQ(instr.getProfile(ns1), ns1_profile);
EXPECT_EQ(instr.getProfile(ns2), ns2_profile);
EXPECT_EQ(instr.getProfile("nonexistent_ns"), "TEST_PROFILE");

auto path_profiles = std::make_shared<ProfileDictionary>();
instr.setPathProfileOverrides(path_profiles);
EXPECT_TRUE(instr.getPathProfileOverrides() == path_profiles);
// Path Profile Overrides
EXPECT_EQ(instr.getPathProfile(ns1), ns1_profile);
EXPECT_EQ(instr.getPathProfile(ns2), ns2_profile);
EXPECT_EQ(instr.getPathProfile("nonexistent_ns"), "TEST_PATH_PROFILE");

EXPECT_TRUE(instr.getManipulatorInfo().empty());
tesseract_common::ManipulatorInfo manip_info("manip", "base_link", "tool0");
Expand Down
Loading

0 comments on commit 8729b17

Please sign in to comment.