virtual PlannerStatus PlanPath(TrajectoryBasePtr ptraj)
{
BOOST_ASSERT(!!_parameters && !!ptraj);
if( ptraj->GetNumWaypoints() < 2 ) {
return PS_Failed;
}
// save velocities
std::vector<KinBody::KinBodyStateSaverPtr> vstatesavers;
std::vector<KinBodyPtr> vusedbodies;
_parameters->_configurationspecification.ExtractUsedBodies(GetEnv(), vusedbodies);
if( vusedbodies.size() == 0 ) {
RAVELOG_WARN("there are no used bodies in this configuration\n");
}
FOREACH(itbody, vusedbodies) {
KinBody::KinBodyStateSaverPtr statesaver;
if( (*itbody)->IsRobot() ) {
statesaver.reset(new RobotBase::RobotStateSaver(RaveInterfaceCast<RobotBase>(*itbody), KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_ActiveDOF|KinBody::Save_ActiveManipulator|KinBody::Save_LinkVelocities));
}
else {
statesaver.reset(new KinBody::KinBodyStateSaver(*itbody, KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_ActiveDOF|KinBody::Save_ActiveManipulator|KinBody::Save_LinkVelocities));
}
vstatesavers.push_back(statesaver);
}
InterfaceBasePtr CreateInterfaceValidated(InterfaceType type, const std::string& interfacename, std::istream& sinput, EnvironmentBasePtr penv)
{
switch(type) {
case PT_Viewer:
#if defined(HAVE_X11_XLIB_H) && defined(Q_WS_X11)
// always check viewers since DISPLAY could change
if ( XOpenDisplay( NULL ) == NULL ) {
RAVELOG_WARN("no display detected, so cannot load viewer");
return InterfaceBasePtr();
}
#endif
if( interfacename == "qtcoin" ) {
// have to lock after initialized since call relies on SoDBP::globalmutex
boost::mutex::scoped_lock lock(g_mutexsoqt);
EnsureSoQtInit();
//SoDBWriteLock dblock;
return InterfaceBasePtr(new QtCoinViewer(penv, sinput));
}
else if( interfacename == "qtcameraviewer" ) {
return InterfaceBasePtr(new QtCameraViewer(penv,sinput));
}
break;
case PT_Module:
if( interfacename == "ivmodelloader" ) {
return CreateIvModelLoader(penv);
}
break;
default:
break;
}
return InterfaceBasePtr();
}
OPENRAVE_PLUGIN_API void DestroyPlugin()
{
if( s_InitRefCount > 0 ) {
RAVELOG_WARN("SoQt releasing all memory\n");
SoQt::done();
s_InitRefCount = 0;
// necessary since QApplication does not destroy all threads when last SoQt viewer is done
//removePostedEvents - sometimes freezes on this function
QApplication::quit();
}
}
void QtOSGViewer::customEvent(QEvent * e)
{
if (e->type() == CALLBACK_EVENT) {
MyCallbackEvent* pe = dynamic_cast<MyCallbackEvent*>(e);
if( !pe ) {
RAVELOG_WARN("got a qt message that isn't of MyCallbackEvent, converting statically (dangerous)\n");
pe = static_cast<MyCallbackEvent*>(e);
}
pe->_fn();
e->setAccepted(true);
}
}
InterfaceBasePtr CreateInterfaceValidated(InterfaceType type, const std::string& interfacename, std::istream& sinput, EnvironmentBasePtr penv)
{
switch(type) {
case PT_Planner:
if( interfacename == "rastar" || interfacename == "ra*" ) {
return CreateRandomizedAStarPlanner(penv,sinput);
}
else if( interfacename == "birrt") {
return InterfaceBasePtr(new BirrtPlanner(penv));
}
else if( interfacename == "rbirrt") {
RAVELOG_WARN("rBiRRT is deprecated, use BiRRT\n");
return InterfaceBasePtr(new BirrtPlanner(penv));
}
else if( interfacename == "basicrrt") {
return InterfaceBasePtr(new BasicRrtPlanner(penv));
}
else if( interfacename == "explorationrrt" ) {
return InterfaceBasePtr(new ExplorationPlanner(penv));
}
else if( interfacename == "graspgradient" ) {
return CreateGraspGradientPlanner(penv,sinput);
}
else if( interfacename == "shortcut_linear" ) {
return CreateShortcutLinearPlanner(penv,sinput);
}
else if( interfacename == "lineartrajectoryretimer" ) {
return CreateLinearTrajectoryRetimer(penv,sinput);
}
else if( interfacename == "workspacetrajectorytracker" ) {
return CreateWorkspaceTrajectoryTracker(penv,sinput);
}
else if( interfacename == "parabolicsmoother" ) {
return CreateParabolicSmoother(penv,sinput);
}
break;
default:
break;
}
return InterfaceBasePtr();
}
extern "C" void openravepy_viewer_sigint_handler(int sig) //, siginfo_t *siginfo, void *context)
{
RAVELOG_VERBOSE("openravepy_viewer_sigint_handler\n");
openravepy::s_nInterruptCount += 1;
//PyErr_SetInterrupt();
//PyGILState_STATE gstate = PyGILState_Ensure();
// try
// {
// exec("raise KeyboardInterrupt()\n"); // boost::python::globals(),locals);
// //exec("import thread; thread.interrupt_main()\n"); // boost::python::globals(),locals);
// }
// catch( const error_already_set& e )
// {
// // should be the interrupt we just raised...
// //RAVELOG_WARN(boost::str(boost::format("failed to throw KeyboardInterrupt from signal handler: %s\n")%e));
// }
////
// PyGILState_Release(gstate);
// FOREACHC(itviewer, openravepy::s_listViewersToQuit) {
// ++(*itviewer)->_nSetInterrupt;
// }
// (*itviewer)->quitmainloop();
// }
// openravepy::s_listViewersToQuit.clear();
// is this call necessary? perhaps could get the C++ planners out of their loops?
// RaveDestroy();
#ifndef _WIN32
//struct sigaction act;
if( sigaction(SIGINT, &openravepy::s_signalActionPrev, NULL) < 0 ) {
RAVELOG_WARN("failed to restore old signal\n");
}
kill(0 /*getpid()*/, SIGINT);
//sigaction(SIGINT,&act,NULL);
#else
signal(SIGINT, openravepy::s_signalActionPrev);
#endif
}
IvJointDragger::IvJointDragger(QtCoinViewerPtr viewer, ItemPtr pItem, int iSelectedLink, float draggerScale, int iJointIndex, bool bHilitJoint) : IvDragger(viewer, pItem, draggerScale)
{
KinBodyItemPtr pbody = boost::dynamic_pointer_cast<KinBodyItem>(pItem);
BOOST_ASSERT( !!pItem );
_trackball = NULL;
_draggerRoot = NULL;
if( !pbody || !pbody->GetBody() ) {
return;
}
if((iSelectedLink < 0)||(iSelectedLink >= (int)pbody->GetBody()->GetLinks().size())) {
return;
}
if((iJointIndex < 0)||(iJointIndex >= (int)pbody->GetBody()->GetJoints().size())) {
return;
}
_iSelectedLink = iSelectedLink;
_iJointIndex = iJointIndex;
KinBody::JointConstPtr pjoint = pbody->GetBody()->GetJoints().at(iJointIndex);
_jointtype = pjoint->GetType();
_dofindex = pjoint->GetDOFIndex();
_jointname = pjoint->GetName();
_jointoffset = 0; //pjoint->GetOffset();
pjoint->GetLimits(_vlower,_vupper);
_pLinkNode = pbody->GetIvLink(iSelectedLink);
if( _pLinkNode == NULL ) {
RAVELOG_WARN("no link is selected\n");
return;
}
Transform tlink = pbody->GetBody()->GetLinks().at(iSelectedLink)->GetTransform();
// create a root node for the dragger nodes
_draggerRoot = new SoSeparator;
SoTransform* draggertrans = new SoTransform();
_pLinkNode->insertChild(_draggerRoot, 1); // insert right after transform
// add a new material to change the color of the nodes being dragged
_bHilitJoint = bHilitJoint;
if (_bHilitJoint) {
_material = new SoMaterial;
_material->set("diffuseColor 0.8 0.6 0.2");
_material->setOverride(true);
_pLinkNode->insertChild(_material, 1);
}
Vector vaxes[3];
for(int i = 0; i < pjoint->GetDOF(); ++i) {
vaxes[i] = tlink.inverse().rotate(pjoint->GetAxis(i));
}
// need to make sure the rotation is pointed towards the joint axis
Vector vnorm = Vector(1,0,0).cross(vaxes[0]);
dReal fsinang = RaveSqrt(vnorm.lengthsqr3());
if( fsinang > 0.0001f ) {
vnorm /= fsinang;
}
else vnorm = Vector(1,0,0);
Vector vtrans = tlink.inverse()*pjoint->GetAnchor();
draggertrans->translation.setValue(vtrans.x, vtrans.y, vtrans.z);
draggertrans->rotation = SbRotation(SbVec3f(vnorm.x, vnorm.y, vnorm.z), atan2f(fsinang,vaxes[0].x));
_draggerRoot->addChild(draggertrans);
// construct an Inventor trackball dragger
float scale = _scale;
_trackball = new SoTrackballDragger;
AABB ab;
_GetBounds(_pLinkNode, ab);
_trackball->scaleFactor.setValue(ab.extents.x * scale, ab.extents.y * scale, ab.extents.z * scale);
_trackball->setAnimationEnabled(false);
_draggerRoot->addChild(_trackball);
// get the material nodes that govern the color of the dragger and
// note the dragger's normal color
const char* rotators[3] = { "XRotator", "YRotator", "ZRotator" };
const char* rotatorsActive[3] = { "XRotatorActive", "YRotatorActive", "ZRotatorActive" };
// enable or disable each axis
for (int i = 0; i < 3; i++) {
if (i < pjoint->GetDOF()) {
SoSeparator *s = (SoSeparator *)_trackball->getPart(rotators[i], false);
_draggerMaterial[i] = (SoMaterial *) s->getChild(0);
_normalColor = _draggerMaterial[i]->diffuseColor[0];
}
else {
// disable the rotator on this axis
_trackball->setPart(rotators[i], NULL);
_trackball->setPart(rotatorsActive[i], NULL);
_draggerMaterial[i] = NULL;
}
}
// add a motion callback handler for the dragger
_trackball->addMotionCallback(_MotionHandler, this);
UpdateDragger();
}
