Пример #1
0
visualization_msgs::MarkerArray OccupancyGrid::getVisualization(std::string type)
{
  visualization_msgs::MarkerArray ma;

  if(type.compare("bounds") == 0)
  {
    double dimx, dimy, dimz, originx, originy, originz;
    std::vector<geometry_msgs::Point> pts(10);
    getOrigin(originx, originy, originz);
    getWorldSize(dimx,dimy,dimz);
    pts[0].x = originx;      pts[0].y = originy;      pts[0].z = originz;
    pts[1].x = originx+dimx; pts[1].y = originy;      pts[1].z = originz;
    pts[2].x = originx+dimx; pts[2].y = originy+dimy; pts[2].z = originz;
    pts[3].x = originx;      pts[3].y = originy+dimy; pts[3].z = originz;
    pts[4].x = originx;      pts[4].y = originy;      pts[4].z = originz;
    pts[5].x = originx;      pts[5].y = originy;      pts[5].z = originz+dimz;
    pts[6].x = originx+dimx; pts[6].y = originy;      pts[6].z = originz+dimz;
    pts[7].x = originx+dimx; pts[7].y = originy+dimy; pts[7].z = originz+dimz;
    pts[8].x = originx;      pts[8].y = originy+dimy; pts[8].z = originz+dimz;
    pts[9].x = originx;      pts[9].y = originy;      pts[9].z = originz+dimz;

    ma.markers.resize(1);
    ma.markers[0] = viz::getLineMarker(pts, 0.05, 10, getReferenceFrame(), "collision_space_bounds", 0);
  }
  else if(type.compare("distance_field") == 0)
  {
    visualization_msgs::Marker m;
    // grid_->getIsoSurfaceMarkers(0.01, 0.03, getReferenceFrame(), ros::Time::now(),  Eigen::Affine3d::Identity(), m);
    //grid_->getIsoSurfaceMarkers(0.01, 0.08, getReferenceFrame(), ros::Time::now(), tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,0)), m);
    grid_->getIsoSurfaceMarkers(0.01, 0.02, getReferenceFrame(), ros::Time::now(), m);
    m.color.a +=0.2;
    ma.markers.push_back(m);
  }
  else if(type.compare("occupied_voxels") == 0)
  {
    visualization_msgs::Marker marker;
    std::vector<geometry_msgs::Point> voxels;
    getOccupiedVoxels(voxels);
    marker.header.seq = 0;
    marker.header.stamp = ros::Time::now();
    marker.header.frame_id = getReferenceFrame();
    marker.ns = "occupied_voxels";
    marker.id = 1;
    marker.type = visualization_msgs::Marker::POINTS;
    marker.action = visualization_msgs::Marker::ADD;
    marker.lifetime = ros::Duration(0.0);
    marker.scale.x = grid_->getResolution() / 2.0;
    marker.scale.y = grid_->getResolution() / 2.0;
    marker.scale.z = grid_->getResolution() / 2.0;
    marker.color.r = 0.8;
    marker.color.g = 0.3;
    marker.color.b = 0.5;
    marker.color.a = 1;
    marker.points = voxels;
    ma.markers.push_back(marker);
  }
  else
    ROS_ERROR("No visualization found of type '%s'.", type.c_str());
  return ma;
}
bool
AbsoluteModelTransform::computeWorldToLocalMatrix( osg::Matrix& matrix, osg::NodeVisitor* nv ) const
{
    osg::Matrix inv( osg::Matrix::inverse( _matrix ) );
    if( getReferenceFrame() == osg::Transform::ABSOLUTE_RF )
    {
        osg::Matrix invView;
        if( !nv )
            osg::notify( osg::NOTICE ) << "AbsoluteModelTransform: NULL NodeVisitor; can't get invView." << std::endl;
        else if( nv->getVisitorType() != osg::NodeVisitor::CULL_VISITOR )
            osg::notify( osg::NOTICE ) << "AbsoluteModelTransform: NodeVisitor is not CullVisitor; can't get invView." << std::endl;
        else
        {
            osgUtil::CullVisitor* cv = dynamic_cast< osgUtil::CullVisitor* >( nv );
            osg::Camera* cam = cv->getCurrentCamera();
            cam->computeWorldToLocalMatrix( invView, cv );
        }
        matrix = ( invView * inv );
    }
    else
        // RELATIVE_RF
        matrix.postMult( inv );

    return( true );
}
Пример #3
0
void Cylinder::getRandomSurfacePoint(Vector3& p, Vector3& N) const {
    float h = height();
    float r = radius();

    // Create a random point on a standard cylinder and then rotate to the global frame.

    // Relative areas (factor of 2PI already taken out)
    float capRelArea  = square(r) / 2.0f;
    float sideRelArea = r * h;

    float r1 = uniformRandom(0, capRelArea * 2 + sideRelArea);

    if (r1 < capRelArea * 2) {

        // Select a point uniformly at random on a disk
        // @cite http://mathworld.wolfram.com/DiskPointPicking.html
        float a = uniformRandom(0, (float)twoPi());
        float r2 = sqrt(uniformRandom(0, 1)) * r;
        p.x = cos(a) * r2;
        p.z = sin(a) * r2;

        N.x = 0;
        N.z = 0;
        if (r1 < capRelArea) {
            // Top
            p.y = h / 2.0f;
            N.y = 1;
        } else {
            // Bottom
            p.y = -h / 2.0f;
            N.y = -1;
        }
    } else {
        // Side
        float a = uniformRandom(0, (float)twoPi());
        N.x = cos(a);
        N.y = 0;
        N.z = sin(a);
        p.x = N.x * r;
        p.z = N.y * r;
        p.y = uniformRandom(-h / 2.0f, h / 2.0f);
    }

    // Transform to world space
    CoordinateFrame cframe;
    getReferenceFrame(cframe);
    
    p = cframe.pointToWorldSpace(p);
    N = cframe.normalToWorldSpace(N);
}
Пример #4
0
void osgParticleHPS::ModularEmitter::emit(double dt) 
{
    int n = counter_->numParticlesToCreate(dt);
    for (int i=0; i<n; ++i) {
        Particle *P = getParticleSystem()->createParticle();
        if (P) {
            placer_->place(P);
            shooter_->shoot(P);
            if (getReferenceFrame() == RELATIVE_TO_PARENTS) {
                P->transformPositionVelocity(getLocalToWorldMatrix());
            }
        }
    }
}
Пример #5
0
Vector3 Cylinder::randomInteriorPoint() const {
    float h = height();
    float r = radius();

    // Create a random point in a standard cylinder and then rotate to the global frame.

    // Select a point uniformly at random on a disk
    // @cite http://mathworld.wolfram.com/DiskPointPicking.html
    float a = uniformRandom(0, (float)twoPi());
    float r2 = sqrt(uniformRandom(0, 1)) * r;

    Vector3 p(  cos(a) * r2,
                uniformRandom(-h / 2.0f, h / 2.0f),
                sin(a) * r2);

    // Transform to world space
    CoordinateFrame cframe;
    getReferenceFrame(cframe);
    
    return cframe.pointToWorldSpace(p);
}
bool
AbsoluteModelTransform::computeLocalToWorldMatrix( osg::Matrix& matrix, osg::NodeVisitor* nv ) const
{
    if( getReferenceFrame() == osg::Transform::ABSOLUTE_RF )
    {
        osg::Matrix view;
        if( !nv )
            osg::notify( osg::INFO ) << "AbsoluteModelTransform: NULL NodeVisitor; can't get view." << std::endl;
        else if( nv->getVisitorType() != osg::NodeVisitor::CULL_VISITOR )
            osg::notify( osg::INFO ) << "AbsoluteModelTransform: NodeVisitor is not CullVisitor; can't get view." << std::endl;
        else
        {
            osgUtil::CullVisitor* cv = dynamic_cast< osgUtil::CullVisitor* >( nv );
#ifdef SCENEVIEW_ANAGLYPHIC_STEREO_SUPPORT
            // If OSG_STEREO=ON is in the environment, SceneView hides the view matrix
            // in a stack rather than placing it in a Camera node. Enable this code
            // (using CMake) to use a less-efficient way to compute the view matrix that
            // is compatible with SceneView's usage.
            osg::NodePath np = nv->getNodePath();
            np.pop_back();
            osg::Matrix l2w = osg::computeLocalToWorld( np );
            osg::Matrix invL2w = osg::Matrix::inverse( l2w );
            view = invL2w * *( cv->getModelViewMatrix() );
#else
            // Faster way to determine the view matrix, but not compatible with
            // SceneView anaglyphic stereo.
            osg::Camera* cam = cv->getCurrentCamera();
            cam->computeLocalToWorldMatrix( view, cv );
#endif
        }
        matrix = ( _matrix * view );
    }
    else
        // RELATIVE_RF
        matrix.preMult(_matrix);

    return( true );
}
Пример #7
0
void Capsule::getRandomSurfacePoint(Vector3& p, Vector3& N) const {
    float h = height();
    float r = radius();

    // Create a random point on a standard capsule and then rotate to the global frame.

    // Relative areas
    float capRelArea  = sqrt(r) / 2.0f;
    float sideRelArea = r * h;

    float r1 = uniformRandom(0, capRelArea * 2 + sideRelArea);

    if (r1 < capRelArea * 2) {

        // Select a point uniformly at random on a sphere
        N = Sphere(Vector3::zero(), 1).randomSurfacePoint();
        p = N * r;
        p.y += sign(p.y) * h / 2.0f;
    } else {
        // Side
        float a = uniformRandom(0, (float)twoPi());
        N.x = cos(a);
        N.y = 0;
        N.z = sin(a);
        p.x = N.x * r;
        p.z = N.y * r;
        p.y = uniformRandom(-h / 2.0f, h / 2.0f);
    }

    // Transform to world space
    CoordinateFrame cframe;
    getReferenceFrame(cframe);
    
    p = cframe.pointToWorldSpace(p);
    N = cframe.normalToWorldSpace(N);
}
Пример #8
0
Vector3 Capsule::randomInteriorPoint() const {
    float h = height();
    float r = radius();

    // Create a random point in a standard capsule and then rotate to the global frame.

    Vector3 p;

    float hemiVolume = pi() * (r*r*r) * 4 / 6.0;
    float cylVolume = pi() * square(r) * h;
    
    float r1 = uniformRandom(0, 2.0 * hemiVolume + cylVolume);

    if (r1 < 2.0 * hemiVolume) {

        p = Sphere(Vector3::zero(), r).randomInteriorPoint();

        p.y += sign(p.y) * h / 2.0f;

    } else {

        // Select a point uniformly at random on a disk
        float a = uniformRandom(0, (float)twoPi());
        float r2 = sqrt(uniformRandom(0, 1)) * r;

        p = Vector3(cos(a) * r2,
                    uniformRandom(-h / 2.0f, h / 2.0f),
                    sin(a) * r2);
    }

    // Transform to world space
    CoordinateFrame cframe;
    getReferenceFrame(cframe);
    
    return cframe.pointToWorldSpace(p);
}
Пример #9
0
/**
 * A map detector ID and Q ranges
 * This method looks unnecessary as it could be calculated on the fly but
 * the parallelization means that lazy instantation slows it down due to the
 * necessary CRITICAL sections required to update the cache. The Q range
 * values are required very frequently so the total time is more than
 * offset by this precaching step
 */
DetectorAngularCache initAngularCaches(const MatrixWorkspace *const workspace) {
  const size_t nhist = workspace->getNumberHistograms();
  std::vector<double> thetas(nhist);
  std::vector<double> thetaWidths(nhist);
  std::vector<double> detectorHeights(nhist);

  auto inst = workspace->getInstrument();
  const auto samplePos = inst->getSample()->getPos();
  const V3D upDirVec = inst->getReferenceFrame()->vecPointingUp();

  for (size_t i = 0; i < nhist; ++i) // signed for OpenMP
  {
    IDetector_const_sptr det;
    try {
      det = workspace->getDetector(i);
    } catch (Exception::NotFoundError &) {
      // Catch if no detector. Next line tests whether this happened - test
      // placed
      // outside here because Mac Intel compiler doesn't like 'continue' in a
      // catch
      // in an openmp block.
    }
    // If no detector found, skip onto the next spectrum
    if (!det || det->isMonitor()) {
      thetas[i] = -1.0; // Indicates a detector to skip
      thetaWidths[i] = -1.0;
      continue;
    }
    // We have to convert theta from radians to degrees
    const double theta = workspace->detectorSignedTwoTheta(*det) * rad2deg;
    thetas[i] = theta;
    /**
     * Determine width from shape geometry. A group is assumed to contain
     * detectors with the same shape & r, theta value, i.e. a ring mapped-group
     * The shape is retrieved and rotated to match the rotation of the detector.
     * The angular width is computed using the l2 distance from the sample
     */
    if (auto group = boost::dynamic_pointer_cast<const DetectorGroup>(det)) {
      // assume they all have same shape and same r,theta
      auto dets = group->getDetectors();
      det = dets[0];
    }
    const auto pos = det->getPos() - samplePos;
    double l2(0.0), t(0.0), p(0.0);
    pos.getSpherical(l2, t, p);
    // Get the shape
    auto shape =
        det->shape(); // Defined in its own reference frame with centre at 0,0,0
    BoundingBox bbox = shape->getBoundingBox();
    auto maxPoint(bbox.maxPoint());
    auto minPoint(bbox.minPoint());
    auto span = maxPoint - minPoint;
    detectorHeights[i] = span.scalar_prod(upDirVec);
    thetaWidths[i] = 2.0 * std::fabs(std::atan((detectorHeights[i] / 2) / l2)) *
                     180.0 / M_PI;
  }
  DetectorAngularCache cache;
  cache.thetas = thetas;
  cache.thetaWidths = thetaWidths;
  cache.detectorHeights = detectorHeights;
  return cache;
}