Commit 614c90d1 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[ContinuousValidation] Implement thread safety

parent c3e10aa6
......@@ -19,6 +19,8 @@
#ifndef HPP_CORE_CONTINUOUS_VALIDATION_HH
# define HPP_CORE_CONTINUOUS_VALIDATION_HH
# include <hpp/pinocchio/pool.hh>
# include <hpp/core/fwd.hh>
# include <hpp/core/path-validation.hh>
# include <hpp/core/path-validation-report.hh>
......@@ -135,6 +137,9 @@ namespace hpp {
BodyPairCollisions_t bodyPairCollisions_;
// BodyPairCollision for which collision is disabled
BodyPairCollisions_t disabledBodyPairCollisions_;
pinocchio::Pool<BodyPairCollisions_t> bodyPairCollisionPool_;
value_type stepSize_;
// Initializer as a delegate
continuousValidation::InitializerPtr_t initializer_;
......
......@@ -119,7 +119,18 @@ namespace hpp {
return true;
}
}
return validateStraightPath(bodyPairCollisions_, path, reverse, validPart, report);
BodyPairCollisions_t* bpc;
if (!bodyPairCollisionPool_.available()) {
// Add an element
bpc = new BodyPairCollisions_t(bodyPairCollisions_.size());
for (std::size_t i = 0; i < bpc->size(); ++i)
(*bpc)[i] = bodyPairCollisions_[i]->copy();
bodyPairCollisionPool_.push_back (bpc);
}
bpc = bodyPairCollisionPool_.acquire();
bool ret = validateStraightPath(*bpc, path, reverse, validPart, report);
bodyPairCollisionPool_.release (bpc);
return ret;
}
void ContinuousValidation::addObstacle(const CollisionObjectConstPtr_t &object)
......@@ -132,6 +143,7 @@ namespace hpp {
ConstObjectStdVector_t objects;
objects.push_back(object);
bodyPairCollisions_.push_back(SolidSolidCollision::create (joint, objects, tolerance_));
bodyPairCollisionPool_.clear();
}
}
}
......@@ -162,6 +174,7 @@ namespace hpp {
if ((*itPair)->pairs().empty())
{
bodyPairCollisions_.erase(itPair);
bodyPairCollisionPool_.clear();
}
}
}
......@@ -204,6 +217,7 @@ namespace hpp {
break;
}
}
bodyPairCollisionPool_.clear();
}
void ContinuousValidation::init (ContinuousValidationWkPtr_t weak)
......
......@@ -53,6 +53,11 @@ MACRO(ADD_TESTCASE NAME GENERATED)
ENDMACRO(ADD_TESTCASE)
FIND_PACKAGE(OpenMP)
IF(OPENMP_FOUND)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
ENDIF()
# ADD_TESTCASE (test-kdTree FALSE)
ADD_TESTCASE (roadmap-1 FALSE)
ADD_TESTCASE (test-intervals FALSE)
......
......@@ -17,6 +17,12 @@
#define BOOST_TEST_MODULE ContinuousValidation
#include <boost/test/included/unit_test.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
namespace bpt = boost::posix_time;
#ifdef _OPENMP
# include <omp.h>
#endif
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/urdf/util.hh>
......@@ -117,11 +123,14 @@ BOOST_AUTO_TEST_CASE (continuous_validation_straight)
{
#include "../tests/random-numbers.hh"
i1 = 0;
i2 = 0;
// Load robot model (ur5)
DevicePtr_t robot (Device::create ("ur5"));
loadRobotModel (robot, "anchor", "ur_description", "ur5_joint_limited_robot",
"", "");
robot->numberDeviceData (4);
// create steering method
Problem problem (robot);
......@@ -136,9 +145,20 @@ BOOST_AUTO_TEST_CASE (continuous_validation_straight)
ConfigValidationPtr_t configValidation (CollisionValidation::create (robot));
ValidationReportPtr_t collisionReport;
// create random paths and test them with different validation instances
Configuration_t q1 (robot->configSize()),
q2 (robot->configSize());
bpt::ptime t0 = bpt::microsec_clock::local_time();
int Nthreads = 1;
#pragma omp parallel for
for (size_type i=0; i<n1; ++i) {
Configuration_t q1 (m1.row (i1)); ++i1;
Configuration_t q2 (m1.row (i1)); ++i1;
#ifdef _OPENMP
Nthreads = omp_get_num_threads();
#endif
#pragma omp critical
{
q1 = m1.row (i1); ++i1;
q2 = m1.row (i1); ++i1;
}
PathValidationReportPtr_t report1;
PathValidationReportPtr_t report2;
PathValidationReportPtr_t report3;
......@@ -149,6 +169,7 @@ BOOST_AUTO_TEST_CASE (continuous_validation_straight)
bool res2 (progressive->validate (path, false, validPart, report2));
bool res3 (dichotomy->validate (path, false, validPart, report3));
#pragma omp critical
if (!res1) {
BOOST_CHECK (!res2);
BOOST_CHECK (!res3);
......@@ -163,6 +184,7 @@ BOOST_AUTO_TEST_CASE (continuous_validation_straight)
hppDout (error, *report1);
}
}
#pragma omp critical
if (res1) {
BOOST_CHECK (res2);
BOOST_CHECK (res3);
......@@ -185,6 +207,7 @@ BOOST_AUTO_TEST_CASE (continuous_validation_straight)
bool res2 (progressive->validate (path, true, validPart, report2));
bool res3 (dichotomy->validate (path, true, validPart, report3));
#pragma omp critical
if (!res1) {
BOOST_CHECK (!res2);
BOOST_CHECK (!res3);
......@@ -199,6 +222,7 @@ BOOST_AUTO_TEST_CASE (continuous_validation_straight)
hppDout (error, *report1);
}
}
#pragma omp critical
if (res1) {
if (!res2) {
hppDout (info, "Progressive found a collision where discretized did "
......@@ -215,15 +239,22 @@ BOOST_AUTO_TEST_CASE (continuous_validation_straight)
}
}
}
bpt::ptime t1 = bpt::microsec_clock::local_time();
BOOST_MESSAGE ("Total time (nthreads " << Nthreads << "): " << (t1-t0).total_milliseconds() << "ms");
// delete problem
}
template <typename SplineSteeringMethod> void test_spline_steering_method ()
{
#include "../tests/random-numbers.hh"
i1 = 0;
i2 = 0;
// Load robot model (ur5)
DevicePtr_t robot (Device::create ("ur5"));
loadRobotModel (robot, "anchor", "ur_description", "ur5_joint_limited_robot",
"", "");
robot->numberDeviceData (4);
// create steering method
Problem problem (robot);
......@@ -240,11 +271,25 @@ template <typename SplineSteeringMethod> void test_spline_steering_method ()
std::vector<int> orders (1, 1);
// create random paths and test them with different validation instances
Configuration_t q1 (robot->configSize()),
q2 (robot->configSize());
vector_t v1 (robot->numberDof()),
v2 (robot->numberDof());
int Nthreads = 1;
bpt::ptime t0 = bpt::microsec_clock::local_time();
#pragma omp parallel for
for (size_type i=0; i<n2; ++i) {
std::cout << i << std::endl;
Configuration_t q1 (m1.row (i1)); ++i1;
Configuration_t q2 (m1.row (i1)); ++i1;
vector_t v1 (m2.row (i2++)), v2 (m2.row (i2++));
#ifdef _OPENMP
Nthreads = omp_get_num_threads();
#endif
#pragma omp critical
{
q1 = m1.row (i1++);
q2 = m1.row (i1++);
v1 = m2.row (i2++);
v2 = m2.row (i2++);
}
PathValidationReportPtr_t report1;
PathValidationReportPtr_t report2;
PathValidationReportPtr_t report3;
......@@ -255,6 +300,7 @@ template <typename SplineSteeringMethod> void test_spline_steering_method ()
bool res2 (progressive->validate (path, false, validPart, report2));
// bool res3 (dichotomy->validate (path, false, validPart, report3));
#pragma omp critical
if (!res1) {
BOOST_CHECK (!res2);
// BOOST_CHECK (!res3);
......@@ -269,6 +315,7 @@ template <typename SplineSteeringMethod> void test_spline_steering_method ()
hppDout (error, *report1);
}*/
}
#pragma omp critical
if (res1) {
BOOST_CHECK (res2);
if (!res2) {
......@@ -290,6 +337,7 @@ template <typename SplineSteeringMethod> void test_spline_steering_method ()
bool res2 (progressive->validate (path, true, validPart, report2));
// bool res3 (dichotomy->validate (path, true, validPart, report3));
#pragma omp critical
if (!res1) {
BOOST_CHECK (!res2);
// BOOST_CHECK (!res3);
......@@ -304,6 +352,7 @@ template <typename SplineSteeringMethod> void test_spline_steering_method ()
hppDout (error, *report1);
}*/
}
#pragma omp critical
if (res1) {
if (!res2) {
hppDout (info, "Progressive found a collision where discretized did "
......@@ -320,6 +369,8 @@ template <typename SplineSteeringMethod> void test_spline_steering_method ()
}
}
}
bpt::ptime t1 = bpt::microsec_clock::local_time();
BOOST_MESSAGE ("Total time (nthreads " << Nthreads << "): " << (t1-t0).total_milliseconds() << "ms");
// delete problem
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment