sr_kinematics.h

Go to the documentation of this file.

#ifndef _SR_KINEMATICS_H_
#define _SR_KINEMATICS_H_

#include <ros/ros.h>
#include <kdl_parser/kdl_parser.hpp>

#include <boost/smart_ptr.hpp>
#include <boost/thread.hpp>

#include <tf/transform_listener.h>
#include <tf_conversions/tf_kdl.h>

#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PoseStamped.h>

#include "kinematics_msgs/GetPositionIK.h"
#include "kinematics_msgs/PositionIKRequest.h"
#include "motion_planning_msgs/RobotState.h"

namespace shadowrobot
{
class SrKinematics
{
public:
    SrKinematics();
    ~SrKinematics();

private:
    ros::NodeHandle node, n_tilde;

    //consts used for the topics on which to publish / subscribe
    static const std::string hand_joint_states_topic, arm_joint_states_topic, hand_sendupdate_topic, arm_sendupdate_topic;

    std::string arm_kinematics_service, root_name, tip_name, rk_target, fixed_frame;

    KDL::Chain chain;

    typedef std::map<std::string, double> JointsMap;
    JointsMap joints_map;

    bool loadModel( const std::string xml );

    boost::mutex mutex;

    //publishes all the targets to the hand and the arm (the target names are then checked internally in the hand/arm)
    //it's easier that way than checking if the target is for the hand or for the arm.
    ros::Publisher pub_hand, pub_arm;
    //we need two different subscribers to joint_states: one for the hand, one for the arm
    ros::Subscriber hand_subscriber, arm_subscriber;

    void jointstatesCallback( const sensor_msgs::JointStateConstPtr& msg );

    void reverseKinematicsCallback( const tf::tfMessageConstPtr& msg );
    ros::Subscriber tf_sub;
    tf::TransformListener tf_listener;

    ros::ServiceClient rk_client;

    geometry_msgs::PoseStamped getPoseFromTransform(tf::StampedTransform transform);

    inline double toDegrees( double rad )
    {
        return rad * 57.2957795;
    }
}; // end class SrKinematics

}
; //end namespace


#endif