23 #include "bimanual_act_thread.h"
25 #include "bimanual_goto_thread.h"
26 #include "bimanual_openrave_thread.h"
28 #include <interfaces/JacoBimanualInterface.h>
42 :
Thread(
"JacoBimanualActThread",
Thread::OPMODE_WAITFORWAKEUP),
72 "Could not close JacoBimanualInterface interface. Er:%s",
110 "CartesianGotoMessage rcvd. left: x:%f y:%f z:%f e1:%f e2:%f e3:%f",
118 "CartesianGotoMessage right: x:%f y:%f z:%f e1:%f e2:%f e3:%f",
127 "CartesianGotoMessage is being passed to openrave",
145 "Failed executing CartesianGotoMessage, could not find IK solution");
149 "OpenRAVE not found. Cannot plan coordinated trajectories. Skipping!");
155 "MoveGripperMessage rcvd. left: f1:%f f2:%f f3:%f",
160 "MoveGripperMessage right: f1:%f f2:%f f3:%f",
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 ¶ms)
Set planner parameters.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
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.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
bool msgq_first_is()
Check if first message has desired type.
void msgq_pop()
Erase first message from queue.
Message * msgq_first()
Get the first message from the message queue.
void write()
Write from local copy into BlackBoard memory.
const char * id() const
Get identifier of interface.
bool msgq_empty()
Check if queue is empty.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
float r_z() const
Get r_z value.
float r_x() const
Get r_x value.
float l_z() const
Get l_z value.
float r_e2() const
Get r_e2 value.
float l_y() const
Get l_y value.
float l_e2() const
Get l_e2 value.
float l_e3() const
Get l_e3 value.
float l_x() const
Get l_x value.
float l_e1() const
Get l_e1 value.
float r_e1() const
Get r_e1 value.
float r_y() const
Get r_y value.
float r_e3() const
Get r_e3 value.
MoveGripperMessage Fawkes BlackBoard Interface Message.
float r_finger1() const
Get r_finger1 value.
float r_finger3() const
Get r_finger3 value.
float l_finger2() const
Get l_finger2 value.
float r_finger2() const
Get r_finger2 value.
float l_finger3() const
Get l_finger3 value.
float l_finger1() const
Get l_finger1 value.
SetConstrainedMessage Fawkes BlackBoard Interface Message.
bool is_constrained() const
Get constrained value.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
char * params() const
Get params value.
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.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
unsigned int id() const
Get message ID.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
Fawkes library namespace.
Jaco struct containing all components required for a dual-arm setup.
JacoBimanualInterface * iface
interface used for coordinated manipulation.
JacoBimanualGotoThread * goto_thread
GotoThread for coordinated manipulation.
JacoBimanualOpenraveThread * openrave_thread
OpenraveThread for coordinated manipulation.