Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20600] pkg/trunk
|
From: <is...@us...> - 2009-08-04 01:28:54
|
Revision: 20600
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20600&view=rev
Author: isucan
Date: 2009-08-04 01:28:47 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
option to get all collision contacts
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
pkg/trunk/world_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h 2009-08-04 01:28:33 UTC (rev 20599)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h 2009-08-04 01:28:47 UTC (rev 20600)
@@ -55,13 +55,15 @@
PlanningMonitor(CollisionModels *cm, tf::TransformListener *tf, std::string frame_id) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), tf, frame_id)
{
- onCollisionContact_ = NULL;
+ onCollisionContact_ = NULL;
+ maxCollisionContacts_ = 1;
loadParams();
}
PlanningMonitor(CollisionModels *cm, tf::TransformListener *tf) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), tf)
{
onCollisionContact_ = NULL;
+ maxCollisionContacts_ = 1;
loadParams();
}
@@ -124,9 +126,10 @@
bool transformJointToFrame(motion_planning_msgs::KinematicJoint &kj, const std::string &target) const;
/** \brief Set a callback to be called when a collision is found */
- void setOnCollisionContactCallback(const boost::function<void(collision_space::EnvironmentModel::Contact&)> &callback)
+ void setOnCollisionContactCallback(const boost::function<void(collision_space::EnvironmentModel::Contact&)> &callback, unsigned int maxContacts = 1)
{
onCollisionContact_ = callback;
+ maxCollisionContacts_ = maxContacts;
}
protected:
@@ -145,6 +148,7 @@
/** \brief User callback when a collision is found */
boost::function<void(collision_space::EnvironmentModel::Contact&)> onCollisionContact_;
+ unsigned int maxCollisionContacts_;
motion_planning_msgs::KinematicConstraints kcPath_;
motion_planning_msgs::KinematicConstraints kcGoal_;
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp 2009-08-04 01:28:33 UTC (rev 20599)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp 2009-08-04 01:28:47 UTC (rev 20600)
@@ -298,7 +298,7 @@
// check for collision
std::vector<collision_space::EnvironmentModel::Contact> contacts;
- bool valid = !getEnvironmentModel()->getCollisionContacts(contacts, 1);
+ bool valid = !getEnvironmentModel()->getCollisionContacts(contacts, maxCollisionContacts_);
getKinematicModel()->unlock();
getEnvironmentModel()->unlock();
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-08-04 01:28:33 UTC (rev 20599)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-08-04 01:28:47 UTC (rev 20600)
@@ -139,7 +139,7 @@
/** \brief Check for self collision. Contacts are not computed */
virtual bool isSelfCollision(void) = 0;
- /** \brief Get the list of contacts (collisions) */
+ /** \brief Get the list of contacts (collisions). The maximum number of contacts to be returned can be specified. If the value is 0, all found contacts are returned. */
virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1) = 0;
Modified: pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-08-04 01:28:33 UTC (rev 20599)
+++ pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-08-04 01:28:47 UTC (rev 20600)
@@ -246,7 +246,7 @@
if (nc > 0)
{
collision = true;
- for (int j = 0 ; j < nc && contacts.size() < max_count ; ++j)
+ for (int j = 0 ; j < nc && (contacts.size() < max_count || max_count == 0) ; ++j)
{
btManifoldPoint& pt = contactManifold->getContactPoint(j);
collision_space::EnvironmentModelBullet::Contact add;
@@ -268,7 +268,7 @@
}
contacts.push_back(add);
}
- if (contacts.size() >= max_count)
+ if (max_count > 0 && contacts.size() >= max_count)
break;
}
}
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-08-04 01:28:33 UTC (rev 20599)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-08-04 01:28:47 UTC (rev 20600)
@@ -514,7 +514,7 @@
cdata->collides = true;
for (int i = 0 ; i < numc ; ++i)
{
- if (cdata->contacts->size() < cdata->max_contacts)
+ if (cdata->max_contacts == 0 || cdata->contacts->size() < cdata->max_contacts)
{
collision_space::EnvironmentModelODE::Contact add;
@@ -537,7 +537,7 @@
break;
}
}
- if (cdata->contacts->size() >= cdata->max_contacts)
+ if (cdata->max_contacts > 0 && cdata->contacts->size() >= cdata->max_contacts)
cdata->done = true;
}
else
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
