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

fix core dumped err when loadplugin

Browse Source
This commit is contained in:
duongtd
2025-12-01 17:34:23 +07:00
parent 2439f2a71d
commit 11ff1baa79
34 changed files with 1177 additions and 760 deletions
Download Patch File Download Diff File Expand all files Collapse all files

137
src/footprint.cpp Normal file → Executable file
View File

@@ -27,12 +27,13 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <costmap_2d/costmap_math.h>
#include<costmap_2d/costmap_math.h>
#include <boost/tokenizer.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>
#include <costmap_2d/footprint.h>
#include <costmap_2d/array_parser.h>
#include<geometry_msgs/Point32.h>
namespace costmap_2d
{
@@ -173,15 +174,15 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
if (error != "")
{
std::printf("Error parsing footprint parameter: '%s'", error.c_str());
std::printf(" Footprint string was '%s'.", footprint_string.c_str());
printf("Error parsing footprint parameter: '%s'\n", error.c_str());
printf(" Footprint string was '%s'.\n", footprint_string.c_str());
return false;
}
// convert vvf into points.
if (vvf.size() < 3)
{
std::printf("You must specify at least three points for the robot footprint, reverting to previous footprint.");
printf("You must specify at least three points for the robot footprint, reverting to previous footprint.\n");
return false;
}
footprint.reserve(vvf.size());
@@ -197,7 +198,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
}
else
{
std::printf("Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.",
printf("Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.\n",
int(vvf[ i ].size()));
return false;
}
@@ -208,66 +209,66 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
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;
// 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;
// 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(ros::NodeHandle& 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)
{
@@ -276,9 +277,9 @@ double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_p
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",
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");
throw std::runtime_error("Values in the footprint specification must be numbers");
}
return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
}
@@ -290,10 +291,10 @@ std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& f
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",
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");
"3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
}
std::vector<geometry_msgs::Point> footprint;
@@ -306,11 +307,11 @@ std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& f
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: "
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");
"[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
}
pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name);