23 #include <visualization_msgs/Marker.h> 38 msg_pose.position.x = urdf_pose.position.x;
39 msg_pose.position.y = urdf_pose.position.y;
40 msg_pose.position.z = urdf_pose.position.z;
42 msg_pose.orientation.x = urdf_pose.rotation.x;
43 msg_pose.orientation.y = urdf_pose.rotation.y;
44 msg_pose.orientation.z = urdf_pose.rotation.z;
45 msg_pose.orientation.w = urdf_pose.rotation.w;
50 const std::string& self_collision_obstacle_link)
57 std::vector<std::string>::iterator sca_begin = this->
self_collision_map_[self_collision_obstacle_link].begin();
58 std::vector<std::string>::iterator sca_end = this->
self_collision_map_[self_collision_obstacle_link].end();
59 return std::find(sca_begin, sca_end, link_of_interest) == sca_end;
87 std::vector<std::string> empty_vec;
90 for (
int j=0; j < it->second.size(); ++j)
111 ptr_obstacle->setDrawable(
false);
112 sm->addShape(it->first, ptr_obstacle);
121 const Eigen::Quaterniond& quat_pos,
122 const std::string& link_of_interest,
127 ROS_ERROR(
"FrameToCollision object has not been initialized correctly.");
131 bool local_success =
true;
135 geometry_msgs::Pose
pose;
138 if (
NULL != link->collision &&
NULL != link->collision->geometry)
144 link->collision->geometry,
145 segment_of_interest_marker_shape);
147 else if (
NULL != link->visual &&
NULL != link->visual->geometry)
149 ROS_WARN_STREAM(
"Could not find a collision or collision geometry for " << link_of_interest <<
150 ". Trying to create the shape from visual.");
155 link->visual->geometry,
156 segment_of_interest_marker_shape);
160 ROS_ERROR_STREAM(
"There is either no collision object or no collision geometry available: " << link_of_interest <<
161 ". Trying fallback solution: getMarker from a default SPHERE.");
162 const Eigen::Vector3d dim(0.05, 0.1, 0.1);
167 segment_of_interest_marker_shape);
168 local_success = segment_of_interest_marker_shape !=
NULL;
173 ROS_ERROR_STREAM(
"Could not find link in URDF model description: " << link_of_interest);
174 local_success =
false;
177 return local_success;
182 const geometry_msgs::Pose&
pose,
183 const std_msgs::ColorRGBA& col,
187 if (urdf::Geometry::MESH == geometry->type)
189 std_msgs::ColorRGBA test_col;
193 PtrMesh_t mesh = boost::static_pointer_cast<urdf::Mesh>(geometry);
199 else if (urdf::Geometry::BOX == geometry->type)
201 PtrBox_t urdf_box = boost::static_pointer_cast<urdf::Box>(geometry);
202 fcl::Box b(urdf_box->dim.x,
206 std_msgs::ColorRGBA test_col;
213 else if (urdf::Geometry::SPHERE == geometry->type)
215 std_msgs::ColorRGBA test_col;
219 PtrSphere_t urdf_sphere = boost::static_pointer_cast<urdf::Sphere>(geometry);
220 fcl::Sphere
s(urdf_sphere->radius);
223 else if (urdf::Geometry::CYLINDER == geometry->type)
225 std_msgs::ColorRGBA test_col;
229 PtrCylinder_t urdf_cyl = boost::static_pointer_cast<urdf::Cylinder>(geometry);
230 fcl::Cylinder c(urdf_cyl->radius, urdf_cyl->length);
241 const Eigen::Vector3d& abs_pos,
242 const Eigen::Quaterniond& quat_pos,
243 const std::string& link_of_interest,
244 const Eigen::Vector3d& dimension,
247 geometry_msgs::Pose
pose;
254 segment_of_interest_marker_shape);
259 const geometry_msgs::Pose&
pose,
260 const std::string& link_of_interest,
261 const Eigen::Vector3d& dimension,
268 uint32_t loc_shape_type = shape_type;
269 std::string mesh_resource;
270 if (visualization_msgs::Marker::MESH_RESOURCE == loc_shape_type)
275 NULL != link->collision &&
276 NULL != link->collision->geometry &&
277 link->collision->geometry->type == urdf::Geometry::MESH)
279 PtrMesh_t mesh = boost::static_pointer_cast<urdf::Mesh>(link->collision->geometry);
280 mesh_resource = mesh->filename;
284 ROS_WARN_STREAM(
"Either link is not available in URDF or there is no MESH collision representation for " <<
285 link_of_interest <<
". Rather using SPHERE.");
286 loc_shape_type = visualization_msgs::Marker::SPHERE;
290 std_msgs::ColorRGBA test_col;
292 switch (loc_shape_type)
294 case visualization_msgs::Marker::CUBE:
299 case visualization_msgs::Marker::SPHERE:
304 case visualization_msgs::Marker::CYLINDER:
309 case visualization_msgs::Marker::MESH_RESOURCE:
316 ROS_ERROR(
"Failed to process request due to unknown shape type: %d", loc_shape_type);
URDF_EXPORT bool initFile(const std::string &filename)
std::shared_ptr< IMarkerShape > PtrIMarkerShape_t
ValueStruct::iterator iterator
urdf::MeshSharedPtr PtrMesh_t
std::unordered_map< std::string, std::vector< std::string > > self_collision_map_
void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m)
std_msgs::ColorRGBA obstacle_color_
std::unordered_map< std::string, std::vector< std::string > >::iterator MapIter_t
Template class implementation for box, sphere and cylinder fcl::shapes. Creates visualization marker...
Type const & getType() const
void poseURDFToMsg(const urdf::Pose &urdf_pose, geometry_msgs::Pose &msg_pose)
bool ignoreSelfCollisionPart(const std::string &link_of_interest, const std::string &self_collision_obstacle_link)
bool getMarkerShapeFromUrdf(const Eigen::Vector3d &abs_pos, const Eigen::Quaterniond &quat_pos, const std::string &link_of_interest, PtrIMarkerShape_t &segment_of_interest_marker_shape)
urdf::CylinderSharedPtr PtrCylinder_t
URDF_EXPORT bool initParam(const std::string ¶m)
#define ROS_WARN_STREAM(args)
void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m)
static ShapeMsgTypeToVisMarkerType g_shapeMsgTypeToVisMarkerType
std::string root_frame_id_
#define ROS_INFO_STREAM(args)
bool initFile(const std::string &root_frame_id, const std::string &urdf_file_name)
#define ROS_ERROR_STREAM(args)
urdf::LinkConstSharedPtr PtrConstLink_t
bool initSelfCollision(XmlRpc::XmlRpcValue &self_collision_params, boost::scoped_ptr< ShapesManager > &sm)
bool getMarkerShapeFromType(const uint32_t &shape_type, const Eigen::Vector3d &abs_pos, const Eigen::Quaterniond &quat_pos, const std::string &link_of_interest, const Eigen::Vector3d &dimension, PtrIMarkerShape_t &segment_of_interest_marker_shape)
void createSpecificMarkerShape(const std::string &link_of_interest, const geometry_msgs::Pose &pose, const std_msgs::ColorRGBA &col, const PtrGeometry_t &geometry, PtrIMarkerShape_t &segment_of_interest_marker_shape)
first: link to be considered 'obstacle', second: links of component to be considered for self-collisi...
urdf::SphereSharedPtr PtrSphere_t
bool initParameter(const std::string &root_frame_id, const std::string &urdf_param)
urdf::BoxSharedPtr PtrBox_t
urdf::GeometrySharedPtr PtrGeometry_t