GridMapVisualization.cpp
Go to the documentation of this file.
00001 /*
00002  * GridMapVisualization.cpp
00003  *
00004  *  Created on: Nov 19, 2013
00005  *      Author: Péter Fankhauser
00006  *       Institute: ETH Zurich, ANYbotics
00007  */
00008 
00009 #include "grid_map_visualization/GridMapVisualization.hpp"
00010 #include <grid_map_core/GridMap.hpp>
00011 #include <grid_map_ros/GridMapRosConverter.hpp>
00012 
00013 using namespace std;
00014 using namespace ros;
00015 
00016 namespace grid_map_visualization {
00017 
00018 GridMapVisualization::GridMapVisualization(ros::NodeHandle& nodeHandle,
00019                                            const std::string& parameterName)
00020     : nodeHandle_(nodeHandle),
00021       visualizationsParameter_(parameterName),
00022       factory_(nodeHandle_),
00023       isSubscribed_(false)
00024 {
00025   ROS_INFO("Grid map visualization node started.");
00026   readParameters();
00027   activityCheckTimer_ = nodeHandle_.createTimer(activityCheckDuration_,
00028                                                 &GridMapVisualization::updateSubscriptionCallback,
00029                                                 this);
00030   initialize();
00031 }
00032 
00033 GridMapVisualization::~GridMapVisualization()
00034 {
00035 }
00036 
00037 bool GridMapVisualization::readParameters()
00038 {
00039   nodeHandle_.param("grid_map_topic", mapTopic_, string("/grid_map"));
00040 
00041   double activityCheckRate;
00042   nodeHandle_.param("activity_check_rate", activityCheckRate, 2.0);
00043   activityCheckDuration_.fromSec(1.0 / activityCheckRate);
00044   ROS_ASSERT(!activityCheckDuration_.isZero());
00045 
00046   // Configure the visualizations from a configuration stored on the parameter server.
00047   XmlRpc::XmlRpcValue config;
00048   if (!nodeHandle_.getParam(visualizationsParameter_, config)) {
00049     ROS_WARN(
00050         "Could not load the visualizations configuration from parameter %s,are you sure it"
00051         "was pushed to the parameter server? Assuming that you meant to leave it empty.",
00052         visualizationsParameter_.c_str());
00053     return false;
00054   }
00055 
00056   // Verify proper naming and structure,
00057   if (config.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00058     ROS_ERROR("%s: The visualization specification must be a list, but it is of XmlRpcType %d",
00059               visualizationsParameter_.c_str(), config.getType());
00060     ROS_ERROR("The XML passed in is formatted as follows:\n %s", config.toXml().c_str());
00061     return false;
00062   }
00063 
00064   // Iterate over all visualizations (may be just one),
00065   for (unsigned int i = 0; i < config.size(); ++i) {
00066     if (config[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
00067       ROS_ERROR("%s: Visualizations must be specified as maps, but they are XmlRpcType:%d",
00068                 visualizationsParameter_.c_str(), config[i].getType());
00069       return false;
00070     } else if (!config[i].hasMember("type")) {
00071       ROS_ERROR("%s: Could not add a visualization because no type was given",
00072                 visualizationsParameter_.c_str());
00073       return false;
00074     } else if (!config[i].hasMember("name")) {
00075       ROS_ERROR("%s: Could not add a visualization because no name was given",
00076                 visualizationsParameter_.c_str());
00077       return false;
00078     } else {
00079       //Check for name collisions within the list itself.
00080       for (int j = i + 1; j < config.size(); ++j) {
00081         if (config[j].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
00082           ROS_ERROR("%s: Visualizations must be specified as maps, but they are XmlRpcType:%d",
00083                     visualizationsParameter_.c_str(), config[j].getType());
00084           return false;
00085         }
00086 
00087         if (!config[j].hasMember("name")
00088             || config[i]["name"].getType() != XmlRpc::XmlRpcValue::TypeString
00089             || config[j]["name"].getType() != XmlRpc::XmlRpcValue::TypeString) {
00090           ROS_ERROR("%s: Visualizations names must be strings, but they are XmlRpcTypes:%d and %d",
00091                     visualizationsParameter_.c_str(), config[i].getType(), config[j].getType());
00092           return false;
00093         }
00094 
00095         std::string namei = config[i]["name"];
00096         std::string namej = config[j]["name"];
00097         if (namei == namej) {
00098           ROS_ERROR("%s: A visualization with the name '%s' already exists.",
00099                     visualizationsParameter_.c_str(), namei.c_str());
00100           return false;
00101         }
00102       }
00103     }
00104 
00105     // Make sure the visualization has a valid type.
00106     if (!factory_.isValidType(config[i]["type"])) {
00107       ROS_ERROR("Could not find visualization of type '%s'.", std::string(config[i]["type"]).c_str());
00108       return false;
00109     }
00110   }
00111 
00112   for (int i = 0; i < config.size(); ++i) {
00113     std::string type = config[i]["type"];
00114     std::string name = config[i]["name"];
00115     auto visualization = factory_.getInstance(type, name);
00116     visualization->readParameters(config[i]);
00117     visualizations_.push_back(visualization);
00118     ROS_INFO("%s: Configured visualization of type '%s' with name '%s'.",
00119              visualizationsParameter_.c_str(), type.c_str(), name.c_str());
00120   }
00121 
00122   return true;
00123 }
00124 
00125 bool GridMapVisualization::initialize()
00126 {
00127   for (auto& visualization : visualizations_) {
00128     visualization->initialize();
00129   }
00130   updateSubscriptionCallback(ros::TimerEvent());
00131   ROS_INFO("Grid map visualization initialized.");
00132   return true;
00133 }
00134 
00135 void GridMapVisualization::updateSubscriptionCallback(const ros::TimerEvent&)
00136 {
00137   bool isActive = false;
00138 
00139   for (auto& visualization : visualizations_) {
00140     if (visualization->isActive()) {
00141       isActive = true;
00142       break;
00143     }
00144   }
00145 
00146   if (!isSubscribed_ && isActive) {
00147     mapSubscriber_ = nodeHandle_.subscribe(mapTopic_, 1, &GridMapVisualization::callback, this);
00148     isSubscribed_ = true;
00149     ROS_DEBUG("Subscribed to grid map at '%s'.", mapTopic_.c_str());
00150   }
00151   if (isSubscribed_ && !isActive) {
00152     mapSubscriber_.shutdown();
00153     isSubscribed_ = false;
00154     ROS_DEBUG("Cancelled subscription to grid map.");
00155   }
00156 }
00157 
00158 void GridMapVisualization::callback(const grid_map_msgs::GridMap& message)
00159 {
00160   ROS_DEBUG("Grid map visualization received a map (timestamp %f) for visualization.",
00161             message.info.header.stamp.toSec());
00162   grid_map::GridMap map;
00163   grid_map::GridMapRosConverter::fromMessage(message, map);
00164 
00165   for (auto& visualization : visualizations_) {
00166     visualization->visualize(map);
00167   }
00168 }
00169 
00170 } /* namespace */


grid_map_visualization
Author(s): Péter Fankhauser
autogenerated on Tue Jul 9 2019 05:06:50