forked from RobotWebTools/roslibjs
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request RobotWebTools#123 from DLu/nav_tools
MORE 3D OBJECTS
- Loading branch information
Showing
11 changed files
with
453 additions
and
4 deletions.
There are no files selected for viewing
File renamed without changes.
File renamed without changes.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; |
Oops, something went wrong.