Skip to content

Commit

Permalink
Removed unnecessary scores from perception pipeline benchmark
Browse files Browse the repository at this point in the history
  • Loading branch information
CihatAltiparmak committed Aug 14, 2024
1 parent a6e71c4 commit afede17
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 30 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -79,23 +79,16 @@ class ScenarioPerceptionPipeline
ScenarioPerceptionPipeline(std::shared_ptr<MoveGroupInterface> 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<int, int> 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 */
Expand Down
23 changes: 4 additions & 19 deletions src/scenarios/scenario_perception_pipeline.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,35 +49,22 @@ ScenarioPerceptionPipeline::ScenarioPerceptionPipeline(std::shared_ptr<MoveGroup
{
}

std::tuple<int, int> 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);

moveit::planning_interface::MoveGroupInterface::Plan plan_msg;
if (move_group_interface_ptr_->plan(plan_msg) == moveit::core::MoveItErrorCode::SUCCESS)
{
move_group_interface_ptr_->execute(plan_msg);
return true;
}
else
{
return false;
}
}

Expand Down Expand Up @@ -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);
}
}

Expand Down

0 comments on commit afede17

Please sign in to comment.