25 const shape_msgs::Mesh& mesh,
26 const geometry_msgs::Pose&
pose,
27 const std_msgs::ColorRGBA& col)
31 for (shape_msgs::MeshTriangle tri : mesh.triangles)
33 uint32_t v_idx_1 = tri.vertex_indices.elems[0];
34 uint32_t v_idx_2 = tri.vertex_indices.elems[1];
35 uint32_t v_idx_3 = tri.vertex_indices.elems[2];
37 fcl::Vec3f v1(mesh.vertices[v_idx_1].x, mesh.vertices[v_idx_1].y, mesh.vertices[v_idx_1].z);
38 fcl::Vec3f v2(mesh.vertices[v_idx_2].x, mesh.vertices[v_idx_2].y, mesh.vertices[v_idx_2].z);
39 fcl::Vec3f v3(mesh.vertices[v_idx_3].x, mesh.vertices[v_idx_3].y, mesh.vertices[v_idx_3].z);
53 marker_.type = visualization_msgs::Marker::MESH_RESOURCE;
58 marker_.action = visualization_msgs::Marker::ADD;
67 double x,
double y,
double z,
68 double quat_x,
double quat_y,
double quat_z,
double quat_w,
69 double color_r,
double color_g,
double color_b,
double color_a)
71 this->
init(mesh_resource, root_frame, x, y, z, quat_x, quat_y, quat_z, quat_w, color_r, color_g, color_b, color_a);
76 double quat_x,
double quat_y,
double quat_z,
double quat_w,
77 double color_r,
double color_g,
double color_b,
double color_a)
82 if (0 != sp.
createBVH(this->ptr_fcl_bvh_))
103 marker_.type = visualization_msgs::Marker::MESH_RESOURCE;
108 marker_.action = visualization_msgs::Marker::ADD;
110 marker_.mesh_resource = mesh_resource;
111 marker_.mesh_use_embedded_materials =
true;
146 marker_.pose.position.x = pos.x;
147 marker_.pose.position.y = pos.y;
148 marker_.pose.position.z = pos.z;
149 marker_.pose.orientation = quat;
168 fcl::Transform3f x(fcl::Quaternion3f(this->
marker_.pose.orientation.w,
169 this->marker_.pose.orientation.x,
170 this->marker_.pose.orientation.y,
171 this->marker_.pose.orientation.z),
172 fcl::Vec3f(this->marker_.pose.position.x,
173 this->marker_.pose.position.y,
174 this->marker_.pose.position.z));
visualization_msgs::Marker marker_
geometry_msgs::Pose getMarkerPose() const
void init(const std::string &root_frame, double x, double y, double z, double quat_x, double quat_y, double quat_z, double quat_w, double color_r, double color_g, double color_b, double color_a)
int8_t createBVH(fcl::BVHModel< T > &bvh)
fcl::BVHModel< fcl::RSS > BVH_RSS_t
void updatePose(const geometry_msgs::Vector3 &pos, const geometry_msgs::Quaternion &quat)
visualization_msgs::Marker getMarker()
static const std::string g_marker_namespace
std::shared_ptr< BVH_RSS_t > ptr_fcl_bvh_
void setColor(double color_r, double color_g, double color_b, double color_a=1.0)
static uint32_t class_ctr_
geometry_msgs::Pose origin_
fcl::CollisionObject getCollisionObject() const
MarkerShape(const std::string &root_frame, T &fcl_object, const geometry_msgs::Pose &pose, const std_msgs::ColorRGBA &col)
geometry_msgs::Pose getOriginRelToFrame() const