Skip to content

Commit b5c5d25

Browse files
marrtsmarrts
authored
Feat/more verbose planning failures (#440)
1 parent c791e74 commit b5c5d25

File tree

9 files changed

+64
-37
lines changed

9 files changed

+64
-37
lines changed

‎tesseract_motion_planners/core/include/tesseract_motion_planners/core/planner.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -72,6 +72,9 @@ class MotionPlanner
7272
/** @brief Check planning request */
7373
static bool checkRequest(const PlannerRequest& request);
7474

75+
/** @brief Check planning request and give reason for failure */
76+
static bool checkRequest(const PlannerRequest& request, std::string& reason);
77+
7578
/** @brief Assign a solution to the move instruction */
7679
static void assignSolution(MoveInstructionPoly& mi,
7780
const std::vector<std::string>& joint_names,

‎tesseract_motion_planners/core/src/planner.cpp‎

Lines changed: 10 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -37,17 +37,25 @@ MotionPlanner::MotionPlanner(std::string name) : name_(std::move(name))
3737
const std::string& MotionPlanner::getName() const { return name_; }
3838

3939
bool MotionPlanner::checkRequest(const PlannerRequest& request)
40+
{
41+
std::string reason;
42+
return checkRequest(request, reason);
43+
}
44+
45+
bool MotionPlanner::checkRequest(const PlannerRequest& request, std::string& reason)
4046
{
4147
// Check that parameters are valid
4248
if (request.env == nullptr)
4349
{
44-
CONSOLE_BRIDGE_logError("In TrajOptPlannerUniversalConfig: tesseract is a required parameter and has not been set");
50+
reason = "PlannerRequest environment is nullptr";
51+
CONSOLE_BRIDGE_logError(reason.c_str());
4552
return false;
4653
}
4754

4855
if (request.instructions.empty())
4956
{
50-
CONSOLE_BRIDGE_logError("TrajOptPlannerUniversalConfig requires at least one instruction");
57+
reason = "PlannerRequest instruction is empty";
58+
CONSOLE_BRIDGE_logError(reason.c_str());
5159
return false;
5260
}
5361

‎tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
4848
#include <tesseract_command_language/utils.h>
4949

5050
constexpr auto SOLUTION_FOUND{ "Found valid solution" };
51-
constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input" };
51+
constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " };
5252
constexpr auto ERROR_FAILED_TO_BUILD_GRAPH{ "Failed to build graph" };
5353
constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" };
5454

‎tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp‎

Lines changed: 15 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -46,8 +46,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
4646
#include <tesseract_command_language/utils.h>
4747

4848
constexpr auto SOLUTION_FOUND{ "Found valid solution" };
49-
constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input" };
50-
constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" };
49+
constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " };
50+
constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution: " };
5151

5252
namespace tesseract_planning
5353
{
@@ -100,10 +100,11 @@ bool OMPLMotionPlanner::terminate()
100100
PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
101101
{
102102
PlannerResponse response;
103-
if (!checkRequest(request)) // NOLINT
103+
std::string reason;
104+
if (!checkRequest(request, reason))
104105
{
105106
response.successful = false;
106-
response.message = ERROR_INVALID_INPUT;
107+
response.message = std::string(ERROR_INVALID_INPUT) + reason;
107108
return response;
108109
}
109110
std::vector<OMPLProblemConfig> problems;
@@ -121,7 +122,7 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
121122
{
122123
CONSOLE_BRIDGE_logError("OMPLPlanner failed to generate problem: %s.", e.what());
123124
response.successful = false;
124-
response.message = ERROR_INVALID_INPUT;
125+
response.message = std::string(ERROR_INVALID_INPUT) + e.what();
125126
return response;
126127
}
127128

@@ -171,7 +172,8 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
171172

172173
if (!pdef->hasOptimizationObjective())
173174
{
174-
CONSOLE_BRIDGE_logDebug("Terminating early since there is no optimization objective specified");
175+
reason = "Terminating early since there is no optimization objective specified";
176+
CONSOLE_BRIDGE_logDebug(reason.c_str());
175177
break;
176178
}
177179

@@ -180,23 +182,27 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
180182

181183
if (pdef->getOptimizationObjective()->isSatisfied(obj_cost))
182184
{
183-
CONSOLE_BRIDGE_logDebug("Terminating early since solution path satisfies the optimization objective");
185+
reason = "Terminating early since solution path satisfies the optimization objective";
186+
CONSOLE_BRIDGE_logDebug(reason.c_str());
184187
break;
185188
}
186189

187190
if (pdef->getSolutionCount() >= static_cast<std::size_t>(p->max_solutions))
188191
{
189-
CONSOLE_BRIDGE_logDebug("Terminating early since %u solutions were generated", p->max_solutions);
192+
reason = "Terminating early since " + std::to_string(p->max_solutions) + " solutions were generated";
193+
CONSOLE_BRIDGE_logDebug(reason.c_str());
190194
break;
191195
}
192196
}
193197
}
198+
if (ompl::time::now() >= end)
199+
reason = "Exceeded allowed time";
194200
}
195201

196202
if (status != ompl::base::PlannerStatus::EXACT_SOLUTION)
197203
{
198204
response.successful = false;
199-
response.message = ERROR_FAILED_TO_FIND_VALID_SOLUTION;
205+
response.message = std::string(ERROR_FAILED_TO_FIND_VALID_SOLUTION) + reason;
200206
return response;
201207
}
202208

‎tesseract_motion_planners/simple/src/simple_motion_planner.cpp‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -41,8 +41,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
4141
#include <tesseract_motion_planners/planner_utils.h>
4242

4343
constexpr auto SOLUTION_FOUND{ "Found valid solution" };
44-
constexpr auto ERROR_INVALID_INPUT{ "Input to planner is invalid. Check that instructions and seed are compatible" };
45-
constexpr auto FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" };
44+
constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " };
45+
constexpr auto FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution: " };
4646

4747
namespace tesseract_planning
4848
{
@@ -61,10 +61,11 @@ MotionPlanner::Ptr SimpleMotionPlanner::clone() const { return std::make_shared<
6161
PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const
6262
{
6363
PlannerResponse response;
64-
if (!checkRequest(request))
64+
std::string reason;
65+
if (!checkRequest(request, reason)) // NOLINT
6566
{
6667
response.successful = false;
67-
response.message = ERROR_INVALID_INPUT;
68+
response.message = std::string(ERROR_INVALID_INPUT) + reason;
6869
return response;
6970
}
7071

@@ -93,7 +94,7 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const
9394
{
9495
CONSOLE_BRIDGE_logError("SimplePlanner failed to generate problem: %s.", e.what());
9596
response.successful = false;
96-
response.message = FAILED_TO_FIND_VALID_SOLUTION;
97+
response.message = std::string(FAILED_TO_FIND_VALID_SOLUTION) + e.what();
9798
return response;
9899
}
99100

‎tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp‎

Lines changed: 12 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -45,8 +45,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
4545
#include <tesseract_command_language/utils.h>
4646

4747
constexpr auto SOLUTION_FOUND{ "Found valid solution" };
48-
constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input" };
49-
constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" };
48+
constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " };
49+
constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution: " };
5050

5151
using namespace trajopt;
5252

@@ -67,10 +67,11 @@ MotionPlanner::Ptr TrajOptMotionPlanner::clone() const { return std::make_shared
6767
PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const
6868
{
6969
PlannerResponse response;
70-
if (!checkRequest(request))
70+
std::string reason;
71+
if (!checkRequest(request, reason))
7172
{
7273
response.successful = false;
73-
response.message = ERROR_INVALID_INPUT;
74+
response.message = std::string(ERROR_INVALID_INPUT) + reason;
7475
return response;
7576
}
7677

@@ -89,7 +90,7 @@ PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const
8990
{
9091
CONSOLE_BRIDGE_logError("TrajOptPlanner failed to generate problem: %s.", e.what());
9192
response.successful = false;
92-
response.message = ERROR_INVALID_INPUT;
93+
response.message = std::string(ERROR_INVALID_INPUT) + e.what();
9394
return response;
9495
}
9596

@@ -126,8 +127,12 @@ PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const
126127
if (opt->results().status != sco::OptStatus::OPT_CONVERGED)
127128
{
128129
response.successful = false;
129-
response.message = ERROR_FAILED_TO_FIND_VALID_SOLUTION;
130-
return response;
130+
response.message = std::string(ERROR_FAILED_TO_FIND_VALID_SOLUTION) + sco::statusToString(opt->results().status);
131+
}
132+
else
133+
{
134+
response.successful = true;
135+
response.message = SOLUTION_FOUND;
131136
}
132137

133138
const std::vector<std::string> joint_names = problem->GetKin()->getJointNames();
@@ -154,8 +159,6 @@ PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const
154159
move_instruction, joint_names, traj.row(static_cast<Eigen::Index>(idx)), request.format_result_as_input);
155160
}
156161

157-
response.successful = true;
158-
response.message = SOLUTION_FOUND;
159162
return response;
160163
}
161164

‎tesseract_motion_planners/trajopt/src/trajopt_utils.cpp‎

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -192,7 +192,8 @@ trajopt::TermInfo::Ptr
192192
createSmoothVelocityTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type)
193193
{
194194
if ((end_index - start_index) < 1)
195-
throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states!");
195+
throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states, failed with start_index " +
196+
std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!");
196197

197198
std::shared_ptr<trajopt::JointVelTermInfo> jv = std::make_shared<trajopt::JointVelTermInfo>();
198199
jv->coeffs = std::vector<double>(static_cast<std::size_t>(n_joints), coeff);
@@ -210,7 +211,8 @@ trajopt::TermInfo::Ptr createSmoothVelocityTermInfo(int start_index,
210211
trajopt::TermType type)
211212
{
212213
if ((end_index - start_index) < 1)
213-
throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states!");
214+
throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states, failed with start_index " +
215+
std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!");
214216

215217
std::shared_ptr<trajopt::JointVelTermInfo> jv = std::make_shared<trajopt::JointVelTermInfo>();
216218
jv->coeffs = std::vector<double>(coeff.data(), coeff.data() + coeff.size());
@@ -226,7 +228,8 @@ trajopt::TermInfo::Ptr
226228
createSmoothAccelerationTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type)
227229
{
228230
if ((end_index - start_index) < 2)
229-
throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states!");
231+
throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states, failed with start_index " +
232+
std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!");
230233

231234
std::shared_ptr<trajopt::JointAccTermInfo> ja = std::make_shared<trajopt::JointAccTermInfo>();
232235
ja->coeffs = std::vector<double>(static_cast<std::size_t>(n_joints), coeff);
@@ -244,7 +247,8 @@ trajopt::TermInfo::Ptr createSmoothAccelerationTermInfo(int start_index,
244247
trajopt::TermType type)
245248
{
246249
if ((end_index - start_index) < 2)
247-
throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states!");
250+
throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states, failed with start_index " +
251+
std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!");
248252

249253
std::shared_ptr<trajopt::JointAccTermInfo> ja = std::make_shared<trajopt::JointAccTermInfo>();
250254
ja->coeffs = std::vector<double>(coeff.data(), coeff.data() + coeff.size());
@@ -260,7 +264,8 @@ trajopt::TermInfo::Ptr
260264
createSmoothJerkTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type)
261265
{
262266
if ((end_index - start_index) < 4)
263-
throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states!");
267+
throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states, failed with start_index " +
268+
std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!");
264269

265270
std::shared_ptr<trajopt::JointJerkTermInfo> jj = std::make_shared<trajopt::JointJerkTermInfo>();
266271
jj->coeffs = std::vector<double>(static_cast<std::size_t>(n_joints), coeff);
@@ -278,7 +283,8 @@ trajopt::TermInfo::Ptr createSmoothJerkTermInfo(int start_index,
278283
trajopt::TermType type)
279284
{
280285
if ((end_index - start_index) < 4)
281-
throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states!");
286+
throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states, failed with start_index " +
287+
std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!");
282288

283289
std::shared_ptr<trajopt::JointJerkTermInfo> jj = std::make_shared<trajopt::JointJerkTermInfo>();
284290
jj->coeffs = std::vector<double>(coeff.data(), coeff.data() + coeff.size());

‎tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
4343
#include <tesseract_command_language/utils.h>
4444

4545
constexpr auto SOLUTION_FOUND{ "Found valid solution" };
46-
constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input" };
46+
constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " };
4747
constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" };
4848

4949
using namespace trajopt_ifopt;
@@ -66,10 +66,11 @@ MotionPlanner::Ptr TrajOptIfoptMotionPlanner::clone() const
6666
PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) const
6767
{
6868
PlannerResponse response;
69-
if (!checkRequest(request))
69+
std::string reason;
70+
if (!checkRequest(request, reason))
7071
{
7172
response.successful = false;
72-
response.message = ERROR_INVALID_INPUT;
73+
response.message = std::string(ERROR_INVALID_INPUT) + reason;
7374
return response;
7475
}
7576

‎tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -157,14 +157,13 @@ class MotionPlannerTask : public TaskComposerTask
157157
if (console_bridge::getLogLevel() == console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG)
158158
request.verbose = true;
159159
PlannerResponse response = planner_->solve(request);
160+
context.data_storage->setData(output_keys_[0], response.results);
160161

161162
// --------------------
162163
// Verify Success
163164
// --------------------
164165
if (response)
165166
{
166-
context.data_storage->setData(output_keys_[0], response.results);
167-
168167
info->return_value = 1;
169168
info->color = "green";
170169
info->message = response.message;

0 commit comments

Comments
 (0)