RobotUnitModuleRobotData.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    RobotAPI::ArmarXObjects::RobotUnit
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "RobotUnitModuleRobotData.h"
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <RobotAPI/libraries/core/RobotPool.h>
 
namespace armarx::RobotUnitModule
{
 
    RobotDataPropertyDefinitions::RobotDataPropertyDefinitions(std::string prefix) :
        ModuleBasePropertyDefinitions(prefix)
    {
        defineRequiredProperty<std::string>("RobotFileName",
                                            "Robot file name, e.g. robot_model.xml");
        defineOptionalProperty<std::string>("RobotFileNameProject",
                                            "",
                                            "Project in which the robot filename is located "
                                            "(if robot is loaded from an external project)");
 
        defineOptionalProperty<std::string>(
            "RobotName",
            "",
            "Override robot name if you want to load multiple robots of the same type");
        defineOptionalProperty<std::string>(
            "RobotNodeSetName",
            "Robot",
            "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
        defineOptionalProperty<std::string>(
            "PlatformName",
            "Platform",
            "Name of the platform needs to correspond to a node in the virtual robot.");
        defineOptionalProperty<bool>("PlatformAndLocalizationUnitsEnabled",
                                     true,
                                     "Enable or disable the platform and localization units.");
        defineOptionalProperty<std::string>("PlatformInstanceName",
                                            "Platform",
                                            "Name of the platform instance (will publish "
                                            "values on PlatformInstanceName + 'State')");
    }
 
    bool
    RobotData::arePlatformAndLocalizationUnitsEnabled() const
    {
        return _arePlatformAndLocalizationUnitsEnabled;
    }
 
    const std::string&
    RobotData::getRobotPlatformName() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_CHECK_EXPRESSION(robot)
            << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
        return robotPlatformName;
    }
 
    std::string
    RobotData::getRobotPlatformInstanceName() const
    {
        return robotPlatformInstanceName;
    }
 
    const std::string&
    RobotData::getRobotNodetSeName() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_CHECK_EXPRESSION(robot)
            << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
        return robotNodeSetName;
    }
 
    const std::string&
    RobotData::getRobotProjectName() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_CHECK_EXPRESSION(robot)
            << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
        return robotProjectName;
    }
 
    const std::string&
    RobotData::getRobotFileName() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        ARMARX_CHECK_EXPRESSION(robot)
            << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
        return robotFileName;
    }
 
    std::string
    RobotData::getRobotName() const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::lock_guard<std::mutex> guard{robotMutex};
        ARMARX_CHECK_EXPRESSION(robot)
            << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
        return robot->getName();
    }
 
    VirtualRobot::RobotPtr
    RobotData::cloneRobot(bool updateCollisionModel) const
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::lock_guard<std::mutex> guard{robotMutex};
        ARMARX_CHECK_EXPRESSION(robot)
            << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
        ARMARX_CHECK_EXPRESSION(robotPool);
        const VirtualRobot::RobotPtr clone = robotPool->getRobot();
        clone->setUpdateVisualization(false);
        clone->setUpdateCollisionModel(updateCollisionModel);
        return clone;
    }
 
    void
    RobotData::_initVirtualRobot()
    {
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::lock_guard<std::mutex> guard{robotMutex};
        ARMARX_CHECK_IS_NULL(robot);
 
        std::string robotName = getProperty<std::string>("RobotName").getValue();
 
        robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
        robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
        robotFileName = getProperty<std::string>("RobotFileName").getValue();
        robotPlatformName = getProperty<std::string>("PlatformName").getValue();
        _arePlatformAndLocalizationUnitsEnabled =
            getProperty<bool>("PlatformAndLocalizationUnitsEnabled").getValue();
        robotPlatformInstanceName = getProperty<std::string>("PlatformInstanceName").getValue();
 
        //load robot
        {
            Ice::StringSeq includePaths;
            if (!robotProjectName.empty())
            {
                CMakePackageFinder finder(robotProjectName);
                auto pathsString = finder.getDataDir();
                includePaths = simox::alg::split(pathsString, ";,");
                ArmarXDataPath::addDataPaths(includePaths);
            }
            if (!ArmarXDataPath::getAbsolutePath(robotFileName, robotFileName, includePaths))
            {
                std::stringstream str;
                str << "Could not find robot file " + robotFileName
                    << "\nCollected paths from RobotFileNameProject '" << robotProjectName
                    << "':" << includePaths;
                throw UserException(str.str());
            }
            // read the robot
            try
            {
                // We initialize the robot pool with this robot instance. This means, these robots will be used in different modules, i.e. for self collision checking and in the NJointControllers. Hence, we need to make sure that at least the collision model is available.
                robot =
                    VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eCollisionModel);
                if (robotName.empty())
                {
                    robotName = robot->getName();
                }
                else
                {
                    robot->setName(robotName);
                }
            }
            catch (VirtualRobot::VirtualRobotException& e)
            {
                throw UserException(e.what());
            }
            ARMARX_INFO << "Loaded robot:"
                        << "\n\tProject      : " << robotProjectName
                        << "\n\tName         : " << robotName
                        << "\n\tRobot file   : " << robotFileName
                        << "\n\tRobotNodeSet : " << robotNodeSetName << "\n\tNodeNames    : "
                        << robot->getRobotNodeSet(robotNodeSetName)->getNodeNames();
            ARMARX_CHECK_NOT_NULL(robot);
            robotPool.reset(new RobotPool(robot, 20));
        }
    }
 
} // namespace armarx::RobotUnitModule