Fawkes API  Fawkes Development Version
openrave_base_thread.cpp
1 
2 /***************************************************************************
3  * openrave_thread.cpp - Kinova Jaco plugin OpenRAVE base Thread
4  *
5  * Created: Tue Jun 04 13:13:20 2013
6  * Copyright 2013 Bahram Maleki-Fard
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "openrave_base_thread.h"
24 
25 #include <interfaces/JacoInterface.h>
26 #include <core/threading/mutex.h>
27 
28 #include <cmath>
29 #include <stdio.h>
30 #include <cstring>
31 
32 #ifdef HAVE_OPENRAVE
33  #include <plugins/openrave/environment.h>
34  #include <plugins/openrave/robot.h>
35  #include <plugins/openrave/manipulator.h>
36  #include <plugins/openrave/manipulators/kinova_jaco.h>
37  using namespace OpenRAVE;
38 #endif
39 
40 using namespace fawkes;
41 using namespace std;
42 
43 /** @class JacoOpenraveBaseThread "openrave_base_thread.h"
44  * Base Jaco Arm thread, integrating OpenRAVE
45  *
46  * @author Bahram Maleki-Fard
47  */
48 
49 /** Constructor.
50  * @param name thread name
51  */
53  : Thread(name, Thread::OPMODE_CONTINUOUS)
54 {
55 #ifdef HAVE_OPENRAVE
56  __cfg_OR_auto_load_ik = false;
57  __plannerparams = "";
58  __plot_current = false;
59 #endif
60 }
61 
62 
63 /** Destructor. */
65 {
66 #ifdef HAVE_OPENRAVE
67  __viewer_env.env = NULL;
68  __viewer_env.robot = NULL;
69  __viewer_env.manip = NULL;
70 #endif
71 }
72 
73 /** Initializer.
74  * Reads common config entries, and loads the viewer-environment.
75  * It calls the _init() and _load_robot() methods from inherited classes,
76  * which can be used to initialize additional data, and load the robot
77  * into the OpenRAVE environment.
78  */
79 void
81 {
82  __planning_mutex = new Mutex();
83 
84 #ifdef HAVE_OPENRAVE
85  __cfg_OR_use_viewer = config->get_bool("/hardware/jaco/openrave/use_viewer");
86  __cfg_OR_auto_load_ik = config->get_bool("/hardware/jaco/openrave/auto_load_ik");
87  __cfg_OR_sampling = config->get_float("/hardware/jaco/openrave/sampling");
88 
89  __cfg_OR_plot_traj_manip = config->get_bool("/hardware/jaco/openrave/plotting/planned_manipulator");
90  __cfg_OR_plot_traj_joints = config->get_bool("/hardware/jaco/openrave/plotting/planned_joints");
91  __cfg_OR_plot_cur_manip = config->get_bool("/hardware/jaco/openrave/plotting/current_manipulator");
92  __cfg_OR_plot_cur_joints = config->get_bool("/hardware/jaco/openrave/plotting/current_joints");
93 
94  // perform other initialization stuff (for child classes, that do not want to overload "init()")
95  _init();
96 
97  __viewer_env.env = openrave->get_environment();
98  __viewer_env.env->enable_debug();
99  __viewer_env.env->set_name("Viewer");
100 
101  // load robot
102  _load_robot();
103 
104  if( __cfg_OR_use_viewer )
105  openrave->start_viewer();
106 
107  _post_init();
108 #endif
109 }
110 
111 void
113 {
114  delete __planning_mutex;
115  __planning_mutex = NULL;
116 
117 #ifdef HAVE_OPENRAVE
118  __viewer_env.robot = NULL;
119  __viewer_env.manip = NULL;
120  __viewer_env.env = NULL;
121 #endif
122 }
123 
124 /** Set planner parameters.
125  * The parameter string is passed as is to OpenRAVE's BaseManipulator
126  * or DualManipulation module. Errors in the string will result in
127  * planning failures.
128  * @param params parameters string
129  */
130 void
132 {
133 #ifdef HAVE_OPENRAVE
134  __plannerparams = params;
135 #endif
136 }
137 
138 /** Set planner parameters.
139  * The parameter string is passed as is to OpenRAVE's BaseManipulator
140  * or DualManipulation module. Errors in the string will result in
141  * planning failures.
142  * @param params parameters string
143  */
144 void
146 {
147 #ifdef HAVE_OPENRAVE
148  __plannerparams = params;
149 #endif
150 }
151 
152 /** Enable/Disable plotting of the current arm position.
153  * @param enable Set the "enabled" state
154  */
155 void
157 {
158 #ifdef HAVE_OPENRAVE
159  __plot_current = enable;
160 #endif
161 }
162 
virtual ~JacoOpenraveBaseThread()
Destructor.
fawkes::Mutex * __planning_mutex
mutex, used to lock when planning.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
STL namespace.
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void _post_init()
Use this in inheriting classes for post_init stuff, e.g.
virtual void _load_robot()
Use this in inheriting classes to load the OpenRaveRobot.
virtual void finalize()
Finalize the thread.
virtual void set_plannerparams(const std::string &params)
Set planner parameters.
JacoOpenraveBaseThread(const char *name)
Constructor.
Mutex mutual exclusion lock.
Definition: mutex.h:32
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual void _init()
Use this in inheriting classes for additiona initializations.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual void plot_current(bool enable)
Enable/Disable plotting of the current arm position.