00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef MARKER_SHAPES_HPP_
00019 #define MARKER_SHAPES_HPP_
00020
00021 #include <ros/ros.h>
00022 #include <visualization_msgs/Marker.h>
00023 #include <shape_msgs/Mesh.h>
00024 #include <shape_msgs/MeshTriangle.h>
00025
00026 #include "fcl/shape/geometric_shapes.h"
00027 #include "fcl/collision_object.h"
00028
00029 #include "cob_obstacle_distance/fcl_marker_converter.hpp"
00030 #include "cob_obstacle_distance/marker_shapes/marker_shapes_interface.hpp"
00031
00032 #include <fcl/distance.h>
00033 #include <fcl/collision_data.h>
00034
00035 static const std::string g_marker_namespace = "collision_object";
00036
00037
00039 template
00040 <typename T>
00041 class MarkerShape : public IMarkerShape
00042 {
00043 private:
00044 FclMarkerConverter<T> fcl_marker_converter_;
00045 boost::shared_ptr<BVH_RSS_t> ptr_fcl_bvh_;
00046
00047 void init(const std::string& root_frame, double x, double y, double z,
00048 double quat_x, double quat_y, double quat_z, double quat_w,
00049 double color_r, double color_g, double color_b, double color_a);
00050
00051 public:
00052 MarkerShape(const std::string& root_frame, T& fcl_object, const geometry_msgs::Pose& pose, const std_msgs::ColorRGBA& col)
00053 : MarkerShape(root_frame, fcl_object,
00054 pose.position.x, pose.position.y, pose.position.z,
00055 pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w,
00056 col.r, col.g, col.b, col.a)
00057 {
00058 }
00059
00060 MarkerShape(const std::string& root_frame, T& fcl_object, const geometry_msgs::Point& pos, const geometry_msgs::Quaternion& quat, const std_msgs::ColorRGBA& col)
00061 : MarkerShape(root_frame, fcl_object, pos.x, pos.y, pos.z, quat.x, quat.y, quat.z, quat.w, col.r, col.g, col.b, col.a)
00062 {
00063 }
00064
00065 MarkerShape(const std::string& root_frame, T& fcl_object,
00066 double x, double y, double z,
00067 double quat_x = 0.0, double quat_y = 0.0, double quat_z = 0.0, double quat_w = 1.0,
00068 double color_r = 0.0, double color_g = 0.0, double color_b = 0.0, double color_a = 1.0);
00069
00070 MarkerShape(const std::string& root_frame, double x, double y, double z,
00071 double quat_x = 0.0, double quat_y = 0.0, double quat_z = 0.0, double quat_w = 1.0,
00072 double color_r = 0.0, double color_g = 0.0, double color_b = 0.0, double color_a = 1.0);
00073
00074 inline geometry_msgs::Pose getMarkerPose() const;
00075
00076 inline geometry_msgs::Pose getOriginRelToFrame() const;
00077
00081 inline uint32_t getId() const;
00082
00083 inline void setColor(double color_r, double color_g, double color_b, double color_a = 1.0);
00084
00088 inline visualization_msgs::Marker getMarker();
00089
00090 inline void updatePose(const geometry_msgs::Vector3& pos, const geometry_msgs::Quaternion& quat);
00091
00092 inline void updatePose(const geometry_msgs::Pose& pose);
00093
00097 fcl::CollisionObject getCollisionObject() const;
00098
00099 virtual ~MarkerShape(){}
00100 };
00101
00102
00103
00104
00105
00106 template <>
00107 class MarkerShape<BVH_RSS_t> : public IMarkerShape
00108 {
00109 private:
00110 boost::shared_ptr<BVH_RSS_t> ptr_fcl_bvh_;
00111
00112 void init(const std::string& root_frame, const std::string& mesh_resource, double x, double y, double z,
00113 double quat_x, double quat_y, double quat_z, double quat_w,
00114 double color_r, double color_g, double color_b, double color_a);
00115
00116 public:
00117 MarkerShape(const std::string& root_frame, const shape_msgs::Mesh& mesh, const geometry_msgs::Pose& pose, const std_msgs::ColorRGBA& col);
00118
00119 MarkerShape(const std::string& root_frame, const std::string& mesh_resource, const geometry_msgs::Pose& pose, const std_msgs::ColorRGBA& col)
00120 : MarkerShape(root_frame, mesh_resource,
00121 pose.position.x, pose.position.y, pose.position.z,
00122 pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w,
00123 col.r, col.g, col.b, col.a)
00124 {
00125 }
00126
00127 MarkerShape(const std::string& root_frame, const std::string& mesh_resource, const geometry_msgs::Point& pos, const geometry_msgs::Quaternion& quat, const std_msgs::ColorRGBA& col)
00128 : MarkerShape(root_frame, mesh_resource, pos.x, pos.y, pos.z, quat.x, quat.y, quat.z, quat.w, col.r, col.g, col.b, col.a)
00129 {
00130 }
00131
00132 MarkerShape(const std::string& root_frame, const std::string& mesh_resource,
00133 double x, double y, double z,
00134 double quat_x = 0.0, double quat_y = 0.0, double quat_z = 0.0, double quat_w = 1.0,
00135 double color_r = 0.0, double color_g = 0.0, double color_b = 0.0, double color_a = 1.0);
00136
00137 inline geometry_msgs::Pose getMarkerPose() const;
00138
00139 inline geometry_msgs::Pose getOriginRelToFrame() const;
00140
00144 inline uint32_t getId() const;
00145
00146 inline void setColor(double color_r, double color_g, double color_b, double color_a = 1.0);
00147
00151 inline visualization_msgs::Marker getMarker();
00152
00153 inline void updatePose(const geometry_msgs::Vector3& pos, const geometry_msgs::Quaternion& quat);
00154
00155 inline void updatePose(const geometry_msgs::Pose& pose);
00156
00160 fcl::CollisionObject getCollisionObject() const;
00161
00162 virtual ~MarkerShape(){}
00163 };
00164
00165
00166
00167
00168
00169 #include "cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp"
00170
00171 #endif