task.h

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2017, Hamburg University
 *  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.
 *********************************************************************/
 
/* Authors: Michael Goerner, Robert Haschke
   Desc:    User-level access to the whole Task constructor infrastructure
*/
 
#pragma once
 
#include "container.h"
 
#include <moveit/task_constructor/introspection.h>
#include <moveit_task_constructor_msgs/Solution.h>
 
#include <moveit/macros/class_forward.h>
 
#include <moveit_msgs/MoveItErrorCodes.h>
#include <moveit/utils/moveit_error_code.h>
 
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(RobotModel);
MOVEIT_CLASS_FORWARD(RobotState);
}  // namespace core
}  // namespace moveit
 
namespace moveit {
namespace task_constructor {
 
MOVEIT_CLASS_FORWARD(Stage);
MOVEIT_CLASS_FORWARD(ContainerBase);
MOVEIT_CLASS_FORWARD(Task);
 
class TaskPrivate;
class Task : protected WrapperBase
{
public:
    PRIVATE_CLASS(Task)
    using WrapperBase::setCostTerm;
    using WrapperBase::operator[];
 
    Task(const std::string& ns = "", bool introspection = true,
         ContainerBase::pointer&& container = std::make_unique<SerialContainer>("task pipeline"));
    Task(Task&& other);  // NOLINT(performance-noexcept-move-constructor)
    Task& operator=(Task&& other);  // NOLINT(performance-noexcept-move-constructor)
    ~Task() override;
 
    const std::string& name() const { return stages()->name(); }
    void setName(const std::string& name) { stages()->setName(name); }
 
    Stage* findChild(const std::string& name) const { return stages()->findChild(name); }
    Stage* operator[](int index) const { return stages()->operator[](index); }
 
    const moveit::core::RobotModelConstPtr& getRobotModel() const;
    void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model);
    void loadRobotModel(const std::string& robot_description = "robot_description");
 
    void add(Stage::pointer&& stage);
    void insert(Stage::pointer&& stage, int before = -1) override;
    void clear() final;
 
    void enableIntrospection(bool enable = true);
    Introspection& introspection();
 
    using TaskCallback = std::function<void(const Task& t)>;
    using TaskCallbackList = std::list<TaskCallback>;
    TaskCallbackList::const_iterator addTaskCallback(TaskCallback&& cb);
    void eraseTaskCallback(TaskCallbackList::const_iterator which);
 
    using WrapperBase::addSolutionCallback;
    using WrapperBase::removeSolutionCallback;
    using WrapperBase::SolutionCallback;
 
    using WrapperBase::setTimeout;
    using WrapperBase::timeout;
 
    using WrapperBase::pruning;
    using WrapperBase::setPruning;
 
    void reset() final;
    void init();
 
    moveit::core::MoveItErrorCode plan(size_t max_solutions = 0);
    void preempt();
    void resetPreemptRequest();
    moveit::core::MoveItErrorCode execute(const SolutionBase& s);
 
    void printState(std::ostream& os = std::cout) const;
 
    void explainFailure(std::ostream& os = std::cout) const override;
 
    size_t numSolutions() const { return solutions().size(); }
    const ordered<SolutionBaseConstPtr>& solutions() const { return stages()->solutions(); }
    const std::list<SolutionBaseConstPtr>& failures() const { return stages()->failures(); }
 
    void publishAllSolutions(bool wait = true);
 
    // +1 TODO: convenient access to arbitrary stage by name. traverse hierarchy using / separator?
    ContainerBase* stages();
    const ContainerBase* stages() const;
 
    PropertyMap& properties();
    const PropertyMap& properties() const { return const_cast<Task*>(this)->properties(); }
    void setProperty(const std::string& name, const boost::any& value);
    inline void setProperty(const std::string& name, const char* value) { setProperty(name, std::string(value)); }
 
protected:
    bool canCompute() const override;
    void compute() override;
    void onNewSolution(const SolutionBase& s) override;
 
private:
    using WrapperBase::init;
};
 
inline std::ostream& operator<<(std::ostream& os, const Task& task) {
    task.printState(os);
    return os;
}
}  // namespace task_constructor
}  // namespace moveit