Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17482] pkg/trunk
|
From: <is...@us...> - 2009-06-23 02:51:37
|
Revision: 17482
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17482&view=rev
Author: isucan
Date: 2009-06-23 02:51:34 +0000 (Tue, 23 Jun 2009)
Log Message:
-----------
separated out the geometric shapes from planning_models and collision_space; added a new collision map node that can deal with occlusions
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/CMakeLists.txt
pkg/trunk/mapping/collision_map/manifest.xml
pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/planning_environment/manifest.xml
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_models/CMakeLists.txt
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/manifest.xml
pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h
pkg/trunk/util/new_robot_self_filter/manifest.xml
pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
pkg/trunk/util/new_robot_self_filter/src/self_mask.cpp
pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp
pkg/trunk/world_models/collision_space/CMakeLists.txt
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/manifest.xml
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
pkg/trunk/world_models/test_collision_space/src/test_cs_point_inclusion.cpp
Added Paths:
-----------
pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
pkg/trunk/util/geometric_shapes/
pkg/trunk/util/geometric_shapes/.build_version
pkg/trunk/util/geometric_shapes/.rosgcov_files
pkg/trunk/util/geometric_shapes/CMakeLists.txt
pkg/trunk/util/geometric_shapes/Makefile
pkg/trunk/util/geometric_shapes/include/
pkg/trunk/util/geometric_shapes/include/geometric_shapes/
pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h
pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h
pkg/trunk/util/geometric_shapes/mainpage.dox
pkg/trunk/util/geometric_shapes/manifest.xml
pkg/trunk/util/geometric_shapes/src/
pkg/trunk/util/geometric_shapes/src/bodies.cpp
pkg/trunk/util/geometric_shapes/src/load_mesh.cpp
pkg/trunk/util/geometric_shapes/test/
pkg/trunk/util/geometric_shapes/test/test_point_inclusion.cpp
Removed Paths:
-------------
pkg/trunk/motion_planning/planning_models/include/planning_models/shapes.h
pkg/trunk/motion_planning/planning_models/src/load_mesh.cpp
pkg/trunk/world_models/collision_space/include/collision_space/bodies.h
pkg/trunk/world_models/collision_space/src/bodies.cpp
pkg/trunk/world_models/collision_space/test/
Modified: pkg/trunk/mapping/collision_map/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-06-23 02:51:34 UTC (rev 17482)
@@ -79,3 +79,5 @@
rospack_add_executable(collision_map_node src/collision_map.cpp)
rospack_add_executable(collision_map_buffer_node src/collision_map_buffer.cpp)
+rospack_add_executable(collision_map_occ_node src/collision_map_occ.cpp)
+rospack_add_openmp_flags(collision_map_occ_node)
Modified: pkg/trunk/mapping/collision_map/manifest.xml
===================================================================
--- pkg/trunk/mapping/collision_map/manifest.xml 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/mapping/collision_map/manifest.xml 2009-06-23 02:51:34 UTC (rev 17482)
@@ -15,4 +15,5 @@
<depend package="robot_srvs" />
<depend package="visualization_msgs" />
<depend package="point_cloud_mapping" />
+ <depend package="new_robot_self_filter" />
</package>
Added: pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp (rev 0)
+++ pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-06-23 02:51:34 UTC (rev 17482)
@@ -0,0 +1,594 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#include <ros/ros.h>
+#include <robot_msgs/PointCloud.h>
+#include <robot_msgs/CollisionMap.h>
+#include <tf/transform_listener.h>
+#include <tf/message_notifier.h>
+#include <robot_self_filter/self_mask.h>
+#include <boost/thread/mutex.hpp>
+#include <boost/bind.hpp>
+#include <vector>
+#include <algorithm>
+#include <set>
+#include <iterator>
+
+class CollisionMapperOcc
+{
+public:
+
+ CollisionMapperOcc(void) : sf_(tf_),
+ mn_(tf_, boost::bind(&CollisionMapperOcc::cloudCallback, this, _1), "cloud_in", "", 1)
+ {
+ // a frame that does not move with the robot
+ nh_.param<std::string>("~fixed_frame", fixedFrame_, "odom");
+
+ // a frame that moves with the robot
+ nh_.param<std::string>("~robot_frame", robotFrame_, "base_link");
+
+ // bounds of collision map in robot frame
+ nh_.param<double>("~dimension_x", bi_.dimensionX, 1.0);
+ nh_.param<double>("~dimension_y", bi_.dimensionY, 1.5);
+ nh_.param<double>("~dimension_z", bi_.dimensionZ, 2.0);
+
+ // origin of collision map in the robot frame
+ nh_.param<double>("~origin_x", bi_.originX, 1.1);
+ nh_.param<double>("~origin_y", bi_.originY, 0.0);
+ nh_.param<double>("~origin_z", bi_.originZ, 0.0);
+
+ // sensor origin in robot frame
+ nh_.param<double>("~sensor_x", bi_.sensorX, 0.05);
+ nh_.param<double>("~sensor_y", bi_.sensorY, 0.0);
+ nh_.param<double>("~sensor_z", bi_.sensorZ, 1.0);
+
+ // resolution
+ nh_.param<double>("~resolution", bi_.resolution, 0.015);
+
+ ROS_INFO("Maintaining occlusion map in frame '%s', with origin at (%f, %f, %f) and dimension (%f, %f, %f), resolution of %f; sensor is at (%f, %f, %f), fixed fame is '%s'.",
+ robotFrame_.c_str(), bi_.dimensionX, bi_.dimensionY, bi_.dimensionZ, bi_.originX, bi_.originY, bi_.originZ, bi_.resolution, bi_.sensorX, bi_.sensorY, bi_.sensorZ, fixedFrame_.c_str());
+
+ // compute some useful values
+ bi_.sx = (int)(0.5 + (bi_.sensorX - bi_.originX) / bi_.resolution);
+ bi_.sy = (int)(0.5 + (bi_.sensorY - bi_.originY) / bi_.resolution);
+ bi_.sz = (int)(0.5 + (bi_.sensorZ - bi_.originZ) / bi_.resolution);
+
+ bi_.minX = (int)(0.5 + (-bi_.dimensionX - bi_.originX) / bi_.resolution);
+ bi_.maxX = (int)(0.5 + (bi_.dimensionX - bi_.originX) / bi_.resolution);
+
+ bi_.minY = (int)(0.5 + (-bi_.dimensionY - bi_.originY) / bi_.resolution);
+ bi_.maxY = (int)(0.5 + (bi_.dimensionY - bi_.originY) / bi_.resolution);
+
+ bi_.minZ = (int)(0.5 + (-bi_.dimensionZ - bi_.originZ) / bi_.resolution);
+ bi_.maxZ = (int)(0.5 + (bi_.dimensionZ - bi_.originZ) / bi_.resolution);
+
+ bi_.real_minX = -bi_.dimensionX + bi_.originX;
+ bi_.real_maxX = bi_.dimensionX + bi_.originX;
+ bi_.real_minY = -bi_.dimensionY + bi_.originY;
+ bi_.real_maxY = bi_.dimensionY + bi_.originY;
+ bi_.real_minZ = -bi_.dimensionZ + bi_.originZ;
+ bi_.real_maxZ = bi_.dimensionZ + bi_.originZ;
+
+
+ // configure the self mask and the message notifier
+ sf_.configure();
+ std::vector<std::string> frames;
+ sf_.getLinkFrames(frames);
+ if (std::find(frames.begin(), frames.end(), robotFrame_) == frames.end())
+ frames.push_back(robotFrame_);
+ mn_.setTargetFrame(frames);
+
+ // advertise our topic
+ cmapPublisher_ = nh_.advertise<robot_msgs::CollisionMap>("collision_map_occ", 1);
+ }
+
+private:
+
+ struct CollisionPoint
+ {
+ int x, y, z;
+ };
+
+ struct CollisionPointOrder
+ {
+ bool operator()(const CollisionPoint &a, const CollisionPoint &b)
+ {
+ if (a.x < b.x)
+ return true;
+ if (a.x > b.x)
+ return false;
+ if (a.y < b.y)
+ return true;
+ if (a.y > b.y)
+ return false;
+ return a.z < b.z;
+ }
+ };
+
+ struct BoxInfo
+ {
+ double dimensionX, dimensionY, dimensionZ;
+ double originX, originY, originZ;
+ double sensorX, sensorY, sensorZ;
+ double resolution;
+ int sx, sy, sz;
+ int minX, minY, minZ;
+ int maxX, maxY, maxZ;
+ double real_minX, real_minY, real_minZ;
+ double real_maxX, real_maxY, real_maxZ;
+ };
+
+ typedef std::set<CollisionPoint, CollisionPointOrder> CMap;
+
+ void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
+ {
+ std::vector<bool> mask;
+ robot_msgs::PointCloud out;
+ ros::WallTime tm = ros::WallTime::now();
+
+#pragma omp parallel sections
+ {
+
+#pragma omp section
+ {
+ // transform the pointcloud to the robot frame
+ // since we need the points in this frame (around the robot)
+ // to compute the collision map
+ tf_.transformPointCloud(robotFrame_, *cloud, out);
+ }
+
+#pragma omp section
+ {
+ // separate the received points into ones on the robot and ones that are obstacles
+ // the frame of the cloud does not matter here
+ sf_.mask(*cloud, mask);
+ }
+ }
+
+ CMap obstacles;
+ CMap self;
+
+ // compute collision maps for the points on the robot and points that define obstacles
+#pragma omp parallel sections
+ {
+
+#pragma omp section
+ {
+ constructCollisionMap(out, mask, true, obstacles);
+ }
+#pragma omp section
+ {
+ constructCollisionMap(out, mask, false, self);
+ }
+ }
+
+ // try to transform the previous map (if it exists) to the new frame
+ if (!currentMap_.empty())
+ if (!transformMap(currentMap_, header_, out.header))
+ currentMap_.clear();
+ header_ = out.header;
+
+ if (currentMap_.empty())
+ {
+ // if we have no previous information, the map is simply what we see + we assume space hidden by self is obstructed
+ CMap occ_self;
+ findSelfOcclusion(self, occ_self);
+ std::set_union(obstacles.begin(), obstacles.end(),
+ occ_self.begin(), occ_self.end(),
+ std::inserter(currentMap_, currentMap_.begin()),
+ CollisionPointOrder());
+ }
+ else
+ {
+ CMap occ_self;
+ CMap diff;
+
+#pragma omp parallel sections
+ {
+#pragma omp section
+ {
+ // find the set of points under the parts seen by the sensor
+ findSelfOcclusion(self, occ_self);
+ }
+#pragma omp section
+ {
+ // find the new obstacles that could be occluding information
+ std::set_difference(obstacles.begin(), obstacles.end(),
+ currentMap_.begin(), currentMap_.end(),
+ std::inserter(diff, diff.begin()),
+ CollisionPointOrder());
+ }
+ }
+
+ // this is the set of points that could be occluding information
+ CMap occ;
+ std::set_union(diff.begin(), diff.end(),
+ occ_self.begin(), occ_self.end(),
+ std::inserter(occ, occ.begin()),
+ CollisionPointOrder());
+
+ // find the points in the previous map that are now occluded
+ CMap keep;
+ findOcclusionsInMap(currentMap_, occ, keep);
+
+ // the new map is the new set of obstacles + occluded information
+ currentMap_.clear();
+ std::set_union(obstacles.begin(), obstacles.end(),
+ keep.begin(), keep.end(),
+ std::inserter(currentMap_, currentMap_.begin()),
+ CollisionPointOrder());
+ }
+
+ double sec = (ros::WallTime::now() - tm).toSec();
+
+ ROS_INFO("Updated collision map with %d points at %f Hz", currentMap_.size(), 1.0/sec);
+
+ publishCollisionMap(currentMap_);
+ }
+
+ bool transformMap(CMap &map, const roslib::Header &from, const roslib::Header &to)
+ {
+ return true;
+
+ if (tf_.canTransform(to.frame_id, to.stamp, from.frame_id, from.stamp, fixedFrame_))
+ {
+ tf::Stamped<btTransform> transf;
+ tf_.lookupTransform(to.frame_id, to.stamp, from.frame_id, from.stamp, fixedFrame_, transf);
+
+ // copy data to temporary location
+ const int n = map.size();
+ std::vector<CollisionPoint> pts(n);
+ std::copy(map.begin(), map.end(), pts.begin());
+ map.clear();
+
+#pragma omp parallel for
+ for (int i = 0 ; i < n ; ++i)
+ {
+ btVector3 p(pts[i].x * bi_.resolution + bi_.originX, pts[i].y * bi_.resolution + bi_.originY, pts[i].z * bi_.resolution + bi_.originZ);
+ p = transf * p;
+ if (p.x() > bi_.real_minX && p.x() < bi_.real_maxX && p.y() > bi_.real_minY && p.y() < bi_.real_maxY && p.z() > bi_.real_minZ && p.z() < bi_.real_maxZ)
+ {
+ CollisionPoint c;
+ c.x = (int)(0.5 + (p.x() - bi_.originX) / bi_.resolution);
+ c.y = (int)(0.5 + (p.y() - bi_.originY) / bi_.resolution);
+ c.z = (int)(0.5 + (p.z() - bi_.originZ) / bi_.resolution);
+ map.insert(c);
+ }
+ }
+
+ return true;
+ }
+ else
+ {
+ ROS_WARN("Unable to transform previous collision map into new frame");
+ return false;
+ }
+ }
+
+ void findOcclusionsInMap(CMap &previous, const CMap &occ, CMap &keep)
+ {
+ // OpenMP need an int as the lookup variable, but for set,
+ // this is not possible, so we copy to a vector
+ const int n = occ.size();
+ std::vector<CollisionPoint> pts(n);
+ std::copy(occ.begin(), occ.end(), pts.begin());
+
+ // we are doing OpenMP parallelism, but we use a mutex to synchronize
+ boost::mutex lock;
+
+#pragma omp parallel for schedule(dynamic)
+ for (int i = 0 ; i < n ; ++i)
+ // run this function in a visitor pattern (it calls the callback at every cell it finds along the line)
+ bresenham_line_3D(bi_.sx, bi_.sy, bi_.sz, pts[i].x, pts[i].y, pts[i].z,
+ bi_.minX, bi_.minY, bi_.minZ, bi_.maxX, bi_.maxY, bi_.maxZ,
+ boost::bind(&CollisionMapperOcc::findOcclusionsInMapAux, this, &previous, &keep, &lock, _1, _2, _3));
+ }
+
+ bool findOcclusionsInMapAux(CMap *previous, CMap *keep, boost::mutex *lock, int x, int y, int z)
+ {
+ CollisionPoint p;
+ p.x = x;
+ p.y = y;
+ p.z = z;
+
+ // if we are occluding this point, remember it
+ if (previous->find(p) != previous->end())
+ {
+ lock->lock();
+ keep->insert(p);
+ lock->unlock();
+ // we can terminate
+ return true;
+ }
+
+ return true;
+ }
+
+ void findSelfOcclusion(const CMap &self, CMap &occ)
+ {
+ // tell the self filter the frame in which we call getMask()
+ sf_.assumeFrame(header_);
+
+ // we are doing OpenMP parallelism, but we use a mutex to synchronize
+ boost::mutex lock;
+
+ // OpenMP need an int as the lookup variable, but for set,
+ // this is not possible, so we copy to a vector
+ const int n = self.size();
+ std::vector<CollisionPoint> pts(n);
+ std::copy(self.begin(), self.end(), pts.begin());
+
+#pragma omp parallel for schedule(dynamic)
+ for (int i = 0 ; i < n ; ++i)
+ {
+ int state = 0;
+ // run this function in a visitor pattern (it calls the callback at every cell it finds along the line)
+ bresenham_line_3D(bi_.sx, bi_.sy, bi_.sz, pts[i].x, pts[i].y, pts[i].z,
+ bi_.minX, bi_.minY, bi_.minZ, bi_.maxX, bi_.maxY, bi_.maxZ,
+ boost::bind(&CollisionMapperOcc::findSelfOcclusionAux, this, &occ, &state, &lock, _1, _2, _3));
+ }
+ }
+
+ bool findSelfOcclusionAux(CMap *occ, int *state, boost::mutex *lock, int x, int y, int z)
+ {
+ // this is the point in the robotFrame_
+ bool out = sf_.getMask(x * bi_.resolution + bi_.originX, y * bi_.resolution + bi_.originY, z * bi_.resolution + bi_.originZ);
+
+ // if we are already inside the robot, we mark the fact we want to stop when we are outside
+ if (out == false)
+ {
+ *state = 8;
+ return false;
+ }
+ else
+ {
+ // if we are now outside, but have seen the inside, we have a point we need to add to the collision map
+ if (*state == 8)
+ {
+ CollisionPoint c;
+ c.x = x;
+ c.y = y;
+ c.z = z;
+ lock->lock();
+ occ->insert(c);
+ lock->unlock();
+ return true;
+ }
+ else
+ // if we have not seen the inside, but we are outside, it could be we are just above the obstacle,
+ // so we check the next cell along the line as well
+ if (*state == 0)
+ {
+ *state = 1;
+ return false;
+ }
+ else
+ // at this point, the ray is probably barely touching the padding of the arm, so it is safe to ignore
+ return true;
+ }
+ }
+
+ /** 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)
+ {
+ const unsigned int n = cloud.pts.size();
+ CollisionPoint c;
+
+ for (unsigned int i = 0 ; i < n ; ++i)
+ if (mask[i] == keep)
+ {
+ const robot_msgs::Point32 &p = cloud.pts[i];
+ if (p.x > bi_.real_minX && p.x < bi_.real_maxX && p.y > bi_.real_minY && p.y < bi_.real_maxY && p.z > bi_.real_minZ && p.z < bi_.real_maxZ)
+ {
+ c.x = (int)(0.5 + (p.x - bi_.originX) / bi_.resolution);
+ c.y = (int)(0.5 + (p.y - bi_.originY) / bi_.resolution);
+ c.z = (int)(0.5 + (p.z - bi_.originZ) / bi_.resolution);
+ map.insert(c);
+ }
+ }
+ }
+
+
+ /* Based on http://www.cit.gu.edu.au/~anthony/info/graphics/bresenham.procs */
+ /* Form the line (x1, y1, z1) -> (x2, y2, z2) and generate points on it starting at (x2, y2, z2)
+ * until it reaches a boundary of the box (minX, minY, minZ) -> (maxX, maxY, maxZ).
+ * If the callback returns true, the segment is stopped */
+ void bresenham_line_3D(int x1, int y1, int z1, int x2, int y2, int z2,
+ int minX, int minY, int minZ, int maxX, int maxY, int maxZ,
+ const boost::function<bool(int, int, int)> &callback) const
+ {
+ int i, dx, dy, dz, l, m, n, x_inc, y_inc, z_inc, err_1, err_2, dx2, dy2, dz2;
+ int pixel[3];
+
+ pixel[0] = x2;
+ pixel[1] = y2;
+ pixel[2] = z2;
+ dx = x2 - x1;
+ dy = y2 - y1;
+ dz = z2 - z1;
+
+ int c = -1;
+ if (dx != 0)
+ {
+ int cx = (maxX - x1) / dx;
+ if (cx <= 0)
+ cx = (minX - x1) / dx;
+ if (cx < c || c < 0)
+ c = cx;
+ }
+
+ if (dy != 0)
+ {
+ int cy = (maxY - y1) / dy;
+ if (cy <= 0)
+ cy = (minY - y1) / dy;
+ if (cy < c || c < 0)
+ c = cy;
+ }
+
+ if (dz != 0)
+ {
+ int cz = (maxZ - z1) / dz;
+ if (cz <= 0)
+ cz = (minZ - z1) / dz;
+ if (cz < c || c < 0)
+ c = cz;
+ }
+
+ if (c > 0)
+ {
+ dx *= c;
+ dy *= c;
+ dz *= c;
+ }
+
+ x_inc = (dx < 0) ? -1 : 1;
+ l = abs(dx);
+ y_inc = (dy < 0) ? -1 : 1;
+ m = abs(dy);
+ z_inc = (dz < 0) ? -1 : 1;
+ n = abs(dz);
+ dx2 = l << 1;
+ dy2 = m << 1;
+ dz2 = n << 1;
+
+ if ((l >= m) && (l >= n)) {
+ err_1 = dy2 - l;
+ err_2 = dz2 - l;
+ for (i = 0; i < l; i++) {
+ if (callback(pixel[0], pixel[1], pixel[2]))
+ return;
+ if (err_1 > 0) {
+ pixel[1] += y_inc;
+ err_1 -= dx2;
+ }
+ if (err_2 > 0) {
+ pixel[2] += z_inc;
+ err_2 -= dx2;
+ }
+ err_1 += dy2;
+ err_2 += dz2;
+ pixel[0] += x_inc;
+ }
+ } else if ((m >= l) && (m >= n)) {
+ err_1 = dx2 - m;
+ err_2 = dz2 - m;
+ for (i = 0; i < m; i++) {
+ if (callback(pixel[0], pixel[1], pixel[2]))
+ return;
+ if (err_1 > 0) {
+ pixel[0] += x_inc;
+ err_1 -= dy2;
+ }
+ if (err_2 > 0) {
+ pixel[2] += z_inc;
+ err_2 -= dy2;
+ }
+ err_1 += dx2;
+ err_2 += dz2;
+ pixel[1] += y_inc;
+ }
+ } else {
+ err_1 = dy2 - n;
+ err_2 = dx2 - n;
+ for (i = 0; i < n; i++) {
+ if (callback(pixel[0], pixel[1], pixel[2]))
+ return;
+ if (err_1 > 0) {
+ pixel[1] += y_inc;
+ err_1 -= dz2;
+ }
+ if (err_2 > 0) {
+ pixel[0] += x_inc;
+ err_2 -= dz2;
+ }
+ err_1 += dy2;
+ err_2 += dx2;
+ pixel[2] += z_inc;
+ }
+ }
+ callback(pixel[0], pixel[1], pixel[2]);
+ }
+
+ void publishCollisionMap(const CMap &map) const
+ {
+ robot_msgs::CollisionMap cmap;
+ cmap.header = header_;
+ const unsigned int ms = map.size();
+
+ for (CMap::const_iterator it = map.begin() ; it != map.end() ; ++it)
+ {
+ const CollisionPoint &cp = *it;
+ robot_msgs::OrientedBoundingBox box;
+ box.extents.x = box.extents.y = box.extents.z = bi_.resolution;
+ box.axis.x = box.axis.y = 0.0; box.axis.z = 1.0;
+ box.angle = 0.0;
+ box.center.x = cp.x * bi_.resolution + bi_.originX;
+ box.center.y = cp.y * bi_.resolution + bi_.originY;
+ box.center.z = cp.z * bi_.resolution + bi_.originZ;
+ cmap.boxes.push_back(box);
+ }
+ cmapPublisher_.publish(cmap);
+
+ ROS_DEBUG("Published collision map with %u boxes", ms);
+ }
+
+ tf::TransformListener tf_;
+ robot_self_filter::SelfMask sf_;
+ tf::MessageNotifier<robot_msgs::PointCloud> mn_;
+ ros::NodeHandle nh_;
+ ros::Publisher cmapPublisher_;
+ roslib::Header header_;
+
+ CMap currentMap_;
+ BoxInfo bi_;
+ std::string fixedFrame_;
+ std::string robotFrame_;
+
+};
+
+int main (int argc, char** argv)
+{
+ ros::init(argc, argv, "collision_map_occ");
+
+ CollisionMapperOcc cm;
+
+ ros::spin();
+
+ return 0;
+}
+
+
Modified: pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-06-23 02:51:34 UTC (rev 17482)
@@ -134,7 +134,7 @@
header.frame_id = link->name;
for (unsigned int i = 0 ; i < link->attachedBodies.size() ; ++i)
{
- planning_models::shapes::Box *box = dynamic_cast<planning_models::shapes::Box*>(link->attachedBodies[i]->shape);
+ shapes::Box *box = dynamic_cast<shapes::Box*>(link->attachedBodies[i]->shape);
if (box)
{
btVector3 &v = link->attachedBodies[i]->attachTrans.getOrigin();
Modified: pkg/trunk/motion_planning/planning_environment/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-06-23 02:51:34 UTC (rev 17482)
@@ -17,6 +17,7 @@
<depend package="robot_msgs" />
<depend package="mechanism_msgs" />
<depend package="motion_planning_msgs" />
+ <depend package="geometric_shapes" />
<depend package="planning_models" />
<depend package="collision_space" />
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-23 02:51:34 UTC (rev 17482)
@@ -219,7 +219,7 @@
link->attachedBodies[j]->attachTrans.setOrigin(btVector3(centerP.point.x, centerP.point.y, centerP.point.z));
// this is a HACK! we should have orientation
- planning_models::shapes::Box *box = new planning_models::shapes::Box();
+ shapes::Box *box = new shapes::Box();
box->size[0] = attachedObject->objects[i].max_bound.x - attachedObject->objects[i].min_bound.x;
box->size[1] = attachedObject->objects[i].max_bound.y - attachedObject->objects[i].min_bound.y;
box->size[2] = attachedObject->objects[i].max_bound.z - attachedObject->objects[i].min_bound.z;
Modified: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-06-23 02:51:34 UTC (rev 17482)
@@ -7,7 +7,6 @@
rospack_add_library(planning_models src/kinematic.cpp
src/kinematic_state_params.cpp
- src/load_mesh.cpp
src/output.cpp)
# Unit tests
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06-23 02:51:34 UTC (rev 17482)
@@ -37,7 +37,7 @@
#ifndef PLANNING_MODELS_KINEMATIC_ROBOT_MODEL_
#define PLANNING_MODELS_KINEMATIC_ROBOT_MODEL_
-#include "planning_models/shapes.h"
+#include <geometric_shapes/shapes.h>
#include "planning_models/output.h"
#include <urdf/URDF.h>
Deleted: pkg/trunk/motion_planning/planning_models/include/planning_models/shapes.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/shapes.h 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/shapes.h 2009-06-23 02:51:34 UTC (rev 17482)
@@ -1,177 +0,0 @@
-/*********************************************************************
-* 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.
-*********************************************************************/
-
-/** \author Ioan Sucan */
-
-#ifndef PLANNING_MODELS_SHAPES_
-#define PLANNING_MODELS_SHAPES_
-
-#include <cstdlib>
-
-namespace planning_models
-{
-
- namespace shapes
- {
-
- /** A basic definition of a shape. Shapes to be centered at origin */
- class Shape
- {
- public:
- Shape(void)
- {
- type = UNKNOWN;
- }
-
- virtual ~Shape(void)
- {
- }
-
- enum { UNKNOWN, SPHERE, CYLINDER, BOX, MESH }
- type;
-
- };
-
- /** Definition of a sphere */
- class Sphere : public Shape
- {
- public:
- Sphere(void) : Shape()
- {
- type = SPHERE;
- radius = 0.0;
- }
-
- Sphere(double r) : Shape()
- {
- type = SPHERE;
- radius = r;
- }
-
- double radius;
- };
-
- /** Definition of a cylinder */
- class Cylinder : public Shape
- {
- public:
- Cylinder(void) : Shape()
- {
- type = CYLINDER;
- length = radius = 0.0;
- }
-
- Cylinder(double r, double l) : Shape()
- {
- type = CYLINDER;
- length = l;
- radius = r;
- }
-
- double length, radius;
- };
-
- /** Definition of a box */
- class Box : public Shape
- {
- public:
- Box(void) : Shape()
- {
- type = BOX;
- size[0] = size[1] = size[2] = 0.0;
- }
-
- Box(double x, double y, double z) : Shape()
- {
- type = BOX;
- size[0] = x;
- size[1] = y;
- size[2] = z;
- }
-
- /** x, y, z */
- double size[3];
- };
-
- /** Definition of a mesh */
- class Mesh : public Shape
- {
- public:
- Mesh(void) : Shape()
- {
- type = MESH;
- vertexCount = 0;
- vertices = NULL;
- triangleCount = 0;
- triangles = NULL;
- normals = NULL;
- }
-
- Mesh(unsigned int vCount, unsigned int tCount) : Shape()
- {
- type = MESH;
- vertexCount = vCount;
- vertices = new double[vCount * 3];
- triangleCount = tCount;
- triangles = new unsigned int[tCount * 3];
- normals = new double[tCount * 3];
- }
-
- virtual ~Mesh(void)
- {
- if (vertices)
- delete[] vertices;
- if (triangles)
- delete[] triangles;
- if (normals)
- delete[] normals;
- }
-
- unsigned int vertexCount; // the number of available vertices
- double *vertices; // the position for each vertex
- // vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
-
- unsigned int triangleCount; // the number of triangles formed with the vertices
- unsigned int *triangles; // the vertex indices for each triangle
- // triangle k has vertices at index (3k, 3k+1, 3k+2) = (v1, v2, v3)
-
- double *normals; // the normal to each triangle
- // unit vector represented as (x,y,z)
- };
-
- }
-}
-
-#endif
-
Modified: pkg/trunk/motion_planning/planning_models/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_models/manifest.xml 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/motion_planning/planning_models/manifest.xml 2009-06-23 02:51:34 UTC (rev 17482)
@@ -9,6 +9,7 @@
<url>http://pr.willowgarage.com/wiki/RobotDescriptionFormat</url>
<depend package="wg_robot_description_parser"/>
+ <depend package="geometric_shapes"/>
<depend package="bullet"/>
<export>
Deleted: pkg/trunk/motion_planning/planning_models/src/load_mesh.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/load_mesh.cpp 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/motion_planning/planning_models/src/load_mesh.cpp 2009-06-23 02:51:34 UTC (rev 17482)
@@ -1,240 +0,0 @@
-/*********************************************************************
-* 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 <cstdio>
-#include <cmath>
-#include <LinearMath/btVector3.h>
-#include <algorithm>
-#include <vector>
-#include <set>
-#include "planning_models/shapes.h"
-
-// \author Ioan Sucan ; based on stl_to_mesh
-
-namespace planning_models
-{
-
- struct myVertex
- {
- btVector3 point;
- unsigned int index;
- };
-
- struct ltVertexValue
- {
- bool operator()(const myVertex &p1, const myVertex &p2) const
- {
- const btVector3 &v1 = p1.point;
- const btVector3 &v2 = p2.point;
- if (v1.getX() < v2.getX())
- return true;
- if (v1.getX() > v2.getX())
- return false;
- if (v1.getY() < v2.getY())
- return true;
- if (v1.getY() > v2.getY())
- return false;
- if (v1.getZ() < v2.getZ())
- return true;
- return false;
- }
- };
-
- struct ltVertexIndex
- {
- bool operator()(const myVertex &p1, const myVertex &p2) const
- {
- return p1.index < p2.index;
- }
- };
-
- shapes::Mesh* create_mesh_from_vertices(const std::vector<btVector3> &source)
- {
- if (source.size() < 3)
- return NULL;
-
- std::set<myVertex, ltVertexValue> vertices;
- std::vector<unsigned int> triangles;
-
- for (unsigned int i = 0 ; i < source.size() / 3 ; ++i)
- {
- // check if we have new vertices
- myVertex vt;
-
- vt.point = source[3 * i];
- std::set<myVertex, ltVertexValue>::iterator p1 = vertices.find(vt);
- if (p1 == vertices.end())
- {
- vt.index = vertices.size();
- vertices.insert(vt);
- }
- else
- vt.index = p1->index;
- triangles.push_back(vt.index);
-
- vt.point = source[3 * i + 1];
- std::set<myVertex, ltVertexValue>::iterator p2 = vertices.find(vt);
- if (p2 == vertices.end())
- {
- vt.index = vertices.size();
- vertices.insert(vt);
- }
- else
- vt.index = p2->index;
- triangles.push_back(vt.index);
-
- vt.point = source[3 * i + 2];
- std::set<myVertex, ltVertexValue>::iterator p3 = vertices.find(vt);
- if (p3 == vertices.end())
- {
- vt.index = vertices.size();
- vertices.insert(vt);
- }
- else
- vt.index = p3->index;
-
- triangles.push_back(vt.index);
- }
-
- // sort our vertices
- std::vector<myVertex> vt;
- vt.insert(vt.begin(), vertices.begin(), vertices.end());
- std::sort(vt.begin(), vt.end(), ltVertexIndex());
-
- // copy the data to a mesh structure
- unsigned int nt = triangles.size() / 3;
-
- shapes::Mesh *mesh = new shapes::Mesh(vt.size(), nt);
- for (unsigned int i = 0 ; i < vt.size() ; ++i)
- {
- mesh->vertices[3 * i ] = vt[i].point.getX();
- mesh->vertices[3 * i + 1] = vt[i].point.getY();
- mesh->vertices[3 * i + 2] = vt[i].point.getZ();
- }
-
- std::copy(triangles.begin(), triangles.end(), mesh->triangles);
-
- // compute normals
- for (unsigned int i = 0 ; i < nt ; ++i)
- {
- btVector3 s1 = vt[triangles[i * 3 ]].point - vt[triangles[i * 3 + 1]].point;
- btVector3 s2 = vt[triangles[i * 3 + 1]].point - vt[triangles[i * 3 + 2]].point;
- btVector3 normal = s1.cross(s2);
- normal.normalize();
- mesh->normals[3 * i ] = normal.getX();
- mesh->normals[3 * i + 1] = normal.getY();
- mesh->normals[3 * i + 2] = normal.getZ();
- }
-
- return mesh;
- }
-
- shapes::Mesh* create_mesh_from_binary_stl(const char *filename)
- {
-
- FILE* input = fopen(filename, "r");
- if (!input)
- return NULL;
-
- fseek(input, 0, SEEK_END);
- long fileSize = ftell(input);
- fseek(input, 0, SEEK_SET);
-
- char* buffer = new char[fileSize];
- size_t rd = fread(buffer, fileSize, 1, input);
-
- fclose(input);
-
- if (rd == 1)
- {
- char* pos = buffer;
- pos += 80; // skip the 80 byte header
-
- unsigned int numTriangles = *(unsigned int*)pos;
- pos += 4;
-
- // make sure we have read enough data
- if ((long)(50 * numTriangles + 84) <= fileSize)
- {
- std::vector<btVector3> vertices;
-
- for (unsigned int currentTriangle = 0 ; currentTriangle < numTriangles ; ++currentTriangle)
- {
- // skip the normal
- pos += 12;
-
- // read vertices
- btVector3 v1(0,0,0);
- btVector3 v2(0,0,0);
- btVector3 v3(0,0,0);
-
- v1.setX(*(float*)pos);
- pos += 4;
- v1.setY(*(float*)pos);
- pos += 4;
- v1.setZ(*(float*)pos);
- pos += 4;
-
- v2.setX(*(float*)pos);
- pos += 4;
- v2.setY(*(float*)pos);
- pos += 4;
- v2.setZ(*(float*)pos);
- pos += 4;
-
- v3.setX(*(float*)pos);
- pos += 4;
- v3.setY(*(float*)pos);
- pos += 4;
- v3.setZ(*(float*)pos);
- pos += 4;
-
- // skip attribute
- pos += 2;
-
- vertices.push_back(v1);
- vertices.push_back(v2);
- vertices.push_back(v3);
- }
-
- delete[] buffer;
-
- return create_mesh_from_vertices(vertices);
- }
- }
-
- return NULL;
- }
-
-}
Added: pkg/trunk/util/geometric_shapes/.build_version
===================================================================
--- pkg/trunk/util/geometric_shapes/.build_version (rev 0)
+++ pkg/trunk/util/geometric_shapes/.build_version 2009-06-23 02:51:34 UTC (rev 17482)
@@ -0,0 +1 @@
+:
Added: pkg/trunk/util/geometric_shapes/.rosgcov_files
===================================================================
--- pkg/trunk/util/geometric_shapes/.rosgcov_files (rev 0)
+++ pkg/trunk/util/geometric_shapes/.rosgcov_files 2009-06-23 02:51:34 UTC (rev 17482)
@@ -0,0 +1,2 @@
+/u/isucan/work/ros-pkg/util/geometric_shapes src/load_mesh.cpp
+/u/isucan/work/ros-pkg/util/geometric_shapes src/bodies.cpp
Added: pkg/trunk/util/geometric_shapes/CMakeLists.txt
===================================================================
--- pkg/trunk/util/geometric_shapes/CMakeLists.txt (rev 0)
+++ pkg/trunk/util/geometric_shapes/CMakeLists.txt 2009-06-23 02:51:34 UTC (rev 17482)
@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rospack(geometric_shapes)
+
+set(ROS_BUILD_TYPE Release)
+
+
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+
+rospack_add_library(${PROJECT_NAME} src/load_mesh.cpp
+ src/bodies.cpp)
+
+
+# Unit tests
+rospack_add_gtest(test_point_inclusion test/test_point_inclusion.cpp)
+target_link_libraries(test_point_inclusion ${PROJECT_NAME})
Added: pkg/trunk/util/geometric_shapes/Makefile
===================================================================
--- pkg/trunk/util/geometric_shapes/Makefile (rev 0)
+++ pkg/trunk/util/geometric_shapes/Makefile 2009-06-23 02:51:34 UTC (rev 17482)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h
===================================================================
--- pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h (rev 0)
+++ pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h 2009-06-23 02:51:34 UTC (rev 17482)
@@ -0,0 +1,316 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#ifndef GEOMETRIC_SHAPES_POINT_INCLUSION_
+#define GEOMETRIC_SHAPES_POINT_INCLUSION_
+
+#include "geometric_shapes/shapes.h"
+#include <LinearMath/btTransform.h>
+
+/**
+ This set of classes allows quickly detecting whether a given point
+ is inside an object or not. Only basic (simple) types of objects
+ are supported: spheres, cylinders, boxes. This capability is useful
+ when removing points from inside the robot (when the robot sees its
+ arms, for example).
+*/
+
+
+namespace bodies
+{
+
+ struct BoundingSphere
+ {
+ btVector3 center;
+ double radius;
+ };
+
+ /** A body is a shape + its pose. Point inclusion can be
+ tested, volumes and bounding spheres can be computed.*/
+ class Body
+ {
+ public:
+
+ Body(void)
+ {
+ m_scale = 1.0;
+ m_padding = 0.0;
+ m_pose.setIdentity();
+ }
+
+ virtual ~Body(void)
+ {
+ }
+
+ /** If the dimension of the body should be scaled, this
+ method sets the scale. Default is 1.0 */
+ void setScale(double scale)
+ {
+ m_scale = scale;
+ updateInternalData();
+ }
+
+ /** Retrieve the current scale */
+ double getScale(void) const
+ {
+ return m_scale;
+ }
+
+ /** If constant padding should be added to the body, this
+ method sets the padding. Default is 0.0 */
+ void setPadding(double padd)
+ {
+ m_padding = padd;
+ updateInternalData();
+ }
+
+ /** Retrieve the current padding */
+ double getPadding(void) const
+ {
+ return m_padding;
+ }
+
+ /** Set the pose of the body. Default is identity */
+ void setPose(const btTransform &pose)
+ {
+ m_pose = pose;
+ updateInternalData();
+ }
+
+ /** Retrieve the pose of the body */
+ const btTransform& getPose(void) const
+ {
+ return m_pose;
+ }
+
+ /** Set the dimensions of the body (from corresponding shape) */
+ void setDimensions(const shapes::Shape *shape)
+ {
+ useDimensions(shape);
+ updateInternalData();
+ }
+
+ /** Check is a point is inside the body */
+ bool containsPoint(double x, double y, double z) const
+ {
+ return containsPoint(btVector3(btScalar(x), btScalar(y), btScalar(z)));
+ }
+
+ /** Check is a point is inside the body */
+ virtual bool containsPoint(const btVector3 &p) const = 0;
+
+ /** Compute the volume of the body. This method includes
+ changes induced by scaling and padding */
+ virtual double computeVolume(void) const = 0;
+
+ /** Compute the bounding radius for the body, in its current
+ pose. Scaling and padding are accounted for. */
+ virtual void computeBoundingSphere(BoundingSphere &sphere) const = 0;
+
+ protected:
+
+ virtual void updateInternalData(void) = 0;
+ virtual void useDimensions(const shapes::Shape *shape) = 0;
+
+ btTransform m_pose;
+ double m_scale;
+ double m_padding;
+ };
+
+ class Sphere : public Body
+ {
+ public:
+ Sphere(void) : Body()
+ {
+ m_radius = 0.0;
+ }
+
+ Sphere(const shapes::Shape *shape) : Body()
+ {
+ setDimensions(shape);
+ }
+
+ virtual ~Sphere(void)
+ {
+ }
+
+ virtual bool containsPoint(const btVector3 &p) const;
+ virtual double computeVolume(void) const;
+ virtual void computeBoundingSphere(BoundingSphere &sphere) const;
+
+ protected:
+
+ virtual void useDimensions(const shapes::Shape *shape);
+ virtual void updateInternalData(void);
+
+ btVector3 m_center;
+ double m_radius;
+ double m_radiusU;
+ double m_radius2;
+ };
+
+ class Cylinder : public Body
+ {
+ public:
+ Cylinder(void) : Body()
+ {
+ m_length = m_radius = 0.0;
+ }
+
+ Cylinder(const shapes::Shape *shape) : Body()
+ {
+ setDimensions(shape);
+ }
+
+ virtual ~Cylinder(void)
+ {
+ }
+
+ virtual bool containsPoint(const btVector3 &p) const;
+ virtual double computeVolume(void) const;
+ virtual void computeBoundingSphere(BoundingSphere &sphere) const;
+
+ protected:
+
+ virtual void useDimensions(const shapes::Shape *shape);
+ virtual void updateInternalData(void);
+
+ btVector3 m_center;
+ btVector3 m_normalH;
+ btVector3 m_normalB1;
+ btVector3 m_normalB2;
+
+ double m_length;
+ double m_length2;
+ double m_radius;
+ double m_radiusU;
+ double m_radiusB;
+ double m_radius2;
+ };
+
+
+ class Box : public Body
+ {
+ public:
+ Box(void) : Body()
+ {
+ m_length = m_width = m_height = 0.0;
+ }
+
+ Box(const shapes::Shape *shape) : Body()
+ {
+ setDimensions(shape);
+ }
+
+ virtual ~Box(void)
+ {
+ }
+
+ virtual bool containsPoint(const btVector3 &p) const;
+ virtual double computeVolume(void) const;
+ virtual void computeBoundingSphere(BoundingSphere &sphere) const;
+
+ protected:
+
+ virtual void useDimensions(const shapes::Shape *shape); // (x, y, z) = (length, width, height)
+ virtual void updateInternalData(void);
+
+ btVector3 m_center;
+ btVector3 m_normalL;
+ btVector3 m_normalW;
+ btVector3 m_normalH;
+
+ double m_length;
+ double m_width;
+ double m_height;
+ double m_length2;
+ double m_width2;
+ double m_height2;
+ double m_radiusB;
+ };
+
+
+ class ConvexMesh : public Body
+ {
+ public:
+
+ ConvexMesh(void) : Body()
+ {
+ m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
+ }
+
+ ConvexMesh(const shapes::Shape *shape) : Body()
+ {
+ m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
+ setDimensions(shape);
+ }
+
+ virtual ~ConvexMesh(void)
+ {
+ }
+
+ virtual bool containsPoint(const btVector3 &p) const;
+ virtual double computeVolume(void) const;
+ virtual void computeBoundingSphere(BoundingSphere &sphere) const;
+
+ protected:
+
+ virtual void useDimensions(const shapes::Shape *shape);
+ virtual void updateInternalData(void);
+
+ unsigned int countVerticesBehindPlane(const btVector4& planeNormal) const;
+ bool isPointInsidePlanes(const btVector3& point) const;
+
+ std::vector<btVector4> m_planes;
+ std::vector<btVector3> m_vertices;
+ std::vector<unsigned int> m_triangles;
+ btTransform m_iPose;
+
+ btVector3 m_center;
+ btVector3 m_meshCenter;
+ double m_radiusB;
+ };
+
+
+ /** Create a body from a given shape */
+ Body* createBodyFromShape(const shapes::Shape *shape);
+
+ /** Compute a bounding sphere to enclose a set of bounding spheres */
+ void mergeBoundingSpheres(const std::vector<BoundingSphere> &spheres, BoundingSphere &mergedSphere);
+
+}
+
+#endif
Added: pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h
===================================================================
--- pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h (rev 0)
+++ pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h 2009-06-23 02:51:34 UTC (rev 17482)
@@ -0,0 +1,176 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#ifndef GEOMETRIC_SHAPES_SHAPES_
+#define GEOMETRIC_SHAPES_SHAPES_
+
+#include <cstdlib>
+#include <vector>
+#include <LinearMath/btVector3.h>
+
+namespace shapes
+{
+
+ /** A basic definition of a shape. Shapes to be centered at origin */
+ class Shape
+ {
+ public:
+ Shape(void)
+ {
+ type = UNKNOWN;
+ }
+
+ virtual ~Shape(void)
+ {
+ }
+
+ enum { UNKNOWN, SPHERE, CYLINDER, BOX, MESH }
+ type;
+
+ };
+
+ /** Definition of a sphere */
+ class Sphere : public Shape
+ {
+ public:
+ Sphere(void) : Shape()
+ {
+ type = SPHERE;
+ radius = 0.0;
+ }
+
+ Sphere(double r) : Shape()
+ {
+ type = SPHERE;
+ radius = r;
+ }
+
+ double radius;
+ };
+
+ /** Definition of a cylinder */
+ class Cylinder : public Shape
+ {
+ public:
+ Cylinder(void) : Shape()
+ {
+ type = CYLINDER;
+ length = radius = 0.0;
+ }
+
+ Cylinder(double r, double l) : Shape()
+ {
+ type = CYLINDER;
+ length = l;
+ radius = r;
+ }
+
+ double length, radius;
+ };
+
+ /** Definition of a box */
+ class Box : public Shape
+ {
+ public:
+ Box(void) : Shape()
+ {
+ type = BOX;
+ size[0] = size[1] = size[2] = 0.0;
+ }
+
+ Box(double x, double y, double z) : Shape()
+ {
+ type = BOX;
+ size[0] = x;
+ size[1] = y;
+ size[2] = z;
+ }
+
+ /** x, y, z */
+ double size[3];
+ };
+
+ /** Definition of a mesh */
+ class Mesh : public Shape
+ {
+ public:
+ Mesh(void) : Shape()
+ {
+ type = MESH;
+ vertexCount = 0;
+ vertices = NULL;
+ triangleCount = 0;
+ triangles = NULL;
+ normals = NULL;
+ }
+
+ Mesh(unsigned int vCount, unsigned int tCount) : Shape()
+ {
+ type = MESH;
+ vertexCount = vCount;
+ vertices = new double[vCount * 3];
+ triangleCount = tCount;
+ triangles = new unsigned int[tCount * 3];
+ normals = new double[tCount * 3];
+ }
+
+ virtual ~Mesh(void)
+ {
+ if (vertices)
+ delete[] vertices;
+ if (triangles)
+ delete[] triangles;
+ if (normals)
+ delete[] normals;
+ }
+
+ unsigned int vertexCount; // the number of available vertices
+ double *vertices; // the position for each vertex
+ // vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
+
+ unsigned int triangleCount; // the number of triangles formed with the vertices
+ unsigned int *triangles; // the vertex indices for each triangle
+ // triangle k has vertices at index (3k, 3k+1, 3k+2) = (v1, v2, v3)
+
+ double *normals; // the normal to each triangle
+ // unit vector represented as (x,y,z)
+ };
+
+ Mesh* create_mesh_from_vertices(const std::vector<btVector3> &source);
+ Mesh* create_mesh_from_binary_stl(const char *filename);
+}
+
+#endif
Added: pkg/trunk/util/geometric_shapes/mainpage.dox
===================================================================
--- pkg/trunk/util/geometric_shapes/mainpage.dox (rev 0)
+++ pkg/trunk/util/geometric_shapes/mainpage.dox 2009-06-23 02:51:34 UTC (rev 17482)
@@ -0,0 +1,119 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b geometric_shapes is a package containing the definition of simple shapes such as spheres, boxes, cylinders and meshes. Routines like tests for point inclusion or volume computation are also available.
+
+<!--
+In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+\section rosapi ROS API
+
+<!--
+Names are very important in ROS because they can be remapped on the
+command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
+APPEAR IN THE CODE. You should list names of every topic, service and
+parameter used in your code. There is a template below that you can
+use to document each node separately.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+-->
+
+<!-- START: copy from here to 'END' for each node
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+END: copy for each node -->
+
+
+<!-- START: Uncomment if you have any command-line tools
+
+\section commandline Command-line tools
+
+This section is a catch-all for any additional tools that your package
+provides or uses that may be of use to the reader. For example:
+
+- tools/scripts (e.g. rospack, roscd)
+- roslaunch .launch files
+- xmlparam files
+
+\subsection script_name script_name
+
+Description of what this script/file does.
+
+\subsubsection Usage
+\verbatim
+$ ./script_name [args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ ./script_name foo bar
+\endverbatim
+
+END: Command-Line Tools Section -->
+
+*/
\ No newline at end of file
Added: pkg/trunk/util/geometric_shapes/manifest.xml
===================================================================
--- pkg/trunk/util/geometric_shapes/manifest.xml (rev 0)
+++ pkg/trunk/util/geometric_shapes/manifest.xml 2009-06-23 02:51:34 UTC (rev 17482)
@@ -0,0 +1,20 @@
+<package>
+ <description brief="geometric_shapes">
+
+ geometric_shapes
+
+ </description>
+ <author>Ioan Sucan</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/geometric_shapes</url>
+
+ <depend package="bullet" />
+
+ <export>
+ <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lgeometric_shapes"/>
+ </export>
+
+</package>
+
+
Added: pkg/trunk/util/geometric_shapes/src/bodies.cpp
===================================================================
--- pkg/trunk/util/geometric_shapes/src/bodies.cpp (rev 0)
+++ pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-06-23 02:51:34 UTC (rev 17482)
@@ -0,0 +1,380 @@
+/*********************************************************************
+* 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 s...
[truncated message content] |
