TrajectoryMonitor.h

Go to the documentation of this file.

#ifndef TRAJECTORYMONITOR_H
#define TRAJECTORYMONITOR_H

#include "robodyn_utilities/GoalStatusSender.h"
#include <sensor_msgs/JointState.h>
#include <r2_msgs/JointTrajectoryReplan.h>
#include "r2_msgs/PoseTrajectoryReplan.h"
#include "r2_msgs/PoseState.h"
#include "r2_msgs/StringArray.h"
#include "r2_msgs/JointControlDataArray.h"
#include "r2_msgs/ReplanType.h"
#include "r2_msgs/JointCommand.h"
#include "r2_msgs/JointCapability.h"
#include "r2_msgs/TrajectoryMonitorFactors.h"
#include "nasa_common_logging/Logger.h"
#include "robodyn_utilities/RosMsgConverter.h"
#include "robot_instance/StringUtilities.h"
#include <tf_conversions/tf_kdl.h>

class TrajectoryMonitor : public GoalStatusSender
{
public:
    TrajectoryMonitor(const std::string& name);
    virtual ~TrajectoryMonitor();

    void setBases(const r2_msgs::StringArray& basesMsg);

    void setFactors(const r2_msgs::TrajectoryMonitorFactors& factorsMsg);

    void evaluateReplanTrigger(const sensor_msgs::JointState& jointStatesMsg, const r2_msgs::JointCommand& jointCommandMsg,
                               const r2_msgs::JointControlDataArray& jointStatusMsg, const r2_msgs::JointCapability& jointCapMsg);

    void replan();

    void updateMonitor(const r2_msgs::JointCommand& jointCommandMsg,
                       const r2_msgs::PoseState& poseCommandsMsg);
    void updateGoals(const actionlib_msgs::GoalStatusArray& goalStatuses);

    void addCartesianWaypoints(r2_msgs::PoseTrajectory trajectory, const r2_msgs::PoseState& poseStatesMsg);
    void addJointWaypoints(const trajectory_msgs::JointTrajectory& trajectory,
                           const std::vector<r2_msgs::ReplanType>& replan = std::vector<r2_msgs::ReplanType>());

protected:
    virtual void writeCartesianWaypoints(const r2_msgs::PoseTrajectoryReplan& trajectory) = 0;
    virtual void writeJointWaypoints(const r2_msgs::JointTrajectoryReplan& trajectory) = 0;
    virtual void writeBases(const r2_msgs::StringArray& basesMsg) = 0;

    struct TrajectoryMonitorFactors
    {
        double torqueLimitFactor;
        double distanceFactor;
    };

    TrajectoryMonitorFactors defaultFactors;
    std::map<std::string, TrajectoryMonitorFactors> factors;

    std::string embeddedSuffix;

private:
    struct GoalIdCompare : public std::binary_function<actionlib_msgs::GoalID, actionlib_msgs::GoalID, bool>
    {
        bool operator()(const actionlib_msgs::GoalID& left, const actionlib_msgs::GoalID& right) const
        {
            return (left.stamp < right.stamp || (left.stamp == right.stamp && left.id < right.id));
        }
    };

    std::vector<std::string> bases;

    struct JointData
    {
        JointData()
        {
            statusValid = true;
            positionValid = false;
            commandValid = false;
            embeddedCommand = false;
            actualPosition = 0.;
            actualVelocity = 0.;
            commandPosition = 0.;
            commandVelocity = 0.;
            torqueLimitCommand = 0.;
            replan.type = r2_msgs::ReplanType::NONE;
        }

        bool   statusValid;
        bool   positionValid;
        bool   commandValid;
        bool   embeddedCommand;
        double actualPosition;
        double actualVelocity;
        double commandPosition;
        double commandVelocity;
        double torqueLimitCommand;
        r2_msgs::ReplanType replan;
    };

    std::map<std::string, JointData> jointData;
    std::map<std::string, JointData>::iterator jointDataIt;
    trajectory_msgs::JointTrajectory replanTraj;
    std::vector<r2_msgs::ReplanType> replanVec;

    // keep track of goals
    typedef std::map<actionlib_msgs::GoalID, r2_msgs::PoseTrajectory, GoalIdCompare> PoseTrajectoriesMap_type;
    PoseTrajectoriesMap_type poseTrajectories;
    typedef std::map<actionlib_msgs::GoalID, trajectory_msgs::JointTrajectory, GoalIdCompare> JointTrajectoriesMap_type;
    JointTrajectoriesMap_type jointTrajectories;

    void sendCartesianWaypoints(const r2_msgs::PoseTrajectory& trajectory,
                                const r2_msgs::ReplanType::_type_type& replan = r2_msgs::ReplanType::NONE);
    void sendJointWaypoints(const trajectory_msgs::JointTrajectory& trajectory,
                            const std::vector<r2_msgs::ReplanType>& replan = std::vector<r2_msgs::ReplanType>());
};

#endif