Fawkes API  Fawkes Development Version
bimanual_act_thread.cpp
1 
2 /***************************************************************************
3  * bimanual_act_thread.cpp - Jaco plugin act-thread for coordinated bimanual manipulation
4  *
5  * Created: Mon Sep 29 03:13:20 2014
6  * Copyright 2014 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 "bimanual_act_thread.h"
24 
25 #include "bimanual_goto_thread.h"
26 #include "bimanual_openrave_thread.h"
27 
28 #include <interfaces/JacoBimanualInterface.h>
29 
30 using namespace fawkes;
31 
32 /** @class JacoBimanualActThread "bimanual_act_thread.h"
33  * Jaco Arm act-thread for coordinate bimanual manipulation.
34  *
35  * @author Bahram Maleki-Fard
36  */
37 
38 /** Constructor.
39  * @param arms pointer to jaco_dual_arm_t struct, to be used in this thread
40  */
42 : Thread("JacoBimanualActThread", Thread::OPMODE_WAITFORWAKEUP),
43  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT),
44  arms_(arms)
45 {
46 }
47 
48 /** Destructor. */
50 {
51 }
52 
53 /** Initialize. */
54 void
56 {
57  // open interface for writing
58  arms_->iface = blackboard->open_for_writing<JacoBimanualInterface>("JacoArm Bimanual");
59 }
60 
61 /** Finalize. */
62 void
64 {
65  arms_->goto_thread = NULL;
66  arms_->openrave_thread = NULL;
67 
68  try {
69  blackboard->close(arms_->iface);
70  } catch (fawkes::Exception &e) {
71  logger->log_warn(name(),
72  "Could not close JacoBimanualInterface interface. Er:%s",
73  e.what_no_backtrace());
74  }
75 }
76 
77 /** Main loop. */
78 void
80 {
81  if (arms_->openrave_thread == NULL || arms_->goto_thread == NULL)
82  return;
83 
84  while (!arms_->iface->msgq_empty()) {
85  Message *m = arms_->iface->msgq_first(m);
86  arms_->iface->set_msgid(m->id());
87  arms_->iface->set_final(false);
88  arms_->iface->set_error_code(JacoBimanualInterface::ERROR_NONE);
89  //~ arms_->iface->write();
90 
93  logger->log_debug(name(), "SetPlannerParamsMessage rcvd. params:%s", msg->params());
94 
95 #ifdef HAVE_OPENRAVE
97 #endif
98 
101  logger->log_debug(name(), "SetConstrainedMessage rcvd. Enabled:%u", msg->is_constrained());
102 
103 #ifdef HAVE_OPENRAVE
105 #endif
106 
109  logger->log_debug(name(),
110  "CartesianGotoMessage rcvd. left: x:%f y:%f z:%f e1:%f e2:%f e3:%f",
111  msg->l_x(),
112  msg->l_y(),
113  msg->l_z(),
114  msg->l_e1(),
115  msg->l_e2(),
116  msg->l_e3());
117  logger->log_debug(name(),
118  "CartesianGotoMessage right: x:%f y:%f z:%f e1:%f e2:%f e3:%f",
119  msg->r_x(),
120  msg->r_y(),
121  msg->r_z(),
122  msg->r_e1(),
123  msg->r_e2(),
124  msg->r_e3());
125 #ifdef HAVE_OPENRAVE
126  logger->log_debug(name(),
127  "CartesianGotoMessage is being passed to openrave",
128  arms_->iface->id());
129  // add target to OpenRAVE queue for planning
130  bool s = arms_->openrave_thread->add_target(msg->l_x(),
131  msg->l_y(),
132  msg->l_z(),
133  msg->l_e1(),
134  msg->l_e2(),
135  msg->l_e3(),
136  msg->r_x(),
137  msg->r_y(),
138  msg->r_z(),
139  msg->r_e1(),
140  msg->r_e2(),
141  msg->r_e3());
142  if (!s) {
143  arms_->iface->set_error_code(JacoBimanualInterface::ERROR_NO_IK);
144  logger->log_warn(name(),
145  "Failed executing CartesianGotoMessage, could not find IK solution");
146  }
147 #else
148  logger->log_warn(name(),
149  "OpenRAVE not found. Cannot plan coordinated trajectories. Skipping!");
150 #endif
151 
154  logger->log_debug(name(),
155  "MoveGripperMessage rcvd. left: f1:%f f2:%f f3:%f",
156  msg->l_finger1(),
157  msg->l_finger2(),
158  msg->l_finger3());
159  logger->log_debug(name(),
160  "MoveGripperMessage right: f1:%f f2:%f f3:%f",
161  msg->r_finger1(),
162  msg->r_finger2(),
163  msg->r_finger3());
164 
165  arms_->goto_thread->move_gripper(msg->l_finger1(),
166  msg->l_finger2(),
167  msg->l_finger3(),
168  msg->r_finger2(),
169  msg->r_finger2(),
170  msg->r_finger3());
171 
172  } else {
173  logger->log_warn(name(), "Unknown message received. Skipping");
174  }
175 
176  arms_->iface->msgq_pop();
177  }
178 
179  arms_->iface->set_final(arms_->goto_thread->final());
180  arms_->iface->write();
181 }
virtual void init()
Initialize.
virtual void finalize()
Finalize.
virtual void loop()
Main loop.
JacoBimanualActThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
virtual ~JacoBimanualActThread()
Destructor.
virtual bool final()
Check if arm is final.
virtual void move_gripper(float l_f1, float l_f2, float l_f3, float r_f1, float r_f2, float r_f3)
Moves only the gripper of both arms.
virtual bool add_target(float l_x, float l_y, float l_z, float l_e1, float l_e2, float l_e3, float r_x, float r_y, float r_z, float r_e1, float r_e2, float r_e3)
Add target for coordinated manipulation to the queue.
virtual void set_constrained(bool enable)
Enable/Disable constrained planning.
virtual void set_plannerparams(const std::string &params)
Set planner parameters.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
const char * id() const
Get identifier of interface.
Definition: interface.cpp:661
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
CartesianGotoMessage Fawkes BlackBoard Interface Message.
MoveGripperMessage Fawkes BlackBoard Interface Message.
SetConstrainedMessage Fawkes BlackBoard Interface Message.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
JacoBimanualInterface Fawkes BlackBoard Interface.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_final(const bool new_final)
Set final value.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
unsigned int id() const
Get message ID.
Definition: message.cpp:181
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:113
JacoBimanualInterface * iface
interface used for coordinated manipulation.
Definition: types.h:116
JacoBimanualGotoThread * goto_thread
GotoThread for coordinated manipulation.
Definition: types.h:117
JacoBimanualOpenraveThread * openrave_thread
OpenraveThread for coordinated manipulation.
Definition: types.h:118