Fawkes API  Fawkes Development Version
imu_plugin.cpp
1 
2 /***************************************************************************
3  * imu_plugin.cpp - Fawkes IMU Plugin
4  *
5  * Created: Sun Jun 22 21:41:38 2014
6  * Copyright 2006-2014 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "acquisition_thread.h"
23 #include "sensor_thread.h"
24 
25 #include <core/plugin.h>
26 #ifdef HAVE_CRUIZCORE
27 # include "imu_cruizcore_xg1010.h"
28 #endif
29 
30 #include <memory>
31 #include <set>
32 
33 using namespace fawkes;
34 
35 /** IMU driver plugin.
36  * @author Tim Niemueller
37  */
38 class IMUPlugin : public fawkes::Plugin
39 {
40 public:
41  /** Constructor.
42  * @param config Fawkes configuration
43  */
44  explicit IMUPlugin(Configuration *config) : Plugin(config)
45  {
46  std::set<std::string> configs;
47  std::set<std::string> ignored_configs;
48 
49  std::string prefix = "/hardware/imu/";
50 
51 #if __cplusplus >= 201103L
52  std::unique_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
53 #else
54  std::auto_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
55 #endif
56  while (i->next()) {
57  std::string cfg_name = std::string(i->path()).substr(prefix.length());
58  cfg_name = cfg_name.substr(0, cfg_name.find("/"));
59 
60  if ((configs.find(cfg_name) == configs.end())
61  && (ignored_configs.find(cfg_name) == ignored_configs.end())) {
62  std::string cfg_prefix = prefix + cfg_name + "/";
63 
64  bool active = true;
65  try {
66  active = config->get_bool((cfg_prefix + "active").c_str());
67  } catch (Exception &e) {
68  } // ignored, assume enabled
69 
70  try {
71  if (active) {
72  std::string type = config->get_string((cfg_prefix + "type").c_str());
73  bool continuous = false;
74  try {
75  continuous = config->get_bool((cfg_prefix + "continuous").c_str());
76  } catch (Exception &e) {
77  } // ignored, use default
78 
79  //printf("Adding IMU acquisition thread for %s\n", cfg_name.c_str());
80  IMUAcquisitionThread *aqt = NULL;
81 #ifdef HAVE_CRUIZCORE
82  if (type == "CruizCore-XG1010") {
83  aqt = new CruizCoreXG1010AcquisitionThread(cfg_name, cfg_prefix, continuous);
84  } else
85 #endif
86 
87  {
88  throw Exception("Unknown IMU type '%s' for config %s",
89  type.c_str(),
90  cfg_name.c_str());
91  }
92 
93  thread_list.push_back(aqt);
94  if (!continuous) {
95  thread_list.push_back(new IMUSensorThread(cfg_name, cfg_prefix, aqt));
96  }
97 
98  configs.insert(cfg_name);
99  } else {
100  //printf("Ignoring IMU config %s\n", cfg_name.c_str());
101  ignored_configs.insert(cfg_name);
102  }
103  } catch (Exception &e) {
104  for (ThreadList::iterator i = thread_list.begin(); i != thread_list.end(); ++i) {
105  delete *i;
106  }
107  throw;
108  }
109  }
110  }
111 
112  if (thread_list.empty()) {
113  throw Exception("No IMU devices configured, aborting");
114  }
115  }
116 };
117 
118 PLUGIN_DESCRIPTION("Driver for inertial measurement units (IMU)")
119 EXPORT_PLUGIN(IMUPlugin)
IMUSensorThread
Definition: sensor_thread.h:40
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
fawkes::Configuration
Definition: config.h:70
fawkes::Configuration::search
virtual ValueIterator * search(const char *path)=0
IMUAcquisitionThread
Definition: acquisition_thread.h:39
fawkes
CruizCoreXG1010AcquisitionThread
Definition: imu_cruizcore_xg1010.h:44
IMUPlugin
IMU driver plugin.
Definition: imu_plugin.cpp:38
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
IMUPlugin::IMUPlugin
IMUPlugin(Configuration *config)
Constructor.
Definition: imu_plugin.cpp:44
fawkes::Configuration::ValueIterator::path
virtual const char * path() const =0
fawkes::Configuration::ValueIterator::next
virtual bool next()=0
fawkes::Plugin
Definition: plugin.h:39
fawkes::Exception
Definition: exception.h:41