Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef _score_map_alg_node_h_
00026 #define _score_map_alg_node_h_
00027
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "iri_score_map_mix.h"
00030
00031 #include <limits>
00032
00033
00034 #include <geometry_msgs/PoseStamped.h>
00035 #include <std_msgs/UInt32.h>
00036 #include <sensor_msgs/PointCloud2.h>
00037 #include <pcl/ros/conversions.h>
00038 #include <pcl/point_cloud.h>
00039 #include <pcl/point_types.h>
00040
00041 #include <pcl/io/pcd_io.h>
00042
00043
00044 #include <iri_deformable_analysis/ScoreMapMixConfig.h>
00045 #include <iri_deformable_analysis/ScoreMapMixResults.h>
00046 #include <iri_deformable_analysis/SetScoreMixAlgorithmID.h>
00047
00048
00049
00050
00051 #define MAXMAPS 8
00052
00053 enum {
00054 FULL0,
00055 FULL1,
00056 FIFTY01
00057 };
00058
00063 class ScoreMapMixAlgNode : public algorithm_base::IriBaseAlgorithm<ScoreMapMixAlgorithm>
00064 {
00065 private:
00066
00067 int selection_algorithm_id_;
00068 int num_score_maps_;
00069 std::vector<pcl::PointCloud<pcl::PointXYZRGB> > input_score_maps_;
00070 std::vector<bool> score_map_ready_;
00071
00072
00073 std::vector<double> weights_;
00074 pcl::PointCloud<pcl::PointXYZRGB> output_score_map_;
00075
00076
00077 ros::Publisher score_map_global_publisher_;
00078 sensor_msgs::PointCloud2 score_map_global_msg_;
00079 ros::Publisher grasping_point_pose_publisher_;
00080 geometry_msgs::PoseStamped best_scored_pose_;
00081 ros::Publisher algorithm_id_publisher_;
00082 std_msgs::UInt32 UInt32_msg_;
00083 ros::Publisher score_results_publisher_;
00084 iri_deformable_analysis::ScoreMapMixResults score_results_;
00085
00086
00087 ros::Subscriber score_map0_subscriber_;
00088 void score_map0_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00089 CMutex score_map0_mutex_;
00090 ros::Subscriber score_map7_subscriber_;
00091 void score_map7_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00092 CMutex score_map7_mutex_;
00093 ros::Subscriber score_map6_subscriber_;
00094 void score_map6_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00095 CMutex score_map6_mutex_;
00096 ros::Subscriber score_map5_subscriber_;
00097 void score_map5_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00098 CMutex score_map5_mutex_;
00099 ros::Subscriber score_map4_subscriber_;
00100 void score_map4_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00101 CMutex score_map4_mutex_;
00102 ros::Subscriber score_map3_subscriber_;
00103 void score_map3_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00104 CMutex score_map3_mutex_;
00105 ros::Subscriber score_map2_subscriber_;
00106 void score_map2_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00107 CMutex score_map2_mutex_;
00108 ros::Subscriber score_map1_subscriber_;
00109 void score_map1_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00110 CMutex score_map1_mutex_;
00111
00112
00113 ros::ServiceServer algorithm_id_server_;
00114 bool algorithm_idCallback(iri_deformable_analysis::SetScoreMixAlgorithmID::Request &req, iri_deformable_analysis::SetScoreMixAlgorithmID::Response &res);
00115 CMutex algorithm_id_mutex_;
00116
00117
00118
00119
00120
00121
00122
00123 void algorithmConfigurationTranslation(int alg_id);
00124
00125
00126 void getMax(const pcl::PointCloud<pcl::PointXYZRGB> score_map, pcl::PointXYZRGB& grasp_point, int& pcl_row, int& pcl_col, float& max_score, float& min_score);
00127
00128 bool all_clouds_ready();
00129
00130
00131 void computeScoreMap();
00132
00133
00134
00135 void computeGraspPoints();
00136
00137 void normalizeScoreMap(const pcl::PointCloud<pcl::PointXYZRGB> score_map);
00138 void normalizeScoreMap(const pcl::PointCloud<pcl::PointXYZRGB> score_map, double max_input, double min_input);
00139
00140
00141 public:
00148 ScoreMapMixAlgNode(void);
00149
00156 ~ScoreMapMixAlgNode(void);
00157
00158 protected:
00171 void mainNodeThread(void);
00172
00185 void node_config_update(Config &config, uint32_t level);
00186
00193 void addNodeDiagnostics(void);
00194
00195
00196
00197
00198 };
00199
00200 #endif