bench.cpp

// Benchmark
// Author: Max Schwarz <max.schwarz@ais.uni-bonn.de>

#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/utils/robot_model_test_utils.h>

#include <moveit/collision_detection_fcl/collision_detector_allocator_fcl.h>

#include <rosfmt/full.h>

int main(int argc, char** argv)
{
    auto model = moveit::core::loadTestingRobotModel("panda");
    if(!model)
        throw std::runtime_error{"Could not load robot model"};

    robot_state::RobotState state{model};

    state.setToDefaultValues();
    double joint2 = -0.785;
    double joint4 = -2.356;
    double joint6 = 1.571;
    double joint7 = 0.785;
    state.setJointPositions("panda_joint2", &joint2);
    state.setJointPositions("panda_joint4", &joint4);
    state.setJointPositions("panda_joint6", &joint6);
    state.setJointPositions("panda_joint7", &joint7);
    state.update();

    collision_detection::CollisionDetectorAllocatorFCL allocator;
    auto env = allocator.allocateEnv(model);

    collision_detection::DistanceRequest req;
    req.enable_nearest_points = true;
    req.enable_signed_distance = true;
    req.type = collision_detection::DistanceRequestType::SINGLE;
    collision_detection::DistanceResult res;

    // Warm up
    for(int i = 0; i < 50; ++i)
    {
        env->distanceSelf(req, res, state);
        res.clear();
    }

    constexpr int ITERATIONS = 1000;

    ros::SteadyTime t0 = ros::SteadyTime::now();

    for(int i = 0; i < ITERATIONS; ++i)
    {
        env->distanceSelf(req, res, state);
        res.clear();
    }

    ros::SteadyTime t1 = ros::SteadyTime::now();

    double ms = (t1-t0).toSec()*1000;
    fmt::print("Took {}ms -> {}ms per iter\n", ms, ms/ITERATIONS);

    return 0;
}