DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update file costmap_2d_robot

Browse Source
This commit is contained in:
duongtd
2025-11-26 15:00:26 +07:00
parent 19a96c27d5
commit 2e0a4348dd
9 changed files with 551 additions and 1200 deletions
Download Patch File Download Diff File Expand all files Collapse all files

206
src/footprint.cpp
View File

@@ -208,117 +208,117 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
// std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
// {
// std::string full_param_name;
// std::string full_radius_param_name;
// std::vector<geometry_msgs::Point> points;
std::vector<geometry_msgs::Point> makeFootprintFromParams(const std::string& file_name)
{
std::string full_param_name;
std::string full_radius_param_name;
std::vector<geometry_msgs::Point> points;
// if (nh.searchParam("footprint", full_param_name))
// {
// XmlRpc::XmlRpcValue footprint_xmlrpc;
// nh.getParam(full_param_name, footprint_xmlrpc);
// if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
// footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
// {
// if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
// {
// writeFootprintToParam(nh, points);
// return points;
// }
// }
// else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
// {
// points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
// writeFootprintToParam(nh, points);
// return points;
// }
// }
// if (nh.searchParam("footprint", full_param_name))
// {
// XmlRpc::XmlRpcValue footprint_xmlrpc;
// nh.getParam(full_param_name, footprint_xmlrpc);
// if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
// footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
// {
// if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
// {
// writeFootprintToParam(nh, points);
// return points;
// }
// }
// else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
// {
// points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
// writeFootprintToParam(nh, points);
// return points;
// }
// }
// if (nh.searchParam("robot_radius", full_radius_param_name))
// {
// double robot_radius;
// nh.param(full_radius_param_name, robot_radius, 1.234);
// points = makeFootprintFromRadius(robot_radius);
// nh.setParam("robot_radius", robot_radius);
// }
// // Else neither param was found anywhere this knows about, so
// // defaults will come from dynamic_reconfigure stuff, set in
// // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
// return points;
// }
// if (nh.searchParam("robot_radius", full_radius_param_name))
// {
// double robot_radius;
// nh.param(full_radius_param_name, robot_radius, 1.234);
// points = makeFootprintFromRadius(robot_radius);
// nh.setParam("robot_radius", robot_radius);
// }
// // Else neither param was found anywhere this knows about, so
// // defaults will come from dynamic_reconfigure stuff, set in
// // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
// return points;
}
// void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint)
// {
// std::ostringstream oss;
// bool first = true;
// for (unsigned int i = 0; i < footprint.size(); i++)
// {
// geometry_msgs::Point p = footprint[ i ];
// if (first)
// {
// oss << "[[" << p.x << "," << p.y << "]";
// first = false;
// }
// else
// {
// oss << ",[" << p.x << "," << p.y << "]";
// }
// }
// oss << "]";
// nh.setParam("footprint", oss.str().c_str());
// }
void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint)
{
// std::ostringstream oss;
// bool first = true;
// for (unsigned int i = 0; i < footprint.size(); i++)
// {
// geometry_msgs::Point p = footprint[ i ];
// if (first)
// {
// oss << "[[" << p.x << "," << p.y << "]";
// first = false;
// }
// else
// {
// oss << ",[" << p.x << "," << p.y << "]";
// }
// }
// oss << "]";
// nh.setParam("footprint", oss.str().c_str());
}
// double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
// {
// // Make sure that the value we're looking at is either a double or an int.
// if (value.getType() != XmlRpc::XmlRpcValue::TypeInt &&
// value.getType() != XmlRpc::XmlRpcValue::TypeDouble)
// {
// std::string& value_string = value;
// std::printf("Values in the footprint specification (param %s) must be numbers. Found value %s.",
// full_param_name.c_str(), value_string.c_str());
// throw std::runtime_error("Values in the footprint specification must be numbers");
// }
// return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
// }
double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
{
// Make sure that the value we're looking at is either a double or an int.
if (value.getType() != XmlRpc::XmlRpcValue::TypeInt &&
value.getType() != XmlRpc::XmlRpcValue::TypeDouble)
{
std::string& value_string = value;
std::printf("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
full_param_name.c_str(), value_string.c_str());
throw std::runtime_error("Values in the footprint specification must be numbers\n");
}
return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
}
// std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc,
// const std::string& full_param_name)
// {
// // Make sure we have an array of at least 3 elements.
// if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
// footprint_xmlrpc.size() < 3)
// {
// std::printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s",
// full_param_name.c_str(), std::string(footprint_xmlrpc).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]]");
// }
std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
const std::string& full_param_name)
{
// Make sure we have an array of at least 3 elements.
if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
footprint_xmlrpc.size() < 3)
{
std::printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
full_param_name.c_str(), std::string(footprint_xmlrpc).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]]\n");
}
// std::vector<geometry_msgs::Point> footprint;
// geometry_msgs::Point pt;
std::vector<geometry_msgs::Point> footprint;
geometry_msgs::Point pt;
// for (int i = 0; i < footprint_xmlrpc.size(); ++i)
// {
// // Make sure each element of the list is an array of size 2. (x and y coordinates)
// XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
// if (point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
// point.size() != 2)
// {
// std::printf("The footprint (parameter %s) 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.",
// full_param_name.c_str());
// 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");
// }
for (int i = 0; i < footprint_xmlrpc.size(); ++i)
{
// Make sure each element of the list is an array of size 2. (x and y coordinates)
XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
if (point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
point.size() != 2)
{
std::printf("The footprint (parameter %s) 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.\n",
full_param_name.c_str());
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\n");
}
// pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name);
// pt.y = getNumberFromXMLRPC(point[ 1 ], full_param_name);
pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name);
pt.y = getNumberFromXMLRPC(point[ 1 ], full_param_name);
// footprint.push_back(pt);
// }
// return footprint;
// }
footprint.push_back(pt);
}
return footprint;
}
} // end namespace costmap_2d