ajockey.h
Go to the documentation of this file.
00001 /*
00002  * Learns a segment, i.e. a series of features with additional information,
00003  * with information based on a camera and odometry.
00004  *
00005  * Long description:
00006  * Learns a segment, i.e. a series of features with additional information,
00007  * with information based on a camera and odometry. A feature with additional
00008  * information is called a landmark. Implemented actions:
00009  * - START: will learn new features indefinitely or on at least a configurable
00010  *   distance.
00011  * - STOP: will interrupt the START action and save the segment.
00012  * - INTERRUPT: TODO
00013  * - CONTINUE: TODO
00014  *
00015  * Interaction with the map (created by this jockey):
00016  * - [Getter][/][Setter], message type, interface default name
00017  *
00018  * Interaction with the map (created by other jockeys):
00019  * - [Getter][/][Setter], message type, interface default name
00020  * - Setter: Segment, the interface name is given by the subclass
00021  *
00022  * Subscribers (other than map-related):
00023  * - message type, topic default name, description
00024  * - sensor_msgs/Image (raw), "~/camera/image_raw", image from a front camera.
00025  * - nav_msgs::Odometry, "~/odom", Odometry.
00026  *
00027  * Publishers (other than map-related):
00028  * - message type, topic default name, description
00029  *
00030  * Services used (other than map-related):
00031  * - none
00032  *
00033  * Parameters:
00034  * - matcher_max_relative_distance, double, 0.8, a potential descriptor is
00035  *   visible in the new image if the distance to the best-match descriptor is
00036  *   smaller than the distance to the second best match multiplied by this factor.
00037  * - min_landmark_dist, double, 0.020 m, a landmark is saved if it was visible
00038  *   on at least such a traveled distance (m).
00039  * - max_segment_length, double, 0, the jockey will successfully end the START
00040  *   action if the traveled distance is greater than max_segment_length (m).
00041  *   Defaults to 0, which means that the START action never finishes.
00042  */
00043 
00044 #ifndef FEATURENAV_BASE_AJOCKEY_H
00045 #define FEATURENAV_BASE_AJOCKEY_H
00046 
00047 #include <algorithm>
00048 #include <cmath>
00049 #include <string>
00050 #include <vector>
00051 
00052 #include <ros/ros.h>
00053 #include <ros/console.h>
00054 #include <tf/tf.h>
00055 #include <image_transport/image_transport.h>
00056 #include <geometry_msgs/Pose.h>
00057 #include <nav_msgs/Odometry.h>
00058 #include <sensor_msgs/Image.h>
00059 #include <sensor_msgs/image_encodings.h>
00060 
00061 #include <lama_jockeys/learning_jockey.h>
00062 
00063 #include <featurenav_base/potential_landmark.h>
00064 #include <featurenav_base/typedef.h>
00065 #include <featurenav_base/Landmark.h>
00066 #include <featurenav_base/Segment.h>
00067 #include <featurenav_base/SetSegment.h>
00068 
00069 namespace featurenav_base
00070 {
00071 
00072 using std::vector;
00073 using cv::Mat;
00074 using cv::KeyPoint;
00075 using cv::DMatch;
00076 
00077 class AJockey : public lama_jockeys::LearningJockey
00078 {
00079   public:
00080 
00081     AJockey(const std::string& name, const std::string& segment_interface_name, const std::string& segment_setter_name);
00082 
00083     void setExtractFeaturesFunction(feature_extractor_function_ptr f) {extract_features_ = f;}
00084     void setDescriptorMatcherFunction(descriptor_matcher_function_ptr f) {match_descriptors_ = f;}
00085 
00086     can_do_function_ptr canDo;
00087     start_do_function_ptr startDo;
00088 
00089   private:
00090 
00091     virtual void onLearn();
00092     virtual void onStop();
00093     virtual void onInterrupt();
00094     virtual void onContinue();
00095 
00096     void reset();
00097     size_t processImage(const sensor_msgs::ImageConstPtr& image);
00098 
00101     inline double distance_from_start()
00102     {
00103       if (!has_odom_)
00104       {
00105         return 0.0;
00106       }
00107       const double dx = odom_.pose.pose.position.x - start_pose_.position.x;
00108       const double dy = odom_.pose.pose.position.y - start_pose_.position.y;
00109       return std::sqrt(dx * dx + dy * dy);
00110     }
00111 
00114     inline lama_msgs::DescriptorLink segmentDescriptorLink(const int32_t id)
00115     {
00116       lama_msgs::DescriptorLink descriptor_link;
00117       descriptor_link.descriptor_id = id;
00118       descriptor_link.interface_name = segment_interface_name_;
00119       return descriptor_link;
00120     }
00121 
00122     void callback_image(const sensor_msgs::ImageConstPtr& msg);
00123     void callback_odom(const nav_msgs::OdometryConstPtr& msg);
00124 
00125     // Publisher and subscribers.
00126     image_transport::ImageTransport it_;
00127     image_transport::Subscriber image_handler_;
00128     ros::Subscriber odom_handler_;
00129 
00130     // Client proxies.
00131     ros::ServiceClient segment_setter_proxy_;
00132 
00133     // ROS parameters, shown outside.
00134     double matcher_max_relative_distance_;  
00135 
00136 
00137     double min_landmark_dist_;  
00138     double max_segment_length_;  
00139 
00140 
00141     // Hard-coded parameters.
00142     static const ros::Duration max_odom_age_;
00143     static const ros::Duration max_landmark_age_;
00144 
00145     // Internals.
00146     std::string segment_interface_name_;  
00147     std::string segment_setter_name_;  
00148     feature_extractor_function_ptr extract_features_;
00149     descriptor_matcher_function_ptr match_descriptors_;
00150     nav_msgs::Odometry odom_;  
00151     bool has_odom_;
00152     bool image_processing_running_;  
00153     geometry_msgs::Pose start_pose_;  
00154     Segment segment_;
00155     std::vector<PotentialLandmark> landmarks_; 
00156 };
00157 
00158 } // namespace featurenav_base
00159 
00160 #endif // FEATURENAV_BASE_AJOCKEY_H
00161 


featurenav_base
Author(s): Gaƫl Ecorchard
autogenerated on Sat Jun 8 2019 19:52:22