iCub-main
SharedCanBus.cpp
Go to the documentation of this file.
1 // -*- Mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4  * Copyright (C) 2012 RobotCub Consortium
5  * Author: Alessandro Scalzo
6  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
7  *
8  */
9 
10 #include <mutex>
11 #include <string>
12 #include <vector>
13 
14 #include <yarp/os/Time.h>
15 #include <yarp/os/Log.h>
16 #include <yarp/os/Property.h>
17 #include <yarp/os/PeriodicThread.h>
18 #include <yarp/dev/PolyDriver.h>
19 
20 #include "SharedCanBus.h"
21 
22 const int CAN_DRIVER_BUFFER_SIZE = 500;
23 const int DEFAULT_THREAD_PERIOD = 10;
24 
25 class SharedCanBus : public yarp::os::PeriodicThread
26 {
27 public:
28  SharedCanBus() : PeriodicThread((double)DEFAULT_THREAD_PERIOD/1000.0)
29  {
30  mBufferSize=0;
31  mCanDeviceNum=-1;
32  mDevice="";
33 
34  reqIdsUnion=new char[0x800];
35 
36  for (int i=0; i<0x800; ++i) reqIdsUnion[i]=UNREQ;
37  }
38 
40  {
41  stop();
42 
43  polyDriver.close();
44 
45  delete [] reqIdsUnion;
46  }
47 
49  {
50  return mBufferSize;
51  }
52 
54  {
55  return mCanDeviceNum;
56  }
57 
58  bool IloveUmom(yarp::os::Searchable &config)
59  {
60  if (!config.check("physDevice"))
61  {
62  yError("SharedCanBus could not find low level can driver specification\n");
63  return false;
64  }
65 
66  if (mDevice!=config.find("physDevice").asString()) return false;
67 
68  if (config.findGroup("CAN").check("sharedCanPeriod"))
69  {
70  int sharedCanPeriod = config.findGroup("CAN").find("sharedCanPeriod").asInt32();
71  int currentCanPeriod = (int)(1000.0 * this->getPeriod());
72  if (currentCanPeriod != sharedCanPeriod && currentCanPeriod != DEFAULT_THREAD_PERIOD)
73  {
74  yWarning("SharedCanBus: Requested a different sharedCanPeriod (%d ms) respect to previous instance (%d ms). Using: %d ms\n", sharedCanPeriod, currentCanPeriod, currentCanPeriod);
75  }
76  }
77 
78  if (!config.check("canDeviceNum")) return true;
79 
80  return mCanDeviceNum==config.find("canDeviceNum").asInt32();
81  }
82 
84  {
85  std::lock_guard<std::mutex> lck(configMutex);
86  accessPoints.push_back(ap);
87  }
88 
90  {
91  if (!ap) return;
92 
93  std::lock_guard<std::mutex> lck(configMutex);
94 
95  int n=accessPoints.size();
96 
97  for (int i=0; i<n; ++i)
98  {
99  if (ap==accessPoints[i])
100  {
101  for (int id=0; id<0x800; ++id)
102  {
103  if (ap->hasId(id)) canIdDeleteUnsafe(id);
104  }
105 
106  accessPoints[i]=accessPoints[n-1];
107 
108  accessPoints.pop_back();
109 
110  break;
111  }
112  }
113 
114  if (accessPoints.size()==0)
115  {
116  // should close the driver here?
117  }
118  }
119 
120  void run()
121  {
122  static const bool NOWAIT=false;
123  unsigned int msgsNum=0;
124 
125  std::lock_guard<std::mutex> lck(configMutex);
126 
127  bool ret=theCanBus->canRead(readBufferUnion,mBufferSize,&msgsNum,NOWAIT);
128 
129  if (ret)
130  {
131  for (unsigned int i=0; i<msgsNum; ++i)
132  {
133  unsigned int id=readBufferUnion[i].getId();
134 
135  for (unsigned int p=0; p<accessPoints.size(); ++p)
136  {
137  if (accessPoints[p]->hasId(id))
138  {
139  if (accessPoints[p]->pushReadMsg(readBufferUnion[i])==false)
140  {
141  yError("run()-pushReadMsg() failed on CAN bus %d", mCanDeviceNum);
142  }
143  }
144  }
145  }
146  }
147  }
148 
149  bool canWrite(const yarp::dev::CanBuffer &msgs, unsigned int size, unsigned int *sent, bool wait,yarp::dev::CanBusAccessPoint* pFrom)
150  {
151  std::lock_guard<std::mutex> lck(writeMutex);
152  bool ret=theCanBus->canWrite(msgs,size,sent,wait);
153 
154  //this allows other istances to read back the sent message (echo)
155  yarp::dev::CanBuffer buff=msgs;
156  for (unsigned int m=0; m<size; ++m)
157  {
158  unsigned int id=buff[m].getId();
159  if (reqIdsUnion[id])
160  {
161  for (unsigned int p=0; p<accessPoints.size(); ++p)
162  {
163  if (accessPoints[p]!=pFrom && accessPoints[p]->hasId(id))
164  {
165  if (accessPoints[p]->pushReadMsg(buff[m])==false)
166  {
167  yError("canWrite()-pushReadMsg() failed on CAN bus %d", mCanDeviceNum);
168  }
169  }
170  }
171  }
172  }
173 
174  return ret;
175  }
176 
177  void canIdAdd(unsigned int id)
178  {
179  std::lock_guard<std::mutex> lck(configMutex);
180  if (reqIdsUnion[id]==UNREQ)
181  {
182  reqIdsUnion[id]=REQST;
183  theCanBus->canIdAdd(id);
184  }
185  }
186 
187  void canIdDelete(unsigned int id)
188  {
189  std::lock_guard<std::mutex> lck(configMutex);
190  canIdDeleteUnsafe(id);
191  }
192 
193  yarp::dev::ICanBus* getCanBus()
194  {
195  return theCanBus;
196  }
197 
198  yarp::dev::ICanBufferFactory* getCanBufferFactory()
199  {
200  if (!theBufferFactory)
201  {
202  yError("SharedCanBus error: no buffer factory\n");
203  }
204 
205  return theBufferFactory;
206  }
207 
208  yarp::dev::ICanBusErrors* getCanBusErrors()
209  {
210  return theCanBusErrors;
211  }
212 
213  bool open(yarp::os::Searchable &config)
214  {
215  std::lock_guard<std::mutex> lck(configMutex);
216 
217  //fprintf(stderr, "sharedCanBus::open() using the following configuration parameters: \n%s\n",config.toString().c_str());
218  if (!config.check("physDevice"))
219  {
220  yError("SharedCanBus::open() could not find low level can driver specification\n");
221  return false;
222  }
223 
224  std::string device=config.find("physDevice").asString();
225 
226  yarp::os::Property prop;
227  prop.fromString(config.toString().c_str());
228 
229  prop.unput("device");
230  prop.unput("subdevice");
231  prop.unput("physDevice");
232 
233  prop.put("device",device.c_str());
234 
235  // low level driver
236  polyDriver.open(prop);
237 
238  if (!polyDriver.isValid())
239  {
240  yError("SharedCanBus: could not instantiate can device\n");
241  return false;
242  }
243 
244  polyDriver.view(theCanBus);
245 
246  if (theCanBus==NULL)
247  {
248  yError("SharedCanBus: could not get ICanBus interface\n");
249  return false;
250  }
251 
252  polyDriver.view(theBufferFactory);
253 
254  if (theBufferFactory==NULL)
255  {
256  yError("SharedCanBus: could not get ICanBufferFactory interface\n");
257  return false;
258  }
259 
260  polyDriver.view(theCanBusErrors);
261 
262  mBufferSize=CAN_DRIVER_BUFFER_SIZE;
263 
264  if (config.check("canRxQueueSize"))
265  {
266  mBufferSize=config.find("canRxQueueSize").asInt32();
267  }
268 
269  readBufferUnion=theBufferFactory->createBuffer(mBufferSize);
270 
271  bool started=start();
272 
273  mDevice=device;
274 
275  if (config.check("canDeviceNum"))
276  {
277  mCanDeviceNum=config.find("canDeviceNum").asInt32();
278  }
279 
280  return started;
281  }
282 
283 private:
284  void canIdDeleteUnsafe(unsigned int id)
285  {
286  if (reqIdsUnion[id]==REQST)
287  {
288  for (int i=0; i<(int)accessPoints.size(); ++i)
289  {
290  if (accessPoints[i]->hasId(id))
291  {
292  return;
293  }
294  }
295 
296  reqIdsUnion[id]=UNREQ;
297 
298  theCanBus->canIdDelete(id);
299  }
300  }
301 
302  int mBufferSize;
303 
304  std::mutex writeMutex;
305  std::mutex configMutex;
306 
307  std::string mDevice;
308  int mCanDeviceNum;
309 
310  yarp::dev::PolyDriver polyDriver;
311 
312  yarp::dev::ICanBus *theCanBus;
313  yarp::dev::ICanBufferFactory *theBufferFactory;
314  yarp::dev::ICanBusErrors *theCanBusErrors;
315 
316  std::vector<yarp::dev::CanBusAccessPoint*> accessPoints;
317 
318  yarp::dev::CanBuffer readBufferUnion;
319 
320  char *reqIdsUnion; //[0x800];
321 };
322 
323 class SharedCanBusManager // singleton
324 {
325 public:
327  {
328  for (unsigned int i=0; i<mDevices.size(); ++i)
329  {
330  if (mDevices[i]) delete mDevices[i];
331  }
332 
333  mDevices.clear();
334  }
335 
337  {
338  static SharedCanBusManager instance;
339 
340  return instance;
341  }
342 
343  SharedCanBus* open(yarp::os::Searchable &config)
344  {
345  for (unsigned int i=0; i<mDevices.size(); ++i)
346  {
347  if (mDevices[i]->IloveUmom(config))
348  {
349  return mDevices[i];
350  }
351  }
352 
353  SharedCanBus *scb=new SharedCanBus();
354 
355  if (!scb->open(config))
356  {
357  delete scb;
358  return NULL;
359  }
360 
361  if (config.findGroup("CAN").check("sharedCanPeriod"))
362  {
363  int sharedCanPeriod = config.findGroup("CAN").find("sharedCanPeriod").asInt32();
364  scb->setPeriod((double)sharedCanPeriod/1000.0);
365  //yDebug("SharedCanBus [%d] using custom thread period = %dms\n", scb->getCanDeviceNum(), sharedCanPeriod);///TOBEREMOVED
366  }
367  else
368  {
369  yWarning("SharedCanBus [%d] using default thread period = %dms\n", scb->getCanDeviceNum(), DEFAULT_THREAD_PERIOD);
370  }
371 
372  mDevices.push_back(scb);
373 
374  return scb;
375  }
376 
377 private:
379  {
380  mDevices.clear();
381  }
382 
383  std::vector<SharedCanBus*> mDevices;
384 };
385 
387 // CanBusAccessPoint methods
389 
390 bool yarp::dev::CanBusAccessPoint::open(yarp::os::Searchable& config)
391 {
393 
394  if (!mSharedPhysDevice) return false;
395 
396  mBufferSize=(unsigned int)(mSharedPhysDevice->getBufferSize());
397 
399 
401 
402  return true;
403 }
404 
406 {
407  if (!mSharedPhysDevice) return false;
408 
409  mSharedPhysDevice->detachAccessPoint(this);
410 
411  return true;
412 }
413 
414 bool yarp::dev::CanBusAccessPoint::canWrite(const CanBuffer &msgs, unsigned int size, unsigned int *sent, bool wait)
415 {
416  if (!mSharedPhysDevice) return false;
417 
418  return mSharedPhysDevice->canWrite(msgs,size,sent,wait,this);
419 }
420 
422 {
423  if (!mSharedPhysDevice) return false;
424 
425  return mSharedPhysDevice->getCanBus()->canGetBaudRate(rate);
426 }
427 
429 {
430  if (!mSharedPhysDevice) return false;
431 
432  if (id>=0x800)
433  {
434  yError("SharedCanBus: Id=%d is out of 11 bit address range\n",id);
435  return false;
436  }
437 
438  reqIds[id]=REQST;
439 
440  mSharedPhysDevice->canIdAdd(id);
441 
442  return true;
443 }
444 
446 {
447  if (!mSharedPhysDevice) return false;
448 
449  if (id>=0x800)
450  {
451  yError("SharedCanBus: Id=%d is out of 11 bit address range\n",id);
452  return false;
453  }
454 
455  reqIds[id]=UNREQ;
456 
457  mSharedPhysDevice->canIdDelete(id);
458 
459  return true;
460 }
461 
462 yarp::dev::CanBuffer yarp::dev::CanBusAccessPoint::createBuffer(int nmessage)
463 {
464  yarp::dev::CanBuffer cb;
465 
466  if (mSharedPhysDevice) cb = mSharedPhysDevice->getCanBufferFactory()->createBuffer(nmessage);
467 
468  return cb;
469 }
470 
472 {
473  if (!mSharedPhysDevice) return;
474 
475  yarp::dev::ICanBufferFactory* tmp = mSharedPhysDevice->getCanBufferFactory();
476 
477  if (tmp) tmp->destroyBuffer(msgs);
478 }
const int DEFAULT_THREAD_PERIOD
const int CAN_DRIVER_BUFFER_SIZE
#define REQST
Definition: SharedCanBus.h:29
#define UNREQ
Copyright (C) 2012 RobotCub Consortium.
Definition: SharedCanBus.h:28
static SharedCanBusManager & getInstance()
SharedCanBus * open(yarp::os::Searchable &config)
bool open(yarp::os::Searchable &config)
void canIdAdd(unsigned int id)
bool canWrite(const yarp::dev::CanBuffer &msgs, unsigned int size, unsigned int *sent, bool wait, yarp::dev::CanBusAccessPoint *pFrom)
yarp::dev::ICanBusErrors * getCanBusErrors()
yarp::dev::ICanBus * getCanBus()
yarp::dev::ICanBufferFactory * getCanBufferFactory()
int getBufferSize()
void attachAccessPoint(yarp::dev::CanBusAccessPoint *ap)
int getCanDeviceNum()
void detachAccessPoint(yarp::dev::CanBusAccessPoint *ap)
bool IloveUmom(yarp::os::Searchable &config)
void canIdDelete(unsigned int id)
sharedcan : implements ICanBus interface for multiple access from a single access can driver (for exa...
Definition: SharedCanBus.h:54
virtual void destroyBuffer(CanBuffer &msgs)
virtual bool open(yarp::os::Searchable &config)
virtual bool canGetBaudRate(unsigned int *rate)
SharedCanBus * mSharedPhysDevice
Definition: SharedCanBus.h:181
bool hasId(unsigned int id)
Definition: SharedCanBus.h:78
virtual bool canIdAdd(unsigned int id)
virtual bool canWrite(const CanBuffer &msgs, unsigned int size, unsigned int *sent, bool wait=false)
virtual bool canIdDelete(unsigned int id)
virtual CanBuffer createBuffer(int nmessage)
int n
static int stop
Definition: iCub_Sim.cpp:41