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
30using namespace fawkes;
31
32/** Driver plugin for Robotis dynamixel servos.
33 * @author Tim Niemueller
34 */
36{
37public:
38 /** Constructor.
39 * @param config Fawkes configuration
40 */
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
104PLUGIN_DESCRIPTION("Robotis Dynamixel servo driver plugin")
105EXPORT_PLUGIN(DynamixelPlugin)
Robotis dynamixel servo act thread.
Definition: act_thread.h:36
void add_driver_thread(DynamixelDriverThread *drv_thread)
Add a driver thread to wake in ACT hook.
Definition: act_thread.cpp:55
Driver thread for Robotis dynamixel servos.
Definition: driver_thread.h:56
Driver plugin for Robotis dynamixel servos.
DynamixelPlugin(Configuration *config)
Constructor.
Robotis dynamixel servo sensor thread.
Definition: sensor_thread.h:38
void add_driver_thread(DynamixelDriverThread *drv_thread)
Add a driver thread to wake in SENSOR_ACQUIRE hook.
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.
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.