PlatformSubUnit.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::PlatformSubUnit
 * @author     Raphael ( raphael dot grimm at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "PlatformSubUnit.h"
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
#include <VirtualRobot/MathTools.h>
 
#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
#include <RobotAPI/interface/core/GeometryBase.h>
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
namespace armarx
{
 
    void
    armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc,
                                    const JointAndNJointControllers&)
    {
        if (!getProxy())
        {
            //this unit is not initialized yet
            ARMARX_IMPORTANT << deactivateSpam(1) << "not initialized yet - skipping this update";
            return;
        }
        if (!listenerPrx)
        {
            ARMARX_INFO << deactivateSpam(1) << "listener is not set - skipping this update";
            return;
        }
        const SensorValueBase* sensorValue = sc.sensors.at(platformSensorIndex).get();
        ARMARX_CHECK_EXPRESSION(sensorValue);
        std::lock_guard<std::mutex> guard{dataMutex};
 
        const auto timestamp = sc.sensorValuesTimestamp.toMicroSeconds();
        ;
 
        // odom velocity is in local robot frame
        FrameHeader odomVelocityHeader;
        odomVelocityHeader.parentFrame = robotRootFrame;
        odomVelocityHeader.frame = robotRootFrame;
        odomVelocityHeader.agent = agentName;
        odomVelocityHeader.timestampInMicroSeconds = timestamp;
 
        // odom pose is in odom frame
        FrameHeader odomPoseHeader;
        odomPoseHeader.parentFrame = OdometryFrame;
        odomPoseHeader.frame = robotRootFrame;
        odomPoseHeader.agent = agentName;
        odomPoseHeader.timestampInMicroSeconds = timestamp;
 
        // odom velocity is in local robot frame
        FrameHeader globalPoseHeader;
        globalPoseHeader.parentFrame = GlobalFrame;
        globalPoseHeader.frame = robotRootFrame;
        globalPoseHeader.agent = agentName;
        globalPoseHeader.timestampInMicroSeconds = timestamp;
 
 
        //pos
        {
            ARMARX_CHECK_EXPRESSION(
                sensorValue->isA<SensorValueHolonomicPlatformRelativePosition>());
            const SensorValueHolonomicPlatformRelativePosition* s =
                sensorValue->asA<SensorValueHolonomicPlatformRelativePosition>();
 
            const auto odom_T_robot = VirtualRobot::MathTools::posrpy2eigen4f(
                s->relativePositionX, s->relativePositionY, 0, 0, 0, s->relativePositionRotation);
 
            // @@@ CHECK BELOW:
            // if (s->isA<SensorValueHolonomicPlatformAbsolutePosition>())
            // {
            //     const SensorValueHolonomicPlatformAbsolutePosition* sabs = s->asA<SensorValueHolonomicPlatformAbsolutePosition>();
            //     abs = positionCorrection * VirtualRobot::MathTools::posrpy2eigen4f(sabs->absolutePositionX, sabs->absolutePositionY, 0, 0, 0, sabs->absolutePositionRotation);
 
            //     TransformStamped currentPose;
            //     currentPose.header = globalPoseHeader;
            //     currentPose.transform = abs;
 
            //     globalPosePrx->reportGlobalRobotPose(currentPose);
 
            //     // legacy interfaces
            //     PlatformPose platformPose = toPlatformPose(currentPose);
            //     listenerPrx->reportPlatformPose(platformPose);
            //     globalPosCtrl->setGlobalPos(platformPose);
            // }
            // else
            // {
            //    global_T_robot = global_T_odom * odom_T_robot;
            // }
 
            // const auto global_T_robot = global_T_odom * odom_T_robot;
 
 
            const TransformStamped odomPose{.header = odomPoseHeader, .transform = odom_T_robot};
            odometryPrx->reportOdometryPose(odomPose);
 
            // legacy interface
            const auto odomLegacyPose = toPlatformPose(odomPose);
            listenerPrx->reportPlatformOdometryPose(
                odomLegacyPose.x, odomLegacyPose.y, odomLegacyPose.rotationAroundZ);
        }
 
        //vel
        {
            ARMARX_CHECK_EXPRESSION(sensorValue->isA<SensorValueHolonomicPlatformVelocity>());
            const SensorValueHolonomicPlatformVelocity* s =
                sensorValue->asA<SensorValueHolonomicPlatformVelocity>();
 
            TwistStamped odomVelocity;
            odomVelocity.header = odomVelocityHeader;
            odomVelocity.twist.linear << s->velocityX, s->velocityY, 0;
            odomVelocity.twist.angular << 0, 0, s->velocityRotation;
 
            odometryPrx->reportOdometryVelocity(odomVelocity);
 
            // legacy interface
            const auto& vel = odomVelocity.twist;
            listenerPrx->reportPlatformVelocity(vel.linear.x(), vel.linear.y(), vel.angular.z());
        }
    }
 
    void
    armarx::PlatformSubUnit::move(Ice::Float vx, Ice::Float vy, Ice::Float vr, const Ice::Current&)
    {
        //holding the mutex here could deadlock
        if (!pt->isControllerActive())
        {
            pt->activateController();
        }
        std::lock_guard<std::mutex> guard{dataMutex};
        pt->setVelocites(std::clamp(vx, -maxVLin, maxVLin),
                         std::clamp(vy, -maxVLin, maxVLin),
                         std::clamp(vr, -maxVAng, maxVAng));
    }
 
    void
    armarx::PlatformSubUnit::moveTo(Ice::Float rx,
                                    Ice::Float ry,
                                    Ice::Float rr,
                                    Ice::Float lac,
                                    Ice::Float rac,
                                    const Ice::Current&)
    {
        globalPosCtrl->setTarget(rx, ry, rr, lac, rac);
        if (!globalPosCtrl->isControllerActive())
        {
            globalPosCtrl->activateController();
        };
    }
 
    void
    armarx::PlatformSubUnit::moveRelative(Ice::Float rx,
                                          Ice::Float ry,
                                          Ice::Float rr,
                                          Ice::Float lac,
                                          Ice::Float rac,
                                          const Ice::Current&)
    {
        relativePosCtrl->setTarget(rx, ry, rr, lac, rac);
        if (!relativePosCtrl->isControllerActive())
        {
            relativePosCtrl->activateController();
        }
        //holding the mutex here could deadlock
        //    std::lock_guard<std::mutex> guard {dataMutex};
        ARMARX_INFO << "target orientation: " << rr;
    }
 
    void
    armarx::PlatformSubUnit::setMaxVelocities(Ice::Float mxVLin,
                                              Ice::Float mxVAng,
                                              const Ice::Current&)
    {
        std::lock_guard<std::mutex> guard{dataMutex};
        maxVLin = std::abs(mxVLin);
        maxVAng = std::abs(mxVAng);
    }
 
    // void armarx::PlatformSubUnit::setGlobalPose(armarx::PoseBasePtr globalPose, const Ice::Current&)
    // {
    //     std::lock_guard<std::mutex> guard {dataMutex};
    //     PosePtr p = PosePtr::dynamicCast(globalPose);
    //     global_T_odom = p->toEigen() * global_T_robot.inverse();
    // }
 
    // armarx::PoseBasePtr armarx::PlatformSubUnit::getGlobalPose(const Ice::Current&)
    // {
    //     std::lock_guard<std::mutex> guard {dataMutex};
    //     return new Pose {global_T_robot};
    // }
 
    void
    armarx::PlatformSubUnit::stopPlatform(const Ice::Current& c)
    {
        move(0, 0, 0);
    }
 
 
} // namespace armarx