Pilot.h

Go to the documentation of this file.

/*
 * Copyright (C) by
 *   MetraLabs GmbH (MLAB), GERMANY
 *  and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Redistribution and modification of this code is strictly prohibited.
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_PILOT_PILOT_H_
#define _MIRA_PILOT_PILOT_H_

#include <serialization/adapters/boost/shared_ptr.hpp>
#include <serialization/adapters/std/vector>
#include <serialization/GetterSetter.h>

#include <fw/Unit.h>
#include <fw/ServiceProperty.h>
#include <robot/RobotModel.h>
#include <robot/Odometry.h>
#include <model/IRigidModelProvider.h>

#include <pilot/AbstractPlanner.h>
#include <pilot/DissatisfactionReasons.h>
#include <navigation/Task.h>
#include <navigation/tasks/OrientationTask.h>
#include <navigation/INavigation.h>

#include <model/Marker.h>

namespace mira { namespace pilot {


class Pilot : public Unit, public navigation::INavigation, public model::IRigidModelConsumer
{
MIRA_OBJECT(Pilot);
public:

    Pilot() : 
        Unit(Duration::milliseconds(250))
    {
        mMute = false;
        mModelProvider = ServiceProperty("IRobotModelProvider");
    }

public:

    template<typename Reflector>
    void reflectServices(Reflector& r)
    {
        r.property("DriveService",  mDriveService,
                   setter(&Pilot::setDriveService, this),
                   "The name of the service that is used for setting the velocity "
                   "(if empty, a suitable service is chosen automatically)",
                   ServiceProperty("IDrive"));
    }

    void reflectServices(XMLDeserializer& r)
    {
        try {
            r.property("DriveService",  mDriveService,
                       setter(&Pilot::setDriveService, this),
                       "The name of the service that is used for setting the velocity "
                       "(if empty, a suitable service is chosen automatically)",
                       ServiceProperty("IDrive"));
        } catch (const XMemberNotFound_NoDefault&) {
            // workaround for old configs where DriveService was a string property
            MIRA_LOG(WARNING) << "Pilot failed reading property DriveService as ServiceProperty. "
                                 "Fallback: trying as string.";
            std::string service;
            r.property("DriveService", service, "", "");
            MIRA_LOG(WARNING) << "Expected ServiceProperty for Pilot's DriveService property, found string '"
                              << service << "'. Please update your config!";
            ServiceProperty p("IDrive");
            p = service;
            setDriveService(p);
        }
    }

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        MIRA_REFLECT_BASE(r, Unit);
        MIRA_REFLECT_BASE(r, IRigidModelConsumer);

        reflectServices(r);

        r.property("CPUUsage", mCPUUsage,
                   "The cpu usage in percent (default: 90%)",
                   90.0f, PropertyHints::limits(0.0f, 100.0f));
        r.property("Mute", mMute,
                   "If enabled, then the robot does not send drive commands",false);

        r.property("UseRecovery", mUseRecovery,
                   "Are recovery strategies enabled", false);

        r.roproperty("Reached Tasks",
                     getter<bool>(&Pilot::reachedTasks, this),
                     "True, if all tasks have been reached");

        r.property("Planner", mPlanner,
                   "The motion planner that is used by the navigator");

        r.property("RecoveryStrategies", mRecoveryStrategies, "The recovery strategies",
                   std::map<std::string, navigation::TaskPtr>());

        MIRA_REFLECT_BASE(r, navigation::INavigation);

        r.method("setMute", &Pilot::setMute, this, "(Un-)Mute the pilot.",
                 "mute", "true/false to mute/unmute", true);

        navigation::TaskPtr task(new navigation::Task());
        task->addSubTask(navigation::SubTaskPtr(
                    new navigation::OrientationTask(deg2rad(180.f), deg2rad(5.f), "/maps/MapFrame")));

        r.method("setRecoverTask", &Pilot::setRecoverTask, this,
                 "Manually trigger recovery. Only valid if UseRecovery=true.",
                 "task", "The task used for recovery.", task);
    }

    boost::shared_ptr<robot::RobotModel> getRobotModel();

public:

    virtual void setGoal(const Pose2& goal, float transT, float rotT);
    virtual void setTask(navigation::TaskPtr task);
    virtual void repeatTask() {
        setTask(mLastTask);
    }

    void setMute(bool mute) {
        mMute = mute;
    }

    bool reachedTasks() const {
        return mPlanner ? mPlanner->reachedTasks() : true;
    }

    const Stamped<robot::Odometry2>& getCurrentOdometry() const {
        return mCurrentOdometry;
    }

    void fireEvent(const std::string& event) {
        mEventChannel.post(event);
    }

    boost::shared_ptr<AbstractPlanner> getPlanner(){
        return mPlanner;
    }

    bool getUseRecovery() const {
        return mUseRecovery;
    }

protected:

    virtual void initialize();

    virtual void process(const Timer& timer);

private:

    void setRecoverTask(navigation::TaskPtr task);

private:

    Velocity2 getAcceleration(const Velocity2& velocity);
    void setVelocity(const Velocity2& v);
    void taskError(const std::string& msg, const std::string& errorEvent="UnknownError");
    void taskInterruption(const std::string& msg,
                          const std::string& interruptionEvent="UnknownInterruption");

    void setDriveService(ServiceProperty service);

    virtual void setModelProviderInternal(ServiceProperty service);
    virtual Authority* getAuthority() { return this; }

private:

    bool mMute;
    float mCPUUsage;

    ServiceProperty mDriveService;

    boost::shared_ptr<robot::RobotModel> mRobotModel;
    boost::mutex mModelMutex;

    boost::shared_ptr<AbstractPlanner> mPlanner;

    navigation::TaskPtr mTask;
    navigation::TaskPtr mRecoveryTask;
    navigation::TaskPtr mLastTask;

    Channel<std::string> mEventChannel;
    Channel<robot::Odometry2> mOdometryChannel;
    Channel<Velocity2> mVelocityChannel;
    Channel<Velocity2> mAccelerationChannel;
    Channel<navigation::TaskPtr> mTaskChannel;
    Stamped<robot::Odometry2> mCurrentOdometry;

    bool mUseRecovery;
    typedef std::map<std::string, navigation::TaskPtr> RecoveryStrategies;
    RecoveryStrategies mRecoveryStrategies;
};


}}

#endif