Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21711] pkg/trunk
|
From: <is...@us...> - 2009-08-12 21:41:32
|
Revision: 21711
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21711&view=rev
Author: isucan
Date: 2009-08-12 21:41:23 +0000 (Wed, 12 Aug 2009)
Log Message:
-----------
moved files around, so we can improve the dependency tree
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/CMakeLists.txt
pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
pkg/trunk/motion_planning/ompl/CMakeLists.txt
pkg/trunk/motion_planning/ompl/code/ompl/base/Planner.h
pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h
pkg/trunk/motion_planning/ompl/code/ompl/base/src/Planner.cpp
pkg/trunk/motion_planning/ompl/code/ompl/base/src/SpaceInformation.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/src/KPIECE1.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/rrt/src/RRT.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/est/src/EST.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/GAIK.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/IKPlanner.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/src/GAIK.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/kpiece/src/KPIECE1.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/kpiece/src/LBKPIECE1.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/rrt/pRRT.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/rrt/src/LazyRRT.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/rrt/src/RRT.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/rrt/src/pRRT.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/sbl/pSBL.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/sbl/src/SBL.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/sbl/src/pSBL.cpp
pkg/trunk/motion_planning/ompl/code/tests/compile/compile.cpp
pkg/trunk/motion_planning/ompl/code/tests/dynamic/2dmap/2dmap.cpp
pkg/trunk/motion_planning/ompl/code/tests/kinematic/2dmap/2dmap.cpp
pkg/trunk/motion_planning/ompl/manifest.xml
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/robot_models.h
pkg/trunk/motion_planning/planning_environment/src/models/robot_models.cpp
pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
pkg/trunk/sandbox/3dnav_pr2/params/collision/collision_checks_both_arms.yaml
pkg/trunk/sandbox/3dnav_pr2/params/collision/collision_checks_left_arm.yaml
pkg/trunk/sandbox/3dnav_pr2/params/collision/collision_checks_right_arm.yaml
pkg/trunk/sandbox/test_arm_vision_calibration/src/test_arm_vision_calibration.cpp
pkg/trunk/stacks/laser_drivers/point_cloud_assembler/mainpage.dox
pkg/trunk/stacks/laser_drivers/point_cloud_assembler/src/merge_clouds.cpp
pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/util/robot_self_filter/self_filter.launch
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
Added Paths:
-----------
pkg/trunk/sandbox/3dnav_pr2/launch/perception/components/
pkg/trunk/sandbox/3dnav_pr2/launch/perception/components/collision_map_self_occ.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/components/narrow_stereoproc.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/components/tilt_laser_self_filter.launch
Removed Paths:
-------------
pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
pkg/trunk/motion_planning/ompl/code/ompl/base/util/output.h
pkg/trunk/motion_planning/ompl/code/ompl/base/util/src/output.cpp
pkg/trunk/motion_planning/ompl/code/ompl/base/util/src/time.cpp
pkg/trunk/motion_planning/ompl/code/ompl/base/util/time.h
pkg/trunk/sandbox/3dnav_pr2/launch/perception/collision_map_occ.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/collision_map_self_occ.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/narrow_stereoproc.launch
Modified: pkg/trunk/mapping/collision_map/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-08-12 21:11:49 UTC (rev 21710)
+++ pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-08-12 21:41:23 UTC (rev 21711)
@@ -79,9 +79,6 @@
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)
-rospack_link_boost(collision_map_occ_node thread)
rospack_add_executable(collision_map_self_occ_node src/collision_map_self_occ.cpp)
rospack_add_openmp_flags(collision_map_self_occ_node)
Deleted: pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-08-12 21:11:49 UTC (rev 21710)
+++ pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-08-12 21:41:23 UTC (rev 21711)
@@ -1,843 +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 */
-
-#include <ros/ros.h>
-#include <sensor_msgs/PointCloud.h>
-#include <mapping_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>
-#include <cstdlib>
-
-class CollisionMapperOcc
-{
-public:
-
- CollisionMapperOcc(void) : sf_(tf_)
- {
- // read ROS params
- loadParams();
-
- // advertise our topics: full map and updates
- cmapPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_occ", 1);
- cmapUpdPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_occ_update", 1);
- if (publishOcclusion_)
- occPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_occ_occlusion", 1);
-
- // create a message notifier (and enable subscription) for both the full map and for the updates
- mnCloud_ = new tf::MessageNotifier<sensor_msgs::PointCloud>(tf_, boost::bind(&CollisionMapperOcc::cloudCallback, this, _1), "cloud_in", "", 1);
- mnCloudIncremental_ = new tf::MessageNotifier<sensor_msgs::PointCloud>(tf_, boost::bind(&CollisionMapperOcc::cloudIncrementalCallback, this, _1), "cloud_incremental_in", "", 1);
-
- // configure the self mask and the message notifier
- std::vector<std::string> frames;
- sf_.getLinkNames(frames);
- if (std::find(frames.begin(), frames.end(), robotFrame_) == frames.end())
- frames.push_back(robotFrame_);
- mnCloud_->setTargetFrame(frames);
- mnCloudIncremental_->setTargetFrame(frames);
- }
-
- ~CollisionMapperOcc(void)
- {
- delete mnCloud_;
- delete mnCloudIncremental_;
- }
-
-private:
-
- struct CollisionPoint
- {
- CollisionPoint(void) {}
- CollisionPoint(int _x, int _y, int _z) : x(_x), y(_y), z(_z) {}
-
- int x, y, z;
-
- ros::Time t;
- };
-
- // define an order on points
- struct CollisionPointOrder
- {
- bool operator()(const CollisionPoint &a, const CollisionPoint &b) const
- {
- 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;
- }
- };
-
- int collisionPointDistance(const CollisionPoint &a, const CollisionPoint &b) const
- {
- return std::max(abs(a.x - b.x), std::max(abs(a.y - b.y), abs(a.z - b.z)));
- }
-
- typedef std::set<CollisionPoint, CollisionPointOrder> CMap;
-
- // parameters & precomputed values for the box that represents the collision map
- // around the robot
- struct BoxInfo
- {
- double dimensionX, dimensionY, dimensionZ;
- double originX, originY, originZ;
- double sensorX, sensorY, sensorZ;
- double resolution;
- int radius;
- 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;
- ros::Duration keepOccluded;
- };
-
- void loadParams(void)
- {
- // flag that indicates whether obstacles should be added for self occlusion
- nh_.param<bool>("~mark_self_occlusion", markSelfOcclusion_, true);
-
- // 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);
-
- // when occluded obstacles are raytraced, keep boxes from occluded space within a given radius
- nh_.param<int>("~radius", bi_.radius, 1);
-
- // when occluded obstacles are raytraced, keep boxes from occluded space for a max amount of time
- double v;
- nh_.param<double>("~keep_occluded", v, 30.0);
- bi_.keepOccluded = ros::Duration(v);
-
- 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', radius for raytraced occlusions is %d.",
- 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(), bi_.radius);
-
- nh_.param<std::string>("~cloud_annotation", cloud_annotation_, std::string());
- nh_.param<bool>("~publish_occlusion", publishOcclusion_, false);
-
- // 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) - 1;
- bi_.maxX = (int)(0.5 + (bi_.dimensionX - bi_.originX) / bi_.resolution) + 1;
-
- bi_.minY = (int)(0.5 + (-bi_.dimensionY - bi_.originY) / bi_.resolution) - 1;
- bi_.maxY = (int)(0.5 + (bi_.dimensionY - bi_.originY) / bi_.resolution) + 1;
-
- bi_.minZ = (int)(0.5 + (-bi_.dimensionZ - bi_.originZ) / bi_.resolution) - 1;
- bi_.maxZ = (int)(0.5 + (bi_.dimensionZ - bi_.originZ) / bi_.resolution) + 1;
-
- 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;
- }
-
- void keepCMapLatest(CMap &map)
- {
- CMap::iterator it = map.begin();
- while (it != map.end())
- {
- const CollisionPoint &cp = *it;
- if (header_.stamp - cp.t > bi_.keepOccluded)
- {
- CMap::iterator e = it;
- ++it;
- map.erase(e);
- }
- else
- ++it;
- }
- }
-
- void computeCloudMask(const sensor_msgs::PointCloud &cloud, std::vector<int> &mask)
- {
- if (cloud_annotation_.empty())
- sf_.maskContainment(cloud, mask);
- else
- {
- int c = -1;
- for (unsigned int i = 0 ; i < cloud.channels.size() ; ++i)
- if (cloud.channels[i].name == cloud_annotation_)
- {
- c = i;
- break;
- }
- if (c < 0)
- {
- ROS_WARN("Cloud annotation channel '%s' is missing", cloud_annotation_.c_str());
- sf_.maskContainment(cloud, mask);
- }
- else
- {
- ROS_ASSERT(cloud.channels[c].values.size() == cloud.points.size());
- mask.resize(cloud.points.size());
- for (unsigned int i = 0 ; i < mask.size() ; ++i)
- mask[i] = cloud.channels[c].values[i] > 0.0;
- }
- }
- }
-
- void cloudIncrementalCallback(const sensor_msgs::PointCloudConstPtr &cloud)
- {
- if (!mapProcessing_.try_lock())
- return;
-
- ROS_DEBUG("Got pointcloud update that is %f seconds old", (ros::Time::now() - cloud->header.stamp).toSec());
-
- std::vector<int> mask;
- sensor_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
- computeCloudMask(*cloud, mask);
- }
- }
-
- CMap obstacles;
- constructCollisionMap(out, mask, 1, obstacles);
- CMap diff;
- set_difference(obstacles.begin(), obstacles.end(), currentMap_.begin(), currentMap_.end(),
- std::inserter(diff, diff.begin()), CollisionPointOrder());
- mapProcessing_.unlock();
- if (!diff.empty())
- publishCollisionMap(diff, out.header, cmapUpdPublisher_);
- }
-
- void cloudCallback(const sensor_msgs::PointCloudConstPtr &cloud)
- {
- ROS_DEBUG("Got pointcloud that is %f seconds old", (ros::Time::now() - cloud->header.stamp).toSec());
-
- mapProcessing_.lock();
-
- std::vector<int> mask;
- sensor_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
- computeCloudMask(*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, 1, obstacles);
- }
-#pragma omp section
- {
- constructCollisionMap(out, mask, 0, self);
- }
-#pragma omp section
- {
- // 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;
- }
- }
-
- // update map
- updateMap(obstacles, self);
-
- 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_, header_, cmapPublisher_);
- mapProcessing_.unlock();
- }
-
- void updateMap(CMap &obstacles, CMap &self)
- {
- if (currentMap_.empty())
- {
- // if we have no previous information, the map is simply what we see + we assume space hidden by self is obstructed
- // we ignore points that were previously occluded by robot but are now free due to motion
-
- if (markSelfOcclusion_)
- {
- CMap occ_self;
- CMap dummy;
- findSelfOcclusion(self, occ_self, dummy);
- std::set_union(obstacles.begin(), obstacles.end(),
- occ_self.begin(), occ_self.end(),
- std::inserter(currentMap_, currentMap_.begin()),
- CollisionPointOrder());
- }
- else
- currentMap_ = obstacles;
- }
- else
- {
- CMap diff;
- CMap occ;
-#pragma omp parallel sections
- {
-#pragma omp section
- {
- // this is the set of points that could be occluding information (the new collision map)
- std::set_union(obstacles.begin(), obstacles.end(),
- self.begin(), self.end(),
- std::inserter(occ, occ.begin()),
- CollisionPointOrder());
- }
-#pragma omp section
- {
- // find the points from the old map that are no longer visible
- set_difference(currentMap_.begin(), currentMap_.end(), obstacles.begin(), obstacles.end(),
- std::inserter(diff, diff.begin()), CollisionPointOrder());
- }
- }
-
- // find the points in the previous map that are now occluded (raytracing)
- CMap keep;
- findOcclusionsInMap(diff, occ, keep);
-
- sf_.assumeFrame(header_);
-
-#pragma omp parallel sections
- {
-
-#pragma omp section
- {
- keepCMapLatest(obstacles);
- }
-
-#pragma omp section
- {
- keepCMapLatest(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());
-
- // this can be used for debugging
- if (publishOcclusion_)
- publishCollisionMap(keep, header_, occPublisher_);
- }
- }
-
- bool transformMap(CMap &map, const roslib::Header &from, const roslib::Header &to)
- {
- 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(((double)pts[i].x - 0.5) * bi_.resolution + bi_.originX,
- ((double)pts[i].y - 0.5) * bi_.resolution + bi_.originY,
- ((double)pts[i].z - 0.5) * 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((int)(0.5 + (p.x() - bi_.originX) / bi_.resolution),
- (int)(0.5 + (p.y() - bi_.originY) / bi_.resolution),
- (int)(0.5 + (p.z() - bi_.originZ) / bi_.resolution));
- c.t = pts[i].t;
-#pragma omp critical
- {
- map.insert(c);
- }
- }
- }
-
- return true;
- }
- else
- {
- ROS_WARN("Unable to transform previous collision map into new frame");
- return false;
- }
- }
-
- void findOcclusionsInMap(const CMap &possiblyOccluded, const CMap &occluding, CMap &keep)
- {
- // OpenMP need an int as the lookup variable, but for set,
- // this is not possible, so we copy to a vector
- int n = possiblyOccluded.size();
- std::vector<CollisionPoint> pts(n);
- std::copy(possiblyOccluded.begin(), possiblyOccluded.end(), pts.begin());
-
- ROS_DEBUG("Raytracing %d cells", n);
-
-#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)
- // we start at a possibly occluded point and go towards the sensor
- // if we hit a point in the occluding set, we have an occluded point. Otherwise, the point was part of a
- // moving obstacle so we ignore it
- int result = 0;
- int state = -1;
- bresenham_line_3D(pts[i].x, pts[i].y, pts[i].z, bi_.sx, bi_.sy, bi_.sz,
- boost::bind(&CollisionMapperOcc::findOcclusionsInMapAux, this, &occluding, &state, &result, _1, _2, _3));
- if (result == 1)
- {
-#pragma omp critical
- {
- keep.insert(pts[i]);
- }
- }
- }
- }
-
- bool findOcclusionsInMapAux(const CMap *occluding, int *state, int *result, int x, int y, int z)
- {
- // we want to ignore the first few cells
- if (*state <= bi_.radius)
- *state = *state + 1;
- else
- {
- CollisionPoint p(x, y, z);
- CMap::const_iterator it = occluding->lower_bound(p);
-
- if (it != occluding->end())
- {
- if (collisionPointDistance(*it, p) <= bi_.radius)
- {
- // we hit a point that is occluding, our start point is occluded
- *result = 1;
- return true;
- }
- }
- }
-
- // continue looking
- return false;
- }
-
- void findSelfOcclusion(const CMap &self, CMap &occ_current, CMap &occ_moving)
- {
- // 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_current;
- boost::mutex lock_moving;
-
- // 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_current, &occ_moving, &state, &lock_current, &lock_moving, _1, _2, _3));
- }
- }
-
- bool findSelfOcclusionAux(CMap *occ_current, CMap *occ_moving, int *state, boost::mutex *lock_current, boost::mutex *lock_moving, int x, int y, int z)
- {
- if (*state == 16)
- {
- // here we have the option of adding further points in the occluded space
- // but for now, we stop
-
- return true;
- }
- else
- {
- // this is the point in the robotFrame_; check if it is currently inside the robot
- bool out = sf_.getMaskContainment(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(x, y, z);
- c.t = header_.stamp;
- lock_current->lock();
- occ_current->insert(c);
- lock_current->unlock();
-
- // mark that we are now outside the robot and have added a first point
- *state = 16;
-
- // continue looking on the ray
- return false;
- }
- 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;
-
- // continue further on the ray
- return false;
- }
- else
- {
- // if we get to this point, it means the point was in self collision, but it no longer is
- // so at least a part of the robot has moved; we will need to raytrace from this point later on
- // and check if we are occluding anything
- CollisionPoint c(x, y, z);
- c.t = header_.stamp;
- lock_moving->lock();
- occ_moving->insert(c);
- lock_moving->unlock();
-
- // and we stop looking on this ray
- return true;
- }
- }
- }
- }
-
- /** Construct an axis-aligned collision map from a point cloud assumed to be in the robot frame */
- void constructCollisionMap(const sensor_msgs::PointCloud &cloud, const std::vector<int> &mask, int keep, CMap &map)
- {
- const unsigned int n = cloud.points.size();
- CollisionPoint c;
-
- for (unsigned int i = 0 ; i < n ; ++i)
- if (mask[i] == keep)
- {
- const geometry_msgs::Point32 &p = cloud.points[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);
- c.t = cloud.header.stamp;
- 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,
- const boost::function<bool(int, int, int)> &callback) const
- {
- int pixel[3];
- int dx, dy, dz;
-
- pixel[0] = x1;
- pixel[1] = y1;
- pixel[2] = z1;
-
- dx = x2 - x1;
- dy = y2 - y1;
- dz = z2 - z1;
-
- bresenham_line_3D(dx, dy, dz, pixel, callback);
- }
-
- 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 pixel[3];
- int dx, dy, dz;
-
- 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;
- }
-
- bresenham_line_3D(dx, dy, dz, pixel, callback);
- }
-
- void bresenham_line_3D(int dx, int dy, int dz, int pixel[3], const boost::function<bool(int, int, int)> &callback) const
- {
- int i, l, m, n, x_inc, y_inc, z_inc, err_1, err_2, dx2, dy2, dz2;
-
- 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 roslib::Header &header, ros::Publisher &pub) const
- {
- mapping_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;
- mapping_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);
- }
- pub.publish(cmap);
-
- ROS_DEBUG("Published collision map with %u boxes", ms);
- }
-
- tf::TransformListener tf_;
- robot_self_filter::SelfMask sf_;
- tf::MessageNotifier<sensor_msgs::PointCloud> *mnCloud_;
- tf::MessageNotifier<sensor_msgs::PointCloud> *mnCloudIncremental_;
- ros::NodeHandle nh_;
- ros::Publisher cmapPublisher_;
- ros::Publisher cmapUpdPublisher_;
- ros::Publisher occPublisher_;
- roslib::Header header_;
- std::string cloud_annotation_;
- bool publishOcclusion_;
- bool markSelfOcclusion_;
-
- boost::mutex mapProcessing_;
- CMap currentMap_;
-
- BoxInfo bi_;
- std::string fixedFrame_;
- std::string robotFrame_;
-
-};
-
-int main (int argc, char** argv)
-{
- ros::init(argc, argv, "collision_map_occ", ros::init_options::AnonymousName);
-
- CollisionMapperOcc cm;
-
- ros::spin();
-
- return 0;
-}
-
-
Modified: pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp 2009-08-12 21:11:49 UTC (rev 21710)
+++ pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp 2009-08-12 21:41:23 UTC (rev 21711)
@@ -52,7 +52,7 @@
{
public:
- CollisionMapperOcc(void) : sf_(tf_)
+ CollisionMapperOcc(void)
{
// read ROS params
loadParams();
@@ -62,14 +62,32 @@
cmapUpdPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_occ_update", 1);
if (publishOcclusion_)
occPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_occ_occlusion", 1);
-
+
+ // configure self filter
+ std::vector<std::string> links;
+ std::string link_names;
+ nh_.param<std::string>("~self_see_links", link_names, std::string());
+ std::stringstream ss(link_names);
+ while (ss.good() && !ss.eof())
+ {
+ std::string link;
+ ss >> link;
+ links.push_back(link);
+ }
+ double padd;
+ nh_.param<double>("~self_see_padd", padd, 0.0);
+ double scale;
+ nh_.param<double>("~self_see_scale", scale, 1.0);
+
+ sf_ = new robot_self_filter::SelfMask(tf_, links, scale, padd);
+
// create a message notifier (and enable subscription) for both the full map and for the updates
mnCloud_ = new tf::MessageNotifier<sensor_msgs::PointCloud>(tf_, boost::bind(&CollisionMapperOcc::cloudCallback, this, _1), "cloud_in", "", 1);
mnCloudIncremental_ = new tf::MessageNotifier<sensor_msgs::PointCloud>(tf_, boost::bind(&CollisionMapperOcc::cloudIncrementalCallback, this, _1), "cloud_incremental_in", "", 1);
// configure the self mask and the message notifier
std::vector<std::string> frames;
- sf_.getLinkNames(frames);
+ sf_->getLinkNames(frames);
if (std::find(frames.begin(), frames.end(), robotFrame_) == frames.end())
frames.push_back(robotFrame_);
mnCloud_->setTargetFrame(frames);
@@ -80,6 +98,7 @@
{
delete mnCloud_;
delete mnCloudIncremental_;
+ delete sf_;
}
void run(void)
@@ -247,7 +266,7 @@
currentMap_ = obstacles;
// find out which of these points are now occluded
- sf_.assumeFrame(header_, bi_.sensor_frame);
+ sf_->assumeFrame(header_, bi_.sensor_frame);
// OpenMP need an int as the lookup variable, but for set,
// this is not possible, so we copy to a vector
@@ -266,7 +285,7 @@
btVector3 p(((double)pts[i].x - 0.5) * bi_.resolution + bi_.originX,
((double)pts[i].y - 0.5) * bi_.resolution + bi_.originY,
((double)pts[i].z - 0.5) * bi_.resolution + bi_.originZ);
- if (sf_.getMaskIntersection(p) == robot_self_filter::SHADOW)
+ if (sf_->getMaskIntersection(p) == robot_self_filter::SHADOW)
{
#pragma omp critical
{
@@ -286,7 +305,7 @@
btVector3 p(((double)pts[i].x - 0.5) * bi_.resolution + bi_.originX,
((double)pts[i].y - 0.5) * bi_.resolution + bi_.originY,
((double)pts[i].z - 0.5) * bi_.resolution + bi_.originZ);
- if (sf_.getMaskIntersection(p) == robot_self_filter::SHADOW)
+ if (sf_->getMaskIntersection(p) == robot_self_filter::SHADOW)
{
#pragma omp critical
{
@@ -385,23 +404,23 @@
ROS_DEBUG("Published collision map with %u boxes", ms);
}
- tf::TransformListener tf_;
- robot_self_filter::SelfMask sf_;
+ tf::TransformListener tf_;
+ robot_self_filter::SelfMask *sf_;
tf::MessageNotifier<sensor_msgs::PointCloud> *mnCloud_;
tf::MessageNotifier<sensor_msgs::PointCloud> *mnCloudIncremental_;
- ros::NodeHandle nh_;
- ros::Publisher cmapPublisher_;
- ros::Publisher cmapUpdPublisher_;
- ros::Publisher occPublisher_;
- roslib::Header header_;
- bool publishOcclusion_;
+ ros::NodeHandle nh_;
+ ros::Publisher cmapPublisher_;
+ ros::Publisher cmapUpdPublisher_;
+ ros::Publisher occPublisher_;
+ roslib::Header header_;
+ bool publishOcclusion_;
- boost::mutex mapProcessing_;
- CMap currentMap_;
+ boost::mutex mapProcessing_;
+ CMap currentMap_;
- BoxInfo bi_;
- std::string fixedFrame_;
- std::string robotFrame_;
+ BoxInfo bi_;
+ std::string fixedFrame_;
+ std::string robotFrame_;
};
Modified: pkg/trunk/motion_planning/ompl/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/ompl/CMakeLists.txt 2009-08-12 21:11:49 UTC (rev 21710)
+++ pkg/trunk/motion_planning/ompl/CMakeLists.txt 2009-08-12 21:41:23 UTC (rev 21711)
@@ -4,9 +4,7 @@
rospack_add_boost_directories()
include_directories(${PROJECT_SOURCE_DIR}/code)
set(CMAKE_BUILD_TYPE Release)
-rospack_add_library(ompl code/ompl/base/util/src/time.cpp
- code/ompl/base/util/src/output.cpp
- code/ompl/base/util/src/random_utils.cpp
+rospack_add_library(ompl code/ompl/base/util/src/random_utils.cpp
code/ompl/base/src/SpaceInformation.cpp
code/ompl/base/src/StateDistanceEvaluator.cpp
code/ompl/base/src/ProjectionEvaluator.cpp
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/Planner.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/Planner.h 2009-08-12 21:11:49 UTC (rev 21710)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/Planner.h 2009-08-12 21:41:23 UTC (rev 21711)
@@ -39,7 +39,7 @@
#include "ompl/base/General.h"
#include "ompl/base/SpaceInformation.h"
-#include "ompl/base/util/time.h"
+#include <ros/time.h>
/** Main namespace */
namespace ompl
@@ -103,7 +103,6 @@
SpaceInformation *m_si;
PlannerType m_type;
bool m_setup;
- msg::Interface m_msg;
};
}
}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h 2009-08-12 21:11:49 UTC (rev 21710)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h 2009-08-12 21:41:23 UTC (rev 21711)
@@ -43,7 +43,6 @@
#include "ompl/base/Path.h"
#include "ompl/base/StateDistanceEvaluator.h"
#include "ompl/base/StateValidityChecker.h"
-#include "ompl/base/util/output.h"
#include "ompl/base/util/random_utils.h"
#include <cstdlib>
@@ -300,7 +299,6 @@
StateDistanceEvaluator *m_stateDistanceEvaluator;
bool m_setup;
- msg::Interface m_msg;
};
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/src/Planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/src/Planner.cpp 2009-08-12 21:11:49 UTC (rev 21710)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/src/Planner.cpp 2009-08-12 21:41:23 UTC (rev 21711)
@@ -35,6 +35,7 @@
/* \author Ioan Sucan */
#include "ompl/base/Planner.h"
+#include <ros/console.h>
ompl::base::PlannerType ompl::base::Planner::getType(void) const
{
@@ -44,9 +45,9 @@
void ompl::base::Planner::setup(void)
{
if (!m_si->isSetup())
- m_msg.error("Space information setup should have been called before planner setup was called");
+ ROS_ERROR("Space information setup should have been called before planner setup was called");
if (m_setup)
- m_msg.error("Planner setup called multiple times");
+ ROS_ERROR("Planner setup called multiple times");
m_setup = true;
}
@@ -56,7 +57,7 @@
if (!goal)
{
- m_msg.error("Goal undefined");
+ ROS_ERROR("Goal undefined");
return false;
}
@@ -77,7 +78,7 @@
}
else
{
- m_msg.error("Initial state is in collision!");
+ ROS_ERROR("Initial state is in collision!");
}
}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/src/SpaceInformation.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/src/SpaceInformation.cpp 2009-08-12 21:11:49 UTC (rev 21710)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/src/SpaceInformation.cpp 2009-08-12 21:41:23 UTC (rev 21711)
@@ -36,6 +36,7 @@
#include "ompl/base/SpaceInformation.h"
#include <angles/angles.h>
+#include <ros/console.h>
#include <sstream>
#include <cstring>
#include <cassert>
@@ -125,7 +126,7 @@
void ompl::base::SpaceInformation::setup(void)
{
if (m_setup)
- m_msg.error("Space information setup called multiple times");
+ ROS_ERROR("Space information setup called multiple times");
assert(m_stateDimension > 0);
assert(m_stateComponent.size() == m_stateDimension);
m_setup = true;
@@ -175,10 +176,10 @@
{
v = isValid(st);
if (!v)
- m_msg.message("Initial state is not valid");
+ ROS_DEBUG("Initial state is not valid");
}
else
- m_msg.message("Initial state is not within space bounds");
+ ROS_DEBUG("Initial state is not within space bounds");
if (!b || !v)
{
@@ -188,13 +189,13 @@
for (unsigned int j = 0 ; j < rhoStart.size() ; ++j)
ss << rhoStart[j] << " ";
ss << "]";
- m_msg.message("Attempting to fix initial state %s", ss.str().c_str());
+ ROS_DEBUG("Attempting to fix initial state %s", ss.str().c_str());
base::State temp(m_stateDimension);
if (searchValidNearby(&temp, st, rhoStart, attempts))
copyState(st, &temp);
else
{
- m_msg.warn("Unable to fix start state %u", i);
+ ROS_WARN("Unable to fix start state %u", i);
result = false;
}
}
@@ -214,10 +215,10 @@
{
v = isValid(st);
if (!v)
- m_msg.message("Goal state is not valid");
+ ROS_DEBUG("Goal state is not valid");
}
else
- m_msg.message("Goal state is not within space bounds");
+ ROS_DEBUG("Goal state is not within space bounds");
if (!b || !v)
{
@@ -228,13 +229,13 @@
for (unsigned int i = 0 ; i < rhoGoal.size() ; ++i)
ss << rhoGoal[i] << " ";
ss << "]";
- m_msg.message("Attempting to fix goal state %s", ss.str().c_str());
+ ROS_DEBUG("Attempting to fix goal state %s", ss.str().c_str());
base::State temp(m_stateDimension);
if (searchValidNearby(&temp, st, rhoGoal, attempts))
copyState(st, &temp);
else
{
- m_msg.warn("Unable to fix goal state");
+ ROS_WARN("Unable to fix goal state");
result = false;
}
}
Deleted: pkg/trunk/motion_planning/ompl/code/ompl/base/util/output.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/util/output.h 2009-08-12 21:11:49 UTC (rev 21710)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/util/output.h 2009-08-12 21:41:23 UTC (rev 21711)
@@ -1,133 +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 OMPL_OUTPUT_INTERFACE_
-#define OMPL_OUTPUT_INTERFACE_
-
-#include <string>
-#include <cstdarg>
-
-/** Main namespace */
-namespace ompl
-{
-
- /** Message namespace */
- namespace msg
- {
-
- /** The piece of code that desires interaction with an action
- or an output handler should use an instance of this
- class */
- class Interface
- {
- public:
-
- Interface(void);
- virtual ~Interface(void);
-
- void inform(const std::string &text) const;
- void warn(const std::string &text) const;
- void error(const std::string &text) const;
- void message(const std::string &text) const;
-
- void inform(const char *msg, ...) const;
- void warn(const char *msg, ...) const;
- void error(const char *msg, ...) const;
- void message(const char *msg, ...) const;
-
- protected:
-
- std::string combine(const char *msg, va_list ap) const;
-
- };
-
- /** Generic class to handle output from a piece of code */
- class OutputHandler
- {
- public:
-
- OutputHandler(void)
- {
- }
-
- virtual ~OutputHandler(void)
- {
- }
-
- /** Print an error message: "Error: ...." */
- virtual void error(const std::string &text) = 0;
-
- /** Print an warning message: "Warning: ...." */
- virtual void warn(const std::string &text) = 0;
-
- /** Print some information: "Information: ...." */
- virtual void inform(const std::string &text) = 0;
-
- /** Print a simple message */
- virtual void message(const std::string &text) = 0;
- };
-
- class OutputHandlerSTD : public OutputHandler
- {
- public:
-
- OutputHandlerSTD(void) : OutputHandler()
- {
- }
-
- /** Print an error message: "Error: ...." */
- virtual void error(const std::string &text);
-
- /** Print an warning message: "Warning: ...." */
- virtual void warn(const std::string &text);
-
- /** Print some information: "Information: ...." */
- virtual void inform(const std::string &text);
-
- /** Print a simple message */
- virtual void message(const std::string &text);
-
-
- };
-
- void noOutputHandler(void);
- void useOutputHandler(OutputHandler *oh);
- OutputHandler* getOutputHandler(void);
- }
-
-}
-
-#endif
Deleted: pkg/trunk/motion_planning/ompl/code/ompl/base/util/src/output.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/util/src/output.cpp 2009-08-12 21:11:49 UTC (rev 21710)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/util/src/output.cpp 2009-08-12 21:41:23 UTC (rev 21711)
@@ -1,176 +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 */
-
-#include "ompl/base/util/output.h"
-#include <boost/thread/mutex.hpp>
-#include <iostream>
-#include <cstdlib>
-
-static ompl::msg::OutputHandlerSTD _defaultOutputHandler;
-static ompl::msg::OutputHandler *OUTPUT_HANDLER = static_cast<ompl::msg::OutputHandler*>(&_defaultOutputHandler);
-static boost::mutex _lock; // it is likely the outputhandler does some I/O, so we serialize it
-
-void ompl::msg::noOutputHandler(void)
-{
- _lock.lock();
- OUTPUT_HANDLER = NULL;
- _lock.unlock();
-}
-
-void ompl::msg::useOutputHandler(OutputHandler *oh)
-{
- _lock.lock();
- OUTPUT_HANDLER = oh;
- _lock.unlock();
-}
-
-ompl::msg::OutputHandler* ompl::msg::getOutputHandler(void)
-{
- return OUTPUT_HANDLER;
-}
-
-ompl::msg::Interface::Interface(void)
-{
-}
-
-ompl::msg::Interface::~Interface(void)
-{
-}
-
-void ompl::msg::Interface::message(const std::string &text) const
-{
- if (OUTPUT_HANDLER)
- {
- _lock.lock();
- OUTPUT_HANDLER->message(text);
- _lock.unlock();
- }
-}
-
-void ompl::msg::Interface::message(const char *msg, ...) const
-{
- va_list ap;
- va_start(ap, msg);
- message(combine(msg, ap));
- va_end(ap);
-}
-
-void ompl::msg::Interface::inform(const std::string &text) const
-{
- if (OUTPUT_HANDLER)
- {
- _lock.lock();
- OUTPUT_HANDLER->inform(text);
- _lock.unlock();
- }
-}
-
-void ompl::msg::Interface::inform(const char *msg, ...) const
-{
- va_list ap;
- va_start(ap, msg);
- inform(combine(msg, ap));
- va_end(ap);
-}
-
-void ompl::msg::Interface::warn(const std::string &text) const
-{
- if (OUTPUT_HANDLER)
- {
- _lock.lock();
- OUTPUT_HANDLER->warn(text);
- _lock.unlock();
- }
-}
-
-void ompl::msg::Interface::warn(const char *msg, ...) const
-{
- va_list ap;
- va_start(ap, msg);
- warn(combine(msg, ap));
- va_end(ap);
-}
-
-void ompl::msg::Interface::error(const std::string &text) const
-{
- if (OUTPUT_HANDLER)
- {
- _lock.lock();
- OUTPUT_HANDLER->error(text);
- _lock.unlock();
- }
-}
-
-void ompl::msg::Interface::error(const char *msg, ...) const
-{
- va_list ap;
- va_start(ap, msg);
- error(combine(msg, ap));
- va_end(ap);
-}
-std::string ompl::msg::Interface::combine(const char *msg, va_list va) const
-{
- va_list ap;
- va_copy(ap, va);
- char buf[1024];
- vsnprintf(buf, sizeof(buf), msg, ap);
- va_end(ap);
- return buf;
-}
-
-void ompl::msg::OutputHandlerSTD::error(const std::string &text)
-{
- std::cerr << "Error: " << text << std::endl;
- std::cerr.flush();
-}
-
-void ompl::msg::OutputHandlerSTD::warn(const std::string &text)
-{
- std::cerr << "Warning: " << text << std::endl;
- std::cerr.flush();
-}
-
-void ompl::msg::OutputHandlerSTD::inform(const std::string &text)
-{
- std::cout << "Info: " << text << std::endl;
- std::cout.flush();
-}
-
-void ompl::msg::OutputHandlerSTD::message(const std::string &text)
-{
- std::cout << text;
- std::cout.flush();
-}
Deleted: pkg/trunk/motion_planning/ompl/code/ompl/base/util/src/time.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/util/src/time.cpp 2009-08-12 21:11:49 UTC (rev 21710)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/util/src/time.cpp 2009-08-12 21:41:23 UTC (rev 21711)
@@ -1,231 +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.
-*********************************************************************/
-
-/* This file is taken from ROS, adaptations by Ioan Sucan */
-
-#include "ompl/base/util/time.h"
-#include <cmath>
-#include <time.h>
-
-#ifndef _WIN32
- #if _POSIX_TIMERS <= 0
- #include <sys/time.h>
- #endif
-#else
- #include <sys/timeb.h>
- static ompl::Time ompl::Time::start_time;
-#endif
-
-using namespace ompl::time_utils;
-using namespace std;
-
-Duration::Duration(int32_t _sec, int32_t _nsec)
- : sec(_sec), nsec(_nsec)
-{
- while (nsec > 1000000000)
- {
- nsec -= 1000000000;
- sec++;
- }
- while (nsec < 0)
- {
- nsec += 1000000000;
- sec--;
- }
-}
-
-Duration::Duration(double d)
-{
- if (d > 0)
- sec = (int32_t)floor(d);
- else
- sec = (int32_t)floor(d) + 1;
-
- nsec = (int32_t)((d - (double)sec)*1000000000);
-}
-
-Duration::Duration(int64_t t)
-{
- sec = (int32_t)(t / 1000000000);
- nsec = (int32_t)(t % 1000000000);
- while (nsec > 1000000000)
- {
- nsec -= 1000000000;
- sec++;
- }
- while (nsec < 0)
- {
- nsec += 1000000000;
- sec--;
- }
-}
-
-Duration Time::operator-(const Time &rhs) const
-{
- return Duration((int32_t)sec - (int32_t)rhs.sec,
- (int32_t)nsec - (int32_t)rhs.nsec); // carry handled in ctor
-}
-
-Time Time::operator+(const Duration &rhs) const
-{
- int64_t sec_sum = (int64_t)sec + (int64_t)rhs.sec;
- int64_t nsec_sum = (int64_t)nsec + (int64_t)rhs.nsec;
- while (nsec_sum < 0)
- {
- nsec_sum += 1000000000;
- sec_sum--;
- }
- while (nsec_sum > 1000000000)
- {
- nsec_sum -= 1000000000;
- sec_sum++;
- }
- return Time((uint32_t)sec_sum, (uint32_t)nsec_sum);
-}
-
-Duration Duration::operator+(const Duration &rhs) const
-{
- return Duration(sec + rhs.sec, nsec + rhs.nsec);
-}
-
-Duration Duration::operator-(const Duration &rhs) const
-{
- return Duration(sec - rhs.sec, nsec - rhs.nsec);
-}
-
-bool Duration::operator<(const Duration &rhs) const
-{
- if (sec < rhs.sec)
- return true;
- else if (sec == rhs.sec && nsec < rhs.nsec)
- return true;
- return false;
-}
-
-bool Duration::operator>(const Duration &rhs) const
-{
- if (sec > rhs.sec)
- return true;
- else if (sec == rhs.sec && nsec > rhs.nsec)
- return true;
- return false;
-}
-
-Time Time::now(void)
-{
- Time t;
-#ifndef _WIN32
-#if _POSIX_TIMERS > 0
- struct timespec start;
- clock_gettime(CLOCK_REALTIME, &start);
- t.sec = start.tv_sec;
- t.nsec = start.tv_nsec;
-#else
- struct timeval timeofday;
- gettimeofday(&timeofday,NULL);
- t.sec = timeofday.tv_sec;
- t.nsec = timeofday.tv_usec * 1000;
-#endif
- return t;
-#else
- if (start_time.isZero())
- {
- QueryPerformanceFrequency(&cpu_freq);
- if (cpu_freq.QuadPart == 0)
- {
- printf("woah! this system (for whatever reason) does not support the "
- "high-performance timing API. ur done.\n");
- abort();
- }
- QueryPerformanceCounter(&init_cpu_time);
- // compute an offset from the Epoch using the lower-performance timer API
- FILETIME ft;
- GetSystemTimeAsFileTime(&ft);
- LARGE_INTEGER start_li;
- start_li.LowPart = ft.dwLowDateTime;
- start_li.HighPart = ft.dwHighDateTime;
- // why did they choose 1601 as the time zero, instead of 1970?
- // there were no outstanding hard rock bands in 1601.
-#ifdef _MSC_VER
- start_li.QuadPart -= 116444736000000000Ui64;
-#else
- start_li.QuadPart -= 116444736000000000ULL;
-#endif
- start_time.sec = (uint32_t)(start_li.QuadPart / 10000000); // 100-ns units. odd.
- start_time.nsec = (start_li.LowPart % 10000000) * 100;
- }
- LARGE_INTEGER cur_time;
- QueryPerformanceCounter(&cur_time);
- LARGE_INTEGER delta_cpu_time;
- delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time.QuadPart;
- // todo: how to handle cpu clock drift. not sure it's a big deal for us.
- // also, think about clock wraparound. seems extremely unlikey, but possible
- double d_delta_cpu_time = delta_cpu_time.QuadPart / (double)cpu_freq.QuadPart;
- return Time(start_time + Duration(d_delta_cpu_time));
-#endif
-}
-
-bool Duration::operator==(const Duration &rhs) const
-{
- return sec == rhs.sec && nsec == rhs.nsec;
-}
-
-bool Time::operator==(const Time &rhs) const
-{
- return sec == rhs.sec && nsec == rhs.nsec;
-}
-
-bool Time::operator<(const Time &rhs) const
-{
- if (sec < rhs.sec)
- return true;
- else if (sec == rhs.sec && nsec < rhs.nsec)
- return true;
- return false;
-}
-
-bool Time::operator>(const Time &rhs) const
-{
- if (sec > rhs.sec)
- return true;
- else if (sec == rhs.sec && nsec > rhs.nsec)
- return true;
- return false;
-}
-
-bool Duration::sleep(void) const
-{
- struct timespec ts = {sec, nsec};
- return nanosleep(&ts, NULL) ? false : true;
-}
Deleted: pkg/trunk/motion_planning/ompl/code/ompl/base/util/time.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/util/time.h 2009-08-12 21:11:49 UTC (rev 21710)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/util/time.h 2009-08-12 21:41:23 UTC (rev 21711)
@@ -1,107 +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 f...
[truncated message content] |
