Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [495] pkg/trunk
|
From: <jl...@us...> - 2008-05-22 00:24:45
|
Revision: 495
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=495&view=rev
Author: jleibs
Date: 2008-05-21 17:24:52 -0700 (Wed, 21 May 2008)
Log Message:
-----------
Assembly and viewing of 3d point clouds with intensity data.
Modified Paths:
--------------
pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc
pkg/trunk/drivers/laser/urg_driver/urg_laser.cc
pkg/trunk/drivers/laser/urg_driver/urg_laser.h
pkg/trunk/drivers/motor/etherdrive/src/etherdrive_node/etherdrive_node.cpp
pkg/trunk/drivers/robot/pr2/tilting_laser/launch_tl
pkg/trunk/drivers/robot/pr2/tilting_laser/manifest.xml
pkg/trunk/drivers/robot/pr2/tilting_laser/params.xml
pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp
pkg/trunk/unported/vision/cloud_viewer/src/Makefile
pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/Makefile
pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/cloud_node.cpp
Added Paths:
-----------
pkg/trunk/unported/vision/cloud_viewer/bin/
Modified: pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc
===================================================================
--- pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc 2008-05-22 00:24:52 UTC (rev 495)
@@ -110,27 +110,27 @@
MsgLaserScan scan_msg;
double min_ang;
double max_ang;
+
+ int cluster;
+ int skip;
+
string port;
HokuyoNode() : ros::node("urglaser"), count(0), next_time(0)
{
advertise<MsgLaserScan>("scan");
- // TODO: add min/max angle support
- /*
- if (!get_double_param(".min_ang", &min_ang))
- min_ang = -90;
- printf("Setting min_ang to: %g\n",min_ang);
+ param("min_ang", min_ang, -90.0);
+ min_ang *= M_PI/180;
- if (!get_double_param(".max_ang", &max_ang))
- max_ang = 90;
- printf("Setting max_ang to: %g\n",max_ang);
- */
+ param("max_ang", max_ang, -90.0);
+ max_ang *= M_PI/180;
- if (!has_param("port") || !get_param("port", port))
- port = "/dev/ttyACM0";
- printf("Setting port to: %s\n",port.c_str());
+ param("cluster", cluster, 1);
+ param("skip", skip, 1);
+ param("port", port, string("/dev/ttyACM0"));
+
scan = new urg_laser_scan_t;
assert(scan);
running = false;
@@ -157,9 +157,8 @@
urg.urg_cmd("BM");
+ int status = urg.request_scans(true, min_ang, max_ang, cluster, skip);
- int status = urg.request_scans(true, -M_PI/2.0, M_PI/2.0);
-
if (status != 0) {
printf("Failed to request scans %d.\n", status);
return -1;
@@ -180,8 +179,8 @@
int publish_scan()
{
- // TODO: add support for pushing readings out
+
// int status = urg.poll_scan(scan, -M_PI/2.0, M_PI/2.0);
int status = urg.service_scan(scan);
if(status != 0)
@@ -198,23 +197,16 @@
next_time += 1000000000;
}
- // printf("%d readings\n", readings->num_readings);
-
scan_msg.angle_min = scan->config.min_angle;
scan_msg.angle_max = scan->config.max_angle;
scan_msg.angle_increment = scan->config.resolution;
-
- // printf("%g %g %g\n", scan.angle_min * 180.0/M_PI, scan.angle_max*180.0/M_PI, scan.angle_increment*180.0/M_PI);
-
scan_msg.range_max = cfg.max_range;
scan_msg.set_ranges_size(scan->num_readings);
scan_msg.set_intensities_size(scan->num_readings);
for(int i = 0; i < scan->num_readings; ++i)
{
- // scan.ranges[i] = readings->ranges[i] < 20 ? (scan.range_max) : (readings->ranges[i] / 1000.0);
scan_msg.ranges[i] = scan->ranges[i];
- // TODO: add intensity support
scan_msg.intensities[i] = scan->intensities[i];
}
publish("scan", scan_msg);
Modified: pkg/trunk/drivers/laser/urg_driver/urg_laser.cc
===================================================================
--- pkg/trunk/drivers/laser/urg_driver/urg_laser.cc 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/laser/urg_driver/urg_laser.cc 2008-05-22 00:24:52 UTC (rev 495)
@@ -151,6 +151,8 @@
///////////////////////////////////////////////////////////////////////////////
int urg_laser::urg_cmd(const char* cmd, int timeout)
{
+ printf("Sending CMD: %s\n",cmd);
+
char buf[100];
if (urg_write(cmd) < 0)
@@ -615,8 +617,6 @@
sprintf(cmdbuf,"M%c%.4d%.4d%.2d%.1d%.2d", intensity_char, min_i, max_i, cluster, skip, count);
- printf("Sending command: %s\n", cmdbuf);
-
status = urg_cmd(cmdbuf, timeout);
return status;
Modified: pkg/trunk/drivers/laser/urg_driver/urg_laser.h
===================================================================
--- pkg/trunk/drivers/laser/urg_driver/urg_laser.h 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/laser/urg_driver/urg_laser.h 2008-05-22 00:24:52 UTC (rev 495)
@@ -60,6 +60,8 @@
float intensities[MAX_READINGS];
//! Number of readings
int num_readings;
+ //! Self reported time
+ int time_stamp;
//! Configuration of scan
urg_laser_config_t config;
} urg_laser_scan_t;
Modified: pkg/trunk/drivers/motor/etherdrive/src/etherdrive_node/etherdrive_node.cpp
===================================================================
--- pkg/trunk/drivers/motor/etherdrive/src/etherdrive_node/etherdrive_node.cpp 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/motor/etherdrive/src/etherdrive_node/etherdrive_node.cpp 2008-05-22 00:24:52 UTC (rev 495)
@@ -69,9 +69,10 @@
for (int i = 0;i < 6;i++) {
ostringstream oss;
- oss << ".pulse_per_rad" << i;
+ oss << "pulse_per_rad" << i;
if (!get_param(oss.str().c_str(), pulse_per_rad[i]))
pulse_per_rad[i] = 1591.54943;
+ printf("Using %g pulse_per_rad for motor %d\n", pulse_per_rad[i], i);
}
ed = new EtherDrive();
@@ -115,6 +116,8 @@
return false;
}
+ // printf("%d %d %d\n",ed->get_enc(0), ed->get_cur(0), ed->get_pwm(0));
+
for (int i = 0; i < 6; i++) {
mot[i].val = val[i] / pulse_per_rad[i];
mot[i].rel = false;
@@ -135,11 +138,14 @@
ros::init(argc, argv);
EtherDrive_Node a;
while (a.ok()) {
+
+ usleep(1000);
if (!a.do_tick())
{
a.log(ros::ERROR,"Etherdrive tick failed.");
break;
}
}
+ ros::fini();
return 0;
}
Modified: pkg/trunk/drivers/robot/pr2/tilting_laser/launch_tl
===================================================================
--- pkg/trunk/drivers/robot/pr2/tilting_laser/launch_tl 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/robot/pr2/tilting_laser/launch_tl 2008-05-22 00:24:52 UTC (rev 495)
@@ -1,6 +1,10 @@
#!/bin/bash
`rospack find xmlparam`/xmlparam `rospack find tilting_laser`/params.xml
-xterm -e `rospack find hokuyourg_player`/hokuyourg_player&
-xterm -e `rospack find etherdrive`/bin/etherdrive&
+export ROS_NAMESPACE=HK
+xterm -e `rospack find hokuyourg_player`/hokuyourg_player scan:=/scan&
+export ROS_NAMESPACE=ED
+xterm -e `rospack find etherdrive`/bin/etherdrive mot0:=/mot0 mot0_cmd:=/mot0_cmd&
+export ROS_NAMESPACE=CV
+xterm -e `rospack find cloud_viewer`/bin/cloud_node shutter:=/shutter cloud:=/cloud&
export ROS_NAMESPACE=TL
-xterm -e `rospack find tilting_laser`/bin/tilting_laser mot:=/mot2 mot_cmd:=/mot2_cmd scan:=/scan&
+xterm -e `rospack find tilting_laser`/bin/tilting_laser mot:=/mot0 mot_cmd:=/mot0_cmd scan:=/scan cloud:=/cloud shutter:=/shutter&
Modified: pkg/trunk/drivers/robot/pr2/tilting_laser/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/tilting_laser/manifest.xml 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/robot/pr2/tilting_laser/manifest.xml 2008-05-22 00:24:52 UTC (rev 495)
@@ -13,6 +13,8 @@
<depend package="std_msgs"/>
<depend package="unstable_msgs"/>
<depend package="hokuyourg_player"/>
+<depend package="etherdrive"/>
+<depend package="cloud_viewer"/>
<depend package="libTF"/>
</package>
Modified: pkg/trunk/drivers/robot/pr2/tilting_laser/params.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/tilting_laser/params.xml 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/robot/pr2/tilting_laser/params.xml 2008-05-22 00:24:52 UTC (rev 495)
@@ -1,5 +1,10 @@
<params>
- <double name="TL/period">5.0</double>
- <double name="TL/min_ang">-1.3</double>
+ <double name="HK/min_ang">-70</double>
+ <double name="HK/max_ang">70</double>
+ <int name="HK/cluster">1</int>
+ <int name="HK/skip">0</int>
+ <double name="ED/pulse_per_rad0">9549.29659</double>
+ <double name="TL/period">10.0</double>
+ <double name="TL/min_ang">-1</double>
<double name="TL/max_ang">0.6</double>
</params>
Modified: pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp 2008-05-22 00:24:52 UTC (rev 495)
@@ -61,7 +61,7 @@
double min_ang;
double max_ang;
- TransformReference TR;
+ libTF::TransformReference TR;
ros::time::clock clock;
@@ -100,10 +100,12 @@
void scans_callback() {
encoder.lock();
- cloud.set_x_size(scans.get_ranges_size());
- cloud.set_y_size(scans.get_ranges_size());
- cloud.set_z_size(scans.get_ranges_size());
+ cloud.set_pts_size(scans.get_ranges_size());
+ cloud.set_chan_size(1);
+ cloud.chan[0].name = "intensities";
+ cloud.chan[0].set_vals_size(scans.get_ranges_size());
+
/*
for (int i = 0; i < scans.get_ranges_size(); i++) {
cloud.x[i] = cos(encoder.val)*cos(scans.angle_min + i*scans.angle_increment) * scans.ranges[i];
@@ -130,17 +132,20 @@
NEWMAT::Matrix rot_points = TR.getMatrix(1,3,clock.ulltime()) * points;
for (uint32_t i = 0; i < scans.get_ranges_size(); i++) {
- cloud.x[i] = rot_points(1,i+1);
- cloud.y[i] = rot_points(2,i+1);
- cloud.z[i] = rot_points(3,i+1);
+ cloud.pts[i].x = rot_points(1,i+1);
+ cloud.pts[i].y = rot_points(2,i+1);
+ cloud.pts[i].z = rot_points(3,i+1);
+ cloud.chan[0].vals[i] = scans.intensities[i];
}
encoder.unlock();
+
publish("cloud", cloud);
}
void encoder_callback() {
unsigned long long time = clock.ulltime();
+
TR.setWithEulers(3, 2,
.02, 0, .02,
0.0, 0.0, 0.0,
@@ -151,7 +156,6 @@
time);
motor_control(); // Control on encoder reads sounds reasonable
- printf("I got some encoder values: %g!\n", encoder.val);
}
void motor_control() {
@@ -169,7 +173,7 @@
next_shutter += 0.5;
publish("shutter", shutter);
}
- publish("mot2_cmd",cmd);
+ publish("mot_cmd",cmd);
}
};
@@ -181,6 +185,7 @@
while (tl.ok()) {
usleep(10000);
}
+ ros::fini();
return 0;
}
Modified: pkg/trunk/unported/vision/cloud_viewer/src/Makefile
===================================================================
--- pkg/trunk/unported/vision/cloud_viewer/src/Makefile 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/unported/vision/cloud_viewer/src/Makefile 2008-05-22 00:24:52 UTC (rev 495)
@@ -1,3 +1,3 @@
-SUBDIRS = libcloud_viewer standalone
+SUBDIRS = libcloud_viewer standalone cloud_node
include $(shell rospack find mk)/recurse_subdirs.mk
Modified: pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/Makefile
===================================================================
--- pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/Makefile 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/Makefile 2008-05-22 00:24:52 UTC (rev 495)
@@ -1,4 +1,4 @@
SRC = cloud_node.cpp
-OUT = ../../nodes/cloud_node
+OUT = ../../bin/cloud_node
PKG = cloud_viewer
-include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
+include $(shell rospack find mk)/node.mk
Modified: pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/cloud_node.cpp
===================================================================
--- pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/cloud_node.cpp 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/cloud_node.cpp 2008-05-22 00:24:52 UTC (rev 495)
@@ -32,143 +32,322 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#include "ros/ros_slave.h"
-#include "common_flows/FlowPointCloudFloat32.h"
-#include "common_flows/FlowEmpty.h"
+#include "ros/node.h"
+#include "std_msgs/MsgPointCloudFloat32.h"
+#include "std_msgs/MsgEmpty.h"
#include "cloud_viewer/cloud_viewer.h"
#include <SDL/SDL.h>
#include "math.h"
+#include "sdlgl/sdlgl.h"
+#include "rosthread/mutex.h"
+#include <fstream>
+#include <sstream>
+#include <sys/stat.h>
-class Cloud_Node : public ROS_Slave
+#include <vector>
+
+struct cloud_point {
+public:
+ float x;
+ float y;
+ float z;
+ float i;
+
+ cloud_point(float _x, float _y, float _z, float _i) : x(_x), y(_y), z(_z), i(_i) {}
+};
+
+class Cloud_Node : public ros::node, public ros::SDLGL
{
public:
- FlowPointCloudFloat32 *cloud;
- FlowEmpty *shutter;
- CloudViewer *cloud_viewer;
+ MsgPointCloudFloat32 cloud;
+ MsgEmpty shutter;
+ CloudViewer cloud_viewer;
+
+ int level;
+ int spread;
+
+ vector<cloud_point> buf[2];
+ int buf_read_ind;
+ int buf_use_ind;
+
+ char dir_name[256];
+ int cloud_cnt;
+
+ bool made_dir;
+ ros::thread::mutex cloud_mutex;
- int count;
-
- Cloud_Node() : ROS_Slave(), cloud_viewer(NULL)
+ Cloud_Node() : ros::node("cloud_viewer"), level(20), spread(400), buf_read_ind(0), buf_use_ind(1), cloud_cnt(0), made_dir(false)
{
- register_sink(cloud = new FlowPointCloudFloat32("cloud"), ROS_CALLBACK(Cloud_Node, cloud_callback));
- register_sink(shutter = new FlowEmpty("shutter"), ROS_CALLBACK(Cloud_Node, shutter_callback));
+ subscribe("cloud", cloud, &Cloud_Node::cloud_callback);
+ subscribe("shutter", shutter, &Cloud_Node::shutter_callback);
- cloud_viewer = new CloudViewer;
+ if (!init_gui(1024, 768))
+ self_destruct();
- count = 0;
+ cloud_viewer.set_opengl_params(1024,768);
- register_with_master();
+ 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);
+
}
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();
+ virtual void render()
+ {
+ cloud_mutex.lock(); // I've commandeered the cloud mutex for my own nefarious purposes
+ cloud_viewer.render();
+ cloud_mutex.unlock();
+ SDL_GL_SwapBuffers();
}
- 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;
- }
- }
+ virtual void mouse_motion(int x, int y, int dx, int dy, int buttons)
+ {
+ cloud_mutex.lock();
+ cloud_viewer.mouse_motion(x, y, dx, dy);
+ cloud_mutex.unlock();
+ request_render();
}
- 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();
+ virtual void mouse_button(int x, int y, int button, bool is_down)
+ {
+ switch(button) {
+ case SDL_BUTTON_LEFT: button = 0; break;
+ case SDL_BUTTON_MIDDLE: button = 1; break;
+ case SDL_BUTTON_RIGHT: button = 2; break;
+ }
+ cloud_mutex.lock();
+ cloud_viewer.mouse_button(x, y, button, is_down);
+ cloud_mutex.unlock();
+ request_render();
}
+ virtual void keypress(char c, uint16_t u, SDLMod mod)
+ {
- 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;
+ if (c == 91 || c == 93)
+ {
+ if (c == 91 && mod == 0)
+ level -= 5;
+ else if (c == 93 && mod == 0)
+ level += 5;
+ else if (c == 91 && mod == 1)
+ spread -= 5;
+ else if (c == 93 && mod == 1)
+ spread += 5;
+
+ printf("Level: %d, Spread: %d\n",level,spread);
+ refill_cloud();
}
- cloud_viewer->set_opengl_params(w,h);
+ if (c == SDLK_RETURN) {
+ save();
+ }
- SDL_EnableKeyRepeat(SDL_DEFAULT_REPEAT_DELAY, SDL_DEFAULT_REPEAT_INTERVAL);//People expect Key Repeat
-
- return true;
+ cloud_mutex.lock();
+ cloud_viewer.keypress(c);
+ cloud_mutex.unlock();
+ request_render();
}
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);
+ int intensity_ind = -1;
+
+ for (int i = 0; i < cloud.get_chan_size(); i++)
+ if (cloud.chan[i].name == string("intensities"))
+ intensity_ind = i;
+
+ double intensity;
+
+ for (int i = 0; i < cloud.get_pts_size(); i++)
+ {
+ intensity = 4000;
+ if (intensity_ind != -1)
+ intensity = cloud.chan[intensity_ind].vals[i];
+
+ buf[buf_read_ind].push_back(cloud_point(cloud.pts[i].x, cloud.pts[i].y, cloud.pts[i].z, intensity));
+
}
+
+ /*
+ int intensity;
+
+ for (int i = 0; i < cloud.get_pts_size(); i++)
+ {
+ if (intensity_ind < 0)
+ {
+ intensity = 0;
+ } else {
+ intensity = level + cloud.chan[intensity_ind].vals[i] / spread;
+ if (intensity > 255)
+ intensity = 255;
+ else if (intensity < 0)
+ intensity = 0;
+ }
+
+ cloud_viewer.add_point(-cloud.pts[i].y, -cloud.pts[i].z, cloud.pts[i].x,
+ intensity, intensity, intensity);
+ }
+ */
+
}
void shutter_callback()
{
- cloud->lock_atom();
- cloud_viewer->clear_cloud();
- cloud->unlock_atom();
+
+ buf_use_ind = buf_read_ind;
+ buf_read_ind = !buf_read_ind;
+
+ buf[buf_read_ind].clear();
+
+ refill_cloud();
}
+ void refill_cloud()
+ {
+ cloud_mutex.lock();
+ cloud_viewer.clear_cloud();
+
+ double val = 0.0;
+ double valsq = 0.0;
+ int cnt = 0;
+ for (int i = 0; i < buf[buf_use_ind].size(); i++) {
+ val += buf[buf_use_ind][i].i;
+ valsq += pow(buf[buf_use_ind][i].i, 2.0);
+ cnt++;
+ }
+ double mean = val / cnt;
+ double std = sqrt( (valsq - cnt*pow(mean, 2.0))/(cnt - 1));
+
+ for (int i = 0; i < buf[buf_use_ind].size(); i++) {
+ float intensity = level + spread * (buf[buf_use_ind][i].i - mean) / (2*std);
+
+ if (intensity > 255)
+ intensity = 255;
+ else if (intensity < 0)
+ intensity = 0;
+
+ cloud_viewer.add_point(-buf[buf_use_ind][i].y, -buf[buf_use_ind][i].z, buf[buf_use_ind][i].x,
+ intensity, intensity, intensity);
+ }
+
+ cloud_mutex.unlock();
+
+ request_render();
+ }
+
+ void save() {
+
+ printf("Trying to save..\n");
+
+ if (!made_dir) {
+ if (mkdir(dir_name, 0755)) {
+ printf("Failed to make directory: %s\n", dir_name);
+ return;
+ } else {
+ made_dir = true;
+ }
+ }
+
+ std::ostringstream oss;
+ oss << dir_name << "/Cloud" << cloud_cnt++ << ".wrl";
+ ofstream out(oss.str().c_str());
+
+ out.setf(ios::fixed, ios::floatfield);
+ out.setf(ios::showpoint);
+ out.precision(4);
+
+ double val = 0.0;
+ double valsq = 0.0;
+ int cnt = 0;
+
+ for (int i = 0; i < buf[buf_use_ind].size(); i++) {
+ val += buf[buf_use_ind][i].i;
+ valsq += pow(buf[buf_use_ind][i].i, 2.0);
+ cnt++;
+ }
+ double mean = val / cnt;
+ double std = sqrt( (valsq - cnt*pow(mean, 2.0))/(cnt - 1));
+
+ out << "#VRML V2.0 utf8" << endl
+ << "DEF InitView Viewpoint" << endl
+ << "{" << endl
+ << "position -2 0 0" << endl
+ << "orientation 0 1 0 -1.57079633" << endl
+ << "" << endl
+ << "}DEF World Transform " << endl
+ << "{" << endl
+ << "translation 0 0 0" << endl
+ << "children [" << endl
+ << "DEF Scene Transform" << endl
+ << "{" << endl
+ << "translation 0 0 0" << endl
+ << "children [" << endl
+ << "Shape" << endl
+ << "{" << endl
+ << "geometry PointSet" << endl
+ << "{" << endl
+ << "coord Coordinate" << endl
+ << "{" << endl
+ << "point[" << endl;
+
+ for (int i = 0; i < buf[buf_use_ind].size(); i++) {
+ out << buf[buf_use_ind][i].x << " " << buf[buf_use_ind][i].z << " " << -buf[buf_use_ind][i].y << ", " << endl;
+ }
+
+ out << "]"
+ << "}" << endl
+ << "color Color" << endl
+ << "{" << endl
+ << "color[" << endl;
+
+ for (int i = 0; i < buf[buf_use_ind].size(); i++) {
+ float intensity = (level + spread * (buf[buf_use_ind][i].i - mean) / (2*std)) / 255;
+
+ if (intensity > 255)
+ intensity = 255;
+ else if (intensity < 0)
+ intensity = 0;
+
+ out << intensity << " " << intensity << " " << intensity << "," << endl;
+ }
+
+ out << "]"
+ << "}" << endl
+ << "}" << endl
+ << "}" << endl
+ << "" << endl
+ << "]" << endl
+ << "} # end of scene transform" << endl
+ << "]" << endl
+ << "} # end of world transform" << endl;
+
+ }
+
};
+
int main(int argc, char **argv)
{
- if (SDL_Init(SDL_INIT_VIDEO) < 0)
- {
- fprintf(stderr, "SDL initialization error: %s\n", SDL_GetError());
- exit(1);
- }
+ ros::init(argc, argv);
+
atexit(SDL_Quit);
Cloud_Node cn;
- if (!cn.sdl_init())
- {
- fprintf(stderr, "SDL video startup error: %s\n", SDL_GetError());
- exit(1);
- }
+ cn.main_loop();
- while (cn.happy()) {
- cn.refresh();
- usleep(1000);
- }
+ ros::fini();
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
