Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [150] pkg/trunk
|
From: <jl...@us...> - 2008-04-21 21:50:02
|
Revision: 150
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=150&view=rev
Author: jleibs
Date: 2008-04-21 14:49:53 -0700 (Mon, 21 Apr 2008)
Log Message:
-----------
New node to control and view the output of a tilting laser.
Added Paths:
-----------
pkg/trunk/tilting_laser/
pkg/trunk/tilting_laser/build.yaml
pkg/trunk/tilting_laser/manifest.xml
pkg/trunk/tilting_laser/nodes/
pkg/trunk/tilting_laser/nodes/launch_tl
pkg/trunk/tilting_laser/rosbuild
pkg/trunk/tilting_laser/src/
pkg/trunk/tilting_laser/src/tilting_laser/
pkg/trunk/tilting_laser/src/tilting_laser/Makefile
pkg/trunk/tilting_laser/src/tilting_laser/tilting_laser.cpp
Added: pkg/trunk/tilting_laser/build.yaml
===================================================================
--- pkg/trunk/tilting_laser/build.yaml (rev 0)
+++ pkg/trunk/tilting_laser/build.yaml 2008-04-21 21:49:53 UTC (rev 150)
@@ -0,0 +1,3 @@
+cpp:
+ make:
+ - src/tilting_laser
\ No newline at end of file
Added: pkg/trunk/tilting_laser/manifest.xml
===================================================================
--- pkg/trunk/tilting_laser/manifest.xml (rev 0)
+++ pkg/trunk/tilting_laser/manifest.xml 2008-04-21 21:49:53 UTC (rev 150)
@@ -0,0 +1,16 @@
+<package>
+<description brief="A package for a laser mounted on a tilting stage.">
+
+This is a package to deal with a laser range finder mounted on a
+tilting stage, such as the one found in the head of PR2.
+
+</description>
+<author>Jeremy Leibs</author>
+<license>BSD</license>
+<url>http://pr.willowgarage.com</url>
+<depend package="roscpp"/>
+<depend package="common_flows"/>
+<depend package="unstable_flows"/>
+<depend package="hokuyourg_player"/>
+</package>
+
Added: pkg/trunk/tilting_laser/nodes/launch_tl
===================================================================
--- pkg/trunk/tilting_laser/nodes/launch_tl (rev 0)
+++ pkg/trunk/tilting_laser/nodes/launch_tl 2008-04-21 21:49:53 UTC (rev 150)
@@ -0,0 +1,17 @@
+#!/usr/bin/env ruby
+require "#{`#{ENV['ROS_ROOT']}/rospack latest yamlgraph`}/lib/yamlgraph/ygl.rb"
+g = YAMLGraph.new
+g.node 'tilting_laser/tilting_laser', {'launch'=>'xterm'}
+g.param 'hokuyourg.port', '/dev/ttyACM0'
+g.node 'hokuyourg_player/hokuyourg', {'launch'=>'xterm'}
+g.node 'etherdrive/etherdrive', {'launch'=>'xterm'}
+g.param 'etherdrive.pulse_per_rad1', '9549.29659'
+g.node 'cloud_viewer/cloud_node', {'launch'=>'xterm'}
+g.flow 'etherdrive:mot1', 'tilting_laser:encoder'
+g.flow 'tilting_laser:cmd', 'etherdrive:mot1_cmd'
+g.param 'tilting_laser.period', '10.0'
+g.flow 'hokuyourg:scans', 'tilting_laser:scans'
+g.flow 'tilting_laser:cloud', 'cloud_node:cloud'
+g.flow 'tilting_laser:shutter', 'cloud_node:shutter'
+YAMLGraphLauncher.new.launch g
+
Property changes on: pkg/trunk/tilting_laser/nodes/launch_tl
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/tilting_laser/rosbuild
===================================================================
--- pkg/trunk/tilting_laser/rosbuild (rev 0)
+++ pkg/trunk/tilting_laser/rosbuild 2008-04-21 21:49:53 UTC (rev 150)
@@ -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/tilting_laser/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/tilting_laser/src/tilting_laser/Makefile
===================================================================
--- pkg/trunk/tilting_laser/src/tilting_laser/Makefile (rev 0)
+++ pkg/trunk/tilting_laser/src/tilting_laser/Makefile 2008-04-21 21:49:53 UTC (rev 150)
@@ -0,0 +1,4 @@
+SRC = tilting_laser.cpp
+OUT = ../../nodes/tilting_laser
+PKG = tilting_laser
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/tilting_laser/src/tilting_laser/tilting_laser.cpp
===================================================================
--- pkg/trunk/tilting_laser/src/tilting_laser/tilting_laser.cpp (rev 0)
+++ pkg/trunk/tilting_laser/src/tilting_laser/tilting_laser.cpp 2008-04-21 21:49:53 UTC (rev 150)
@@ -0,0 +1,134 @@
+/*********************************************************************
+* 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/FlowEmpty.h"
+#include "common_flows/FlowLaserScan.h"
+#include "common_flows/FlowPointCloudFloat32.h"
+#include "unstable_flows/FlowActuator.h"
+#include "math.h"
+
+class Tilting_Laser : public ROS_Slave
+{
+public:
+ FlowLaserScan *scans;
+ FlowActuator *encoder;
+ FlowActuator *cmd;
+ FlowPointCloudFloat32 *cloud;
+ FlowEmpty *shutter;
+
+ struct timeval starttime;
+
+ double next_shutter;
+ double period;
+
+ Tilting_Laser() : ROS_Slave(), next_shutter(0.0)
+ {
+ register_source(cloud = new FlowPointCloudFloat32("cloud"));
+ register_source(shutter = new FlowEmpty("shutter"));
+ register_sink(scans = new FlowLaserScan("scans"), ROS_CALLBACK(Tilting_Laser, scans_callback));
+ register_sink(encoder = new FlowActuator("encoder"), ROS_CALLBACK(Tilting_Laser, encoder_callback));
+ register_source(cmd = new FlowActuator("cmd"));
+ register_with_master();
+ printf("package path is [%s]\n", get_my_package_path().c_str());
+
+ if (!get_double_param(".period", &period))
+ period = 5.0;
+
+ gettimeofday(&starttime,NULL);
+ }
+
+ virtual ~Tilting_Laser()
+ {
+
+ }
+
+ void scans_callback() {
+ encoder->lock_atom();
+ cloud->set_x_size(scans->get_ranges_size());
+ cloud->set_y_size(scans->get_ranges_size());
+ cloud->set_z_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];
+ cloud->y[i] = sin(encoder->val)*cos(scans->angle_min + i*scans->angle_increment) * scans->ranges[i];
+ cloud->z[i] = sin(scans->angle_min + i*scans->angle_increment) * scans->ranges[i];
+ }
+ encoder->unlock_atom();
+ cloud->publish();
+ }
+
+ void encoder_callback() {
+ motor_control(); // Control on encoder reads sounds reasonable
+ // printf("I got some encoder values: %g!\n", encoder->val);
+ }
+
+ double timeval_diff (struct timeval x,
+ struct timeval y) {
+ double dsec = x.tv_sec - y.tv_sec;
+ double dusec = x.tv_usec - y.tv_usec;
+ return dsec + dusec/1000000.0;
+ }
+
+ void motor_control() {
+
+ struct timeval nowtime;
+ gettimeofday(&nowtime,NULL);
+
+ double elapsed_cycles = timeval_diff(nowtime, starttime) / (period);
+ double index = fabs(fmod(elapsed_cycles, 1) * 2.0 - 1.0);
+
+ cmd->val = index * (0.6 + 1.3) - 1.3;
+ cmd->rel = false;
+ cmd->valid = true;
+
+ if (elapsed_cycles > next_shutter) {
+ next_shutter += 0.5;
+ shutter->publish();
+ }
+
+ cmd->publish();
+ }
+
+};
+
+int main(int argc, char **argv)
+{
+ Tilting_Laser tl;
+ while (tl.happy()) {
+ usleep(10000);
+ }
+ return 0;
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
