Go to the documentation of this file.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 <hector_object_tracker/types.h>
00008 #include <hector_worldmodel_msgs/SetObjectState.h>
00009 #include <hector_worldmodel_msgs/SetObjectName.h>
00010 #include <hector_worldmodel_msgs/AddObject.h>
00011 #include <hector_worldmodel_msgs/GetObjectModel.h>
00012 #include <sensor_msgs/CameraInfo.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 "ObjectModel.h"
00020
00021 namespace hector_object_tracker {
00022
00023 class ObjectTracker {
00024 public:
00025 ObjectTracker();
00026 virtual ~ObjectTracker();
00027
00028 protected:
00029 void sysCommandCb(const std_msgs::StringConstPtr &);
00030 void imagePerceptCb(const hector_worldmodel_msgs::ImagePerceptConstPtr &);
00031 void posePerceptCb(const hector_worldmodel_msgs::PosePerceptConstPtr &);
00032 void objectAgeingCb(const std_msgs::Float32ConstPtr &);
00033
00034 void modelUpdateCb(const hector_worldmodel_msgs::ObjectModelConstPtr &);
00035
00036 bool setObjectStateCb(hector_worldmodel_msgs::SetObjectState::Request& request, hector_worldmodel_msgs::SetObjectState::Response& response);
00037 bool setObjectNameCb(hector_worldmodel_msgs::SetObjectName::Request& request, hector_worldmodel_msgs::SetObjectName::Response& response);
00038 bool addObjectCb(hector_worldmodel_msgs::AddObject::Request& request, hector_worldmodel_msgs::AddObject::Response& response);
00039 bool getObjectModelCb(hector_worldmodel_msgs::GetObjectModel::Request& request, hector_worldmodel_msgs::GetObjectModel::Response& response);
00040
00041 ObjectModel& getModel() { return model; }
00042 const ObjectModel& getModel() const { return model; }
00043
00044 ObjectModel getMergedModel();
00045
00046 void publishModelEvent(const ros::TimerEvent&);
00047 void publishModel();
00048
00049 protected:
00050 bool transformPose(const geometry_msgs::Pose& from, geometry_msgs::Pose &to, std_msgs::Header &header, tf::StampedTransform *transform = 0);
00051 bool transformPose(const geometry_msgs::PoseWithCovariance& from, geometry_msgs::PoseWithCovariance &to, std_msgs::Header &header);
00052 bool mapToNextObstacle(const geometry_msgs::Pose& source, const std_msgs::Header &header, const ObjectInfo &info, geometry_msgs::Pose &mapped);
00053
00054 private:
00055 ros::NodeHandle nh;
00056 ros::NodeHandle priv_nh;
00057 ros::Subscriber imagePerceptSubscriber;
00058 ros::Subscriber posePerceptSubscriber;
00059 ros::Subscriber sysCommandSubscriber;
00060 ros::Subscriber objectAgeingSubscriber;
00061
00062 struct NegativeUpdateInfo {
00063 std::string class_id;
00064 ObjectInfo::_support_type negative_support;
00065 ObjectInfo::_support_type min_support;
00066 float min_distance;
00067 float max_distance;
00068 float ignore_border_pixels;
00069 ros::Duration not_seen_duration;
00070 ros::Subscriber subscriber;
00071 };
00072 typedef boost::shared_ptr<NegativeUpdateInfo> NegativeUpdatePtr;
00073 std::vector<NegativeUpdatePtr> negativeUpdate;
00074 void negativeUpdateCallback(const sensor_msgs::CameraInfoConstPtr &, const NegativeUpdatePtr& info);
00075
00076 ros::Publisher modelPublisher;
00077 ros::Publisher modelUpdatePublisher;
00078 ros::Publisher poseDebugPublisher;
00079 ros::Publisher pointDebugPublisher;
00080 ros::Subscriber modelUpdateSubscriber;
00081
00082 ros::ServiceServer setObjectState;
00083 ros::ServiceServer addObject;
00084 ros::ServiceServer getObjectModel;
00085 ros::ServiceServer setObjectName;
00086
00087 ros::Timer publishTimer;
00088
00089 typedef std::pair<ros::ServiceClient,XmlRpc::XmlRpcValue> VerificationService;
00090 std::map<std::string,std::map<std::string,std::vector<VerificationService> > > verificationServices;
00091
00092 tf::TransformListener tf;
00093
00094 HectorDrawings drawings;
00095
00096 ObjectModel model;
00097 typedef boost::shared_ptr<image_geometry::PinholeCameraModel> CameraModelPtr;
00098 std::map<std::string,CameraModelPtr> cameraModels;
00099 ObjectTracker::CameraModelPtr getCameraModel(const std::string& frame_id, const sensor_msgs::CameraInfo& camera_info);
00100
00101 struct MergedModelInfo {
00102 std::string prefix;
00103 ObjectModel model;
00104 ros::Subscriber subscriber;
00105 };
00106 typedef boost::shared_ptr<MergedModelInfo> MergedModelPtr;
00107 std::vector<MergedModelPtr> merged_models;
00108 void mergeModelCallback(const hector_worldmodel_msgs::ObjectModelConstPtr &new_model, const MergedModelPtr& info);
00109
00110 std::string _frame_id;
00111 std::string _worldmodel_ns;
00112
00113 double _ageing_threshold;
00114 double _publish_interval;
00115 };
00116
00117 }
00118
00119 #endif // OBJECT_TRACKER_OBJECT_TRACKER_H