From f1d66aa5af9f3d123f6f376824c5cd2a72c7d656 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 9 Sep 2019 22:10:22 +0200 Subject: [PATCH] marker check (#1275): allow more fine-grained log levels - distinguish all ros::console::levels - issue rviz status warning for any message, but don't clutter console - use logger namespace to allow disabling console output --- src/rviz/default_plugin/marker_utils.cpp | 431 ++++++++++++----------- src/rviz/default_plugin/marker_utils.h | 26 -- 2 files changed, 217 insertions(+), 240 deletions(-) diff --git a/src/rviz/default_plugin/marker_utils.cpp b/src/rviz/default_plugin/marker_utils.cpp index f15dcf76fd..530fdc3b48 100644 --- a/src/rviz/default_plugin/marker_utils.cpp +++ b/src/rviz/default_plugin/marker_utils.cpp @@ -40,6 +40,7 @@ #include "rviz/display_context.h" #include "rviz/properties/property.h" #include "rviz/properties/property_tree_model.h" +#include "rviz/properties/status_property.h" #include "rviz/properties/status_list.h" #include "rviz/validate_quaternions.h" #include "rviz/validate_floats.h" @@ -83,6 +84,22 @@ MarkerBase* createMarker(int marker_type, MarkerDisplay* owner, DisplayContext* } } +namespace +{ +void addSeparatorIfRequired(std::stringstream& ss) +{ + if (ss.tellp() != 0) // check if string is not empty + { + ss << " \n"; + } +} + +void increaseLevel(::ros::console::levels::Level new_status, ::ros::console::levels::Level& current_status) +{ + if(new_status > current_status) + current_status = new_status; +} + template constexpr const char* fieldName(); template <> @@ -95,177 +112,17 @@ template <> constexpr const char* fieldName<::std_msgs::ColorRGBA>() { return "Color"; } template -void checkFloats(const T& t, std::stringstream& ss, StatusProperty::Level& level) +void checkFloats(const T& t, std::stringstream& ss, ::ros::console::levels::Level& level) { if (!validateFloats(t)) { addSeparatorIfRequired(ss); ss << fieldName() << " contains invalid floating point values (nans or infs)"; - increaseWarningLevel(StatusProperty::Error, level); - } -} - -bool checkMarkerMsg(const visualization_msgs::Marker& marker, MarkerDisplay* owner) -{ - if(marker.action != visualization_msgs::Marker::ADD) - return true; - - std::stringstream ss; - StatusProperty::Level level = StatusProperty::Ok; - checkFloats(marker.pose.position, ss, level); - - switch (marker.type) { - case visualization_msgs::Marker::ARROW: - checkQuaternion(marker, ss, level); - checkScale(marker, ss, level); - checkColor(marker, ss, level); - checkPointsArrow(marker, ss, level); - checkColorsEmpty(marker, ss, level); - checkTextEmpty(marker, ss, level); - checkMeshEmpty(marker, ss, level); - break; - - case visualization_msgs::Marker::CUBE: - case visualization_msgs::Marker::CYLINDER: - case visualization_msgs::Marker::SPHERE: - checkQuaternion(marker, ss, level); - checkScale(marker, ss, level); - checkColor(marker, ss, level); - checkPointsEmpty(marker, ss, level); - checkColorsEmpty(marker, ss, level); - checkTextEmpty(marker, ss, level); - checkMeshEmpty(marker, ss, level); - break; - - - case visualization_msgs::Marker::LINE_STRIP: - case visualization_msgs::Marker::LINE_LIST: - checkQuaternion(marker, ss, level); - checkScaleLineStripAndList(marker, ss, level); - checkPointsNotEmpty(marker, ss, level); - checkColors(marker, ss, level); - checkTextEmpty(marker, ss, level); - checkMeshEmpty(marker, ss, level); - break; - - case visualization_msgs::Marker::SPHERE_LIST: - case visualization_msgs::Marker::CUBE_LIST: - case visualization_msgs::Marker::TRIANGLE_LIST: - checkQuaternion(marker, ss, level); - checkScale(marker, ss, level); - checkPointsNotEmpty(marker, ss, level); - checkColors(marker, ss, level, marker.type == visualization_msgs::Marker::TRIANGLE_LIST ? 3 : 1); - checkTextEmpty(marker, ss, level); - checkMeshEmpty(marker, ss, level); - break; - - case visualization_msgs::Marker::POINTS: - checkScalePoints(marker, ss, level); - checkPointsNotEmpty(marker, ss, level); - checkColors(marker, ss, level); - checkTextEmpty(marker, ss, level); - checkMeshEmpty(marker, ss, level); - break; - - case visualization_msgs::Marker::TEXT_VIEW_FACING: - checkColor(marker, ss, level); - checkScaleText(marker, ss, level); - checkTextNotEmptyOrWhitespace(marker, ss, level); - checkPointsEmpty(marker, ss, level); - checkColorsEmpty(marker, ss, level); - checkMeshEmpty(marker, ss, level); - break; - - case visualization_msgs::Marker::MESH_RESOURCE: - checkQuaternion(marker, ss, level); - checkColor(marker, ss, level); - checkScale(marker, ss, level); - checkPointsEmpty(marker, ss, level); - checkColorsEmpty(marker, ss, level); - checkTextEmpty(marker, ss, level); - break; - - default: - ss << "Unknown marker type: " << marker.type << '.' ; - level = StatusProperty::Error; + increaseLevel(::ros::console::levels::Error, level); } - - if(ss.tellp() != 0) //stringstream is not empty - { - std::string warning = ss.str(); - - if (owner) - owner->setMarkerStatus(MarkerID(marker.ns, marker.id), level, warning); - ROS_LOG((level == StatusProperty::Warn ? ::ros::console::levels::Warn : ::ros::console::levels::Error), - ROSCONSOLE_DEFAULT_NAME, "Marker '%s/%d': %s", marker.ns.c_str(), marker.id, warning.c_str()); - - return level != StatusProperty::Error; - } - - if (owner) - owner->deleteMarkerStatus(MarkerID(marker.ns, marker.id)); - return true; } -bool checkMarkerArrayMsg(const visualization_msgs::MarkerArray& array, MarkerDisplay* owner) -{ - std::vector marker_ids; - marker_ids.reserve(array.markers.size()); - - StatusProperty::Level level = StatusProperty::Ok; - std::stringstream ss; - bool markers_added = false; - - for(const visualization_msgs::Marker& marker : array.markers) - { - if (marker.action == visualization_msgs::Marker::ADD) - markers_added = true; - - else if (marker.action == visualization_msgs::Marker::DELETEALL) - { - if (markers_added) - { - addSeparatorIfRequired(ss); - ss << "Found a DELETEALL after having markers added. These markers will never show"; - increaseWarningLevel(StatusProperty::Warn, level); - } - markers_added = false; - // marker_ids.clear(); - } - - MarkerID current_id(marker.ns, marker.id); - std::vector::iterator search = std::lower_bound(marker_ids.begin(), marker_ids.end(), current_id); - if (search != marker_ids.end() && *search == current_id) - { - addSeparatorIfRequired(ss); - ss << "Found '" << marker.ns.c_str() << "/" << marker.id << "' multiple times"; - increaseWarningLevel(StatusProperty::Warn, level); - } - else - { - marker_ids.insert(search, current_id); - } - } - - if(ss.tellp() != 0) //stringstream is not empty - { - std::string warning = ss.str(); - - if (owner) - owner->setStatusStd(level, "marker_array", warning); - ROS_LOG((level == StatusProperty::Warn ? ::ros::console::levels::Warn : ::ros::console::levels::Error), - ROSCONSOLE_DEFAULT_NAME, "MarkerArray: %s", warning.c_str()); - - return false; - } - - if (owner) - owner->deleteStatusStd("marker_array"); - return true; -} - - -void checkQuaternion(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level) +void checkQuaternion(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level) { checkFloats(marker.pose.orientation, ss, level); if (marker.pose.orientation.x == 0.0 && marker.pose.orientation.y == 0.0 && marker.pose.orientation.z == 0.0 && @@ -273,17 +130,17 @@ void checkQuaternion(const visualization_msgs::Marker& marker, std::stringstream { addSeparatorIfRequired(ss); ss << "Uninitialized quaternion, assuming identity."; - increaseWarningLevel(StatusProperty::Warn, level); + increaseLevel(::ros::console::levels::Info, level); } else if(!validateQuaternions(marker.pose)) { addSeparatorIfRequired(ss); ss << "Unnormalized quaternion in marker message."; - increaseWarningLevel(StatusProperty::Warn, level); + increaseLevel(::ros::console::levels::Warn, level); } } -void checkScale(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level) +void checkScale(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level) { checkFloats(marker.scale, ss, level); // for ARROW markers, scale.z is the optional head length @@ -294,88 +151,86 @@ void checkScale(const visualization_msgs::Marker& marker, std::stringstream& ss, { addSeparatorIfRequired(ss); ss << "Scale contains 0.0 in x, y or z."; - increaseWarningLevel(StatusProperty::Warn, level); + increaseLevel(::ros::console::levels::Warn, level); } } -void checkScaleLineStripAndList(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level) +void checkScaleLineStripAndList(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level) { if(marker.scale.x == 0.0) { addSeparatorIfRequired(ss); ss << "Width LINE_LIST or LINE_STRIP is 0.0 (scale.x)."; - increaseWarningLevel(StatusProperty::Warn, level); + increaseLevel(::ros::console::levels::Warn, level); } else if(marker.scale.y != 0.0 || marker.scale.z != 0.0) { addSeparatorIfRequired(ss); ss << "scale.y and scale.z of LINE_LIST or LINE_STRIP are ignored."; - increaseWarningLevel(StatusProperty::Warn, level); + increaseLevel(::ros::console::levels::Warn, level); } } -void checkScalePoints(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level) +void checkScalePoints(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level) { if(marker.scale.x == 0.0 || marker.scale.y == 0.0) { addSeparatorIfRequired(ss); ss << "Width and/or height of POINTS is 0.0 (scale.x, scale.y)."; - increaseWarningLevel(StatusProperty::Warn, level); + increaseLevel(::ros::console::levels::Warn, level); } else if(marker.scale.z != 0.0) { addSeparatorIfRequired(ss); ss << "scale.z of POINTS is ignored."; - increaseWarningLevel(StatusProperty::Warn, level); + increaseLevel(::ros::console::levels::Warn, level); } } -void checkScaleText(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level) +void checkScaleText(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level) { if(marker.scale.z == 0.0) { addSeparatorIfRequired(ss); ss << "Text height of TEXT_VIEW_FACING is 0.0 (scale.z)."; - increaseWarningLevel(StatusProperty::Warn, level); + increaseLevel(::ros::console::levels::Warn, level); } -#if 0 // There is too much code out there, which sets all 3 axis scales else if(marker.scale.x != 0.0 || marker.scale.y != 0.0) { addSeparatorIfRequired(ss); ss << "scale.x and scale.y of TEXT_VIEW_FACING are ignored."; - increaseWarningLevel(StatusProperty::Warn, level); + increaseLevel(::ros::console::levels::Debug, level); } -#endif } -void checkColor(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level) +void checkColor(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level) { checkFloats(marker.color, ss, level); if(marker.color.a == 0.0) { addSeparatorIfRequired(ss); ss << "Marker is fully transparent (color.a is 0.0)."; - increaseWarningLevel(StatusProperty::Warn, level); + increaseLevel(::ros::console::levels::Info, level); } } -void checkPointsArrow(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level) +void checkPointsArrow(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level) { if(marker.points.size() != 0 && marker.points.size() != 2) { addSeparatorIfRequired(ss); ss << "Number of points for an ARROW marker should be either 0 or 2."; - increaseWarningLevel(StatusProperty::Error, level); + increaseLevel(::ros::console::levels::Error, level); } } -void checkPointsNotEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level) +void checkPointsNotEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level) { if(marker.points.empty()) { addSeparatorIfRequired(ss); ss << "Points should not be empty for specified marker type."; - increaseWarningLevel(StatusProperty::Error, level); + increaseLevel(::ros::console::levels::Error, level); } switch (marker.type) { @@ -384,7 +239,7 @@ void checkPointsNotEmpty(const visualization_msgs::Marker& marker, std::stringst { addSeparatorIfRequired(ss); ss << "Number of points should be a multiple of 3 for TRIANGLE_LIST Marker."; - increaseWarningLevel(StatusProperty::Error, level); + increaseLevel(::ros::console::levels::Error, level); } break; case visualization_msgs::Marker::LINE_LIST: @@ -392,7 +247,7 @@ void checkPointsNotEmpty(const visualization_msgs::Marker& marker, std::stringst { addSeparatorIfRequired(ss); ss << "Number of points should be a multiple of 2 for LINE_LIST Marker."; - increaseWarningLevel(StatusProperty::Error, level); + increaseLevel(::ros::console::levels::Error, level); } break; case visualization_msgs::Marker::LINE_STRIP: @@ -400,7 +255,7 @@ void checkPointsNotEmpty(const visualization_msgs::Marker& marker, std::stringst { addSeparatorIfRequired(ss); ss << "At least two points are required for a LINE_STRIP Marker."; - increaseWarningLevel(StatusProperty::Error, level); + increaseLevel(::ros::console::levels::Error, level); } break; default: @@ -408,17 +263,17 @@ void checkPointsNotEmpty(const visualization_msgs::Marker& marker, std::stringst } } -void checkPointsEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level) +void checkPointsEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level) { if(!marker.points.empty()) { addSeparatorIfRequired(ss); - ss << "Points array is ignored by specified marker type."; - increaseWarningLevel(StatusProperty::Warn, level); + ss << "Non-empty points array is ignored."; + increaseLevel(::ros::console::levels::Warn, level); } } -void checkColors(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level, unsigned int multiple) +void checkColors(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level, unsigned int multiple) { if(marker.colors.size() == 0) { @@ -429,77 +284,225 @@ void checkColors(const visualization_msgs::Marker& marker, std::stringstream& ss { addSeparatorIfRequired(ss); ss << "Number of colors doesn't match number of points."; - increaseWarningLevel(StatusProperty::Error, level); + increaseLevel(::ros::console::levels::Error, level); } } -void checkColorsEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level) +void checkColorsEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level) { if(!marker.colors.empty()) { addSeparatorIfRequired(ss); - ss << "Colors array is ignored by specified marker type."; - increaseWarningLevel(StatusProperty::Warn, level); + ss << "Non-empty colors array is ignored."; + increaseLevel(::ros::console::levels::Warn, level); } } -void checkTextNotEmptyOrWhitespace(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level) +void checkTextNotEmptyOrWhitespace(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level) { if(marker.text.find_first_not_of(" \t\n\v\f\r") == std::string::npos) { addSeparatorIfRequired(ss); ss << "Text is empty or only consists whitespace."; - increaseWarningLevel(StatusProperty::Error, level); + increaseLevel(::ros::console::levels::Error, level); } } -void checkTextEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level) +void checkTextEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level) { if(!marker.text.empty()) { addSeparatorIfRequired(ss); - ss << "Text is ignored for specified marker type."; - increaseWarningLevel(StatusProperty::Warn, level); + ss << "Non-empty text is ignored."; + increaseLevel(::ros::console::levels::Info, level); } } -void checkMesh(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level) +void checkMesh(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level) { if(marker.mesh_resource.empty()) { addSeparatorIfRequired(ss); - ss << "Path to mesh resource is empty for MESH_RESOURCE marker."; - increaseWarningLevel(StatusProperty::Error, level); + ss << "Empty mesh_resource path."; + increaseLevel(::ros::console::levels::Error, level); } } -void checkMeshEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level) +void checkMeshEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, ::ros::console::levels::Level& level) { if (!marker.mesh_resource.empty()) { addSeparatorIfRequired(ss); - ss << "Mesh_resource is ignored for specified marker type."; - increaseWarningLevel(StatusProperty::Warn, level); + ss << "Non-empty mesh_resource is ignored."; + increaseLevel(::ros::console::levels::Info, level); } if (marker.mesh_use_embedded_materials) { addSeparatorIfRequired(ss); - ss << "Using embedded materials is not supported for markers other than MESH_RESOURCE."; - increaseWarningLevel(StatusProperty::Warn, level); + ss << "mesh_use_embedded_materials is ignored."; + increaseLevel(::ros::console::levels::Info, level); } } -void addSeparatorIfRequired(std::stringstream& ss) +} + +bool checkMarkerMsg(const visualization_msgs::Marker& marker, MarkerDisplay* owner) { - if (ss.tellp() != 0) // check if string is not empty + if(marker.action != visualization_msgs::Marker::ADD) + return true; + + std::stringstream ss; + ::ros::console::levels::Level level = ::ros::console::levels::Debug; + checkFloats(marker.pose.position, ss, level); + + switch (marker.type) { + case visualization_msgs::Marker::ARROW: + checkQuaternion(marker, ss, level); + checkScale(marker, ss, level); + checkColor(marker, ss, level); + checkPointsArrow(marker, ss, level); + checkColorsEmpty(marker, ss, level); + checkTextEmpty(marker, ss, level); + checkMeshEmpty(marker, ss, level); + break; + + case visualization_msgs::Marker::CUBE: + case visualization_msgs::Marker::CYLINDER: + case visualization_msgs::Marker::SPHERE: + checkQuaternion(marker, ss, level); + checkScale(marker, ss, level); + checkColor(marker, ss, level); + checkPointsEmpty(marker, ss, level); + checkColorsEmpty(marker, ss, level); + checkTextEmpty(marker, ss, level); + checkMeshEmpty(marker, ss, level); + break; + + + case visualization_msgs::Marker::LINE_STRIP: + case visualization_msgs::Marker::LINE_LIST: + checkQuaternion(marker, ss, level); + checkScaleLineStripAndList(marker, ss, level); + checkPointsNotEmpty(marker, ss, level); + checkColors(marker, ss, level, 1); + checkTextEmpty(marker, ss, level); + checkMeshEmpty(marker, ss, level); + break; + + case visualization_msgs::Marker::SPHERE_LIST: + case visualization_msgs::Marker::CUBE_LIST: + case visualization_msgs::Marker::TRIANGLE_LIST: + checkQuaternion(marker, ss, level); + checkScale(marker, ss, level); + checkPointsNotEmpty(marker, ss, level); + checkColors(marker, ss, level, marker.type == visualization_msgs::Marker::TRIANGLE_LIST ? 3 : 1); + checkTextEmpty(marker, ss, level); + checkMeshEmpty(marker, ss, level); + break; + + case visualization_msgs::Marker::POINTS: + checkScalePoints(marker, ss, level); + checkPointsNotEmpty(marker, ss, level); + checkColors(marker, ss, level, 1); + checkTextEmpty(marker, ss, level); + checkMeshEmpty(marker, ss, level); + break; + + case visualization_msgs::Marker::TEXT_VIEW_FACING: + checkColor(marker, ss, level); + checkScaleText(marker, ss, level); + checkTextNotEmptyOrWhitespace(marker, ss, level); + checkPointsEmpty(marker, ss, level); + checkColorsEmpty(marker, ss, level); + checkMeshEmpty(marker, ss, level); + break; + + case visualization_msgs::Marker::MESH_RESOURCE: + checkQuaternion(marker, ss, level); + checkColor(marker, ss, level); + checkScale(marker, ss, level); + checkPointsEmpty(marker, ss, level); + checkColorsEmpty(marker, ss, level); + checkTextEmpty(marker, ss, level); + break; + + default: + ss << "Unknown marker type: " << marker.type << '.' ; + level = ::ros::console::levels::Error; + } + + if(ss.tellp() != 0) //stringstream is not empty { - ss << " \n"; + std::string warning = ss.str(); + + bool fatal = level >= ::ros::console::levels::Error; + ROS_LOG(level, ROSCONSOLE_DEFAULT_NAME ".marker", "Marker '%s/%d': %s", marker.ns.c_str(), marker.id, warning.c_str()); + if (owner) + owner->setMarkerStatus(MarkerID(marker.ns, marker.id), fatal ? StatusProperty::Error : StatusProperty::Warn, warning); + + return !fatal; } + + if (owner) + owner->deleteMarkerStatus(MarkerID(marker.ns, marker.id)); + return true; } -void increaseWarningLevel(StatusProperty::Level new_status, StatusProperty::Level& current_status) +bool checkMarkerArrayMsg(const visualization_msgs::MarkerArray& array, MarkerDisplay* owner) { - if(new_status > current_status) - current_status = new_status; + std::vector marker_ids; + marker_ids.reserve(array.markers.size()); + + ::ros::console::levels::Level level = ::ros::console::levels::Debug; + std::stringstream ss; + bool markers_added = false; + + for(const visualization_msgs::Marker& marker : array.markers) + { + if (marker.action == visualization_msgs::Marker::ADD) + markers_added = true; + + else if (marker.action == visualization_msgs::Marker::DELETEALL) + { + if (markers_added) + { + addSeparatorIfRequired(ss); + ss << "Found a DELETEALL after having markers added. These markers will never show"; + increaseLevel(::ros::console::levels::Warn, level); + } + markers_added = false; + // marker_ids.clear(); + } + + MarkerID current_id(marker.ns, marker.id); + std::vector::iterator search = std::lower_bound(marker_ids.begin(), marker_ids.end(), current_id); + if (search != marker_ids.end() && *search == current_id) + { + addSeparatorIfRequired(ss); + ss << "Found '" << marker.ns.c_str() << "/" << marker.id << "' multiple times"; + increaseLevel(::ros::console::levels::Warn, level); + } + else + { + marker_ids.insert(search, current_id); + } + } + + if(ss.tellp() != 0) //stringstream is not empty + { + std::string warning = ss.str(); + + bool fatal = level >= ::ros::console::levels::Error; + ROS_LOG(level, ROSCONSOLE_DEFAULT_NAME ".marker", "MarkerArray: %s", warning.c_str()); + if (owner) + owner->setStatusStd(fatal ? StatusProperty::Error : StatusProperty::Warn, "marker_array", warning); + + return !fatal; + } + + if (owner) + owner->deleteStatusStd("marker_array"); + return true; } + } // namespace rviz diff --git a/src/rviz/default_plugin/marker_utils.h b/src/rviz/default_plugin/marker_utils.h index 284afd00b3..b12175c3ef 100644 --- a/src/rviz/default_plugin/marker_utils.h +++ b/src/rviz/default_plugin/marker_utils.h @@ -33,7 +33,6 @@ #include #include -#include "rviz/properties/status_property.h" namespace Ogre { @@ -55,31 +54,6 @@ MarkerBase* createMarker(int marker_type, MarkerDisplay *owner, DisplayContext * bool checkMarkerMsg(const visualization_msgs::Marker& marker, MarkerDisplay* owner); bool checkMarkerArrayMsg(const visualization_msgs::MarkerArray& array, MarkerDisplay* owner); -void checkQuaternion(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level); - -void checkScale(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level); -void checkScaleLineStripAndList(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level); -void checkScalePoints(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level); -void checkScaleText(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level); - -void checkColor(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level); - - -void checkPointsArrow(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level); -void checkPointsNotEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level); -void checkPointsEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level); - -void checkColors(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level, unsigned int multiple = 1); -void checkColorsEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level); - -void checkTextNotEmptyOrWhitespace(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level); -void checkTextEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level); - -void checkMesh(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level); -void checkMeshEmpty(const visualization_msgs::Marker& marker, std::stringstream& ss, StatusProperty::Level& level); - -void addSeparatorIfRequired(std::stringstream& ss); -void increaseWarningLevel(StatusProperty::Level new_status, StatusProperty::Level& current_status); } #endif