Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17341] pkg/trunk
|
From: <is...@us...> - 2009-06-19 03:02:21
|
Revision: 17341
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17341&view=rev
Author: isucan
Date: 2009-06-19 03:02:19 +0000 (Fri, 19 Jun 2009)
Log Message:
-----------
a number of fixes
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h
pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -77,8 +77,15 @@
ROS_INFO("Starting move_arm for '%s' (IK is %senabled)", arm_.c_str(), perform_ik_ ? "" : "not ");
if (valid_)
- valid_ = collisionModels_->loadedModels() && getControlJointNames(arm_joint_names_);
+ valid_ = collisionModels_->loadedModels();
+ if (valid_)
+ {
+ planningMonitor_->waitForState();
+ planningMonitor_->waitForMap();
+ valid_ = getControlJointNames(arm_joint_names_);
+ }
+
if (!valid_)
ROS_ERROR("Move arm action is invalid");
@@ -138,8 +145,33 @@
ROS_INFO("Converting pose constraint to joint constraint using IK...");
std::vector<double> solution;
- if (computeIK(req.goal_constraints.pose_constraint[0].pose, solution))
+ bool foundIKsolution = false;
+ unsigned int IKsteps = 0;
+ while (IKsteps < 5 && !foundIKsolution)
{
+ if (!computeIK(req.goal_constraints.pose_constraint[0].pose, solution))
+ break;
+
+ // if IK did not fail, check if the state is valid
+ planning_models::StateParams spTest(*planningMonitor_->getRobotState());
+ unsigned int n = 0;
+ for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
+ {
+ unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
+ for (unsigned int j = 0 ; j < u ; ++j)
+ {
+ std::vector<double> params(solution.begin() + n, solution.begin() + n + u);
+ spTest.setParamsJoint(params, arm_joint_names_[i]);
+ }
+ n += u;
+ }
+ if (planningMonitor_->isStateValidAtGoal(&spTest))
+ foundIKsolution = true;
+ IKsteps++;
+ }
+
+ if (foundIKsolution)
+ {
unsigned int n = 0;
for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
{
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -148,6 +148,10 @@
{
std::stringstream ss;
printState(st, ss);
+ ss << " within margins [ ";
+ 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());
State temp(m_stateDimension);
if (searchValidNearby(&temp, st, rhoStart, attempts))
@@ -181,6 +185,10 @@
std::stringstream ss;
printState(st, ss);
+ ss << " within margins [ ";
+ 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());
State temp(m_stateDimension);
if (searchValidNearby(&temp, st, rhoGoal, attempts))
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -214,6 +214,7 @@
{
std::vector<planning_models::KinematicModel::Joint*> joints;
psetup->model->kmodel->getJoints(joints);
+ res.path.start_state.resize(joints.size());
for (unsigned int i = 0 ; i < joints.size() ; ++i)
{
res.path.start_state[i].header = res.path.header;
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -105,7 +105,6 @@
ROS_ERROR("Frame '%s' for joint '%s' in starting state is unknown", given[i].header.frame_id.c_str(), given[i].joint_name.c_str());
continue;
}
-
motion_planning_msgs::KinematicJoint kj = given[i];
if (planningMonitor_->transformJointToFrame(kj, planningMonitor_->getFrameId()))
s->setParamsJoint(kj.value, kj.joint_name);
@@ -117,6 +116,7 @@
{
if (planningMonitor_->haveState())
{
+ ROS_INFO("Using the current state to fill in the starting state for the motion plan");
std::vector<planning_models::KinematicModel::Joint*> joints;
planningMonitor_->getKinematicModel()->getJoints(joints);
for (unsigned int i = 0 ; i < joints.size() ; ++i)
@@ -149,6 +149,9 @@
if (startState)
{
+ std::stringstream ss;
+ startState->print(ss);
+ ROS_DEBUG("Complete starting state:\n%s", ss.str().c_str());
st = requestHandler_.computePlan(models_, startState, req, res);
delete startState;
}
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-19 03:02:19 UTC (rev 17341)
@@ -121,6 +121,10 @@
/** \brief Return true if a map update has been received in the last sec seconds. If sec < 10us, this function always returns true. */
bool isMapUpdated(double sec) const;
+
+ /** \brief Wait until a map is received */
+ void waitForMap(void) const;
+
/** \brief Return the last update time for the map */
const ros::Time& lastMapUpdate(void) const
{
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-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -78,6 +78,21 @@
return true;
}
+void planning_environment::CollisionSpaceMonitor::waitForMap(void) const
+{
+ int s = 0;
+ while (nh_.ok() && !haveMap())
+ {
+ if (s == 0)
+ ROS_INFO("Waiting for map ...");
+ s = (s + 1) % 40;
+ ros::spinOnce();
+ ros::Duration().fromSec(0.05).sleep();
+ }
+ if (haveMap())
+ ROS_INFO("Map received!");
+}
+
void planning_environment::CollisionSpaceMonitor::collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap)
{
int n = collisionMap->get_boxes_size();
Modified: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -168,9 +168,12 @@
void planning_environment::KinematicModelStateMonitor::waitForState(void) const
{
+ int s = 0;
while (nh_.ok() && !haveState())
{
- ROS_INFO("Waiting for mechanism state ...");
+ if (s == 0)
+ ROS_INFO("Waiting for mechanism state ...");
+ s = (s + 1) % 40;
ros::spinOnce();
ros::Duration().fromSec(0.05).sleep();
}
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -191,6 +191,7 @@
if (joint)
{
double *dparams = new double[joint->usedParams];
+ std::copy(params.begin(), params.end(), dparams);
result = setParamsJoint(dparams, name);
delete[] dparams;
}
@@ -223,8 +224,7 @@
bool planning_models::StateParams::setParams(const std::vector<double> ¶ms)
{
double *dparams = new double[m_mi.stateDimension];
- for (unsigned int i = 0 ; i < params.size() ; ++i)
- dparams[i] = params[i];
+ std::copy(params.begin(), params.end(), dparams);
bool result = setParams(dparams);
delete[] dparams;
return result;
@@ -251,8 +251,7 @@
bool planning_models::StateParams::setParamsGroup(const std::vector<double> ¶ms, int groupID)
{
double *dparams = new double[m_owner->getGroupDimension(groupID)];
- for (unsigned int i = 0 ; i < params.size() ; ++i)
- dparams[i] = params[i];
+ std::copy(params.begin(), params.end(), dparams);
bool result = setParamsGroup(dparams, groupID);
delete[] dparams;
return result;
Modified: pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h
===================================================================
--- pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h 2009-06-19 03:02:19 UTC (rev 17341)
@@ -36,8 +36,10 @@
#include <planning_environment/robot_models.h>
#include <collision_space/bodies.h>
#include <tf/transform_listener.h>
+#include <boost/shared_ptr.hpp>
#include <string>
#include <algorithm>
+#include <climits>
namespace filters
{
@@ -51,14 +53,14 @@
{
protected:
-
+
struct SeeLink
{
std::string name;
collision_space::bodies::Body* body;
btTransform constTransf;
};
-
+
struct SortBodies
{
bool operator()(const SeeLink &b1, const SeeLink &b2)
@@ -68,169 +70,331 @@
};
public:
- /** \brief Construct the filter */
- SelfFilter(void) : rm_("robot_description"), tf_(ros::Duration(10))
- {
- tf_.setExtrapolationLimit(ros::Duration().fromSec(10));
- }
+
+ /** \brief Construct the filter */
+ SelfFilter(void) : rm_("robot_description")
+ {
+ myTf_.reset(new tf::TransformListener());
+ tf_ = myTf_.get();
+ }
+ SelfFilter(tf::TransformListener *tf) : rm_("robot_description")
+ {
+ tf_ = tf;
+ }
+
+ /** \brief Destructor to clean up
+ */
+ ~SelfFilter(void)
+ {
+ for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
+ if (bodies_[i].body)
+ delete bodies_[i].body;
+ }
+
+ virtual bool configure(void)
+ {
+ // keep only the points that are outside of the robot
+ // for testing purposes this may be changed to true
+ nh_.param("~invert", invert_, false);
+
+ invert_ = false;
+
+ if (invert_)
+ ROS_INFO("Inverting filter output");
+
+ nh_.param("~accurate_timing", accurate_, true);
+
+ accurate_ = false;
+
+ if (accurate_)
+ ROS_INFO("Using accurate timing");
+ else
+ ROS_INFO("Using simple timing");
- /** \brief Destructor to clean up
- */
- ~SelfFilter(void)
- {
- for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
- if (bodies_[i].body)
- delete bodies_[i].body;
- }
+ std::vector<std::string> links = rm_.getSelfSeeLinks();
+ double scale = rm_.getSelfSeeScale();
+ double padd = rm_.getSelfSeePadding();
+
+ scale = 1.0;
+ padd = 0.0;
+
+ // from the geometric model, find the shape of each link of interest
+ // and create a body from it, one that knows about poses and can
+ // check for point inclusion
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ {
+ SeeLink sl;
+ sl.body = collision_space::bodies::createBodyFromShape(rm_.getKinematicModel()->getLink(links[i])->shape);
+ if (sl.body)
+ {
+ sl.name = links[i];
+
+ // collision models may have an offset, in addition to what TF gives
+ // so we keep it around
+ sl.constTransf = rm_.getKinematicModel()->getLink(links[i])->constGeomTrans;
+ sl.body->setScale(scale);
+ sl.body->setPadding(padd);
+ bodies_.push_back(sl);
+ }
+ else
+ ROS_WARN("Unable to create point inclusion body for link '%s'", links[i].c_str());
+ }
+
+ if (bodies_.empty())
+ ROS_WARN("No robot links will be checked for self collision");
+
+ // put larger volume bodies first -- higher chances of containing a point
+ std::sort(bodies_.begin(), bodies_.end(), SortBodies());
+
+ bspheres_.resize(bodies_.size());
+ bspheresRadius2_.resize(bodies_.size());
+ bodiesAtIdentity_ = false;
+
+ for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
+ ROS_INFO("Self see link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].body->computeVolume());
+
+ return true;
+ }
- virtual bool configure(void)
- {
- // keep only the points that are outside of the robot
- // for testing purposes this may be changed to true
- invert_ = false;
-
- if (invert_)
- ROS_WARN("Inverting filter output");
-
- std::vector<std::string> links = rm_.getSelfSeeLinks();
- double scale = rm_.getSelfSeeScale();
- double padd = rm_.getSelfSeePadding();
+
+ /** \brief Update the filter and return the data seperately
+ * \param data_in T array with length width
+ * \param data_out T array with length width
+ */
+ virtual bool update(const robot_msgs::PointCloud& data_in, robot_msgs::PointCloud& data_out)
+ {
+ if (bodies_.empty())
+ data_out = data_in;
+ else
+ {
+ std::vector<bool> keep(data_in.pts.size());
+
+ if (accurate_)
+ {
+ int chan = -1;
+ for (unsigned int i = 0 ; i < data_in.chan.size() ; ++i)
+ if (data_in.chan[i].name == "stamps")
+ {
+ chan = i;
+ break;
+ }
+ if (chan < 0)
+ {
+ ROS_WARN("'stamps' channel not available; pruning with simple timing");
+ pruneSimple(data_in, keep);
+ }
+ else
+ {
+ ROS_ASSERT(data_in.chan[chan].vals.size() == data_in.pts.size());
+ pruneAccurate(data_in, data_in.chan[chan], keep);
+ }
+ }
+ else
+ pruneSimple(data_in, keep);
+ fillResult(data_in, keep, data_out);
+ }
+ return true;
+ }
- // from the geometric model, find the shape of each link of interest
- // and create a body from it, one that knows about poses and can
- // check for point inclusion
- for (unsigned int i = 0 ; i < links.size() ; ++i)
- {
- SeeLink sl;
- sl.body = collision_space::bodies::createBodyFromShape(rm_.getKinematicModel()->getLink(links[i])->shape);
- if (sl.body)
- {
- sl.name = links[i];
-
- // collision models may have an offset, in addition to what TF gives
- // so we keep it around
- sl.constTransf = rm_.getKinematicModel()->getLink(links[i])->constGeomTrans;
- sl.body->setScale(scale);
- sl.body->setPadding(padd);
- bodies_.push_back(sl);
- }
- else
- ROS_WARN("Unable to create point inclusion body for link '%s'", links[i].c_str());
- }
-
- if (bodies_.empty())
- ROS_WARN("No robot links will be checked for self collision");
-
- // put larger volume bodies first -- higher chances of containing a point
- std::sort(bodies_.begin(), bodies_.end(), SortBodies());
-
- for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
- ROS_INFO("Self see link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].body->computeVolume());
-
- return true;
- }
+ void identityPoses(void)
+ {
+ if (!bodiesAtIdentity_)
+ {
+ // put all links at origin & lookup the needed transforms
+ const unsigned int bs = bodies_.size();
+ for (unsigned int i = 0 ; i < bs ; ++i)
+ bodies_[i].body->setPose(bodies_[i].constTransf);
+ computeBoundingSpheres();
+ bodiesAtIdentity_ = true;
+ }
+ }
+ void computeBoundingSpheres(void)
+ {
+ const unsigned int bs = bodies_.size();
+ for (unsigned int i = 0 ; i < bs ; ++i)
+ {
+ bodies_[i].body->computeBoundingSphere(bspheres_[i]);
+ bspheresRadius2_[i] = bspheres_[i].radius * bspheres_[i].radius;
+ }
+ }
+
+ void pruneAccurate(const robot_msgs::PointCloud& data_in, const robot_msgs::ChannelFloat32& times, std::vector<bool> &keep)
+ {
+ float maxT = *std::max_element(times.vals.begin(), times.vals.end());
+ if (maxT <= FLT_MIN)
+ {
+ ROS_WARN("'stamp' channel is bogus");
+ pruneSimple(data_in, keep);
+ return;
+ }
- /** \brief Update the filter and return the data seperately
- * \param data_in T array with length width
- * \param data_out T array with length width
- */
- virtual bool update(const robot_msgs::PointCloud& data_in, robot_msgs::PointCloud& data_out)
- {
- if (bodies_.empty())
- data_out = data_in;
- else
- {
- const unsigned int bs = bodies_.size();
- const unsigned int np = data_in.pts.size();
- std::vector<collision_space::bodies::BoundingSphere> bspheres(bs);
-
- // place the links in the frame of the pointcloud
- for (unsigned int i = 0 ; i < bs ; ++i)
- {
- // find the transform between the link's frame and the pointcloud frame
- tf::Stamped<btTransform> transf;
- if (tf_.canTransform(data_in.header.frame_id, bodies_[i].name, data_in.header.stamp))
- tf_.lookupTransform(data_in.header.frame_id, bodies_[i].name, data_in.header.stamp, transf);
- else
- {
- transf.setIdentity();
- ROS_ERROR("Unable to lookup transform from %s to %s", bodies_[i].name.c_str(), data_in.header.frame_id.c_str());
- }
-
- // set it for each body; we also include the offset specified in URDF
- bodies_[i].body->setPose(transf * bodies_[i].constTransf);
- bodies_[i].body->computeBoundingSphere(bspheres[i]);
- }
-
- // compute a sphere that bounds the entire robot
- collision_space::bodies::BoundingSphere bound;
- collision_space::bodies::mergeBoundingSpheres(bspheres, bound);
- btScalar radiusSquared = bound.radius * bound.radius;
-
- // we now decide which points we keep
- std::vector<bool> keep(np);
-
-#pragma omp parallel for
- for (int i = 0 ; i < (int)np ; ++i)
- {
- btVector3 pt = btVector3(btScalar(data_in.pts[i].x), btScalar(data_in.pts[i].y), btScalar(data_in.pts[i].z));
- bool out = true;
- if (bound.center.distance2(pt) < radiusSquared)
- for (unsigned int j = 0 ; out && j < bs ; ++j)
- out = !bodies_[j].body->containsPoint(pt);
-
- keep[i] = invert_ ? !out : out;
- }
-
-
- // fill in output data
- data_out.header = data_in.header;
-
- data_out.pts.resize(0);
- data_out.pts.reserve(np);
-
- data_out.chan.resize(data_in.chan.size());
- for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
- {
- ROS_ASSERT(data_in.chan[i].vals.size() == data_in.pts.size());
- data_out.chan[i].name = data_in.chan[i].name;
- data_out.chan[i].vals.reserve(data_in.chan[i].vals.size());
- }
-
- for (unsigned int i = 0 ; i < np ; ++i)
- if (keep[i])
- {
- data_out.pts.push_back(data_in.pts[i]);
- for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
- data_out.chan[j].vals.push_back(data_in.chan[j].vals[i]);
- }
- }
- return true;
- }
+ // put all links at origin
+ identityPoses();
+
+ const unsigned int bs = bodies_.size();
+ const unsigned int np = data_in.pts.size();
+
+ ros::Time timeStart = data_in.header.stamp;
+ ros::Time timeEnd = timeStart + ros::Duration(maxT);
+ std::vector< std::pair< btVector3, btVector3 > > origs(bs);
+ std::vector< std::pair< btQuaternion, btQuaternion > > quats(bs);
+ tf::Stamped<btTransform> transf;
+
+ // lookup the needed transforms
+ for (unsigned int i = 0 ; i < bs ; ++i)
+ {
+ if (tf_->canTransform(data_in.header.frame_id, bodies_[i].name, timeStart))
+ tf_->lookupTransform(data_in.header.frame_id, bodies_[i].name, timeStart, transf);
+ else
+ {
+ transf.setIdentity();
+ ROS_ERROR("Unable to lookup transform from '%s' to '%s' at start time", data_in.header.frame_id.c_str(), bodies_[i].name.c_str());
+ }
+ quats[i].first = transf.getRotation();
+ origs[i].first = transf.getOrigin();
+
+ if (tf_->canTransform(bodies_[i].name, data_in.header.frame_id, timeEnd))
+ tf_->lookupTransform(bodies_[i].name, data_in.header.frame_id, timeEnd, transf);
+ else
+ {
+ transf.setIdentity();
+ ROS_ERROR("Unable to lookup transform from '%s' to '%s' at end time", data_in.header.frame_id.c_str(), bodies_[i].name.c_str());
+ }
+ quats[i].second = transf.getRotation();
+ origs[i].first = transf.getOrigin();
+ }
+
+ // we now decide which points we keep
+#pragma omp parallel for schedule(dynamic)
+ for (int i = 0 ; i < (int)np ; ++i)
+ {
+ // this point is the cloud's frame
+ btVector3 pt = btVector3(btScalar(data_in.pts[i].x), btScalar(data_in.pts[i].y), btScalar(data_in.pts[i].z));
+ float time01 = times.vals[i] / maxT;
+ float time01i = 1.0f - time01;
+
+ bool out = true;
+ for (unsigned int j = 0 ; out && j < bs ; ++j)
+ {
+ // find the transform to bring the point to the link frame
+ btTransform t(quats[j].first.slerp(quats[j].second, time01i),
+ origs[j].first * time01i + origs[j].second * time01);
+
+ out = !bodies_[j].body->containsPoint(t * pt);
+ }
+
+ keep[i] = invert_ ? !out : out;
+ }
+ }
- virtual bool update(const std::vector<robot_msgs::PointCloud> & data_in, std::vector<robot_msgs::PointCloud>& data_out)
- {
- data_out.resize(data_in.size());
- for (unsigned int i = 0 ; i < data_in.size() ; ++i)
- update(data_in[i], data_out[i]);
- return true;
- }
+ void pruneSimple(const robot_msgs::PointCloud& data_in, std::vector<bool> &keep)
+ {
+ const unsigned int bs = bodies_.size();
+ const unsigned int np = data_in.pts.size();
+
+ // mark that links are no longer at origin
+ bodiesAtIdentity_ = false;
+
+ // place the links in the frame of the pointcloud
+ for (unsigned int i = 0 ; i < bs ; ++i)
+ {
+ // find the transform between the link's frame and the pointcloud frame
+ tf::Stamped<btTransform> transf;
+ if (tf_->canTransform(data_in.header.frame_id, bodies_[i].name, data_in.header.stamp))
+ tf_->lookupTransform(data_in.header.frame_id, bodies_[i].name, data_in.header.stamp, transf);
+ else
+ {
+ transf.setIdentity();
+ ROS_ERROR("Unable to lookup transform from %s to %s", bodies_[i].name.c_str(), data_in.header.frame_id.c_str());
+ }
+
+ // set it for each body; we also include the offset specified in URDF
+ bodies_[i].body->setPose(transf * bodies_[i].constTransf);
+ }
+ computeBoundingSpheres();
+
+ // compute a sphere that bounds the entire robot
+ collision_space::bodies::BoundingSphere bound;
+ collision_space::bodies::mergeBoundingSpheres(bspheres_, bound);
+ btScalar radiusSquared = bound.radius * bound.radius;
+
+ // we now decide which points we keep
+#pragma omp parallel for schedule(dynamic)
+ for (int i = 0 ; i < (int)np ; ++i)
+ {
+ btVector3 pt = btVector3(btScalar(data_in.pts[i].x), btScalar(data_in.pts[i].y), btScalar(data_in.pts[i].z));
+ bool out = true;
+ if (bound.center.distance2(pt) < radiusSquared)
+ for (unsigned int j = 0 ; out && j < bs ; ++j)
+ if (bspheres_[j].center.distance2(pt) < bspheresRadius2_[j])
+ out = !bodies_[j].body->containsPoint(pt);
+
+ keep[i] = invert_ ? !out : out;
+ }
+ }
+ void fillResult(const robot_msgs::PointCloud& data_in, const std::vector<bool> &keep, robot_msgs::PointCloud& data_out)
+ {
+ const unsigned int np = data_in.pts.size();
+
+ // fill in output data
+ data_out.header = data_in.header;
+
+ data_out.pts.resize(0);
+ data_out.pts.reserve(np);
+
+ data_out.chan.resize(data_in.chan.size());
+ for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
+ {
+ ROS_ASSERT(data_in.chan[i].vals.size() == data_in.pts.size());
+ data_out.chan[i].name = data_in.chan[i].name;
+ data_out.chan[i].vals.reserve(data_in.chan[i].vals.size());
+ }
+
+ for (unsigned int i = 0 ; i < np ; ++i)
+ if (keep[i])
+ {
+ data_out.pts.push_back(data_in.pts[i]);
+ for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
+ data_out.chan[j].vals.push_back(data_in.chan[j].vals[i]);
+ }
+ }
+
+ virtual bool update(const std::vector<robot_msgs::PointCloud> & data_in, std::vector<robot_msgs::PointCloud>& data_out)
+ {
+ bool result = true;
+ data_out.resize(data_in.size());
+ for (unsigned int i = 0 ; i < data_in.size() ; ++i)
+ if (!update(data_in[i], data_out[i]))
+ result = false;
+ return true;
+ }
+
protected:
-
- planning_environment::RobotModels rm_;
- ros::NodeHandle nh_;
- tf::TransformListener tf_;
- std::vector<SeeLink> bodies_;
- bool invert_;
-
-};
+
+ planning_environment::RobotModels rm_;
+ tf::TransformListener *tf_;
+ ros::NodeHandle nh_;
+
+ std::vector<SeeLink> bodies_;
+ std::vector<double> bspheresRadius2_;
+ std::vector<collision_space::bodies::BoundingSphere> bspheres_;
+ bool bodiesAtIdentity_;
-typedef robot_msgs::PointCloud robot_msgs_PointCloud;
-FILTERS_REGISTER_FILTER(SelfFilter, robot_msgs_PointCloud);
+ bool accurate_;
+ bool invert_;
+
+private:
+ boost::shared_ptr<tf::TransformListener> myTf_;
+
+};
+
+ typedef robot_msgs::PointCloud robot_msgs_PointCloud;
+ FILTERS_REGISTER_FILTER(SelfFilter, robot_msgs_PointCloud);
+
}
#endif //#ifndef FILTERS_SELF_SEE_H_
Modified: pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -41,7 +41,7 @@
{
public:
- SelfFilter(void)
+ SelfFilter(void) : tf_(ros::Duration(10.0))
{
sf_.configure();
pointCloudSubscriber_ = nh_.subscribe("full_cloud", 1, &SelfFilter::cloudCallback, this);
@@ -59,11 +59,12 @@
}
private:
-
+
+ tf::TransformListener tf_;
+ filters::SelfFilter<robot_msgs::PointCloud> sf_;
ros::Publisher pointCloudPublisher_;
ros::Subscriber pointCloudSubscriber_;
- ros::NodeHandle nh_;
- filters::SelfFilter<robot_msgs::PointCloud> sf_;
+ ros::NodeHandle nh_;
};
Modified: pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -83,16 +83,26 @@
in.header.stamp = ros::Time::now();
in.header.frame_id = "base_link";
+ in.chan.resize(1);
+ in.chan[0].name = "stamps";
const unsigned int N = 100000;
in.pts.resize(N);
+ in.chan[0].vals.resize(N);
for (unsigned int i = 0 ; i < N ; ++i)
{
in.pts[i].x = uniform(1);
in.pts[i].y = uniform(1);
in.pts[i].z = uniform(1);
+ in.chan[0].vals[i] = (double)i/(double)N;
}
+ for (unsigned int i = 0 ; i < 1000 ; ++i)
+ {
+ ros::Duration(0.001).sleep();
+ ros::spinOnce();
+ }
+
ros::WallTime tm = ros::WallTime::now();
sf_.update(in, out);
printf("%f points per second\n", (double)N/(ros::WallTime::now() - tm).toSec());
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
