added low-res pointcloud streaming + fixed param server lookup in poi…

…ntcloud_image.cpp
willowgarage · Jan 24, 2013 · 7a67ea8 · 7a67ea8
1 parent 6b96549
commit 7a67ea8
Show file tree

Hide file tree

Showing 4 changed files with 166 additions and 7 deletions.
diff --git a/pointcloud_image_streamer/src/pointcloud_image.cpp b/pointcloud_image_streamer/src/pointcloud_image.cpp
@@ -66,18 +66,24 @@ class DepthRGBEncoder
     pub_it_(nh_),
     crop_size_(512)
   {
+    // ROS parameters
+    ros::NodeHandle priv_nh_("~");
+
     // read depth map topic from param server
     std::string depthmap_topic;
-    nh_.param<std::string>("depth", depthmap_topic, "/camera/depth/image");
+    priv_nh_.param<std::string>("depth", depthmap_topic, "/camera/depth/image");
 
     // read rgb topic from param server
     std::string rgb_image_topic;
-    nh_.param<std::string>("rgb", rgb_image_topic, "/camera/rgb/image_color");
+    priv_nh_.param<std::string>("rgb", rgb_image_topic, "/camera/rgb/image_color");
 
-    nh_.param<std::string>("frame", base_frame_, "/camera_link");
+    priv_nh_.param<std::string>("frame", base_frame_, "/camera_link");
 
     subscribe(depthmap_topic, rgb_image_topic);
 
+    ROS_INFO_STREAM("Subscribing to "<< rgb_image_topic);
+    ROS_INFO_STREAM("Subscribing to "<< depthmap_topic);
+
     pub_ = pub_it_.advertise("depth_color_combined", 1);
   }
   virtual ~DepthRGBEncoder()
@@ -119,7 +125,7 @@ class DepthRGBEncoder
 
   void depth_with_color_cb(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::ImageConstPtr& color_msg)
   {
-    ROS_INFO("Image depth/color pair received");
+    ROS_DEBUG("Image depth/color pair received");
     process(depth_msg, color_msg, crop_size_);
   }
 

diff --git a/www/demo/demo-low-res.html b/www/demo/demo-low-res.html
@@ -1,7 +1,7 @@
 <!DOCTYPE html>
 <html lang="en">
   <head>
-    <title>Interactive Marker Example: Basic Controls</title>
+    <title>Depth Cloud + Interactive Markers Demo</title>
     <meta name="viewport" content="width=device-width, user-scalable=no, minimum-scale=1.0, maximum-scale=1.0">
     <style>
       body {
@@ -50,7 +50,7 @@
 
     <script src="../depthcloud.js"></script>
 
-    <script src="demo.js"></script>
+    <script src="demo-low-res.js"></script>
 
   </body>
 </html>

diff --git a/www/demo/demo-low-res.js b/www/demo/demo-low-res.js
@@ -0,0 +1,153 @@
+InteractiveMarkerDisplay=new (function(THREE) {
+
+  var camera, cameraControls, scene, renderer;
+
+  var selectableObjs;
+
+  var directionalLight;
+
+  var mouseHandler, highlighter;
+
+  var imClient, imViewer;
+
+  init();
+  animate();
+
+  function init() {
+
+    // setup camera
+    camera = new THREE.PerspectiveCamera(40, window.innerWidth / window.innerHeight, 0.01, 1000);
+    camera.position.x = 3;
+    camera.position.y = 3;
+    camera.position.z = 3;
+
+    // setup scene
+    scene = new THREE.Scene();
+
+    // setup camera mouse control
+    cameraControls = new THREE.RosOrbitControls(scene,camera);
+
+    // add node to host selectable objects
+    selectableObjs = new THREE.Object3D;
+    scene.add(selectableObjs);
+
+    // add lights
+    scene.add(new THREE.AmbientLight(0x555555));
+    directionalLight = new THREE.DirectionalLight(0xffffff);
+    scene.add(directionalLight);
+
+    // add x/y grid
+    var numCells = 50;
+    var gridGeom = new THREE.PlaneGeometry(numCells, numCells, numCells, numCells);
+    var gridMaterial = new THREE.MeshBasicMaterial({
+      color : 0x999999,
+      wireframe : true,
+      wireframeLinewidth : 1,
+      transparent : true
+    });
+    var gridObj = new THREE.Mesh(gridGeom, gridMaterial);
+    scene.add(gridObj);
+
+    // add coordinate frame visualization
+    axes = new THREE.Axes();
+    scene.add(axes);
+
+    renderer = new THREE.WebGLRenderer({
+      antialias : true
+    });
+    renderer.setClearColorHex(0x333333, 1.0);
+    renderer.sortObjects = false;
+    renderer.setSize(window.innerWidth, window.innerHeight);
+    renderer.shadowMapEnabled = false;
+    renderer.autoClear = false;
+
+    var container = document.getElementById("container");
+    container.appendChild(renderer.domElement);
+
+    // propagates mouse events to three.js objects
+    mouseHandler = new ThreeInteraction.MouseHandler(renderer, camera, selectableObjs, cameraControls);
+
+    // highlights the receiver of mouse events
+    highlighter = new ThreeInteraction.Highlighter(mouseHandler);
+
+    // connect to rosbridge
+    var rosUrl = "ws://"+window.location.hostname+':9099';
+    console.log("Connecting to ROS at ", rosUrl );
+    var ros = new ROS(rosUrl);
+
+    var meshBaseUrl = "http://"+window.location.hostname+':8000/resources/';
+    console.log("Getting meshes from ", meshBaseUrl );
+
+    var depthNode = new THREE.Object3D;
+    scene.add(depthNode);
+
+    cloudStream = new DepthCloud.Viewer({
+      url : '../streams/depth_color_combined.webm?width=256&height=256',
+      width : 256,
+      height : 256,
+      sceneNode : depthNode,
+      f : 525.0,
+      shaderUrl: '../shaders/'
+    });
+
+    cloudStream.startStream();
+
+    var tfClient = new TfClient( {
+      ros: ros,
+      fixedFrame: 'base_link',
+      angularThres: 2.0,
+      transThres: 0.01
+    } );
+
+    tfClient.subscribe('head_mount_kinect_rgb_optical_frame',function(transform){
+      //console.log(transform);
+      depthNode.position.x = transform.translation.x;
+      depthNode.position.y = transform.translation.y;
+      depthNode.position.z = transform.translation.z;
+      depthNode.useQuaternion = true;
+      depthNode.quaternion.x = transform.rotation.x;
+      depthNode.quaternion.y = transform.rotation.y;
+      depthNode.quaternion.z = transform.rotation.z;
+      depthNode.quaternion.w = transform.rotation.w;
+
+      axes.position.x = transform.translation.x;
+      axes.position.y = transform.translation.y;
+      axes.position.z = transform.translation.z;
+      axes.useQuaternion = true;
+      axes.quaternion.x = transform.rotation.x;
+      axes.quaternion.y = transform.rotation.y;
+      axes.quaternion.z = transform.rotation.z;
+      axes.quaternion.w = transform.rotation.w;
+    });
+
+    // show interactive markers
+    imClient = new ImProxy.Client(ros,tfClient);
+    imViewer = new ImThree.Viewer(selectableObjs, camera, imClient, meshBaseUrl);
+  }
+
+  this.subscribe = function( topic ) {
+    imClient.subscribe(topic);
+  }
+
+  this.unsubscribe = function( topic ) {
+    imClient.unsubscribe();
+  }
+
+  function animate() {
+
+    cameraControls.update();
+
+    // put light to the top-left of the camera
+    directionalLight.position = camera.localToWorld(new THREE.Vector3(-1, 1, 0));
+    directionalLight.position.normalize();
+
+    renderer.clear(true, true, true);
+    renderer.render(scene, camera);
+
+    highlighter.renderHighlight(renderer, scene, camera);
+
+    requestAnimationFrame(animate);
+  }
+
+})(THREE);
+
diff --git a/www/demo/demo.js b/www/demo/demo.js
@@ -82,7 +82,7 @@ InteractiveMarkerDisplay=new (function(THREE) {
     scene.add(depthNode);
 
     cloudStream = new DepthCloud.Viewer({
-      url : '../streams/webGL_pointcloud_image_encoder/depth_color_combined.webm?',
+      url : '../streams/depth_color_combined.webm?',
       sceneNode : depthNode,
       f : 525.0,
       shaderUrl: '../shaders/'