object_tracker.h
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 } // namespace hector_object_tracker
00116 
00117 #endif // OBJECT_TRACKER_OBJECT_TRACKER_H


hector_object_tracker
Author(s): Johannes Meyer
autogenerated on Thu Jun 6 2019 20:20:14