Go to the documentation of this file.00001 #include <jsk_interactive_marker/yaml_menu_handler.h>
00002 #include <fstream>
00003
00004 using namespace jsk_interactive_marker;
00005 YamlMenuHandler::YamlMenuHandler(ros::NodeHandle* node_ptr, std::string file_name) {
00006 ROS_INFO("hoge %s ", file_name.c_str());
00007 _node_ptr = node_ptr;
00008 initMenu(file_name);
00009 }
00010
00011 bool YamlMenuHandler::initMenu(std::string file_name) {
00012 YAML::Node doc;
00013 #ifdef USE_OLD_YAML
00014 std::ifstream fin(file_name.c_str());
00015 ROS_INFO("opening yaml file %s", file_name.c_str());
00016 if (!fin.good()){
00017 ROS_INFO("Unable to open yaml file %s", file_name.c_str());
00018 return false;
00019 }
00020 YAML::Parser parser(fin);
00021 if (!parser) {
00022 ROS_INFO("Unable to create YAML parser for marker_set");
00023 return false;
00024 }
00025 parser.GetNextDocument(doc);
00026 #else
00027
00028 ROS_INFO("opening yaml file with new yaml-cpp %s", file_name.c_str());
00029 if ( !(access(file_name.c_str(), F_OK) != -1)) {
00030 ROS_INFO("file not exists :%s", file_name.c_str());
00031 return false;
00032 }
00033 doc = YAML::LoadFile(file_name);
00034 #endif
00035 for (int i=0; i<doc.size() ;i++) {
00036 std::string text, topic_name;
00037 const YAML::Node& single_menu = doc[i];
00038 #ifdef USE_OLD_YAML
00039 single_menu["text"] >> text;
00040 single_menu["topic"] >> topic_name;
00041 #else
00042 text = single_menu["text"].as<std::string>();
00043 topic_name = single_menu["topic"].as<std::string>();
00044 #endif
00045 ROS_INFO("Regist %s, %s", text.c_str(), topic_name.c_str());
00046 _publisher_map[topic_name] = _node_ptr->advertise<std_msgs::String>(topic_name, 1);
00047 _menu_handler.insert(text, boost::bind(&YamlMenuHandler::pubTopic, this, _1, topic_name));
00048 }
00049 }
00050
00051 void YamlMenuHandler::pubTopic(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, std::string topic_name) {
00052 std_msgs::String text_msg;
00053 text_msg.data = feedback->marker_name;
00054 if (_publisher_map.find(topic_name)==_publisher_map.end()) {
00055 return;
00056 }
00057 else {
00058 _publisher_map[topic_name].publish(text_msg);
00059 }
00060 }
00061
00062 void YamlMenuHandler::applyMenu(interactive_markers::InteractiveMarkerServer* server_ptr, std::string name) {
00063 _menu_handler.apply(*server_ptr, name);
00064 }