Skip to content

Commit

Permalink
Merge pull request RobotWebTools#123 from DLu/nav_tools
Browse files Browse the repository at this point in the history
MORE 3D OBJECTS
  • Loading branch information
rctoris committed Jul 27, 2015
2 parents 86c73f3 + e83a762 commit 3b61946
Show file tree
Hide file tree
Showing 11 changed files with 453 additions and 4 deletions.
File renamed without changes.
File renamed without changes.
67 changes: 67 additions & 0 deletions src/navigation/Odometry.js
Original file line number Diff line number Diff line change
@@ -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;
61 changes: 61 additions & 0 deletions src/navigation/Path.js
Original file line number Diff line number Diff line change
@@ -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.poses.length;i++){
var v3 = new THREE.Vector3( message.poses[i].pose.position.x, message.poses[i].pose.position.y,
message.poses[i].pose.position.z);
lineGeometry.vertices.push(v3);
}

lineGeometry.computeLineDistances();
var lineMaterial = new THREE.LineBasicMaterial( { color: that.color } );
var line = new THREE.Line( lineGeometry, lineMaterial );

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

that.rootObject.add(that.sn);
});
};
ROS3D.Path.prototype.__proto__ = THREE.Object3D.prototype;
56 changes: 56 additions & 0 deletions src/navigation/Point.js
Original file line number Diff line number Diff line change
@@ -0,0 +1,56 @@
/**
* @author David V. Lu!! - davidvlu@gmail.com
*/

/**
* A PointStamped 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
* * color (optional) - color for line (default: 0xcc00ff)
* * radius (optional) - radius of the point (default: 0.2)
*/
ROS3D.Point = function(options) {
this.options = options || {};
var ros = options.ros;
var topic = options.topic || '/point';
this.tfClient = options.tfClient;
this.color = options.color || 0xcc00ff;
this.rootObject = options.rootObject || new THREE.Object3D();
this.radius = options.radius || 0.2;
var that = this;
THREE.Object3D.call(this);

this.sn = null;

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

rosTopic.subscribe(function(message) {
if(that.sn!==null){
that.rootObject.remove(that.sn);
}

var sphereGeometry = new THREE.SphereGeometry( that.radius );
var sphereMaterial = new THREE.MeshBasicMaterial( {color: that.color} );
var sphere = new THREE.Mesh(sphereGeometry, sphereMaterial);
sphere.position.set(message.point.x, message.point.y, message.point.z);

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

that.rootObject.add(that.sn);
});
};
ROS3D.Point.prototype.__proto__ = THREE.Object3D.prototype;
64 changes: 64 additions & 0 deletions src/navigation/Polygon.js
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
/**
* @author David V. Lu!! - davidvlu@gmail.com
*/

/**
* A PolygonStamped client that listens to a given topic and displays the polygon
*
* @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.Polygon = 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 : 'geometry_msgs/PolygonStamped'
});

rosTopic.subscribe(function(message) {
if(that.sn!==null){
that.rootObject.remove(that.sn);
}

var lineGeometry = new THREE.Geometry();
var v3;
for(var i=0; i<message.polygon.points.length;i++){
v3 = new THREE.Vector3( message.polygon.points[i].x, message.polygon.points[i].y,
message.polygon.points[i].z);
lineGeometry.vertices.push(v3);
}
v3 = new THREE.Vector3( message.polygon.points[0].x, message.polygon.points[0].y,
message.polygon.points[0].z);
lineGeometry.vertices.push(v3);
lineGeometry.computeLineDistances();
var lineMaterial = new THREE.LineBasicMaterial( { color: that.color } );
var line = new THREE.Line( lineGeometry, lineMaterial );

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

that.rootObject.add(that.sn);
});
};
ROS3D.Polygon.prototype.__proto__ = THREE.Object3D.prototype;
63 changes: 63 additions & 0 deletions src/navigation/Pose.js
Original file line number Diff line number Diff line change
@@ -0,0 +1,63 @@
/**
* @author David V. Lu!! - davidvlu@gmail.com
*/

/**
* A PoseStamped 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
* * 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.Pose = function(options) {
this.options = options || {};
var ros = options.ros;
var topic = options.topic || '/pose';
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;

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

rosTopic.subscribe(function(message) {
if(that.sn!==null){
that.rootObject.remove(that.sn);
}

that.options.origin = new THREE.Vector3( message.pose.position.x, message.pose.position.y,
message.pose.position.z);

var rot = new THREE.Quaternion(message.pose.orientation.x, message.pose.orientation.y,
message.pose.orientation.z, message.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.sn = new ROS3D.SceneNode({
frameID : message.header.frame_id,
tfClient : that.tfClient,
object : arrow
});

that.rootObject.add(that.sn);
});
};
ROS3D.Pose.prototype.__proto__ = THREE.Object3D.prototype;
83 changes: 83 additions & 0 deletions src/navigation/PoseArray.js
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
/**
* @author David V. Lu!! - davidvlu@gmail.com
*/

/**
* A PoseArray 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
* * color (optional) - color for line (default: 0xcc00ff)
* * length (optional) - the length of the arrow (default: 1.0)
*/
ROS3D.PoseArray = 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();
var that = this;
THREE.Object3D.call(this);

this.sn = null;

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

rosTopic.subscribe(function(message) {
if(that.sn!==null){
that.rootObject.remove(that.sn);
}

var group = new THREE.Object3D();
var line;

for(var i=0;i<message.poses.length;i++){
var lineGeometry = new THREE.Geometry();

var v3 = new THREE.Vector3( message.poses[i].position.x, message.poses[i].position.y,
message.poses[i].position.z);
lineGeometry.vertices.push(v3);

var rot = new THREE.Quaternion(message.poses[i].orientation.x, message.poses[i].orientation.y,
message.poses[i].orientation.z, message.poses[i].orientation.w);

var tip = new THREE.Vector3(that.length,0,0);
var side1 = new THREE.Vector3(that.length*0.8, that.length*0.2, 0);
var side2 = new THREE.Vector3(that.length*0.8, -that.length*0.2, 0);
tip.applyQuaternion(rot);
side1.applyQuaternion(rot);
side2.applyQuaternion(rot);

lineGeometry.vertices.push(tip.add(v3));
lineGeometry.vertices.push(side1.add(v3));
lineGeometry.vertices.push(side2.add(v3));
lineGeometry.vertices.push(tip);

lineGeometry.computeLineDistances();
var lineMaterial = new THREE.LineBasicMaterial( { color: that.color } );
line = new THREE.Line( lineGeometry, lineMaterial );

group.add(line);
}

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

that.rootObject.add(that.sn);
});
};
ROS3D.PoseArray.prototype.__proto__ = THREE.Object3D.prototype;
Loading

0 comments on commit 3b61946

Please sign in to comment.