Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7901] pkg/trunk
|
From: <tf...@us...> - 2008-12-10 07:09:37
|
Revision: 7901
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7901&view=rev
Author: tfoote
Date: 2008-12-10 07:09:28 +0000 (Wed, 10 Dec 2008)
Log Message:
-----------
moving high performance laser scan projection into laser_scan package
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
pkg/trunk/util/laser_scan/include/laser_scan/laser_scan.h
pkg/trunk/util/laser_scan/manifest.xml
pkg/trunk/util/laser_scan/src/laser_scan.cc
pkg/trunk/util/tf/include/tf/transform_listener.h
pkg/trunk/util/tf/manifest.xml
pkg/trunk/util/tf/src/transform_listener.cpp
pkg/trunk/visualization/ogre_visualizer/manifest.xml
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.h
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-12-10 06:52:35 UTC (rev 7900)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp 2008-12-10 07:09:28 UTC (rev 7901)
@@ -180,7 +180,7 @@
}
else // Do it the slower (more accurate) way
{
- tf_.transformLaserScanToPointCloud(req.target_frame_id, target_frame_cloud, cur_scan) ;
+ projector_.transformLaserScanToPointCloud(req.target_frame_id, target_frame_cloud, cur_scan, tf_) ;
}
for(unsigned int j = 0; j < target_frame_cloud.get_pts_size(); j++) // Populate full_cloud from the cloud
Modified: pkg/trunk/util/laser_scan/include/laser_scan/laser_scan.h
===================================================================
--- pkg/trunk/util/laser_scan/include/laser_scan/laser_scan.h 2008-12-10 06:52:35 UTC (rev 7900)
+++ pkg/trunk/util/laser_scan/include/laser_scan/laser_scan.h 2008-12-10 07:09:28 UTC (rev 7901)
@@ -38,6 +38,8 @@
#include <newmat10/newmatio.h>
#include <newmat10/newmatap.h>
+#include "tf/tf.h"
+
#include "std_msgs/LaserScan.h"
#include "std_msgs/PointCloud.h"
#include "std_msgs/PointCloud.h"
@@ -69,6 +71,10 @@
*/
void projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloud & cloud_out, double range_cutoff=-1.0, bool preservative = false);
+
+ /** \brief Transform a std_msgs::LaserScan into a PointCloud in target frame */
+ void transformLaserScanToPointCloud(const std::string& target_frame, std_msgs::PointCloud & cloudOut, const std_msgs::LaserScan & scanIn, tf::Transformer & tf);
+
private:
/** \brief Return the unit vectors for this configuration
Modified: pkg/trunk/util/laser_scan/manifest.xml
===================================================================
--- pkg/trunk/util/laser_scan/manifest.xml 2008-12-10 06:52:35 UTC (rev 7900)
+++ pkg/trunk/util/laser_scan/manifest.xml 2008-12-10 07:09:28 UTC (rev 7901)
@@ -11,6 +11,7 @@
<depend package="newmat10"/>
<depend package="std_msgs"/>
<depend package="roscpp"/>
+<depend package="tf"/>
<depend package="rosthread"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -llaser_scan"/>
Modified: pkg/trunk/util/laser_scan/src/laser_scan.cc
===================================================================
--- pkg/trunk/util/laser_scan/src/laser_scan.cc 2008-12-10 06:52:35 UTC (rev 7900)
+++ pkg/trunk/util/laser_scan/src/laser_scan.cc 2008-12-10 07:09:28 UTC (rev 7901)
@@ -149,4 +149,100 @@
}
};
+void LaserProjection::transformLaserScanToPointCloud(const std::string & target_frame, std_msgs::PointCloud & cloudOut, const std_msgs::LaserScan & scanIn,
+ tf::Transformer& tf)
+{
+ cloudOut.header = scanIn.header;
+ cloudOut.header.frame_id = target_frame;
+ cloudOut.set_pts_size(scanIn.get_ranges_size());
+ if (scanIn.get_intensities_size() > 0)
+ {
+ cloudOut.set_chan_size(2);
+ cloudOut.chan[0].name ="intensities";
+ cloudOut.chan[0].set_vals_size(scanIn.get_intensities_size());
+
+ cloudOut.chan[1].name ="index";
+ cloudOut.chan[1].set_vals_size(scanIn.get_ranges_size());
+ }
+
+ tf::Stamped<tf::Point> pointIn;
+ tf::Stamped<tf::Point> pointOut;
+
+ pointIn.frame_id_ = scanIn.header.frame_id;
+
+ ///\todo this can be optimized
+ std_msgs::PointCloud intermediate; //optimize out
+
+ 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().fromSec(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 ;
+
+ tf.lookupTransform(target_frame, scanIn.header.frame_id, start_time, start_transform) ;
+ tf.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();
+
+ //Copy index over from projected point cloud
+ cloudOut.chan[1].vals[count] = intermediate.chan[1].vals[i];
+
+
+ if (scanIn.get_intensities_size() >= i) /// \todo optimize and catch length difference better
+ cloudOut.chan[0].vals[count] = scanIn.intensities[i];
+ count++;
+ }
+
+ }
+ //downsize if necessary
+ cloudOut.set_pts_size(count);
+ cloudOut.chan[0].set_vals_size(count);
+ cloudOut.chan[1].set_vals_size(count);
+}
+
+
} //laser_scan
Modified: pkg/trunk/util/tf/include/tf/transform_listener.h
===================================================================
--- pkg/trunk/util/tf/include/tf/transform_listener.h 2008-12-10 06:52:35 UTC (rev 7900)
+++ pkg/trunk/util/tf/include/tf/transform_listener.h 2008-12-10 07:09:28 UTC (rev 7901)
@@ -33,7 +33,6 @@
#define TF_TRANSFORMLISTENER_H
#include "std_msgs/PointCloud.h"
-#include "laser_scan/laser_scan.h"
#include "tf/tfMessage.h"
#include "tf/tf.h"
#include "ros/node.h"
@@ -127,8 +126,6 @@
const std_msgs::PointCloud& pcin,
const std::string& fixed_frame, std_msgs::PointCloud& pcout);
- /** \brief Transform a std_msgs::LaserScan into a PointCloud in target frame */
- void transformLaserScanToPointCloud(const std::string& target_frame, std_msgs::PointCloud & cloudOut, const std_msgs::LaserScan & scanIn);
///\todo move to high precision laser projector class void projectAndTransformLaserScan(const std_msgs::LaserScan& scan_in, std_msgs::PointCloud& pcout);
@@ -150,10 +147,6 @@
void transformPointCloud(const std::string & target_frame, const Transform& transform, const ros::Time& target_time, const std_msgs::PointCloud& pcin, std_msgs::PointCloud& pcout);
- /** @brief A helper class for projecting laser scans */
- laser_scan::LaserProjection projector_;
-
-
///\todo Remove : for backwards compatability only
void receiveArray()
{
Modified: pkg/trunk/util/tf/manifest.xml
===================================================================
--- pkg/trunk/util/tf/manifest.xml 2008-12-10 06:52:35 UTC (rev 7900)
+++ pkg/trunk/util/tf/manifest.xml 2008-12-10 07:09:28 UTC (rev 7901)
@@ -15,7 +15,6 @@
<depend package="std_msgs"/>
<depend package="roscpp"/>
<depend package="bullet"/>
-<depend package="laser_scan"/>
<depend package="boost"/>
<depend package="rospy"/>
<export>
Modified: pkg/trunk/util/tf/src/transform_listener.cpp
===================================================================
--- pkg/trunk/util/tf/src/transform_listener.cpp 2008-12-10 06:52:35 UTC (rev 7900)
+++ pkg/trunk/util/tf/src/transform_listener.cpp 2008-12-10 07:09:28 UTC (rev 7901)
@@ -179,100 +179,8 @@
}
-void TransformListener::transformLaserScanToPointCloud(const std::string & target_frame, std_msgs::PointCloud & cloudOut, const std_msgs::LaserScan & scanIn)
-{
- cloudOut.header = scanIn.header;
- cloudOut.header.frame_id = target_frame;
- cloudOut.set_pts_size(scanIn.get_ranges_size());
- if (scanIn.get_intensities_size() > 0)
- {
- cloudOut.set_chan_size(2);
- cloudOut.chan[0].name ="intensities";
- cloudOut.chan[0].set_vals_size(scanIn.get_intensities_size());
- cloudOut.chan[1].name ="index";
- cloudOut.chan[1].set_vals_size(scanIn.get_ranges_size());
- }
- tf::Stamped<tf::Point> pointIn;
- tf::Stamped<tf::Point> pointOut;
-
- pointIn.frame_id_ = scanIn.header.frame_id;
-
- ///\todo this can be optimized
- 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().fromSec(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();
-
- //Copy index over from projected point cloud
- cloudOut.chan[1].vals[count] = intermediate.chan[1].vals[i];
-
-
- if (scanIn.get_intensities_size() >= i) /// \todo optimize and catch length difference better
- cloudOut.chan[0].vals[count] = scanIn.intensities[i];
- count++;
- }
-
- }
- //downsize if necessary
- cloudOut.set_pts_size(count);
- cloudOut.chan[0].set_vals_size(count);
- cloudOut.chan[1].set_vals_size(count);
-}
-
-
void TransformListener::subscription_callback()
{
for (uint i = 0; i < msg_in_.transforms.size(); i++)
Modified: pkg/trunk/visualization/ogre_visualizer/manifest.xml
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/manifest.xml 2008-12-10 06:52:35 UTC (rev 7900)
+++ pkg/trunk/visualization/ogre_visualizer/manifest.xml 2008-12-10 07:09:28 UTC (rev 7901)
@@ -15,6 +15,7 @@
<depend package="std_msgs"/>
<depend package="rosthread"/>
<depend package="tf"/>
+ <depend package="laser_scan"/>
<depend package="wg_robot_description"/>
<depend package="gazebo_robot_description"/>
<depend package="wg_robot_description_parser"/>
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.cpp 2008-12-10 06:52:35 UTC (rev 7900)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.cpp 2008-12-10 07:09:28 UTC (rev 7901)
@@ -65,7 +65,8 @@
setStyle( style_ );
setBillboardSize( billboard_size_ );
-
+
+ projector_ = new laser_scan::LaserProjection();
scan_notifier_ = new tf::MessageNotifier<std_msgs::LaserScan>(tf_, ros_node_, boost::bind(&LaserScanDisplay::incomingScanCallback, this, _1), "", "", 10);
cloud_notifier_ = new tf::MessageNotifier<std_msgs::PointCloud>(tf_, ros_node_, boost::bind(&LaserScanDisplay::incomingCloudCallback, this, _1), "", "", 10);
}
@@ -368,7 +369,7 @@
frame_id = fixed_frame_;
}
- tf_->transformLaserScanToPointCloud( scan->header.frame_id, cloud, *scan );
+ projector_->transformLaserScanToPointCloud( scan->header.frame_id, cloud, *scan , *tf_);
transformCloud( cloud );
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.h 2008-12-10 06:52:35 UTC (rev 7900)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.h 2008-12-10 07:09:28 UTC (rev 7901)
@@ -34,6 +34,8 @@
#include "ogre_tools/point_cloud.h"
#include "helpers/color.h"
+#include "laser_scan/laser_scan.h"
+
#include "std_msgs/LaserScan.h"
#include "std_msgs/PointCloud.h"
#include "std_msgs/Empty.h"
@@ -204,6 +206,8 @@
ColorProperty* color_property_;
EnumProperty* style_property_;
+ laser_scan::LaserProjection* projector_; ///< A Helper class to project laser scans
+
tf::MessageNotifier<std_msgs::PointCloud>* cloud_notifier_;
tf::MessageNotifier<std_msgs::LaserScan>* scan_notifier_;
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
