Skip to content

Commit

Permalink
Added developer documentation and renamed some variables
Browse files Browse the repository at this point in the history
  • Loading branch information
CihatAltiparmak committed Aug 18, 2024
1 parent 2594789 commit b4474e3
Show file tree
Hide file tree
Showing 5 changed files with 48 additions and 22 deletions.
2 changes: 1 addition & 1 deletion docs/how_to_run.md
Original file line number Diff line number Diff line change
Expand Up @@ -97,4 +97,4 @@ In this benchmark scenario, the benchmarker node only has client interface. The

### [Basic Topic Subscription Publishing Benchmark](scenarios/basic_topic_sub_pub.md)

This benchmark aims to measure the ROS message latency by sending some ROS message which has array section and timestamp section and receiving this message in topic listener side. Firstly, in the topic publisher side, message is created using given `pose_array_size`, `bwnchmarked_topic_name` and `benchmarked_topic_hz`. The topic listener subscribes the topic named `benchmarked_topic_name` to listen this ROS message is published. When message is received from topic publisher side, It's found the message latency by subtracting message timestamp (indicates the time message is published) inside the message callback from current time (indicates the time message is received). Finally, this message latencies are added to total elapsed time. When scenario is finished, this total elapsed time is used to compare middleware effects in scenario of topic subscription-publishing.
This benchmark aims to measure the ROS message latency by sending some ROS message which has array section and timestamp section and receiving this message in topic listener side. Firstly, in the topic publisher side, message is created using given `pose_array_size`, `bwnchmarked_topic_name` and `benchmarked_topic_hz`. The topic listener subscribes the topic named `benchmarked_topic_name` to listen this ROS message is published. When message is received from topic publisher side, It's found the message latency by subtracting message timestamp (indicates the time message is published) inside the message callback from current time (indicates the time message is received). Finally, this message latencies are added to total elapsed time. When message number handled achieved `max_received_message_number`, this total elapsed time is used to compare middleware effects in scenario of topic subscription-publishing.
8 changes: 4 additions & 4 deletions docs/scenarios/basic_topic_sub_pub.md
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ It will be defaultly benchmarked with 6 repetitions. It will be created the json
If you want to customize your benchmark arguments or select different test case, you can use below command.

```shell
ros2 launch moveit_middleware_benchmark scenario_basic_subscription_benchmark.launch.py benchmark_command_args:="--benchmark_out=middleware_benchmark_results.json --benchmark_out_format=json --benchmark_repetitions=1" pose_array_size:=1000 benchmarked_topic_name:="/dummy_benchmark_topic_name" benchmarked_topic_hz:=10 max_received_topic_number:=100
ros2 launch moveit_middleware_benchmark scenario_basic_subscription_benchmark.launch.py benchmark_command_args:="--benchmark_out=middleware_benchmark_results.json --benchmark_out_format=json --benchmark_repetitions=1" pose_array_size:=1000 benchmarked_topic_name:="/dummy_benchmark_topic_name" benchmarked_topic_hz:=10 max_received_topic_number:=100
```

Let's explain some parameters used in benchmark.
Expand All @@ -39,9 +39,9 @@ Let's explain some parameters used in benchmark.
| ---- | ----------- |
| `benchmark_command_args` | you can utilize this parameter to use custom benchmark arguments in your benchmark |
| `pose_array_size` | In this benchmark, [geometry_msgs/msg/PoseArray](https://docs.ros.org/en/rolling/p/geometry_msgs/interfaces/msg/PoseArray.html) is used. This parameter presents how many poses will be published from [basic_topic_publisher](../../src/scenarios/basic_topic_sub_pub/basic_topic_publisher.cpp). You can set message size sent by [basic_topic_publisher](../../src/scenarios/basic_topic_sub_pub/basic_topic_publisher.cpp) using this parameter. |
| `benchmarked_topic_hz` | This parameter presents in how many rate the messsage will be published from [basic_topic_publisher](../../src/scenarios/basic_topic_sub_pub/basic_topic_publisher.cpp). |
| `max_received_topic_number` | This parameter sets how many message to handled by [scenario_basic_subscription benchmarking module](../../src/scenarios/basic_topic_sub_pub/scenario_basic_subscription.cpp). The more message number is handled, the more reliable bechmark results is achieved in `scenario_basic_subscription` scenario. |
| `benchmarked_topic_hz` | This parameter presents in how many rate the message will be published from [basic_topic_publisher](../../src/scenarios/basic_topic_sub_pub/basic_topic_publisher.cpp). |
| `max_received_message_number` | This parameter sets how many message to handled by [scenario_basic_subscription benchmarking module](../../src/scenarios/basic_topic_sub_pub/scenario_basic_subscription.cpp). The more message number is handled, the more reliable benchmark results is achieved in `scenario_basic_subscription` scenario. |

## How to benchmark the basic topic subscription publishing works

The main idea here is to firstly save the message timestamp when published from [basic_topic_publisher](../../src/scenarios/basic_topic_sub_pub/basic_topic_publisher.cpp) and then handle this message in [scenario_basic_subscription benchmarking module](../../src/scenarios/basic_topic_sub_pub/scenario_basic_subscription.cpp) by which this message is received. This benchmark module uses [geometry_msgs/msg/PoseArray](https://docs.ros.org/en/rolling/p/geometry_msgs/interfaces/msg/PoseArray.html) which has arrangable size and timestamp sections in message body. When message is received by [scenario_basic_subscription benchmarking module](../../src/scenarios/basic_topic_sub_pub/scenario_basic_subscription.cpp), the difference between the time message is received and the time message is sent is added to total elapsed time. In the end, this total elapsed time is saved to benchmark results.
The main idea here is to firstly save the message timestamp when published from [basic_topic_publisher](../../src/scenarios/basic_topic_sub_pub/basic_topic_publisher.cpp) and then handle this message in [scenario_basic_subscription benchmarking module](../../src/scenarios/basic_topic_sub_pub/scenario_basic_subscription.cpp) by which this message is received. This benchmark module uses [geometry_msgs/msg/PoseArray](https://docs.ros.org/en/rolling/p/geometry_msgs/interfaces/msg/PoseArray.html) which has arrangeable size and timestamp sections in message body. When message is received by [scenario_basic_subscription benchmarking module](../../src/scenarios/basic_topic_sub_pub/scenario_basic_subscription.cpp), the difference between the time message is received and the time message is sent is added to total elapsed time. In the end, this total elapsed time is saved to benchmark results.
Original file line number Diff line number Diff line change
Expand Up @@ -55,22 +55,45 @@ namespace middleware_benchmark
class ScenarioBasicSubPub
{
public:
/** \brief Constructor
* \param [in] node The ros node for subscribing the benchmarking topic
* and getting the necessary ROS params
*/
ScenarioBasicSubPub(rclcpp::Node::SharedPtr node);

void runTestCase(benchmark::State&);
/** \brief the method to measure the message latency
* by handling maximum \e max_received_message_number_ message
* \param [in] benchmark_state the google benchmark instance
* to save message latency to benchmark results
*/
void runTestCase(benchmark::State& benchmark_state);

/** \brief the method to subscribe ROS message and add
* the message latencies to total elapsed time until
* \e max_received_message_number_ message is received
* \param [in] msg ROS message
*/
void subCallback(geometry_msgs::msg::PoseArray::SharedPtr msg);

private:
rclcpp::Node::SharedPtr node_;
rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr sub_;
size_t received_topic_number_;
size_t max_received_topic_number_;
size_t received_message_number_;

/* max message number to be able to handle */
size_t max_received_message_number_;

/* topic name to subscribe determined topic */
std::string benchmarked_topic_name_;

/* topic publishing hz */
int benchmarked_topic_hz_;

std::condition_variable test_case_cv_;
std::mutex is_test_case_finished_mutex_;
bool is_test_case_finished_;

/* total message latency */
std::chrono::duration<double> elapsed_time_;
};

Expand All @@ -79,15 +102,20 @@ class ScenarioBasicSubPubFixture : public benchmark::Fixture
public:
ScenarioBasicSubPubFixture();

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

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

protected:
rclcpp::Node::SharedPtr node_;
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor_;
std::thread node_thread_;
int max_receiving_topic_number_;
};

} // namespace middleware_benchmark
Expand Down
12 changes: 6 additions & 6 deletions launch/scenario_basic_subscription_benchmark.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,8 @@ def launch_setup(context, *args, **kwargs):
LaunchConfiguration("benchmarked_topic_name")
)

max_received_topic_number = int(
context.perform_substitution(LaunchConfiguration("max_received_topic_number"))
max_received_message_number = int(
context.perform_substitution(LaunchConfiguration("max_received_message_number"))
)

pose_array_size = int(
Expand All @@ -52,7 +52,7 @@ def launch_setup(context, *args, **kwargs):
output="both",
arguments=benchmark_command_args,
parameters=[
{"max_received_topic_number": max_received_topic_number},
{"max_received_message_number": max_received_message_number},
{"benchmarked_topic_name": benchmarked_topic_name},
{"benchmarked_topic_hz": benchmarked_topic_hz},
],
Expand Down Expand Up @@ -82,10 +82,10 @@ def generate_launch_description():
)
declared_arguments.append(benchmarked_topic_name_arg)

max_received_topic_number_arg = DeclareLaunchArgument(
"max_received_topic_number", default_value="1000"
max_received_message_number_arg = DeclareLaunchArgument(
"max_received_message_number", default_value="1000"
)
declared_arguments.append(max_received_topic_number_arg)
declared_arguments.append(max_received_message_number_arg)

pose_array_size_arg = DeclareLaunchArgument(
"pose_array_size", default_value="1000000"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -47,10 +47,10 @@ namespace middleware_benchmark
ScenarioBasicSubPub::ScenarioBasicSubPub(rclcpp::Node::SharedPtr node) : node_(node)
{
is_test_case_finished_ = false;
received_topic_number_ = 0;
received_message_number_ = 0;
node_->get_parameter("benchmarked_topic_name", benchmarked_topic_name_);
node_->get_parameter("benchmarked_topic_hz", benchmarked_topic_hz_);
node_->get_parameter("max_received_topic_number", max_received_topic_number_);
node_->get_parameter("max_received_message_number", max_received_message_number_);
}

void ScenarioBasicSubPub::runTestCase(benchmark::State& benchmark_state)
Expand All @@ -66,7 +66,7 @@ void ScenarioBasicSubPub::runTestCase(benchmark::State& benchmark_state)
RCLCPP_INFO(node_->get_logger(),
"Successfully subscribed to topic %s with hz %d! When received msg number is bigger than %ld, benchmark "
"will be finished!",
benchmarked_topic_name_.c_str(), benchmarked_topic_hz_, max_received_topic_number_);
benchmarked_topic_name_.c_str(), benchmarked_topic_hz_, max_received_message_number_);

std::unique_lock lk(is_test_case_finished_mutex_);
test_case_cv_.wait(lk, [this]() { return is_test_case_finished_; });
Expand All @@ -78,9 +78,9 @@ void ScenarioBasicSubPub::runTestCase(benchmark::State& benchmark_state)
void ScenarioBasicSubPub::subCallback(geometry_msgs::msg::PoseArray::SharedPtr pose_array_msg)
{
std::unique_lock lk(is_test_case_finished_mutex_);
received_topic_number_++;
received_message_number_++;

if (received_topic_number_ > max_received_topic_number_)
if (received_message_number_ > max_received_message_number_)
{
is_test_case_finished_ = true;
lk.unlock();
Expand All @@ -104,8 +104,6 @@ void ScenarioBasicSubPubFixture::SetUp(benchmark::State& /*state*/)
node_ = std::make_shared<rclcpp::Node>("test_scenario_basic_sub_pub",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

node_->get_parameter("max_receiving_topic_number", max_receiving_topic_number_);

executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
executor_->add_node(node_);
node_thread_ = std::thread([this]() { executor_->spin(); });
Expand Down

0 comments on commit b4474e3

Please sign in to comment.