diff --git a/CMakeLists.txt b/CMakeLists.txt index e10ed96..d2d2ba1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ project(obstacle_detector) set(CMAKE_CXX_FLAGS "-std=c++11 -fpermissive ${CMAKE_CXX_FLAGS} -Wfatal-errors\ ") -find_package(catkin REQUIRED COMPONENTS roscpp roslaunch nodelet rviz std_msgs nav_msgs geometry_msgs sensor_msgs std_srvs tf laser_geometry message_generation) +find_package(catkin REQUIRED COMPONENTS roscpp roslaunch nodelet rviz std_msgs std_srvs geometry_msgs sensor_msgs tf laser_geometry message_generation) find_package(Armadillo REQUIRED) find_package(Boost 1.54.0 REQUIRED system) @@ -13,7 +13,7 @@ generate_messages(DEPENDENCIES std_msgs geometry_msgs) catkin_package( INCLUDE_DIRS include LIBRARIES scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui - CATKIN_DEPENDS roscpp nodelet rviz std_msgs nav_msgs geometry_msgs sensor_msgs std_srvs tf laser_geometry message_runtime + CATKIN_DEPENDS roscpp nodelet rviz std_msgs std_srvs geometry_msgs sensor_msgs tf laser_geometry message_runtime ) include_directories(include ${catkin_INCLUDE_DIRS} ${ARMADILLO_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) diff --git a/README.md b/README.md index 3ca18d1..d3913a6 100644 --- a/README.md +++ b/README.md @@ -16,8 +16,7 @@ The package requires [Armadillo C++](http://arma.sourceforge.net) library for co 1.1 The scans_merger 1.2 The obstacle_extractor 1.3 The obstacle_tracker - 1.4 The obstacle_publisher - 1.5 The obstacle_recorder + 1.4 The obstacle_publisher 2. The messages 3. Launch files 4. The displays @@ -40,6 +39,8 @@ For the ease of use it is recomended to use appropriate Rviz panels provided for This node converts two messages of type `sensor_msgs/LaserScan` from topics `front_scan` and `rear_scan` into a single laser scan of the same type, published under topic `scan` and/or a point cloud of type `sensor_msgs/PointCloud`, published under topic `pcl`. The difference between both is that the resulting laser scan divides the area into finite number of circular sectors and put one point (or actually one range value) in each section occupied by some measured points, whereas the resulting point cloud simply copies all of the points obtained from sensors. +The input laser scans are firstly rectified to incorporate the motion of the scanner in time. + -----------------------

Visual example of obstacle detector output @@ -48,23 +49,22 @@ This node converts two messages of type `sensor_msgs/LaserScan` from topics `fro

----------------------- -The resulting messages contain geometric data described with respect to a specific coordinate frame (e.g. `scanners_base`). Assuming that the coordinate frames attached to two laser scanners are called `front_scanner` and `rear_scanner`, both transformation from `scanners_base` frame to `front_scanner` frame and from `scanners_base` frame to `rear_scanner` frame must be provided. The node allows to artificially restrict measured points to some rectangular region around the `scanners_base` frame as well as to limit the resulting laser scan range. The points falling behind this region or ranges excluded from the limit will be discarded. +The resulting messages contain geometric data described with respect to a specific coordinate frame (e.g. `robot`). Assuming that the coordinate frames attached to two laser scanners are called `front_scanner` and `rear_scanner`, both transformation from `robot` frame to `front_scanner` frame and from `robot` frame to `rear_scanner` frame must be provided. The node allows to artificially restrict measured points to some rectangular region around the `robot` frame as well as to limit the resulting laser scan range. The points falling behind this region will be discarded. -Even if only one laser scanner is used, the node can be useful for simple data pre-processing, e.g. range restriction or recalculation of points to a different coordinate frame. The node uses the following set of local parameters: +Even if only one laser scanner is used, the node can be useful for simple data pre-processing, e.g. rectification, range restriction or recalculation of points to a different coordinate frame. The node uses the following set of local parameters: * `~active` (`bool`, default: `true`) - active/sleep mode, -* `~publish_scan` (`bool`, default: `true`) - publish the merged laser scan message, -* `~publish_pcl` (`bool`, default: `false`) - publish the merged point cloud message, +* `~publish_scan` (`bool`, default: `false`) - publish the merged laser scan message, +* `~publish_pcl` (`bool`, default: `true`) - publish the merged point cloud message, * `~ranges_num` (`int`, default: `1000`) - number of ranges (circular sectors) contained in the 360 deg laser scan message, * `~min_scanner_range` (`double`, default: `0.05`) - minimal allowable range value for produced laser scan message, * `~max_scanner_range` (`double`, default: `10.0`) - maximal allowable range value for produced laser scan message, -* `~max_x_range` (`double`, default: `10.0`) - limitation for points coordinates (points with coordinates behind these limitations will be discarded), -* `~min_x_range` (`double`, default: `-10.0`) - as above, -* `~max_y_range` (`double`, default: `10.0`) - as above, +* `~min_x_range` (`double`, default: `-10.0`) - limitation for points coordinates (points with coordinates behind these limitations will be discarded), +* `~max_x_range` (`double`, default: `10.0`) - as above, * `~min_y_range` (`double`, default: `-10.0`) - as above, -* `~front_scan_frame_id` (`string`, default: `front_scanner`) - name of the coordinate frame attached to the front scanner, -* `~rear_scan_frame_id` (`string`, default: `rear_scanner`) - name of the coordinate frame attached to the rear scanner, -* `~target_frame_id` (`string`, default: `scanners_base`) - name of the coordinate frame used as the origin for the produced laser scan or point cloud. +* `~max_y_range` (`double`, default: `10.0`) - as above, +* `~fixed_frame_id` (`string`, default: `map`) - name of the fixed coordinate frame used for scan rectification in time (see `laser_geometry` package), +* `~target_frame_id` (`string`, default: `robot`) - name of the coordinate frame used as the origin for the produced laser scan or point cloud. The package comes with Rviz panel for this node. @@ -93,23 +93,23 @@ The input points are firstly grouped into subsets. The algorithm extracts segmen The node is configurable with the following set of local parameters: * `~active` (`bool`, default: `true`) - active/sleep mode, -* `~use_scan` (`bool`, default: `true`) - use laser scan messages, -* `~use_pcl` (`bool`, default: `false`) - use point cloud messages (if both scan and pcl are chosen, scans will have priority), +* `~use_scan` (`bool`, default: `false`) - use laser scan messages, +* `~use_pcl` (`bool`, default: `true`) - use point cloud messages (if both scan and pcl are chosen, scans will have priority), * `~discard_converted_segments` (`bool`, default: `true`) - do not publish segments, from which the circles were spawned, * `~transform_coordinates` (`bool`, default: `true`) - transform the coordinates of obstacles to a frame described with `frame_id` parameter, * `~frame_id` (`string`, default: `map`) - name of the coordinate frame used as origin for produced obstacles (used only if `transform_coordinates` flag is set to true). The following set of local parameters is dedicated to the algorithm itself: -* `~use_split_and_merge` (`bool`, default: `false`) - choose wether to use Iterative End Point Fit (false) or Split And Merge (true) algorithm to detect segments, +* `~use_split_and_merge` (`bool`, default: `true`) - choose wether to use Iterative End Point Fit (false) or Split And Merge (true) algorithm to detect segments, * `~min_group_points` (`int`, default: `5`) - minimum number of points comprising a group to be further processed, -* `~max_group_distance` (`double`, default: `0.100`) - if the distance between two points is greater than this value, start a new group, -* `~distance_proportion` (`double`, default: `0.006136`) - enlarge the allowable distance between points proportionally to the range of point (use scan angle increment in radians), -* `~max_split_distance` (`double`, default: `0.070`) - if a point in group lays further from a leading line than this value, split the group, -* `~max_merge_separation` (`double`, default: `0.150`) - if distance between obstacles is smaller than this value, consider merging them, -* `~max_merge_spread` (`double`, default: `0.070`) - merge two segments if all of their extreme points lay closer to the leading line than this value, -* `~max_circle_radius` (`double`, default: `0.500`) - if a circle would have greater radius than this value, skip it, -* `~radius_enlargement` (`double`, default: `0.100`) - artificially enlarge the circles radius by this value. +* `~max_group_distance` (`double`, default: `0.1`) - if the distance between two points is greater than this value, start a new group, +* `~distance_proportion` (`double`, default: `0.00628`) - enlarge the allowable distance between points proportionally to the range of point (use scan angle increment in radians), +* `~max_split_distance` (`double`, default: `0.2`) - if a point in group lays further from a leading line than this value, split the group, +* `~max_merge_separation` (`double`, default: `0.2`) - if distance between obstacles is smaller than this value, consider merging them, +* `~max_merge_spread` (`double`, default: `0.2`) - merge two segments if all of their extreme points lay closer to the leading line than this value, +* `~max_circle_radius` (`double`, default: `0.6`) - if a circle would have greater radius than this value, skip it, +* `~radius_enlargement` (`double`, default: `0.3`) - artificially enlarge the circles radius by this value. The package comes with Rviz panel for this node. diff --git a/include/obstacle_detector/obstacle_extractor.h b/include/obstacle_detector/obstacle_extractor.h index d23e586..bc6c970 100644 --- a/include/obstacle_detector/obstacle_extractor.h +++ b/include/obstacle_detector/obstacle_extractor.h @@ -54,6 +54,7 @@ class ObstacleExtractor { public: ObstacleExtractor(ros::NodeHandle& nh, ros::NodeHandle& nh_local); + ~ObstacleExtractor(); private: bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); @@ -96,6 +97,7 @@ class ObstacleExtractor bool p_active_; bool p_use_scan_; bool p_use_pcl_; + bool p_use_split_and_merge_; bool p_discard_converted_segments_; bool p_transform_coordinates_; diff --git a/include/obstacle_detector/obstacle_publisher.h b/include/obstacle_detector/obstacle_publisher.h index e7d762e..eb10f3b 100644 --- a/include/obstacle_detector/obstacle_publisher.h +++ b/include/obstacle_detector/obstacle_publisher.h @@ -46,6 +46,7 @@ class ObstaclePublisher { public: ObstaclePublisher(ros::NodeHandle &nh, ros::NodeHandle &nh_local); + ~ObstaclePublisher(); private: bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); diff --git a/include/obstacle_detector/obstacle_tracker.h b/include/obstacle_detector/obstacle_tracker.h index bd48d51..e243eb2 100644 --- a/include/obstacle_detector/obstacle_tracker.h +++ b/include/obstacle_detector/obstacle_tracker.h @@ -51,6 +51,7 @@ namespace obstacle_detector class ObstacleTracker { public: ObstacleTracker(ros::NodeHandle& nh, ros::NodeHandle& nh_local); + ~ObstacleTracker(); private: bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); diff --git a/include/obstacle_detector/panels/obstacle_extractor_panel.h b/include/obstacle_detector/panels/obstacle_extractor_panel.h index fb618cc..9233a6d 100644 --- a/include/obstacle_detector/panels/obstacle_extractor_panel.h +++ b/include/obstacle_detector/panels/obstacle_extractor_panel.h @@ -95,15 +95,16 @@ private Q_SLOTS: ros::ServiceClient params_cli_; // Parameters - int p_min_group_points_; - bool p_active_; bool p_use_scan_; bool p_use_pcl_; + bool p_use_split_and_merge_; bool p_discard_converted_segments_; bool p_transform_coordinates_; + int p_min_group_points_; + double p_distance_proportion_; double p_max_group_distance_; diff --git a/include/obstacle_detector/scans_merger.h b/include/obstacle_detector/scans_merger.h index a81e376..0bbcd58 100644 --- a/include/obstacle_detector/scans_merger.h +++ b/include/obstacle_detector/scans_merger.h @@ -50,6 +50,7 @@ class ScansMerger { public: ScansMerger(ros::NodeHandle& nh, ros::NodeHandle& nh_local); + ~ScansMerger(); private: bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); diff --git a/launch/demo.launch b/launch/demo.launch index 6144c5e..8c6470d 100644 --- a/launch/demo.launch +++ b/launch/demo.launch @@ -27,9 +27,8 @@ - - - + + @@ -46,7 +45,7 @@ - + diff --git a/launch/nodelets.launch b/launch/nodelets.launch index c6a136a..b9f6888 100644 --- a/launch/nodelets.launch +++ b/launch/nodelets.launch @@ -5,44 +5,43 @@ - - - + + + - + - - + + - - - - + + + + - - - + + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + diff --git a/launch/nodes.launch b/launch/nodes.launch index 7efe5d9..ffd635b 100644 --- a/launch/nodes.launch +++ b/launch/nodes.launch @@ -5,8 +5,8 @@ - - + + @@ -18,9 +18,8 @@ - - - + + @@ -34,14 +33,14 @@ - - - - - - - - + + + + + + + + diff --git a/package.xml b/package.xml index bf18d37..f7cee95 100644 --- a/package.xml +++ b/package.xml @@ -17,11 +17,10 @@ roslaunch nodelet std_msgs - nav_msgs + std_srvs sensor_msgs geometry_msgs laser_geometry - std_srvs diff --git a/src/obstacle_extractor.cpp b/src/obstacle_extractor.cpp index 1bfec26..4a10f98 100644 --- a/src/obstacle_extractor.cpp +++ b/src/obstacle_extractor.cpp @@ -47,13 +47,40 @@ ObstacleExtractor::ObstacleExtractor(ros::NodeHandle& nh, ros::NodeHandle& nh_lo initialize(); } +ObstacleExtractor::~ObstacleExtractor() { + nh_local_.deleteParam("active"); + nh_local_.deleteParam("use_scan"); + nh_local_.deleteParam("use_pcl"); + + nh_local_.deleteParam("use_split_and_merge"); + nh_local_.deleteParam("discard_converted_segments"); + nh_local_.deleteParam("transform_coordinates"); + + nh_local_.deleteParam("min_group_points"); + + nh_local_.deleteParam("max_group_distance"); + nh_local_.deleteParam("distance_proportion"); + nh_local_.deleteParam("max_split_distance"); + nh_local_.deleteParam("max_merge_separation"); + nh_local_.deleteParam("max_merge_spread"); + nh_local_.deleteParam("max_circle_radius"); + nh_local_.deleteParam("radius_enlargement"); + + nh_local_.deleteParam("min_x_limit"); + nh_local_.deleteParam("max_x_limit"); + nh_local_.deleteParam("min_y_limit"); + nh_local_.deleteParam("max_y_limit"); + + nh_local_.deleteParam("frame_id"); +} + bool ObstacleExtractor::updateParams(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { bool prev_active = p_active_; nh_local_.param("active", p_active_, true); - nh_local_.param("use_scan", p_use_scan_, true); - nh_local_.param("use_pcl", p_use_pcl_, false); - nh_local_.param("use_split_and_merge", p_use_split_and_merge_, false); + nh_local_.param("use_scan", p_use_scan_, false); + nh_local_.param("use_pcl", p_use_pcl_, true); + nh_local_.param("use_split_and_merge", p_use_split_and_merge_, true); nh_local_.param("discard_converted_segments", p_discard_converted_segments_, true); nh_local_.param("transform_coordinates", p_transform_coordinates_, true); @@ -147,7 +174,7 @@ void ObstacleExtractor::groupPoints() { point_set.begin = input_points_.begin(); point_set.end = input_points_.begin(); point_set.num_points = 1; - point_set.is_visible = false; // Previous point is unknown + point_set.is_visible = true; for (PointIterator point = input_points_.begin()++; point != input_points_.end(); ++point) { double range = (*point).length(); @@ -180,40 +207,9 @@ void ObstacleExtractor::groupPoints() { } } - point_set.is_visible = false; // Next point is unknown detectSegments(point_set); // Check the last point set too! } -//void ObstacleExtractor::groupPoints() { -// PointSet point_set; - -// Point first_point = *input_points_.begin(); -// Point recent_point; -// for (PointIterator point = input_points_.begin(); point != input_points_.end(); ++point) { -// if (point_set.num_points != 0) { -// double r = (*point).length(); - -// if ((*point - *point_set.end).lengthSquared() > pow(p_max_group_distance_ + r * p_distance_proportion_, 2.0)) { -// detectSegments(point_set); -// point_set.num_points = -1; -// advance(point, -1); -// } -// else { -// point_set.end = point; -// } -// } -// else { -// point_set.begin = point; -// point_set.end = point; -// } - -// point_set.num_points++; -// recent_point = *point; -// } - -// detectSegments(point_set); // Check the last point set too! -//} - void ObstacleExtractor::detectSegments(const PointSet& point_set) { if (point_set.num_points < p_min_group_points_) return; @@ -279,7 +275,7 @@ void ObstacleExtractor::mergeSegments() { auto temp_itr = segments_.insert(i, merged_segment); segments_.erase(i); segments_.erase(j); - i = --temp_itr; + i = --temp_itr; // Check the new segment against others break; } } @@ -326,15 +322,15 @@ bool ObstacleExtractor::checkSegmentsCollinearity(const Segment& segment, const void ObstacleExtractor::detectCircles() { for (auto segment = segments_.begin(); segment != segments_.end(); ++segment) { -// bool segment_is_visible = true; -// for (const PointSet& ps : segment->point_sets) { -// if (!ps.is_visible) { -// segment_is_visible = false; -// break; -// } -// } -// if (!segment_is_visible) -// continue; + bool segment_is_visible = true; + for (const PointSet& ps : segment->point_sets) { + if (!ps.is_visible) { + segment_is_visible = false; + break; + } + } + if (!segment_is_visible) + continue; Circle circle(*segment); circle.radius += p_radius_enlargement_; @@ -446,7 +442,8 @@ void ObstacleExtractor::publishObstacles() { } for (const Circle& c : circles_) { - if (c.center.x > p_min_x_limit_ && c.center.x < p_max_x_limit_ && c.center.y > p_min_y_limit_ && c.center.y < p_max_y_limit_) { + if (c.center.x > p_min_x_limit_ && c.center.x < p_max_x_limit_ && + c.center.y > p_min_y_limit_ && c.center.y < p_max_y_limit_) { CircleObstacle circle; circle.center.x = c.center.x; diff --git a/src/obstacle_publisher.cpp b/src/obstacle_publisher.cpp index 2e0a9ad..9da44ea 100644 --- a/src/obstacle_publisher.cpp +++ b/src/obstacle_publisher.cpp @@ -47,6 +47,25 @@ ObstaclePublisher::ObstaclePublisher(ros::NodeHandle& nh, ros::NodeHandle& nh_lo initialize(); } +ObstaclePublisher::~ObstaclePublisher() { + nh_local_.deleteParam("active"); + nh_local_.deleteParam("reset"); + nh_local_.deleteParam("fusion_example"); + nh_local_.deleteParam("fission_example"); + + nh_local_.deleteParam("loop_rate"); + nh_local_.deleteParam("radius_margin"); + + nh_local_.deleteParam("x_vector"); + nh_local_.deleteParam("y_vector"); + nh_local_.deleteParam("r_vector"); + + nh_local_.deleteParam("vx_vector"); + nh_local_.deleteParam("vy_vector"); + + nh_local_.deleteParam("frame_id"); +} + bool ObstaclePublisher::updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { bool prev_active = p_active_; diff --git a/src/obstacle_tracker.cpp b/src/obstacle_tracker.cpp index 81360d9..52efe1b 100644 --- a/src/obstacle_tracker.cpp +++ b/src/obstacle_tracker.cpp @@ -48,6 +48,21 @@ ObstacleTracker::ObstacleTracker(ros::NodeHandle& nh, ros::NodeHandle& nh_local) initialize(); } +ObstacleTracker::~ObstacleTracker() { + nh_local_.deleteParam("active"); + nh_local_.deleteParam("copy_segments"); + + nh_local_.deleteParam("loop_rate"); + nh_local_.deleteParam("tracking_duration"); + nh_local_.deleteParam("min_correspondence_cost"); + nh_local_.deleteParam("std_correspondence_dev"); + nh_local_.deleteParam("process_variance"); + nh_local_.deleteParam("process_rate_variance"); + nh_local_.deleteParam("measurement_variance"); + + nh_local_.deleteParam("frame_id"); +} + bool ObstacleTracker::updateParams(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { bool prev_active = p_active_; @@ -65,7 +80,7 @@ bool ObstacleTracker::updateParams(std_srvs::Empty::Request &req, std_srvs::Empt nh_local_.param("process_rate_variance", p_process_rate_variance_, 0.1); nh_local_.param("measurement_variance", p_measurement_variance_, 1.0); - nh_local_.param("frame_id", p_frame_id_, "map"); + nh_local_.param("frame_id", p_frame_id_, string("map")); obstacles_.header.frame_id = p_frame_id_; TrackedObstacle::setSamplingTime(p_sampling_time_); diff --git a/src/panels/obstacle_extractor_panel.cpp b/src/panels/obstacle_extractor_panel.cpp index ad25192..ff29471 100644 --- a/src/panels/obstacle_extractor_panel.cpp +++ b/src/panels/obstacle_extractor_panel.cpp @@ -231,24 +231,25 @@ void ObstacleExtractorPanel::setParams() { } void ObstacleExtractorPanel::getParams() { - nh_local_.param("min_group_points", p_min_group_points_, 5); - - nh_local_.param("active", p_active_, true); - nh_local_.param("use_scan", p_use_scan_, true); - nh_local_.param("use_pcl", p_use_pcl_, false); - nh_local_.param("use_split_and_merge", p_use_split_and_merge_, false); - nh_local_.param("discard_converted_segments", p_discard_converted_segments_, true); - nh_local_.param("transform_coordinates", p_transform_coordinates_, true); - - nh_local_.param("max_group_distance", p_max_group_distance_, 0.100); - nh_local_.param("distance_proportion", p_distance_proportion_, 0.006136); - nh_local_.param("max_split_distance", p_max_split_distance_, 0.070); - nh_local_.param("max_merge_separation", p_max_merge_separation_, 0.150); - nh_local_.param("max_merge_spread", p_max_merge_spread_, 0.070); - nh_local_.param("max_circle_radius", p_max_circle_radius_, 0.300); - nh_local_.param("radius_enlargement", p_radius_enlargement_, 0.030); - - nh_local_.param("frame_id", p_frame_id_, "world"); + p_active_ = nh_local_.param("active", false); + p_use_scan_ = nh_local_.param("use_scan", false); + p_use_pcl_ = nh_local_.param("use_pcl", false); + + p_use_split_and_merge_ = nh_local_.param("use_split_and_merge", false); + p_discard_converted_segments_ = nh_local_.param("discard_converted_segments", false); + p_transform_coordinates_ = nh_local_.param("transform_coordinates", false); + + p_min_group_points_ = nh_local_.param("min_group_points", 0); + + p_max_group_distance_ = nh_local_.param("max_group_distance", 0.0); + p_distance_proportion_ = nh_local_.param("distance_proportion", 0.0); + p_max_split_distance_ = nh_local_.param("max_split_distance", 0.0); + p_max_merge_separation_ = nh_local_.param("max_merge_separation", 0.0); + p_max_merge_spread_ = nh_local_.param("max_merge_spread", 0.0); + p_max_circle_radius_ = nh_local_.param("max_circle_radius", 0.0); + p_radius_enlargement_ = nh_local_.param("radius_enlargement", 0.0); + + p_frame_id_ = nh_local_.param("frame_id", std::string("")); } void ObstacleExtractorPanel::evaluateParams() { diff --git a/src/panels/obstacle_publisher_panel.cpp b/src/panels/obstacle_publisher_panel.cpp index ce09a10..21e8cb1 100644 --- a/src/panels/obstacle_publisher_panel.cpp +++ b/src/panels/obstacle_publisher_panel.cpp @@ -231,12 +231,12 @@ void ObstaclePublisherPanel::setParams() { } void ObstaclePublisherPanel::getParams() { - nh_local_.param("active", p_active_, true); - nh_local_.param("reset", p_reset_, false); - nh_local_.param("fusion_example", p_fusion_example_, false); - nh_local_.param("fission_example", p_fission_example_, false); + p_active_ = nh_local_.param("active", false); + p_reset_ = nh_local_.param("reset", false); + p_fusion_example_ = nh_local_.param("fusion_example", false); + p_fission_example_ = nh_local_.param("fission_example", false); - nh_local_.param("loop_rate", p_loop_rate_, 10.0); + p_loop_rate_ = nh_local_.param("loop_rate", 0.0); nh_local_.getParam("x_vector", p_x_vector_); nh_local_.getParam("y_vector", p_y_vector_); @@ -245,7 +245,7 @@ void ObstaclePublisherPanel::getParams() { nh_local_.getParam("vx_vector", p_vx_vector_); nh_local_.getParam("vy_vector", p_vy_vector_); - nh_local_.getParam("frame_id", p_frame_id_); + p_frame_id_ = nh_local_.param("frame_id", std::string("")); } void ObstaclePublisherPanel::evaluateParams() { diff --git a/src/panels/obstacle_tracker_panel.cpp b/src/panels/obstacle_tracker_panel.cpp index 3e5f515..8e5a481 100644 --- a/src/panels/obstacle_tracker_panel.cpp +++ b/src/panels/obstacle_tracker_panel.cpp @@ -171,15 +171,15 @@ void ObstacleTrackerPanel::setParams() { } void ObstacleTrackerPanel::getParams() { - nh_local_.param("active", p_active_, true); - - nh_local_.param("loop_rate", p_loop_rate_, 100.0); - nh_local_.param("tracking_duration", p_tracking_duration_, 2.0); - nh_local_.param("min_correspondence_cost", p_min_correspondence_cost_, 0.3); - nh_local_.param("std_correspondence_dev", p_std_correspondence_dev_, 0.15); - nh_local_.param("process_variance", p_process_variance_, 0.001); - nh_local_.param("process_rate_variance", p_process_rate_variance_, 0.01); - nh_local_.param("measurement_variance", p_measurement_variance_, 1.0); + p_active_ = nh_local_.param("active", false); + + p_loop_rate_ = nh_local_.param("loop_rate", 0.0); + p_tracking_duration_ = nh_local_.param("tracking_duration", 0.0); + p_min_correspondence_cost_ = nh_local_.param("min_correspondence_cost", 0.0); + p_std_correspondence_dev_ = nh_local_.param("std_correspondence_dev", 0.0); + p_process_variance_ = nh_local_.param("process_variance", 0.0); + p_process_rate_variance_ = nh_local_.param("process_rate_variance", 0.0); + p_measurement_variance_ = nh_local_.param("measurement_variance", 0.0); } void ObstacleTrackerPanel::evaluateParams() { diff --git a/src/panels/scans_merger_panel.cpp b/src/panels/scans_merger_panel.cpp index 652b9ab..cc1969f 100644 --- a/src/panels/scans_merger_panel.cpp +++ b/src/panels/scans_merger_panel.cpp @@ -222,38 +222,21 @@ void ScansMergerPanel::setParams() { } void ScansMergerPanel::getParams() { - if (!nh_local_.getParam("active", p_active_)) - p_active_ = false; - - if (!nh_local_.getParam("publish_scan", p_publish_scan_)) - p_publish_scan_ = false; - - if (!nh_local_.getParam("publish_pcl", p_publish_pcl_)) - p_publish_pcl_ = false; - - if (!nh_local_.getParam("ranges_num", p_ranges_num_)) - p_ranges_num_ = 0; - - if (!nh_local_.getParam("min_scanner_range", p_min_scanner_range_)) - p_min_scanner_range_ = 0.0; - - if (!nh_local_.getParam("max_scanner_range", p_max_scanner_range_)) - p_max_scanner_range_ = 0.0; - - if (!nh_local_.getParam("max_x_range", p_max_x_range_)) - p_max_x_range_ = 0.0; + p_active_ = nh_local_.param("active", false); + p_publish_scan_ = nh_local_.param("publish_scan", false); + p_publish_pcl_ = nh_local_.param("publish_pcl", false); - if (!nh_local_.getParam("min_x_range", p_min_x_range_)) - p_min_x_range_ = 0.0; + p_ranges_num_ = nh_local_.param("publish_pcl", 0); - if (!nh_local_.getParam("max_y_range", p_max_y_range_)) - p_max_y_range_ = 0.0; + p_min_scanner_range_ = nh_local_.param("min_scanner_range", 0.0); + p_max_scanner_range_ = nh_local_.param("max_scanner_range", 0.0); - if (!nh_local_.getParam("min_y_range", p_min_y_range_)) - p_min_y_range_ = 0.0; + p_min_x_range_ = nh_local_.param("min_x_range", 0.0); + p_max_x_range_ = nh_local_.param("max_x_range", 0.0); + p_min_y_range_ = nh_local_.param("min_y_range", 0.0); + p_max_y_range_ = nh_local_.param("max_y_range", 0.0); - if (!nh_local_.getParam("target_frame_id", p_target_frame_id_)) - p_target_frame_id_ = std::string("-"); + p_target_frame_id_ = nh_local_.param("target_frame_id", std::string("")); } void ScansMergerPanel::evaluateParams() { diff --git a/src/scans_merger.cpp b/src/scans_merger.cpp index fc15d08..9c2f9b6 100644 --- a/src/scans_merger.cpp +++ b/src/scans_merger.cpp @@ -52,12 +52,31 @@ ScansMerger::ScansMerger(ros::NodeHandle& nh, ros::NodeHandle& nh_local) : nh_(n initialize(); } +ScansMerger::~ScansMerger() { + nh_local_.deleteParam("active"); + nh_local_.deleteParam("publish_scan"); + nh_local_.deleteParam("publish_pcl"); + + nh_local_.deleteParam("ranges_num"); + + nh_local_.deleteParam("min_scanner_range"); + nh_local_.deleteParam("max_scanner_range"); + + nh_local_.deleteParam("min_x_range"); + nh_local_.deleteParam("max_x_range"); + nh_local_.deleteParam("min_y_range"); + nh_local_.deleteParam("max_y_range"); + + nh_local_.deleteParam("fixed_frame_id"); + nh_local_.deleteParam("target_frame_id"); +} + bool ScansMerger::updateParams(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { bool prev_active = p_active_; nh_local_.param("active", p_active_, true); - nh_local_.param("publish_scan", p_publish_scan_, true); - nh_local_.param("publish_pcl", p_publish_pcl_, false); + nh_local_.param("publish_scan", p_publish_scan_, false); + nh_local_.param("publish_pcl", p_publish_pcl_, true); nh_local_.param("ranges_num", p_ranges_num_, 1000);