ros_conversions.cpp

Go to the documentation of this file.

#include "ros_conversions.h"
 
#include <math.h>
 
#include <cmath>
#include <optional>
 
#include <boost/none.hpp>
#include <boost/optional/optional.hpp>
#include <boost/smart_ptr/make_shared_object.hpp>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/human/shapes.h>
#include <armarx/navigation/human/types.h>
 
#include <geometry_msgs/Twist.h>
#include <range/v3/view/drop.hpp>
#include <range/v3/view/zip.hpp>
#include <stuff/misc.h>
#include <teb_local_planner/costmap.h>
#include <teb_local_planner/penalty_models.h>
#include <teb_local_planner/pose_se2.h>
#include <teb_local_planner/timed_elastic_band.h>
 
namespace armarx::navigation::conv
{
    Eigen::Vector2d
    toRos2D(const Eigen::Vector2f& vec)
    {
        return vec.cast<double>() / 1000; // [mm] to [m]
    }
 
    Eigen::Vector2d
    toRos(const Eigen::Vector3f& vec)
    {
        return conv::to2D(vec).cast<double>() / 1000; // [mm] to [m]
    }
 
    Eigen::Vector3f
    fromRos(const Eigen::Vector2d& vec)
    {
        return conv::to3D(vec.cast<float>()) * 1000; // [m] to [mm]
    }
 
    teb_local_planner::PoseSE2
    toRos(const core::Pose& pose)
    {
        core::Pose2D pose2D = to2D(pose);
        return toRos(pose2D);
    }
 
    teb_local_planner::PoseSE2
    toRos(const core::Pose2D& pose)
    {
        Eigen::Vector2d pos = pose.translation().cast<double>();
        double theta = Eigen::Rotation2Df(pose.linear()).angle();
 
        // we change it such that x is pointing forward.
        // ROS: x pointing forward, ArmarX: y pointing forward
        theta += M_PI_2;
        // normalize angle
        theta = g2o::normalize_theta(theta);
 
        return {pos / 1000., theta}; // [mm] to [m]
    }
 
    core::Pose
    fromRos(const teb_local_planner::PoseSE2& pose)
    {
        Eigen::Vector2d pos = pose.position() * 1000; // [m] to [mm]
        double theta = pose.theta();
 
        // we change it such that x is pointing forward.
        // ROS: x pointing forward, ArmarX: y pointing forward
        theta -= M_PI_2;
 
 
        core::Pose2D ret;
        ret.translation() = pos.cast<float>();
        ret.linear() = Eigen::Rotation2Df(theta).toRotationMatrix();
 
        return to3D(ret);
    }
 
    geometry_msgs::Twist
    toRos(const core::Twist& velocity)
    {
        geometry_msgs::Twist ret;
 
        // we change it such that x is pointing forward.
        // ROS: x pointing forward, ArmarX: y pointing forward
 
        ret.linear.x = velocity.linear.y() / 1000; // [mm] to [m]
        ret.linear.y = -velocity.linear.x() / 1000; // [mm] to [m]
        ret.angular.z = velocity.angular.z();
 
        return ret;
    }
 
    teb_local_planner::TimedElasticBand
    toRos(const core::GlobalTrajectory& trajectory)
    {
        teb_local_planner::TimedElasticBand teb;
 
        bool first = true;
        teb_local_planner::PoseSE2 lastPose;
        for (const core::GlobalTrajectoryPoint& point : trajectory.points())
        {
            teb_local_planner::PoseSE2 pose = toRos(point.waypoint.pose);
            teb.addPose(pose);
 
            if (!first)
            {
                Eigen::Vector2d distance = pose.position() - lastPose.position();
                double timeDiff = distance.norm() / (point.velocity / 1000); // [mm/s] -> [m/s]
                teb.addTimeDiff(timeDiff);
 
                first = false;
            }
 
            lastPose = pose;
        }
 
        return teb;
    }
 
    core::LocalTrajectory
    fromRos(const teb_local_planner::TimedElasticBand& teb, DateTime timestamp)
    {
        const auto& tebPoses = teb.poses();
 
        if (tebPoses.empty())
        {
            return {};
        }
 
        core::LocalTrajectoryPoints trajectory;
        trajectory.reserve(tebPoses.size());
 
        // TODO this timestamp should be given via the argument list
        //DateTime timestamp = Clock::Now();
 
        ARMARX_CHECK_EQUAL(tebPoses.size() - 1, teb.timediffs().size());
 
        // add the first pose at this timestamp
        trajectory.push_back(core::LocalTrajectoryPoint{
            .timestamp = timestamp, .pose = {fromRos(tebPoses.front()->pose())}});
 
        // all consecutive poses
        for (const auto& [poseVertex, timediff] :
             ranges::views::zip(tebPoses | ranges::views::drop(1), teb.timediffs()))
        {
            const core::Pose pose = fromRos(poseVertex->pose());
            const Duration dt = Duration::SecondsDouble(timediff->dt());
 
            timestamp += dt;
            trajectory.push_back(
                core::LocalTrajectoryPoint{.timestamp = timestamp, .pose = {pose}});
        }
 
        ARMARX_CHECK_EQUAL(trajectory.size(), tebPoses.size());
 
        return {trajectory};
    }
 
    human::shapes::Ellipse
    toRos(const human::shapes::Ellipse& ellipse)
    {
        // a,b parameter are along the x and y axis respectively
        // ROS and ArmarX have the x axis pointing in a different direction
        // ROS: x pointing forward, ArmarX: y pointing forward
        return {.a = ellipse.b / 1000, .b = ellipse.a / 1000}; // [mm] to [m]
    }
 
    teb_local_planner::Costmap
    toRos(const algorithms::Costmap& costmap)
    {
        const algorithms::Costmap::Parameters& params = costmap.params();
        const teb_local_planner::Costmap::Parameters teb_params = {
            .binaryGrid = params.binaryGrid, .cellSize = params.cellSize / 1000}; // [mm] to [m]
 
        const algorithms::SceneBounds& bounds = costmap.getLocalSceneBounds();
        const teb_local_planner::Costmap::SceneBounds teb_bounds = {
            .min = toRos2D(bounds.min).cast<float>(), .max = toRos2D(bounds.max).cast<float>()};
 
        const std::optional<algorithms::Costmap::Mask>& mask = costmap.getMask();
        boost::optional<teb_local_planner::Costmap::Mask> teb_mask = boost::none;
        if (mask)
        {
            teb_mask = mask.value();
        }
 
        teb_local_planner::Costmap::Pose2D teb_origin = costmap.origin();
        teb_origin.translation() /= 1000; // [mm] to[m]
 
        algorithms::Costmap::Grid teb_grid = costmap.getGrid();
        teb_grid /= 1000; // [mm] to [m]
 
        return teb_local_planner::Costmap{teb_grid, teb_params, teb_bounds, teb_mask, teb_origin};
    }
 
    teb_local_planner::PenaltyModelPtr
    toRos(const human::LinearPenaltyModel& model)
    {
        return boost::make_shared<teb_local_planner::LinearPenaltyModel>(
            teb_local_planner::LinearPenaltyModel(model.minDistance, model.epsilon));
    }
 
    teb_local_planner::PenaltyModelPtr
    toRos(const human::ExponentialPenaltyModel& model)
    {
        return boost::make_shared<teb_local_planner::ExponentialPenaltyModel>(
            teb_local_planner::LinearPenaltyModel(model.minDistance, model.epsilon),
            model.exponent);
    }
 
} // namespace armarx::navigation::conv