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    mobile_manipulation::ArmarXObjects::handover_visualization
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "Component.h"
 
#include <memory>
#include <string>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <range/v3/algorithm/find.hpp>
#include <range/v3/algorithm/find_if.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
 
#include <VirtualRobot/SceneObjectSet.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/ScopedStopWatch.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/ArmarXObjects/util.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/util.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/memory/client/human/Reader.h>
#include <armarx/navigation/util/util.h>
 
#include <VisionX/libraries/armem_human/client/HumanPoseReader.h>
#include <VisionX/libraries/armem_human/client/PersonInstanceReader.h>
#include <VisionX/libraries/armem_human/util.h>
 
#include "SocialCostmapBuilder.h"
 
namespace armarx::navigation::components::social_cost_map
{
    Component::Component()
    {
        addPlugin(humanPoseReaderPlugin);
        addPlugin(humanPersonInstanceReaderPlugin);
        addPlugin(navigationHumanPoseReaderPlugin);
        addPlugin(costmapWriterPlugin);
        addPlugin(navigationRoomReaderPlugin);
    }
 
    const std::string Component::defaultName = "social_cost_map";
 
    armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        // Publish to a topic (passing the TopicListenerPrx).
        // def->topic(myTopicListener);
 
        // Subscribe to a topic (passing the topic name).
        // def->topic<PlatformUnitListener>("MyTopic");
 
        // Use (and depend on) another component (passing the ComponentInterfacePrx).
        // def->component(myComponentProxy)
 
 
        // Add a required property. (The component won't start without a value being set.)
        // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
 
        // Add an optionalproperty.
        def->optional(properties.robotName, "p.robotName", "");
        def->optional(properties.human.providerName, "p.human.providerName", "");
        def->optional(properties.human.naviProviderName, "p.human.naviProviderName", "");
 
        def->optional(
            properties.taskPeriodMs, "p.taskPeriodMs", "Period in ms for updating the costmap.");
        def->optional(properties.enableHumanPoseReader,
                      "p.enableHumanPoseReader",
                      "Enable human pose reader");
        def->optional(properties.enableHumanPersonInstanceReader,
                      "p.enableHumanPersonInstanceReader",
                      "Enable human person instance reader");
        def->optional(properties.enableNavigationHumanPoseReader,
                      "p.enableNavigationHumanPoseReader",
                      "Enable navigation human pose reader");
        def->optional(properties.restrictToRooms,
                      "p.restrictToRooms",
                      "Restrict considered objects to those inside the given rooms.");
        def->optional(properties.roomNames,
                      "p.roomNames",
                      "Comma separated list of room names to restrict considered objects to.");
 
        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()
    {
        setDebugObserverBatchModeEnabled(true);
 
        const auto timestamp = armarx::Clock::Now();
 
        ARMARX_TRACE;
 
        const auto objectPoseClient = ObjectPoseClientPluginUser::getClient();
        const armarx::objpose::ObjectPoseSeq objectPoses = objectPoseClient.fetchObjectPoses();
 
        std::vector<armarx::navigation::algorithms::Room> rooms;
        if (properties.restrictToRooms)
        {
            const std::vector<std::string> roomNames =
                simox::alg::split(properties.roomNames, ",", true);
 
            {
                const armarx::navigation::memory::client::rooms::Reader::Query query{
                    .providerName = std::nullopt, .timestamp = timestamp};
 
                const auto result = navigationRoomReaderPlugin->get().query(query);
 
                if (not result)
                {
                    ARMARX_INFO << "Failed to obtain rooms from navigation memory. Reason: "
                                << result.errorMessage;
                }
                else
                {
                    for (const auto& roomName : roomNames)
                    {
                        auto it = ranges::find(
                            result.rooms, roomName, &armarx::navigation::algorithms::Room::name);
 
                        if (it != result.rooms.end())
                        {
                            rooms.push_back(*it);
                        }
                        else
                        {
                            ARMARX_WARNING << "Room " << QUOTED(roomName)
                                           << " not found in navigation memory.";
                        }
                    }
                    ARMARX_INFO << "Found " << rooms.size() << " rooms in navigation memory.";
                }
            }
        }
 
 
        auto objectsAll = armarx::objpose::util::asSceneObjects(
            objectPoses, VirtualRobot::ObjectIO::ObjectDescription::eCollisionModel);
 
        // reset
        objects = std::make_shared<VirtualRobot::SceneObjectSet>();
 
        for (const auto& object : objectsAll->getSceneObjects())
        {
            const Eigen::Vector3f global_P_obj = object->getGlobalPosition();
 
            if (not object->getCollisionModel())
            {
                ARMARX_DEBUG << "Object " << QUOTED(object->getName())
                             << " has no collision model. Skipping.";
                continue;
            }
 
            if (not properties.restrictToRooms)
            {
                objects->addSceneObject(object);
                continue;
            }
 
            for (const auto& room : rooms)
            {
                if (room.isInside(global_P_obj, true))
                {
                    objects->addSceneObject(object);
                    break;
                }
            }
        }
        ARMARX_INFO << "Number of objects considered for costmap: "
                    << objects->getSceneObjects().size();
 
        ARMARX_INFO << "Creating task";
        periodicTask = new armarx::PeriodicTask<Component>(
            this, &Component::visualizeOnce, properties.taskPeriodMs, "runningTask");
 
        ARMARX_TRACE;
 
        ARMARX_INFO << "Starting control thread";
        periodicTask->start();
    }
 
    void
    Component::onDisconnectComponent()
    {
    }
 
    void
    Component::onExitComponent()
    {
    }
 
    void
    Component::visualizeOnce()
    {
        const auto debugObserverLogCb =
            [this](const std::string& name, const armarx::Duration& duration)
        {
            ARMARX_DEBUG << name << " " << duration;
            setDebugObserverDatafield("timing." + name + " [ms]", duration.toMilliSeconds());
        };
        {};
 
        const auto makeSwCb = [&](const std::string& name)
        { return [=](const armarx::Duration& duration) { debugObserverLogCb(name, duration); }; };
 
 
        // will be populated with human positions from multiple sources
        std::vector<Eigen::Vector3f> humanPositions;
 
        const auto timestamp = armarx::Clock::Now();
 
        if (properties.enableHumanPoseReader)
        {
            const armarx::armem::human::client::Reader::Query query{
                .providerName = properties.human.providerName,
                .timestamp = timestamp,
                .maxAge = armarx::Duration::MilliSeconds(2000)};
 
            const auto humanPoseResult = humanPoseReaderPlugin->get().query(query);
 
            if (not humanPoseResult)
            {
                ARMARX_VERBOSE << "Failed to obtain humans from memory. Reason: "
                               << humanPoseResult.errorMessage;
            }
            else
            {
 
                for (const auto& humanPose : humanPoseResult.humanPoses)
                {
                    if (const auto meanPos = armarx::armem::human::computeMeanPosition(
                            humanPose, armem::human::KeyPointCoordinateSystem::Global))
                    {
                        humanPositions.push_back(meanPos.value());
                    }
                }
 
                ARMARX_INFO << "Found " << humanPoseResult.humanPoses.size()
                            << " humans in human pose memory.";
            }
        }
 
        // get humans from human person instance memory
        if(properties.enableHumanPersonInstanceReader)
        {
            const armarx::armem::human::client::PersonInstanceReader::Query query{
                .providerName = "", // all providers
                .timestamp = timestamp,
                .maxAge = armarx::Duration::MilliSeconds(2000)};
 
            const auto result = humanPersonInstanceReaderPlugin->get().query(query);
 
            if (not result)
            {
                ARMARX_INFO << "Failed to obtain human person instances from memory. Reason: "
                            << result.errorMessage;
            }
            else
            {
 
                ARMARX_INFO << "Found " << result.personInstances.size()
                            << " human persons in human person instance memory.";
 
                for (const auto& person : result.personInstances)
                {
                    humanPositions.emplace_back(person.globalPose.translation());
                }
            }
        }
 
        if (properties.enableNavigationHumanPoseReader)
        {
 
            const armarx::navigation::memory::client::human::Reader::Query queryNavigationHumans{
                .providerName = properties.human.naviProviderName,
                .timestamp = timestamp,
                .maxAge = armarx::Duration::MilliSeconds(2000)};
 
            const auto resultNavi =
                navigationHumanPoseReaderPlugin->get().queryHumans(queryNavigationHumans);
 
            if (not resultNavi)
            {
                ARMARX_INFO << "Failed to obtain humans from navigation memory. Reason: "
                            << resultNavi.errorMessage;
            }
 
 
            if (resultNavi.humans.empty())
            {
                ARMARX_DEBUG << "No humans in navigation memory.";
            }
 
            ARMARX_INFO << "Found " << resultNavi.humans.size() << " humans in navigation memory.";
 
            for (const auto& human : resultNavi.humans)
            {
                humanPositions.emplace_back(conv::to3D(human.pose.translation()));
            }
        }
 
        ARMARX_INFO << "Total number of humans considered: " << humanPositions.size();
 
 
        /*if (result.humanPoses.empty())
        {
            ARMARX_VERBOSE << "No humans";
            return;
        }*/
 
 
        SocialCostmapBuilder socialCostmapBuilder(
            armarx::navigation::algorithms::computeSceneBounds(objects, {}),
            armarx::navigation::algorithms::Costmap::Parameters{.binaryGrid = false,
                                                                .cellSize = 100});
 
 
        // prepare humans for costmap building
        const std::vector<navigation::core::Pose2D> humanPoses =
            humanPositions |
            ranges::views::transform(
                [](const Eigen::Vector3f& humanPosition)
                {
                    return Eigen::Isometry2f{
                        Eigen::Translation2f(humanPosition.x(), humanPosition.y())};
                }) |
            ranges::to_vector;
 
 
        {
            armarx::core::time::ScopedStopWatch sw(makeSwCb("platform"));
 
            auto costmapPlatform =
                socialCostmapBuilder.create(humanPoses, SocialCostmapBuilder::Mode::Mode1);
            //ARMARX_INFO << "platform: vmin=" << costmapPlatform.getGrid().minCoeff() << " vmax=" << costmapPlatform.getGrid().maxCoeff();
 
            const float intimateZoneCostTh = 0.6f;
 
            // cut out the high values
            const Eigen::Matrix<bool, -1, -1> mask =
                (costmapPlatform.getMutableGrid().array() < intimateZoneCostTh);
            costmapPlatform.getMutableMask().emplace(mask);
 
            armarx::core::time::ScopedStopWatch sw2(makeSwCb("platform_storing"));
 
            costmapWriterPlugin->get().store(
                costmapPlatform, "social_costmap_platform", getName(), timestamp);
 
            //const auto x = costmapPlatform.getGrid().array().max();
        }
 
        sendDebugObserverBatch();
    }
 
    std::string
    Component::getDefaultName() const
    {
        return Component::defaultName;
    }
 
    std::string
    Component::GetDefaultName()
    {
        return Component::defaultName;
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
} // namespace armarx::navigation::components::social_cost_map