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 #include "bimanual_goto_thread.h"
25 #include "bimanual_openrave_thread.h"
26 
27 #include <interfaces/JacoBimanualInterface.h>
28 
29 using namespace fawkes;
30 
31 /** @class JacoBimanualActThread "bimanual_act_thread.h"
32  * Jaco Arm act-thread for coordinate bimanual manipulation.
33  *
34  * @author Bahram Maleki-Fard
35  */
36 
37 /** Constructor.
38  * @param arms pointer to jaco_dual_arm_t struct, to be used in this thread
39  */
41  : Thread("JacoBimanualActThread", Thread::OPMODE_WAITFORWAKEUP),
42  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT),
43  __arms( arms )
44 {
45 }
46 
47 /** Destructor. */
49 {
50 }
51 
52 /** Initialize. */
53 void
55 {
56  // open interface for writing
57  __arms->iface = blackboard->open_for_writing<JacoBimanualInterface>("JacoArm Bimanual");
58 }
59 
60 /** Finalize. */
61 void
63 {
64  __arms->goto_thread = NULL;
65  __arms->openrave_thread = NULL;
66 
67  try {
68  blackboard->close(__arms->iface);
69  } catch(fawkes::Exception& e) {
70  logger->log_warn(name(), "Could not close JacoBimanualInterface interface. Er:%s", e.what_no_backtrace());
71  }
72 }
73 
74 /** Main loop. */
75 void
77 {
78  if( __arms->openrave_thread==NULL || __arms->goto_thread==NULL )
79  return;
80 
81  while( ! __arms->iface->msgq_empty() ) {
82  Message *m = __arms->iface->msgq_first(m);
83  __arms->iface->set_msgid(m->id());
84  __arms->iface->set_final(false);
85  __arms->iface->set_error_code(JacoBimanualInterface::ERROR_NONE);
86  //~ __arms->iface->write();
87 
90  logger->log_debug(name(), "SetPlannerParamsMessage rcvd. params:%s", msg->params());
91 
92  #ifdef HAVE_OPENRAVE
93  __arms->openrave_thread->set_plannerparams(msg->params());
94  #endif
95 
98  logger->log_debug(name(), "SetConstrainedMessage rcvd. Enabled:%u", msg->is_constrained());
99 
100  #ifdef HAVE_OPENRAVE
102  #endif
103 
106  logger->log_debug(name(), "CartesianGotoMessage rcvd. left: x:%f y:%f z:%f e1:%f e2:%f e3:%f",
107  msg->l_x(), msg->l_y(), msg->l_z(), msg->l_e1(), msg->l_e2(), msg->l_e3());
108  logger->log_debug(name(), "CartesianGotoMessage right: x:%f y:%f z:%f e1:%f e2:%f e3:%f",
109  msg->r_x(), msg->r_y(), msg->r_z(), msg->r_e1(), msg->r_e2(), msg->r_e3());
110  #ifdef HAVE_OPENRAVE
111  logger->log_debug(name(), "CartesianGotoMessage is being passed to openrave", __arms->iface->id());
112  // add target to OpenRAVE queue for planning
113  bool s = __arms->openrave_thread->add_target(msg->l_x(), msg->l_y(), msg->l_z(), msg->l_e1(), msg->l_e2(), msg->l_e3(),
114  msg->r_x(), msg->r_y(), msg->r_z(), msg->r_e1(), msg->r_e2(), msg->r_e3());
115  if( !s ) {
116  __arms->iface->set_error_code(JacoBimanualInterface::ERROR_NO_IK);
117  logger->log_warn(name(), "Failed executing CartesianGotoMessage, could not find IK solution");
118  }
119  #else
120  logger->log_warn(name(), "OpenRAVE not found. Cannot plan coordinated trajectories. Skipping!");
121  #endif
122 
125  logger->log_debug(name(), "MoveGripperMessage rcvd. left: f1:%f f2:%f f3:%f",
126  msg->l_finger1(), msg->l_finger2(), msg->l_finger3());
127  logger->log_debug(name(), "MoveGripperMessage right: f1:%f f2:%f f3:%f",
128  msg->r_finger1(), msg->r_finger2(), msg->r_finger3());
129 
130  __arms->goto_thread->move_gripper(msg->l_finger1(), msg->l_finger2(), msg->l_finger3(),
131  msg->r_finger2(), msg->r_finger2(), msg->r_finger3());
132 
133  } else {
134  logger->log_warn(name(), "Unknown message received. Skipping");
135  }
136 
137  __arms->iface->msgq_pop();
138  }
139 
140  __arms->iface->set_final(__arms->goto_thread->final());
141  __arms->iface->write();
142 }
143 
MoveGripperMessage Fawkes BlackBoard Interface Message.
void set_final(const bool new_final)
Set final value.
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:197
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
virtual ~JacoBimanualActThread()
Destructor.
JacoBimanualGotoThread * goto_thread
GotoThread for coordinated manipulation.
Definition: types.h:116
JacoBimanualInterface * iface
interface used for coordinated manipulation.
Definition: types.h:115
Fawkes library namespace.
JacoBimanualInterface Fawkes BlackBoard Interface.
const char * id() const
Get identifier of interface.
Definition: interface.cpp:661
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void loop()
Main loop.
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
JacoBimanualOpenraveThread * openrave_thread
OpenraveThread for coordinated manipulation.
Definition: types.h:117
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1193
SetConstrainedMessage Fawkes BlackBoard Interface Message.
virtual bool final()
Check if arm is final.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1180
virtual void set_constrained(bool enable)
Enable/Disable constrained planning.
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.
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:686
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
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 void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void set_plannerparams(const std::string &params)
Set planner parameters.
JacoBimanualActThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:112
virtual void init()
Initialize.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
virtual void finalize()
Finalize.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
void set_msgid(const uint32_t new_msgid)
Set msgid value.
virtual void close(Interface *interface)=0
Close interface.