00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef MARKER_SHAPES_IMPL_HPP_
00019 #define MARKER_SHAPES_IMPL_HPP_
00020
00021 #include "cob_obstacle_distance/marker_shapes/marker_shapes.hpp"
00022
00023
00024 template <typename T>
00025 MarkerShape<T>::MarkerShape(const std::string& root_frame, T& fcl_object,
00026 double x, double y, double z,
00027 double quat_x, double quat_y, double quat_z, double quat_w,
00028 double color_r, double color_g, double color_b, double color_a) : fcl_marker_converter_(fcl_object)
00029 {
00030 this->init(root_frame, x, y, z, quat_x, quat_y, quat_z, quat_w, color_r, color_g, color_b, color_a);
00031 }
00032
00033
00034 template <typename T>
00035 MarkerShape<T>::MarkerShape(const std::string& root_frame, double x, double y, double z,
00036 double quat_x, double quat_y, double quat_z, double quat_w,
00037 double color_r, double color_g, double color_b, double color_a)
00038 {
00039 this->init(root_frame, x, y, z, quat_x, quat_y, quat_z, quat_w, color_r, color_g, color_b, color_a);
00040 }
00041
00042 template <typename T>
00043 geometry_msgs::Pose MarkerShape<T>::getMarkerPose() const
00044 {
00045 return this->marker_.pose;
00046 }
00047
00048
00049 template <typename T>
00050 geometry_msgs::Pose MarkerShape<T>::getOriginRelToFrame() const
00051 {
00052 return this->origin_;
00053 }
00054
00055
00056 template <typename T>
00057 void MarkerShape<T>::init(const std::string& root_frame, double x, double y, double z,
00058 double quat_x, double quat_y, double quat_z, double quat_w,
00059 double color_r, double color_g, double color_b, double color_a)
00060 {
00061 marker_.pose.position.x = origin_.position.x = x;
00062 marker_.pose.position.y = origin_.position.y = y;
00063 marker_.pose.position.z = origin_.position.z = z;
00064 marker_.pose.orientation.x = origin_.orientation.x = quat_x;
00065 marker_.pose.orientation.y = origin_.orientation.y = quat_y;
00066 marker_.pose.orientation.z = origin_.orientation.z = quat_z;
00067 marker_.pose.orientation.w = origin_.orientation.w = quat_w;
00068
00069 marker_.color.r = color_r;
00070 marker_.color.g = color_g;
00071 marker_.color.b = color_b;
00072 marker_.color.a = color_a;
00073
00074 marker_.header.frame_id = root_frame;
00075 marker_.header.stamp = ros::Time::now();
00076 marker_.ns = g_marker_namespace;
00077 marker_.action = visualization_msgs::Marker::ADD;
00078 marker_.id = class_ctr_;
00079
00080 marker_.lifetime = ros::Duration();
00081
00082 fcl_marker_converter_.assignValues(marker_);
00083
00084 BVH_RSS_t bvh;
00085 fcl_marker_converter_.getBvhModel(bvh);
00086 this->ptr_fcl_bvh_.reset(new BVH_RSS_t(bvh));
00087 this->ptr_fcl_bvh_->computeLocalAABB();
00088 }
00089
00090
00091 template <typename T>
00092 inline uint32_t MarkerShape<T>::getId() const
00093 {
00094 return this->marker_.id;
00095 }
00096
00097
00098 template <typename T>
00099 inline void MarkerShape<T>::setColor(double color_r, double color_g, double color_b, double color_a)
00100 {
00101 marker_.color.r = color_r;
00102 marker_.color.g = color_g;
00103 marker_.color.b = color_b;
00104 marker_.color.a = color_a;
00105 fcl_marker_converter_.assignValues(marker_);
00106 }
00107
00108
00109 template <typename T>
00110 inline visualization_msgs::Marker MarkerShape<T>::getMarker()
00111 {
00112 this->marker_.header.stamp = ros::Time::now();
00113 return this->marker_;
00114 }
00115
00116
00117 template <typename T>
00118 fcl::CollisionObject MarkerShape<T>::getCollisionObject() const
00119 {
00120 fcl::Transform3f x(fcl::Quaternion3f(this->marker_.pose.orientation.w,
00121 this->marker_.pose.orientation.x,
00122 this->marker_.pose.orientation.y,
00123 this->marker_.pose.orientation.z),
00124 fcl::Vec3f(this->marker_.pose.position.x,
00125 this->marker_.pose.position.y,
00126 this->marker_.pose.position.z));
00127 fcl::CollisionObject cobj(this->ptr_fcl_bvh_, x);
00128 return cobj;
00129 }
00130
00131
00132 template <typename T>
00133 inline void MarkerShape<T>::updatePose(const geometry_msgs::Vector3& pos, const geometry_msgs::Quaternion& quat)
00134 {
00135 marker_.pose.position.x = pos.x;
00136 marker_.pose.position.y = pos.y;
00137 marker_.pose.position.z = pos.z;
00138 marker_.pose.orientation = quat;
00139 }
00140
00141
00142 template <typename T>
00143 inline void MarkerShape<T>::updatePose(const geometry_msgs::Pose& pose)
00144 {
00145 marker_.pose = pose;
00146 }
00147
00148
00149
00150 #endif