PolyTrajectoryFactory.h

Go to the documentation of this file.

#ifndef POLY_TRAJECTORY_FACTORY_H
#define POLY_TRAJECTORY_FACTORY_H
 
#include <trajectory/Trajectory6DOFLimits.h>
#include <trajectory/factory/TrajectoryFactoryInterface.h>
#include <utils/Transforms.h>
 
#include <bb_controls_msgs/action/locomotion.hpp>
#include <bb_controls_msgs/srv/spline_traj.hpp>
#include <memory>
#include <string>
#include <vector>
 
class PolyTrajectoryFactory : public TrajectoryFactoryInterface {
public:
    using Request    = bb_controls_msgs::srv::SplineTraj::Request;
    using Locomotion = bb_controls_msgs::action::Locomotion;
 
    PolyTrajectoryFactory(const Request& req, const State& origin, std::shared_ptr<Trajectory6DOFLimits> limits);
 
    PolyTrajectoryFactory(const Locomotion::Goal& goal, const State& origin,
                          std::shared_ptr<Trajectory6DOFLimits> limits);
 
    PolyTrajectoryFactory& with_origin_twist_threshold(double max_norm);
 
    PolyTrajectoryFactory& with_frame_id(const std::string& frame_id);
 
    PolyTrajectoryFactory& with_angle_unwrapping(bool needs_angle_unwrapping = true);
 
    std::unique_ptr<TrajectoryBase> build() override;
 
private:
    State origin_;
    std::shared_ptr<Trajectory6DOFLimits> limits_;
    std::vector<Pose> raw_goals_;
    double origin_twist_norm_threshold_ = 0.03;
    bool relative_xy_                   = false;
    bool relative_z_                    = false;
    bool relative_heading_              = false;
    bool interpolate_heading_           = false;
    bool needs_angle_unwrapping_        = true;
    std::string frame_id_               = "world_ned";
    bool is_valid_                      = true;
 
    std::vector<Pose> compute_absolute_goals() const;
 
    std::vector<Pose> add_heading_interpolation(const std::vector<Pose>& goals) const;
};
 
#endif  // POLY_TRAJECTORY_FACTORY_H