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
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef MOVEIT_COLLISION_DETECTION_COLLISION_TOOLS_
00038 #define MOVEIT_COLLISION_DETECTION_COLLISION_TOOLS_
00039
00040 #include <moveit/collision_detection/collision_common.h>
00041 #include <moveit_msgs/CostSource.h>
00042 #include <moveit_msgs/ContactInformation.h>
00043 #include <visualization_msgs/MarkerArray.h>
00044
00045 namespace collision_detection
00046 {
00047
00048 void getCollisionMarkersFromContacts(visualization_msgs::MarkerArray& arr,
00049 const std::string& frame_id,
00050 const CollisionResult::ContactMap &con,
00051 const std_msgs::ColorRGBA& color,
00052 const ros::Duration& lifetime,
00053 const double radius = 0.035);
00054
00055 void getCollisionMarkersFromContacts(visualization_msgs::MarkerArray& arr,
00056 const std::string& frame_id,
00057 const CollisionResult::ContactMap &con);
00058
00059
00061 void getCostMarkers(visualization_msgs::MarkerArray& arr,
00062 const std::string& frame_id,
00063 std::set<CostSource> &cost_sources);
00064
00065 void getCostMarkers(visualization_msgs::MarkerArray& arr,
00066 const std::string& frame_id,
00067 std::set<CostSource> &cost_sources,
00068 const std_msgs::ColorRGBA& color,
00069 const ros::Duration& lifetime);
00070
00071 double getTotalCost(const std::set<CostSource> &cost_sources);
00072
00073 void removeCostSources(std::set<CostSource> &cost_sources, const std::set<CostSource> &cost_sources_to_remove, double overlap_fraction);
00074 void intersectCostSources(std::set<CostSource> &cost_sources, const std::set<CostSource> &a, const std::set<CostSource> &b);
00075 void removeOverlapping(std::set<CostSource> &cost_sources, double overlap_fraction);
00076
00077
00078 bool getSensorPositioning(geometry_msgs::Point &point,
00079 const std::set<CostSource> &cost_sources);
00080
00081 void costSourceToMsg(const CostSource &cost_source, moveit_msgs::CostSource &msg);
00082 void contactToMsg(const Contact& contact, moveit_msgs::ContactInformation &msg);
00083
00084 }
00085
00086 #endif