Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Point Clouds Object #120

Merged
merged 15 commits into from
Jul 13, 2015
Merged

Point Clouds Object #120

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
b448614
Single Point Cloud Display
DLu Jun 26, 2015
4ab3a33
Fix indentation
DLu Jun 26, 2015
e36ae82
Dynamic sizing
DLu Jun 26, 2015
d9c9487
Update global particlesystem
DLu Jun 26, 2015
9215c00
Working point clouds
DLu Jun 26, 2015
c12d74d
Documentation and cleanup
DLu Jun 26, 2015
55e95a6
Change name to PointCloud2
DLu Jun 26, 2015
eb35c7b
Style fixes
DLu Jul 10, 2015
338d3da
Ability to decrease the number of points displayed
DLu Jul 10, 2015
8f21581
Minor edits
DLu Jul 10, 2015
f2ed2bd
Scene node for proper frame
DLu Jul 11, 2015
c0d5080
Dynamically set frame of point cloud
DLu Jul 12, 2015
97de3cd
Allow non-bson encoding of point clouds
DLu Jul 12, 2015
16b6dc9
Minor optimization
DLu Jul 12, 2015
3f43348
Grunt fixes
DLu Jul 12, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
202 changes: 196 additions & 6 deletions build/ros3d.js
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1648,9 +1648,12 @@ ROS3D.OccupancyGrid = function(options) {

// create the mesh
THREE.Mesh.call(this, geom, material);
// move the map so the corner is at 0, 0
this.position.x = (width * message.info.resolution) / 2;
this.position.y = (height * message.info.resolution) / 2;
// move the map so the corner is at X, Y and correct orientation (informations from message.info)
this.useQuaternion = true;
this.quaternion = message.info.origin.orientation;
this.position.x = (width * message.info.resolution) / 2 + message.info.origin.position.x;
this.position.y = (height * message.info.resolution) / 2 + message.info.origin.position.y;
this.position.z = message.info.origin.position.z;
this.scale.x = message.info.resolution;
this.scale.y = message.info.resolution;
};
Expand Down Expand Up @@ -1710,8 +1713,7 @@ ROS3D.OccupancyGridClient = function(options) {
that.currentGrid = new ROS3D.SceneNode({
frameID : message.header.frame_id,
tfClient : that.tfClient,
object : newGrid,
pose : message.info.origin
object : newGrid
});
} else {
that.currentGrid = newGrid;
Expand Down Expand Up @@ -2725,6 +2727,181 @@ ROS3D.TriangleList.prototype.setColor = function(hex) {
this.mesh.material.color.setHex(hex);
};

/**
* @author David V. Lu!! - davidvlu@gmail.com
*/

function read_point(msg, index, buffer){
var pt = [];
var base = msg.point_step * index;
var n = 4;
var ar = new Uint8Array(n);
for(var fi=0; fi<msg.fields.length; fi++){
var si = base + msg.fields[fi].offset;
for(var i=0; i<n; i++){
ar[i] = buffer[si + i];
}

var dv = new DataView(ar.buffer);

if( msg.fields[fi].name === 'rgb' ){
pt[ 'rgb' ] =dv.getInt32(0, 1);
}else{
pt[ msg.fields[fi].name ] = dv.getFloat32(0, 1);
}
}
return pt;
}

var BASE64 = 'ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/=';
function decode64(x) {
var a = [], z = 0, bits = 0;

for (var i = 0, len = x.length; i < len; i++) {
z += BASE64.indexOf( x[i] );
bits += 6;
if(bits>=8){
bits -= 8;
a.push(z >> bits);
z = z & (Math.pow(2, bits)-1);
}
z = z << 6;
}
return a;
}

/**
* A PointCloud2 client that listens to a given topic and displays the points.
*
* @constructor
* @param options - object with following keys:
*
* * ros - the ROSLIB.Ros connection handle
* * topic - the marker topic to listen to
* * tfClient - the TF client handle to use
* * rootObject (optional) - the root object to add this marker to
* * size (optional) - size to draw each point (default 0.05)
* * max_pts (optional) - number of points to draw (default 100)
*/
ROS3D.PointCloud2 = function(options) {
options = options || {};
var ros = options.ros;
var topic = options.topic || '/points';
this.tfClient = options.tfClient;
var size = options.size || 0.05;
var max_pts = options.max_pts || 100;
this.prev_pts = 0;
this.rootObject = options.rootObject || new THREE.Object3D();
var that = this;
THREE.Object3D.call(this);

this.vertex_shader = [
'attribute vec3 customColor;',
'attribute float alpha;',
'varying vec3 vColor;',
'varying float falpha;',
'void main() ',
'{',
' vColor = customColor; // set color associated to vertex; use later in fragment shader',
' vec4 mvPosition = modelViewMatrix * vec4( position, 1.0 );',
' falpha = alpha; ',
'',
' // option (1): draw particles at constant size on screen',
' // gl_PointSize = size;',
' // option (2): scale particles as objects in 3D space',
' gl_PointSize = ', size, '* ( 300.0 / length( mvPosition.xyz ) );',
' gl_Position = projectionMatrix * mvPosition;',
'}'
].join('\n');

this.fragment_shader = [
'uniform sampler2D texture;',
'varying vec3 vColor; // colors associated to vertices; assigned by vertex shader',
'varying float falpha;',
'void main() ',
'{',
' // calculates a color for the particle',
' gl_FragColor = vec4( vColor, falpha );',
' // sets particle texture to desired color',
' gl_FragColor = gl_FragColor * texture2D( texture, gl_PointCoord );',
'}'
].join('\n');

this.geom = new THREE.Geometry();
for(var i=0;i<max_pts;i++){
this.geom.vertices.push(new THREE.Vector3( ));
}

var customUniforms =
{
texture: { type: 't', value: THREE.ImageUtils.loadTexture( 'pixel.png' ) },
};

this.attribs =
{
customColor: { type: 'c', value: [] },
alpha: { type: 'f', value: [] }
};

this.shaderMaterial = new THREE.ShaderMaterial(
{
uniforms: customUniforms,
attributes: this.attribs,
vertexShader: this.vertex_shader,
fragmentShader: this.fragment_shader,
transparent: true, alphaTest: 0.5
});

this.ps = new THREE.ParticleSystem( this.geom, this.shaderMaterial );
this.sn = null;

var rosTopic = new ROSLIB.Topic({
ros : ros,
name : topic,
messageType : 'sensor_msgs/PointCloud2'
});

rosTopic.subscribe(function(message) {
rosTopic.unsubscribe();

if(that.sn===null){
that.sn = new ROS3D.SceneNode({
frameID : message.header.frame_id,
tfClient : that.tfClient,
object : that.ps
});

that.rootObject.add(that.sn);
}

var n = message.height*message.width;

var buffer;
if(message.data.buffer){
buffer = message.data.buffer;
}else{
buffer = decode64(message.data);
}
for(var i=0;i<n;i++){
var pt = read_point(message, i, buffer);
that.geom.vertices[i] = new THREE.Vector3( pt['x'], pt['y'], pt['z'] );
that.attribs.customColor.value[ i ] = new THREE.Color( pt['rgb'] );
that.attribs.alpha.value[i] = 1.0;
}
for(i=n; i<that.prev_pts; i++){
that.attribs.alpha.value[i] = 0.0;
}
that.prev_pts = n;

that.geom.verticesNeedUpdate = true;
that.attribs.customColor.needsUpdate = true;
that.attribs.alpha.needsUpdate = true;
});


};
ROS3D.PointCloud2.prototype.__proto__ = THREE.Object3D.prototype;

/**
* @author Jihoon Lee - jihoonlee.in@gmail.com
* @author Russell Toris - rctoris@wpi.edu
Expand Down Expand Up @@ -2998,6 +3175,7 @@ ROS3D.Viewer = function(options) {
y : 3,
z : 3
};
var cameraZoomSpeed = options.cameraZoomSpeed || 0.5;

// create the canvas to render to
this.renderer = new THREE.WebGLRenderer({
Expand All @@ -3022,7 +3200,7 @@ ROS3D.Viewer = function(options) {
scene : this.scene,
camera : this.camera
});
this.cameraControls.userZoomSpeed = 0.5;
this.cameraControls.userZoomSpeed = cameraZoomSpeed;

// lights
this.scene.add(new THREE.AmbientLight(0x555555));
Expand Down Expand Up @@ -3087,6 +3265,18 @@ ROS3D.Viewer.prototype.addObject = function(object, selectable) {
}
};

/**
* Resize 3D viewer
*
* @param width - new width value
* @param height - new height value
*/
ROS3D.Viewer.prototype.resize = function(width, height) {
this.camera.aspect = width / height;
this.camera.updateProjectionMatrix();
this.renderer.setSize(width, height);
};

/**
* @author David Gossow - dgossow@willowgarage.com
*/
Expand Down
Loading