mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
list of random todos
This commit is contained in:
parent
51e1dda6ec
commit
ade42456b8
@ -64,6 +64,7 @@ MOVEIT_CLASS_FORWARD(Task)
|
||||
*/
|
||||
class Task : protected WrapperBase {
|
||||
public:
|
||||
// TODO: move into MoveIt! core
|
||||
static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model,
|
||||
const std::string &ns = "move_group",
|
||||
const std::string &planning_plugin_param_name = "planning_plugin",
|
||||
@ -81,6 +82,7 @@ public:
|
||||
/// load robot model from given parameter
|
||||
void loadRobotModel(const std::string& robot_description = "robot_description");
|
||||
|
||||
// TODO: use Stage::insert as well?
|
||||
void add(Stage::pointer &&stage);
|
||||
void clear() override;
|
||||
|
||||
@ -111,6 +113,7 @@ public:
|
||||
/// publish all top-level solutions
|
||||
void publishAllSolutions(bool wait = true);
|
||||
|
||||
// TODO: convenient access to arbitrary stage by name
|
||||
/// access stage tree
|
||||
ContainerBase *stages();
|
||||
const ContainerBase *stages() const;
|
||||
|
||||
@ -116,6 +116,7 @@ typedef std::vector<std::vector<double>> IKSolutions;
|
||||
|
||||
namespace {
|
||||
|
||||
// TODO: provide callback methods in PlanningScene class / probably not very useful here though...
|
||||
bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene,
|
||||
Eigen::Affine3d pose, const robot_model::LinkModel* link)
|
||||
{
|
||||
@ -215,6 +216,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator
|
||||
planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff();
|
||||
|
||||
// TODO: this should not be necessary in my opinion
|
||||
// enforced initialization from interface ensures that new target_pose is read
|
||||
properties().performInitFrom(INTERFACE, s.start()->properties(), true);
|
||||
const auto& props = properties();
|
||||
@ -376,6 +378,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
spawn(InterfaceState(scene), std::move(solution));
|
||||
}
|
||||
|
||||
// TODO: magic constant should be a property instead ("current_seed_only", or equivalent)
|
||||
if (!succeeded && max_ik_solutions == 1)
|
||||
break; // first and only attempt failed
|
||||
}
|
||||
|
||||
@ -241,10 +241,12 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
||||
|
||||
// add an arrow marker
|
||||
visualization_msgs::Marker m;
|
||||
// TODO: make "marker" a common property of all stages
|
||||
m.ns = props.get<std::string>("marker_ns");
|
||||
if (!m.ns.empty()) {
|
||||
m.header.frame_id = scene->getPlanningFrame();
|
||||
if (linear_norm > 1e-3) {
|
||||
// TODO: arrow could be split into "valid" and "invalid" part
|
||||
rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN
|
||||
: rviz_marker_tools::RED);
|
||||
rviz_marker_tools::makeArrow(m, linear_norm);
|
||||
|
||||
@ -48,7 +48,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
|
||||
, planner_(planner)
|
||||
{
|
||||
auto& p = properties();
|
||||
p.declare<double>("timeout", 10.0, "planning timeout");
|
||||
p.declare<double>("timeout", 10.0, "planning timeout"); // TODO: make this private in Stage
|
||||
p.declare<std::string>("group", "name of planning group");
|
||||
p.declare<std::string>("link", "", "link to move (default is tip of jmg)");
|
||||
|
||||
@ -184,6 +184,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
|
||||
return success;
|
||||
}
|
||||
|
||||
// TODO: move these as default implementation to PropagateEitherWay?
|
||||
bool MoveTo::computeForward(const InterfaceState &from){
|
||||
planning_scene::PlanningScenePtr to;
|
||||
SubTrajectory trajectory;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user