Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [171] pkg/trunk/cloud_viewer
|
From: <jl...@us...> - 2008-04-24 00:28:22
|
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.
|
