From afede17e9de99b24c6a90d9afecd2841791b6d14 Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Thu, 15 Aug 2024 02:04:25 +0300 Subject: [PATCH] Removed unnecessary scores from perception pipeline benchmark --- .../scenario_perception_pipeline.hpp | 15 ++++-------- .../scenario_perception_pipeline.cpp | 23 ++++--------------- 2 files changed, 8 insertions(+), 30 deletions(-) diff --git a/include/moveit_middleware_benchmark/scenarios/scenario_perception_pipeline.hpp b/include/moveit_middleware_benchmark/scenarios/scenario_perception_pipeline.hpp index 02f14bd..e74b078 100644 --- a/include/moveit_middleware_benchmark/scenarios/scenario_perception_pipeline.hpp +++ b/include/moveit_middleware_benchmark/scenarios/scenario_perception_pipeline.hpp @@ -79,23 +79,16 @@ class ScenarioPerceptionPipeline ScenarioPerceptionPipeline(std::shared_ptr move_group_interface_ptr); /** \brief Given a \e pose_list, sends poses in pose_list to plan, - * if the pipeline in \e sendTargetPose returns true, - * then success_number is increased. - * If the pipeline in \e sendTargetPose returns false, - * failure_number is increased. * \param [in] test_case The pose list to benchmark - * \return success_number and failure_number in tuple respectively */ - std::tuple runTestCase(const nav_msgs::msg::Path& pose_list); + void runTestCase(const nav_msgs::msg::Path& test_case); /** \brief Given a \e target_pose, sends this pose in order to plan via move_group_interface, - * if the planning is successful, - * planning message is sent to execute trajectory. Then it is returned true. - * If the planning is failed, the execution is ignored and then it is returned false + * if the planning is successful, planning message is sent to execute trajectory. + * If the planning is failed, the execution is ignored * \param [in] target_pose The pose to plan - * \return if the planning successful, returns true, or false */ - bool sendTargetPose(const geometry_msgs::msg::Pose& target_pose); + void sendTargetPose(const geometry_msgs::msg::Pose& target_pose); private: /** move_group_interface_ptr to communicate with move_group_server */ diff --git a/src/scenarios/scenario_perception_pipeline.cpp b/src/scenarios/scenario_perception_pipeline.cpp index 53dcc62..d4ca7da 100644 --- a/src/scenarios/scenario_perception_pipeline.cpp +++ b/src/scenarios/scenario_perception_pipeline.cpp @@ -49,23 +49,15 @@ ScenarioPerceptionPipeline::ScenarioPerceptionPipeline(std::shared_ptr ScenarioPerceptionPipeline::runTestCase(const nav_msgs::msg::Path& test_case) +void ScenarioPerceptionPipeline::runTestCase(const nav_msgs::msg::Path& test_case) { - int success_number = 0; - int failure_number = 0; for (auto& pose_stamped : test_case.poses) { - bool is_successful = sendTargetPose(pose_stamped.pose); - if (is_successful) - success_number++; - else - failure_number++; + sendTargetPose(pose_stamped.pose); } - - return { success_number, failure_number }; } -bool ScenarioPerceptionPipeline::sendTargetPose(const geometry_msgs::msg::Pose& target_pose) +void ScenarioPerceptionPipeline::sendTargetPose(const geometry_msgs::msg::Pose& target_pose) { move_group_interface_ptr_->setPoseTarget(target_pose); @@ -73,11 +65,6 @@ bool ScenarioPerceptionPipeline::sendTargetPose(const geometry_msgs::msg::Pose& if (move_group_interface_ptr_->plan(plan_msg) == moveit::core::MoveItErrorCode::SUCCESS) { move_group_interface_ptr_->execute(plan_msg); - return true; - } - else - { - return false; } } @@ -151,9 +138,7 @@ BENCHMARK_DEFINE_F(ScenarioPerceptionPipelineFixture, test_scenario_perception_p for (auto _ : st) { auto sc = ScenarioPerceptionPipeline(move_group_interface_ptr_); - auto [success_number, failure_number] = sc.runTestCase(selected_test_case); - st.counters["success_number"] = success_number; - st.counters["failure_number"] = failure_number; + sc.runTestCase(selected_test_case); } }