Skip to content

Commit

Permalink
Revert "Changed name to namespace for motion planner tasks"
Browse files Browse the repository at this point in the history
This reverts commit 88ef0b2.
  • Loading branch information
marrts committed Jan 10, 2024
1 parent 7fdff47 commit ee1aac4
Show file tree
Hide file tree
Showing 14 changed files with 61 additions and 57 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -37,18 +37,18 @@ class MotionPlanner
using ConstPtr = std::shared_ptr<const MotionPlanner>;
/** @brief Construct a basic planner */
MotionPlanner() = default;
MotionPlanner(std::string ns);
MotionPlanner(std::string name);
virtual ~MotionPlanner() = default;
MotionPlanner(const MotionPlanner&) = delete;
MotionPlanner& operator=(const MotionPlanner&) = delete;
MotionPlanner(MotionPlanner&&) = delete;
MotionPlanner& operator=(MotionPlanner&&) = delete;

/**
* @brief Get the namespace of this planner
* @brief Get the name of this planner
* @details This is also used as the namespace for the profiles in the profile dictionary
*/
const std::string& getNamespace() const;
const std::string& getName() const;

/**
* @brief Solve the planner request problem
Expand Down Expand Up @@ -79,7 +79,7 @@ class MotionPlanner
bool format_result_as_input);

protected:
std::string ns_;
std::string name_;
};
} // namespace tesseract_planning
#endif // TESSERACT_PLANNING_PLANNER_H
8 changes: 4 additions & 4 deletions tesseract_motion_planners/core/src/planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,13 +28,13 @@

namespace tesseract_planning
{
MotionPlanner::MotionPlanner(std::string ns) : ns_(std::move(ns))
MotionPlanner::MotionPlanner(std::string name) : name_(std::move(name))
{
if (ns_.empty())
throw std::runtime_error("MotionPlanner namespace is empty!");
if (name_.empty())
throw std::runtime_error("MotionPlanner name is empty!");
}

const std::string& MotionPlanner::getNamespace() const { return ns_; }
const std::string& MotionPlanner::getName() const { return name_; }

bool MotionPlanner::checkRequest(const PlannerRequest& request)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ class DescartesMotionPlanner : public MotionPlanner
{
public:
/** @brief Construct a basic planner */
DescartesMotionPlanner(std::string ns);
DescartesMotionPlanner(std::string name);
~DescartesMotionPlanner() override = default;
DescartesMotionPlanner(const DescartesMotionPlanner&) = delete;
DescartesMotionPlanner& operator=(const DescartesMotionPlanner&) = delete;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid soluti
namespace tesseract_planning
{
template <typename FloatType>
DescartesMotionPlanner<FloatType>::DescartesMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) // NOLINT
DescartesMotionPlanner<FloatType>::DescartesMotionPlanner(std::string name) : MotionPlanner(std::move(name)) // NOLINT
{
}

Expand Down Expand Up @@ -213,7 +213,7 @@ void DescartesMotionPlanner<FloatType>::clear()
template <typename FloatType>
MotionPlanner::Ptr DescartesMotionPlanner<FloatType>::clone() const
{
return std::make_shared<DescartesMotionPlanner<FloatType>>(ns_);
return std::make_shared<DescartesMotionPlanner<FloatType>>(name_);
}

template <typename FloatType>
Expand Down Expand Up @@ -284,10 +284,10 @@ DescartesMotionPlanner<FloatType>::createProblem(const PlannerRequest& request)

// Get Plan Profile
std::string profile = plan_instruction.getProfile();
profile = getProfileString(ns_, profile, request.plan_profile_remapping);
profile = getProfileString(name_, profile, request.plan_profile_remapping);
auto cur_plan_profile = getProfile<DescartesPlanProfile<FloatType>>(
ns_, profile, *request.profiles, std::make_shared<DescartesDefaultPlanProfile<FloatType>>());
// cur_plan_profile = applyProfileOverrides(ns_, profile, cur_plan_profile,
name_, profile, *request.profiles, std::make_shared<DescartesDefaultPlanProfile<FloatType>>());
// cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile,
// plan_instruction.profile_overrides);
if (!cur_plan_profile)
throw std::runtime_error("DescartesMotionPlanner: Invalid profile");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ class OMPLMotionPlanner : public MotionPlanner
{
public:
/** @brief Construct a planner */
OMPLMotionPlanner(std::string ns);
OMPLMotionPlanner(std::string name);

/**
* @brief Sets up the OMPL problem then solves. It is intended to simplify setting up
Expand Down
10 changes: 5 additions & 5 deletions tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def,
}

/** @brief Construct a basic planner */
OMPLMotionPlanner::OMPLMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) {}
OMPLMotionPlanner::OMPLMotionPlanner(std::string name) : MotionPlanner(std::move(name)) {}

bool OMPLMotionPlanner::terminate()
{
Expand Down Expand Up @@ -307,7 +307,7 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const

void OMPLMotionPlanner::clear() { parallel_plan_ = nullptr; }

MotionPlanner::Ptr OMPLMotionPlanner::clone() const { return std::make_shared<OMPLMotionPlanner>(ns_); }
MotionPlanner::Ptr OMPLMotionPlanner::clone() const { return std::make_shared<OMPLMotionPlanner>(name_); }

OMPLProblemConfig OMPLMotionPlanner::createSubProblem(const PlannerRequest& request,
const tesseract_common::ManipulatorInfo& composite_mi,
Expand All @@ -322,10 +322,10 @@ OMPLProblemConfig OMPLMotionPlanner::createSubProblem(const PlannerRequest& requ

// Get Plan Profile
std::string profile = end_instruction.getProfile();
profile = getProfileString(ns_, profile, request.plan_profile_remapping);
profile = getProfileString(name_, profile, request.plan_profile_remapping);
auto cur_plan_profile =
getProfile<OMPLPlanProfile>(ns_, profile, *request.profiles, std::make_shared<OMPLDefaultPlanProfile>());
cur_plan_profile = applyProfileOverrides(ns_, profile, cur_plan_profile, end_instruction.getProfileOverrides());
getProfile<OMPLPlanProfile>(name_, profile, *request.profiles, std::make_shared<OMPLDefaultPlanProfile>());
cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, end_instruction.getProfileOverrides());
if (!cur_plan_profile)
throw std::runtime_error("OMPLMotionPlanner: Invalid profile");

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ class SimpleMotionPlanner : public MotionPlanner
using ConstPtr = std::shared_ptr<const SimpleMotionPlanner>;

/** @brief Construct a basic planner */
SimpleMotionPlanner(std::string ns);
SimpleMotionPlanner(std::string name);
~SimpleMotionPlanner() override = default;
SimpleMotionPlanner(const SimpleMotionPlanner&) = delete;
SimpleMotionPlanner& operator=(const SimpleMotionPlanner&) = delete;
Expand Down
4 changes: 2 additions & 2 deletions tesseract_motion_planners/simple/src/interpolation.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1302,11 +1302,11 @@ CompositeInstruction generateInterpolatedProgram(const CompositeInstruction& ins

// Create profile dictionary
auto profiles = std::make_shared<ProfileDictionary>();
profiles->addProfile<SimplePlannerPlanProfile>(planner.getNamespace(), instructions.getProfile(), profile);
profiles->addProfile<SimplePlannerPlanProfile>(planner.getName(), instructions.getProfile(), profile);
auto flat = instructions.flatten(&moveFilter);
for (const auto& i : flat)
profiles->addProfile<SimplePlannerPlanProfile>(
planner.getNamespace(), i.get().as<MoveInstructionPoly>().getProfile(), profile);
planner.getName(), i.get().as<MoveInstructionPoly>().getProfile(), profile);

// Assign profile dictionary
request.profiles = profiles;
Expand Down
17 changes: 9 additions & 8 deletions tesseract_motion_planners/simple/src/simple_motion_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ constexpr auto FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" };

namespace tesseract_planning
{
SimpleMotionPlanner::SimpleMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) {}
SimpleMotionPlanner::SimpleMotionPlanner(std::string name) : MotionPlanner(std::move(name)) {}

bool SimpleMotionPlanner::terminate()
{
Expand All @@ -56,7 +56,7 @@ bool SimpleMotionPlanner::terminate()

void SimpleMotionPlanner::clear() {}

MotionPlanner::Ptr SimpleMotionPlanner::clone() const { return std::make_shared<SimpleMotionPlanner>(ns_); }
MotionPlanner::Ptr SimpleMotionPlanner::clone() const { return std::make_shared<SimpleMotionPlanner>(name_); }

PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const
{
Expand Down Expand Up @@ -196,17 +196,18 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp
SimplePlannerPlanProfile::ConstPtr plan_profile;
if (base_instruction.getPathProfile().empty())
{
std::string profile = getProfileString(ns_, base_instruction.getProfile(), request.plan_profile_remapping);
std::string profile = getProfileString(name_, base_instruction.getProfile(), request.plan_profile_remapping);
plan_profile = getProfile<SimplePlannerPlanProfile>(
ns_, profile, *request.profiles, std::make_shared<SimplePlannerLVSNoIKPlanProfile>());
plan_profile = applyProfileOverrides(ns_, profile, plan_profile, base_instruction.getProfileOverrides());
name_, profile, *request.profiles, std::make_shared<SimplePlannerLVSNoIKPlanProfile>());
plan_profile = applyProfileOverrides(name_, profile, plan_profile, base_instruction.getProfileOverrides());
}
else
{
std::string profile = getProfileString(ns_, base_instruction.getPathProfile(), request.plan_profile_remapping);
std::string profile =
getProfileString(name_, base_instruction.getPathProfile(), request.plan_profile_remapping);
plan_profile = getProfile<SimplePlannerPlanProfile>(
ns_, profile, *request.profiles, std::make_shared<SimplePlannerLVSNoIKPlanProfile>());
plan_profile = applyProfileOverrides(ns_, profile, plan_profile, base_instruction.getProfileOverrides());
name_, profile, *request.profiles, std::make_shared<SimplePlannerLVSNoIKPlanProfile>());
plan_profile = applyProfileOverrides(name_, profile, plan_profile, base_instruction.getProfileOverrides());
}

if (!plan_profile)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ class TrajOptMotionPlanner : public MotionPlanner
{
public:
/** @brief Construct a basic planner */
TrajOptMotionPlanner(std::string ns);
TrajOptMotionPlanner(std::string name);

~TrajOptMotionPlanner() override = default;
TrajOptMotionPlanner(const TrajOptMotionPlanner&) = delete;
Expand Down
24 changes: 12 additions & 12 deletions tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ using namespace trajopt;

namespace tesseract_planning
{
TrajOptMotionPlanner::TrajOptMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) {}
TrajOptMotionPlanner::TrajOptMotionPlanner(std::string name) : MotionPlanner(std::move(name)) {}

bool TrajOptMotionPlanner::terminate()
{
Expand All @@ -62,7 +62,7 @@ bool TrajOptMotionPlanner::terminate()

void TrajOptMotionPlanner::clear() {}

MotionPlanner::Ptr TrajOptMotionPlanner::clone() const { return std::make_shared<TrajOptMotionPlanner>(ns_); }
MotionPlanner::Ptr TrajOptMotionPlanner::clone() const { return std::make_shared<TrajOptMotionPlanner>(name_); }

PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const
{
Expand Down Expand Up @@ -212,10 +212,10 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const
// Apply Solver parameters
std::string profile = request.instructions.getProfile();
ProfileDictionary::ConstPtr profile_overrides = request.instructions.getProfileOverrides();
profile = getProfileString(ns_, profile, request.plan_profile_remapping);
profile = getProfileString(name_, profile, request.plan_profile_remapping);
TrajOptSolverProfile::ConstPtr solver_profile = getProfile<TrajOptSolverProfile>(
ns_, profile, *request.profiles, std::make_shared<TrajOptDefaultSolverProfile>());
solver_profile = applyProfileOverrides(ns_, profile, solver_profile, profile_overrides);
name_, profile, *request.profiles, std::make_shared<TrajOptDefaultSolverProfile>());
solver_profile = applyProfileOverrides(name_, profile, solver_profile, profile_overrides);
if (!solver_profile)
throw std::runtime_error("TrajOptMotionPlanner: Invalid profile");

Expand Down Expand Up @@ -250,10 +250,10 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const
throw std::runtime_error("TrajOpt, working_frame is empty!");

// Get Plan Profile
std::string profile = getProfileString(ns_, move_instruction.getProfile(), request.plan_profile_remapping);
TrajOptPlanProfile::ConstPtr cur_plan_profile =
getProfile<TrajOptPlanProfile>(ns_, profile, *request.profiles, std::make_shared<TrajOptDefaultPlanProfile>());
cur_plan_profile = applyProfileOverrides(ns_, profile, cur_plan_profile, move_instruction.getProfileOverrides());
std::string profile = getProfileString(name_, move_instruction.getProfile(), request.plan_profile_remapping);
TrajOptPlanProfile::ConstPtr cur_plan_profile = getProfile<TrajOptPlanProfile>(
name_, profile, *request.profiles, std::make_shared<TrajOptDefaultPlanProfile>());
cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, move_instruction.getProfileOverrides());
if (!cur_plan_profile)
throw std::runtime_error("TrajOptMotionPlanner: Invalid profile");

Expand Down Expand Up @@ -333,11 +333,11 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const
for (long i = 0; i < pci->basic_info.n_steps; ++i)
pci->init_info.data.row(i) = seed_states[static_cast<std::size_t>(i)];

profile = getProfileString(ns_, request.instructions.getProfile(), request.composite_profile_remapping);
profile = getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping);
TrajOptCompositeProfile::ConstPtr cur_composite_profile = getProfile<TrajOptCompositeProfile>(
ns_, profile, *request.profiles, std::make_shared<TrajOptDefaultCompositeProfile>());
name_, profile, *request.profiles, std::make_shared<TrajOptDefaultCompositeProfile>());
cur_composite_profile =
applyProfileOverrides(ns_, profile, cur_composite_profile, request.instructions.getProfileOverrides());
applyProfileOverrides(name_, profile, cur_composite_profile, request.instructions.getProfileOverrides());
if (!cur_composite_profile)
throw std::runtime_error("TrajOptMotionPlanner: Invalid profile");

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ class TrajOptIfoptMotionPlanner : public MotionPlanner
{
public:
/** @brief Construct a basic planner */
TrajOptIfoptMotionPlanner(std::string ns);
TrajOptIfoptMotionPlanner(std::string name);

~TrajOptIfoptMotionPlanner() override = default;
TrajOptIfoptMotionPlanner(const TrajOptIfoptMotionPlanner&) = delete;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -50,15 +50,18 @@ using namespace trajopt_ifopt;

namespace tesseract_planning
{
TrajOptIfoptMotionPlanner::TrajOptIfoptMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) {}
TrajOptIfoptMotionPlanner::TrajOptIfoptMotionPlanner(std::string name) : MotionPlanner(std::move(name)) {}

bool TrajOptIfoptMotionPlanner::terminate()
{
CONSOLE_BRIDGE_logWarn("Termination of ongoing optimization is not implemented yet");
return false;
}

MotionPlanner::Ptr TrajOptIfoptMotionPlanner::clone() const { return std::make_shared<TrajOptIfoptMotionPlanner>(ns_); }
MotionPlanner::Ptr TrajOptIfoptMotionPlanner::clone() const
{
return std::make_shared<TrajOptIfoptMotionPlanner>(name_);
}

PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) const
{
Expand Down Expand Up @@ -215,10 +218,10 @@ std::shared_ptr<TrajOptIfoptProblem> TrajOptIfoptMotionPlanner::createProblem(co
// Apply Solver parameters
std::string profile = request.instructions.getProfile();
ProfileDictionary::ConstPtr profile_overrides = request.instructions.getProfileOverrides();
profile = getProfileString(ns_, profile, request.plan_profile_remapping);
profile = getProfileString(name_, profile, request.plan_profile_remapping);
TrajOptIfoptSolverProfile::ConstPtr solver_profile = getProfile<TrajOptIfoptSolverProfile>(
ns_, profile, *request.profiles, std::make_shared<TrajOptIfoptDefaultSolverProfile>());
solver_profile = applyProfileOverrides(ns_, profile, solver_profile, profile_overrides);
name_, profile, *request.profiles, std::make_shared<TrajOptIfoptDefaultSolverProfile>());
solver_profile = applyProfileOverrides(name_, profile, solver_profile, profile_overrides);
if (!solver_profile)
throw std::runtime_error("TrajOptIfoptMotionPlanner: Invalid profile");

Expand Down Expand Up @@ -253,10 +256,10 @@ std::shared_ptr<TrajOptIfoptProblem> TrajOptIfoptMotionPlanner::createProblem(co
throw std::runtime_error("TrajOpt, working_frame is empty!");

// Get Plan Profile
std::string profile = getProfileString(ns_, move_instruction.getProfile(), request.plan_profile_remapping);
std::string profile = getProfileString(name_, move_instruction.getProfile(), request.plan_profile_remapping);
TrajOptIfoptPlanProfile::ConstPtr cur_plan_profile = getProfile<TrajOptIfoptPlanProfile>(
ns_, profile, *request.profiles, std::make_shared<TrajOptIfoptDefaultPlanProfile>());
cur_plan_profile = applyProfileOverrides(ns_, profile, cur_plan_profile, move_instruction.getProfileOverrides());
name_, profile, *request.profiles, std::make_shared<TrajOptIfoptDefaultPlanProfile>());
cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, move_instruction.getProfileOverrides());
if (!cur_plan_profile)
throw std::runtime_error("TrajOptMotionPlanner: Invalid profile");

Expand Down Expand Up @@ -341,11 +344,11 @@ std::shared_ptr<TrajOptIfoptProblem> TrajOptIfoptMotionPlanner::createProblem(co
// ----------------
// Translate TCL for CompositeInstructions
// ----------------
profile = getProfileString(ns_, request.instructions.getProfile(), request.composite_profile_remapping);
profile = getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping);
TrajOptIfoptCompositeProfile::ConstPtr cur_composite_profile = getProfile<TrajOptIfoptCompositeProfile>(
ns_, profile, *request.profiles, std::make_shared<TrajOptIfoptDefaultCompositeProfile>());
name_, profile, *request.profiles, std::make_shared<TrajOptIfoptDefaultCompositeProfile>());
cur_composite_profile =
applyProfileOverrides(ns_, profile, cur_composite_profile, request.instructions.getProfileOverrides());
applyProfileOverrides(name_, profile, cur_composite_profile, request.instructions.getProfileOverrides());
if (!cur_composite_profile)
throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: Invalid profile");

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -173,7 +173,7 @@ class MotionPlannerTask : public TaskComposerTask
}

CONSOLE_BRIDGE_logInform("%s motion planning failed (%s) for process input: %s",
planner_->getNamespace().c_str(),
planner_->getName().c_str(),
response.message.c_str(),
instructions.getDescription().c_str());

Expand Down

0 comments on commit ee1aac4

Please sign in to comment.