From 5395231f74aa7b3beda1d849a975be724f5cde74 Mon Sep 17 00:00:00 2001 From: Giorgos Tsamis Date: Wed, 13 Mar 2024 17:05:02 +0200 Subject: [PATCH 1/4] Pad polygon on filter load --- src/polygon_filter.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/polygon_filter.cpp b/src/polygon_filter.cpp index a95cb55..97c68a3 100644 --- a/src/polygon_filter.cpp +++ b/src/polygon_filter.cpp @@ -274,6 +274,8 @@ bool LaserScanPolygonFilterBase::configure() param_config.invert = invert_filter_; dyn_server_->updateConfig(param_config); + padPolygon(polygon_, polygon_padding); + polygon_pub_ = private_nh.advertise("polygon", 1, true); is_polygon_published_ = false; From 45c48610d222e275dfacb946326381fca11db866 Mon Sep 17 00:00:00 2001 From: Giorgos Tsamis Date: Tue, 19 Mar 2024 10:50:12 +0200 Subject: [PATCH 2/4] Set reconfigure CB after updating the config params --- src/polygon_filter.cpp | 292 ++++++++++++++++++++++------------------- 1 file changed, 154 insertions(+), 138 deletions(-) diff --git a/src/polygon_filter.cpp b/src/polygon_filter.cpp index 97c68a3..19e8262 100644 --- a/src/polygon_filter.cpp +++ b/src/polygon_filter.cpp @@ -51,17 +51,14 @@ #include /** @brief Same as sign(x) but returns 0 if x is 0. */ -inline double sign0(double x) -{ - return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0); -} +inline double sign0(double x) { return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0); } void padPolygon(geometry_msgs::Polygon& polygon, double padding) { // pad polygon in place for (unsigned int i = 0; i < polygon.points.size(); i++) { - geometry_msgs::Point32& pt = polygon.points[ i ]; + geometry_msgs::Point32& pt = polygon.points[i]; pt.x += sign0(pt.x) * padding; pt.y += sign0(pt.y) * padding; } @@ -70,7 +67,8 @@ void padPolygon(geometry_msgs::Polygon& polygon, double padding) 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) + 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.", @@ -87,12 +85,15 @@ geometry_msgs::Polygon makePolygonFromXMLRPC(const XmlRpc::XmlRpcValue& polygon_ 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]]"); + 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; @@ -103,11 +104,14 @@ geometry_msgs::Polygon makePolygonFromXMLRPC(const XmlRpc::XmlRpcValue& polygon_ 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"); + 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); @@ -119,7 +123,8 @@ geometry_msgs::Polygon makePolygonFromXMLRPC(const XmlRpc::XmlRpcValue& polygon_ } std::vector > 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 +{ // Source: + // https://github.com/ros-planning/navigation/blob/melodic-devel/costmap_2d/src/array_parser.cpp std::vector > result; std::stringstream input_ss(input); @@ -129,51 +134,51 @@ std::vector > parseVVF(const std::string& input, std::string& { 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; + 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; } } @@ -189,54 +194,59 @@ std::vector > parseVVF(const std::string& input, std::string& return result; } -geometry_msgs::Polygon makePolygonFromString(const std::string& polygon_string, const geometry_msgs::Polygon& last_polygon) +geometry_msgs::Polygon makePolygonFromString(const std::string& polygon_string, + const geometry_msgs::Polygon& last_polygon) { std::string error; std::vector > 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; - } + 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; - 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; + } - // convert vvf into points. - if (vvf.size() < 3 && vvf.size() > 0) + for (unsigned int i = 0; i < vvf.size(); i++) + { + if (vvf[i].size() == 2) { - ROS_WARN("You must specify at least three points for the robot polygon"); - return last_polygon; + point.x = vvf[i][0]; + point.y = vvf[i][1]; + point.z = 0; + polygon.points.push_back(point); } - - for (unsigned int i = 0; i < vvf.size(); i++) + else { - 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; - } + 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; + return polygon; } std::string polygonToString(geometry_msgs::Polygon polygon) { std::string polygon_string = "["; bool first = true; - for (auto point : polygon.points) { - if (!first) { + for (auto point : polygon.points) + { + if (!first) + { polygon_string += ", "; } first = false; @@ -246,7 +256,8 @@ std::string polygonToString(geometry_msgs::Polygon polygon) return polygon_string; } -namespace laser_filters{ +namespace laser_filters +{ bool LaserScanPolygonFilterBase::configure() { @@ -255,10 +266,10 @@ bool LaserScanPolygonFilterBase::configure() PolygonFilterConfig param_config; ros::NodeHandle private_nh("~" + getName()); - dyn_server_.reset(new dynamic_reconfigure::Server(own_mutex_, private_nh)); + dyn_server_.reset( + new dynamic_reconfigure::Server(own_mutex_, private_nh)); dynamic_reconfigure::Server::CallbackType f; - f = [this](auto& config, auto level){ reconfigureCB(config, level); }; - dyn_server_->setCallback(f); + f = [this](auto& config, auto level) { reconfigureCB(config, level); }; bool polygon_set = getParam("polygon", polygon_xmlrpc); bool polygon_frame_set = getParam("polygon_frame", polygon_frame_); @@ -274,7 +285,8 @@ bool LaserScanPolygonFilterBase::configure() param_config.invert = invert_filter_; dyn_server_->updateConfig(param_config); - padPolygon(polygon_, polygon_padding); + // Calling setCallback(f) here calls reconfigureCB() which updates the polygon padding. + dyn_server_->setCallback(f); polygon_pub_ = private_nh.advertise("polygon", 1, true); is_polygon_published_ = false; @@ -297,14 +309,16 @@ bool LaserScanPolygonFilterBase::configure() } // See https://web.cs.ucdavis.edu/~okreylos/TAship/Spring2000/PointInPolygon.html -bool LaserScanPolygonFilterBase::inPolygon(tf::Point& point) const { +bool LaserScanPolygonFilterBase::inPolygon(tf::Point& point) const +{ int i, j; bool c = false; for (i = 0, j = polygon_.points.size() - 1; i < polygon_.points.size(); j = i++) { if ((polygon_.points.at(i).y > point.y() != (polygon_.points.at(j).y > point.y()) && - (point.x() < (polygon_.points[j].x - polygon_.points[i].x) * (point.y() - polygon_.points[i].y) / + (point.x() < (polygon_.points[j].x - polygon_.points[i].x) * + (point.y() - polygon_.points[i].y) / (polygon_.points[j].y - polygon_.points[i].y) + polygon_.points[i].x))) c = !c; @@ -325,7 +339,8 @@ void LaserScanPolygonFilterBase::publishPolygon() } } -void LaserScanPolygonFilterBase::reconfigureCB(laser_filters::PolygonFilterConfig& config, uint32_t level) +void LaserScanPolygonFilterBase::reconfigureCB(laser_filters::PolygonFilterConfig& config, + uint32_t level) { invert_filter_ = config.invert; polygon_ = makePolygonFromString(config.polygon, polygon_); @@ -348,7 +363,8 @@ bool LaserScanPolygonFilter::update(const sensor_msgs::LaserScan& input_scan, bool success = tf_.waitForTransform( polygon_frame_, input_scan.header.frame_id, - input_scan.header.stamp + ros::Duration().fromSec(input_scan.ranges.size() * input_scan.time_increment), + input_scan.header.stamp + + ros::Duration().fromSec(input_scan.ranges.size() * input_scan.time_increment), ros::Duration(1.0), ros::Duration(0.01), &error_msg); if (!success) { @@ -423,7 +439,7 @@ bool StaticLaserScanPolygonFilter::configure() { is_polygon_transformed_ = false; - transform_timeout_ = 5; // Default + transform_timeout_ = 5; // Default getParam("transform_timeout", transform_timeout_); return LaserScanPolygonFilterBase::configure(); @@ -433,11 +449,9 @@ void StaticLaserScanPolygonFilter::checkCoSineMap(const sensor_msgs::LaserScan& { size_t n_pts = scan_in.ranges.size(); - if ( - co_sine_map_.rows() != (int)n_pts || - co_sine_map_angle_min_ != scan_in.angle_min || - co_sine_map_angle_max_ != scan_in.angle_max - ) { + if (co_sine_map_.rows() != (int)n_pts || co_sine_map_angle_min_ != scan_in.angle_min || + co_sine_map_angle_max_ != scan_in.angle_max) + { ROS_DEBUG_NAMED("StaticLaserScanPolygonFilter", "No precomputed map given. Computing one."); co_sine_map_ = Eigen::ArrayXXd(n_pts, 2); co_sine_map_angle_min_ = scan_in.angle_min; @@ -446,18 +460,19 @@ void StaticLaserScanPolygonFilter::checkCoSineMap(const sensor_msgs::LaserScan& // Spherical->Cartesian projection for (size_t i = 0; i < n_pts; ++i) { - co_sine_map_(i, 0) = cos(scan_in.angle_min + (double) i * scan_in.angle_increment); - co_sine_map_(i, 1) = sin(scan_in.angle_min + (double) i * scan_in.angle_increment); + co_sine_map_(i, 0) = cos(scan_in.angle_min + (double)i * scan_in.angle_increment); + co_sine_map_(i, 1) = sin(scan_in.angle_min + (double)i * scan_in.angle_increment); } } } // Note: This implementation transforms the polygon relative to the laser. -// It does this lazily and only once. This has the advantage that the check if points fall inside the polygon is fast -// as the transform is not needed there. Furthermore, it means that the filter chain node -// does not need to be continuously subscribed to the transform topic, which significantly reduces CPU load. -// A pre-requisite for this to work is that the transform is static, i.e. the position and orientation of the laser with regard to -// the base of the robot does not change. +// It does this lazily and only once. This has the advantage that the check if points fall inside +// the polygon is fast as the transform is not needed there. Furthermore, it means that the filter +// chain node does not need to be continuously subscribed to the transform topic, which +// significantly reduces CPU load. A pre-requisite for this to work is that the transform is static, +// i.e. the position and orientation of the laser with regard to the base of the robot does not +// change. bool StaticLaserScanPolygonFilter::update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& output_scan) { @@ -465,28 +480,25 @@ bool StaticLaserScanPolygonFilter::update(const sensor_msgs::LaserScan& input_sc publishPolygon(); - if (!is_polygon_transformed_) { + if (!is_polygon_transformed_) + { tf::TransformListener transform_listener; std::string error_msg; - ROS_DEBUG_NAMED( - "StaticLaserScanPolygonFilter", "waitForTransform %s -> %s", - polygon_frame_.c_str(), input_scan.header.frame_id.c_str() - ); + ROS_DEBUG_NAMED("StaticLaserScanPolygonFilter", "waitForTransform %s -> %s", + polygon_frame_.c_str(), input_scan.header.frame_id.c_str()); bool success = transform_listener.waitForTransform( - input_scan.header.frame_id, polygon_frame_, - ros::Time(), // No restrictions on transform time. It is static. - ros::Duration(transform_timeout_), - ros::Duration(0), // This setting has no effect - &error_msg - ); + input_scan.header.frame_id, polygon_frame_, + ros::Time(), // No restrictions on transform time. It is static. + ros::Duration(transform_timeout_), + ros::Duration(0), // This setting has no effect + &error_msg); if (!success) { - ROS_WARN_THROTTLE_NAMED( - 1, "StaticLaserScanPolygonFilter", - "Could not get transform, ignoring laser scan! %s", error_msg.c_str() - ); + ROS_WARN_THROTTLE_NAMED(1, "StaticLaserScanPolygonFilter", + "Could not get transform, ignoring laser scan! %s", + error_msg.c_str()); return false; } else @@ -494,15 +506,17 @@ bool StaticLaserScanPolygonFilter::update(const sensor_msgs::LaserScan& input_sc ROS_INFO_NAMED("StaticLaserScanPolygonFilter", "Obtained transform"); } - try { - // Transform each point of polygon. This includes multiple type convertions because of transformPoint API requiring Stamped - // which does not in turn expose coordinate values + try + { + // Transform each point of polygon. This includes multiple type convertions because of + // transformPoint API requiring Stamped which does not in turn expose coordinate values for (int i = 0; i < polygon_.points.size(); ++i) { tf::Point point(polygon_.points[i].x, polygon_.points[i].y, 0); tf::Stamped point_stamped(point, ros::Time(), polygon_frame_); tf::Stamped point_stamped_new; - transform_listener.transformPoint(input_scan.header.frame_id, point_stamped, point_stamped_new); + transform_listener.transformPoint(input_scan.header.frame_id, point_stamped, + point_stamped_new); geometry_msgs::PointStamped result_point; tf::pointStampedTFToMsg(point_stamped_new, result_point); polygon_.points[i].x = result_point.point.x; @@ -513,7 +527,8 @@ bool StaticLaserScanPolygonFilter::update(const sensor_msgs::LaserScan& input_sc } catch (tf::TransformException& ex) { - ROS_WARN_THROTTLE_NAMED(1, "StaticLaserScanPolygonFilter", "Exception while transforming polygon"); + ROS_WARN_THROTTLE_NAMED(1, "StaticLaserScanPolygonFilter", + "Exception while transforming polygon"); return false; } } @@ -543,9 +558,10 @@ bool StaticLaserScanPolygonFilter::update(const sensor_msgs::LaserScan& input_sc return true; } -void StaticLaserScanPolygonFilter::reconfigureCB(laser_filters::PolygonFilterConfig& config, uint32_t level) +void StaticLaserScanPolygonFilter::reconfigureCB(laser_filters::PolygonFilterConfig& config, + uint32_t level) { is_polygon_transformed_ = false; LaserScanPolygonFilterBase::reconfigureCB(config, level); } -} +} // namespace laser_filters From 431c31f79ae4703cf892ba448ba574e3b9dfef41 Mon Sep 17 00:00:00 2001 From: Giorgos Tsamis Date: Thu, 21 Mar 2024 12:49:14 +0200 Subject: [PATCH 3/4] Revert auto-formatting --- src/polygon_filter.cpp | 289 +++++++++++++++++++---------------------- 1 file changed, 136 insertions(+), 153 deletions(-) diff --git a/src/polygon_filter.cpp b/src/polygon_filter.cpp index 19e8262..e00258d 100644 --- a/src/polygon_filter.cpp +++ b/src/polygon_filter.cpp @@ -51,14 +51,17 @@ #include /** @brief Same as sign(x) but returns 0 if x is 0. */ -inline double sign0(double x) { return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0); } +inline double sign0(double x) +{ + return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0); +} void padPolygon(geometry_msgs::Polygon& polygon, double padding) { // pad polygon in place for (unsigned int i = 0; i < polygon.points.size(); i++) { - geometry_msgs::Point32& pt = polygon.points[i]; + geometry_msgs::Point32& pt = polygon.points[ i ]; pt.x += sign0(pt.x) * padding; pt.y += sign0(pt.y) * padding; } @@ -67,8 +70,7 @@ void padPolygon(geometry_msgs::Polygon& polygon, double padding) 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) + 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.", @@ -85,15 +87,12 @@ geometry_msgs::Polygon makePolygonFromXMLRPC(const XmlRpc::XmlRpcValue& polygon_ 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]]"); + 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; @@ -104,14 +103,11 @@ geometry_msgs::Polygon makePolygonFromXMLRPC(const XmlRpc::XmlRpcValue& polygon_ 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"); + 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); @@ -123,8 +119,7 @@ geometry_msgs::Polygon makePolygonFromXMLRPC(const XmlRpc::XmlRpcValue& polygon_ } std::vector > 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 +{ // Source: https://github.com/ros-planning/navigation/blob/melodic-devel/costmap_2d/src/array_parser.cpp std::vector > result; std::stringstream input_ss(input); @@ -134,51 +129,51 @@ std::vector > parseVVF(const std::string& input, std::string& { 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; + 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; } } @@ -194,59 +189,54 @@ std::vector > parseVVF(const std::string& input, std::string& return result; } -geometry_msgs::Polygon makePolygonFromString(const std::string& polygon_string, - const geometry_msgs::Polygon& last_polygon) +geometry_msgs::Polygon makePolygonFromString(const std::string& polygon_string, const geometry_msgs::Polygon& last_polygon) { std::string error; std::vector > 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; + 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; + } - // 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; - } + geometry_msgs::Polygon polygon; + geometry_msgs::Point32 point; - for (unsigned int i = 0; i < vvf.size(); i++) - { - if (vvf[i].size() == 2) + // convert vvf into points. + if (vvf.size() < 3 && vvf.size() > 0) { - point.x = vvf[i][0]; - point.y = vvf[i][1]; - point.z = 0; - polygon.points.push_back(point); + ROS_WARN("You must specify at least three points for the robot polygon"); + return last_polygon; } - else + + for (unsigned int i = 0; i < vvf.size(); i++) { - 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; + 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; + return polygon; } std::string polygonToString(geometry_msgs::Polygon polygon) { std::string polygon_string = "["; bool first = true; - for (auto point : polygon.points) - { - if (!first) - { + for (auto point : polygon.points) { + if (!first) { polygon_string += ", "; } first = false; @@ -256,8 +246,7 @@ std::string polygonToString(geometry_msgs::Polygon polygon) return polygon_string; } -namespace laser_filters -{ +namespace laser_filters{ bool LaserScanPolygonFilterBase::configure() { @@ -266,10 +255,9 @@ bool LaserScanPolygonFilterBase::configure() PolygonFilterConfig param_config; ros::NodeHandle private_nh("~" + getName()); - dyn_server_.reset( - new dynamic_reconfigure::Server(own_mutex_, private_nh)); + dyn_server_.reset(new dynamic_reconfigure::Server(own_mutex_, private_nh)); dynamic_reconfigure::Server::CallbackType f; - f = [this](auto& config, auto level) { reconfigureCB(config, level); }; + f = [this](auto& config, auto level){ reconfigureCB(config, level); }; bool polygon_set = getParam("polygon", polygon_xmlrpc); bool polygon_frame_set = getParam("polygon_frame", polygon_frame_); @@ -285,7 +273,6 @@ bool LaserScanPolygonFilterBase::configure() param_config.invert = invert_filter_; dyn_server_->updateConfig(param_config); - // Calling setCallback(f) here calls reconfigureCB() which updates the polygon padding. dyn_server_->setCallback(f); polygon_pub_ = private_nh.advertise("polygon", 1, true); @@ -309,16 +296,14 @@ bool LaserScanPolygonFilterBase::configure() } // See https://web.cs.ucdavis.edu/~okreylos/TAship/Spring2000/PointInPolygon.html -bool LaserScanPolygonFilterBase::inPolygon(tf::Point& point) const -{ +bool LaserScanPolygonFilterBase::inPolygon(tf::Point& point) const { int i, j; bool c = false; for (i = 0, j = polygon_.points.size() - 1; i < polygon_.points.size(); j = i++) { if ((polygon_.points.at(i).y > point.y() != (polygon_.points.at(j).y > point.y()) && - (point.x() < (polygon_.points[j].x - polygon_.points[i].x) * - (point.y() - polygon_.points[i].y) / + (point.x() < (polygon_.points[j].x - polygon_.points[i].x) * (point.y() - polygon_.points[i].y) / (polygon_.points[j].y - polygon_.points[i].y) + polygon_.points[i].x))) c = !c; @@ -339,8 +324,7 @@ void LaserScanPolygonFilterBase::publishPolygon() } } -void LaserScanPolygonFilterBase::reconfigureCB(laser_filters::PolygonFilterConfig& config, - uint32_t level) +void LaserScanPolygonFilterBase::reconfigureCB(laser_filters::PolygonFilterConfig& config, uint32_t level) { invert_filter_ = config.invert; polygon_ = makePolygonFromString(config.polygon, polygon_); @@ -363,8 +347,7 @@ bool LaserScanPolygonFilter::update(const sensor_msgs::LaserScan& input_scan, bool success = tf_.waitForTransform( polygon_frame_, input_scan.header.frame_id, - input_scan.header.stamp + - ros::Duration().fromSec(input_scan.ranges.size() * input_scan.time_increment), + input_scan.header.stamp + ros::Duration().fromSec(input_scan.ranges.size() * input_scan.time_increment), ros::Duration(1.0), ros::Duration(0.01), &error_msg); if (!success) { @@ -439,7 +422,7 @@ bool StaticLaserScanPolygonFilter::configure() { is_polygon_transformed_ = false; - transform_timeout_ = 5; // Default + transform_timeout_ = 5; // Default getParam("transform_timeout", transform_timeout_); return LaserScanPolygonFilterBase::configure(); @@ -449,9 +432,11 @@ void StaticLaserScanPolygonFilter::checkCoSineMap(const sensor_msgs::LaserScan& { size_t n_pts = scan_in.ranges.size(); - if (co_sine_map_.rows() != (int)n_pts || co_sine_map_angle_min_ != scan_in.angle_min || - co_sine_map_angle_max_ != scan_in.angle_max) - { + if ( + co_sine_map_.rows() != (int)n_pts || + co_sine_map_angle_min_ != scan_in.angle_min || + co_sine_map_angle_max_ != scan_in.angle_max + ) { ROS_DEBUG_NAMED("StaticLaserScanPolygonFilter", "No precomputed map given. Computing one."); co_sine_map_ = Eigen::ArrayXXd(n_pts, 2); co_sine_map_angle_min_ = scan_in.angle_min; @@ -460,19 +445,18 @@ void StaticLaserScanPolygonFilter::checkCoSineMap(const sensor_msgs::LaserScan& // Spherical->Cartesian projection for (size_t i = 0; i < n_pts; ++i) { - co_sine_map_(i, 0) = cos(scan_in.angle_min + (double)i * scan_in.angle_increment); - co_sine_map_(i, 1) = sin(scan_in.angle_min + (double)i * scan_in.angle_increment); + co_sine_map_(i, 0) = cos(scan_in.angle_min + (double) i * scan_in.angle_increment); + co_sine_map_(i, 1) = sin(scan_in.angle_min + (double) i * scan_in.angle_increment); } } } // Note: This implementation transforms the polygon relative to the laser. -// It does this lazily and only once. This has the advantage that the check if points fall inside -// the polygon is fast as the transform is not needed there. Furthermore, it means that the filter -// chain node does not need to be continuously subscribed to the transform topic, which -// significantly reduces CPU load. A pre-requisite for this to work is that the transform is static, -// i.e. the position and orientation of the laser with regard to the base of the robot does not -// change. +// It does this lazily and only once. This has the advantage that the check if points fall inside the polygon is fast +// as the transform is not needed there. Furthermore, it means that the filter chain node +// does not need to be continuously subscribed to the transform topic, which significantly reduces CPU load. +// A pre-requisite for this to work is that the transform is static, i.e. the position and orientation of the laser with regard to +// the base of the robot does not change. bool StaticLaserScanPolygonFilter::update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& output_scan) { @@ -480,25 +464,28 @@ bool StaticLaserScanPolygonFilter::update(const sensor_msgs::LaserScan& input_sc publishPolygon(); - if (!is_polygon_transformed_) - { + if (!is_polygon_transformed_) { tf::TransformListener transform_listener; std::string error_msg; - ROS_DEBUG_NAMED("StaticLaserScanPolygonFilter", "waitForTransform %s -> %s", - polygon_frame_.c_str(), input_scan.header.frame_id.c_str()); + ROS_DEBUG_NAMED( + "StaticLaserScanPolygonFilter", "waitForTransform %s -> %s", + polygon_frame_.c_str(), input_scan.header.frame_id.c_str() + ); bool success = transform_listener.waitForTransform( - input_scan.header.frame_id, polygon_frame_, - ros::Time(), // No restrictions on transform time. It is static. - ros::Duration(transform_timeout_), - ros::Duration(0), // This setting has no effect - &error_msg); + input_scan.header.frame_id, polygon_frame_, + ros::Time(), // No restrictions on transform time. It is static. + ros::Duration(transform_timeout_), + ros::Duration(0), // This setting has no effect + &error_msg + ); if (!success) { - ROS_WARN_THROTTLE_NAMED(1, "StaticLaserScanPolygonFilter", - "Could not get transform, ignoring laser scan! %s", - error_msg.c_str()); + ROS_WARN_THROTTLE_NAMED( + 1, "StaticLaserScanPolygonFilter", + "Could not get transform, ignoring laser scan! %s", error_msg.c_str() + ); return false; } else @@ -506,17 +493,15 @@ bool StaticLaserScanPolygonFilter::update(const sensor_msgs::LaserScan& input_sc ROS_INFO_NAMED("StaticLaserScanPolygonFilter", "Obtained transform"); } - try - { - // Transform each point of polygon. This includes multiple type convertions because of - // transformPoint API requiring Stamped which does not in turn expose coordinate values + try { + // Transform each point of polygon. This includes multiple type convertions because of transformPoint API requiring Stamped + // which does not in turn expose coordinate values for (int i = 0; i < polygon_.points.size(); ++i) { tf::Point point(polygon_.points[i].x, polygon_.points[i].y, 0); tf::Stamped point_stamped(point, ros::Time(), polygon_frame_); tf::Stamped point_stamped_new; - transform_listener.transformPoint(input_scan.header.frame_id, point_stamped, - point_stamped_new); + transform_listener.transformPoint(input_scan.header.frame_id, point_stamped, point_stamped_new); geometry_msgs::PointStamped result_point; tf::pointStampedTFToMsg(point_stamped_new, result_point); polygon_.points[i].x = result_point.point.x; @@ -527,8 +512,7 @@ bool StaticLaserScanPolygonFilter::update(const sensor_msgs::LaserScan& input_sc } catch (tf::TransformException& ex) { - ROS_WARN_THROTTLE_NAMED(1, "StaticLaserScanPolygonFilter", - "Exception while transforming polygon"); + ROS_WARN_THROTTLE_NAMED(1, "StaticLaserScanPolygonFilter", "Exception while transforming polygon"); return false; } } @@ -558,10 +542,9 @@ bool StaticLaserScanPolygonFilter::update(const sensor_msgs::LaserScan& input_sc return true; } -void StaticLaserScanPolygonFilter::reconfigureCB(laser_filters::PolygonFilterConfig& config, - uint32_t level) +void StaticLaserScanPolygonFilter::reconfigureCB(laser_filters::PolygonFilterConfig& config, uint32_t level) { is_polygon_transformed_ = false; LaserScanPolygonFilterBase::reconfigureCB(config, level); } -} // namespace laser_filters +} \ No newline at end of file From aaecd18e79d98855a4e0d975c947c0d8e8816ded Mon Sep 17 00:00:00 2001 From: Giorgos Tsamis Date: Sun, 24 Mar 2024 12:51:16 +0200 Subject: [PATCH 4/4] Add end line back --- src/polygon_filter.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/polygon_filter.cpp b/src/polygon_filter.cpp index e00258d..3edf976 100644 --- a/src/polygon_filter.cpp +++ b/src/polygon_filter.cpp @@ -273,6 +273,7 @@ bool LaserScanPolygonFilterBase::configure() param_config.invert = invert_filter_; dyn_server_->updateConfig(param_config); + // Calling setCallback(f) here calls reconfigureCB() which updates the polygon padding. dyn_server_->setCallback(f); polygon_pub_ = private_nh.advertise("polygon", 1, true); @@ -547,4 +548,4 @@ void StaticLaserScanPolygonFilter::reconfigureCB(laser_filters::PolygonFilterCon is_polygon_transformed_ = false; LaserScanPolygonFilterBase::reconfigureCB(config, level); } -} \ No newline at end of file +}