19 #include <unordered_map> 22 #include <Eigen/Dense> 27 #include <boost/shared_ptr.hpp> 28 #include <boost/scoped_ptr.hpp> 29 #include <boost/pointer_cast.hpp> 31 #include <fcl/collision_object.h> 32 #include <fcl/collision.h> 33 #include <fcl/distance.h> 34 #include <fcl/collision_data.h> 36 #include <std_msgs/ColorRGBA.h> 37 #include <geometry_msgs/Vector3.h> 38 #include <geometry_msgs/Quaternion.h> 39 #include <geometry_msgs/Pose.h> 59 typedef std::unordered_map<std::string, std::vector<std::string> >::iterator
MapIter_t;
75 const geometry_msgs::Pose&
pose,
76 const std_msgs::ColorRGBA& col,
77 const PtrGeometry_t& geometry,
86 void poseURDFToMsg(
const urdf::Pose& urdf_pose, geometry_msgs::Pose& msg_pose);
96 return this->self_collision_map_.begin();
101 return this->self_collision_map_.end();
104 bool ignoreSelfCollisionPart(
const std::string& link_of_interest,
const std::string& self_collision_obstacle_link);
112 bool initFile(
const std::string& root_frame_id,
const std::string& urdf_file_name);
120 bool initParameter(
const std::string& root_frame_id,
const std::string& urdf_param);
143 const Eigen::Quaterniond& quat_pos,
144 const std::string& link_of_interest,
158 const Eigen::Vector3d& abs_pos,
159 const Eigen::Quaterniond& quat_pos,
160 const std::string& link_of_interest,
161 const Eigen::Vector3d& dimension,
173 const geometry_msgs::Pose&
pose,
174 const std::string& link_of_interest,
175 const Eigen::Vector3d& dimension,
std::shared_ptr< IMarkerShape > PtrIMarkerShape_t
urdf::MeshSharedPtr PtrMesh_t
std::unordered_map< std::string, std::vector< std::string > > self_collision_map_
std::vector< PtrLink_t > VecPtrLink_t
MapSelfCollisions_t::iterator getSelfCollisionsIterBegin()
std::unordered_map< std::string, std::vector< std::string > >::iterator MapIter_t
MapSelfCollisions_t::iterator getSelfCollisionsIterEnd()
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
std::string root_frame_id_
std::unordered_map< std::string, std::vector< std::string > > MapSelfCollisions_t
bool initFile(const std::string &root_frame_id, const std::string &urdf_file_name)
urdf::LinkSharedPtr PtrLink_t
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::CollisionSharedPtr PtrCollision_t
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