8 #ifndef ECL_ODOMETRY_ODOMETRY_HELPER_HPP_ 9 #define ECL_ODOMETRY_ODOMETRY_HELPER_HPP_ 146 #if defined(ECL_CXX11_FOUND) 155 ecl_geometry_PUBLIC Odom2DTrajectoryPtr vectorToTrajectoryPtr(
const std::vector<Odom2D>& vec);
ecl_geometry_PUBLIC Twist2DVector getTwists(const Odom2DTrajectory &trajectory)
Extract twists of odom trajectory.
Embedded control libraries.
Eigen::Vector3f Twist2D
Float representation of velocities in 2D (v_x, v_y, w).
ecl_geometry_PUBLIC void setX(Odom2D &odom, const float &value)
Set x position.
ecl_geometry_PUBLIC Trajectory2D getPoses(const Odom2DTrajectory &trajectory)
Extract poses of odom trajectory.
ecl_geometry_PUBLIC Position2D getPosition(const Odom2D &odom)
Extract position from odometry.
ecl_geometry_PUBLIC void setVelocityAngular(Odom2D &odom, const float &value)
Set angular velocity.
ecl_geometry_PUBLIC void resize(Trajectory2D &trajectory, const int &size)
Resizes trajectory appending uninitialised values if needed.
Eigen::Matrix3Xf Trajectory2D
Float representation of a trajectory in 2D (poses in 2D).
ecl_geometry_PUBLIC float getY(const Odom2D &odom)
Get y position.
#define ecl_geometry_PUBLIC
Eigen::Matrix< float, 6, Eigen::Dynamic > Odom2DTrajectory
Float collection of 2D odometries (x, y, heading, v_x, v_y, w).
ecl_geometry_PUBLIC Pose2D getPose(const Odom2D &odom)
Extract pose from odometry.
ecl_geometry_PUBLIC float getX(const Odom2D &odom)
Get x position.
ecl_geometry_PUBLIC void setY(Odom2D &odom, const float &value)
Set y position.
ecl_geometry_PUBLIC bool empty(const Trajectory2D &trajectory)
Check if trajectory ptr is empty (ptr not set or has no poses)
ecl_geometry_PUBLIC Pose2D getBack(const Trajectory2D &trajectory)
Get back (last) element of trajectory.
Eigen::Vector2f Position2D
Float representation for a 2D position (x-, y-position).
Eigen::Matrix3Xf Twist2DVector
Float collection of 2D twists (twist: v_x, v_y, w).
Eigen::Matrix< float, 6, 1 > Odom2D
Float representation of 2D odometry (x, y, heading, v_x, v_y, w).
ecl_geometry_PUBLIC Trajectory2D vectorToTrajectory(const std::vector< Pose2D > &vec)
Convert vector of Pose2D to Trajectory2D.
ecl_geometry_PUBLIC float getYaw(const Odom2D &odom)
Get yaw (heading)
ecl_geometry_PUBLIC void setVelocityY(Odom2D &odom, const float &value)
Set linear velocity y direction.
ecl_geometry_PUBLIC double distanceSqared(const Odom2D &a, const Odom2D &b)
Squared distance between the positions of odometries.
Eigen::Vector3f Pose2D
Float representation of a pose in 2D (x, y, heading).
ecl_geometry_PUBLIC int size(const Trajectory2D &trajectory)
Get the size of the trajectory.
ecl_geometry_PUBLIC void setYaw(Odom2D &odom, const float &value)
Set yaw (heading)
ecl_geometry_PUBLIC void addAtEnd(Trajectory2D &target, const Trajectory2D &addition)
Concat two odometry trajectories.
ecl_geometry_PUBLIC float getVelocityX(const Odom2D &odom)
Get linear velocity x direction.
ecl_geometry_PUBLIC Pose2D getFront(const Trajectory2D &trajectory)
Get front (first) element of trajectory.
ecl_geometry_PUBLIC Pose2D getAt(const Trajectory2D &trajectory, const int &index)
Get element of trajectory.
ecl_geometry_PUBLIC void setAt(Trajectory2D &trajectory, const int &index, const Pose2D &pose)
Set element at index of trajectory.
ecl_geometry_PUBLIC float getVelocityAngular(const Odom2D &odom)
Get angular velocity.
ecl_geometry_PUBLIC float getVelocityY(const Odom2D &odom)
Get linear velocity y direction.
ecl_geometry_PUBLIC void setVelocityX(Odom2D &odom, const float &value)
Set linear velocity x direction.
ecl_geometry_PUBLIC double distance(const Pose2D &pose, const Trajectory2D &trajectory)
Shortest distance between a pose and a trajectory.
ecl_geometry_PUBLIC Twist2D getTwist(const Odom2D &odom)
Extract twist from odometry.