OpenRAVE::BaseXMLReader::ProcessElement kdata_parser::startElement(const std::string& name, const OpenRAVE::AttributesList& atts)
{
if (name == "spheres")
{
if (this->inside_spheres) RAVELOG_ERROR("you can't have <spheres> inside <spheres>!\n");
this->inside_spheres = true;
return PE_Support;
}
if (name == "sphere")
{
struct sphere * s;
struct sphereelem * e;
if (!this->inside_spheres) { RAVELOG_ERROR("you can't have <sphere> not inside <spheres>!\n"); return PE_Pass; }
s = (struct sphere *) malloc(sizeof(struct sphere));
for(OpenRAVE::AttributesList::const_iterator itatt = atts.begin(); itatt != atts.end(); ++itatt)
{
if (itatt->first=="link")
strcpy(s->linkname, itatt->second.c_str());
else if (itatt->first=="radius")
s->radius = strtod(itatt->second.c_str(), 0);
else if (itatt->first=="pos")
sscanf(itatt->second.c_str(), "%lf %lf %lf", &s->pos[0], &s->pos[1], &s->pos[2]);
else
RAVELOG_ERROR("unknown attribute %s=%s!\n",itatt->first.c_str(),itatt->second.c_str());
}
/* insert at head of kdata list */
e = (struct sphereelem *) malloc(sizeof(struct sphereelem));
e->s = s;
e->next = this->d->sphereelems;
this->d->sphereelems = e;
return PE_Support;
}
return PE_Pass;
}
bool kdata_parser::endElement(const std::string& name)
{
if (name == "orchomp"){ return true; }
if ( name == "ignorables" ){
if (this->inside_spheres){
RAVELOG_ERROR("you can't have </ignorables>"
" inside <spheres>!\n");
}
if (!this->inside_ignorables){
RAVELOG_ERROR("you can't have </ignorables>"
" without matching <ignorables>!\n");
}
this->inside_ignorables = false;
}
else if (name == "ignore")
{
if (this->inside_spheres){
RAVELOG_ERROR("you can't have </ignore> inside <spheres>!\n");
}
if (!this->inside_ignorables){
RAVELOG_ERROR("you can't have </ignore> not"
" inside <ignorables>!\n");
}
}
else if (name == "spheres")
{
if (this->inside_ignorables){
RAVELOG_ERROR("you can't have </spheres> "
"inside <ignorables>!\n");
}
if (!this->inside_spheres){
RAVELOG_ERROR("you can't have </spheres> "
"without matching <spheres>!\n");
}
this->inside_spheres = false;
}
else if (name == "sphere")
{
if (!this->inside_spheres){
RAVELOG_ERROR("you can't have </sphere> inside <ignorables>!\n");
}
if (!this->inside_spheres){
RAVELOG_ERROR("you can't have </sphere> not inside <spheres>!\n");
}
}
else{
RAVELOG_ERROR("unknown field %s\n", name.c_str());
}
return false;
}
bool kdata_parser::endElement(const std::string& name)
{
if (name == "orgpmp2") return true;
if (name == "spheres")
{
if (!this->inside_spheres) RAVELOG_ERROR("you can't have </spheres> without matching <spheres>!\n");
this->inside_spheres = false;
}
else if (name == "sphere")
{
if (!this->inside_spheres) RAVELOG_ERROR("you can't have </sphere> not inside <spheres>!\n");
}
else
RAVELOG_ERROR("unknown field %s\n", name.c_str());
return false;
}
static bool _ViewerCallback(object fncallback, PyEnvironmentBasePtr pyenv, KinBody::LinkPtr plink,RaveVector<float> position,RaveVector<float> direction)
{
object res;
PyGILState_STATE gstate = PyGILState_Ensure();
try {
res = fncallback(openravepy::toPyKinBodyLink(plink,pyenv),toPyVector3(position),toPyVector3(direction));
}
catch(...) {
RAVELOG_ERROR("exception occured in python viewer callback:\n");
PyErr_Print();
}
PyGILState_Release(gstate);
extract<bool> xb(res);
if( xb.check() ) {
return (bool)xb;
}
extract<int> xi(res);
if( xi.check() ) {
return (int)xi;
}
extract<double> xd(res);
if( xd.check() ) {
return (double)xd>0;
}
return true;
}
OpenRAVE::InterfaceBasePtr CreateInterfaceValidated(OpenRAVE::InterfaceType type,
std::string const &interface_name, std::istream &sinput, OpenRAVE::EnvironmentBasePtr env)
{
if (type == OpenRAVE::PT_Sensor && interface_name == "bhtactilesensor") {
std::string node_name, owd_namespace, robot_name, link_prefix;
sinput >> node_name >> owd_namespace >> robot_name >> link_prefix;
// Initialize the ROS node.
RAVELOG_DEBUG("name = %s namespace = %s\n", node_name.c_str(), owd_namespace.c_str());
if (sinput.fail()) {
RAVELOG_ERROR("BHTactileSensor is missing the node_name, owd_namespace, robot_name, or link_prefix parameter(s).\n");
return OpenRAVE::InterfaceBasePtr();
}
if (!ros::isInitialized()) {
int argc = 0;
ros::init(argc, NULL, node_name, ros::init_options::AnonymousName);
RAVELOG_DEBUG("Starting ROS node '%s'.\n", node_name.c_str());
} else {
RAVELOG_DEBUG("Using existing ROS node '%s'\n", ros::this_node::getName().c_str());
}
OpenRAVE::RobotBasePtr robot = env->GetRobot(robot_name);
if (!robot) {
throw OPENRAVE_EXCEPTION_FORMAT("There is no robot named '%s'.",
robot_name.c_str(), OpenRAVE::ORE_InvalidArguments);
}
return boost::make_shared<BHTactileSensor>(env, robot, owd_namespace, link_prefix);
} else {
virtual void demothread(int argc, char ** argv) {
string scenefilename = "data/pa10grasp2.env.xml";
penv->Load(scenefilename);
vector<RobotBasePtr> vrobots;
penv->GetRobots(vrobots);
RobotBasePtr probot = vrobots.at(0);
// find a manipulator chain to move
for(size_t i = 0; i < probot->GetManipulators().size(); ++i) {
if( probot->GetManipulators()[i]->GetName().find("arm") != string::npos ) {
probot->SetActiveManipulator(probot->GetManipulators()[i]);
break;
}
}
RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
// load inverse kinematics using ikfast
ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
penv->Add(pikfast,true,"");
stringstream ssin,ssout;
vector<dReal> vsolution;
ssin << "LoadIKFastSolver " << probot->GetName() << " " << (int)IKP_Transform6D;
if( !pikfast->SendCommand(ssout,ssin) ) {
RAVELOG_ERROR("failed to load iksolver\n");
}
if( !pmanip->GetIkSolver()) {
throw OPENRAVE_EXCEPTION_FORMAT0("need ik solver",ORE_Assert);
}
ModuleBasePtr pbasemanip = RaveCreateModule(penv,"basemanipulation"); // create the module
penv->Add(pbasemanip,true,probot->GetName()); // load the module
while(IsOk()) {
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
// find a new manipulator position and feed that into the planner. If valid, robot will move to it safely.
Transform t = pmanip->GetEndEffectorTransform();
t.trans += Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f);
t.rot = quatMultiply(t.rot,quatFromAxisAngle(Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f)*0.2f));
ssin.str("");
ssin.clear();
ssin << "MoveToHandPosition pose " << t;
// start the planner and run the robot
RAVELOG_INFO("%s\n",ssin.str().c_str());
if( !pbasemanip->SendCommand(ssout,ssin) ) {
continue;
}
}
// unlock the environment and wait for the robot to finish
while(!probot->GetController()->IsDone() && IsOk()) {
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
}
}
}
void QtOSGViewer::_Refresh()
{
_UpdateEnvironment(1.0/60.0);
//UpdateFromModel();
// _posgWidget->update();
{
std::list<UserDataWeakPtr> listRegisteredViewerThreadCallbacks;
{
boost::mutex::scoped_lock lock(_mutexCallbacks);
listRegisteredViewerThreadCallbacks = _listRegisteredViewerThreadCallbacks;
}
FOREACH(it,listRegisteredViewerThreadCallbacks) {
ViewerThreadCallbackDataPtr pdata = boost::dynamic_pointer_cast<ViewerThreadCallbackData>(it->lock());
if( !!pdata ) {
try {
pdata->_callback();
}
catch(const std::exception& e) {
RAVELOG_ERROR(str(boost::format("Viewer Thread Callback Failed with error %s")%e.what()));
}
}
}
}
OpenRAVE::BaseXMLReader::ProcessElement
kdata_parser::startElement(const std::string& name,
const OpenRAVE::AttributesList& atts)
{
if (name == "ignorables"){
if (this->inside_spheres) {
RAVELOG_ERROR("you can't have <ignorables> inside <spheres>!\n");
}
if (this->inside_ignorables) {
RAVELOG_ERROR("you can't have <ignorables> inside <ignorables>!\n");
}
this->inside_ignorables = true;
return PE_Support;
}
else if (name == "ignore"){
if (!this->inside_ignorables) {
RAVELOG_ERROR("you can't have <ignore> "
"not inside <ignorables>!\n");
return PE_Pass;
}
if (this->inside_spheres) {
RAVELOG_ERROR("you can't have <ignore> "
"inside <spheres>!\n");
return PE_Pass;
}
//increase the size of the vector,
// add an ignorable pair to the end.
d->ignorables.resize( d->ignorables.size() + 1 );
//iterate through the arguments, and assign things to the to
for(OpenRAVE::AttributesList::const_iterator itatt = atts.begin();
itatt != atts.end();
++itatt)
{
if (itatt->first=="link1"){
d->ignorables.back().first = itatt->second;
}else if (itatt->first=="link2"){
d->ignorables.back().second = itatt->second;
}else{
RAVELOG_ERROR("unknown attribute %s=%s!\n",
itatt->first.c_str(),itatt->second.c_str());
}
}
return PE_Support;
}
else if (name == "spheres")
{
if (this->inside_spheres) {
RAVELOG_ERROR("you can't have <spheres> inside <spheres>!\n");
}
if (this->inside_ignorables) {
RAVELOG_ERROR("you can't have <spheres> inside <ignorables>!\n");
}
this->inside_spheres = true;
return PE_Support;
}
else if (name == "sphere")
{
if (!this->inside_spheres) {
RAVELOG_ERROR("you can't have <sphere> not inside <spheres>!\n");
return PE_Pass;
}
if (this->inside_ignorables) {
RAVELOG_ERROR("you can't have <sphere> inside <ignorables>!\n");
return PE_Pass;
}
//increase the size of the vector, add a sphere to the end.
d->spheres.resize( d->spheres.size() + 1 );
Sphere & current_sphere = d->spheres.back();
//iterate through the arguments, and assign things to the to
for(OpenRAVE::AttributesList::const_iterator itatt = atts.begin();
itatt != atts.end();
++itatt)
{
if (itatt->first=="link"){
current_sphere.linkname = itatt->second;
}else if (itatt->first=="radius"){
current_sphere.radius = strtod(itatt->second.c_str(), 0);
}else if (itatt->first=="pos"){
double pose[3];
sscanf(itatt->second.c_str(), "%lf %lf %lf",
&pose[0], &pose[1], &pose[2] );
current_sphere.position = OpenRAVE::Vector( pose );
}else if (itatt->first =="inactive"){
if ( itatt->second =="true" ){
current_sphere.inactive = true;
}
}else{
RAVELOG_ERROR("unknown attribute %s=%s!\n",
itatt->first.c_str(),itatt->second.c_str());
}
}
return PE_Support;
}
return PE_Pass;
}
