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    control::ArmarXObjects::collision_avoidance_skill_provider
 * @author     Meixner ( andre dot meixner at kit dot edu )
 * @date       2025
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "Component.h"
 
// Include headers you only need in function definitions in the .cpp.
 
// #include <Eigen/Core>
 
// #include <SimoxUtility/color/Color.h>
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
// Skill libraries
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
 
#include <armarx/control/collision_avoidance/helpers/CollisionAvoidanceHelper.h>
#include <armarx/control/collision_avoidance/skills/CollisionAvoidance.h>
 
namespace armarx::control::components::collision_avoidance
{
 
    const std::string Component::defaultName = "collision_avoidance";
 
    Component::Component()
    {
        //
    }
 
    ::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)
        //def->required(properties.robotUnitName, "RobotUnitName", "Name of the robot unit.");
 
        def->required(
            properties.controllerNames,
            "p.controllerNames",
            "Names of controllers to send the collision objects to. E.g. \"col:right,col:left\"");
 
 
        // 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 optional property.
        // def->optional(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
        // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
 
        return def;
    }
 
    void
    Component::onInitComponent()
    {
        // Topics and properties defined above are automagically registered.
 
        // Keep debug observer data until calling `sendDebugObserverBatch()`.
        // (Requires the armarx::DebugObserverComponentPluginUser.)
        // setDebugObserverBatchModeEnabled(true);
    }
 
    void
    Component::onConnectComponent()
    {
        // Do things after connecting to topics and components.
 
        //skills::CollisionAvoidance::Properties properties;
        armarx::armem::robot_state::VirtualRobotReader robotStateReader;
        robotStateReader.connect(memoryNameSystem());
        ::armarx::control::client::ComponentPlugin* controlComponentPluginUser =
            &this->getControlComponentPlugin();
        robotUnit = controlComponentPluginUser->getRobotUnitPlugin().getRobotUnit();
 
        objectClassReader.connect(memoryNameSystem());
        objectInstanceReader.connect(memoryNameSystem());
        articulatedObjectReader.connect(memoryNameSystem());
 
        /* (Requires 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);
        */
 
        /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
        // 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;
    }
 
    std::vector<std::vector<std::string>>
    parseColumnConfig(const std::string& config)
    {
        std::vector<std::vector<std::string>> result;
 
        std::stringstream ss(config);
        std::string pair;
 
        while (std::getline(ss, pair, ','))
        {
            std::vector<std::string> parts;
            std::stringstream pairStream(pair);
            std::string part;
 
            while (std::getline(pairStream, part, ':'))
            {
                parts.push_back(part);
            }
            result.push_back(parts);
        }
        return result;
    }
 
    std::string
    toLower(const std::string& str)
    {
        std::string result = str;
        std::transform(result.begin(), result.end(), result.begin(), ::tolower);
        return result;
    }
 
    bool
    containsAll(std::string name, std::vector<std::string> list)
    {
        std::string lowerCaseName = toLower(name);
        return std::all_of(list.begin(),
                           list.end(),
                           [&lowerCaseName](const std::string& item)
                           {
                               std::string lowerCaseItem = toLower(item);
                               return lowerCaseName.find(lowerCaseItem) != std::string::npos;
                           });
    }
 
    void
    Component::sendCollisionObjectsToController(const Ice::StringSeq& objectIDs,
                                                const Ice::Current&)
    {
        auto names = robotUnit->getActivatedNJointControllerNames();
        // find the controllers containing "controllerNames" in their name
        auto controllerNamePatterns = parseColumnConfig(properties.controllerNames);
        std::vector<std::string> controllerNames;
 
        for (const auto& pattern : controllerNamePatterns)
        {
            bool controllerFound = false;
            for (const auto& name : names)
            {
                if (containsAll(name, pattern))
                {
                    controllerNames.push_back(name);
                    ARMARX_LOG << "found controller name: " << name;
                    controllerFound = true;
                    break;
                }
            }
            if (!controllerFound)
            {
                ARMARX_ERROR_S << "No matching controller for pattern " << pattern;
            }
        }
        if (controllerNames.size() != 0)
        {
            auto scene = control::collision_avoidance::helpers::getCollisionSceneFromMemory(
                objectIDs,
                objectClassReader,
                objectInstanceReader,
                articulatedObjectReader);
 
            for (const auto& name : controllerNames)
            {
                controller = Ice::checkedCast<armarx::control::TSColAvoidCtrlInterfacePrx>(
                    robotUnit->getNJointController(name));
 
                controller->updateCollisionObjects("CollisionAvoidanceSkill", scene.toAronDTO());
            }
        }
    }
 
    void
    Component::deleteCollisionObjects(const Ice::Current&)
    {
        auto names = robotUnit->getActivatedNJointControllerNames();
        // find the controllers containing "controllerNames" in their name
        auto controllerNamePatterns = parseColumnConfig(properties.controllerNames);
        std::vector<std::string> controllerNames;
 
        for (const auto& pattern : controllerNamePatterns)
        {
            bool controllerFound = false;
            for (const auto& name : names)
            {
                if (containsAll(name, pattern))
                {
                    controllerNames.push_back(name);
                    ARMARX_LOG << "found controller name: " << name;
                    controllerFound = true;
                    break;
                }
            }
            if (!controllerFound)
            {
                ARMARX_ERROR_S << "No matching controller for pattern " << pattern;
            }
        }
        if (controllerNames.size() != 0)
        {
            for (const auto& name : controllerNames)
            {
                controller = Ice::checkedCast<armarx::control::TSColAvoidCtrlInterfacePrx>(
                    robotUnit->getNJointController(name));
 
                controller->deleteCollisionObjects();
            }
        }
    }
 
    /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
    void
    Component::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        // Setup the widgets.
 
        tab.boxLayerName.setValue(properties.boxLayerName);
 
        tab.numBoxes.setValue(properties.numBoxes);
        tab.numBoxes.setRange(0, 100);
 
        tab.drawBoxes.setLabel("Draw Boxes");
 
        // Setup the layout.
 
        GridLayout grid;
        int row = 0;
        {
            grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
            ++row;
 
            grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
            ++row;
 
            grid.add(tab.drawBoxes, {row, 0}, {2, 1});
            ++row;
        }
 
        VBoxLayout root = {grid, VSpacer()};
        RemoteGui_createTab(getName(), root, &tab);
    }
 
 
    void
    Component::RemoteGui_update()
    {
        if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
        {
            std::scoped_lock lock(propertiesMutex);
            properties.boxLayerName = tab.boxLayerName.getValue();
            properties.numBoxes = tab.numBoxes.getValue();
 
            {
                setDebugObserverDatafield("numBoxes", properties.numBoxes);
                setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
                sendDebugObserverBatch();
            }
        }
        if (tab.drawBoxes.wasClicked())
        {
            // Lock shared variables in methods running in separate threads
            // and pass them to functions. This way, the called functions do
            // not need to think about locking.
            std::scoped_lock lock(propertiesMutex);
            drawBoxes(properties, arviz);
        }
    }
    */
 
 
    /* (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::control::components::collision_avoidance