CalcPath.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2014-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotSkillTemplates::MovePlatformToLandmarkGroup
 * @author     Valerij Wittenbeck ( valerij dot wittenbeck at student dot kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CalcPath.h"
 
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/observers/variant/ChannelRef.h>
 
#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
 
#include "../PlatformContext.h"
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/variants/GraphNode/GraphNode.h>
 
 
using namespace armarx;
using namespace PlatformGroup;
 
// DO NOT EDIT NEXT LINE
CalcPath::SubClassRegistry CalcPath::Registry(CalcPath::GetName(), &CalcPath::CreateInstance);
 
CalcPath::CalcPath(XMLStateConstructorParams stateData) :
    XMLStateTemplate<CalcPath>(stateData), CalcPathGeneratedBase<CalcPath>(stateData)
{
    ARMARX_IMPORTANT << "CalcPath";
}
 
bool
getNearestPositionOnEdge(const ::memoryx::GraphNodeBaseList& nodes,
                         float currentX,
                         float currentY,
                         Eigen::Vector2f& closest,
                         memoryx::GraphNodePtr& edgeNode1,
                         memoryx::GraphNodePtr& edgeNode2)
{
    Eigen::Vector2f robotLocation(currentX, currentY);
    closest.setZero();
 
    for (auto& basenode : nodes)
    {
        memoryx::GraphNodePtr node = memoryx::GraphNodePtr::dynamicCast(basenode);
 
        auto size = node->getOutdegree();
        for (int i = 0; i < size; ++i)
        {
            memoryx::GraphNodePtr nextNode =
                memoryx::GraphNodePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
 
            Eigen::Vector2f node1 =
                FramedPosePtr::dynamicCast(node->getPose())->toEigen().block<2, 1>(0, 3);
            Eigen::Vector2f node2 =
                FramedPosePtr::dynamicCast(nextNode->getPose())->toEigen().block<2, 1>(0, 3);
 
            if ((node1 - node2).norm() > 1e-9)
            {
                Eigen::Vector2f result =
                    VirtualRobot::MathTools::nearestPointOnSegment<Eigen::Vector2f>(
                        node1, node2, robotLocation);
 
                if ((closest.x() == 0 && closest.y() == 0) ||
                    (result - robotLocation).norm() < (closest - robotLocation).norm())
                {
                    closest = result;
                    edgeNode1 = node;
                    edgeNode2 = nextNode;
                }
            }
        }
    }
 
    return (closest.x() != 0 || closest.y() != 0);
}
 
/**
 * @brief Projection of point on an edge of the graph.
 * (node)-------(position)-------->(targetNode)
 */
struct EdgeProjection
{
    memoryx::GraphNodePtr targetNode;
    Eigen::Vector2f position;
};
 
std::vector<EdgeProjection>
getNearestPositionOnEdges(const ::memoryx::GraphNodeBaseList& nodes, Eigen::Vector2f robotPos)
{
    std::vector<EdgeProjection> projections;
 
    for (auto& basenode : nodes)
    {
        memoryx::GraphNodePtr node = memoryx::GraphNodePtr::dynamicCast(basenode);
 
        auto size = node->getOutdegree();
        for (int i = 0; i < size; ++i)
        {
            memoryx::GraphNodePtr nextNode =
                memoryx::GraphNodePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
 
            Eigen::Vector2f node1 =
                FramedPosePtr::dynamicCast(node->getPose())->toEigen().block<2, 1>(0, 3);
            Eigen::Vector2f node2 =
                FramedPosePtr::dynamicCast(nextNode->getPose())->toEigen().block<2, 1>(0, 3);
 
            if ((node1 - node2).norm() > 1e-9)
            {
                Eigen::Vector2f result =
                    VirtualRobot::MathTools::nearestPointOnSegment<Eigen::Vector2f>(
                        node1, node2, robotPos);
 
                projections.push_back(EdgeProjection{.targetNode = nextNode, .position = result});
            }
        }
    }
 
    return projections;
}
 
void
CalcPath::onEnter()
{
    ARMARX_VERBOSE << "CalcPath::onEnter()";
    PlatformContext* context = getContext<PlatformContext>();
 
    auto graphSegment = context->priorKnowledgePrx->getGraphSegment();
    viz::ScopedClient arviz(
        viz::Client::createFromTopic("MovePlatformToLandmark/CalcPath", context->arvizTopic));
 
    std::vector<armarx::Vector3Ptr> targetPositions;
 
    // Does the scene exist
    const std::string sceneName = in.getSceneName();
    if (!graphSegment->hasScene(sceneName))
    {
        ARMARX_WARNING << "Target scene '" << sceneName << "' does not exist in menory";
        out.settargetPositions(targetPositions);
        emitEvNoPathFound();
        return;
    }
 
    // Does the target landmark exist
    const std::string landmark = in.gettargetLandmark();
    if (!graphSegment->hasNodeWithName(sceneName, landmark))
    {
        ARMARX_WARNING << "Target landmark '" << landmark << "' doesn't exist in graph (scene) '"
                       << sceneName << "'";
        auto nodes = graphSegment->getNodesByScene(sceneName);
        for (auto& node : nodes)
        {
            ARMARX_INFO << "node: " << node->getName();
        }
        out.settargetPositions(targetPositions);
        sendEvent<EvNoPathFound>();
        return;
    }
 
    memoryx::GraphNodeBasePtr goalNode;
    {
        memoryx::GraphNodeBasePtr landmarkNode =
            graphSegment->getNodeFromSceneByName(sceneName, landmark);
        ARMARX_CHECK_NOT_NULL(landmarkNode)
            << "Could not find node with name '" << landmark << "' in scene '" << sceneName << "'";
 
        goalNode = context->graphNodePoseResolverPrx->getNearestRobotLocationNode(landmarkNode);
        ARMARX_CHECK_NOT_NULL(goalNode) << "No nearest node found for landmark '" << landmark
                                        << "' in scene '" << sceneName << "'";
    }
    ARMARX_INFO << "Resolved target landmark to '" << goalNode->getName() << "'";
 
 
    viz::Layer layerRobotPose = arviz.layer("Robot Pose");
    viz::Layer layerClosestPoint = arviz.layer("Closest Point");
 
    // Get current position
    ChannelRefPtr poseRef =
        context->getChannelRef(context->getPlatformUnitObserverName(), "platformPose");
    const float platformPositionX = poseRef->getDataField("positionX")->getFloat();
    const float platformPositionY = poseRef->getDataField("positionY")->getFloat();
    const float platformYaw = poseRef->getDataField("rotation")->getFloat();
 
    Eigen::Vector3f currentPos(platformPositionX, platformPositionY, 0);
    const Eigen::Vector2f currentPos2d = currentPos.head<2>();
 
    {
        layerRobotPose.add(
            viz::Pose("RobotPose")
                .position(currentPos)
                .orientation(VirtualRobot::MathTools::rpy2eigen3f(0, 0, platformYaw)));
    }
 
    std::vector<EdgeProjection> projections =
        getNearestPositionOnEdges(graphSegment->getNodesByScene(sceneName), currentPos2d);
    if (projections.empty())
    {
        ARMARX_ERROR << "Could not find nearest point on graph";
        emitEvNoPathFound();
        return;
    }
 
    std::sort(projections.begin(),
              projections.end(),
              [&currentPos2d](const auto& p1, const auto& p2)
              {
                  const float c1 = (p1.position - currentPos2d).norm();
                  const float c2 = (p2.position - currentPos2d).norm();
 
                  return c1 < c2;
              });
 
    // Projections are sorted by distance to current position.
    std::optional<EdgeProjection> bestProjection;
    ::memoryx::GraphNodeBaseList bestPath;
    float bestPathCosts = std::numeric_limits<float>::max();
 
    for (const EdgeProjection& projection : projections)
    {
        const Eigen::Vector3f nodePosition = fromIce(projection.targetNode->getPose()->position);
        const float distToNode = (nodePosition - currentPos).norm();
        ARMARX_INFO << "Current node: '" << projection.targetNode->getName() << "' | "
                    << VAROUT(distToNode);
 
        // Find a path from the closest node to the target
        ::memoryx::GraphNodeBaseList path =
            graphSegment->aStar(projection.targetNode->getId(), goalNode->getId());
        // ToDo: Cost = (current pos -> proj.position) + (proj.position -> proj.targetNode)
        float pathCost = distToNode + getPathLength(path);
 
        if (path.empty())
        {
            continue;
        }
 
        // Projections can have two entries with the same length right after another (undirected edge)
        // We only stop after we visited both directions.
        if (bestProjection.has_value() and
            (bestProjection->position - projection.position).norm() > 1e-4)
        {
            break;
        }
 
        if (pathCost < bestPathCosts)
        {
            bestProjection = projection;
            bestPath = path;
            bestPathCosts = pathCost;
        }
    }
 
    if (not bestProjection.has_value())
    {
        ARMARX_WARNING << "No path found";
        out.settargetPositions(targetPositions);
        emitEvNoPathFound();
        return;
    }
 
    const Eigen::Vector2f nearestPoint = bestProjection->position;
    ARMARX_INFO << "Nearest Point on graph: " << nearestPoint.transpose();
    {
        Eigen::Vector3f closestPointPos;
        closestPointPos << nearestPoint, 0;
 
        layerClosestPoint.add(viz::Sphere("Closest Point")
                                  .position(closestPointPos)
                                  .radius(100)
                                  .color(simox::Color::green()));
        layerClosestPoint.add(viz::Cylinder("To Closest Point")
                                  .fromTo(currentPos, closestPointPos)
                                  .radius(30)
                                  .color(simox::Color::green(96)));
    }
 
    // arviz.commit({layerRobotPose, layerClosestPoint});
 
    bool fixedOrentation = true;
    float length = getPathLength(bestPath);
 
    bool useWaypointOrientations = in.getUseWaypointOrientations();
    if (!useWaypointOrientations && length > in.getdriveMode_lookAhead_thresholdMM())
    {
        ARMARX_INFO << "long path (" << length
                    << " mm) -> rotating platform towards moving direction";
        fixedOrentation = false;
    }
 
    Eigen::Vector3f rpy;
    PosePtr pose = PosePtr::dynamicCast(bestPath.back()->getPose());
    VirtualRobot::MathTools::eigen4f2rpy(pose->toEigen(), rpy);
    const float goalYaw = rpy[2];
    ::armarx::FramedPosePtr lastPose;
    float lastAlpha = 0;
    int count = 0;
 
    for (auto& node : bestPath)
    {
        ::armarx::FramedPosePtr currentPose = FramedPosePtr::dynamicCast(node->getPose());
        Vector3Ptr v = new Vector3();
        v->x = currentPose->position->x;
        v->y = currentPose->position->y;
 
        if (useWaypointOrientations)
        {
            VirtualRobot::MathTools::eigen4f2rpy(currentPose->toEigen(), rpy);
            v->z = rpy[2];
            ARMARX_INFO << "Using waypoint orientation: " << v->z;
        }
        else if (fixedOrentation)
        {
            v->z = goalYaw;
        }
        else
        {
            if (count <= 0 || count >= int(bestPath.size() - 1))
            {
                // first /last pos
                VirtualRobot::MathTools::eigen4f2rpy(currentPose->toEigen(), rpy);
                ARMARX_DEBUG << "start or last pose of path, using orientation of waypoint: "
                             << rpy[2];
                v->z = rpy[2];
                lastAlpha = rpy[2];
            }
            else
            {
                double x = currentPose->position->x - lastPose->position->x;
                double y = currentPose->position->y - lastPose->position->y;
                double dist = std::sqrt(x * x + y * y);
 
                if (dist < 500)
                {
                    ARMARX_DEBUG << "points close together, using last orientation: " << lastAlpha;
                    v->z = lastAlpha;
                }
                else
                {
                    // compute alpha
                    Eigen::Vector2d a(x, y);
                    a.normalize();
                    Eigen::Vector2d b(0, 1); // vector in y dir == 0 degrees
                    v->z = -(std::atan2(b.y(), b.x()) - std::atan2(a.y(), a.x()));
 
                    if (v->z < 0)
                    {
                        v->z += 2.0 * M_PI;
                    }
 
                    //v->z = acos(a.dot(b));
                    lastAlpha = v->z;
                    ARMARX_DEBUG << "waypoint angle between " << a.transpose() << " and "
                                 << b.transpose() << ": " << v->z;
                }
            }
 
            lastPose = currentPose;
            count++;
        }
 
        targetPositions.push_back(v);
        ARMARX_INFO << "waypoint #" << targetPositions.size() << ": " << node->getName()
                    << " Coordinates:\n " << v->output();
    }
 
    // Replace initial node with closest point on edge
    Vector3Ptr v(new Vector3(nearestPoint.x(),
                             nearestPoint.y(),
                             Vector3Ptr::dynamicCast(targetPositions[0])->toEigen().z()));
    targetPositions.insert(targetPositions.begin(), v);
 
    out.settargetPositions(targetPositions);
    emitEvPathFound();
}
 
void
CalcPath::onExit()
{
    PlatformContext* context = getContext<PlatformContext>();
    context->debugDrawer->removeSphereDebugLayerVisu("closestPoint");
}
 
float
CalcPath::getPathLength(::memoryx::GraphNodeBaseList& path)
{
    float l = 0.0f;
    ::armarx::FramedPoseBasePtr lastPose;
 
    for (auto& node : path)
    {
        if (!lastPose)
        {
            lastPose = node->getPose();
            continue;
        }
 
        ::armarx::FramedPoseBasePtr currentPose = node->getPose();
        float x = currentPose->position->x - lastPose->position->x;
        float y = currentPose->position->y - lastPose->position->y;
        l += std::sqrt(x * x + y * y);
        lastPose = currentPose;
    }
 
    return l;
}
 
// DO NOT EDIT NEXT FUNCTION
std::string
CalcPath::GetName()
{
    return "CalcPath";
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
CalcPath::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new CalcPath(stateData));
}