Go to the documentation of this file.00001 #include "ros_type_introspection/ros_introspection.hpp"
00002 #include <ros/ros.h>
00003 #include <topic_tools/shape_shifter.h>
00004
00005 using namespace RosIntrospection;
00006
00007 void topicCallback(const topic_tools::ShapeShifter::ConstPtr& msg,
00008 const std::string &topic_name,
00009 RosIntrospection::Parser& parser)
00010 {
00011
00012 const std::string& datatype = msg->getDataType();
00013 const std::string& definition = msg->getMessageDefinition();
00014
00015
00016 parser.registerMessageDefinition( topic_name,
00017 RosIntrospection::ROSType(datatype),
00018 definition );
00019
00020
00021 static std::vector<uint8_t> buffer;
00022 static std::map<std::string,FlatMessage> flat_containers;
00023 static std::map<std::string,RenamedValues> renamed_vectors;
00024
00025 FlatMessage& flat_container = flat_containers[topic_name];
00026 RenamedValues& renamed_values = renamed_vectors[topic_name];
00027
00028
00029 buffer.resize( msg->size() );
00030 ros::serialization::OStream stream(buffer.data(), buffer.size());
00031 msg->write(stream);
00032
00033
00034
00035 parser.deserializeIntoFlatContainer( topic_name, absl::Span<uint8_t>(buffer), &flat_container, 100);
00036 parser.applyNameTransform( topic_name, flat_container, &renamed_values );
00037
00038
00039 printf("--------- %s ----------\n", topic_name.c_str() );
00040 for (auto it: renamed_values)
00041 {
00042 const std::string& key = it.first;
00043 const Variant& value = it.second;
00044 printf(" %s = %f\n", key.c_str(), value.convert<double>() );
00045 }
00046 for (auto it: flat_container.name)
00047 {
00048 const std::string& key = it.first.toStdString();
00049 const std::string& value = it.second;
00050 printf(" %s = %s\n", key.c_str(), value.c_str() );
00051 }
00052 }
00053
00054
00055
00056 int main(int argc, char** argv)
00057 {
00058 Parser parser;
00059
00060 if( argc == 1 ){
00061 printf("Usage: rosbag_example list_of_topics_names\n");
00062 return 1;
00063 }
00064
00065 ros::init(argc, argv, "universal_subscriber");
00066
00067 std::set<std::string> topic_names;
00068
00069 for (int i=1; i< argc; i++)
00070 {
00071 topic_names.insert( argv[i] );
00072 }
00073 ros::master::V_TopicInfo advertized_topics;
00074 ros::master::getTopics(advertized_topics);
00075
00076 for (const auto& topic_info: advertized_topics)
00077 {
00078 printf("advertized: %s\n", topic_info.name.c_str());
00079 }
00080
00081
00082 for (const std::string& topic_name: topic_names)
00083 {
00084 bool found = false;
00085 for (const auto& topic_info: advertized_topics)
00086 {
00087 if( topic_info.name == topic_name)
00088 {
00089 found = true;
00090 break;
00091 }
00092 }
00093 if( !found )
00094 {
00095 printf("This topic has not been published yet: %s\n", topic_name.c_str() );
00096 return 1;
00097 }
00098 }
00099
00100 ros::NodeHandle nh;
00101
00102
00103 std::vector<ros::Subscriber> subscribers;
00104
00105 for (const std::string& topic_name: topic_names)
00106 {
00107
00108 boost::function<void(const topic_tools::ShapeShifter::ConstPtr&) > callback;
00109 callback = [&parser, topic_name](const topic_tools::ShapeShifter::ConstPtr& msg) -> void
00110 {
00111 topicCallback(msg, topic_name, parser) ;
00112 };
00113 subscribers.push_back( nh.subscribe(topic_name, 10, callback) );
00114 }
00115
00116 ros::spin();
00117
00118 return 0;
00119 }