R2RosTreeId.hpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>

#include "r2_msgs/WrenchState.h"
#include "r2_msgs/StringArray.h"

#include "robodyn_controllers/RosMsgTreeId.h"
#include "robodyn_controllers/JointDynamicsData.h"
#include "robodyn_controllers/KdlTreeId.h"
#include "robodyn_utilities/RosMsgConverter.h"

class R2RosTreeId
{
public:
    R2RosTreeId(std::string urdf, ros::NodeHandle& nh);
    virtual ~R2RosTreeId() {}

    void baseFrameCallback(const r2_msgs::StringArray& msg);
    void forceCallback(const r2_msgs::WrenchState& msg);
    void jointStateCallback(const sensor_msgs::JointState& msg);
    void trajCallback(const sensor_msgs::JointState& msg);
    void imuCallback(const sensor_msgs::Imu& msg);

    bool setGravity(double x, double y, double z, std::string frame, std::string mode);

private:
    // Inv Dyn objects
    RosMsgTreeId rmt;
    KdlTreeId id;
    JointDynamicsData jd;

    // messages
    r2_msgs::StringArray baseMsg, completionMsg;
    r2_msgs::WrenchState forceMsg, emptyForceMsg, segForceMsg;
    sensor_msgs::JointState          actualStateMsg;
    sensor_msgs::JointState          trajStateMsg, emptyTrajMsg;
    sensor_msgs::JointState          jointTorqueMsg;
    sensor_msgs::JointState          jointInertiaMsg;
    sensor_msgs::Imu                 imuInMsg;

    //Publishers/Subscribers

    ros::Subscriber baseFrameIn;
    ros::Subscriber forceIn;
    ros::Subscriber jointStateIn;
    ros::Subscriber trajIn;
    ros::Subscriber imuIn;

    ros::Publisher torqueOut;
    ros::Publisher inertiaOut;
    ros::Publisher segForceOut;
    ros::Publisher completionOut;

    // properties
    std::vector<double> gravity;
    KDL::Vector gravity_kdl;
    std::string gravityFrame;
    KDL::Frame gravityXform;
    bool argos, useImu;
    std::string gravityBase, idMode;
    double yankLimBFF, loopRate;

    // variables for the id process
    std::string baseFrame, bfCmd;
    double bfFactor;
    KDL::Twist v_in, a_in, zeroTwist;
    KDL::Wrench f_out;
    KDL::RigidBodyInertia I_out;
    bool newBase;

};