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
33using namespace fawkes;
34
35/** IMU driver plugin.
36 * @author Tim Niemueller
37 */
39{
40public:
41 /** Constructor.
42 * @param config Fawkes configuration
43 */
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
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
118PLUGIN_DESCRIPTION("Driver for inertial measurement units (IMU)")
119EXPORT_PLUGIN(IMUPlugin)
IMU acquisition thread for CruizCore XG1010 gyros.
IMU acqusition thread.
IMU driver plugin.
Definition: imu_plugin.cpp:39
IMUPlugin(Configuration *config)
Constructor.
Definition: imu_plugin.cpp:44
IMU sensor thread.
Definition: sensor_thread.h:45
Interface for configuration handling.
Definition: config.h:68
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Plugin interface class.
Definition: plugin.h:34
ThreadList thread_list
Thread list member.
Definition: plugin.h:53
Configuration * config
Fawkes configuration.
Definition: plugin.h:58
void push_back(Thread *thread)
Add thread to the end.
Fawkes library namespace.