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 <core/threading/mutex.h>
26 #include <interfaces/JacoInterface.h>
27 
28 #include <cmath>
29 #include <cstring>
30 #include <stdio.h>
31 
32 #ifdef HAVE_OPENRAVE
33 # include <plugins/openrave/environment.h>
34 # include <plugins/openrave/manipulator.h>
35 # include <plugins/openrave/manipulators/kinova_jaco.h>
36 # include <plugins/openrave/robot.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  plot_current_ = false;
58 #endif
59 }
60 
61 /** Destructor. */
63 {
64 #ifdef HAVE_OPENRAVE
65  viewer_env_.env = NULL;
66  viewer_env_.robot = NULL;
67  viewer_env_.manip = NULL;
68 #endif
69 }
70 
71 /** Initializer.
72  * Reads common config entries, and loads the viewer-environment.
73  * It calls the _init() and _load_robot() methods from inherited classes,
74  * which can be used to initialize additional data, and load the robot
75  * into the OpenRAVE environment.
76  */
77 void
79 {
80  planning_mutex_ = new Mutex();
81 
82 #ifdef HAVE_OPENRAVE
83  cfg_OR_use_viewer_ = config->get_bool("/hardware/jaco/openrave/use_viewer");
84  cfg_OR_auto_load_ik_ = config->get_bool("/hardware/jaco/openrave/auto_load_ik");
85  cfg_OR_sampling_ = config->get_float("/hardware/jaco/openrave/sampling");
86 
87  cfg_OR_plot_traj_manip_ =
88  config->get_bool("/hardware/jaco/openrave/plotting/planned_manipulator");
89  cfg_OR_plot_traj_joints_ = config->get_bool("/hardware/jaco/openrave/plotting/planned_joints");
90  cfg_OR_plot_cur_manip_ = config->get_bool("/hardware/jaco/openrave/plotting/current_manipulator");
91  cfg_OR_plot_cur_joints_ = config->get_bool("/hardware/jaco/openrave/plotting/current_joints");
92 
93  // perform other initialization stuff (for child classes, that do not want to overload "init()")
94  _init();
95 
96  viewer_env_.env = openrave->get_environment();
97  viewer_env_.env->enable_debug();
98  viewer_env_.env->set_name("Viewer");
99 
100  // load robot
101  _load_robot();
102 
103  if (cfg_OR_use_viewer_)
104  openrave->start_viewer();
105 
106  _post_init();
107 #endif
108 }
109 
110 void
112 {
113  delete planning_mutex_;
114  planning_mutex_ = NULL;
115 
116 #ifdef HAVE_OPENRAVE
117  viewer_env_.robot = NULL;
118  viewer_env_.manip = NULL;
119  viewer_env_.env = NULL;
120 #endif
121 }
122 
123 /** Set planner parameters.
124  * The parameter string is passed as is to OpenRAVE's BaseManipulator
125  * or DualManipulation module. Errors in the string will result in
126  * planning failures.
127  * @param params parameters string
128  */
129 void
131 {
132 #ifdef HAVE_OPENRAVE
133  plannerparams_ = params;
134 #endif
135 }
136 
137 /** Set planner parameters.
138  * The parameter string is passed as is to OpenRAVE's BaseManipulator
139  * or DualManipulation module. Errors in the string will result in
140  * planning failures.
141  * @param params parameters string
142  */
143 void
145 {
146 #ifdef HAVE_OPENRAVE
147  plannerparams_ = params;
148 #endif
149 }
150 
151 /** Enable/Disable plotting of the current arm position.
152  * @param enable Set the "enabled" state
153  */
154 void
156 {
157 #ifdef HAVE_OPENRAVE
158  plot_current_ = enable;
159 #endif
160 }
JacoOpenraveBaseThread(const char *name)
Constructor.
virtual void plot_current(bool enable)
Enable/Disable plotting of the current arm position.
fawkes::Mutex * planning_mutex_
mutex, used to lock when planning.
virtual void _init()
Use this in inheriting classes for additiona initializations.
virtual void _post_init()
Use this in inheriting classes for post_init stuff, e.g.
virtual ~JacoOpenraveBaseThread()
Destructor.
virtual void finalize()
Finalize the thread.
virtual void _load_robot()
Use this in inheriting classes to load the OpenRaveRobot.
virtual void set_plannerparams(const std::string &params)
Set planner parameters.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
Mutex mutual exclusion lock.
Definition: mutex.h:33
Thread class encapsulation of pthreads.
Definition: thread.h:46
Fawkes library namespace.