container.h

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2017, Bielefeld University
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Bielefeld University nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
/* Author: Robert Haschke
   Desc:   Container Stages to combine multiple planning Stage in different ways
*/
 
#pragma once
 
#include "stage.h"
 
namespace moveit {
namespace task_constructor {
 
class ContainerBasePrivate;
class ContainerBase : public Stage
{
public:
    PRIVATE_CLASS(ContainerBase)
    using pointer = std::unique_ptr<ContainerBase>;
 
    void setPruning(bool pruning) { setProperty("pruning", pruning); }
    bool pruning() const { return properties().get<bool>("pruning"); }
 
    size_t numChildren() const;
    Stage* findChild(const std::string& name) const;
    Stage* operator[](int index) const;
 
    using StageCallback = std::function<bool(const Stage&, unsigned int)>;
    bool traverseChildren(const StageCallback& processor) const;
    bool traverseRecursively(const StageCallback& processor) const;
 
    void add(Stage::pointer&& stage);
 
    virtual void insert(Stage::pointer&& stage, int before = -1);
    virtual Stage::pointer remove(int pos);
    virtual Stage::pointer remove(Stage* child);
    virtual void clear();
 
    void reset() override;
    void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 
    virtual bool canCompute() const = 0;
    virtual void compute() = 0;
    void explainFailure(std::ostream& os) const override;
 
    virtual void onNewSolution(const SolutionBase& s) = 0;
 
protected:
    ContainerBase(ContainerBasePrivate* impl);
};
std::ostream& operator<<(std::ostream& os, const ContainerBase& stage);
 
class SerialContainerPrivate;
class SerialContainer : public ContainerBase
{
public:
    PRIVATE_CLASS(SerialContainer)
    SerialContainer(const std::string& name = "serial container");
 
    bool canCompute() const override;
    void compute() override;
 
protected:
    void onNewSolution(const SolutionBase& s) override;
 
    SerialContainer(SerialContainerPrivate* impl);
};
 
class ParallelContainerBasePrivate;
class ParallelContainerBase;
class ParallelContainerBase : public ContainerBase
{
public:
    PRIVATE_CLASS(ParallelContainerBase)
    ParallelContainerBase(const std::string& name = "parallel container");
 
protected:
    ParallelContainerBase(ParallelContainerBasePrivate* impl);
 
    inline void liftSolution(const SolutionBase& solution) { liftSolution(solution, solution.cost()); }
    void liftSolution(const SolutionBase& solution, double cost) { liftSolution(solution, cost, solution.comment()); }
 
    void liftSolution(const SolutionBase& solution, double cost, std::string comment);
 
    void spawn(InterfaceState&& state, SubTrajectory&& trajectory);
    void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& trajectory);
    void sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& trajectory);
};
 
class Alternatives : public ParallelContainerBase
{
public:
    Alternatives(const std::string& name = "alternatives") : ParallelContainerBase(name) {}
 
    bool canCompute() const override;
    void compute() override;
 
    void onNewSolution(const SolutionBase& s) override;
};
 
class FallbacksPrivate;
class Fallbacks : public ParallelContainerBase
{
    inline void replaceImpl();
 
public:
    PRIVATE_CLASS(Fallbacks);
    Fallbacks(const std::string& name = "fallbacks");
 
    void reset() override;
    void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 
protected:
    Fallbacks(FallbacksPrivate* impl);
    void onNewSolution(const SolutionBase& s) override;
 
private:
    // not needed, we directly use corresponding virtual methods of FallbacksPrivate
    bool canCompute() const final { return false; }
    void compute() final {}
};
 
class MergerPrivate;
class Merger : public ParallelContainerBase
{
public:
    PRIVATE_CLASS(Merger)
    Merger(const std::string& name = "merger");
 
    void reset() override;
    void init(const core::RobotModelConstPtr& robot_model) override;
    bool canCompute() const override;
    void compute() override;
 
protected:
    Merger(MergerPrivate* impl);
    void onNewSolution(const SolutionBase& s) override;
};
 
class WrapperBasePrivate;
class WrapperBase : public ParallelContainerBase
{
public:
    PRIVATE_CLASS(WrapperBase)
    WrapperBase(const std::string& name = "wrapper", Stage::pointer&& child = Stage::pointer());
 
    void insert(Stage::pointer&& stage, int before = -1) override;
 
    Stage* wrapped();
    inline const Stage* wrapped() const { return const_cast<WrapperBase*>(this)->wrapped(); }
 
    bool canCompute() const override;
    void compute() override;
 
protected:
    WrapperBase(WrapperBasePrivate* impl, Stage::pointer&& child = Stage::pointer());
};
}  // namespace task_constructor
}  // namespace moveit