PathObjective.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * 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_TOPOMAPPATHOBJECTIVE_H_
#define _MIRA_TOPOMAPPATHOBJECTIVE_H_


#include <navigation/tasks/PositionTask.h>

#include <pilot/Pilot.h>
#include <pilot/Objective.h>
#include <pilot/PathObjectiveBase.h>

#include <model/Marker.h>

#include <maps/OccupancyGrid.h>

#include <topomap/TopoMap.h>
#include <topomap/TopoMapPlanner.h>
#include <topomap/ActionAttribute.h>
#include <planner/MetricCostmapTopoMapPlanner.h>

namespace viros { namespace topomap {


class PathObjective : public mira::pilot::PathObjectiveBase, private MetricCostmapTopoMapPlanner::IEventListener
{
MIRA_OBJECT(PathObjective);

public:
    template<typename Reflector>
    void reflect(Reflector& r)
    {
        MIRA_REFLECT_BASE(r, mira::pilot::PathObjectiveBase);
        mPlanner.reflect(r);

        r.method("waitForInitCostMapUpdate", &PathObjective::waitForInitCostMapUpdate, this,
                 "If called, the objective will wait for the next InitCostMap update before updating the path");

        r.method("setAdditionalLinkCosts", &PathObjective::setAdditionalLinkCosts,
                 this, "Define additional costs for a specific link.",
                 "fromGWName", "from this gateway", "Gateway1",
                 "toGWName", "to this gateway", "Gateway2",
                 "cost", "additional cost from gateway1 to gateway2", 5);
        r.method("resetAllAdditionalLinkCosts", &PathObjective::resetAllAdditionalLinkCosts,
                 this, "Reset all additional gateways costs to zero.");
    }


    virtual void initialize();
    virtual void setTask(boost::shared_ptr<navigation::Task> task);
    virtual bool inGoalRegion(const Pose2& iRobotInMap, const Point2f& iGoalInMap, const boost::shared_ptr<navigation::PositionTask>& task);
    virtual bool reachedTask();
    virtual void update();
    virtual void plan(Duration plantime);
    virtual void prepareVote(float lookAheadTime);
    virtual Vote vote(const mira::robot::PoseVelocityTrajectory& trajectory, float lookAheadTime);

    virtual bool haveValidPath();

    virtual void updateMetricPlannerGoal(mira::pilot::AbstractMetricPlannerPtr planner, bool initial);

    void setAdditionalLinkCosts(const std::string& fromGWName, const std::string& toGWName, double costs) {
        mPlanner.setAdditionalLinkCosts(fromGWName, toGWName, costs);
    }

    void resetAllAdditionalLinkCosts() {
        mPlanner.resetAllAdditionalLinkCosts();
    }

    void waitForInitCostMapUpdate() {
        mWaitForInitCostMapUpdate = true;
    }

protected:

    void onInitCostMapChanged(ChannelRead<maps::GridMap<double>> data);

    virtual bool inGoalHysteresis() const;

    virtual bool causesWork(navigation::TaskPtr task);

protected:

    TopoMapPtr mTopoMap;

    double mOffset;

    bool mInGoalHysteresis;
    bool mWaitForInitCostMapUpdate;

    Channel<std::vector<std::string>> mLocalizedNodesChannel;
    Channel<maps::GridMap<uint8,3>> mDbgNavStatusChannel;
    Channel<mira::model::Marker> mDbgMarkerChannel;
    Channel<MetricCostmapTopoMapPlanner::NextSubGoal> mDbgNextSubGoalChannel;
    Channel<std::vector<Point2f>> mPathChannel;

    boost::shared_ptr<navigation::PositionTask> mTask;

    // stores whether the path can be used for voting
    bool mHaveValidPath;


protected:

    void clearGoal();
    void setGoal(const Point2f& goal, const std::string& frame);

    void computeLeafCosts(NodePtr node);
    void localizeGoal(NodePtr node, const std::string& frame, const Point2i& cell, const Point2f& posinmap, MetricCostmapTopoMapPlanner::NodePositionList& nodes);
    void localizeGoalWorld(NodePtr node, const Point2f& pos, const std::string& goalFrame, MetricCostmapTopoMapPlanner::NodePositionList& nodes, bool stayWithinMapTf);

private:

    //void getBaseNodesWithMap(NodePtr node, std::vector<std::string>& nodes);

private: // implementation if IEventListener
    void onNodeChanged(NodePtr currentNode, GatewayNodePtr usedGateway, GatewayNodePtr nextGateway);

protected:

    MetricCostmapTopoMapPlanner mPlanner;

    Channel<maps::GridMap<double>> mInitMapChannel;
    Channel<maps::GridMap<double>> mCurrentMapChannel;
    boost::optional<uint32> mLastSequence;

};


} } // namespace

#endif