38 #include <boost/program_options/parsers.hpp> 39 #include <boost/program_options/variables_map.hpp> 44 const robot_state::RobotState* state)
49 for (
unsigned int i = 0; i < trials; ++i)
55 ROS_INFO(
"Thread %u performed %lf collision checks per second",
id, (
double)trials / duration);
58 int main(
int argc,
char** argv)
60 ros::init(argc, argv,
"evaluate_collision_checking_speed");
62 unsigned int nthreads = 2;
63 unsigned int trials = 10000;
64 boost::program_options::options_description desc;
65 desc.add_options()(
"nthreads", boost::program_options::value<unsigned int>(&nthreads)->
default_value(nthreads),
66 "Number of threads to use")(
67 "trials", boost::program_options::value<unsigned int>(&trials)->default_value(trials),
68 "Number of collision checks to perform with each thread")(
"wait",
69 "Wait for a user command (so the planning scene can be " 70 "updated in thre background)")(
"help",
"this screen");
71 boost::program_options::variables_map vm;
72 boost::program_options::parsed_options po = boost::program_options::parse_command_line(argc, argv, desc);
73 boost::program_options::store(po, vm);
74 boost::program_options::notify(vm);
78 std::cout << desc << std::endl;
92 std::cout <<
"Listening to planning scene updates. Press Enter to continue ..." << std::endl;
98 std::vector<robot_state::RobotStatePtr> states;
99 ROS_INFO(
"Sampling %u valid states...", nthreads);
100 for (
unsigned int i = 0; i < nthreads; ++i)
103 robot_state::RobotState* state =
new robot_state::RobotState(psm.
getPlanningScene()->getRobotModel());
107 state->setToRandomPositions();
114 states.push_back(robot_state::RobotStatePtr(state));
117 std::vector<boost::thread*> threads;
119 for (
unsigned int i = 0; i < states.size(); ++i)
120 threads.push_back(
new boost::thread(
123 for (
unsigned int i = 0; i < states.size(); ++i)
130 ROS_ERROR(
"Planning scene not configured");
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const planning_scene::PlanningScenePtr & getPlanningScene()
Avoid this function! Returns an unsafe pointer to the current planning scene.
static const std::string ROBOT_DESCRIPTION
void startWorldGeometryMonitor(const std::string &collision_objects_topic=DEFAULT_COLLISION_OBJECT_TOPIC, const std::string &planning_scene_world_topic=DEFAULT_PLANNING_SCENE_WORLD_TOPIC, const bool load_octomap_monitor=true)
Start listening for objects in the world, the collision map and attached collision objects...
PlanningSceneMonitor Subscribes to the topic planning_scene.
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
int main(int argc, char **argv)
void runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene *scene, const robot_state::RobotState *state)