00001 00023 #ifndef APP2_H_ 00024 #define APP2_H_ 00025 00026 #include "std_msgs/String.h" 00027 #include "nav_msgs/Odometry.h" 00028 #include "geometry_msgs/Twist.h" 00029 00030 #include "micros_swarm/micros_swarm.h" 00031 00032 namespace app2{ 00033 00034 struct XY; 00035 00036 class App2 : public micros_swarm::Application 00037 { 00038 public: 00039 ros::Timer red_timer; 00040 ros::Timer blue_timer; 00041 ros::Publisher pub; 00042 ros::Subscriber sub; 00043 00044 //app parameters 00045 int delta_kin; 00046 int epsilon_kin; 00047 int delta_nonkin; 00048 int epsilon_nonkin; 00049 00050 App2(); 00051 ~App2(); 00052 virtual void init(); 00053 virtual void start(); 00054 virtual void stop(); 00055 00056 //app functions 00057 float force_mag_kin(float dist); 00058 float force_mag_nonkin(float dist); 00059 XY force_sum_kin(micros_swarm::NeighborBase n, XY &s); 00060 XY force_sum_nonkin(micros_swarm::NeighborBase n, XY &s); 00061 XY direction_red(); 00062 XY direction_blue(); 00063 bool red(int id); 00064 bool blue(int id); 00065 void motion_red(); 00066 void motion_blue(); 00067 void publish_red_cmd(const ros::TimerEvent&); 00068 void publish_blue_cmd(const ros::TimerEvent&); 00069 void baseCallback(const nav_msgs::Odometry& lmsg); 00070 }; 00071 }; 00072 00073 #endif