$search
00001 #ifndef OBJECT_TRACKER_OBJECT_TRACKER_H 00002 #define OBJECT_TRACKER_OBJECT_TRACKER_H 00003 00004 #include <ros/ros.h> 00005 #include <std_msgs/String.h> 00006 #include <std_msgs/Float32.h> 00007 #include <worldmodel_msgs/ImagePercept.h> 00008 #include <worldmodel_msgs/PosePercept.h> 00009 #include <worldmodel_msgs/SetObjectState.h> 00010 #include <worldmodel_msgs/SetObjectName.h> 00011 #include <worldmodel_msgs/AddObject.h> 00012 #include <worldmodel_msgs/GetObjectModel.h> 00013 00014 #include <tf/transform_listener.h> 00015 #include <image_geometry/pinhole_camera_model.h> 00016 00017 #include <hector_marker_drawing/HectorDrawings.h> 00018 00019 #include <map> 00020 00021 #include "ObjectModel.h" 00022 00023 namespace object_tracker { 00024 00025 class ObjectTracker { 00026 public: 00027 ObjectTracker(); 00028 virtual ~ObjectTracker(); 00029 00030 void sysCommandCb(const std_msgs::StringConstPtr &); 00031 void imagePerceptCb(const worldmodel_msgs::ImagePerceptConstPtr &); 00032 void posePerceptCb(const worldmodel_msgs::PosePerceptConstPtr &); 00033 void objectAgeingCb(const std_msgs::Float32ConstPtr &); 00034 00035 bool setObjectStateCb(worldmodel_msgs::SetObjectState::Request& request, worldmodel_msgs::SetObjectState::Response& response); 00036 bool setObjectNameCb(worldmodel_msgs::SetObjectName::Request& request, worldmodel_msgs::SetObjectName::Response& response); 00037 bool addObjectCb(worldmodel_msgs::AddObject::Request& request, worldmodel_msgs::AddObject::Response& response); 00038 bool getObjectModelCb(worldmodel_msgs::GetObjectModel::Request& request, worldmodel_msgs::GetObjectModel::Response& response); 00039 00040 ObjectModel& getModel() { return model; } 00041 const ObjectModel& getModel() const { return model; } 00042 00043 void publishModel(); 00044 00045 protected: 00046 bool transformPose(const geometry_msgs::Pose& from, geometry_msgs::Pose &to, std_msgs::Header &header, tf::StampedTransform *transform = 0); 00047 bool transformPose(const geometry_msgs::PoseWithCovariance& from, geometry_msgs::PoseWithCovariance &to, std_msgs::Header &header); 00048 bool mapToNextObstacle(const geometry_msgs::Pose& source, const std_msgs::Header &header, geometry_msgs::Pose &mapped); 00049 00050 private: 00051 ros::NodeHandle nh; 00052 ros::Subscriber imagePerceptSubscriber; 00053 ros::Subscriber posePerceptSubscriber; 00054 ros::Subscriber sysCommandSubscriber; 00055 ros::Subscriber objectAgeingSubscriber; 00056 00057 ros::Publisher modelPublisher; 00058 ros::Publisher modelUpdatePublisher; 00059 ros::Publisher poseDebugPublisher; 00060 ros::Publisher pointDebugPublisher; 00061 00062 ros::ServiceClient distanceToObstacle; 00063 ros::ServiceServer setObjectState; 00064 ros::ServiceServer addObject; 00065 ros::ServiceServer getObjectModel; 00066 ros::ServiceServer setObjectName; 00067 00068 typedef std::pair<ros::ServiceClient,XmlRpc::XmlRpcValue> VerificationService; 00069 std::map<std::string,std::map<std::string,std::vector<VerificationService> > > verificationServices; 00070 00071 tf::TransformListener tf; 00072 00073 HectorDrawings drawings; 00074 00075 ObjectModel model; 00076 typedef boost::shared_ptr<image_geometry::PinholeCameraModel> CameraModelPtr; 00077 std::map<std::string,CameraModelPtr> cameraModels; 00078 00079 bool _project_objects; 00080 std::string _frame_id; 00081 std::string _worldmodel_ns; 00082 double _default_distance; 00083 double _distance_variance; 00084 double _angle_variance; 00085 double _min_height; 00086 double _max_height; 00087 double _pending_support; 00088 double _pending_time; 00089 double _active_support; 00090 double _active_time; 00091 double _ageing_threshold; 00092 }; 00093 00094 } // namespace object_tracker 00095 00096 #endif // OBJECT_TRACKER_OBJECT_TRACKER_H