diff --git a/src/maps/OccupancyGrid.js b/src/navigation/OccupancyGrid.js similarity index 100% rename from src/maps/OccupancyGrid.js rename to src/navigation/OccupancyGrid.js diff --git a/src/maps/OccupancyGridClient.js b/src/navigation/OccupancyGridClient.js similarity index 100% rename from src/maps/OccupancyGridClient.js rename to src/navigation/OccupancyGridClient.js diff --git a/src/navigation/Odometry.js b/src/navigation/Odometry.js new file mode 100644 index 00000000..cf244f5d --- /dev/null +++ b/src/navigation/Odometry.js @@ -0,0 +1,67 @@ +/** + * @author David V. Lu!! - davidvlu@gmail.com + */ + +/** + * An Odometry client + * + * @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 + * * keep (optional) - number of markers to keep around (default: 1) + * * color (optional) - color for line (default: 0xcc00ff) + * * length (optional) - the length of the arrow (default: 1.0) + * * headLength (optional) - the head length of the arrow (default: 0.2) + * * shaftDiameter (optional) - the shaft diameter of the arrow (default: 0.05) + * * headDiameter (optional) - the head diameter of the arrow (default: 0.1) + */ +ROS3D.Odometry = function(options) { + this.options = options || {}; + var ros = options.ros; + var topic = options.topic || '/particlecloud'; + this.tfClient = options.tfClient; + this.color = options.color || 0xcc00ff; + this.length = options.length || 1.0; + this.rootObject = options.rootObject || new THREE.Object3D(); + this.keep = options.keep || 1; + var that = this; + THREE.Object3D.call(this); + + this.sns = []; + + var rosTopic = new ROSLIB.Topic({ + ros : ros, + name : topic, + messageType : 'nav_msgs/Odometry' + }); + + rosTopic.subscribe(function(message) { + if(that.sns.length >= that.keep) { + that.rootObject.remove(that.sns[0]); + that.sns.shift(); + } + + that.options.origin = new THREE.Vector3( message.pose.pose.position.x, message.pose.pose.position.y, + message.pose.pose.position.z); + + var rot = new THREE.Quaternion(message.pose.pose.orientation.x, message.pose.pose.orientation.y, + message.pose.pose.orientation.z, message.pose.pose.orientation.w); + that.options.direction = new THREE.Vector3(1,0,0); + that.options.direction.applyQuaternion(rot); + that.options.material = new THREE.MeshBasicMaterial({color: that.color}); + var arrow = new ROS3D.Arrow(that.options); + + that.sns.push(new ROS3D.SceneNode({ + frameID : message.header.frame_id, + tfClient : that.tfClient, + object : arrow + })); + + that.rootObject.add(that.sns[ that.sns.length - 1]); + }); +}; +ROS3D.Odometry.prototype.__proto__ = THREE.Object3D.prototype; diff --git a/src/navigation/Path.js b/src/navigation/Path.js new file mode 100644 index 00000000..2b4d6f34 --- /dev/null +++ b/src/navigation/Path.js @@ -0,0 +1,61 @@ +/** + * @author David V. Lu!! - davidvlu@gmail.com + */ + +/** + * A Path client that listens to a given topic and displays a line connecting the poses. + * + * @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 + * * color (optional) - color for line (default: 0xcc00ff) + */ +ROS3D.Path = function(options) { + options = options || {}; + var ros = options.ros; + var topic = options.topic || '/path'; + this.tfClient = options.tfClient; + this.color = options.color || 0xcc00ff; + this.rootObject = options.rootObject || new THREE.Object3D(); + var that = this; + THREE.Object3D.call(this); + + this.sn = null; + this.line = null; + + var rosTopic = new ROSLIB.Topic({ + ros : ros, + name : topic, + messageType : 'nav_msgs/Path' + }); + + rosTopic.subscribe(function(message) { + if(that.sn!==null){ + that.rootObject.remove(that.sn); + } + + var lineGeometry = new THREE.Geometry(); + for(var i=0; i message.range_max){ + that.particles.alpha[i] = 0.0; + }else{ + var angle = message.angle_min + i * message.angle_increment; + that.particles.points[i] = new THREE.Vector3( range * Math.cos(angle), range * Math.sin(angle), 0.0 ); + that.particles.alpha[i] = 1.0; + } + that.particles.colors[ i ] = new THREE.Color( that.color ); + } + + finishedUpdate(that.particles, n); + }); +}; +ROS3D.LaserScan.prototype.__proto__ = THREE.Object3D.prototype; diff --git a/src/pointcloud/Particles.js b/src/sensors/Particles.js similarity index 98% rename from src/pointcloud/Particles.js rename to src/sensors/Particles.js index 426005ed..37cd1f03 100644 --- a/src/pointcloud/Particles.js +++ b/src/sensors/Particles.js @@ -19,7 +19,7 @@ ROS3D.Particles = function(options) { this.tfClient = options.tfClient; var texture = options.texture || 'https://upload.wikimedia.org/wikipedia/commons/a/a2/Pixel-white.png'; var size = options.size || 0.05; - this.max_pts = options.max_pts || 100; + this.max_pts = options.max_pts || 10000; this.first_size = null; this.prev_pts = 0; this.rootObject = options.rootObject || new THREE.Object3D(); @@ -85,11 +85,11 @@ ROS3D.Particles = function(options) { this.ps = new THREE.ParticleSystem( this.geom, this.shaderMaterial ); this.sn = null; - + this.points = this.geom.vertices; this.colors = this.attribs.customColor.value; this.alpha = this.attribs.alpha.value; - + }; function setFrame(particles, frame) @@ -116,7 +116,7 @@ function finishedUpdate(particles, n) particles.alpha[i] = 0.0; } particles.prev_pts = n; - + particles.geom.verticesNeedUpdate = true; particles.attribs.customColor.needsUpdate = true; particles.attribs.alpha.needsUpdate = true; diff --git a/src/pointcloud/PointCloud2.js b/src/sensors/PointCloud2.js similarity index 100% rename from src/pointcloud/PointCloud2.js rename to src/sensors/PointCloud2.js