void StompCollisionSpace::setPlanningScene(const arm_navigation_msgs::PlanningScene& planning_scene)
{
ros::WallTime start = ros::WallTime::now();
distance_field_->reset();
std::vector<tf::Vector3> all_points;
//tf::Transform id;
//id.setIdentity();
for (unsigned int i=0; i<planning_scene.collision_objects.size(); ++i)
{
const arm_navigation_msgs::CollisionObject& object = planning_scene.collision_objects[i];
ROS_ASSERT(object.shapes.size() == object.poses.size());
for (unsigned int j=0; j<object.shapes.size(); ++j)
{
shapes::Shape* shape = planning_environment::constructObject(object.shapes[j]);
bodies::Body *body = bodies::createBodyFromShape(shape);
tf::Transform body_pose;
tf::poseMsgToTF(object.poses[j], body_pose);
body->setPose(body_pose);
getVoxelsInBody(*body, all_points);
delete body;
delete shape;
}
}
ROS_DEBUG_STREAM("All points size " << all_points.size());
distance_field_->addPointsToField(all_points);
ros::WallDuration t_diff = ros::WallTime::now() - start;
ROS_DEBUG_STREAM("Took " << t_diff.toSec() << " to set distance field");
}
void StompCollisionSpace::addCollisionObjectToPoints(std::vector<tf::Vector3>& points, const arm_navigation_msgs::CollisionObject& object)
{
ROS_ASSERT(object.shapes.size() == object.poses.size());
for (unsigned int j=0; j<object.shapes.size(); ++j)
{
// TODO everything handled using MESH method, could be much faster for primitive types!!
tf::Transform pose;
tf::poseMsgToTF(object.poses[j], pose);
if (object.shapes[j].type == arm_navigation_msgs::Shape::BOX)
{
double x_extent = object.shapes[j].dimensions[0]/2.0;
double y_extent = object.shapes[j].dimensions[1]/2.0;
double z_extent = object.shapes[j].dimensions[2]/2.0;
for(double x = -x_extent; x <= x_extent+resolution_; x += resolution_)
{
for(double y = -y_extent; y <= y_extent+resolution_; y += resolution_)
{
for(double z = -z_extent; z <= z_extent+resolution_; z += resolution_)
{
tf::Vector3 p(x, y, z);
tf::Vector3 pp = pose*p;
//ROS_INFO("Point: %f, %f, %f", pp.x(), pp.y(), pp.z());
points.push_back(pp);
}
}
}
}
else if (object.shapes[j].type == arm_navigation_msgs::Shape::MESH)
{
shapes::Shape* shape = planning_environment::constructObject(object.shapes[j]);
bodies::Body *body = bodies::createBodyFromShape(shape);
tf::Transform body_pose;
tf::poseMsgToTF(object.poses[j], body_pose);
body->setPose(body_pose);
getVoxelsInBody(*body, points);
delete body;
delete shape;
}
}
/*
for(unsigned int j = 0; j < n; j++) {
if (no.shape[j]->type == shapes::MESH) {
bodies::Body *body = bodies::createBodyFromShape(no.shape[j]);
body->setPose(inv*no.shapePose[j]);
std::vector<tf::Vector3> body_points;
getVoxelsInBody(*body, body_points);
points.insert(points.end(), body_points.begin(), body_points.end());
delete body;
} else {
geometric_shapes_msgs::Shape object;
if(!planning_environment::constructObjectMsg(no.shape[j], object)) {
ROS_WARN("Shap cannot be converted");
continue;
}
geometry_msgs::Pose pose;
tf::poseTFToMsg(no.shapePose[j], pose);
KDL::Rotation rotation = KDL::Rotation::Quaternion(pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w);
KDL::Vector position(pose.position.x, pose.position.y, pose.position.z);
KDL::Frame f(rotation, position);
if (object.type == geometric_shapes_msgs::Shape::CYLINDER)
{
if (object.dimensions.size() != 2) {
ROS_INFO_STREAM("Cylinder must have exactly 2 dimensions, not "
<< object.dimensions.size());
continue;
}
// generate points:
double radius = object.dimensions[0];
//ROS_INFO_STREAM("Divs " << xdiv << " " << ydiv << " " << zdiv);
double xlow = pose.position.x - object.dimensions[0];
double ylow = pose.position.y - object.dimensions[0];
double zlow = pose.position.z - object.dimensions[1]/2.0;
//ROS_INFO("pose " << pose.position.x << " " << pose.position.y);
for(double x = xlow; x <= xlow+object.dimensions[0]*2.0+resolution_; x += resolution_) {
for(double y = ylow; y <= ylow+object.dimensions[0]*2.0+resolution_; y += resolution_) {
for(double z = zlow; z <= zlow+object.dimensions[1]+resolution_; z += resolution_) {
double xdist = fabs(pose.position.x-x);
double ydist = fabs(pose.position.y-y);
//if(pose.position.z-z == 0.0) {
// ROS_INFO_STREAM("X " << x << " Y " << y << " Dists " << xdist << " " << ydist << " Rad " << sqrt(xdist*xdist+ydist*ydist));
// }
if(sqrt(xdist*xdist+ydist*ydist) <= radius) {
KDL::Vector p(pose.position.x-x,pose.position.y-y,pose.position.z-z);
KDL::Vector p2;
p2 = f*p;
points.push_back(inv*tf::Vector3(p2(0),p2(1),p2(2)));
}
}
}
}
} else if (object.type == geometric_shapes_msgs::Shape::BOX) {
if(object.dimensions.size() != 3) {
ROS_INFO_STREAM("Box must have exactly 3 dimensions, not "
<< object.dimensions.size());
continue;
}
double xlow = pose.position.x - object.dimensions[0]/2.0;
double ylow = pose.position.y - object.dimensions[1]/2.0;
double zlow = pose.position.z - object.dimensions[2]/2.0;
for(double x = xlow; x <= xlow+object.dimensions[0]+resolution_; x += resolution_) {
for(double y = ylow; y <= ylow+object.dimensions[1]+resolution_; y += resolution_) {
for(double z = zlow; z <= zlow+object.dimensions[2]+resolution_; z += resolution_) {
KDL::Vector p(pose.position.x-x,pose.position.y-y,pose.position.z-z);
KDL::Vector p2;
p2 = f*p;
points.push_back(inv*tf::Vector3(p2(0),p2(1),p2(2)));
}
}
}
}
}
}
}*/
}
