void ViewerTest::SetViewer(OpenRAVE::EnvironmentBasePtr &penv, const std::string &viewername)
{
OpenRAVE::ViewerBasePtr viewer = OpenRAVE::RaveCreateViewer(penv, viewername);
BOOST_ASSERT(!!viewer);
// attach it to the environment:
penv->Add(viewer);
// finally call the viewer's infinite loop (this is why a separate thread is needed)
bool showgui = true;
viewer->main(showgui);
}
// this method is copied from OctomapServer.cpp and modified
void OctomapInterface::insertScan(const tf::Point& sensorOriginTf, const PCLPointCloud& ground, const PCLPointCloud& nonground){
octomap::point3d sensorOrigin = octomap::pointTfToOctomap(sensorOriginTf);
if (!m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMin)
|| !m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMax))
{
ROS_ERROR_STREAM("Could not generate Key for origin "<<sensorOrigin);
}
//
// add mask information to the point cloud
//
OpenRAVE::EnvironmentBasePtr penv = GetEnv();
// create a vector of masked bodies
std::vector<OpenRAVE::KinBodyPtr > maskedBodies;
{
boost::mutex::scoped_lock(m_maskedObjectsMutex);
for (std::vector<std::string >::const_iterator o_it = m_maskedObjects.begin(); o_it != m_maskedObjects.end(); o_it++)
{
KinBodyPtr pbody = penv->GetKinBody(*o_it);
if (pbody.get() != NULL)
{
maskedBodies.push_back( pbody );
}
}
}
int numBodies = maskedBodies.size();
// instead of direct scan insertion, compute update to filter ground:
octomap::KeySet free_cells, occupied_cells;
// insert ground points only as free:
for (PCLPointCloud::const_iterator it = ground.begin(); it != ground.end(); ++it){
octomap::point3d point(it->x, it->y, it->z);
// maxrange check
if ((m_maxRange > 0.0) && ((point - sensorOrigin).norm() > m_maxRange) ) {
point = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange;
}
// only clear space (ground points)
if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){
free_cells.insert(m_keyRay.begin(), m_keyRay.end());
}
octomap::OcTreeKey endKey;
if (m_octree->coordToKeyChecked(point, endKey)){
updateMinKey(endKey, m_updateBBXMin);
updateMaxKey(endKey, m_updateBBXMax);
} else{
ROS_ERROR_STREAM("Could not generate Key for endpoint "<<point);
}
}
penv->Add(m_sphere, true);
OpenRAVE::Transform tr;
// all other points: free on ray, occupied on endpoint:
for (PCLPointCloud::const_iterator it = nonground.begin(); it != nonground.end(); ++it){
octomap::point3d point(it->x, it->y, it->z);
tr.trans.x = it->x;
tr.trans.y = it->y;
tr.trans.z = it->z;
m_sphere->SetTransform(tr);
bool maskedHit = false;
for (int b_idx = 0; b_idx < numBodies; b_idx++)
{
if (penv->CheckCollision(maskedBodies[b_idx], m_sphere))
{
maskedHit = true;
break;
}
}
if (maskedHit)
{
// free cells
if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){
free_cells.insert(m_keyRay.begin(), m_keyRay.end());
}
}
else if ((m_maxRange < 0.0) || ((point - sensorOrigin).norm() <= m_maxRange) ) { // maxrange check
// free cells
if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){
free_cells.insert(m_keyRay.begin(), m_keyRay.end());
}
// occupied endpoint
octomap::OcTreeKey key;
if (m_octree->coordToKeyChecked(point, key)){
occupied_cells.insert(key);
updateMinKey(key, m_updateBBXMin);
updateMaxKey(key, m_updateBBXMax);
}
} else {// ray longer than maxrange:;
octomap::point3d new_end = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange;
if (m_octree->computeRayKeys(sensorOrigin, new_end, m_keyRay)){
free_cells.insert(m_keyRay.begin(), m_keyRay.end());
octomap::OcTreeKey endKey;
if (m_octree->coordToKeyChecked(new_end, endKey)){
updateMinKey(endKey, m_updateBBXMin);
updateMaxKey(endKey, m_updateBBXMax);
} else{
ROS_ERROR_STREAM("Could not generate Key for endpoint "<<new_end);
}
}
}
}
penv->Remove(m_sphere);
// mark free cells only if not seen occupied in this cloud
for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it){
if (occupied_cells.find(*it) == occupied_cells.end()){
m_octree->updateNode(*it, false);
}
}
// now mark all occupied cells:
for (octomap::KeySet::iterator it = occupied_cells.begin(), end=free_cells.end(); it!= end; it++) {
m_octree->updateNode(*it, true);
}
// TODO: eval lazy+updateInner vs. proper insertion
// non-lazy by default (updateInnerOccupancy() too slow for large maps)
//m_octree->updateInnerOccupancy();
octomap::point3d minPt, maxPt;
ROS_DEBUG_STREAM("Bounding box keys (before): " << m_updateBBXMin[0] << " " <<m_updateBBXMin[1] << " " << m_updateBBXMin[2] << " / " <<m_updateBBXMax[0] << " "<<m_updateBBXMax[1] << " "<< m_updateBBXMax[2]);
// TODO: snap max / min keys to larger voxels by m_maxTreeDepth
// if (m_maxTreeDepth < 16)
// {
// OcTreeKey tmpMin = getIndexKey(m_updateBBXMin, m_maxTreeDepth); // this should give us the first key at depth m_maxTreeDepth that is smaller or equal to m_updateBBXMin (i.e. lower left in 2D grid coordinates)
// OcTreeKey tmpMax = getIndexKey(m_updateBBXMax, m_maxTreeDepth); // see above, now add something to find upper right
// tmpMax[0]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
// tmpMax[1]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
// tmpMax[2]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
// m_updateBBXMin = tmpMin;
// m_updateBBXMax = tmpMax;
// }
// TODO: we could also limit the bbx to be within the map bounds here (see publishing check)
minPt = m_octree->keyToCoord(m_updateBBXMin);
maxPt = m_octree->keyToCoord(m_updateBBXMax);
ROS_DEBUG_STREAM("Updated area bounding box: "<< minPt << " - "<<maxPt);
ROS_DEBUG_STREAM("Bounding box keys (after): " << m_updateBBXMin[0] << " " <<m_updateBBXMin[1] << " " << m_updateBBXMin[2] << " / " <<m_updateBBXMax[0] << " "<<m_updateBBXMax[1] << " "<< m_updateBBXMax[2]);
if (m_compressMap)
m_octree->prune();
}
TEST_F(FeatureConstraintsTest, constrainConfiguration)
{
// setting up environment and robot
OpenRAVE::RaveInitialize();
OpenRAVE::EnvironmentBasePtr env = OpenRAVE::RaveCreateEnvironment();
OpenRAVE::RobotBasePtr herb = env->ReadRobotXMLFile("robots/herb2_padded_nosensors.robot.xml");
env->Add(herb);
OpenRAVE::RobotBase::ManipulatorPtr right_arm;
std::vector<OpenRAVE::RobotBase::ManipulatorPtr> herb_manipulators = herb->GetManipulators();
for (unsigned int i=0; i<herb_manipulators.size(); i++)
{
if(herb_manipulators[i] && (herb_manipulators[i]->GetName().compare("right_wam") == 0))
{
right_arm = herb_manipulators[i];
}
}
herb->SetActiveManipulator(right_arm);
herb->SetActiveDOFs(right_arm->GetArmIndices());
// putting the bottle in the robot's right hand
OpenRAVE::KinBodyPtr bottle_body = OpenRAVE::RaveCreateKinBody(env, "");
ASSERT_TRUE(env->ReadKinBodyURI(bottle_body, "objects/household/fuze_bottle.kinbody.xml"));
bottle_body->SetName("bottle");
env->Add(bottle_body);
OpenRAVE::geometry::RaveTransformMatrix<OpenRAVE::dReal> m;
m.rotfrommat(-0.86354026, 0.50427591, 0.00200643,
-0.25814712, -0.44547139, 0.85727201,
0.43319543, 0.73977094, 0.51485985);
m.trans.x = 1.10322189;
m.trans.y = -0.24693695;
m.trans.z = 0.87205762;
bottle_body->SetTransform(OpenRAVE::Transform(m));
OpenRAVE::Transform bottle_transform1 = bottle_body->GetTransform();
ASSERT_TRUE(herb->Grab(bottle_body));
// adding a flying pan
OpenRAVE::KinBodyPtr pan_body = OpenRAVE::RaveCreateKinBody(env, "");
ASSERT_TRUE(env->ReadKinBodyURI(pan_body, "objects/household/pan.kinbody.xml"));
pan_body->SetName("pan");
env->Add(pan_body);
m.rotfrommat(2.89632833e-03, 9.99995806e-01, 1.19208782e-07,
-9.99995806e-01, 2.89632833e-03, -1.18864027e-07,
-1.19208797e-07, -1.18864013e-07, 1.00000000e+00);
m.trans.x = 0.58;
m.trans.y = -0.02;
m.trans.z = 0.33;
pan_body->SetTransform(OpenRAVE::Transform(m));
// giving constraints access to environment and robot
RobotPtr robot = RobotPtr(new Robot(herb));
EnvironmentPtr environment = EnvironmentPtr(new Environment(env));
constraints.setRobot(robot);
constraints.setEnvironment(environment);
// first test
constraints.calculateConstraintValues();
EXPECT_FALSE(constraints.areConstraintsFulfilled());
// moving robot and bottle to new start configuration
std::vector<double> new_config;
new_config.push_back(2.0);
new_config.push_back(1.0);
new_config.push_back(-0.8);
new_config.push_back(1.0);
new_config.push_back(-1.0);
new_config.push_back(0.0);
new_config.push_back(0.0);
robot->setJointValues(new_config);
constraints.setJointValues(robot->getJointValues());
constraints.calculateConstraintValues();
// test we've really moved
OpenRAVE::Transform bottle_transform2 = bottle_body->GetTransform();
ASSERT_GT((bottle_transform1.trans - bottle_transform2.trans).lengthsqr3(), 0.1);
ASSERT_GT((bottle_transform1.rot - bottle_transform2.rot).lengthsqr4(), 0.1);
// actual projection test
ASSERT_EQ(robot->getDOF(), constraints.getJointValues().size());
std::cout << "\nprior to constraining:\n";
std::vector<double> tmp = constraints.getTaskValues();
for(unsigned int i=0; i<tmp.size(); i++)
{
std::cout << tmp[i] << " ";
}
constraints.constrainCurrentConfiguration();
std::cout << "\npost to constraining:\n";
tmp = constraints.getTaskValues();
for(unsigned int i=0; i<tmp.size(); i++)
{
std::cout << tmp[i] << " ";
}
std::cout << "\n";
EXPECT_TRUE(constraints.areConstraintsFulfilled());
OpenRAVE::RaveDestroy();
}