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>
37using namespace OpenRAVE;
38#endif
39
40using namespace fawkes;
41using 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 */
77void
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
110void
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 */
129void
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 */
143void
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 */
154void
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.