personalrobots-commit

[Personalrobots-commit] SF.net SVN: personalrobots: [147] pkg/trunk/cloud_viewer

[<jl...@us...>]

Revision: 147
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=147&view=rev
Author:   jleibs
Date:     2008-04-21 14:44:30 -0700 (Mon, 21 Apr 2008)

Log Message:
-----------
New node for viewing 3D point clouds.

Modified Paths:
--------------
    pkg/trunk/cloud_viewer/build.yaml
    pkg/trunk/cloud_viewer/manifest.xml

Added Paths:
-----------
    pkg/trunk/cloud_viewer/src/cloud_node/
    pkg/trunk/cloud_viewer/src/cloud_node/Makefile
    pkg/trunk/cloud_viewer/src/cloud_node/cloud_node.cpp

Modified: pkg/trunk/cloud_viewer/build.yaml
===================================================================
--- pkg/trunk/cloud_viewer/build.yaml	2008-04-21 21:42:07 UTC (rev 146)
+++ pkg/trunk/cloud_viewer/build.yaml	2008-04-21 21:44:30 UTC (rev 147)
@@ -2,3 +2,4 @@
   make:
     - src/libcloud_viewer
     - src/standalone
+    - src/cloud_node

Modified: pkg/trunk/cloud_viewer/manifest.xml
===================================================================
--- pkg/trunk/cloud_viewer/manifest.xml	2008-04-21 21:42:07 UTC (rev 146)
+++ pkg/trunk/cloud_viewer/manifest.xml	2008-04-21 21:44:30 UTC (rev 147)
@@ -5,6 +5,10 @@
 <author>Morgan Quigley mqu...@cs...</author>
 <license>BSD</license>
 <url>http://stair.stanford.edu</url>
+<depend package="roscpp"/>
+<depend package="common_flows"/>
 <depend package="sdl"/>
+<sys_depend lib="GL"/>
+<sys_depend lib="GLU"/>
 </package>
 

Added: pkg/trunk/cloud_viewer/src/cloud_node/Makefile
===================================================================
--- pkg/trunk/cloud_viewer/src/cloud_node/Makefile	                        (rev 0)
+++ pkg/trunk/cloud_viewer/src/cloud_node/Makefile	2008-04-21 21:44:30 UTC (rev 147)
@@ -0,0 +1,4 @@
+SRC = cloud_node.cpp
+OUT = ../../nodes/cloud_node
+PKG = cloud_viewer
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk

Added: pkg/trunk/cloud_viewer/src/cloud_node/cloud_node.cpp
===================================================================
--- pkg/trunk/cloud_viewer/src/cloud_node/cloud_node.cpp	                        (rev 0)
+++ pkg/trunk/cloud_viewer/src/cloud_node/cloud_node.cpp	2008-04-21 21:44:30 UTC (rev 147)
@@ -0,0 +1,174 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "ros/ros_slave.h"
+#include "common_flows/FlowPointCloudFloat32.h"
+#include "common_flows/FlowEmpty.h"
+#include "cloud_viewer/cloud_viewer.h"
+#include <SDL/SDL.h>
+#include "math.h"
+
+class Cloud_Node : public ROS_Slave
+{
+public:
+  FlowPointCloudFloat32 *cloud;
+  FlowEmpty *shutter;
+  CloudViewer *cloud_viewer;
+  
+
+  int count;
+
+  Cloud_Node() : ROS_Slave(), cloud_viewer(NULL)
+  {
+    register_sink(cloud = new FlowPointCloudFloat32("cloud"), ROS_CALLBACK(Cloud_Node, cloud_callback));
+    register_sink(shutter = new FlowEmpty("shutter"), ROS_CALLBACK(Cloud_Node, shutter_callback));
+
+    cloud_viewer =  new CloudViewer;
+
+    count = 0;
+
+    register_with_master();
+  }
+
+  virtual ~Cloud_Node()
+  { 
+    if (cloud_viewer)
+      delete cloud_viewer;
+  }
+
+
+  void refresh() {
+    // SDL (and Xlib) don't handle threads well, so we do both of these
+    check_keyboard();
+    display_image();
+  }
+
+  void check_keyboard() {
+    SDL_Event event;
+    while(SDL_PollEvent(&event))
+      {
+	switch(event.type)
+	  {
+	  case SDL_MOUSEMOTION:
+	    cloud_viewer->mouse_motion(event.motion.x, event.motion.y, event.motion.xrel, event.motion.yrel);
+	    break;
+	  case SDL_MOUSEBUTTONDOWN:
+	  case SDL_MOUSEBUTTONUP:
+	    {
+	      int button = -1;
+	      switch(event.button.button)
+		{
+		case SDL_BUTTON_LEFT: button = 0; break;
+		case SDL_BUTTON_MIDDLE: button = 1; break;
+		case SDL_BUTTON_RIGHT: button = 2; break;
+		}
+	      cloud_viewer->mouse_button(event.button.x, event.button.y,
+					button, (event.type == SDL_MOUSEBUTTONDOWN ? true : false));
+	      break;
+	    }
+	  case SDL_KEYDOWN:
+	    cloud_viewer->keypress(event.key.keysym.sym);
+	    break;
+	  }
+      }
+  }
+
+  void display_image() {
+    cloud->lock_atom();     // I've commandeered the cloud mutex for my own nefarious purposes
+    cloud_viewer->render();
+    SDL_GL_SwapBuffers();
+    cloud->unlock_atom();
+  }
+
+
+  bool sdl_init()
+  {
+    SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE,   24);
+    SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
+    const int w = 640, h = 480;
+    if (SDL_SetVideoMode(w, h, 32, SDL_OPENGL | SDL_HWSURFACE) == 0)  {
+      fprintf(stderr, "setvideomode failed: %s\n", SDL_GetError());
+      return false;
+    }
+
+    cloud_viewer->set_opengl_params(w,h);
+
+    SDL_EnableKeyRepeat(SDL_DEFAULT_REPEAT_DELAY, SDL_DEFAULT_REPEAT_INTERVAL);//People expect Key Repeat
+
+    return true;
+  }
+
+  void cloud_callback()
+  {
+    for (int i = 0; i < cloud->get_x_size(); i++) {
+      cloud_viewer->add_point(cloud->x[i], cloud->y[i], cloud->z[i],
+			      255,255,255);
+    }
+  }
+
+  void shutter_callback()
+  {
+    cloud->lock_atom();
+    cloud_viewer->clear_cloud();
+    cloud->unlock_atom();
+  }
+
+
+
+};
+
+int main(int argc, char **argv)
+{
+  if (SDL_Init(SDL_INIT_VIDEO) < 0)
+  {
+    fprintf(stderr, "SDL initialization error: %s\n", SDL_GetError());
+    exit(1);
+  }
+  atexit(SDL_Quit);
+
+  Cloud_Node cn;
+
+  if (!cn.sdl_init())
+  {
+    fprintf(stderr, "SDL video startup error: %s\n", SDL_GetError());
+    exit(1);
+  }
+
+  while (cn.happy()) {
+    cn.refresh();
+    usleep(1000);
+  }
+  return 0;
+}
+


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

[Personalrobots-commit] SF.net SVN: personalrobots: [171] pkg/trunk/cloud_viewer

[<jl...@us...>]

Revision: 171
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=171&view=rev
Author:   jleibs
Date:     2008-04-23 17:28:29 -0700 (Wed, 23 Apr 2008)

Log Message:
-----------
Adding grid lines and the ability to save point clouds to the cloud viewer.

Modified Paths:
--------------
    pkg/trunk/cloud_viewer/include/cloud_viewer/cloud_viewer.h
    pkg/trunk/cloud_viewer/src/cloud_node/cloud_node.cpp
    pkg/trunk/cloud_viewer/src/libcloud_viewer/cloud_viewer.cpp

Added Paths:
-----------
    pkg/trunk/cloud_viewer/clouds/

Modified: pkg/trunk/cloud_viewer/include/cloud_viewer/cloud_viewer.h
===================================================================
--- pkg/trunk/cloud_viewer/include/cloud_viewer/cloud_viewer.h	2008-04-23 22:25:47 UTC (rev 170)
+++ pkg/trunk/cloud_viewer/include/cloud_viewer/cloud_viewer.h	2008-04-24 00:28:29 UTC (rev 171)
@@ -19,7 +19,8 @@
 	~CloudViewer();
 
 	void clear_cloud();
-	void add_point(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b);
+        void add_point(CloudViewerPoint p);
+        void add_point(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b);
 	void set_opengl_params(unsigned width, unsigned height);
 	void render();
 	void mouse_button(int x, int y, int button, bool is_down);

Modified: pkg/trunk/cloud_viewer/src/cloud_node/cloud_node.cpp
===================================================================
--- pkg/trunk/cloud_viewer/src/cloud_node/cloud_node.cpp	2008-04-23 22:25:47 UTC (rev 170)
+++ pkg/trunk/cloud_viewer/src/cloud_node/cloud_node.cpp	2008-04-24 00:28:29 UTC (rev 171)
@@ -39,16 +39,28 @@
 #include <SDL/SDL.h>
 #include "math.h"
 
+#include <fstream>
+#include <sstream>
+#include <sys/stat.h>
+#include <time.h>
+
+
 class Cloud_Node : public ROS_Slave
 {
 public:
   FlowPointCloudFloat32 *cloud;
   FlowEmpty *shutter;
+
   CloudViewer *cloud_viewer;
+  ROS_Mutex cloud_mutex;
   
+  std::vector<CloudViewerPoint> cloud_buffer;
+  int cloud_cnt;
 
-  int count;
+  char dir_name[256];
 
+  int save;
+
   Cloud_Node() : ROS_Slave(), cloud_viewer(NULL)
   {
     register_sink(cloud = new FlowPointCloudFloat32("cloud"), ROS_CALLBACK(Cloud_Node, cloud_callback));
@@ -56,9 +68,25 @@
 
     cloud_viewer =  new CloudViewer;
 
-    count = 0;
+    register_with_master();
 
-    register_with_master();
+    if (!get_int_param(".save", &save))
+      save = 0;
+
+    if (save) {
+      time_t rawtime;
+      struct tm* timeinfo;
+      time(&rawtime);
+      timeinfo = localtime(&rawtime);
+      
+      sprintf(dir_name, "clouds/%.2d%.2d%.2d_%.2d%.2d%.2d", timeinfo->tm_mon + 1, timeinfo->tm_mday,timeinfo->tm_year - 100,timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec);
+      
+      if (mkdir(dir_name, 0755)) {
+	printf("Failed to make directory: %s", dir_name);
+      }
+    }
+
+    cloud_cnt = 0;
   }
 
   virtual ~Cloud_Node()
@@ -69,10 +97,10 @@
 
 
   void refresh() {
-    // SDL (and Xlib) don't handle threads well, so we do both of these
     check_keyboard();
     display_image();
   }
+    
 
   void check_keyboard() {
     SDL_Event event;
@@ -105,10 +133,10 @@
   }
 
   void display_image() {
-    cloud->lock_atom();     // I've commandeered the cloud mutex for my own nefarious purposes
+    cloud_mutex.lock();
     cloud_viewer->render();
     SDL_GL_SwapBuffers();
-    cloud->unlock_atom();
+    cloud_mutex.unlock();
   }
 
 
@@ -116,7 +144,7 @@
   {
     SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE,   24);
     SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
-    const int w = 640, h = 480;
+    const int w = 1024, h = 768;
     if (SDL_SetVideoMode(w, h, 32, SDL_OPENGL | SDL_HWSURFACE) == 0)  {
       fprintf(stderr, "setvideomode failed: %s\n", SDL_GetError());
       return false;
@@ -132,16 +160,52 @@
   void cloud_callback()
   {
     for (int i = 0; i < cloud->get_x_size(); i++) {
-      cloud_viewer->add_point(cloud->x[i], cloud->y[i], cloud->z[i],
-			      255,255,255);
+      cloud_buffer.push_back(CloudViewerPoint(cloud->x[i], cloud->z[i], -cloud->y[i], 
+					      255,255,255));
     }
   }
 
   void shutter_callback()
   {
+    cloud_mutex.lock();
+
+    cloud_viewer->clear_cloud();
+
     cloud->lock_atom();
-    cloud_viewer->clear_cloud();
+
+    for (int i = 0; i < cloud_buffer.size(); i++) {
+      cloud_viewer->add_point(cloud_buffer[i]);
+    }
+
+    if (save) {
+      std::ostringstream oss;
+      cloud_cnt++;
+      oss << dir_name << "/Cloud" << cloud_cnt << ".cld";    
+      ofstream out(oss.str().c_str());
+      
+      out.setf(ios::fixed, ios::floatfield);
+      out.setf(ios::showpoint);
+      out.precision(4);
+      
+      
+      out << "# Hokuyo Point Cloud" << endl;
+      out << "# First line is number of points, all subsequent lines are points (x y z)" << endl;
+      out << cloud_buffer.size() << endl;
+      
+      for (int i = 0; i < cloud_buffer.size(); i++) {
+	out << cloud_buffer[i].x << " " 
+	    << cloud_buffer[i].y << " " 
+	    << cloud_buffer[i].z << endl;
+      }
+      
+      out.close();
+    }
+
     cloud->unlock_atom();
+
+    cloud_buffer.clear();
+
+    cloud_mutex.unlock();
   }
 
 

Modified: pkg/trunk/cloud_viewer/src/libcloud_viewer/cloud_viewer.cpp
===================================================================
--- pkg/trunk/cloud_viewer/src/libcloud_viewer/cloud_viewer.cpp	2008-04-23 22:25:47 UTC (rev 170)
+++ pkg/trunk/cloud_viewer/src/libcloud_viewer/cloud_viewer.cpp	2008-04-24 00:28:29 UTC (rev 171)
@@ -28,6 +28,58 @@
 	points.push_back(CloudViewerPoint(x,y,z,r,g,b));
 }
 
+void CloudViewer::add_point(CloudViewerPoint p)
+{
+	points.push_back(p);
+}
+
+void drawrectgrid(float x_distance, float y_distance, float z_distance, float center_x, float center_y, float center_z, float x_div, float y_div, float z_div)
+{
+  float i = 0;
+  float j = 0;
+
+  glPushMatrix();
+  glTranslatef(center_x,center_y,center_z);
+
+  glColor3f(0.2, 0.2, 0.4);
+
+  for(i =-x_distance/2; i<=x_distance/2; i+=x_div)
+    {
+      glBegin(GL_LINES);
+      glVertex3f(i, 0.0,  z_distance/2);
+      glVertex3f(i, 0.0, -z_distance/2);
+      glEnd();
+    }
+
+  for(i =-z_distance/2; i<=z_distance/2; i+=z_div)
+    {
+      glBegin(GL_LINES);
+      glVertex3f(x_distance/2, 0.0,  i);
+      glVertex3f(-x_distance/2, 0.0, i);
+      glEnd();
+    }
+
+  glColor3f(0.2, 0.4, 0.2);
+  for(i =-x_distance/2; i<=x_distance/2; i+=x_div)
+    {
+      glBegin(GL_LINES);
+      glVertex3f(i, y_distance/2,  0.0);
+      glVertex3f(i, -y_distance/2, 0.0);
+      glEnd();
+    }
+
+  for(i =-y_distance/2; i<=y_distance/2; i+=y_div)
+    {
+      glBegin(GL_LINES);
+      glVertex3f(x_distance/2, i,  0.0);
+      glVertex3f(-x_distance/2, i, 0.0);
+      glEnd();
+    }
+
+  glPopMatrix();
+}
+
+
 void CloudViewer::render()
 {
 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
@@ -62,14 +114,15 @@
   	glPopMatrix();
 	
     // draw a vector from the origin so we don't get lost
-    glBegin(GL_LINES);
+	glBegin(GL_LINES);
   		glColor3f(1,1,1);
   		glVertex3f(0,0,0);
   		glVertex3f(look_tgt_x, look_tgt_y, look_tgt_z);
   	glEnd();
+
+	drawrectgrid(4, 4, 4, 2, 0, 0, 1, 1, 1);
   }
 
-
 	glBegin(GL_POINTS);
 	for (size_t i = 0; i < points.size(); i++)
 	{


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.