Component.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    navigation::ArmarXObjects::navigation_skill_provider
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2023
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "Component.h"
 
#include <experimental/memory>
#include <mutex>
#include <string>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
 
#include <RobotAPI/libraries/skills/core/SkillID.h>
 
#include <armarx/navigation/client/services/IceNavigatorFactory.h>
#include <armarx/navigation/core/NavigationStackGeneralConfig.h>
#include <armarx/navigation/safety_guard/LaserBasedProximity.h>
#include <armarx/navigation/skills/GuideHumanToRoom.h>
#include <armarx/navigation/skills/MoveRelativePlanar.h>
#include <armarx/navigation/skills/MoveXMeters.h>
#include <armarx/navigation/skills/NavigateTo.h>
#include <armarx/navigation/skills/NavigateToAlternatives.h>
#include <armarx/navigation/skills/NavigateToChargingStation.h>
#include <armarx/navigation/skills/NavigateToLocation.h>
#include <armarx/navigation/skills/NavigateToNamedLocation.h>
#include <armarx/navigation/skills/RotateTowards.h>
#include <armarx/navigation/skills/RotateTowardsLocation.h>
#include <armarx/navigation/skills/RotateXDegrees.h>
#include <armarx/navigation/skills/constants.h>
 
namespace armarx::navigation::components::navigation_skill_provider
{
    Component::Component()
    {
        addPlugin(virtualRobotReaderPlugin);
        addPlugin(costmapReaderPlugin);
        addPlugin(graphReaderPlugin);
        addPlugin(roomsReaderPlugin);
        addPlugin(objectClassReaderPlugin);
    }
 
    const std::string Component::defaultName = skills::constants::NavigationSkillProviderName;
 
    namespace
    {
        void
        defineProximityFieldParams(const armarx::PropertyDefinitionsPtr& def,
                                   const std::string& prefix,
                                   safety_guard::ProximityFieldParams& params)
        {
            def->optional(params.mode, prefix + ".mode", "The mode of the proximity field.")
                .map("Disabled", safety_guard::ProximityFieldParams::Mode::Disabled)
                .map("DirectionDependent",
                      safety_guard::ProximityFieldParams::Mode::DirectionDependent)
                .map("DirectionIndependent",
                      safety_guard::ProximityFieldParams::Mode::DirectionIndependent);
            def->optional(params.reduceVelocity,
                          prefix + ".reduceVelocity",
                          "Whether to first reduce the velocity when coming closer to an obstacle "
                          "before stopping.");
            def->optional(params.safetyDistance,
                          prefix + ".minDistance",
                          "The minimum distance, below which the robot will stop.");
            def->optional(
                params.influenceDistance,
                prefix + ".maxDistance",
                "The maximum distance, above which the robot will move with the maximum velocity.");
            def->optional(
                   params.k,
                   prefix + ".k",
                   "A parameter to adjust how the velocity is reduced when coming closer to "
                   "an obstacle. A higher value means the robot will reduce it's speed later.")
                .setMin(1);
            def->optional(
                   params.lambda,
                   prefix + ".lambda",
                   "A parameter to adjust how the velocity is reduced when coming closer to an "
                   "obstacle. A higher value means the robot will reduce it's speed earlier.")
                .setMin(1);
        }
    } // namespace
 
    armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        def->component(navigatorPrx, "navigator");
 
        def->required(properties.robotName, "RobotName", "Default robot name.");
 
        auto optionalSubSkillID =
            [&def](armarx::skills::SkillID& skillID, const std::string& nameBase)
        {
            ARMARX_CHECK(skillID.providerId.has_value());
            def->optional(skillID.providerId->providerName, nameBase + ".providerName");
            def->optional(skillID.skillName, nameBase + ".skillName");
        };
 
        def->required(properties.safetyGuardParams.robotRadius,
                      "p.navigateToLocation.safetyGuardParams.robotRadius",
                      "The robot radius used for distance calculation to the platform.");
        def->optional(properties.safetyGuardParams.enableHumans,
                      "p.navigateToLocation.safetyGuardParams.enableHumans",
                      "Whether to consider humans for the safety guard.");
        def->optional(properties.safetyGuardParams.enableLaserScanners,
                      "p.navigateToLocation.safetyGuardParams.enableLaserscanners",
                      "Whether to consider laser scanners for the safety guard.");
        defineProximityFieldParams(def,
                                   "p.navigateToLocation.safetyGuardParams.humanProximityField",
                                   properties.safetyGuardParams.humanProximityField);
        defineProximityFieldParams(
            def,
            "p.navigateToLocation.safetyGuardParams.laserScannerProximityField",
            properties.safetyGuardParams.laserScannerProximityField);
        def->optional(properties.safetyGuardParams.ignoreAttachedObjects,
                      "p.navigateToLocation.safetyGuardParams.ignoreAttachedObjects",
                      "Whether to ignore laser scanner features lying inside attached objects.");
        def->optional(properties.safetyGuardParams.enableLaserScannerFiltering,
                      "p.navigateToLocation.safetyGuardParams.enableLaserScannerFiltering",
                      "Whether to filter out small laser scanner features close to the robot.");
        def->optional(properties.safetyGuardParams.laserScannerFilteringThreshold,
                      "p.navigateToLocation.safetyGuardParams.laserScannerFilteringThreshold",
                      "Features with less or equal number of points are filtered out.");
        def->optional(properties.safetyGuardParams.laserScannerMaxFilteringDistance,
                      "p.navigateToLocation.safetyGuardParams.laserScannerMaxFilteringDistance",
                      "Only features where all points are within this distance of the robot are "
                      "filtered out [mm].");
 
 
        def->required(properties.generalConfig.maxVel.linear,
                      "p.default.generalConfig.maxVelocity.linear",
                      "The default maximum linear velocity.");
        def->required(properties.generalConfig.maxVel.angular,
                      "p.default.generalConfig.maxVelocity.angular",
                      "The default maximum angular velocity.");
 
        def->optional(
            properties.generalConfig.enableRampingStart,
            "p.default.generalConfig.enableRampingStart",
            "The default value for whether ramping is enabled at the start of the trajectory.");
        def->optional(
            properties.generalConfig.enableRampingEnd,
            "p.default.generalConfig.enableRampingEnd",
            "The default value for whether ramping is enabled at the end of the trajectory.");
        def->optional(
            properties.generalConfig.enableRampingCorners,
            "p.default.generalConfig.enableRampingCorners",
            "The default value for whether ramping is enabled in corners of the trajectory.");
        def->optional(properties.generalConfig.rampLength,
                      "p.default.generalConfig.rampLength",
                      "The default value for the distance over which ramping is applied [mm].");
        def->optional(properties.generalConfig.cornerVelocity,
                      "p.default.generalConfig.cornerVelocity",
                      "The default value for the velocity in a corner [mm/s].");
        def->optional(properties.generalConfig.boundaryVelocity,
                      "p.default.generalConfig.boundaryVelocity",
                      "The default value for the velocity at the ends [mm/s].");
        def->optional(
            properties.generalConfig.cornerLimit,
            "p.default.generalConfig.cornerLimit",
            "The default value for the angle above which a bend is considered a corner [deg].");
 
        // Keep the default that the skills have been shipping (150 mm). `core::GeneralConfig`'s
        // own default stays 100 mm, so the legacy-config path and the statecharts are unaffected.
        properties.generalConfig.inCollisionDistanceThresholdForRecovery = 150.0F;
        def->optional(properties.generalConfig.inCollisionDistanceThresholdForRecovery,
                      "p.default.generalConfig.inCollisionDistanceThresholdForRecovery",
                      "The default maximum distance [mm] to search for a collision-free recovery "
                      "position when the robot starts in collision. Skills can override this per "
                      "call via the parameter of the same name.");
 
        // The fleet-wide default. Deliberately set here rather than on `core::GeneralConfig`
        // itself: that struct's default stays `Ramping`, so the legacy-config compatibility
        // path, the statecharts and the simulated human are unaffected by this choice.
        //
        // A skill can override it per call; the navigator degrades to ramping when TOPP-RA is
        // unavailable or the path is too short to parametrize.
        properties.generalConfig.parametrization = core::TrajectoryParametrization::Toppra;
        def->optional(properties.generalConfig.parametrization,
                      "p.default.generalConfig.trajectoryParametrization",
                      "How the velocities along the planned path are assigned by default.")
            .map("None", core::TrajectoryParametrization::None)
            .map("Ramping", core::TrajectoryParametrization::Ramping)
            .map("Toppra", core::TrajectoryParametrization::Toppra);
 
 
        optionalSubSkillID(properties.navigateToNamedLocation.subSkillIDs.navigateToLocation,
                           "p.navigateToNamedLocation.subSkillIDs.navigateToLocation");
        optionalSubSkillID(properties.navigateToChargingStation.subSkillIDs.navigateToNamedLocation,
                           "p.navigateToChargingStation.subSkillIDs.navigateToNamedLocation");
 
        return def;
    }
 
    void
    Component::onInitComponent()
    {
        // Topics and properties defined above are automagically registered.
 
        // Keep debug observer data until calling `sendDebugObserverBatch()`.
        // (Requies the armarx::DebugObserverComponentPluginUser.)
        // setDebugObserverBatchModeEnabled(true);
    }
 
    void
    Component::onConnectComponent()
    {
        // Do things after connecting to topics and components.
 
        iceNavigatorFactory.emplace(navigatorPrx);
 
        skills::NavigatingSkillHelper::Services helperSrv{.iceNavigatorFactory =
                                                              iceNavigatorFactory.value(),
                                                          .memoryNameSystem = memoryNameSystem()};
        skills::NavigatingSkillHelper::Properties helperProperties{
            .safetyGuardParamsMutex = std::experimental::make_observer(&safetyGuardParamsMutex),
            .safetyGuardParams = std::experimental::make_observer(&properties.safetyGuardParams),
            .defaultGeneralConfig = properties.generalConfig};
 
        {
            addSkillFactory<skills::NavigateTo>(helperProperties, helperSrv);
        }
 
        {
            addSkillFactory<skills::NavigateToAlternatives>(helperProperties, helperSrv);
        }
 
        {
            addSkillFactory<skills::NavigateToLocation>(helperProperties, helperSrv);
        }
 
        {
            skills::NavigateToNamedLocation::Services services{
                .locationReader = &graphReaderPlugin->get(),
                .objectPoseClient = ObjectPoseClientPluginUser::getClient(),
                .objectClassReader = &objectClassReaderPlugin->get(),
            };
            addSkillFactory<skills::NavigateToNamedLocation>(properties.navigateToNamedLocation,
                                                             services);
        }
 
        {
            addSkillFactory<skills::NavigateToChargingStation>(
                properties.navigateToChargingStation);
        }
 
        {
            addSkillFactory<skills::MoveXMeters>(helperProperties, helperSrv);
        }
 
        {
            addSkillFactory<skills::RotateXDegrees>(helperProperties, helperSrv);
        }
 
        {
            skills::MoveRelativePlanar::Properties properties{
                .robotName = this->properties.robotName,
            };
            skills::MoveRelativePlanar::Services srv{
                .robotReader = virtualRobotReaderPlugin->get(),
            };
            addSkillFactory<skills::MoveRelativePlanar>(
                helperProperties, helperSrv, properties, srv);
        }
 
        {
            ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
            ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
            ARMARX_CHECK_NOT_NULL(roomsReaderPlugin);
 
            skills::GuideHumanToRoom::Services srv{.virtualRobotReader =
                                                       virtualRobotReaderPlugin->get(),
                                                   .costmapReader = costmapReaderPlugin->get(),
                                                   .roomsReader = roomsReaderPlugin->get(),
                                                   .arviz = arviz};
 
            addSkillFactory<skills::GuideHumanToRoom>(helperProperties, helperSrv, srv);
        }
 
        {
            skills::RotateTowards::Properties props{.robotName = properties.robotName};
            skills::RotateTowards::Services srv{.robotReader = virtualRobotReaderPlugin->get()};
            addSkillFactory<skills::RotateTowards>(helperProperties, helperSrv, props, srv);
        }
 
        {
            skills::RotateTowardsLocation::Properties props{};
            skills::RotateTowardsLocation::Services srv{.locationReader = graphReaderPlugin->get()};
            addSkillFactory<skills::RotateTowardsLocation>(props, srv);
        }
 
 
        /* (Requies the armarx::DebugObserverComponentPluginUser.)
        // Use the debug observer to log data over time.
        // The data can be viewed in the ObserverView and the LivePlotter.
        // (Before starting any threads, we don't need to lock mutexes.)
        {
            setDebugObserverDatafield("numBoxes", properties.numBoxes);
            setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
            sendDebugObserverBatch();
        }
        */
 
        /* (Requires the armarx::ArVizComponentPluginUser.)
        // Draw boxes in ArViz.
        // (Before starting any threads, we don't need to lock mutexes.)
        drawBoxes(properties, arviz);
        */
 
        // Setup the remote GUI.
        {
            createRemoteGuiTab();
            RemoteGui_startRunningTask();
        }
    }
 
    void
    Component::onDisconnectComponent()
    {
    }
 
    void
    Component::onExitComponent()
    {
    }
 
    std::string
    Component::getDefaultName() const
    {
        return Component::defaultName;
    }
 
    std::string
    Component::GetDefaultName()
    {
        return Component::defaultName;
    }
 
    void
    Component::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        VBoxLayout root;
 
        // thread-safe access to params
        const auto safetyGuardParams = [this]()
        {
            std::lock_guard g{safetyGuardParamsMutex};
            return properties.safetyGuardParams;
        }();
 
        const auto setSpinBox = [](armarx::RemoteGui::Client::FloatSpinBox& w,
                                   float min,
                                   float max,
                                   int decimals,
                                   float step)
        {
            w.setRange(min, max);
            w.setDecimals(decimals);
            w.setSteps(static_cast<int>((max - min) / step));
        };
 
        {
            GridLayout grid;
            int row = 0;
            grid.add(Label("Enable Humans"), {.row = row, .column = 0})
                .add(tab.enableHumans, {.row = row++, .column = 1});
            grid.add(Label("Enable Laserscanner"), {.row = row, .column = 0})
                .add(tab.enableLaserScanners, {.row = row++, .column = 1});
            root.addChild(grid);
 
            tab.enableHumans.setValue(safetyGuardParams.enableHumans);
            tab.enableLaserScanners.setValue(safetyGuardParams.enableLaserScanners);
        }
        root.addChild(VSpacer());
 
        const auto setupProximityFieldGui =
            [&root, &setSpinBox](RemoteGuiTab::ProximityField& field,
                                 const safety_guard::ProximityFieldParams& values,
                                 const std::string& label)
        {
            GridLayout grid;
            int row = 0;
            grid.add(Label("Reduce speed"), {.row = row, .column = 0})
                .add(field.reduceSpeed, {.row = row++, .column = 1});
            grid.add(Label("Min distance"), {.row = row, .column = 0})
                .add(field.minDistance, {.row = row++, .column = 1});
            grid.add(armarx::RemoteGui::Client::VSpacer(), {.row = row++, .column = 0});
            grid.add(Label("Max distance"), {.row = row, .column = 0})
                .add(field.maxDistance, {.row = row++, .column = 1});
            grid.add(Label("k"), {.row = row, .column = 0})
                .add(field.k, {.row = row++, .column = 1});
            grid.add(Label("lambda"), {.row = row, .column = 0})
                .add(field.lambda, {.row = row++, .column = 1});
 
            GroupBox box({grid});
            box.setLabel(label);
            root.addChild(box);
 
            field.reduceSpeed.setValue(values.reduceVelocity);
            field.minDistance.setValue(values.safetyDistance);
            setSpinBox(field.minDistance, 0, 5000, 1, 1);
            field.maxDistance.setValue(values.influenceDistance);
            setSpinBox(field.maxDistance, 0, 10000, 1, 1);
            field.k.setValue(values.k);
            setSpinBox(field.k, 1, 20, 2, 0.1);
            field.lambda.setValue(values.lambda);
            setSpinBox(field.lambda, 1, 20, 2, 0.1);
        };
 
        setupProximityFieldGui(tab.humanProximityField,
                               safetyGuardParams.humanProximityField,
                               "Human proximity field");
        root.addChild(VSpacer());
        setupProximityFieldGui(tab.laserScannerProximityField,
                               safetyGuardParams.laserScannerProximityField,
                               "LaserScanner proximity field");
 
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    Component::RemoteGui_update()
    {
        std::lock_guard g{safetyGuardParamsMutex};
 
        auto& safetyGuardParams = properties.safetyGuardParams;
 
        safetyGuardParams.enableHumans = tab.enableHumans.getValue();
        safetyGuardParams.enableLaserScanners = tab.enableLaserScanners.getValue();
 
        const auto readProximityField = [&](const RemoteGuiTab::ProximityField& field,
                                            safety_guard::ProximityFieldParams& values)
        {
            values.reduceVelocity = field.reduceSpeed.getValue();
            values.safetyDistance = field.minDistance.getValue();
            values.influenceDistance = field.maxDistance.getValue();
            values.k = field.k.getValue();
            values.lambda = field.lambda.getValue();
        };
 
        readProximityField(tab.humanProximityField, safetyGuardParams.humanProximityField);
        readProximityField(tab.laserScannerProximityField,
                           safetyGuardParams.laserScannerProximityField);
    }
 
    /* (Requires the armarx::ArVizComponentPluginUser.)
    void
    Component::drawBoxes(const Component::Properties& p, viz::Client& arviz)
    {
        // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
        // See the ArVizExample in RobotAPI for more examples.
 
        viz::Layer layer = arviz.layer(p.boxLayerName);
        for (int i = 0; i < p.numBoxes; ++i)
        {
            layer.add(viz::Box("box_" + std::to_string(i))
                      .position(Eigen::Vector3f(i * 100, 0, 0))
                      .size(20).color(simox::Color::blue()));
        }
        arviz.commit(layer);
    }
    */
 
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
} // namespace armarx::navigation::components::navigation_skill_provider