39 #include <boost/program_options/parsers.hpp>
40 #include <boost/program_options/variables_map.hpp>
43 using namespace std::chrono_literals;
45 static const std::string ROBOT_DESCRIPTION =
"robot_description";
47 static const rclcpp::Logger
LOGGER = rclcpp::get_logger(
"evaluate_collision_checking_speed");
52 RCLCPP_INFO(LOGGER,
"Starting thread %u",
id);
53 rclcpp::Clock clock(RCL_ROS_TIME);
55 rclcpp::Time start = clock.now();
56 for (
unsigned int i = 0; i < trials; ++i)
59 scene.checkCollision(req, res, state);
61 double duration = (clock.now() - start).seconds();
62 RCLCPP_INFO(LOGGER,
"Thread %u performed %lf collision checks per second",
id, (
double)trials / duration);
65 int main(
int argc,
char** argv)
67 rclcpp::init(argc, argv);
68 auto node = rclcpp::Node::make_shared(
"evaluate_collision_checking_speed");
70 unsigned int nthreads = 2;
71 unsigned int trials = 10000;
72 boost::program_options::options_description desc;
73 desc.add_options()(
"nthreads", boost::program_options::value<unsigned int>(&nthreads)->default_value(nthreads),
74 "Number of threads to use")(
75 "trials", boost::program_options::value<unsigned int>(&trials)->default_value(trials),
76 "Number of collision checks to perform with each thread")(
"wait",
77 "Wait for a user command (so the planning scene can be "
78 "updated in the background)")(
"help",
"this screen");
79 boost::program_options::variables_map vm;
80 boost::program_options::parsed_options po = boost::program_options::parse_command_line(argc, argv, desc);
81 boost::program_options::store(po, vm);
82 boost::program_options::notify(vm);
86 std::cout << desc <<
'\n';
90 rclcpp::executors::MultiThreadedExecutor executor;
91 executor.add_node(node);
100 std::cout <<
"Listening to planning scene updates. Press Enter to continue ..." <<
'\n';
104 rclcpp::sleep_for(500ms);
106 std::vector<moveit::core::RobotStatePtr> states;
107 RCLCPP_INFO(LOGGER,
"Sampling %u valid states...", nthreads);
108 for (
unsigned int i = 0; i < nthreads; ++i)
122 states.push_back(moveit::core::RobotStatePtr(state));
125 std::vector<std::thread*> threads;
127 for (
unsigned int i = 0; i < states.size(); ++i)
128 threads.push_back(
new std::thread([i, trials, &
scene = *psm.
getPlanningScene(), &state = *states[i]] {
129 return runCollisionDetection(i, trials, scene, state);
132 for (
unsigned int i = 0; i < states.size(); ++i)
139 RCLCPP_ERROR(LOGGER,
"Planning scene not configured");
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
void update(bool force=false)
Update all transforms.
This class maintains the representation of the environment as seen by a planning instance....
PlanningSceneMonitor Subscribes to the topic planning_scene.
const planning_scene::PlanningScenePtr & getPlanningScene()
Avoid this function! Returns an unsafe pointer to the current planning scene.
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor (ROS topic-based)
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 the OccupancyMapMonitor and listening for:
void runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene &scene, const moveit::core::RobotState &state)
int main(int argc, char **argv)
const rclcpp::Logger LOGGER
Representation of a collision checking request.
Representation of a collision checking result.
bool collision
True if collision was found, false otherwise.