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))); } } } } } } }*/ }