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