Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[6961] pkg/trunk
|
From: <vij...@us...> - 2008-11-19 02:39:08
|
Revision: 6961
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6961&view=rev
Author: vijaypradeep
Date: 2008-11-19 02:39:06 +0000 (Wed, 19 Nov 2008)
Log Message:
-----------
Transition point_cloud utils to use to tf instead of rosTF
Now directly calling slerp commands in tf to do laser skew calculations.
Catching tf exceptions in PointCloudAssembler. Removed trailing whitespace.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
pkg/trunk/util/tf/src/transform_listener.cpp
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp 2008-11-19 02:35:25 UTC (rev 6960)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp 2008-11-19 02:39:06 UTC (rev 6961)
@@ -64,7 +64,7 @@
#include "ros/node.h"
-#include "rosTF/rosTF.h"
+#include "tf/transform_listener.h"
#include "std_msgs/LaserScan.h"
#include "std_msgs/PointCloud.h"
@@ -90,7 +90,7 @@
{
public:
- rosTFClient tf_;
+ tf::TransformListener tf_;
laser_scan::LaserProjection projector_;
LaserScan scan_;
@@ -103,6 +103,8 @@
PointCloudAssembler() : ros::node("point_cloud_assembler"), tf_(*this)
{
+ tf_.setExtrapolationLimit(ros::Duration(1.0)) ;
+
advertise_service("build_cloud", &PointCloudAssembler::buildCloud, this, 0) ; // Don't spawn threads so that we can avoid dealing with mutexing [for now]
subscribe("scan", scan_, &PointCloudAssembler::scans_callback, 40) ;
@@ -128,7 +130,7 @@
scan_hist_.push_back(scan_) ; // Add the newest scan to the back of the deque
total_pts_ += scan_.get_ranges_size() ; // Add the new scan to the running total of points
- printf("Got Scan: TotalPoints=%u\n", total_pts_) ;
+ //printf("PointCloudAssembler:: Got Scan: TotalPoints=%u", total_pts_) ;
}
bool buildCloud(BuildCloud::request& req, BuildCloud::response& resp)
@@ -152,29 +154,36 @@
{
i++ ;
}
+
printf(" Start i=%u\n", i) ;
-
+
// Populate the cloud
while ( i < scan_hist_.size() && // Don't go past end of deque
scan_hist_[i].header.stamp < req.end ) // Don't go past the end-time of the request
{
const LaserScan& cur_scan = scan_hist_[i] ;
-
- projector_.projectLaser(cur_scan, projector_cloud) ; // CRP: This takes care of converting the scan msg to the right format, so you don't really need to know what the format is.
- // CRP: Will be a transform in TF that takes scan time into account. High-precision, recalc for every point, from scan to point cloud. More expensive. Wait for new library to do this.
- target_frame_cloud=tf_.transformPointCloud(req.target_frame_id, projector_cloud) ;// CRP: We'll make people do this themselves for one scan line, we'll just tansform and publish aggregate clouds.
-
- resp.cloud.header = target_frame_cloud.header ; // Find a better place to do this/way to do this
- // full_cloud_.header.stamp = ros::Time::now(); //HACK
-
- for(unsigned int j = 0; j < target_frame_cloud.get_pts_size(); j++) // Populate full_cloud from the cloud
+
+ try
{
- resp.cloud.pts[cloud_count].x = target_frame_cloud.pts[j].x;
- resp.cloud.pts[cloud_count].y = target_frame_cloud.pts[j].y;
- resp.cloud.pts[cloud_count].z = target_frame_cloud.pts[j].z;
- resp.cloud.chan[0].vals[cloud_count] = target_frame_cloud.chan[0].vals[j];
- cloud_count++ ;
+ tf_.transformLaserScanToPointCloud(req.target_frame_id, target_frame_cloud, cur_scan) ;
+
+ for(unsigned int j = 0; j < target_frame_cloud.get_pts_size(); j++) // Populate full_cloud from the cloud
+ {
+ resp.cloud.pts[cloud_count].x = target_frame_cloud.pts[j].x ;
+ resp.cloud.pts[cloud_count].y = target_frame_cloud.pts[j].y ;
+ resp.cloud.pts[cloud_count].z = target_frame_cloud.pts[j].z ;
+ resp.cloud.chan[0].vals[cloud_count] = target_frame_cloud.chan[0].vals[j] ;
+ cloud_count++ ;
+ }
}
+ catch(tf::TransformException& ex)
+ {
+ ROS_WARN("Transform Exception %s", ex.what()) ;
+
+ return true ;
+ }
+
+ resp.cloud.header = target_frame_cloud.header ; // Find a better place to do this/way to do this
i++ ; // Check the next scan in the scan history
}
@@ -183,7 +192,6 @@
resp.cloud.set_pts_size( cloud_count ) ; // Resize the output accordingly
resp.cloud.chan[0].set_vals_size( cloud_count ) ;
-
return true ;
}
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2008-11-19 02:35:25 UTC (rev 6960)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2008-11-19 02:39:06 UTC (rev 6961)
@@ -58,12 +58,17 @@
pr2_mechanism_controllers::LaserScannerSignal prev_signal_;
pr2_mechanism_controllers::LaserScannerSignal cur_signal_;
+ bool first_time_ ;
+
+
PointCloudSnapshotter() : ros::node("point_cloud_snapshotter")
{
prev_signal_.header.stamp.fromNSec(0) ;
advertise<PointCloud> ("full_cloud", 1) ;
subscribe("laser_scanner_signal", cur_signal_, &PointCloudSnapshotter::scannerSignalCallback, 40) ;
+
+ first_time_ = true ;
}
~PointCloudSnapshotter()
@@ -73,21 +78,29 @@
void scannerSignalCallback()
{
- BuildCloud::request req ;
- BuildCloud::response resp ;
-
- req.begin = prev_signal_.header.stamp ;
- req.end = cur_signal_.header.stamp ;
- req.target_frame_id = "base" ;
-
- printf("Making Service Call...\n") ;
- ros::service::call("build_cloud", req, resp) ;
- printf("Done with service call\n") ;
-
- publish("full_cloud", resp.cloud) ;
- printf("Published Cloud size=%u\n", resp.cloud.get_pts_size()) ;
-
- prev_signal_ = cur_signal_ ;
+ if (first_time_)
+ {
+ prev_signal_ = cur_signal_ ;
+ first_time_ = false ;
+ }
+ else
+ {
+ BuildCloud::request req ;
+ BuildCloud::response resp ;
+
+ req.begin = prev_signal_.header.stamp ;
+ req.end = cur_signal_.header.stamp ;
+ req.target_frame_id = "torso" ;
+
+ printf("PointCloudSnapshotter::Making Service Call...\n") ;
+ ros::service::call("build_cloud", req, resp) ;
+ printf("PointCloudSnapshotter::Done with service call\n") ;
+
+ publish("full_cloud", resp.cloud) ;
+ printf("PointCloudSnapshotter::Published Cloud size=%u\n", resp.cloud.get_pts_size()) ;
+
+ prev_signal_ = cur_signal_ ;
+ }
}
} ;
Modified: pkg/trunk/util/tf/src/transform_listener.cpp
===================================================================
--- pkg/trunk/util/tf/src/transform_listener.cpp 2008-11-19 02:35:25 UTC (rev 6960)
+++ pkg/trunk/util/tf/src/transform_listener.cpp 2008-11-19 02:39:06 UTC (rev 6961)
@@ -200,17 +200,55 @@
std_msgs::PointCloud intermediate; //optimize out
projector_.projectLaser(scanIn, intermediate, -1.0, true);
+ // Extract transforms for the beginning and end of the laser scan
+ ros::Time start_time = scanIn.header.stamp ;
+ ros::Time end_time = scanIn.header.stamp + ros::Duration(scanIn.get_ranges_size()*scanIn.time_increment) ;
+
+ tf::Stamped<tf::Transform> start_transform ;
+ tf::Stamped<tf::Transform> end_transform ;
+ tf::Stamped<tf::Transform> cur_transform ;
+
+ lookupTransform(target_frame, scanIn.header.frame_id, start_time, start_transform) ;
+ lookupTransform(target_frame, scanIn.header.frame_id, end_time, end_transform) ;
+
+
unsigned int count = 0;
for (unsigned int i = 0; i < scanIn.get_ranges_size(); i++)
{
if (scanIn.ranges[i] < scanIn.range_max
&& scanIn.ranges[i] > scanIn.range_min) //only when valid
{
+ // Looking up transforms in tree is too expensive. Need more optimized way
+ /*
pointIn = tf::Stamped<tf::Point>(btVector3(intermediate.pts[i].x, intermediate.pts[i].y, intermediate.pts[i].z),
ros::Time(scanIn.header.stamp.to_ull() + (uint64_t) (scanIn.time_increment * 1000000000)),
pointIn.frame_id_ = scanIn.header.frame_id);///\todo optimize to no copy
transformPoint(target_frame, pointIn, pointOut);
+ */
+
+ // Instead, assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
+ btScalar ratio = i / ( (double) scanIn.get_ranges_size() - 1.0) ;
+
+ //! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector)
+ //Interpolate translation
+ btVector3 v ;
+ v.setInterpolate3(start_transform.getOrigin(), end_transform.getOrigin(), ratio) ;
+ cur_transform.setOrigin(v) ;
+
+ //Interpolate rotation
+ btQuaternion q1, q2 ;
+ start_transform.getBasis().getRotation(q1) ;
+ end_transform.getBasis().getRotation(q2) ;
+
+ // Compute the slerp-ed rotation
+ cur_transform.setRotation( slerp( q1, q2 , ratio) ) ;
+
+ // Apply the transform to the current point
+ btVector3 pointIn(intermediate.pts[i].x, intermediate.pts[i].y, intermediate.pts[i].z) ;
+ btVector3 pointOut = cur_transform * pointIn ;
+
+ // Copy transformed point into cloud
cloudOut.pts[count].x = pointOut.x();
cloudOut.pts[count].y = pointOut.y();
cloudOut.pts[count].z = pointOut.z();
@@ -224,7 +262,6 @@
//downsize if necessary
cloudOut.set_pts_size(count);
cloudOut.chan[0].set_vals_size(count);
-
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
