00001 00023 #ifndef SCDS_PSO_H_ 00024 #define SCDS_PSO_H_ 00025 00026 #include <iostream> 00027 #include <fstream> 00028 #include <vector> 00029 #include <set> 00030 #include <boost/function.hpp> 00031 #include <ros/ros.h> 00032 00033 #include "micros_swarm/scds_pso_tuple.h" 00034 00035 namespace micros_swarm{ 00036 00037 class Agent{ 00038 public: 00039 Agent(); 00040 Agent(std::string name); 00041 ~Agent(); 00042 void set_param(float w, float c1, float c2); 00043 void set_dim(int dim); 00044 void set_fitness(const boost::function<float(const std::vector<float>& )>& fitness); 00045 void set_min_pos(const std::vector<float>& pos); 00046 void set_max_pos(const std::vector<float>& pos); 00047 void set_min_vel(const std::vector<float>& pos); 00048 void set_max_vel(const std::vector<float>& pos); 00049 void init_pos(const std::vector<float>& pos); 00050 void init_vel(const std::vector<float>& vel); 00051 void rand_init(); 00052 void start(); 00053 void start(int loop_gen); 00054 void stop(); 00055 private: 00056 bool has_pos_limit(int index); 00057 bool has_vel_limit(int index); 00058 00059 void loop(const ros::TimerEvent&); 00060 00061 std::string name_; 00062 bool run_; 00063 int dim_; 00064 float w_; 00065 float c1_; 00066 float c2_; 00067 boost::function<float(const std::vector<float>& )> fitness_; 00068 std::vector<float> min_pos_; 00069 std::vector<float> max_pos_; 00070 std::vector<float> min_vel_; 00071 std::vector<float> max_vel_; 00072 std::vector<float> cur_pos_; 00073 std::vector<float> cur_vel_; 00074 int cur_gen_; 00075 int gen_limit_; 00076 SCDSPSODataTuple pbest_; 00077 SCDSPSODataTuple gbest_; 00078 SCDSPSOTuple best_tuple_; 00079 00080 int robot_id_; 00081 boost::shared_ptr<RuntimeHandle> rth_; 00082 00083 ros::NodeHandle nh_; 00084 ros::Timer timer_; 00085 00086 std::ofstream file; 00087 }; 00088 }; 00089 00090 #endif