AStarPathPlannerTestComponent.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* 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    RobotComponents::applications::AStarPathPlannerTestApp
* @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
* @date       2015 Humanoids Group, H2T, KIT
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "AStarPathPlannerTestComponent.h"
 
#include <chrono>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/updater/ObjectLocalization/MemoryXUpdaterObjectFactories.h>
 
static const std::string layerName{"AStarPathPlannerTestLayer"};
 
void
armarx::AStarPathPlannerTestComponent::onInitComponent()
{
    usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
    offeringTopic(getProperty<std::string>("DebugDrawerName").getValue());
    usingProxy(getProperty<std::string>("AStarPathPlannerName").getValue());
}
 
void
armarx::AStarPathPlannerTestComponent::onConnectComponent()
{
    workingMemoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(
        getProperty<std::string>("WorkingMemoryName").getValue());
    debugDrawerPrx = getTopic<armarx::DebugDrawerInterfacePrx>(
        getProperty<std::string>("DebugDrawerName").getValue());
    aStarPathPlannerPrx = getProxy<armarx::AStarPathPlannerBasePrx>(
        getProperty<std::string>("AStarPathPlannerName").getValue());
 
    //pass all objects from the scene to the planner
    auto objInstPrx = workingMemoryPrx->getObjectInstancesSegment();
    const auto objIds = objInstPrx->getAllEntityIds();
 
    armarx::ObjectPositionBaseList objects{};
 
    for (const auto& id : objIds)
    {
        const memoryx::EntityBasePtr entityBase = objInstPrx->getEntityById(id);
        const memoryx::ObjectInstanceBasePtr object =
            memoryx::ObjectInstancePtr::dynamicCast(entityBase);
 
        assert(object);
 
        const std::string className = object->getMostProbableClass();
 
        memoryx::ObjectClassList classes = workingMemoryPrx->getPriorKnowledge()
                                               ->getObjectClassesSegment()
                                               ->getClassWithSubclasses(className);
 
        if (!classes.size())
        {
            ARMARX_INFO << "No classes for most probable class '" << className << "' of object '"
                        << object->getName() << "' with id " << id;
            continue;
        }
 
        armarx::ObjectPositionBase obj;
 
        obj.objectClassBase = classes.at(0);
 
        obj.objectPose = ::armarx::PoseBasePtr{
            new Pose{object->getPositionBase(), object->getOrientationBase()}};
 
        objects.push_back(obj);
 
        ARMARX_VERBOSE << "Added class '" << className << "' of object '" << object->getName()
                       << "' with id " << id;
    }
 
    ARMARX_INFO << "using " << objects.size() << "objects";
 
    aStarPathPlannerPrx->setCollisionObjects(objects);
 
    memoryx::AgentInstancesSegmentBasePrx agSegPrx = workingMemoryPrx->getAgentInstancesSegment();
 
    //set agent
    const auto agentIds = agSegPrx->getAllEntityIds();
 
    if (!agentIds.size())
    {
        ARMARX_ERROR << "No agent found in memory";
        return;
    }
 
    const auto agent0Id = agentIds.at(0);
 
    ARMARX_INFO << "using agent " << agent0Id;
 
    memoryx::AgentInstanceBasePtr agent = agSegPrx->getAgentInstanceById(agent0Id);
 
    std::string agentCollisionModelName{"Platform"};
 
    aStarPathPlannerPrx->setAgent(agent, agentCollisionModelName);
    aStarPathPlannerPrx->setSafetyMargin(0);
 
 
    //plan the path
    armarx::Vector3BasePtr from{new armarx::Vector3{1900.f, 2300.f, 0.f}};
    //armarx::Vector3BasePtr to {new armarx::Vector3{2500.f, 7000.f, 1.f}};
    armarx::Vector3BasePtr to{new armarx::Vector3{2600.f, 9500.f, 1.f}};
 
 
    ARMARX_INFO << "Starting path planning";
    Vector3BaseList path;
    std::size_t iterations = 25;
    std::chrono::high_resolution_clock::duration duration{0};
 
    for (std::size_t i = 1; i <= iterations; ++i)
    {
        ARMARX_INFO_S << "iteration " << i;
        const auto tStart = std::chrono::high_resolution_clock::now();
        path = aStarPathPlannerPrx->getPath(from, to);
        const auto tEnd = std::chrono::high_resolution_clock::now();
        duration += (tEnd - tStart);
    }
    ARMARX_INFO
        << "Path planning done (avg). T[ms] = "
        << std::chrono::duration_cast<std::chrono::milliseconds>(duration / iterations).count();
 
    if (!path.size())
    {
        ARMARX_INFO << "Found no path!";
        return;
    }
 
    //draw the path
 
    std::stringstream lineName{};
    DrawColor color{1.f, 0.f, 0.f, 1.f};
 
    for (std::size_t i = 0; i < path.size() - 1; ++i)
    {
        lineName.str(layerName);
        lineName << "from_" << i << "_to_" << i + 1;
        debugDrawerPrx->setLineVisu(
            layerName, lineName.str(), path.at(i), path.at(i + 1), 5.f, color);
 
        //change color to make the first edge distinct
        if (!i)
        {
            color.r = 0.f;
            color.g = 1.f;
        }
    }
 
    ARMARX_INFO << "Found path whith " << path.size() - 1 << " edges";
}
 
void
armarx::AStarPathPlannerTestComponent::onExitComponent()
{
    debugDrawerPrx->clearLayer(layerName);
    debugDrawerPrx->clearDebugLayer();
}