/// hook that is called when traversing occupied nodes of the updated Octree (does nothing here) void srs_env_model::CCMapPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapParameters & mp) { if( m_bLocked ) return; // Should we publish something? if (! m_publishCollisionMap) return; // Is this point near enough to the robot? if( ! isNearRobot( btVector3( it.getX(), it.getY(), it.getZ() ), it.getSize() ) ) return; Eigen::Vector3f point( it.getX(), it.getY(), it.getZ() ); if( m_bConvertPoint ) { // Transform point from the world to the CMap TF point = m_worldToCMapRot * point + m_worldToCMapTrans; } // Add point to the collision map arm_navigation_msgs::OrientedBoundingBox box; double size = it.getSize(); box.extents.x = box.extents.y = box.extents.z = size; box.axis.x = box.axis.y = 0.0; box.axis.z = 1.0; box.angle = 0.0; box.center.x = point[0]; box.center.y = point[1]; box.center.z = point[2]; m_dataBuffer->boxes.push_back(box); }
void srs_env_model::CMarkerArrayPlugin::handleNode(const srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp) { unsigned idx = it.getDepth(); assert(idx < m_data->markers.size()); geometry_msgs::Point cubeCenter; if( m_bTransform ) { // Transform input point Eigen::Vector3f point( it.getX(), it.getY(), it.getZ() ); point = m_ocToMarkerArrayRot * point + m_ocToMarkerArrayTrans; cubeCenter.x = it.getX(); cubeCenter.y = it.getY(); cubeCenter.z = it.getZ(); }else{ cubeCenter.x = it.getX(); cubeCenter.y = it.getY(); cubeCenter.z = it.getZ(); } m_data->markers[idx].points.push_back(cubeCenter); if (m_bHeightMap){ double h = (1.0 - std::min(std::max((cubeCenter.z-m_minZ)/ (m_maxZ - m_minZ), 0.0), 1.0)) *m_colorFactor; m_data->markers[idx].colors.push_back(heightMapColor(h)); } }
/// hook that is called when traversing occupied nodes of the updated Octree (does nothing here) void srs_env_model::CPointCloudPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapWithParameters & mp) { // std::cerr << "PCP: handle occupied" << std::endl; tPclPoint point; // Set position point.x = it.getX(); point.y = it.getY(); point.z = it.getZ(); // Set color point.r = it->r(); point.g = it->g(); point.b = it->b(); // std::cerr << "Occupied node r " << (int)point.r << ", g " << (int)point.g << ", b " << (int)point.b << std::endl; /* // Set color point.r = 255 - counter % 255; point.g = counter % 255; point.b = 128; */ m_data->push_back( point ); ++counter; }
/** * hook that is called when traversing occupied nodes of the updated Octree (does nothing here) */ void srs_env_model::CCompressedPointCloudPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapWithParameters & mp) { // PERROR("OnHandleOccupied"); ++m_countAll; if( ! m_bCamModelInitialized ) return; if( ! m_bPublishComplete ) { // Test input point tf::Point pos(it.getX(), it.getY(), it.getZ()); tf::Point posRel = m_to_sensor(pos); cv::Point2d uv = m_camera_model.project3dToPixel(cv::Point3d( posRel.x(), posRel.y(), posRel.z())); // ignore point if not in sensor cone if (!inSensorCone(uv)) return; } // Ok, add it... CPointCloudPlugin::handleOccupiedNode( it, mp ); ++m_countVisible; }
/** * hook that is called when traversing occupied nodes of the updated Octree (does nothing here) */ void srs_env_model::COcPatchMaker::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapWithParameters & mp) { // PERROR("OnHandleOccupied"); if( ! m_bCamModelInitialized ) return; // Test input point tf::Point pos(it.getX(), it.getY(), it.getZ()); // if( m_bTransformCamera ) pos = m_to_sensorTf(pos); pos = pos * m_fracMatrix; if( pos.z() < 0 ) return; cv::Point2d uv = m_camera_model.project3dToPixel(cv::Point3d( pos.x(), pos.y(), pos.z())); // ignore point if not in sensor cone if (!inSensorCone(uv)) return; // Ok, add it... // std::cerr << "PCP: handle occupied" << std::endl; tPclPoint point; // Set position point.x = it.getX(); point.y = it.getY(); point.z = it.getZ(); // Set color point.r = it->r(); point.g = it->g(); point.b = it->b(); // std::cerr << "Occupied node r " << (int)point.r << ", g " << (int)point.g << ", b " << (int)point.b << std::endl; m_cloud.push_back( point ); }
/** * hook that is called when traversing occupied nodes of the updated Octree (does nothing here) */ void srs_env_model::CLimitedPointCloudPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapWithParameters & mp) { Eigen::Vector3f p( it.getX(), it.getY(), it.getZ() ); float f( p.dot( m_normal ) + m_d); if( f > max ) max = f; if( f < min) min = f; // Test if point is in front of camera if( (p.dot( m_normal ) + m_d) > 0 ) { // Ok, add it... CPointCloudPlugin::handleOccupiedNode( it, mp ); ++m_countVisible; } else { ++m_countHidden; // PERROR( "Point behind camera... "); } }
void srs_env_model::CCollisionObjectPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp) { // Transform input point Eigen::Vector3f point( it.getX(), it.getY(), it.getZ() ); // Convert point if needed... if( m_bConvert ) point = m_ocToCoRot * point + m_ocToCoTrans; // Add shape arm_navigation_msgs::Shape shape; shape.type = arm_navigation_msgs::Shape::BOX; shape.dimensions.resize(3); shape.dimensions[0] = shape.dimensions[1] = shape.dimensions[2] = it.getSize(); m_data->shapes.push_back(shape); // Add pose geometry_msgs::Pose pose; pose.orientation = tf::createQuaternionMsgFromYaw(0.0); pose.position.x = point.x(); pose.position.y = point.y(); pose.position.z = point.z(); m_data->poses.push_back(pose); }