phalange.h

Go to the documentation of this file.

#ifndef phalange_h___
#define phalange_h___

#include "ros/ros.h"
#include "icr_msgs/SetPose.h"
#include "icr_msgs/StampedContactPose.h"
#include <icr_msgs/ContactState.h>
#include <tf/transform_listener.h>
#include <tf/message_filter.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/tf.h>
#include <string>
#include "model_server.h"
#include <boost/thread/mutex.hpp>
#include <boost/shared_ptr.hpp>


namespace ICR
{

class Phalange
{
 public:

 
  Phalange(tf::Transform const & L_T_Cref,std::string const & phl_name,std::string const & phl_frame_id,std::string const & sensor_topic);
  ~Phalange();

  icr_msgs::StampedContactPose::ConstPtr getStampedContactPose();
  void setTargetObjFrameId(std::string const & obj_frame_id);  
  std::string getPhalangeName(); 
  std::string getPhalangeFrameId();
 
 private:

  Phalange();
  
  ros::NodeHandle nh_, nh_private_;
  ros::Publisher ct_pose_pub_;
  ros::Subscriber contacts_subs_;
  ros::ServiceServer set_pose_srv_;
  tf::Transform L_T_Cref_;
  boost::shared_ptr<icr_msgs::StampedContactPose> C_T_O_;

  std::string phl_name_;
  std::string phl_frame_id_;

  std::string obj_frame_id_;

 boost::mutex lock_;
 tf::TransformListener tf_list_;

 tf::MessageFilter<icr_msgs::StampedContactPose>* tf_filter_;

  std::string tf_prefix_;

  tf::Quaternion projectPose(tf::Vector3 const & z);

  //  CALLBACKS  //

  void listenContacts(const icr_msgs::ContactState::ConstPtr& cts_st);
  bool setPose(icr_msgs::SetPose::Request  &req, icr_msgs::SetPose::Response &res);
  void transformContactPose(const boost::shared_ptr<const icr_msgs::StampedContactPose>& C_T_L); 
};
}//end namespace
#endif