ros 動的パラメーター サービスに配列キューを追加するためのリファレンス デザイン
動的配列パラメータ設定ポリゴン: [[0.0, 0.0], [0.1, 0.1], [0.1, 0.0], [0.0, -0.1]]
参考： https://github.com/ros-perception/laser_filters
cfg動的パラメータファイルの作成
参照：laser_filters/PolygonFilter.cfg
#!/usr/bin/env python
from dynamic_reconfigure.parameter_generator_catkin import *
PACKAGE = "laser_filters"
gen = ParameterGenerator()
gen.add("polygon", str_t, 0, "A list of coordinates that represents a polygon in the format [ [x1, y1], [x2, y2], ..., [xn, yn] ]", "[]")
gen.add("polygon_padding", double_t, 0, "Polygon padding in meters", 0, 0, 100.)
gen.add("invert", bool_t, 0, "A Boolean to invert the filter", False)
exit(gen.generate(PACKAGE, "laser_filters", "PolygonFilter"))
ポリゴン ポリゴンは文字列で埋められ、実際には多次元配列として使用されます。配列内のデータは '[' ']' 括弧で区切られます。
動的パラメータサービスコール
配列クラスの動的パラメータの設計
配列パラメータは文字列に変換され、文字列は配列に変換され、対応する参照関数
cmake が変換されます。
find_package(catkin REQUIRED COMPONENTS message_filters dynamic_reconfigure )
generate_dynamic_reconfigure_options(
cfg/PolygonFilter.cfg
)
add_dependencies(laser_scan_filters ${PROJECT_NAME}_gencfg)
「polygon_filter.cpp」を参照してください。
#include <dynamic_reconfigure/server.h>
#include <laser_filters/PolygonFilterConfig.h>
std::string polygon_string;
PolygonFilterConfig param_config;
ros::NodeHandle private_nh("~" + getName());
dyn_server_.reset(new dynamic_reconfigure::Server<laser_filters::PolygonFilterConfig>(own_mutex_, private_nh));
dynamic_reconfigure::Server<laser_filters::PolygonFilterConfig>::CallbackType f;
f = boost::bind(&laser_filters::LaserScanPolygonFilter::reconfigureCB, this, _1, _2);
dyn_server_->setCallback(f);
polygon_string = polygonToString(polygon_);
param_config.polygon = polygon_string;
param_config.polygon_padding = polygon_padding;
param_config.invert = invert_filter_;
dyn_server_->updateConfig(param_config);
std::vector<std::vector > parseVVF (const std::string& input, std::string& error_return)
geometry_msgs::Polygon makePolygonFromString (const std::string& Polygon_string, const geometry_msgs::Polygon& last_polygon)
std::string PolygonToString (geometry_msgs::Polygon ポリゴン)
std::vector<std::vector<float> > parseVVF(const std::string& input, std::string& error_return)
{ // Source: https://github.com/ros-planning/navigation/blob/melodic-devel/costmap_2d/src/array_parser.cpp
std::vector<std::vector<float> > result;
std::stringstream input_ss(input);
int depth = 0;
std::vector<float> current_vector;
while (!!input_ss && !input_ss.eof())
{
switch (input_ss.peek())
{
case EOF:
break;
case '[':
depth++;
if (depth > 2)
{
error_return = "Array depth greater than 2";
return result;
}
input_ss.get();
current_vector.clear();
break;
case ']':
depth--;
if (depth < 0)
{
error_return = "More close ] than open [";
return result;
}
input_ss.get();
if (depth == 1)
{
result.push_back(current_vector);
}
break;
case ',':
case ' ':
case '\t':
input_ss.get();
break;
default: // All other characters should be part of the numbers.
if (depth != 2)
{
std::stringstream err_ss;
err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'.";
error_return = err_ss.str();
return result;
}
float value;
input_ss >> value;
if (!!input_ss)
{
current_vector.push_back(value);
}
break;
}
}
if (depth != 0)
{
error_return = "Unterminated vector string.";
}
else
{
error_return = "";
}
return result;
}
geometry_msgs::Polygon makePolygonFromString(const std::string& polygon_string, const geometry_msgs::Polygon& last_polygon)
{
std::string error;
std::vector<std::vector<float> > vvf = parseVVF(polygon_string, error);
if (error != "")
{
ROS_ERROR("Error parsing polygon parameter: '%s'", error.c_str());
ROS_ERROR(" Polygon string was '%s'.", polygon_string.c_str());
return last_polygon;
}
geometry_msgs::Polygon polygon;
geometry_msgs::Point32 point;
// convert vvf into points.
if (vvf.size() < 3 && vvf.size() > 0)
{
ROS_WARN("You must specify at least three points for the robot polygon");
return last_polygon;
}
for (unsigned int i = 0; i < vvf.size(); i++)
{
if (vvf[ i ].size() == 2)
{
point.x = vvf[ i ][ 0 ];
point.y = vvf[ i ][ 1 ];
point.z = 0;
polygon.points.push_back(point);
}
else
{
ROS_ERROR("Points in the polygon specification must be pairs of numbers. Found a point with %d numbers.",
int(vvf[ i ].size()));
return last_polygon;
}
}
return polygon;
}
std::string polygonToString(geometry_msgs::Polygon polygon)
{
std::string polygon_string = "[";
bool first = true;
for (auto point : polygon.points) {
if (!first) {
polygon_string += ", ";
}
first = false;
polygon_string += "[" + std::to_string(point.x) + ", " + std::to_string(point.y) + "]";
}
polygon_string += "]";
return polygon_string;
}
xmlパラメータロード2次元配列[XmlRpc::XmlRpcValue]
scan_filter_chain:
- name: polygon_filter
type: laser_filters/LaserScanPolygonFilter
params:
polygon_frame: base_link
polygon: [[0.0, 0.0], [0.1, 0.1], [0.1, 0.0], [0.0, -0.1]]
invert: false
<launch>
<node pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen" name="laser_filter">
<remap from="scan" to="base_scan" />
<rosparam command="load" file="$(find laser_filters)/examples/polygon_filter.yaml" />
</node>
</launch>
geometry_msgs::Polygon makePolygonFromXMLRPC (const XmlRpc::XmlRpcValue& Polygon_xmlrpc, const std::string& full_param_name)
データ型 XmlRpcValue:
XmlRpc::XmlRpcValue Polygon_xmlrpc;
XmlRpc::XmlRpcValue polygon_xmlrpc;
bool polygon_set = getParam("polygon", polygon_xmlrpc);
geometry_msgs::Polygon polygon_ = makePolygonFromXMLRPC(polygon_xmlrpc, "polygon");
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;
ROS_FATAL("Values in the polygon 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 polygon specification must be numbers");
}
return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
}
geometry_msgs::Polygon makePolygonFromXMLRPC(const XmlRpc::XmlRpcValue& polygon_xmlrpc,
const std::string& full_param_name)
{
// Make sure we have an array of at least 3 elements.
if (polygon_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
polygon_xmlrpc.size() > 0 && polygon_xmlrpc.size() < 3)
{
ROS_FATAL("The polygon (parameter %s) must be specified as nested list on the parameter server with at least "
"3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]",
full_param_name.c_str());
throw std::runtime_error("The polygon must be specified as nested list on the parameter server with at least "
"3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
}
geometry_msgs::Polygon polygon;
geometry_msgs::Point32 pt;
for (int i = 0; i < polygon_xmlrpc.size(); ++i)
{
// Make sure each element of the list is an array of size 2. (x and y coordinates)
XmlRpc::XmlRpcValue point = polygon_xmlrpc[i];
if (point.getType() != XmlRpc::XmlRpcValue::TypeArray || point.size() != 2)
{
ROS_FATAL("The polygon (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 polygon 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");
}
pt.x = getNumberFromXMLRPC(point[0], full_param_name);
pt.y = getNumberFromXMLRPC(point[1], full_param_name);
polygon.points.push_back(pt);
}
return polygon;
}
一次元配列をロードする XML パラメータ [直接]
参数调用：
std::vector<double> start_angles_;
getParam("start_angles", start_angles_);
配置yaml：
scan_filter_chain:
- name: limitAngleFilter
type: laser_filters/LaserScanAngularMutiBoundsFilterInPlace
params:
start_angles: [0, 108, 208, 290] # [0,110,215, 290] #[28,147,208,323]