Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18345] pkg/trunk
|
From: <is...@us...> - 2009-07-06 21:32:45
|
Revision: 18345
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18345&view=rev
Author: isucan
Date: 2009-07-06 21:32:42 +0000 (Mon, 06 Jul 2009)
Log Message:
-----------
fixed the most awesome bug I have *ever* seen
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
pkg/trunk/util/robot_self_filter/CMakeLists.txt
pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/util/robot_self_filter/manifest.xml
pkg/trunk/util/robot_self_filter/src/self_filter.cpp
pkg/trunk/util/robot_self_filter/src/self_mask.cpp
pkg/trunk/util/robot_self_filter/src/test_filter.cpp
Modified: pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-07-06 21:31:39 UTC (rev 18344)
+++ pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-07-06 21:32:42 UTC (rev 18345)
@@ -190,7 +190,7 @@
bi_.real_maxZ = bi_.dimensionZ + bi_.originZ;
}
- void computeCloudMask(const robot_msgs::PointCloud &cloud, std::vector<bool> &mask)
+ void computeCloudMask(const robot_msgs::PointCloud &cloud, std::vector<int> &mask)
{
if (cloud_annotation_.empty())
sf_.mask(cloud, mask);
@@ -223,7 +223,7 @@
if (!mapProcessing_.try_lock())
return;
- std::vector<bool> mask;
+ std::vector<int> mask;
robot_msgs::PointCloud out;
ros::WallTime tm = ros::WallTime::now();
@@ -247,7 +247,7 @@
}
CMap obstacles;
- constructCollisionMap(out, mask, true, obstacles);
+ constructCollisionMap(out, mask, 1, obstacles);
CMap diff;
set_difference(obstacles.begin(), obstacles.end(), currentMap_.begin(), currentMap_.end(),
std::inserter(diff, diff.begin()), CollisionPointOrder());
@@ -260,7 +260,7 @@
{
mapProcessing_.lock();
- std::vector<bool> mask;
+ std::vector<int> mask;
robot_msgs::PointCloud out;
ros::WallTime tm = ros::WallTime::now();
@@ -292,11 +292,11 @@
#pragma omp section
{
- constructCollisionMap(out, mask, true, obstacles);
+ constructCollisionMap(out, mask, 1, obstacles);
}
#pragma omp section
{
- constructCollisionMap(out, mask, false, self);
+ constructCollisionMap(out, mask, 0, self);
}
#pragma omp section
{
@@ -553,7 +553,7 @@
}
/** Construct an axis-aligned collision map from a point cloud assumed to be in the robot frame */
- void constructCollisionMap(const robot_msgs::PointCloud &cloud, const std::vector<bool> &mask, bool keep, CMap &map)
+ void constructCollisionMap(const robot_msgs::PointCloud &cloud, const std::vector<int> &mask, int keep, CMap &map)
{
const unsigned int n = cloud.pts.size();
CollisionPoint c;
Modified: pkg/trunk/util/robot_self_filter/CMakeLists.txt
===================================================================
--- pkg/trunk/util/robot_self_filter/CMakeLists.txt 2009-07-06 21:31:39 UTC (rev 18344)
+++ pkg/trunk/util/robot_self_filter/CMakeLists.txt 2009-07-06 21:32:42 UTC (rev 18345)
@@ -12,13 +12,13 @@
rospack(robot_self_filter)
-rospack_add_library(new_robot_self_filter src/self_mask.cpp)
-rospack_add_openmp_flags(new_robot_self_filter)
+rospack_add_library(robot_self_filter src/self_mask.cpp)
+rospack_add_openmp_flags(robot_self_filter)
rospack_add_executable(test_filter src/test_filter.cpp)
rospack_add_openmp_flags(test_filter)
-target_link_libraries(test_filter new_robot_self_filter)
+target_link_libraries(test_filter robot_self_filter)
rospack_add_executable(self_filter src/self_filter.cpp)
rospack_add_openmp_flags(self_filter)
-target_link_libraries(self_filter new_robot_self_filter)
+target_link_libraries(self_filter robot_self_filter)
Modified: pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
===================================================================
--- pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h 2009-07-06 21:31:39 UTC (rev 18344)
+++ pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h 2009-07-06 21:32:42 UTC (rev 18345)
@@ -70,6 +70,12 @@
{
configure();
}
+
+ /** \brief Construct the filter */
+ SelfMask(tf::TransformListener &tf, const std::vector<std::string> &links, double scale, double padd) : rm_("robot_description"), tf_(tf)
+ {
+ configure(links, scale, padd);
+ }
/** \brief Destructor to clean up
*/
@@ -83,14 +89,14 @@
/** \brief Compute the mask for a given pointcloud. If a mask element is true, the point
is outside the robot
*/
- void mask(const robot_msgs::PointCloud& data_in, std::vector<bool> &mask);
+ void mask(const robot_msgs::PointCloud& data_in, std::vector<int> &mask);
/** \brief Assume subsequent calls to getMask() will be in the frame passed to this function */
void assumeFrame(const roslib::Header& header);
/** \brief Get the mask value for an individual point. No
setup is performed, assumeFrame() should be called before use */
- bool getMask(double x, double y, double z) const;
+ int getMask(double x, double y, double z) const;
/** \brief Get the set of frames that correspond to the links */
void getLinkFrames(std::vector<std::string> &frames) const;
@@ -100,11 +106,14 @@
/** \brief Configure the filter. */
bool configure(void);
+ /** \brief Configure the filter. */
+ bool configure(const std::vector<std::string> &links, double scale, double padd);
+
/** \brief Compute bounding spheres for the checked robot links. */
void computeBoundingSpheres(void);
/** \brief Perform the actual mask computation. */
- void maskSimple(const robot_msgs::PointCloud& data_in, std::vector<bool> &mask);
+ void maskAux(const robot_msgs::PointCloud& data_in, std::vector<int> &mask);
planning_environment::RobotModels rm_;
tf::TransformListener &tf_;
Modified: pkg/trunk/util/robot_self_filter/manifest.xml
===================================================================
--- pkg/trunk/util/robot_self_filter/manifest.xml 2009-07-06 21:31:39 UTC (rev 18344)
+++ pkg/trunk/util/robot_self_filter/manifest.xml 2009-07-06 21:32:42 UTC (rev 18345)
@@ -18,7 +18,7 @@
<depend package="planning_environment"/>
<export>
- <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lnew_robot_self_filter" />
+ <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lrobot_self_filter" />
</export>
</package>
Modified: pkg/trunk/util/robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_filter.cpp 2009-07-06 21:31:39 UTC (rev 18344)
+++ pkg/trunk/util/robot_self_filter/src/self_filter.cpp 2009-07-06 21:32:42 UTC (rev 18345)
@@ -58,7 +58,7 @@
void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
{
robot_msgs::PointCloud out;
- std::vector<bool> mask;
+ std::vector<int> mask;
ros::WallTime tm = ros::WallTime::now();
sf_.mask(*cloud, mask);
double sec = (ros::WallTime::now() - tm).toSec();
@@ -67,7 +67,7 @@
ROS_DEBUG("Self filter: reduced %d points to %d points in %f seconds", (int)cloud->pts.size(), (int)out.pts.size(), sec);
}
- void fillResult(const robot_msgs::PointCloud& data_in, const std::vector<bool> &keep, robot_msgs::PointCloud& data_out)
+ void fillResult(const robot_msgs::PointCloud& data_in, const std::vector<int> &keep, robot_msgs::PointCloud& data_out)
{
const unsigned int np = data_in.pts.size();
Modified: pkg/trunk/util/robot_self_filter/src/self_mask.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_mask.cpp 2009-07-06 21:31:39 UTC (rev 18344)
+++ pkg/trunk/util/robot_self_filter/src/self_mask.cpp 2009-07-06 21:32:42 UTC (rev 18345)
@@ -32,12 +32,8 @@
#include <climits>
#include <ros/console.h>
-bool robot_self_filter::SelfMask::configure(void)
+bool robot_self_filter::SelfMask::configure(const std::vector<std::string> &links, double scale, double padd)
{
- std::vector<std::string> links = rm_.getSelfSeeLinks();
- double scale = rm_.getSelfSeeScale();
- double padd = rm_.getSelfSeePadding();
-
// from the geometric model, find the shape of each link of interest
// and create a body from it, one that knows about poses and can
// check for point inclusion
@@ -72,22 +68,27 @@
for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
ROS_INFO("Self mask includes link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].body->computeVolume());
- return true;
+ return true;
}
+bool robot_self_filter::SelfMask::configure(void)
+{
+ return configure(rm_.getSelfSeeLinks(), rm_.getSelfSeeScale(), rm_.getSelfSeePadding());
+}
+
void robot_self_filter::SelfMask::getLinkFrames(std::vector<std::string> &frames) const
{
for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
frames.push_back(bodies_[i].name);
}
-void robot_self_filter::SelfMask::mask(const robot_msgs::PointCloud& data_in, std::vector<bool> &mask)
+void robot_self_filter::SelfMask::mask(const robot_msgs::PointCloud& data_in, std::vector<int> &mask)
{
mask.resize(data_in.pts.size());
if (bodies_.empty())
std::fill(mask.begin(), mask.end(), true);
else
- maskSimple(data_in, mask);
+ maskAux(data_in, mask);
}
void robot_self_filter::SelfMask::computeBoundingSpheres(void)
@@ -123,7 +124,7 @@
computeBoundingSpheres();
}
-void robot_self_filter::SelfMask::maskSimple(const robot_msgs::PointCloud& data_in, std::vector<bool> &mask)
+void robot_self_filter::SelfMask::maskAux(const robot_msgs::PointCloud& data_in, std::vector<int> &mask)
{
const unsigned int bs = bodies_.size();
const unsigned int np = data_in.pts.size();
@@ -136,25 +137,27 @@
btScalar radiusSquared = bound.radius * bound.radius;
// we now decide which points we keep
-#pragma omp parallel for schedule(dynamic)
+#pragma omp parallel for schedule(dynamic)
for (int i = 0 ; i < (int)np ; ++i)
{
btVector3 pt = btVector3(btScalar(data_in.pts[i].x), btScalar(data_in.pts[i].y), btScalar(data_in.pts[i].z));
- bool out = true;
+ int out = 1;
if (bound.center.distance2(pt) < radiusSquared)
for (unsigned int j = 0 ; out && j < bs ; ++j)
- out = !bodies_[j].body->containsPoint(pt);
+ if (bodies_[j].body->containsPoint(pt))
+ out = 0;
mask[i] = out;
}
}
-bool robot_self_filter::SelfMask::getMask(double x, double y, double z) const
+int robot_self_filter::SelfMask::getMask(double x, double y, double z) const
{
btVector3 pt = btVector3(btScalar(x), btScalar(y), btScalar(z));
const unsigned int bs = bodies_.size();
- bool out = true;
+ int out = 1;
for (unsigned int j = 0 ; out && j < bs ; ++j)
- out = !bodies_[j].body->containsPoint(pt);
+ if (bodies_[j].body->containsPoint(pt))
+ out = 0;
return out;
}
Modified: pkg/trunk/util/robot_self_filter/src/test_filter.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/test_filter.cpp 2009-07-06 21:31:39 UTC (rev 18344)
+++ pkg/trunk/util/robot_self_filter/src/test_filter.cpp 2009-07-06 21:32:42 UTC (rev 18345)
@@ -42,11 +42,18 @@
{
public:
- TestSelfFilter(void) : sf_(tf_)
+ TestSelfFilter(void) : rm_("robot_description")
{
id_ = 1;
vmPub_ = nodeHandle_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
+ std::vector<std::string> links = rm_.getCollisionCheckLinks();
+ sf_ = new robot_self_filter::SelfMask(tf_, links, 1.0, 0.0);
}
+
+ ~TestSelfFilter(void)
+ {
+ delete sf_;
+ }
void sendPoint(double x, double y, double z)
{
@@ -65,7 +72,7 @@
mk.pose.position.z = z;
mk.pose.orientation.w = 1.0;
- mk.scale.x = mk.scale.y = mk.scale.z = 0.02;
+ mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
mk.color.a = 1.0;
mk.color.r = 1.0;
@@ -84,7 +91,7 @@
in.chan.resize(1);
in.chan[0].name = "stamps";
- const unsigned int N = 100000;
+ const unsigned int N = 500000;
in.pts.resize(N);
in.chan[0].vals.resize(N);
for (unsigned int i = 0 ; i < N ; ++i)
@@ -102,18 +109,26 @@
}
ros::WallTime tm = ros::WallTime::now();
- std::vector<bool> mask;
- sf_.mask(in, mask);
+ std::vector<int> mask;
+ sf_->mask(in, mask);
printf("%f points per second\n", (double)N/(ros::WallTime::now() - tm).toSec());
+ sf_->assumeFrame(in.header);
+ int k = 0;
for (unsigned int i = 0 ; i < mask.size() ; ++i)
{
- if (!mask[i])
- sendPoint(in.pts[i].x, in.pts[i].y, in.pts[i].z);
+ bool v = sf_->getMask(in.pts[i].x, in.pts[i].y, in.pts[i].z);
+ if (v != mask[i])
+ ROS_ERROR("Mask does not match");
+ if (mask[i]) continue;
+ sendPoint(in.pts[i].x, in.pts[i].y, in.pts[i].z);
+ k++;
}
+ printf("%d points inside\n", k);
ros::spin();
}
+
protected:
double uniform(double magnitude)
@@ -121,11 +136,12 @@
return (2.0 * drand48() - 1.0) * magnitude;
}
- tf::TransformListener tf_;
- robot_self_filter::SelfMask sf_;
- ros::Publisher vmPub_;
- ros::NodeHandle nodeHandle_;
- int id_;
+ tf::TransformListener tf_;
+ robot_self_filter::SelfMask *sf_;
+ planning_environment::RobotModels rm_;
+ ros::Publisher vmPub_;
+ ros::NodeHandle nodeHandle_;
+ int id_;
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
