DynamicPlatformObstacleAvoidance.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    Armar6Skills::DynamicPlatformObstacleAvoidanceGroup
 * @author     Mirko Wächter ( mirko dot waechter at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "DynamicPlatformObstacleAvoidance.h"
using namespace armarx::DynamicPlatformObstacleAvoidanceGroup;
 
 
// Eigen
#include <Eigen/Geometry>
 
// Ice
#include <IceUtil/Time.h>
 
// Simox
#include <VirtualRobot/Nodes/RobotNode.h>
 
// ArmarX
#include <ArmarXCore/core/time/CycleUtil.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h>
 
 
// DO NOT EDIT NEXT LINE
DynamicPlatformObstacleAvoidance::SubClassRegistry
DynamicPlatformObstacleAvoidance::Registry(
    DynamicPlatformObstacleAvoidance::GetName(),
    &DynamicPlatformObstacleAvoidance::CreateInstance);
 
 
void
DynamicPlatformObstacleAvoidance::onEnter()
{
    // put your user code for the enter-point here
    // execution time should be short (<100ms)
}
 
 
void
DynamicPlatformObstacleAvoidance::run()
{
    ARMARX_VERBOSE << "Starting platform movement towards target position.";
 
    // Component proxies.
    //OpenPoseEstimationInterfacePrx openPoseEstimation = getOpenPoseEstimation();
    //HumanObstacleDetectionInterfacePrx humanObstacleDetection = getHumanObstacleDetection();
    // LaserScannerObstacleDetectionInterfacePrx laserScannerObstacleDetection = getLaserScannerObstacleDetection();
    // DynamicObstacleManagerInterfacePrx dynamicObstacleManager = getDynamicObstacleManager();
 
    PlatformUnitInterfacePrx platform = getPlatformUnit2();
    VirtualRobot::RobotPtr robot = getLocalRobot();
 
    // LEGACY, will be removed.
    if (in.isGoalNameSet())
    {
        ARMARX_WARNING << "Property `GoalName` with value `" << in.getGoalName()
                       << "` is deprecated and will be ignored! Unset this property and use `GoalPose` instead. ";
    }
 
    // Get parameters.
    const Eigen::Vector3f target_pose = in.getGoalPose()->toEigen();
    const std::vector<ObstacleAvoidingPlatformUnitHelper::Target> waypoints = [&]
    {
        std::vector<ObstacleAvoidingPlatformUnitHelper::Target> waypoints;
 
        if (in.isWaypointsPoseListSet())
        {
            const std::vector<armarx::Vector3Ptr> waypoints_eigen = in.getWaypointsPoseList();
            waypoints.reserve(waypoints_eigen.size());
            std::transform(waypoints_eigen.begin(), waypoints_eigen.end(),
                           std::back_inserter(waypoints),
                           [](const armarx::Vector3Ptr av)
            {
                const Eigen::Vector3f v = av->toEigen();
                return ObstacleAvoidingPlatformUnitHelper::Target{v.head<2>(), v[2]};
            });
        }
 
        return waypoints;
    }();
    const Eigen::Vector2f target_pos = target_pose.head<2>();
    const float target_ori = target_pose(2);
 
    ObstacleAvoidingPlatformUnitHelper::Config cfg;
    cfg.pos_reached_threshold = in.getPositionReachedThreshold();
    cfg.ori_reached_threshold = in.getAngleReachedThreshold();
    cfg.pos_near_threshold = in.getPositionNearThreshold();
    cfg.ori_near_threshold = in.getAngleNearThreshold();
 
    // enable obstacle detection
    //openPoseEstimation->start3DPoseEstimation();
    //humanObstacleDetection->enable();
    // laserScannerObstacleDetection->enable();
    // dynamicObstacleManager->wait_unitl_obstacles_are_ready();
 
    ObstacleAvoidingPlatformUnitHelper platform_ctrl{platform, robot, cfg};
    platform_ctrl.setMaxVelocities(in.getMaxVelocity(), in.getMaxAngularVelocity());
    platform_ctrl.setWaypoints(waypoints);
    platform_ctrl.addWaypoint(target_pos, target_ori);
 
    CycleUtil cycle{IceUtil::Time::milliSeconds(10)};
    while (not isRunningTaskStopped() and not platform_ctrl.isFinalTargetReached())
    {
        RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
        platform_ctrl.update();
        cycle.waitForCycleDuration();
    }
 
    ARMARX_VERBOSE << "Stopping platform now.";
    platform->stopPlatform();
 
    //openPoseEstimation->stop();
    //humanObstacleDetection->disable();
    // laserScannerObstacleDetection->disable();
 
    platform_ctrl.isFinalTargetReached() ? emitSuccess() : emitFailure();
}
 
 
void
DynamicPlatformObstacleAvoidance::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
    PlatformUnitInterfacePrx platform = getPlatformUnit2();
    platform->stopPlatform();
}
 
 
// DO NOT EDIT NEXT FUNCTION
armarx::XMLStateFactoryBasePtr
DynamicPlatformObstacleAvoidance::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new DynamicPlatformObstacleAvoidance(stateData));
}