37 #ifndef COLLISION_DISTANCE_FIELD_HELPERS_H_ 38 #define COLLISION_DISTANCE_FIELD_HELPERS_H_ 41 #include <planning_models/robot_model.h> 42 #include <collision_distance_field/collision_distance_field_types.h> 47 ros::NodeHandle& nh,
const planning_models::RobotModelConstPtr& kmodel,
48 std::map<std::string, std::vector<collision_detection::CollisionSphere> >& link_body_spheres)
50 if (!nh.hasParam(
"link_spheres"))
52 ROS_INFO_STREAM(
"No parameter for link spheres");
55 XmlRpc::XmlRpcValue link_spheres;
56 nh.getParam(
"link_spheres", link_spheres);
58 if (link_spheres.getType() != XmlRpc::XmlRpcValue::TypeArray)
60 ROS_WARN_STREAM(
"Link spheres not an array");
64 if (link_spheres.size() == 0)
66 ROS_WARN_STREAM(
"Link spheres has no entries");
69 for (
int i = 0; i < link_spheres.size(); i++)
71 if (!link_spheres[i].hasMember(
"link"))
73 ROS_WARN_STREAM(
"All link spheres must have link");
76 if (!link_spheres[i].hasMember(
"spheres"))
78 ROS_WARN_STREAM(
"All link spheres must have spheres");
81 std::string link = link_spheres[i][
"link"];
82 XmlRpc::XmlRpcValue spheres = link_spheres[i][
"spheres"];
83 if (spheres.getType() != XmlRpc::XmlRpcValue::TypeArray)
85 if (std::string(spheres) ==
"none")
87 ROS_DEBUG_STREAM(
"No spheres for " << link);
88 std::vector<collision_detection::CollisionSphere> coll_spheres;
89 link_body_spheres[link_spheres[i][
"link"]] = coll_spheres;
93 std::vector<collision_detection::CollisionSphere> coll_spheres;
94 for (
int j = 0; j < spheres.size(); j++)
96 if (!spheres[j].hasMember(
"x"))
98 ROS_WARN_STREAM(
"All spheres must specify a value for x");
101 if (!spheres[j].hasMember(
"y"))
103 ROS_WARN_STREAM(
"All spheres must specify a value for y");
106 if (!spheres[j].hasMember(
"z"))
108 ROS_WARN_STREAM(
"All spheres must specify a value for z");
111 if (!spheres[j].hasMember(
"radius"))
113 ROS_WARN_STREAM(
"All spheres must specify a value for radius");
116 Eigen::Vector3d rel(spheres[j][
"x"], spheres[j][
"y"], spheres[j][
"z"]);
117 ROS_DEBUG_STREAM(
"Link " << link_spheres[i][
"link"] <<
" sphere " << coll_spheres.size() <<
" " << rel.x() <<
" " 118 << rel.y() <<
" " << rel.z());
119 collision_detection::CollisionSphere cs(rel, spheres[j][
"radius"]);
120 coll_spheres.push_back(cs);
122 link_body_spheres[link_spheres[i][
"link"]] = coll_spheres;
static bool loadLinkBodySphereDecompositions(ros::NodeHandle &nh, const planning_models::RobotModelConstPtr &kmodel, std::map< std::string, std::vector< collision_detection::CollisionSphere > > &link_body_spheres)