$search
00001 #ifndef COP_CLIENT_H 00002 #define COP_CLIENT_H 00003 00004 00005 #include <ros/ros.h> 00006 #include <vision_msgs/cop_answer.h> 00007 #include <stdio.h> 00008 #include <tf/tf.h> 00009 00010 00011 class CopClient 00012 { 00013 public: 00014 CopClient(ros::NodeHandle &nh, std::string stTopicName = "/tracking/cop_handler", std::string stCopName = "cop"); 00015 00016 unsigned long LOFrameQuery(unsigned long child, unsigned long parent); 00017 void LOFreeID(unsigned long id); 00018 double LODistanceQuery(unsigned long child, unsigned long parent); 00019 void LOPointQuery(unsigned long child, float *vector); 00020 unsigned long LONameQuery(std::string name); 00021 tf::Stamped<tf::Pose> LOPoseQuery(unsigned long child); 00022 00023 unsigned long LOOffset(unsigned long id, unsigned long parent, float x, float y, float z); 00024 00025 00026 size_t HasResult(long perception_primitive) 00027 { 00028 if(m_msg_cluster.find(perception_primitive) != m_msg_cluster.end()) 00029 return m_msg_cluster[perception_primitive].size(); 00030 else 00031 return 0; 00032 } 00033 std::vector<vision_msgs::cop_answer> GetResult(long perception_primitive) 00034 { 00035 if(m_msg_cluster.find(perception_primitive) != m_msg_cluster.end()) 00036 return m_msg_cluster[perception_primitive]; 00037 else 00038 return std::vector<vision_msgs::cop_answer> (); 00039 } 00040 00041 long CallCop(std::string object_class, unsigned long position_id, int num_objects = 1, unsigned long alterantive_id = 0, unsigned long action_type = 0); 00042 long CallCop(std::string object_class, std::vector<unsigned long> position_ids, int num_of_objects, unsigned long alterantive_id, unsigned long action_type); 00043 void CopFeedBack(long primitive, double evaluation, unsigned long error_id); 00044 private: 00045 ros::Subscriber m_readAnswer; 00046 ros::Publisher m_pubFeedBack; 00047 ros::ServiceClient m_client; 00048 ros::ServiceClient m_jlo_client; 00049 std::string m_answerTopic; 00050 std::map<unsigned long, std::vector<vision_msgs::cop_answer> > m_msg_cluster; 00051 00052 void callback(const boost::shared_ptr<const vision_msgs::cop_answer> &msg); 00053 00054 }; 00055 00056 #endif /*COP_CLIENT_H*/