Commit 6df6a949 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add benchmark output

parent 32ac237d
......@@ -34,6 +34,10 @@ SET (HPP_DEBUG FALSE CACHE BOOL "trigger hpp-util debug output")
IF (HPP_DEBUG)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHPP_DEBUG")
ENDIF()
SET (HPP_BENCHMARK FALSE CACHE BOOL "trigger hpp-util benchmarking output")
IF (HPP_BENCHMARK)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHPP_ENABLE_BENCHMARK")
ENDIF()
SET(TEST_WITH_ROMEO OFF CACHE BOOL
"Enable test with Romeo model: trigger dependency to romeo package.")
......
......@@ -31,6 +31,7 @@
#include <hpp/util/assertion.hh>
#include <hpp/util/debug.hh>
#include <hpp/util/timer.hh>
#include <hpp/model/collision-object.hh>
#include <hpp/model/joint.hh>
......@@ -42,6 +43,10 @@ namespace hpp
{
namespace srdf
{
namespace {
HPP_DEFINE_TIMECOUNTER(is_collision_pair_disabled);
}
Parser::Parser (urdf::Parser* parser)
: urdfParser_ (parser), srdfModel_ (), robot_ ()
{}
......@@ -66,6 +71,7 @@ namespace hpp
void Parser::sortCollisionPairs ()
{
hppStartBenchmark (sort_collision_pairs);
// Retrieve collision pairs that will NOT be taken into account.
const CollisionPairsType& disabledColPairs
= srdfModel_.getDisabledCollisionPairs ();
......@@ -74,10 +80,13 @@ namespace hpp
std::partial_sort_copy (disabledColPairs.begin (), disabledColPairs.end (),
sortedDisabledCollisions_.begin (), sortedDisabledCollisions_.end (),
disabledCollisionComp_);
hppStopBenchmark (sort_collision_pairs);
hppDisplayBenchmark (sort_collision_pairs);
}
void Parser::addCollisionPairs ()
{
hppStartBenchmark (add_collision_pairs);
typedef urdf::Parser::MapHppJointType MapHppJointType;
MapHppJointType& jmap = urdfParser_->jointsMap_;
......@@ -119,20 +128,26 @@ namespace hpp
}
}
}
hppStopBenchmark (add_collision_pairs);
hppDisplayBenchmark (add_collision_pairs);
}
bool
Parser::isCollisionPairDisabled (const std::string& bodyName_1,
const std::string& bodyName_2)
{
HPP_START_TIMECOUNTER(is_collision_pair_disabled);
DisabledCollision dc;
dc.link1_ = bodyName_1;
dc.link2_ = bodyName_2;
if (std::binary_search (sortedDisabledCollisions_.begin (),
sortedDisabledCollisions_.end (), dc, disabledCollisionComp_))
sortedDisabledCollisions_.end (), dc, disabledCollisionComp_)) {
HPP_STOP_TIMECOUNTER(is_collision_pair_disabled);
return true;
}
dc.link2_ = bodyName_1;
dc.link1_ = bodyName_2;
HPP_STOP_TIMECOUNTER(is_collision_pair_disabled);
return std::binary_search (sortedDisabledCollisions_.begin (),
sortedDisabledCollisions_.end (), dc, disabledCollisionComp_);
}
......@@ -249,6 +264,7 @@ namespace hpp
{
// Add collision pairs.
addCollisionPairs ();
HPP_DISPLAY_TIMECOUNTER(is_collision_pair_disabled);
}
} // end of namespace srdf.
} // end of namespace model.
......
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