Go to the documentation of this file.00001
00002 #include <topic_tools/shape_shifter.h>
00003 #include <boost/algorithm/string.hpp>
00004 #include <boost/serialization/serialization.hpp>
00005 #include <boost/utility/string_ref.hpp>
00006 #include <geometry_msgs/Pose.h>
00007 #include <sensor_msgs/JointState.h>
00008 #include <sstream>
00009 #include <iostream>
00010 #include <chrono>
00011 #include <ros_type_introspection/ros_introspection.hpp>
00012
00013
00014 #include <benchmark/benchmark.h>
00015
00016
00017 using namespace ros::message_traits;
00018 using namespace RosIntrospection;
00019
00020
00021 static std::vector<SubstitutionRule> Rules()
00022 {
00023 std::vector<SubstitutionRule> rules;
00024
00025
00026 rules.push_back( SubstitutionRule( "transforms.#.transform",
00027 "transforms.#.header.frame_id",
00028 "transforms.#" ));
00029
00030 rules.push_back( SubstitutionRule( "transforms.#.header",
00031 "transforms.#.header.frame_id",
00032 "transforms.#.header" ));
00033
00034 rules.push_back( SubstitutionRule( "position.#", "name.#", "@.position" ));
00035 rules.push_back( SubstitutionRule( "velocity.#", "name.#", "@.velocity" ));
00036 rules.push_back( SubstitutionRule( "effort.#", "name.#", "@.effort" ));
00037 return rules;
00038 }
00039
00040
00041 static void BM_Joints(benchmark::State& state)
00042 {
00043 RosIntrospection::Parser parser;
00044
00045 ROSType main_type(DataType<sensor_msgs::JointState>::value());
00046
00047 parser.registerMessageDefinition(
00048 "joint_state",
00049 main_type,
00050 Definition<sensor_msgs::JointState>::value());
00051
00052 parser.registerRenamingRules( main_type, Rules() );
00053
00054 sensor_msgs::JointState js_msg;
00055
00056 js_msg.name.resize(6);
00057 js_msg.position.resize(6);
00058 js_msg.velocity.resize(6);
00059 js_msg.effort.resize(6);
00060
00061 const char* suffix[6] = { "_A", "_B", "_C", "_D" , "_E", "_F"};
00062
00063 for (size_t i=0; i< js_msg.name.size() ; i++)
00064 {
00065 js_msg.header.seq = 100+i;
00066 js_msg.header.stamp.sec = 1234;
00067 js_msg.header.frame_id = std::string("frame").append(suffix[i]);
00068
00069 js_msg.name[i] = std::string("child").append(suffix[i]);
00070 js_msg.position[i] = 10 +i;
00071 js_msg.velocity[i] = 20 +i;
00072 js_msg.effort[i] = 30 +i;
00073 }
00074
00075 std::vector<uint8_t> buffer( ros::serialization::serializationLength(js_msg) );
00076 ros::serialization::OStream stream(buffer.data(), buffer.size());
00077 ros::serialization::Serializer<sensor_msgs::JointState>::write(stream, js_msg);
00078
00079 FlatMessage flat_container;
00080 RenamedValues renamed_values;
00081
00082 while (state.KeepRunning())
00083 {
00084 parser.deserializeIntoFlatContainer("joint_state", absl::Span<uint8_t>(buffer), &flat_container, 100);
00085 parser.applyNameTransform("joint_state", flat_container, &renamed_values );
00086 }
00087 }
00088
00089 BENCHMARK(BM_Joints);
00090
00091 BENCHMARK_MAIN();
00092