Skip to content

Commit

Permalink
Removed ScenarioPerceptionPipelineTestCaseGenerator class
Browse files Browse the repository at this point in the history
- Removed ScenarioPerceptionPipelineTestCaseGenerator class and added its methods to ScenarioPerceptionPipelineFixture
- Renamed ScenarioPerceptionPipelineFixture/selectTestCase method as ScenarioPerceptionPipelineFixture/selectTestCase
- Added documentation for ScenarioPerceptionPipelineFixture/setUp and ScenarioPerceptionPipelineFixture/tearDown methods
  • Loading branch information
CihatAltiparmak committed Jun 23, 2024
1 parent 95bfbc7 commit a682568
Show file tree
Hide file tree
Showing 2 changed files with 61 additions and 55 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ class ScenarioPerceptionPipeline
/** \brief Constructor for interface to send goal poses
* \param [in] move_group_interface_ptr The move_group_interface for sending goal poses
*/
ScenarioPerceptionPipeline(std::shared_ptr<MoveGroupInterface>);
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,
Expand All @@ -102,55 +102,61 @@ class ScenarioPerceptionPipeline
std::shared_ptr<MoveGroupInterface> move_group_interface_ptr_;
};

class ScenarioPerceptionPipelineTestCaseCreator
class ScenarioPerceptionPipelineFixture : public benchmark::Fixture
{
private:
protected:
/* ros node shared ptr*/
rclcpp::Node::SharedPtr node_;

/* move_group_interface_ptr for communicating with move_group_server */
std::shared_ptr<MoveGroupInterface> move_group_interface_ptr_;

/* selected test case index for benchmarking */
size_t selected_test_case_index_;

/** the list of test cases to be tested */
static inline std::vector<nav_msgs::msg::Path> test_cases_ = {};
std::vector<nav_msgs::msg::Path> test_cases_ = {};

public:
ScenarioPerceptionPipelineFixture();

/** \brief This method runs once each benchmark starts
* \param [in] state
* \return nothing is returned
*/
void SetUp(::benchmark::State& /*state*/);

/** \brief This method runs as soon as each benchmark finishes
* \param [in] state
* \return nothing is returned
*/
void TearDown(::benchmark::State& /*state*/);

/** \brief Creates test_cases by filling \e test_cases_
* If the planning is failed, the execution is ignored and then it is returned false
* \return nothing is returned
*/
static void createTestCases();
void createTestCases();

/** \brief Given a \e test_case_index, returns relevant test case.
* \param [in] test_case_index The index of test_case
* \return selected test case
*/
static nav_msgs::msg::Path selectTestCases(size_t test_case_index);
nav_msgs::msg::Path selectTestCase(size_t test_case_index);

/** \brief Given a \e yaml_file_name, reads test cases from given yaml file
* and adds this read test cases to \e test_cases_
* \param [in] yaml_file_name the path of yaml file which includes test cases
* \return nothing is returned
*/
static void readTestCasesFromFile(const std::string& yaml_file_name);
void readTestCasesFromFile(const std::string& yaml_file_name);

/** \brief Given a \e yaml_string, converts yaml_string to ros message
* and returns this ros message.
* \param [in] yaml_string the yaml string storing ros message fields
* \return nav_msgs::msg::Path which contains pose_list is returned
*/
static nav_msgs::msg::Path getTestCaseFromYamlString(const std::string& yaml_string);
};

class ScenarioPerceptionPipelineFixture : public benchmark::Fixture
{
protected:
/* ros node shared ptr*/
rclcpp::Node::SharedPtr node_;
/* move_group_interface_ptr for communicating with move_group_server */
std::shared_ptr<MoveGroupInterface> move_group_interface_ptr_;

/* selected test case index for benchmarking */
size_t selected_test_case_index_;

public:
ScenarioPerceptionPipelineFixture();
void SetUp(::benchmark::State& /*state*/);
void TearDown(::benchmark::State& /*state*/);
nav_msgs::msg::Path getTestCaseFromYamlString(const std::string& yaml_string);
};

} // namespace middleware_benchmark
Expand Down
60 changes: 30 additions & 30 deletions src/scenarios/scenario_perception_pipeline.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -81,17 +81,42 @@ bool ScenarioPerceptionPipeline::sendTargetPose(const geometry_msgs::msg::Pose&
}
}

void ScenarioPerceptionPipelineTestCaseCreator::createTestCases()
ScenarioPerceptionPipelineFixture::ScenarioPerceptionPipelineFixture()
{
createTestCases();
}

void ScenarioPerceptionPipelineFixture::SetUp(::benchmark::State& /*state*/)
{
if (node_.use_count() == 0)
{
node_ = std::make_shared<rclcpp::Node>("test_scenario_perception_pipeline_node",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

node_->get_parameter("selected_test_case_index", selected_test_case_index_);
}

if (move_group_interface_ptr_.use_count() == 0)
{
move_group_interface_ptr_ = std::make_shared<MoveGroupInterface>(node_, PLANNING_GROUP);
}
}

void ScenarioPerceptionPipelineFixture::TearDown(::benchmark::State& /*state*/)
{
}

void ScenarioPerceptionPipelineFixture::createTestCases()
{
readTestCasesFromFile(TEST_CASES_YAML_FILE);
}

nav_msgs::msg::Path ScenarioPerceptionPipelineTestCaseCreator::selectTestCases(size_t test_case_index)
nav_msgs::msg::Path ScenarioPerceptionPipelineFixture::selectTestCase(size_t test_case_index)
{
return test_cases_.at(test_case_index);
}

void ScenarioPerceptionPipelineTestCaseCreator::readTestCasesFromFile(const std::string& yaml_file_name)
void ScenarioPerceptionPipelineFixture::readTestCasesFromFile(const std::string& yaml_file_name)
{
YAML::Node config = YAML::LoadFile(yaml_file_name.c_str());
for (YAML::const_iterator it = config["test_cases"].begin(); it != config["test_cases"].end(); ++it)
Expand All @@ -103,7 +128,7 @@ void ScenarioPerceptionPipelineTestCaseCreator::readTestCasesFromFile(const std:
}
}

nav_msgs::msg::Path ScenarioPerceptionPipelineTestCaseCreator::getTestCaseFromYamlString(const std::string& yaml_string)
nav_msgs::msg::Path ScenarioPerceptionPipelineFixture::getTestCaseFromYamlString(const std::string& yaml_string)
{
nav_msgs::msg::Path path_msg;
void* ros_message = reinterpret_cast<void*>(&path_msg);
Expand All @@ -113,34 +138,9 @@ nav_msgs::msg::Path ScenarioPerceptionPipelineTestCaseCreator::getTestCaseFromYa
return path_msg;
}

ScenarioPerceptionPipelineFixture::ScenarioPerceptionPipelineFixture()
{
ScenarioPerceptionPipelineTestCaseCreator::createTestCases();
}

void ScenarioPerceptionPipelineFixture::SetUp(::benchmark::State& /*state*/)
{
if (node_.use_count() == 0)
{
node_ = std::make_shared<rclcpp::Node>("test_scenario_perception_pipeline_node",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

node_->get_parameter("selected_test_case_index", selected_test_case_index_);
}

if (move_group_interface_ptr_.use_count() == 0)
{
move_group_interface_ptr_ = std::make_shared<MoveGroupInterface>(node_, PLANNING_GROUP);
}
}

void ScenarioPerceptionPipelineFixture::TearDown(::benchmark::State& /*state*/)
{
}

BENCHMARK_DEFINE_F(ScenarioPerceptionPipelineFixture, test_scenario_perception_pipeline)(benchmark::State& st)
{
auto selected_test_case = ScenarioPerceptionPipelineTestCaseCreator::selectTestCases(selected_test_case_index_);
auto selected_test_case = selectTestCase(selected_test_case_index_);
for (auto _ : st)
{
auto sc = ScenarioPerceptionPipeline(move_group_interface_ptr_);
Expand Down

0 comments on commit a682568

Please sign in to comment.