Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef OBSTACLE_DISTANCE_DATA_TYPES_HPP_
00019 #define OBSTACLE_DISTANCE_DATA_TYPES_HPP_
00020
00021 #include <ros/ros.h>
00022 #include <stdint.h>
00023 #include <unordered_map>
00024 #include <shape_msgs/SolidPrimitive.h>
00025 #include <visualization_msgs/Marker.h>
00026
00027 #define FCL_BOX_X 0u
00028 #define FCL_BOX_Y 1u
00029 #define FCL_BOX_Z 2u
00030
00031 #define FCL_RADIUS 0u
00032 #define FCL_CYL_LENGTH 1u
00033
00034 #define MIN_DISTANCE 0.5 // [m]: filter for distances to be published!
00035
00036 #define DEFAULT_COL_ALPHA 0.6 // MoveIt! CollisionGeometry does not provide color -> Therefore use default value. 0.5 = Test for taking pictures -> robot arm should be visible behind obstacle
00037
00038 struct ShapeMsgTypeToVisMarkerType
00039 {
00040 public:
00041 std::unordered_map<uint8_t, uint32_t> map_;
00042 std_msgs::ColorRGBA obstacle_color_;
00043
00044 ShapeMsgTypeToVisMarkerType()
00045 {
00046 map_[shape_msgs::SolidPrimitive::BOX] = visualization_msgs::Marker::CUBE;
00047 map_[shape_msgs::SolidPrimitive::SPHERE] = visualization_msgs::Marker::SPHERE;
00048 map_[shape_msgs::SolidPrimitive::CYLINDER] = visualization_msgs::Marker::CYLINDER;
00049
00050 obstacle_color_.r = 1.0;
00051 obstacle_color_.g = 0.0;
00052 obstacle_color_.b = 0.0;
00053 obstacle_color_.a = DEFAULT_COL_ALPHA;
00054 }
00055 };
00056
00057
00058 struct TriangleSupport
00059 {
00060 fcl::Vec3f a;
00061 fcl::Vec3f b;
00062 fcl::Vec3f c;
00063 };
00064
00065 static ShapeMsgTypeToVisMarkerType g_shapeMsgTypeToVisMarkerType;
00066
00067 #endif