Merge branch 'master' of gitlab.com:ArmarX/RobotAPI

source/RobotAPI/interface/CMakeLists.txt

@@ -72,7 +72,6 @@ set(SLICE_FILES
     units/RobotUnit/NJointCartesianWaypointController.ice
     units/RobotUnit/NJointCartesianNaturalPositionController.ice
     units/RobotUnit/RobotUnitInterface.ice
-    units/RobotUnit/GazeController.ice
 
     units/RobotUnit/NJointBimanualForceController.ice
     units/RobotUnit/NJointBimanualObjLevelController.ice

source/RobotAPI/interface/units/RobotUnit/GazeController.ice

-
-/*
-* 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    ArmarX::RobotAPI
-* @author     Raphael Grimm
-* @copyright  2019 Humanoids Group, H2T, KIT
-* @license    http://www.gnu.org/licenses/gpl-2.0.txt
-*             GNU General Public License
-*/
-
-#pragma once
-
-#include <RobotAPI/interface/core/FramedPoseBase.ice>
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
-
-module armarx { module control { module gaze_controller {
-
-    class GazeControllerConfig extends NJointControllerConfig
-    {
-        string cameraFrameName = "DepthCamera";
-        string yawNodeName = "Neck_1_Yaw";
-        string pitchNodeName = "Neck_2_Pitch";
-        string cameraNodeName = "DepthCamera";
-        string torsoNodeName = "TorsoJoint";
-
-        float Kp = 1.9f;
-        float Ki = 0.0f;
-        float Kd = 0.0f;
-        double maxControlValue = 1.0;
-        double maxDerivation = 0.5;
-
-        float yawAngleTolerance = 0.005;
-        float pitchAngleTolerance = 0.005;
-        bool abortIfUnreachable = false;
-    };
-
-    interface GazeControllerInterface extends
-        NJointControllerInterface
-    {
-        void submitTarget(FramedPositionBase target, long requestTimestamp);
-        void removeTarget();
-        void removeTargetAfter(long durationMilliSeconds);
-    };
-
-    interface GazeControllerListener
-    {
-        void reportGazeTarget(long requestTimestamp, long reachedTimestamp, long releasedTimestamp, long abortedTimestamp, FramedPositionBase target);
-    };
-
-};};};

source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp

@@ -11,6 +11,8 @@
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/time/Clock.h>
 
+#include "RobotAPI/libraries/armem_robot_state/common/localization/types.h"
+#include "RobotAPI/libraries/core/FramedPose.h"
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
 #include <RobotAPI/libraries/armem/core/Time.h>
 #include <RobotAPI/libraries/armem/core/error.h>
@@ -203,6 +205,31 @@ namespace armarx::armem::robot_state
             .timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
     }
 
+    std::optional<robot::RobotState::Pose>
+    RobotReader::queryOdometryPose(const robot::RobotDescription& description,
+                                   const armem::Time& timestamp) const
+    {
+
+        common::robot_state::localization::TransformQuery query
+        {
+            .header = {
+                .parentFrame = OdometryFrame,
+                .frame = "root",
+                .agent = description.name,
+                .timestamp = timestamp
+            }
+        };
+
+        const auto result = transformReader.lookupTransform(query);
+        if (not result)
+        {
+            return std::nullopt;
+        }
+
+        return result.transform.transform;
+    }
+
+
     std::optional<robot::RobotState::JointMap>
     RobotReader::queryJointState(const robot::RobotDescription& description,
                                  const armem::Time& timestamp) const // Why timestamp?!?!

source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h

@@ -24,9 +24,9 @@
 #include <mutex>
 #include <optional>
 
-#include <RobotAPI/libraries/armem/core/Time.h>
 #include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
 #include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
 #include <RobotAPI/libraries/armem_robot/client/interfaces.h>
 #include <RobotAPI/libraries/armem_robot/types.h>
 #include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
@@ -99,9 +99,22 @@ namespace armarx::armem::robot_state
                           const armem::Time& start,
                           const armem::Time& end) const;
 
+        /**
+         * @brief retrieve the robot's pose in the odometry frame. 
+         *
+         * This pose is an integration of the robot's platform velocity and undergoes a significant drift.
+         * 
+         * @param description 
+         * @param timestamp 
+         * @return std::optional<robot::RobotState::Pose> 
+         */
+        std::optional<robot::RobotState::Pose>
+        queryOdometryPose(const robot::RobotDescription& description,
+                          const armem::Time& timestamp) const;
+
     protected:
         // by default, no timeout mechanism
-        armem::Duration syncTimeout  = armem::Duration::MicroSeconds(0);
+        armem::Duration syncTimeout = armem::Duration::MicroSeconds(0);
         armem::Duration sleepAfterFailure = armem::Duration::MicroSeconds(0);
 
     private: