serverTest.cpp

/*
 * 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    Navigation::ArmarXObjects::server
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */

#include "Navigation/libraries/client/NavigationStackConfig.h"
#include "Navigation/libraries/core/types.h"
#include "Navigation/libraries/factories/NavigationStackFactory.h"
#include "Navigation/libraries/global_planning/AStar.h"
#include "Navigation/libraries/global_planning/Point2Point.h"
#include "Navigation/libraries/server/NavigationStack.h"
#include "Navigation/libraries/server/Navigator.h"
#include "Navigation/libraries/trajectory_control/TrajectoryFollowingController.h"
#include <Navigation/libraries/server/execution/DummyExecutor.h>

#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::server

#define ARMARX_BOOST_TEST

#include <iostream>

#include <VirtualRobot/Robot.h>

#include <Navigation/Test.h>

using namespace armarx::nav;


BOOST_AUTO_TEST_CASE(testNavigator)
{

    const core::Pose goal = core::Pose::Identity();

    // create Navigator
    core::Scene scene;
    scene.robot = std::make_shared<VirtualRobot::LocalRobot>("foo", "bar");
    scene.robot->setGlobalPose(Eigen::Matrix4f::Identity(), false);

    // TODO create static shared ctor
    server::NavigationStack stack
    {
        .globalPlanner =
        std::make_shared<glob_plan::Point2Point>(glob_plan::Point2PointParams(), scene),
        .trajectoryControl = std::make_shared<traj_ctrl::TrajectoryFollowingController>(
            traj_ctrl::TrajectoryFollowingControllerParams(), scene)};

    // Executor.
    server::DummyExecturo executor{scene.robot};

    server::Navigator navigator(stack, scene, executor);
    navigator.moveTo(goal, core::NavigationFrames::Absolute);

    BOOST_CHECK_EQUAL(true, true);
}

/*
BOOST_AUTO_TEST_CASE(testNavigatorWithFactory)
{

    // create navigation stack config
    client::NavigationStackConfig cfg;
    cfg.configureGlobalPlanner(glob_plan::AStarParams());
    cfg.configureTrajectoryController(traj_ctrl::TrajectoryFollowingControllerParams());

    const auto stackConfig = cfg.toAron();
    const core::Pose goal = core::Pose::Identity();

    // here, we would send data over Ice ...

    // NavigatorPrx navigator;
    // navigator->moveTo(goal, stackConfig, core::NavigationFrame::Absolute);
    // navigator->moveTorwards(direction, stackConfig, core::NavigationFrame::Relative);

    // CLIENT
    ////////////////////////////////////////////////
    // SERVER

    // create Navigator
    core::Context ctx;
    server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, ctx);

    server::Navigator navigator(stack, ctx);
    navigator.moveTo(goal, core::NavigationFrames::Absolute);

    BOOST_CHECK_EQUAL(true, true);
}*/