Fawkes API  Fawkes Development Version
dynamixel_plugin.cpp
1 
2 /***************************************************************************
3  * dynamixel_plugin.cpp - Robotis Servo driver plugin
4  *
5  * Created: Mon Mar 23 20:12:10 2015
6  * Copyright 2011-2015 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 "act_thread.h"
23 #include "driver_thread.h"
24 #include "sensor_thread.h"
25 
26 #include <core/plugin.h>
27 
28 #include <set>
29 
30 using namespace fawkes;
31 
32 /** Driver plugin for Robotis dynamixel servos.
33  * @author Tim Niemueller
34  */
36 {
37 public:
38  /** Constructor.
39  * @param config Fawkes configuration
40  */
41  explicit DynamixelPlugin(Configuration *config) : Plugin(config)
42  {
43  DynamixelSensorThread *sensor_thread = new DynamixelSensorThread();
44  DynamixelActThread * act_thread = new DynamixelActThread();
45 
46  std::set<std::string> configs;
47  std::set<std::string> ignored_configs;
48 
49  std::string prefix = "/dynamixel/";
50 
51  // Read configurations and spawn LaserFilterThreads
52 #if __cplusplus >= 201103L
53  std::unique_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
54 #else
55  std::auto_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
56 #endif
57  while (i->next()) {
58  std::string cfg_name = std::string(i->path()).substr(prefix.length());
59  cfg_name = cfg_name.substr(0, cfg_name.find("/"));
60 
61  if ((configs.find(cfg_name) == configs.end())
62  && (ignored_configs.find(cfg_name) == ignored_configs.end())) {
63  std::string cfg_prefix = prefix + cfg_name + "/";
64 
65  bool active = true;
66  try {
67  active = config->get_bool((cfg_prefix + "active").c_str());
68  } catch (Exception &e) {
69  } // ignored, assume enabled
70 
71  try {
72  if (active) {
73  DynamixelDriverThread *drv_thread = new DynamixelDriverThread(cfg_name, cfg_prefix);
74  act_thread->add_driver_thread(drv_thread);
75  sensor_thread->add_driver_thread(drv_thread);
76  thread_list.push_back(drv_thread);
77  configs.insert(cfg_name);
78  } else {
79  //printf("Ignoring dynamixel config %s\n", cfg_name.c_str());
80  ignored_configs.insert(cfg_name);
81  }
82  } catch (Exception &e) {
83  for (ThreadList::iterator i = thread_list.begin(); i != thread_list.end(); ++i) {
84  delete *i;
85  }
86  delete act_thread;
87  delete sensor_thread;
88  throw;
89  }
90  }
91  }
92 
93  if (thread_list.empty()) {
94  delete act_thread;
95  delete sensor_thread;
96  throw Exception("No active servo configs, aborting");
97  }
98 
99  thread_list.push_back(sensor_thread);
100  thread_list.push_back(act_thread);
101  }
102 };
103 
104 PLUGIN_DESCRIPTION("Robotis Dynamixel servo driver plugin")
105 EXPORT_PLUGIN(DynamixelPlugin)
DynamixelDriverThread
Definition: driver_thread.h:50
DynamixelSensorThread::add_driver_thread
void add_driver_thread(DynamixelDriverThread *drv_thread)
Add a driver thread to wake in SENSOR_ACQUIRE hook.
Definition: sensor_thread.cpp:53
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
DynamixelPlugin::DynamixelPlugin
DynamixelPlugin(Configuration *config)
Constructor.
Definition: dynamixel_plugin.cpp:41
DynamixelPlugin
Driver plugin for Robotis dynamixel servos.
Definition: dynamixel_plugin.cpp:35
fawkes::Configuration
Definition: config.h:68
fawkes::Configuration::search
virtual ValueIterator * search(const char *path)=0
DynamixelActThread
Definition: act_thread.h:31
fawkes
DynamixelSensorThread
Definition: sensor_thread.h:33
DynamixelActThread::add_driver_thread
void add_driver_thread(DynamixelDriverThread *drv_thread)
Add a driver thread to wake in ACT hook.
Definition: act_thread.cpp:55
fawkes::Configuration::ValueIterator::path
virtual const char * path() const =0
fawkes::Configuration::ValueIterator::next
virtual bool next()=0
fawkes::Plugin
Definition: plugin.h:37
fawkes::Exception
Definition: exception.h:39