// load robot footprint from costmap_2d_ros to keep same footprint format
std::vector<geometry_msgs::Point> CollisionVelocityFilter::loadRobotFootprint(ros::NodeHandle node){
std::vector<geometry_msgs::Point> footprint;
geometry_msgs::Point pt;
double padding;
std::string padding_param, footprint_param;
if(!node.searchParam("footprint_padding", padding_param))
padding = 0.01;
else
node.param(padding_param, padding, 0.01);
//grab the footprint from the parameter server if possible
XmlRpc::XmlRpcValue footprint_list;
std::string footprint_string;
std::vector<std::string> footstring_list;
if(node.searchParam("footprint", footprint_param)){
node.getParam(footprint_param, footprint_list);
if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) {
footprint_string = std::string(footprint_list);
//if there's just an empty footprint up there, return
if(footprint_string == "[]" || footprint_string == "")
return footprint;
boost::erase_all(footprint_string, " ");
boost::char_separator<char> sep("[]");
boost::tokenizer<boost::char_separator<char> > tokens(footprint_string, sep);
footstring_list = std::vector<std::string>(tokens.begin(), tokens.end());
}
//make sure we have a list of lists
if(!(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_list.size() > 2) && !(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString && footstring_list.size() > 5)){
ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", footprint_param.c_str(), std::string(footprint_list).c_str());
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
}
if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for(int i = 0; i < footprint_list.size(); ++i){
//make sure we have a list of lists of size 2
XmlRpc::XmlRpcValue point = footprint_list[i];
if(!(point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2)){
ROS_FATAL("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
}
//make sure that the value we're looking at is either a double or an int
if(!(point[0].getType() == XmlRpc::XmlRpcValue::TypeInt || point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
ROS_FATAL("Values in the footprint specification must be numbers");
throw std::runtime_error("Values in the footprint specification must be numbers");
}
pt.x = point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[0]) : (double)(point[0]);
pt.x += sign(pt.x) * padding;
//make sure that the value we're looking at is either a double or an int
if(!(point[1].getType() == XmlRpc::XmlRpcValue::TypeInt || point[1].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
ROS_FATAL("Values in the footprint specification must be numbers");
throw std::runtime_error("Values in the footprint specification must be numbers");
}
pt.y = point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[1]) : (double)(point[1]);
pt.y += sign(pt.y) * padding;
footprint.push_back(pt);
node.deleteParam(footprint_param);
std::ostringstream oss;
bool first = true;
BOOST_FOREACH(geometry_msgs::Point p, footprint) {
if(first) {
oss << "[[" << p.x << "," << p.y << "]";
first = false;
}
else {
oss << ",[" << p.x << "," << p.y << "]";
}
}
oss << "]";
node.setParam(footprint_param, oss.str().c_str());
node.setParam("footprint", oss.str().c_str());
}
}
else if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) {
