RosMsgTrajectoryFactory.h

Go to the documentation of this file.

#ifndef ROS_MSG_TRAJECTORY_FACTORY_H
#define ROS_MSG_TRAJECTORY_FACTORY_H

#include <robodyn_controllers/TrajectoryFactory.h>
#include <robodyn_controllers/RosMsgTrajectory.h>

#include <boost/concept/assert.hpp>
#include <boost/concept_check.hpp>

#include "r2_msgs/ControllerJointSettings.h"
#include "r2_msgs/ControllerPoseSettings.h"
#include <sensor_msgs/JointState.h>
#include "r2_msgs/PoseState.h"

#include <stdexcept>
#include "nasa_common_logging/Logger.h"
#include "robodyn_utilities/RosMsgConverter.h"

class RosMsgJointTrajectoryFactory
{
public:
    RosMsgJointTrajectoryFactory() {}
    virtual ~RosMsgJointTrajectoryFactory() {}

    void setFactory(boost::shared_ptr<JointTrajectoryFactory<> > factory_in)
    {
        factory = factory_in;
        sequenceFactory.setTrajectoryFactory(factory);
        motionLimiter.setJointMotionLimiter(factory);
    }

    void setSettings(const r2_msgs::ControllerJointSettings& settings);
    void getSettings(r2_msgs::ControllerJointSettings& settings);
    void setLimits(const std::vector<std::string>& jointNames, double velLim, double accLim);
    void setPositionLimiter(boost::shared_ptr<JointNamePositionLimiter> posLimiter);

    virtual boost::shared_ptr<RosMsgJointTrajectory> getTrajectory(const sensor_msgs::JointState& jointPositions,
            const sensor_msgs::JointState& jointVels,
            const sensor_msgs::JointState& prevJointVels,
            const trajectory_msgs::JointTrajectory& goalTraj);

protected:
    typedef JointTrajectorySequenceFactory<> sequenceFactoryType;
    sequenceFactoryType sequenceFactory;
    boost::shared_ptr<JointTrajectoryFactory<> > factory;
    JointNameMotionLimiter motionLimiter;
    boost::shared_ptr<JointNamePositionLimiter> positionLimiter;
};

class RosMsgCartesianTrajectoryFactory
{
public:
    RosMsgCartesianTrajectoryFactory() {}
    virtual ~RosMsgCartesianTrajectoryFactory() {}

    void setFactory(boost::shared_ptr<CartesianTrajectoryFactory<SynchedTrajectory> > factory_in)
    {
        factory = factory_in;
        sequenceFactory.setTrajectoryFactory(factory);
    }

    void setSettings(const r2_msgs::ControllerPoseSettings& settings);

    virtual boost::shared_ptr<RosMsgSynchedCartesianTrajectory> getTrajectory(const r2_msgs::PoseState& poseState,
            const r2_msgs::PoseTrajectory& goalTraj);

protected:
    typedef CartesianTrajectorySequenceFactory<SynchedTrajectory> sequenceFactoryType;
    sequenceFactoryType sequenceFactory;
    boost::shared_ptr<CartesianTrajectoryFactory<SynchedTrajectory> > factory;
};

#endif