Skip to content

Commit

Permalink
pre-commit fix
Browse files Browse the repository at this point in the history
  • Loading branch information
CihatAltiparmak committed Aug 18, 2024
1 parent aae09af commit 8a88cbf
Show file tree
Hide file tree
Showing 3 changed files with 41 additions and 35 deletions.
9 changes: 5 additions & 4 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -94,10 +94,11 @@ target_include_directories(
install(TARGETS scenario_basic_subscription_basic_topic_publisher
DESTINATION lib/moveit_middleware_benchmark)

install(TARGETS scenario_perception_pipeline_benchmark_main
scenario_basic_service_client_benchmark_main
scenario_basic_subscription_benchmark_main
DESTINATION lib/moveit_middleware_benchmark)
install(
TARGETS scenario_perception_pipeline_benchmark_main
scenario_basic_service_client_benchmark_main
scenario_basic_subscription_benchmark_main
DESTINATION lib/moveit_middleware_benchmark)

install(DIRECTORY launch config DESTINATION share/moveit_middleware_benchmark)

Expand Down
52 changes: 26 additions & 26 deletions src/scenarios/basic_topic_sub_pub/basic_topic_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,44 +33,44 @@
*********************************************************************/

/* Author: Cihat Kurtuluş Altıparmak
Description: Basic pose_array_msg topic publisher to measure the middleware
effects in basic topic subscription-publishing scenario
Description: Basic pose_array_msg topic publisher to measure the middleware
effects in basic topic subscription-publishing scenario
*/

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_array.hpp>

int main(int argc, char ** argv) {
int main(int argc, char** argv)
{
size_t pose_array_size = 10;
double benchmarked_topic_hz = 10;
std::string benchmarked_topic_name = "/benchmarked_topic1";

size_t pose_array_size = 10;
double benchmarked_topic_hz = 10;
std::string benchmarked_topic_name = "/benchmarked_topic1";
rclcpp::init(argc, argv);

rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("basic_topic_publisher");

auto node = rclcpp::Node::make_shared("basic_topic_publisher");
node->get_parameter("benchmarked_topic_hz", benchmarked_topic_hz);
node->get_parameter("benchmarked_topic_name", benchmarked_topic_name);
node->get_parameter("pose_array_size", pose_array_size);

node->get_parameter("benchmarked_topic_hz", benchmarked_topic_hz);
node->get_parameter("benchmarked_topic_name", benchmarked_topic_name);
node->get_parameter("pose_array_size", pose_array_size);
auto topic_pub = node->create_publisher<geometry_msgs::msg::PoseArray>(benchmarked_topic_name, 10);

geometry_msgs::msg::PoseArray sample_pose_array_msg;
sample_pose_array_msg.poses.resize(pose_array_size);

auto topic_pub = node->create_publisher<geometry_msgs::msg::PoseArray>(benchmarked_topic_name, 10);
rclcpp::Rate sleep_rate(benchmarked_topic_hz);

geometry_msgs::msg::PoseArray sample_pose_array_msg;
sample_pose_array_msg.poses.resize(pose_array_size);
while (rclcpp::ok())
{
// save the publishing date of message
sample_pose_array_msg.header.stamp = node->now();

rclcpp::Rate sleep_rate(benchmarked_topic_hz);
// publish the message
topic_pub->publish(sample_pose_array_msg);

while (rclcpp::ok()) {
// save the publishing date of message
sample_pose_array_msg.header.stamp = node->now();
sleep_rate.sleep();
}

// publish the message
topic_pub->publish(sample_pose_array_msg);

sleep_rate.sleep();
}

rclcpp::shutdown();
}
rclcpp::shutdown();
}
15 changes: 10 additions & 5 deletions src/scenarios/basic_topic_sub_pub/scenario_basic_subscription.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,8 +69,8 @@ void ScenarioBasicSubPub::runTestCase(benchmark::State& benchmark_state)
benchmarked_topic_name_.c_str(), benchmarked_topic_hz_, max_received_topic_number_);

std::unique_lock lk(is_test_case_finished_mutex_);
test_case_cv_.wait(lk, [this](){return is_test_case_finished_;});
test_case_cv_.wait(lk, [this]() { return is_test_case_finished_; });

benchmark_state.SetIterationTime(elapsed_time_.count());
RCLCPP_INFO(node_->get_logger(), "Benchmarked test case is finished!");
}
Expand All @@ -80,11 +80,14 @@ void ScenarioBasicSubPub::subCallback(geometry_msgs::msg::PoseArray::SharedPtr p
std::unique_lock lk(is_test_case_finished_mutex_);
received_topic_number_++;

if (received_topic_number_ > max_received_topic_number_) {
if (received_topic_number_ > max_received_topic_number_)
{
is_test_case_finished_ = true;
lk.unlock();
test_case_cv_.notify_one();
} else {
}
else
{
auto msg_latency_time = (node_->now() - pose_array_msg->header.stamp).to_chrono<std::chrono::duration<double>>();
elapsed_time_ += msg_latency_time;
}
Expand Down Expand Up @@ -129,7 +132,9 @@ BENCHMARK_DEFINE_F(ScenarioBasicSubPubFixture, test_scenario_basic_sub_pub)(benc
}
}

BENCHMARK_REGISTER_F(ScenarioBasicSubPubFixture, test_scenario_basic_sub_pub)->UseManualTime()->Unit(benchmark::kNanosecond);
BENCHMARK_REGISTER_F(ScenarioBasicSubPubFixture, test_scenario_basic_sub_pub)
->UseManualTime()
->Unit(benchmark::kNanosecond);

} // namespace middleware_benchmark
} // namespace moveit

0 comments on commit 8a88cbf

Please sign in to comment.