olfati_saber_flocking.cpp
Go to the documentation of this file.
1 
24 
25 #define PI 3.14159265358979323846
26 #define EPSILON 0.1
27 #define A 5
28 #define B 5
29 const double C = abs(A-B) / sqrt(4*A*B);
30 #define H 0.2
31 #define D 10
32 #define R 12
33 #define C1 0.3
34 #define C2 0.3
35 
36 double pm1=1,pm2=3,pm3=1;
37 
38 using namespace std;
39 
41 {
42 public:
43  double _px,_py,_vx,_vy;
44  pair<double,double> _position,_velocity;
45  int _r_id;
46  double mypm_g;
47  NeighborHandle(int r_id, float x, float y, float vx, float vy)
48  {
49 
50  _px=x;
51  _py=y;
52  _vx=vx;
53  _vy=vy;
54  _position=pair<double,double>(x,y);
55  _velocity=pair<double,double>(vx,vy);
56  _r_id = r_id;
57  mypm_g=1;
58  }
59 };
60 
61 static list<NeighborHandle*> neighbor_list;
62 pair<double,double> my_position=pair<double,double>(0,0);
63 pair<double,double> my_velocity=pair<double,double>(0,0);
64 
65 pair<double,double> get_vector(pair<double,double> start,pair<double,double> end)
66 {
67  pair<double,double> re=pair<double,double>(0,0);
68  re.first=end.first-start.first;
69  re.second=end.second-start.second;
70  return re;
71 }
72 
73 double segma_norm(pair<double,double> v)
74 {
75  double re = EPSILON*(v.first*v.first+v.second*v.second);
76  re = sqrt(1+re)-1;
77  re /= EPSILON;
78  return re;
79 }
80 
81 double R_alpha = segma_norm(pair<double,double>(R,0));
82 double D_alpha = segma_norm(pair<double,double>(D,0));
83 
84 pair<double,double> segma_epsilon(pair<double,double> v)
85 {
86  pair<double,double> re = pair<double,double>(0,0);
87  double scale = 1+EPSILON*(v.first*v.first+v.second*v.second);
88  scale = sqrt(scale);
89  re.first = v.first / scale;
90  re.second = v.second / scale;
91  return re;
92 }
93 
94 double segma_1(double z)
95 {
96  return z / sqrt(1+z*z);
97 }
98 
99 double phi(double z)
100 {
101  return 0.5*((A+B)*segma_1(z+C)+A-B);
102 }
103 
104 double rho(double z)
105 {
106  if(z<H)
107  return 1;
108  if(z>1)
109  return 0;
110  return 0.5*(1+cos(PI*(z-H)/(1-H)));
111 }
112 
113 double phi_alpha(double z)
114 {
115  return rho(z/R_alpha)*phi(z-D_alpha);
116 }
117 
118 pair<double,double> f_g()
119 {
120  pair<double,double> re = pair<double,double>(0,0);
121  for(list<NeighborHandle*>::iterator i=neighbor_list.begin();i!=neighbor_list.end();i++)
122  {
123  pair<double,double> q_ij = get_vector(my_position,(*i)->_position);
124  pair<double,double> n_ij = segma_epsilon(q_ij);
125  re.first += phi_alpha(segma_norm(q_ij))*n_ij.first*(*i)->mypm_g;
126  re.second += phi_alpha(segma_norm(q_ij))*n_ij.second*(*i)->mypm_g;
127  }
128  return re;
129 }
130 
131 double a_ij(pair<double,double> j_p)
132 {
133  return rho(segma_norm(get_vector(my_position,j_p)) / R_alpha);
134 }
135 
136 pair<double,double> f_d()
137 {
138  pair<double,double> re = pair<double,double>(0,0);
139  for(list<NeighborHandle*>::iterator i=neighbor_list.begin();i!=neighbor_list.end();i++)
140  {
141  pair<double,double> p_ij = get_vector(my_velocity,(*i)->_velocity);
142  re.first += a_ij((*i)->_position) * p_ij.first;
143  re.second += a_ij((*i)->_position) * p_ij.second;
144  }
145  return re;
146 }
147 
148 double interval = 0.01;
149 pair<double,double> f_r()
150 {
151  pair<double,double> re = pair<double,double>(0,0);
152  static pair<double,double> q_r = pair<double,double>(0,0);
153  pair<double,double> p_r = pair<double,double>(0.5,0.5);
154  q_r.first += p_r.first * interval;
155  q_r.second += p_r.second * interval;
156  re.first = -C1*get_vector(q_r,my_position).first - C2*get_vector(p_r,my_velocity).first;
157  re.second = -C1*get_vector(q_r,my_position).second - C2*get_vector(p_r,my_velocity).second;
158  return re;
159 }
160 
161 // Register the application
163 
164 namespace olfati_saber_flocking{
165 
166  OlfatiSaberFlocking::OlfatiSaberFlocking() {}
167 
168  OlfatiSaberFlocking::~OlfatiSaberFlocking() {}
169 
170  void OlfatiSaberFlocking::stop() {}
171 
172  void OlfatiSaberFlocking::init()
173  {
174  //set parameters
175  hz=10;
176  interval=1.0/hz;
177  }
178 
179  void OlfatiSaberFlocking::publish_cmd(const ros::TimerEvent&)
180  {
181  geometry_msgs:: Twist msg;
183 
184  typename std::map<int, micros_swarm::NeighborBase>::iterator it;
185  for(it=n.data().begin();it!=n.data().end();it++)
186  {
187  NeighborHandle* nh=new NeighborHandle(it->first, it->second.x, it->second.y, it->second.vx, it->second.vy);
188  neighbor_list.push_back(nh);
189  }
190 
191  micros_swarm::Base nl = get_base();
192 
193  my_position=pair<double,double>(nl.x, nl.y);
194  my_velocity=pair<double,double>(nl.vx, nl.vy);
195 
196  msg.linear.x += (f_g().first*pm1+f_d().first*pm2+f_r().first*pm3)/hz;
197  msg.linear.y += (f_g().second*pm1+f_d().second*pm2+f_r().second*pm3)/hz;
198  //cout<<f_g().second<<' '<<f_d().second<<' '<<msg.linear.y<<endl;
199 
200  if (msg.linear.x >1)
201  msg.linear.x=1;
202  if (msg.linear.x <-1)
203  msg.linear.x=-1;
204  if (msg.linear.y >1)
205  msg.linear.y=1;
206  if (msg.linear.y <-1)
207  msg.linear.y=-1;
208  pub.publish(msg);
209 
210  neighbor_list.clear();
211  }
212 
213  void OlfatiSaberFlocking::baseCallback(const nav_msgs::Odometry& lmsg)
214  {
215  float x=lmsg.pose.pose.position.x;
216  float y=lmsg.pose.pose.position.y;
217 
218  float vx=lmsg.twist.twist.linear.x;
219  float vy=lmsg.twist.twist.linear.y;
220 
221  micros_swarm::Base l(x, y, 0, vx, vy, 0, 1);
222  set_base(l);
223  }
224 
225  void OlfatiSaberFlocking::start()
226  {
227  init();
228 
229  ros::NodeHandle nh;
230  set_dis(12);
231  pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
232  sub = nh.subscribe("base_pose_ground_truth", 1000, &OlfatiSaberFlocking::baseCallback, this, ros::TransportHints().udp());
233  ros::Duration(5).sleep(); //this is necessary, in order that the runtime platform kernel of the robot has enough time to publish it's base information.
234 
235  timer = nh.createTimer(ros::Duration(interval), &OlfatiSaberFlocking::publish_cmd, this);
236  }
237 };
238 
#define EPSILON
#define PI
double pm3
#define R
double R_alpha
const double C
pair< double, double > my_position
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void init(const M_string &remappings)
pair< double, double > f_g()
bool sleep() const
#define B
double pm2
#define C2
double interval
double phi(double z)
#define C1
pair< double, double > segma_epsilon(pair< double, double > v)
pair< double, double > f_d()
double segma_1(double z)
#define H
double segma_norm(pair< double, double > v)
pair< double, double > get_vector(pair< double, double > start, pair< double, double > end)
#define A
pair< double, double > my_velocity
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
std::map< int, Type > & data()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
pair< double, double > _velocity
NeighborHandle(int r_id, float x, float y, float vx, float vy)
double phi_alpha(double z)
double a_ij(pair< double, double > j_p)
#define D
static list< NeighborHandle * > neighbor_list
pair< double, double > f_r()
double pm1
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
double D_alpha
double rho(double z)


olfati_saber_flocking
Author(s):
autogenerated on Mon Jun 10 2019 14:02:21