Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [261] pkg/branches/rosbus/unported/ tilting_laser
|
From: <jl...@us...> - 2008-04-30 23:03:34
|
Revision: 261
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=261&view=rev
Author: jleibs
Date: 2008-04-30 16:03:41 -0700 (Wed, 30 Apr 2008)
Log Message:
-----------
Changes to tilting_laser before everything gets shuffled.
Modified Paths:
--------------
pkg/branches/rosbus/unported/tilting_laser/manifest.xml
pkg/branches/rosbus/unported/tilting_laser/src/tilting_laser/Makefile
pkg/branches/rosbus/unported/tilting_laser/src/tilting_laser/tilting_laser.cpp
Added Paths:
-----------
pkg/branches/rosbus/unported/tilting_laser/Makefile
pkg/branches/rosbus/unported/tilting_laser/bin/
pkg/branches/rosbus/unported/tilting_laser/src/Makefile
Removed Paths:
-------------
pkg/branches/rosbus/unported/tilting_laser/build.yaml
pkg/branches/rosbus/unported/tilting_laser/nodes/
pkg/branches/rosbus/unported/tilting_laser/rosbuild
Added: pkg/branches/rosbus/unported/tilting_laser/Makefile
===================================================================
--- pkg/branches/rosbus/unported/tilting_laser/Makefile (rev 0)
+++ pkg/branches/rosbus/unported/tilting_laser/Makefile 2008-04-30 23:03:41 UTC (rev 261)
@@ -0,0 +1,2 @@
+SUBDIRS = src
+include $(shell rospack find mk)/recurse_subdirs.mk
Deleted: pkg/branches/rosbus/unported/tilting_laser/build.yaml
===================================================================
--- pkg/branches/rosbus/unported/tilting_laser/build.yaml 2008-04-30 23:03:33 UTC (rev 260)
+++ pkg/branches/rosbus/unported/tilting_laser/build.yaml 2008-04-30 23:03:41 UTC (rev 261)
@@ -1,3 +0,0 @@
-cpp:
- make:
- - src/tilting_laser
\ No newline at end of file
Modified: pkg/branches/rosbus/unported/tilting_laser/manifest.xml
===================================================================
--- pkg/branches/rosbus/unported/tilting_laser/manifest.xml 2008-04-30 23:03:33 UTC (rev 260)
+++ pkg/branches/rosbus/unported/tilting_laser/manifest.xml 2008-04-30 23:03:41 UTC (rev 261)
@@ -9,8 +9,8 @@
<license>BSD</license>
<url>http://pr.willowgarage.com</url>
<depend package="roscpp"/>
-<depend package="common_flows"/>
-<depend package="unstable_flows"/>
+<depend package="std_msgs"/>
+<depend package="unstable_msgs"/>
<depend package="hokuyourg_player"/>
<depend package="libTF"/>
</package>
Deleted: pkg/branches/rosbus/unported/tilting_laser/rosbuild
===================================================================
--- pkg/branches/rosbus/unported/tilting_laser/rosbuild 2008-04-30 23:03:33 UTC (rev 260)
+++ pkg/branches/rosbus/unported/tilting_laser/rosbuild 2008-04-30 23:03:41 UTC (rev 261)
@@ -1,2 +0,0 @@
-#!/usr/bin/env ruby
-exec("#{`#{ENV['ROS_ROOT']}/rospack find rostools`}/scripts/yamlbuild", 'build.yaml', *ARGV)
Added: pkg/branches/rosbus/unported/tilting_laser/src/Makefile
===================================================================
--- pkg/branches/rosbus/unported/tilting_laser/src/Makefile (rev 0)
+++ pkg/branches/rosbus/unported/tilting_laser/src/Makefile 2008-04-30 23:03:41 UTC (rev 261)
@@ -0,0 +1,2 @@
+SUBDIRS = tilting_laser
+include $(shell rospack find mk)/recurse_subdirs.mk
Modified: pkg/branches/rosbus/unported/tilting_laser/src/tilting_laser/Makefile
===================================================================
--- pkg/branches/rosbus/unported/tilting_laser/src/tilting_laser/Makefile 2008-04-30 23:03:33 UTC (rev 260)
+++ pkg/branches/rosbus/unported/tilting_laser/src/tilting_laser/Makefile 2008-04-30 23:03:41 UTC (rev 261)
@@ -1,4 +1,4 @@
SRC = tilting_laser.cpp
OUT = ../../nodes/tilting_laser
PKG = tilting_laser
-include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
+include $(shell rospack find mk)/node.mk
Modified: pkg/branches/rosbus/unported/tilting_laser/src/tilting_laser/tilting_laser.cpp
===================================================================
--- pkg/branches/rosbus/unported/tilting_laser/src/tilting_laser/tilting_laser.cpp 2008-04-30 23:03:33 UTC (rev 260)
+++ pkg/branches/rosbus/unported/tilting_laser/src/tilting_laser/tilting_laser.cpp 2008-04-30 23:03:41 UTC (rev 261)
@@ -32,23 +32,26 @@
* 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 "ros/node.h"
+#include "std_msgs/MsgEmpty.h"
+#include "std_msgs/MsgLaserScan.h"
+#include "std_msgs/MsgPointCloudFloat32.h"
+#include "unstable_msgs/MsgActuator.h"
#include "libTF/libTF.h"
#include "math.h"
-class Tilting_Laser : public ROS_Slave
+class Tilting_Laser : public ros::node
{
public:
- FlowLaserScan *scans;
- FlowActuator *encoder;
- FlowActuator *cmd;
- FlowPointCloudFloat32 *cloud;
- FlowEmpty *shutter;
+ MsgPointCloudFloat32 cloud;
+ MsgEmpty shutter;
+ MsgActuator cmd;
+
+ MsgLaserScan scans;
+ MsgActuator encoder;
+
+
struct timeval starttime;
double next_shutter;
@@ -57,27 +60,25 @@
double min_ang;
double max_ang;
- int scancount;
-
TransformReference TR;
- Tilting_Laser() : ROS_Slave(), next_shutter(0.0), scancount(0)
+ Tilting_Laser() : ros::node("tilting_laser"), 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());
+
+ advertise("cloud", cloud);
+ advertise("shutter", shutter);
+ advertise("mot2_cmd", cmd);
- if (!get_double_param(".period", &period))
+ subscribe("scans", scans, &Tilting_Laser::scans_callback);
+ subscribe("mot2", encoder, &Tilting_Laser::encoder_callback);
+
+ if (!get_param(".period", period))
period = 5.0;
- if (!get_double_param(".min_ang", &min_ang))
+ if (!get_param(".min_ang", min_ang))
min_ang = -1.3;
- if (!get_double_param(".max_ang", &max_ang))
+ if (!get_param(".max_ang", max_ang))
max_ang = 0.6;
unsigned long long time = Quaternion3D::Qgettime();
@@ -100,24 +101,24 @@
}
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());
+ 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());
/*
- 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(scans->angle_min + i*scans->angle_increment) * scans->ranges[i];
- cloud->z[i] = sin(encoder->val)*cos(scans->angle_min + i*scans->angle_increment) * scans->ranges[i];
+ 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(scans.angle_min + i*scans.angle_increment) * scans.ranges[i];
+ cloud.z[i] = sin(encoder.val)*cos(scans.angle_min + i*scans.angle_increment) * scans.ranges[i];
}
*/
- NEWMAT::Matrix points(4,scans->get_ranges_size());
- for (int i = 0; i < scans->get_ranges_size(); i++) {
- if (scans->ranges[i] < 20.0) {
- points(1,i+1) = cos(scans->angle_min + i*scans->angle_increment) * scans->ranges[i];
- points(2,i+1) = sin(scans->angle_min + i*scans->angle_increment) * scans->ranges[i];
+ NEWMAT::Matrix points(4,scans.get_ranges_size());
+ for (int i = 0; i < scans.get_ranges_size(); i++) {
+ if (scans.ranges[i] < 20.0) {
+ points(1,i+1) = cos(scans.angle_min + i*scans.angle_increment) * scans.ranges[i];
+ points(2,i+1) = sin(scans.angle_min + i*scans.angle_increment) * scans.ranges[i];
points(3,i+1) = 0.0;
points(4,i+1) = 1.0;
} else {
@@ -128,34 +129,32 @@
}
}
- unsigned long long time = (unsigned long long)scans->get_stamp_secs()*1000000 + (unsigned long long)scans->get_stamp_nsecs()/1000 - 20000;
+ unsigned long long time = (unsigned long long)scans.get_stamp_secs()*1000000 + (unsigned long long)scans.get_stamp_nsecs()/1000 - 20000;
NEWMAT::Matrix rot_points = TR.getMatrix(1,3,time) * points;
- for (int 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);
+ for (int 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);
}
- scancount++;
-
- encoder->unlock_atom();
- cloud->publish();
+ encoder.unlock();
+ publish("cloud", cloud);
}
void encoder_callback() {
- unsigned long long time = (unsigned long long)encoder->get_stamp_secs()*1000000 + (unsigned long long)encoder->get_stamp_nsecs()/1000;
+ unsigned long long time = (unsigned long long)encoder.get_stamp_secs()*1000000 + (unsigned long long)encoder.get_stamp_nsecs()/1000;
TR.setWithEulers(3, 2,
.02, 0, .02,
0.0, 0.0, 0.0,
time);
TR.setWithEulers(2, 1,
0, 0, 0,
- 0, -encoder->val, 0,
+ 0, -encoder.val, 0,
time);
motor_control(); // Control on encoder reads sounds reasonable
- // printf("I got some encoder values: %g!\n", encoder->val);
+ // printf("I got some encoder values: %g!\n", encoder.val);
}
double timeval_diff (struct timeval x,
@@ -173,15 +172,15 @@
double elapsed_cycles = timeval_diff(nowtime, starttime) / (period);
double index = fabs(fmod(elapsed_cycles, 1) * 2.0 - 1.0);
- cmd->val = index * (max_ang - min_ang) + min_ang;
- cmd->rel = false;
- cmd->valid = true;
+ cmd.val = index * (max_ang - min_ang) + min_ang;
+ cmd.rel = false;
+ cmd.valid = true;
if (elapsed_cycles > next_shutter) {
next_shutter += 0.5;
- shutter->publish();
+ publish("shutter", shutter);
}
- cmd->publish();
+ publish("mot2_cmd",cmd);
}
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|