Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [133] pkg/trunk
|
From: <jl...@us...> - 2008-04-18 02:26:51
|
Revision: 133
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=133&view=rev
Author: jleibs
Date: 2008-04-17 19:26:07 -0700 (Thu, 17 Apr 2008)
Log Message:
-----------
Simple node using Morgans point cloud viewer to view laser data.
Modified Paths:
--------------
pkg/trunk/camera_calibration/src/calibrator/calibrator.cpp
pkg/trunk/etherdrive/build.yaml
pkg/trunk/etherdrive/manifest.xml
pkg/trunk/etherdrive/src/libetherdrive/etherdrive.cpp
Added Paths:
-----------
pkg/trunk/laser_viewer/
pkg/trunk/laser_viewer/build.yaml
pkg/trunk/laser_viewer/manifest.xml
pkg/trunk/laser_viewer/nodes/
pkg/trunk/laser_viewer/nodes/laser_viewer.log
pkg/trunk/laser_viewer/nodes/run_viewer
pkg/trunk/laser_viewer/rosbuild
pkg/trunk/laser_viewer/src/
pkg/trunk/laser_viewer/src/laser_viewer/
pkg/trunk/laser_viewer/src/laser_viewer/Makefile
pkg/trunk/laser_viewer/src/laser_viewer/laser_viewer.cpp
Modified: pkg/trunk/camera_calibration/src/calibrator/calibrator.cpp
===================================================================
--- pkg/trunk/camera_calibration/src/calibrator/calibrator.cpp 2008-04-18 02:14:17 UTC (rev 132)
+++ pkg/trunk/camera_calibration/src/calibrator/calibrator.cpp 2008-04-18 02:26:07 UTC (rev 133)
@@ -91,6 +91,8 @@
char dir_name[256];
+ CvPoint2D32f* last_corners;
+
Calibrator() : ROS_Slave()
{
register_sink(image_in = new FlowImage("image_in"), ROS_CALLBACK(Calibrator, image_received));
@@ -138,6 +140,8 @@
if (mkdir(dir_name, 0755)) {
std::cout << "Failed to make directory: " << dir_name;
}
+
+ last_corners = new CvPoint2D32f[12*12];
}
virtual ~Calibrator() { }
@@ -312,6 +316,22 @@
take_pic = false;
}
+ float maxdiff = 0;
+
+ for(int c=0; c<12*12; c++) {
+ float diff = sqrt( pow(corners[c].x - last_corners[c].x, 2.0) +
+ pow(corners[c].y - last_corners[c].y, 2.0));
+ last_corners[c].x = corners[c].x;
+ last_corners[c].y = corners[c].y;
+
+ if (diff > maxdiff) {
+ maxdiff = diff;
+ }
+ }
+
+ printf("Max diff: %g\n", maxdiff);
+
+
cvDrawChessboardCorners(cvimage_bgr, board_sz, corners, corner_count, found);
if (undistort) {
Modified: pkg/trunk/etherdrive/build.yaml
===================================================================
--- pkg/trunk/etherdrive/build.yaml 2008-04-18 02:14:17 UTC (rev 132)
+++ pkg/trunk/etherdrive/build.yaml 2008-04-18 02:26:07 UTC (rev 133)
@@ -1,5 +1,6 @@
cpp:
make:
- src/libetherdrive
- - test
+ - src/etherdrive_control
+ - test/test_etherdrive
Modified: pkg/trunk/etherdrive/manifest.xml
===================================================================
--- pkg/trunk/etherdrive/manifest.xml 2008-04-18 02:14:17 UTC (rev 132)
+++ pkg/trunk/etherdrive/manifest.xml 2008-04-18 02:26:07 UTC (rev 133)
@@ -11,5 +11,6 @@
<url>http://pr.willowgarage.com</url>
<depend package='roscpp'/>
<depend package='common_flows'/>
+<depend package='unstable_flows'/>
<repository>http://pr.willowgarage.com/repos</repository>
</package>
Modified: pkg/trunk/etherdrive/src/libetherdrive/etherdrive.cpp
===================================================================
--- pkg/trunk/etherdrive/src/libetherdrive/etherdrive.cpp 2008-04-18 02:14:17 UTC (rev 132)
+++ pkg/trunk/etherdrive/src/libetherdrive/etherdrive.cpp 2008-04-18 02:26:07 UTC (rev 133)
@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#include "etherdrive.h"
+#include "etherdrive/etherdrive.h"
EtherDrive::EtherDrive()
{
Added: pkg/trunk/laser_viewer/build.yaml
===================================================================
--- pkg/trunk/laser_viewer/build.yaml (rev 0)
+++ pkg/trunk/laser_viewer/build.yaml 2008-04-18 02:26:07 UTC (rev 133)
@@ -0,0 +1,3 @@
+cpp:
+ make:
+ - src/laser_viewer
\ No newline at end of file
Added: pkg/trunk/laser_viewer/manifest.xml
===================================================================
--- pkg/trunk/laser_viewer/manifest.xml (rev 0)
+++ pkg/trunk/laser_viewer/manifest.xml 2008-04-18 02:26:07 UTC (rev 133)
@@ -0,0 +1,14 @@
+<package>
+<description brief="Use the cloud viewer to display laster data.">
+
+</description>
+<author>Jeremy Leibs (email: le...@wi...)</author>
+<license>BSD</license>
+<url>http://pr.willowgarage.com</url>
+<depend package="common_flows"/>
+<depend package="hokuyourg_player"/>
+<depend package="cloud_viewer"/>
+<sys_depend lib="GL"/>
+<sys_depend lib="GLU"/>
+</package>
+
Added: pkg/trunk/laser_viewer/nodes/laser_viewer.log
===================================================================
--- pkg/trunk/laser_viewer/nodes/laser_viewer.log (rev 0)
+++ pkg/trunk/laser_viewer/nodes/laser_viewer.log 2008-04-18 02:26:07 UTC (rev 133)
@@ -0,0 +1,5 @@
+0.000987 ROS_PORT = 0
+0.001046 starting abyss XMLRPC server on port 32900
+0.011219 calling registerNode on master at http://127.0.1.1:11311/
+0.011307 sending port = 32900 to registerNode
+0.011782 TCPROS server up and running on port 34990
Added: pkg/trunk/laser_viewer/nodes/run_viewer
===================================================================
--- pkg/trunk/laser_viewer/nodes/run_viewer (rev 0)
+++ pkg/trunk/laser_viewer/nodes/run_viewer 2008-04-18 02:26:07 UTC (rev 133)
@@ -0,0 +1,8 @@
+#!/usr/bin/env ruby
+require "#{`#{ENV['ROS_ROOT']}/rospack latest yamlgraph`}/lib/yamlgraph/ygl.rb"
+g = YAMLGraph.new
+g.node 'laser_viewer/laser_viewer', {'launch'=>'xterm'}
+g.node 'hokuyourg_player/hokuyourg_player'#, {'launch'=>'xterm'}
+g.flow 'hokuyourg_player:scans', 'laser_viewer:scans'
+YAMLGraphLauncher.new.launch g
+
Property changes on: pkg/trunk/laser_viewer/nodes/run_viewer
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/laser_viewer/rosbuild
===================================================================
--- pkg/trunk/laser_viewer/rosbuild (rev 0)
+++ pkg/trunk/laser_viewer/rosbuild 2008-04-18 02:26:07 UTC (rev 133)
@@ -0,0 +1,2 @@
+#!/usr/bin/env ruby
+exec("#{`#{ENV['ROS_ROOT']}/rospack find rostools`}/scripts/yamlbuild", 'build.yaml', *ARGV)
Property changes on: pkg/trunk/laser_viewer/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/laser_viewer/src/laser_viewer/Makefile
===================================================================
--- pkg/trunk/laser_viewer/src/laser_viewer/Makefile (rev 0)
+++ pkg/trunk/laser_viewer/src/laser_viewer/Makefile 2008-04-18 02:26:07 UTC (rev 133)
@@ -0,0 +1,4 @@
+SRC = laser_viewer.cpp
+OUT = ../../nodes/laser_viewer
+PKG = laser_viewer
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/laser_viewer/src/laser_viewer/laser_viewer.cpp
===================================================================
--- pkg/trunk/laser_viewer/src/laser_viewer/laser_viewer.cpp (rev 0)
+++ pkg/trunk/laser_viewer/src/laser_viewer/laser_viewer.cpp 2008-04-18 02:26:07 UTC (rev 133)
@@ -0,0 +1,161 @@
+/*********************************************************************
+* 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/FlowLaserScan.h"
+#include "cloud_viewer/cloud_viewer.h"
+#include <SDL/SDL.h>
+#include "math.h"
+
+class Laser_Viewer : public ROS_Slave
+{
+public:
+ FlowLaserScan *laser;
+ CloudViewer *cloud_viewer;
+
+ Laser_Viewer() : ROS_Slave(), cloud_viewer(NULL)
+ {
+ register_sink(laser = new FlowLaserScan("scans"), ROS_CALLBACK(Laser_Viewer, cloud_callback));
+
+ cloud_viewer = new CloudViewer;
+
+ register_with_master();
+ }
+
+ virtual ~Laser_Viewer()
+ {
+ 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);
+ cloud_viewer->render();
+ SDL_GL_SwapBuffers();
+ 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);
+ cloud_viewer->render();
+ SDL_GL_SwapBuffers();
+ break;
+ }
+ }
+ }
+
+ void display_image() {
+ cloud_viewer->render();
+ SDL_GL_SwapBuffers();
+ }
+
+
+ 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()
+ {
+ cloud_viewer->clear_cloud();
+ for (int i = 0; i < laser->get_ranges_size(); i++) {
+ cloud_viewer->add_point(0,
+ cos(laser->angle_min + i*laser->angle_increment) * laser->ranges[i],
+ sin(laser->angle_min + i*laser->angle_increment) * laser->ranges[i],
+ 255,255,255);
+ }
+ }
+};
+
+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);
+
+ Laser_Viewer lv;
+
+ if (!lv.sdl_init())
+ {
+ fprintf(stderr, "SDL video startup error: %s\n", SDL_GetError());
+ exit(1);
+ }
+
+ while (lv.happy()) {
+ lv.refresh();
+ usleep(1000);
+ }
+ return 0;
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
