├── .gitignore ├── CMakeLists.txt ├── README.md ├── icons └── classes │ ├── Obstacle Extractor.png │ ├── Obstacle Publisher.png │ ├── Obstacle Tracker.png │ ├── Obstacles.png │ └── Scans Merger.png ├── include └── obstacle_detector │ ├── displays │ ├── circle_visual.h │ ├── obstacles_display.h │ └── segment_visual.h │ ├── obstacle_extractor.h │ ├── obstacle_publisher.h │ ├── obstacle_tracker.h │ ├── panels │ ├── obstacle_extractor_panel.h │ ├── obstacle_publisher_panel.h │ ├── obstacle_tracker_panel.h │ └── scans_merger_panel.h │ ├── scans_merger.h │ └── utilities │ ├── circle.h │ ├── figure_fitting.h │ ├── kalman.h │ ├── math_utilities.h │ ├── point.h │ ├── point_set.h │ ├── segment.h │ └── tracked_obstacle.h ├── launch ├── demo.launch ├── nodelets.launch └── nodes.launch ├── msg ├── CircleObstacle.msg ├── Obstacles.msg └── SegmentObstacle.msg ├── nodelet_plugins.xml ├── package.xml ├── resources ├── ObstacleDetector.pdf ├── Using hokuyo_node with udev.txt ├── obstacle_detector.rviz ├── play.png ├── scans_demo.bag └── stop.png ├── rviz_plugins.xml └── src ├── displays ├── circle_visual.cpp ├── obstacles_display.cpp └── segment_visual.cpp ├── nodelets ├── obstacle_extractor_nodelet.cpp ├── obstacle_publisher_nodelet.cpp ├── obstacle_tracker_nodelet.cpp └── scans_merger_nodelet.cpp ├── nodes ├── obstacle_extractor_node.cpp ├── obstacle_publisher_node.cpp ├── obstacle_tracker_node.cpp └── scans_merger_node.cpp ├── obstacle_extractor.cpp ├── obstacle_publisher.cpp ├── obstacle_tracker.cpp ├── panels ├── obstacle_extractor_panel.cpp ├── obstacle_publisher_panel.cpp ├── obstacle_tracker_panel.cpp └── scans_merger_panel.cpp └── scans_merger.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt.user 2 | *~ 3 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(obstacle_detector) 3 | 4 | set(CMAKE_CXX_FLAGS "-std=c++11 -fpermissive ${CMAKE_CXX_FLAGS} -Wfatal-errors\ ") 5 | 6 | find_package(catkin REQUIRED COMPONENTS roscpp roslaunch nodelet rviz std_msgs std_srvs geometry_msgs sensor_msgs tf laser_geometry message_generation) 7 | find_package(Armadillo REQUIRED) 8 | find_package(Boost 1.54.0 REQUIRED system) 9 | 10 | add_message_files(FILES CircleObstacle.msg SegmentObstacle.msg Obstacles.msg) 11 | generate_messages(DEPENDENCIES std_msgs geometry_msgs) 12 | 13 | catkin_package( 14 | INCLUDE_DIRS include 15 | LIBRARIES scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui 16 | CATKIN_DEPENDS roscpp nodelet rviz std_msgs std_srvs geometry_msgs sensor_msgs tf laser_geometry message_runtime 17 | ) 18 | 19 | include_directories(include ${catkin_INCLUDE_DIRS} ${ARMADILLO_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 20 | 21 | # 22 | # Build libs 23 | # 24 | add_library(scans_merger src/scans_merger.cpp) 25 | target_link_libraries(scans_merger ${catkin_LIBRARIES}) 26 | add_dependencies(scans_merger ${catkin_EXPORTED_TARGETS}) 27 | 28 | add_library(obstacle_extractor src/obstacle_extractor.cpp) 29 | target_link_libraries(obstacle_extractor ${catkin_LIBRARIES} ${ARMADILLO_LIBRARIES}) 30 | add_dependencies(obstacle_extractor ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 31 | 32 | add_library(obstacle_tracker src/obstacle_tracker.cpp) 33 | target_link_libraries(obstacle_tracker ${catkin_LIBRARIES} ${ARMADILLO_LIBRARIES}) 34 | add_dependencies(obstacle_tracker ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 35 | 36 | add_library(obstacle_publisher src/obstacle_publisher.cpp) 37 | target_link_libraries(obstacle_publisher ${catkin_LIBRARIES}) 38 | add_dependencies(obstacle_publisher ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 39 | 40 | # 41 | # Build nodes 42 | # 43 | add_executable(scans_merger_node src/nodes/scans_merger_node.cpp) 44 | target_link_libraries(scans_merger_node scans_merger) 45 | 46 | add_executable(obstacle_extractor_node src/nodes/obstacle_extractor_node.cpp) 47 | target_link_libraries(obstacle_extractor_node obstacle_extractor) 48 | 49 | add_executable(obstacle_tracker_node src/nodes/obstacle_tracker_node.cpp) 50 | target_link_libraries(obstacle_tracker_node obstacle_tracker) 51 | 52 | add_executable(obstacle_publisher_node src/nodes/obstacle_publisher_node.cpp) 53 | target_link_libraries(obstacle_publisher_node obstacle_publisher) 54 | 55 | # 56 | # Build nodelets 57 | # 58 | add_library(${PROJECT_NAME}_nodelets 59 | src/nodelets/scans_merger_nodelet.cpp 60 | src/nodelets/obstacle_extractor_nodelet.cpp 61 | src/nodelets/obstacle_tracker_nodelet.cpp 62 | src/nodelets/obstacle_publisher_nodelet.cpp) 63 | target_link_libraries(${PROJECT_NAME}_nodelets scans_merger obstacle_extractor obstacle_tracker obstacle_publisher) 64 | 65 | # 66 | # Build rviz plugins 67 | # 68 | set(CMAKE_AUTOMOC ON) 69 | 70 | if(rviz_QT_VERSION VERSION_LESS "5") 71 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 72 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 73 | include(${QT_USE_FILE}) 74 | else() 75 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 76 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 77 | set(QT_LIBRARIES Qt5::Widgets) 78 | endif() 79 | 80 | add_definitions(-DQT_NO_KEYWORDS) 81 | 82 | add_library(${PROJECT_NAME}_gui 83 | src/displays/obstacles_display.cpp include/${PROJECT_NAME}/displays/obstacles_display.h 84 | src/displays/circle_visual.cpp include/${PROJECT_NAME}/displays/circle_visual.h 85 | src/displays/segment_visual.cpp include/${PROJECT_NAME}/displays/segment_visual.h 86 | # 87 | src/panels/scans_merger_panel.cpp include/${PROJECT_NAME}/panels/scans_merger_panel.h 88 | src/panels/obstacle_extractor_panel.cpp include/${PROJECT_NAME}/panels/obstacle_extractor_panel.h 89 | src/panels/obstacle_tracker_panel.cpp include/${PROJECT_NAME}/panels/obstacle_tracker_panel.h 90 | src/panels/obstacle_publisher_panel.cpp include/${PROJECT_NAME}/panels/obstacle_publisher_panel.h) 91 | target_link_libraries(${PROJECT_NAME}_gui ${QT_LIBRARIES} ${catkin_LIBRARIES}) 92 | add_dependencies(${PROJECT_NAME}_gui ${catkin_EXPORTED_TARGETS}) 93 | 94 | # 95 | # Install libraries 96 | # 97 | install(TARGETS scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui 98 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 99 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 100 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 101 | 102 | # 103 | # Install nodes 104 | # 105 | install(TARGETS scans_merger_node obstacle_extractor_node obstacle_tracker_node obstacle_publisher_node 106 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 107 | 108 | # 109 | # Install header files 110 | # 111 | install(DIRECTORY include/${PROJECT_NAME}/ 112 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 113 | 114 | # 115 | # Install nodelet and rviz plugins description 116 | # 117 | install(FILES nodelet_plugins.xml rviz_plugins.xml 118 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 119 | 120 | # 121 | # Install launch files 122 | # 123 | install(DIRECTORY launch/ 124 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 125 | 126 | # 127 | # Create folders and copy resources 128 | # 129 | file(MAKE_DIRECTORY $ENV{HOME}/.local/share/icons/robor) 130 | file(COPY resources/play.png resources/stop.png DESTINATION $ENV{HOME}/.local/share/icons/robor) 131 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # The obstacle_detector package 2 | 3 | The `obstacle_detector` package provides utilities to detect and track obstacles from data provided by 2D laser scanners. Detected obstacles come in a form of line segments or circles. The package was designed for a robot equipped with two laser scanners therefore it contains several additional utilities. The working principles of the method are described in an article provided in the `resources` folder. 4 | 5 | The package requires [Armadillo C++](http://arma.sourceforge.net) library for compilation and runtime. 6 | 7 | ----------------------- 8 |

9 | Visual example of obstacle detector output. 10 |
11 | Fig. 1. Visual example of obstacle detector output. 12 |

13 | 14 | ----------------------- 15 | 16 | 1. The nodes and nodelets 17 | 1.1 The scans_merger 18 | 1.2 The obstacle_extractor 19 | 1.3 The obstacle_tracker 20 | 1.4 The obstacle_publisher 21 | 2. The messages 22 | 3. Launch files 23 | 4. The displays 24 | 25 | ## 1. The nodes and nodelets 26 | 27 | The package provides separate nodes/nodelets to perform separate tasks. In general solution the data is processed in a following manner: 28 | 29 | `two laser scans` -> `scans merger` -> `merged scan or pcl` -> `obstacle extractor` -> `obstacles` -> `obstacle tracker` -> `refined obstacles` 30 | 31 | For some scenarios the pure obstacle extraction directly from a laser scan (without tracking) might be sufficient. 32 | 33 | The nodes are configurable with the use of ROS parameter server. All of the nodes provide a private `params` service, which allows the process to get the latest parameters from the parameter server. 34 | 35 | All of the nodes can be in either active or sleep mode, triggered by setting the appropriate variable in the parameter server and calling `params` service. In the sleep mode, any subscribers or publishers are shut down and the node does nothing until activated again. 36 | 37 | For the ease of use it is recomended to use appropriate Rviz panels provided for the nodes with the package. The Rviz panels communicate via parameter server and service-client calls, therefore the names of the nodes must be preserved unchanged (cf. launch files for examples). 38 | 39 | ### 1.1. The scans_merger node 40 | 41 | 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. 42 | 43 | The input laser scans are firstly rectified to incorporate the motion of the scanner in time (see `laser_geometry` package). Next, two PCLs obtained from the previous step are synchronized and transformed into the target coordinate frame at the current point in time. 44 | 45 | ----------------------- 46 |

47 | Visual example of scans_merger output 48 |
49 | Fig. 2. Visual example of `scans_merger` output. 50 |

51 | 52 | ----------------------- 53 | 54 | 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. 55 | 56 | 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: 57 | 58 | * `~active` (`bool`, default: `true`) - active/sleep mode, 59 | * `~publish_scan` (`bool`, default: `false`) - publish the merged laser scan message, 60 | * `~publish_pcl` (`bool`, default: `true`) - publish the merged point cloud message, 61 | * `~ranges_num` (`int`, default: `1000`) - number of ranges (circular sectors) contained in the 360 deg laser scan message, 62 | * `~min_scanner_range` (`double`, default: `0.05`) - minimal allowable range value for produced laser scan message, 63 | * `~max_scanner_range` (`double`, default: `10.0`) - maximal allowable range value for produced laser scan message, 64 | * `~min_x_range` (`double`, default: `-10.0`) - limitation for points coordinates (points with coordinates behind these limitations will be discarded), 65 | * `~max_x_range` (`double`, default: `10.0`) - as above, 66 | * `~min_y_range` (`double`, default: `-10.0`) - as above, 67 | * `~max_y_range` (`double`, default: `10.0`) - as above, 68 | * `~fixed_frame_id` (`string`, default: `map`) - name of the fixed coordinate frame used for scan rectification in time, 69 | * `~target_frame_id` (`string`, default: `robot`) - name of the coordinate frame used as the origin for the produced laser scan or point cloud. 70 | 71 | The package comes with Rviz panel for this node. 72 | 73 | ----------------------- 74 |

75 | Rviz panel for the scans_merger node. 76 |
77 | Fig. 3. Rviz panel for the `scans_merger` node. 78 |

79 | 80 | ----------------------- 81 | 82 | ### 1.2. The obstacle_extractor node 83 | 84 | This node converts messages of type `sensor_msgs/LaserScan` from topic `scan` or messages of type `sensor_msgs/PointCloud` from topic `pcl` into obstacles which are published as messages of custom type `obstacles_detector/Obstacles` under topic `raw_obstacles`. The PCL message must be ordered in the angular fashion, because the algorithm exploits the polar nature of laser scanners. 85 | 86 | ----------------------- 87 |

88 | Visual example of obstacle_extractor output. 89 |
90 | Fig. 4. Visual example of `obstacle_extractor` output. 91 |

92 | 93 | ----------------------- 94 | 95 | The input points are firstly grouped into subsets and marked as visible or not (if a group is in front of neighbouring groups, it is visible. Otherwise it is assumed to be occluded). The algorithm extracts segments from each points subset. Next, the segments are checked for possible merging between each other. The circular obstacles are then extracted from segments and also merged if possible. Resulting set of obstacles can be transformed to a dedicated coordinate frame. 96 | 97 | The node is configurable with the following set of local parameters: 98 | 99 | * `~active` (`bool`, default: `true`) - active/sleep mode, 100 | * `~use_scan` (`bool`, default: `false`) - use laser scan messages, 101 | * `~use_pcl` (`bool`, default: `true`) - use point cloud messages (if both scan and pcl are chosen, scans will have priority), 102 | * `~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, 103 | * `~circles_from_visibles` (`bool`, default: `true`) - detect circular obstacles only from fully visible (not occluded) segments, 104 | * `~discard_converted_segments` (`bool`, default: `true`) - do not publish segments, from which the circles were spawned, 105 | * `~transform_coordinates` (`bool`, default: `true`) - transform the coordinates of obstacles to a frame described with `frame_id` parameter, 106 | * `~min_group_points` (`int`, default: `5`) - minimum number of points comprising a group to be further processed, 107 | * `~max_group_distance` (`double`, default: `0.1`) - if the distance between two points is greater than this value, start a new group, 108 | * `~distance_proportion` (`double`, default: `0.00628`) - enlarge the allowable distance between points proportionally to the range of point (use scan angle increment in radians), 109 | * `~max_split_distance` (`double`, default: `0.2`) - if a point in group lays further from a leading line than this value, split the group, 110 | * `~max_merge_separation` (`double`, default: `0.2`) - if distance between obstacles is smaller than this value, consider merging them, 111 | * `~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, 112 | * `~max_circle_radius` (`double`, default: `0.6`) - if a circle would have greater radius than this value, skip it, 113 | * `~radius_enlargement` (`double`, default: `0.25`) - artificially enlarge the circles radius by this value, 114 | * `~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). 115 | 116 | The package comes with Rviz panel for this node. 117 | 118 | ----------------------- 119 |

120 | Rviz panel for the obstacle_extractor node. 121 |
122 | Fig. 5. Rviz panel for the `obstacle_extractor` node. 123 |

124 | 125 | ----------------------- 126 | 127 | ### 1.3. The obstacle_tracker node 128 | 129 | This node tracks and filters the circular obstacles with the use of Kalman filter. It subscribes to messages of custom type `obstacle_detector/Obstacles` from topic `raw_obstacles` and publishes messages of the same type under topic `tracked_obstacles`. The tracking algorithm is applied only to the circular obstacles, hence the segments list in the published message is simply a copy of the original segments. The tracked obstacles are supplemented with additional information on their velocity. 130 | 131 | ----------------------- 132 |

133 | Visual example of obstacle_tracker output. 134 |
135 | Fig. 6. Visual example of `obstacle_tracker` output. 136 |

137 | 138 | ----------------------- 139 | 140 | The node works in a synchronous manner with the default rate of 100 Hz. If detected obstacles are published less often, the tracker will supersample them and smoothen their position and radius. The following set of local parameters can be used to tune the node: 141 | 142 | * `~active` (`bool`, default: `true`) - active/sleep mode, 143 | * `~copy_segments` (`bool`, default: `true`) - copy detected segments into tracked obstacles message, 144 | * `~loop_rate` (`double`, default: `100.0`) - the main loop rate in Hz, 145 | * `~tracking_duration` (`double`, default: `2.0`) - the duration of obstacle tracking in the case of lack of incomming data (after this time from the last corresponding measurement the tracked obstacle will be removed from the list), 146 | * `~min_correspondence_cost` (`double`, default `0.3`) - a threshold for correspondence test, 147 | * `~std_correspondence_dev` (`double`, default `0.15`) - (experimental) standard deviation of the position ellipse in the correspondence test, 148 | * `~process_variance` (`double`, default `0.01`) - variance of obstacles position and radius (parameter of Kalman Filter), 149 | * `~process_rate_variance` (`double`, default `0.1`) - variance of rate of change of obstacles values (parameter of Kalman Filter), 150 | * `~measurement_variance` (`double`, default `1.0`) - variance of measured obstacles values (parameter of Kalman Filter), 151 | * `~frame_id` (`string`, default: `map`) - name of the coordinate frame in which the obstacles are described, 152 | 153 | The package comes with Rviz panel for this node. 154 | 155 | ----------------------- 156 |

157 | Rviz panel for the obstacle_tracker node. 158 |
159 | Fig. 7. Rviz panel for the `obstacle_tracker` node. 160 |

161 | 162 | ----------------------- 163 | 164 | ### 1.4. The obstacle_publisher node 165 | 166 | The auxiliary node which allows to publish a set of virtual, circular obstacles in the form of message of type `obstacles_detector/Obstacles` under topic `obstacles`. The node is mostly used for off-line tests. The following parameters are used to configure the node: 167 | 168 | * `~active` (`bool`, default: `true`) - active/sleep mode, 169 | * `~reset` (`bool`, default: `false`) - reset time for obstacles motion calculation (used by dedicated Rviz panel), 170 | * `~fusion_example` (`bool`, default: `false`) - produce obstacles showing fusion, 171 | * `~fission_example` (`bool`, default: `false`) - produce obstacles showing fission, 172 | * `~radius_margin` (`double`, default: `0.25`) - artificially enlarge the circles radius by this value in meters, 173 | * `~loop_rate` (`double`, default: `10.0`) - the main loop rate in Hz, 174 | * `~frame_id` (`string`, default: `map`) - name of the coordinate frame in which the obstacles are described. 175 | 176 | The following parameters are used to provide the node with a set of obstacles: 177 | 178 | * `~x_vector` (`std::vector`, default: `[]`) - the array of x-coordinates of obstacles center points, 179 | * `~y_vector` (`std::vector`, default: `[]`) - the array of y-coordinates of obstacles center points, 180 | * `~r_vector` (`std::vector`, default: `[]`) - the array of obstacles radii, 181 | * `~x_vector` (`std::vector`, default: `[]`) - the array of velocities of obstacles center points in x direction, 182 | * `~y_vector` (`std::vector`, default: `[]`) - the array of velocities of obstacles center points in y direction. 183 | 184 | The package comes with Rviz panel for this node. 185 | 186 | ----------------------- 187 |

188 | Rviz panel for the obstacle_publisher node. 189 |
190 | Fig. 8. Rviz panel for the `obstacle_publisher` node. 191 |

192 | 193 | ----------------------- 194 | 195 | ## 2. The messages 196 | 197 | The package provides three custom message types. All of their numerical values are provided in SI units. 198 | 199 | * `CircleObstacle` 200 | - `geometry_msgs/Point center` - center of circular obstacle, 201 | - `geometry_msgs/Vector3 velocity` - linear velocity of circular obstacle, 202 | - `float64 radius` - radius of circular obstacle with added safety margin, 203 | - `float64 true_radius` - measured radius of obstacle without the safety margin. 204 | * `SegmentObstacle` 205 | - `geometry_msgs/Point first_point` - first point of the segment (in counter-clockwise direction), 206 | - `geometry_msgs/Point last_point` - end point of the segment. 207 | * `Obstacles` 208 | - `Header header` 209 | - `obstacle_detector/SegmentObstacle[] segments` 210 | - `obstacle_detector/CircleObstacle[] circles` 211 | 212 | ## 3. The launch files 213 | 214 | Provided launch files are good examples of how to use `obstacle_detector` package. They give a full list of parameters used by each of provided nodes. 215 | * `demo.launch` - Plays a rosbag with recorded scans and starts all of the nodes with Rviz configured with appropriate panels. 216 | * `nodes_example.launch` - Runs all of the nodes with their parameters set to default values. 217 | * `nodelets_example.launch` - Runs all of the nodelets with their parameters set to default values. 218 | 219 | ## 4. The displays 220 | 221 | For better visual effects, appropriate Rviz display for `Obstacles` messages was prepared. Via its properties, one can change the colors of the obstacles. 222 | 223 | Author: 224 | _Mateusz Przybyla_ 225 | 226 | -------------------------------------------------------------------------------- /icons/classes/Obstacle Extractor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tysik/obstacle_detector/bd59d405b2284f05c46f7b99cb9cb4247934e2ff/icons/classes/Obstacle Extractor.png -------------------------------------------------------------------------------- /icons/classes/Obstacle Publisher.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tysik/obstacle_detector/bd59d405b2284f05c46f7b99cb9cb4247934e2ff/icons/classes/Obstacle Publisher.png -------------------------------------------------------------------------------- /icons/classes/Obstacle Tracker.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tysik/obstacle_detector/bd59d405b2284f05c46f7b99cb9cb4247934e2ff/icons/classes/Obstacle Tracker.png -------------------------------------------------------------------------------- /icons/classes/Obstacles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tysik/obstacle_detector/bd59d405b2284f05c46f7b99cb9cb4247934e2ff/icons/classes/Obstacles.png -------------------------------------------------------------------------------- /icons/classes/Scans Merger.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tysik/obstacle_detector/bd59d405b2284f05c46f7b99cb9cb4247934e2ff/icons/classes/Scans Merger.png -------------------------------------------------------------------------------- /include/obstacle_detector/displays/circle_visual.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #ifndef Q_MOC_RUN 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | #endif 47 | 48 | namespace obstacles_display 49 | { 50 | 51 | class CircleVisual 52 | { 53 | public: 54 | CircleVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node); 55 | 56 | virtual ~CircleVisual(); 57 | 58 | void setData(const obstacle_detector::CircleObstacle& circle); 59 | void setFramePosition(const Ogre::Vector3& position); 60 | void setFrameOrientation(const Ogre::Quaternion& orientation); 61 | void setMainColor(float r, float g, float b, float a); 62 | void setMarginColor(float r, float g, float b, float a); 63 | 64 | private: 65 | boost::shared_ptr obstacle_; 66 | boost::shared_ptr margin_; 67 | 68 | Ogre::SceneNode* frame_node_1_; 69 | Ogre::SceneNode* frame_node_2_; 70 | Ogre::SceneManager* scene_manager_; 71 | }; 72 | 73 | } // end namespace obstacles_display 74 | -------------------------------------------------------------------------------- /include/obstacle_detector/displays/obstacles_display.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #ifndef Q_MOC_RUN 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include "obstacle_detector/displays/circle_visual.h" 52 | #include "obstacle_detector/displays/segment_visual.h" 53 | #endif 54 | 55 | namespace obstacles_display 56 | { 57 | 58 | class ObstaclesDisplay: public rviz::MessageFilterDisplay 59 | { 60 | Q_OBJECT 61 | public: 62 | ObstaclesDisplay(); 63 | virtual ~ObstaclesDisplay(); 64 | 65 | protected: 66 | virtual void onInitialize(); 67 | virtual void reset(); 68 | 69 | private Q_SLOTS: 70 | void updateCircleColor(); 71 | void updateSegmentColor(); 72 | void updateAlpha(); 73 | void updateThickness(); 74 | 75 | private: 76 | void processMessage(const obstacle_detector::Obstacles::ConstPtr& obstacles_msg); 77 | 78 | std::vector< boost::shared_ptr > circle_visuals_; 79 | std::vector< boost::shared_ptr > segment_visuals_; 80 | 81 | rviz::ColorProperty* circle_color_property_; 82 | rviz::ColorProperty* margin_color_property_; 83 | rviz::ColorProperty* segment_color_property_; 84 | rviz::FloatProperty* alpha_property_; 85 | rviz::FloatProperty* thickness_property_; 86 | }; 87 | 88 | } // end namespace obstacles_display 89 | -------------------------------------------------------------------------------- /include/obstacle_detector/displays/segment_visual.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #ifndef Q_MOC_RUN 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | #endif 48 | 49 | namespace obstacles_display 50 | { 51 | 52 | class SegmentVisual 53 | { 54 | public: 55 | SegmentVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node); 56 | 57 | virtual ~SegmentVisual(); 58 | 59 | void setData(const obstacle_detector::SegmentObstacle& segment); 60 | void setFramePosition(const Ogre::Vector3& position); 61 | void setFrameOrientation(const Ogre::Quaternion& orientation); 62 | void setColor(float r, float g, float b, float a); 63 | void setWidth(float w); 64 | 65 | private: 66 | boost::shared_ptr line_; 67 | 68 | Ogre::SceneNode* frame_node_; 69 | Ogre::SceneManager* scene_manager_; 70 | }; 71 | 72 | } // end namespace obstacles_display 73 | -------------------------------------------------------------------------------- /include/obstacle_detector/obstacle_extractor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include "obstacle_detector/utilities/point.h" 46 | #include "obstacle_detector/utilities/segment.h" 47 | #include "obstacle_detector/utilities/circle.h" 48 | #include "obstacle_detector/utilities/point_set.h" 49 | 50 | namespace obstacle_detector 51 | { 52 | 53 | class ObstacleExtractor 54 | { 55 | public: 56 | ObstacleExtractor(ros::NodeHandle& nh, ros::NodeHandle& nh_local); 57 | ~ObstacleExtractor(); 58 | 59 | private: 60 | bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); 61 | void scanCallback(const sensor_msgs::LaserScan::ConstPtr scan_msg); 62 | void pclCallback(const sensor_msgs::PointCloud::ConstPtr pcl_msg); 63 | 64 | void initialize() { std_srvs::Empty empt; updateParams(empt.request, empt.response); } 65 | 66 | void processPoints(); 67 | void groupPoints(); 68 | void publishObstacles(); 69 | 70 | void detectSegments(const PointSet& point_set); 71 | void mergeSegments(); 72 | bool compareSegments(const Segment& s1, const Segment& s2, Segment& merged_segment); 73 | bool checkSegmentsProximity(const Segment& s1, const Segment& s2); 74 | bool checkSegmentsCollinearity(const Segment& segment, const Segment& s1, const Segment& s2); 75 | 76 | void detectCircles(); 77 | void mergeCircles(); 78 | bool compareCircles(const Circle& c1, const Circle& c2, Circle& merged_circle); 79 | 80 | ros::NodeHandle nh_; 81 | ros::NodeHandle nh_local_; 82 | 83 | ros::Subscriber scan_sub_; 84 | ros::Subscriber pcl_sub_; 85 | ros::Publisher obstacles_pub_; 86 | ros::ServiceServer params_srv_; 87 | 88 | ros::Time stamp_; 89 | std::string base_frame_id_; 90 | tf::TransformListener tf_listener_; 91 | 92 | std::list input_points_; 93 | std::list segments_; 94 | std::list circles_; 95 | 96 | // Parameters 97 | bool p_active_; 98 | bool p_use_scan_; 99 | bool p_use_pcl_; 100 | 101 | bool p_use_split_and_merge_; 102 | bool p_circles_from_visibles_; 103 | bool p_discard_converted_segments_; 104 | bool p_transform_coordinates_; 105 | 106 | int p_min_group_points_; 107 | 108 | double p_distance_proportion_; 109 | double p_max_group_distance_; 110 | double p_max_split_distance_; 111 | double p_max_merge_separation_; 112 | double p_max_merge_spread_; 113 | double p_max_circle_radius_; 114 | double p_radius_enlargement_; 115 | 116 | double p_min_x_limit_; 117 | double p_max_x_limit_; 118 | double p_min_y_limit_; 119 | double p_max_y_limit_; 120 | 121 | std::string p_frame_id_; 122 | }; 123 | 124 | } // namespace obstacle_detector 125 | -------------------------------------------------------------------------------- /include/obstacle_detector/obstacle_publisher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | namespace obstacle_detector 43 | { 44 | 45 | class ObstaclePublisher 46 | { 47 | public: 48 | ObstaclePublisher(ros::NodeHandle &nh, ros::NodeHandle &nh_local); 49 | ~ObstaclePublisher(); 50 | 51 | private: 52 | bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); 53 | void timerCallback(const ros::TimerEvent& e); 54 | 55 | void initialize() { std_srvs::Empty empt; updateParams(empt.request, empt.response); } 56 | 57 | void calculateObstaclesPositions(double dt); 58 | void fusionExample(double t); 59 | void fissionExample(double t); 60 | void publishObstacles(); 61 | void reset(); 62 | 63 | ros::NodeHandle nh_; 64 | ros::NodeHandle nh_local_; 65 | 66 | ros::Publisher obstacle_pub_; 67 | ros::ServiceServer params_srv_; 68 | ros::Timer timer_; 69 | 70 | obstacle_detector::Obstacles obstacles_; 71 | double t_; 72 | 73 | // Parameters 74 | bool p_active_; 75 | bool p_reset_; 76 | bool p_fusion_example_; 77 | bool p_fission_example_; 78 | 79 | double p_loop_rate_; 80 | double p_sampling_time_; 81 | double p_radius_margin_; 82 | 83 | std::vector p_x_vector_; 84 | std::vector p_y_vector_; 85 | std::vector p_r_vector_; 86 | 87 | std::vector p_vx_vector_; 88 | std::vector p_vy_vector_; 89 | 90 | std::string p_frame_id_; 91 | }; 92 | 93 | } // namespace obstacle_detector 94 | -------------------------------------------------------------------------------- /include/obstacle_detector/obstacle_tracker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include "obstacle_detector/utilities/tracked_obstacle.h" 46 | #include "obstacle_detector/utilities/math_utilities.h" 47 | 48 | namespace obstacle_detector 49 | { 50 | 51 | class ObstacleTracker { 52 | public: 53 | ObstacleTracker(ros::NodeHandle& nh, ros::NodeHandle& nh_local); 54 | ~ObstacleTracker(); 55 | 56 | private: 57 | bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); 58 | void timerCallback(const ros::TimerEvent&); 59 | void obstaclesCallback(const obstacle_detector::Obstacles::ConstPtr new_obstacles); 60 | 61 | void initialize() { std_srvs::Empty empt; updateParams(empt.request, empt.response); } 62 | 63 | double obstacleCostFunction(const CircleObstacle& new_obstacle, const CircleObstacle& old_obstacle); 64 | void calculateCostMatrix(const std::vector& new_obstacles, arma::mat& cost_matrix); 65 | void calculateRowMinIndices(const arma::mat& cost_matrix, std::vector& row_min_indices); 66 | void calculateColMinIndices(const arma::mat& cost_matrix, std::vector& col_min_indices); 67 | 68 | bool fusionObstacleUsed(const int idx, const std::vector& col_min_indices, const std::vector& used_new, const std::vector& used_old); 69 | bool fusionObstaclesCorrespond(const int idx, const int jdx, const std::vector& col_min_indices, const std::vector& used_old); 70 | bool fissionObstacleUsed(const int idx, const int T, const std::vector& row_min_indices, const std::vector& used_new, const std::vector& used_old); 71 | bool fissionObstaclesCorrespond(const int idx, const int jdx, const std::vector& row_min_indices, const std::vector& used_new); 72 | 73 | void fuseObstacles(const std::vector& fusion_indices, const std::vector& col_min_indices, 74 | std::vector& new_tracked, const Obstacles::ConstPtr& new_obstacles); 75 | void fissureObstacle(const std::vector& fission_indices, const std::vector& row_min_indices, 76 | std::vector& new_tracked, const Obstacles::ConstPtr& new_obstacles); 77 | 78 | void updateObstacles(); 79 | void publishObstacles(); 80 | 81 | ros::NodeHandle nh_; 82 | ros::NodeHandle nh_local_; 83 | 84 | ros::Subscriber obstacles_sub_; 85 | ros::Publisher obstacles_pub_; 86 | ros::ServiceServer params_srv_; 87 | ros::Timer timer_; 88 | 89 | double radius_margin_; 90 | obstacle_detector::Obstacles obstacles_; 91 | 92 | std::vector tracked_obstacles_; 93 | std::vector untracked_obstacles_; 94 | 95 | // Parameters 96 | bool p_active_; 97 | bool p_copy_segments_; 98 | 99 | double p_tracking_duration_; 100 | double p_loop_rate_; 101 | double p_sampling_time_; 102 | double p_sensor_rate_; 103 | double p_min_correspondence_cost_; 104 | double p_std_correspondence_dev_; 105 | double p_process_variance_; 106 | double p_process_rate_variance_; 107 | double p_measurement_variance_; 108 | 109 | std::string p_frame_id_; 110 | }; 111 | 112 | } // namespace obstacle_detector 113 | -------------------------------------------------------------------------------- /include/obstacle_detector/panels/obstacle_extractor_panel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | namespace obstacle_detector 53 | { 54 | 55 | class ObstacleExtractorPanel : public rviz::Panel 56 | { 57 | Q_OBJECT 58 | public: 59 | ObstacleExtractorPanel(QWidget* parent = 0); 60 | 61 | virtual void load(const rviz::Config& config); 62 | virtual void save(rviz::Config config) const; 63 | 64 | private Q_SLOTS: 65 | void processInputs(); 66 | 67 | private: 68 | void verifyInputs(); 69 | void setParams(); 70 | void getParams(); 71 | void evaluateParams(); 72 | void notifyParamsUpdate(); 73 | 74 | private: 75 | QCheckBox* activate_checkbox_; 76 | QCheckBox* use_scan_checkbox_; 77 | QCheckBox* use_pcl_checkbox_; 78 | QCheckBox* use_split_merge_checkbox_; 79 | QCheckBox* circ_from_visib_checkbox_; 80 | QCheckBox* discard_segments_checkbox_; 81 | QCheckBox* transform_coords_checkbox_; 82 | 83 | QLineEdit* min_n_input_; 84 | QLineEdit* dist_prop_input_; 85 | QLineEdit* group_dist_input_; 86 | QLineEdit* split_dist_input_; 87 | QLineEdit* merge_sep_input_; 88 | QLineEdit* merge_spread_input_; 89 | QLineEdit* max_radius_input_; 90 | QLineEdit* radius_enl_input_; 91 | QLineEdit* frame_id_input_; 92 | 93 | ros::NodeHandle nh_; 94 | ros::NodeHandle nh_local_; 95 | 96 | ros::ServiceClient params_cli_; 97 | 98 | // Parameters 99 | bool p_active_; 100 | bool p_use_scan_; 101 | bool p_use_pcl_; 102 | 103 | bool p_use_split_and_merge_; 104 | bool p_circles_from_visibles_; 105 | bool p_discard_converted_segments_; 106 | bool p_transform_coordinates_; 107 | 108 | int p_min_group_points_; 109 | 110 | double p_distance_proportion_; 111 | double p_max_group_distance_; 112 | 113 | double p_max_split_distance_; 114 | double p_max_merge_separation_; 115 | double p_max_merge_spread_; 116 | double p_max_circle_radius_; 117 | double p_radius_enlargement_; 118 | 119 | std::string p_frame_id_; 120 | }; 121 | 122 | } // namespace obstacle_detector 123 | -------------------------------------------------------------------------------- /include/obstacle_detector/panels/obstacle_publisher_panel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | namespace obstacle_detector 55 | { 56 | 57 | class ObstaclePublisherPanel : public rviz::Panel 58 | { 59 | Q_OBJECT 60 | public: 61 | ObstaclePublisherPanel(QWidget* parent = 0); 62 | 63 | virtual void load(const rviz::Config& config); 64 | virtual void save(rviz::Config config) const; 65 | 66 | private Q_SLOTS: 67 | void processInputs(); 68 | void addObstacle(); 69 | void removeObstacles(); 70 | void reset(); 71 | 72 | private: 73 | void verifyInputs(); 74 | void setParams(); 75 | void getParams(); 76 | void evaluateParams(); 77 | void notifyParamsUpdate(); 78 | 79 | private: 80 | QCheckBox* activate_checkbox_; 81 | QCheckBox* fusion_example_checkbox_; 82 | QCheckBox* fission_example_checkbox_; 83 | 84 | QListWidget* obstacles_list_; 85 | std::vector obstacles_list_items_; 86 | 87 | QPushButton* add_button_; 88 | QPushButton* remove_button_; 89 | QPushButton* reset_button_; 90 | 91 | QLineEdit* x_input_; 92 | QLineEdit* y_input_; 93 | QLineEdit* r_input_; 94 | 95 | QLineEdit* vx_input_; 96 | QLineEdit* vy_input_; 97 | 98 | QLineEdit* frame_input_; 99 | 100 | ros::NodeHandle nh_; 101 | ros::NodeHandle nh_local_; 102 | 103 | ros::ServiceClient params_cli_; 104 | 105 | double x_, y_, r_, vx_, vy_; 106 | 107 | // Parameters 108 | bool p_active_; 109 | bool p_reset_; 110 | bool p_fusion_example_; 111 | bool p_fission_example_; 112 | 113 | double p_loop_rate_; 114 | 115 | std::vector p_x_vector_; 116 | std::vector p_y_vector_; 117 | std::vector p_r_vector_; 118 | 119 | std::vector p_vx_vector_; 120 | std::vector p_vy_vector_; 121 | 122 | std::string p_frame_id_; 123 | }; 124 | 125 | } 126 | -------------------------------------------------------------------------------- /include/obstacle_detector/panels/obstacle_tracker_panel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | namespace obstacle_detector 53 | { 54 | 55 | class ObstacleTrackerPanel : public rviz::Panel 56 | { 57 | Q_OBJECT 58 | public: 59 | ObstacleTrackerPanel(QWidget* parent = 0); 60 | 61 | virtual void load(const rviz::Config& config); 62 | virtual void save(rviz::Config config) const; 63 | 64 | private Q_SLOTS: 65 | void processInputs(); 66 | 67 | private: 68 | void verifyInputs(); 69 | void setParams(); 70 | void getParams(); 71 | void evaluateParams(); 72 | void notifyParamsUpdate(); 73 | 74 | private: 75 | QCheckBox* activate_checkbox_; 76 | 77 | QLineEdit* tracking_duration_input_; 78 | QLineEdit* loop_rate_input_; 79 | QLineEdit* min_corr_cost_input_; 80 | QLineEdit* std_corr_dev_input_; 81 | QLineEdit* process_var_input_; 82 | QLineEdit* process_rate_var_input_; 83 | QLineEdit* measure_var_input_; 84 | 85 | ros::NodeHandle nh_; 86 | ros::NodeHandle nh_local_; 87 | 88 | ros::ServiceClient params_cli_; 89 | 90 | // Parameters 91 | bool p_active_; 92 | 93 | double p_tracking_duration_; 94 | double p_loop_rate_; 95 | double p_min_correspondence_cost_; 96 | double p_std_correspondence_dev_; 97 | double p_process_variance_; 98 | double p_process_rate_variance_; 99 | double p_measurement_variance_; 100 | }; 101 | 102 | } 103 | -------------------------------------------------------------------------------- /include/obstacle_detector/panels/scans_merger_panel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | namespace obstacle_detector 52 | { 53 | 54 | class ScansMergerPanel : public rviz::Panel 55 | { 56 | Q_OBJECT 57 | public: 58 | ScansMergerPanel(QWidget* parent = 0); 59 | 60 | virtual void load(const rviz::Config& config); 61 | virtual void save(rviz::Config config) const; 62 | 63 | private Q_SLOTS: 64 | void processInputs(); 65 | 66 | private: 67 | void verifyInputs(); 68 | void setParams(); 69 | void getParams(); 70 | void evaluateParams(); 71 | void notifyParamsUpdate(); 72 | 73 | private: 74 | QCheckBox* activate_checkbox_; 75 | QCheckBox* scan_checkbox_; 76 | QCheckBox* pcl_checkbox_; 77 | 78 | QLineEdit* n_input_; 79 | QLineEdit* r_min_input_; 80 | QLineEdit* r_max_input_; 81 | 82 | QLineEdit* x_min_input_; 83 | QLineEdit* x_max_input_; 84 | QLineEdit* y_min_input_; 85 | QLineEdit* y_max_input_; 86 | 87 | QLineEdit* fixed_frame_id_input_; 88 | QLineEdit* target_frame_id_input_; 89 | 90 | ros::NodeHandle nh_; 91 | ros::NodeHandle nh_local_; 92 | 93 | ros::ServiceClient params_cli_; 94 | 95 | // Parameters 96 | bool p_active_; 97 | bool p_publish_scan_; 98 | bool p_publish_pcl_; 99 | 100 | int p_ranges_num_; 101 | 102 | double p_min_scanner_range_; 103 | double p_max_scanner_range_; 104 | 105 | double p_min_x_range_; 106 | double p_max_x_range_; 107 | double p_min_y_range_; 108 | double p_max_y_range_; 109 | 110 | std::string p_fixed_frame_id_; 111 | std::string p_target_frame_id_; 112 | }; 113 | 114 | } // namespace obstacle_detector 115 | -------------------------------------------------------------------------------- /include/obstacle_detector/scans_merger.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace obstacle_detector 47 | { 48 | 49 | class ScansMerger 50 | { 51 | public: 52 | ScansMerger(ros::NodeHandle& nh, ros::NodeHandle& nh_local); 53 | ~ScansMerger(); 54 | 55 | private: 56 | bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); 57 | void frontScanCallback(const sensor_msgs::LaserScan::ConstPtr front_scan); 58 | void rearScanCallback(const sensor_msgs::LaserScan::ConstPtr rear_scan); 59 | 60 | void initialize() { std_srvs::Empty empt; updateParams(empt.request, empt.response); } 61 | 62 | void publishMessages(); 63 | 64 | ros::NodeHandle nh_; 65 | ros::NodeHandle nh_local_; 66 | 67 | ros::ServiceServer params_srv_; 68 | 69 | ros::Subscriber front_scan_sub_; 70 | ros::Subscriber rear_scan_sub_; 71 | ros::Publisher scan_pub_; 72 | ros::Publisher pcl_pub_; 73 | 74 | tf::TransformListener tf_ls_; 75 | laser_geometry::LaserProjection projector_; 76 | 77 | bool front_scan_received_; 78 | bool rear_scan_received_; 79 | bool front_scan_error_; 80 | bool rear_scan_error_; 81 | 82 | sensor_msgs::PointCloud front_pcl_; 83 | sensor_msgs::PointCloud rear_pcl_; 84 | 85 | // Parameters 86 | bool p_active_; 87 | bool p_publish_scan_; 88 | bool p_publish_pcl_; 89 | 90 | int p_ranges_num_; 91 | 92 | double p_min_scanner_range_; 93 | double p_max_scanner_range_; 94 | double p_min_x_range_; 95 | double p_max_x_range_; 96 | double p_min_y_range_; 97 | double p_max_y_range_; 98 | 99 | std::string p_fixed_frame_id_; 100 | std::string p_target_frame_id_; 101 | }; 102 | 103 | } // namespace obstacle_detector 104 | -------------------------------------------------------------------------------- /include/obstacle_detector/utilities/circle.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include "obstacle_detector/utilities/point.h" 39 | #include "obstacle_detector/utilities/segment.h" 40 | 41 | namespace obstacle_detector 42 | { 43 | 44 | class Circle 45 | { 46 | public: 47 | Circle(const Point& p = Point(), const double r = 0.0) : center(p), radius(r) { } 48 | 49 | /* 50 | * Create a circle by taking the segment as a base of equilateral 51 | * triangle. The circle is circumscribed on this triangle. 52 | */ 53 | Circle(const Segment& s) { 54 | radius = 0.5773502 * s.length(); // sqrt(3)/3 * length 55 | center = (s.first_point + s.last_point - radius * s.normal()) / 2.0; 56 | point_sets = s.point_sets; 57 | } 58 | 59 | double distanceTo(const Point& p) { return (p - center).length() - radius; } 60 | 61 | friend std::ostream& operator<<(std::ostream& out, const Circle& c) 62 | { out << "C: " << c.center << ", R: " << c.radius; return out; } 63 | 64 | Point center; 65 | double radius; 66 | std::vector point_sets; 67 | }; 68 | 69 | } // namespace obstacle_detector 70 | -------------------------------------------------------------------------------- /include/obstacle_detector/utilities/figure_fitting.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | 40 | #include "obstacle_detector/utilities/point.h" 41 | #include "obstacle_detector/utilities/segment.h" 42 | #include "obstacle_detector/utilities/circle.h" 43 | 44 | namespace obstacle_detector 45 | { 46 | 47 | /* 48 | * Returns a total best fit approximation of 49 | * segment based on given point set. The equation 50 | * used for fitting is given by 51 | * Ax + By = -C 52 | * and the A, B, C parameters are normalized. 53 | */ 54 | Segment fitSegment(const PointSet& point_set) { 55 | static int num_calls = 0; 56 | num_calls++; 57 | 58 | int N = point_set.num_points; 59 | assert(N >= 2); 60 | 61 | arma::mat input = arma::mat(N, 2).zeros(); // [x_i, y_i] 62 | arma::vec output = arma::vec(N).ones(); // [-C] 63 | arma::vec params = arma::vec(2).zeros(); // [A ; B] 64 | 65 | PointIterator point = point_set.begin; 66 | for (int i = 0; i < N; ++i) { 67 | input(i, 0) = point->x; 68 | input(i, 1) = point->y; 69 | std::advance(point, 1); 70 | } 71 | 72 | // Find A and B coefficients from linear regression (assuming C = -1.0) 73 | params = arma::pinv(input) * output; 74 | 75 | double A, B, C; 76 | A = params(0); 77 | B = params(1); 78 | C = -1.0; 79 | 80 | // Find end points 81 | Point p1 = *point_set.begin; 82 | Point p2 = *point_set.end; 83 | 84 | Segment segment(p1, p2); 85 | segment.point_sets.push_back(point_set); 86 | 87 | double D = (A * A + B * B); 88 | 89 | // Project end points on the line 90 | if (D > 0.0) { 91 | Point projected_p1, projected_p2; 92 | 93 | projected_p1.x = ( B * B * p1.x - A * B * p1.y - A * C) / D; 94 | projected_p1.y = (-A * B * p1.x + A * A * p1.y - B * C) / D; 95 | 96 | projected_p2.x = ( B * B * p2.x - A * B * p2.y - A * C) / D; 97 | projected_p2.y = (-A * B * p2.x + A * A * p2.y - B * C) / D; 98 | 99 | segment.first_point = projected_p1; 100 | segment.last_point = projected_p2; 101 | } 102 | 103 | return segment; 104 | } 105 | 106 | Segment fitSegment(const std::vector& point_sets) { 107 | static int num_calls = 0; 108 | num_calls++; 109 | 110 | int N = 0; 111 | for (PointSet ps : point_sets) 112 | N += ps.num_points; 113 | 114 | assert(N >= 2); 115 | 116 | arma::mat input = arma::mat(N, 2).zeros(); // [x_i, y_i] 117 | arma::vec output = arma::vec(N).ones(); // [-C] 118 | arma::vec params = arma::vec(2).zeros(); // [A ; B] 119 | 120 | int n = 0; 121 | for (PointSet ps : point_sets) { 122 | PointIterator point = ps.begin; 123 | for (int i = 0; i < ps.num_points; ++i) { 124 | input(i + n, 0) = point->x; 125 | input(i + n, 1) = point->y; 126 | std::advance(point, 1); 127 | } 128 | 129 | n += ps.num_points; 130 | } 131 | 132 | // Find A and B coefficients from linear regression (assuming C = -1.0) 133 | params = arma::pinv(input) * output; 134 | 135 | double A, B, C; 136 | A = params(0); 137 | B = params(1); 138 | C = -1.0; 139 | 140 | // Find end points 141 | Point p1 = *point_sets.front().begin; 142 | Point p2 = *point_sets.back().end; 143 | 144 | Segment segment(p1, p2); 145 | segment.point_sets = point_sets; 146 | 147 | double D = (A * A + B * B); 148 | 149 | // Project end points on the line 150 | if (D > 0.0) { 151 | Point projected_p1, projected_p2; 152 | 153 | projected_p1.x = ( B * B * p1.x - A * B * p1.y - A * C) / D; 154 | projected_p1.y = (-A * B * p1.x + A * A * p1.y - B * C) / D; 155 | 156 | projected_p2.x = ( B * B * p2.x - A * B * p2.y - A * C) / D; 157 | projected_p2.y = (-A * B * p2.x + A * A * p2.y - B * C) / D; 158 | 159 | segment.first_point = projected_p1; 160 | segment.last_point = projected_p2; 161 | } 162 | 163 | return segment; 164 | } 165 | 166 | /* 167 | * Returns a total best fit approximation of 168 | * a circle based on given point set. The equation 169 | * used for fitting is given by 170 | * a1 * x + a2 * y + a3 = -(x^2 + y^2) 171 | * where parameters a1, a2, a3 are obtained from 172 | * circle equation 173 | * (x-x0)^2 + (y-y0)^2 = r^2. 174 | */ 175 | Circle fitCircle(const std::list& point_set) 176 | { 177 | int N = point_set.size(); 178 | assert(N >= 3); 179 | 180 | arma::mat input = arma::mat(N, 3).zeros(); // [x_i, y_i, 1] 181 | arma::vec output = arma::vec(N).zeros(); // [-(x_i^2 + y_i^2)] 182 | arma::vec params = arma::vec(3).zeros(); // [a_1 ; a_2 ; a_3] 183 | 184 | int i = 0; 185 | for (const Point& point : point_set) { 186 | input(i, 0) = point.x; 187 | input(i, 1) = point.y; 188 | input(i, 2) = 1.0; 189 | 190 | output(i) = -(pow(point.x, 2) + pow(point.y, 2)); 191 | i++; 192 | } 193 | 194 | // Find a_1, a_2 and a_3 coefficients from linear regression 195 | params = arma::pinv(input) * output; 196 | 197 | Point center(-params(0) / 2.0, -params(1) / 2.0); 198 | double radius = sqrt((params(0) * params(0) + params(1) * params(1)) / 4.0 - params(2)); 199 | 200 | return Circle(center, radius); 201 | } 202 | 203 | } // namespace obstacle_detector 204 | -------------------------------------------------------------------------------- /include/obstacle_detector/utilities/kalman.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | 40 | class KalmanFilter 41 | { 42 | public: 43 | KalmanFilter(uint dim_in, uint dim_out, uint dim_state) : l(dim_in), m(dim_out), n(dim_state) { 44 | using arma::mat; 45 | using arma::vec; 46 | 47 | A = mat(n,n).eye(); 48 | B = mat(n,l).zeros(); 49 | C = mat(m,n).zeros(); 50 | 51 | Q = mat(n,n).eye(); 52 | R = mat(m,m).eye(); 53 | P = mat(n,n).eye(); 54 | 55 | K = mat(n,m).eye(); 56 | I = arma::eye(n,n); 57 | 58 | u = vec(l).zeros(); 59 | q_pred = vec(n).zeros(); 60 | q_est = vec(n).zeros(); 61 | y = vec(m).zeros(); 62 | } 63 | 64 | void predictState() { 65 | q_pred = A * q_est + B * u; 66 | P = A * P * trans(A) + Q; 67 | } 68 | 69 | void correctState() { 70 | K = P * trans(C) * inv(C * P * trans(C) + R); 71 | q_est = q_pred + K * (y - C * q_pred); 72 | P = (I - K * C) * P; 73 | } 74 | 75 | void updateState() { 76 | predictState(); 77 | correctState(); 78 | } 79 | 80 | public: 81 | // System matrices: 82 | arma::mat A; // State 83 | arma::mat B; // Input 84 | arma::mat C; // Output 85 | 86 | // Covariance matrices: 87 | arma::mat Q; // Process 88 | arma::mat R; // Measurement 89 | arma::mat P; // Estimate error 90 | 91 | // Kalman gain matrix: 92 | arma::mat K; 93 | 94 | // Identity matrix 95 | arma::mat I; 96 | 97 | // Signals: 98 | arma::vec u; // Input 99 | arma::vec q_pred; // Predicted state 100 | arma::vec q_est; // Estimated state 101 | arma::vec y; // Measurement 102 | 103 | private: 104 | // Dimensions: 105 | uint l; // Input 106 | uint m; // Output 107 | uint n; // State 108 | }; 109 | -------------------------------------------------------------------------------- /include/obstacle_detector/utilities/math_utilities.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | #include "obstacle_detector/utilities/point.h" 43 | 44 | namespace obstacle_detector 45 | { 46 | 47 | inline double signum(double x) { return (x < 0.0) ? -1.0 : 1.0; } 48 | inline double abs(double x) { return (x < 0.0) ? -x : x; } 49 | inline double max(double x, double y) { return (x > y) ? x : y; } 50 | 51 | inline double length(const geometry_msgs::Point& point) { 52 | return sqrt(point.x * point.x + point.y * point.y); 53 | } 54 | 55 | inline double squaredLength(const geometry_msgs::Point& point) { 56 | return point.x * point.x + point.y * point.y; 57 | } 58 | 59 | inline double length(const geometry_msgs::Vector3& vec) { 60 | return sqrt(vec.x * vec.x + vec.y * vec.y); 61 | } 62 | 63 | inline double squaredLength(const geometry_msgs::Vector3& vec) { 64 | return vec.x * vec.x + vec.y * vec.y; 65 | } 66 | 67 | inline geometry_msgs::Point transformPoint(const geometry_msgs::Point& point, double x, double y, double theta) { 68 | geometry_msgs::Point p; 69 | 70 | p.x = point.x * cos(theta) - point.y * sin(theta) + x; 71 | p.y = point.x * sin(theta) + point.y * cos(theta) + y; 72 | 73 | return p; 74 | } 75 | 76 | inline geometry_msgs::Point32 transformPoint(const geometry_msgs::Point32& point, double x, double y, double theta) { 77 | geometry_msgs::Point32 p; 78 | 79 | p.x = point.x * cos(theta) - point.y * sin(theta) + x; 80 | p.y = point.x * sin(theta) + point.y * cos(theta) + y; 81 | 82 | return p; 83 | } 84 | 85 | inline Point transformPoint(const Point point, double x, double y, double theta) { 86 | Point p; 87 | 88 | p.x = point.x * cos(theta) - point.y * sin(theta) + x; 89 | p.y = point.x * sin(theta) + point.y * cos(theta) + y; 90 | 91 | return p; 92 | } 93 | 94 | inline Point transformPoint(const Point& point, const tf::StampedTransform& transform) { 95 | tf::Vector3 v(point.x, point.y, 0); 96 | v = transform * v; 97 | 98 | return {v.x(), v.y()}; 99 | } 100 | 101 | inline bool checkPointInLimits(const geometry_msgs::Point32& p, double x_min, double x_max, double y_min, double y_max) { 102 | if ((p.x > x_max) || (p.x < x_min) || (p.y > y_max) || (p.y < y_min)) 103 | return false; 104 | else 105 | return true; 106 | } 107 | 108 | } // namespace obstacle_detector 109 | -------------------------------------------------------------------------------- /include/obstacle_detector/utilities/point.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | #include 40 | 41 | namespace obstacle_detector 42 | { 43 | 44 | class Point 45 | { 46 | public: 47 | Point(double x = 0.0, double y = 0.0) : x(x), y(y) {} 48 | Point(const Point& p) : x(p.x), y(p.y) {} 49 | static Point fromPoolarCoords(const double r, const double phi) { return Point(r * cos(phi), r * sin(phi)); } 50 | 51 | double length() const { return sqrt(pow(x, 2.0) + pow(y, 2.0)); } 52 | double lengthSquared() const { return pow(x, 2.0) + pow(y, 2.0); } 53 | double angle() const { return atan2(y, x); } 54 | double angleDeg() const { return 180.0 * atan2(y, x) / M_PI; } 55 | double dot(const Point& p) const { return x * p.x + y * p.y; } 56 | double cross(const Point& p) const { return x * p.y - y * p.x; } 57 | 58 | Point normalized() { return (length() > 0.0) ? *this / length() : *this; } 59 | Point reflected(const Point& normal) const { return *this - 2.0 * normal * (normal.dot(*this)); } 60 | Point perpendicular() const { return Point(-y, x); } 61 | 62 | friend Point operator+ (const Point& p1, const Point& p2) { return Point(p1.x + p2.x, p1.y + p2.y); } 63 | friend Point operator- (const Point& p1, const Point& p2) { return Point(p1.x - p2.x, p1.y - p2.y); } 64 | friend Point operator* (const double f, const Point& p) { return Point(f * p.x, f * p.y); } 65 | friend Point operator* (const Point& p, const double f) { return Point(f * p.x, f * p.y); } 66 | friend Point operator/ (const Point& p, const double f) { return (f != 0.0) ? Point(p.x / f, p.y / f) : Point(); } 67 | 68 | Point operator- () { return Point(-x, -y); } 69 | Point operator+ () { return Point( x, y); } 70 | 71 | Point& operator= (const Point& p) { if (this != &p) { x = p.x; y = p.y; } return *this; } 72 | Point& operator+= (const Point& p) { x += p.x; y += p.y; return *this; } 73 | Point& operator-= (const Point& p) { x -= p.x; y -= p.y; return *this; } 74 | 75 | friend bool operator== (const Point& p1, const Point& p2) { return (p1.x == p2.x && p1.y == p2.y); } 76 | friend bool operator!= (const Point& p1, const Point& p2) { return !(p1 == p2); } 77 | friend bool operator< (const Point& p1, const Point& p2) { return (p1.lengthSquared() < p2.lengthSquared()); } 78 | friend bool operator<= (const Point& p1, const Point& p2) { return (p1.lengthSquared() <= p2.lengthSquared()); } 79 | friend bool operator> (const Point& p1, const Point& p2) { return (p1.lengthSquared() > p2.lengthSquared()); } 80 | friend bool operator>= (const Point& p1, const Point& p2) { return (p1.lengthSquared() >= p2.lengthSquared()); } 81 | friend bool operator! (const Point& p1) { return (p1.x == 0.0 && p1.y == 0.0); } 82 | 83 | friend std::ostream& operator<<(std::ostream& out, const Point& p) 84 | { out << "(" << p.x << ", " << p.y << ")"; return out; } 85 | 86 | double x; 87 | double y; 88 | }; 89 | 90 | } // namespace obstacle_detector 91 | -------------------------------------------------------------------------------- /include/obstacle_detector/utilities/point_set.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | 40 | #include "obstacle_detector/utilities/point.h" 41 | 42 | namespace obstacle_detector 43 | { 44 | 45 | typedef std::list::iterator PointIterator; 46 | 47 | class PointSet 48 | { 49 | public: 50 | PointSet() : num_points(0), is_visible(false) {} 51 | 52 | PointIterator begin, end; // The iterators point to the list of points existing somewhere else 53 | int num_points; 54 | bool is_visible; // The point set is not occluded by any other point set 55 | }; 56 | 57 | } // namespace obstacle_detector 58 | 59 | -------------------------------------------------------------------------------- /include/obstacle_detector/utilities/segment.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | 40 | #include "obstacle_detector/utilities/point.h" 41 | #include "obstacle_detector/utilities/point_set.h" 42 | 43 | namespace obstacle_detector 44 | { 45 | 46 | class Segment 47 | { 48 | public: 49 | Segment(const Point& p1 = Point(), const Point& p2 = Point()) { 50 | // Swap if not counter-clockwise 51 | if (p1.cross(p2) > 0.0) 52 | first_point = p1, last_point = p2; 53 | else 54 | first_point = p2, last_point = p1; 55 | } 56 | 57 | double length() const { 58 | return (last_point - first_point).length(); 59 | } 60 | 61 | double lengthSquared() const { 62 | return (last_point - first_point).lengthSquared(); 63 | } 64 | 65 | Point normal() const { 66 | return (last_point - first_point).perpendicular().normalized(); 67 | } 68 | 69 | Point projection(const Point& p) const { 70 | Point a = last_point - first_point; 71 | Point b = p - first_point; 72 | return first_point + a.dot(b) * a / a.lengthSquared(); 73 | } 74 | 75 | Point trueProjection(const Point& p) const { 76 | Point a = last_point - first_point; 77 | Point b = p - first_point; 78 | Point c = p - last_point; 79 | 80 | double t = a.dot(b) / a.lengthSquared(); 81 | 82 | if (t < 0.0) 83 | return (first_point); 84 | else if (t > 1.0) 85 | return (last_point); 86 | else 87 | return first_point + a.dot(b) * a / a.lengthSquared(); 88 | } 89 | 90 | double distanceTo(const Point& p) const { 91 | return (p - projection(p)).length(); 92 | } 93 | 94 | double trueDistanceTo(const Point& p) const { 95 | Point a = last_point - first_point; 96 | Point b = p - first_point; 97 | Point c = p - last_point; 98 | 99 | double t = a.dot(b) / a.lengthSquared(); 100 | 101 | if (t < 0.0) 102 | return b.length(); 103 | else if (t > 1.0) 104 | return c.length(); 105 | 106 | Point projection = first_point + t * a; 107 | return (p - projection).length(); 108 | } 109 | 110 | 111 | friend std::ostream& operator<<(std::ostream& out, const Segment& s) { 112 | out << "p1: " << s.first_point << ", p2: " << s.last_point; 113 | return out; 114 | } 115 | 116 | Point first_point; 117 | Point last_point; 118 | std::vector point_sets; 119 | }; 120 | 121 | } // namespace obstacle_detector 122 | -------------------------------------------------------------------------------- /include/obstacle_detector/utilities/tracked_obstacle.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | #include "obstacle_detector/utilities/kalman.h" 40 | 41 | namespace obstacle_detector 42 | { 43 | 44 | class TrackedObstacle { 45 | public: 46 | TrackedObstacle(const CircleObstacle& obstacle) : obstacle_(obstacle), kf_x_(0, 1, 2), kf_y_(0, 1, 2), kf_r_(0, 1, 2) { 47 | fade_counter_ = s_fade_counter_size_; 48 | initKF(); 49 | } 50 | 51 | void predictState() { 52 | kf_x_.predictState(); 53 | kf_y_.predictState(); 54 | kf_r_.predictState(); 55 | 56 | obstacle_.center.x = kf_x_.q_pred(0); 57 | obstacle_.center.y = kf_y_.q_pred(0); 58 | 59 | obstacle_.velocity.x = kf_x_.q_pred(1); 60 | obstacle_.velocity.y = kf_y_.q_pred(1); 61 | 62 | obstacle_.radius = kf_r_.q_pred(0); 63 | 64 | fade_counter_--; 65 | } 66 | 67 | void correctState(const CircleObstacle& new_obstacle) { 68 | kf_x_.y(0) = new_obstacle.center.x; 69 | kf_y_.y(0) = new_obstacle.center.y; 70 | kf_r_.y(0) = new_obstacle.radius; 71 | 72 | kf_x_.correctState(); 73 | kf_y_.correctState(); 74 | kf_r_.correctState(); 75 | 76 | obstacle_.center.x = kf_x_.q_est(0); 77 | obstacle_.center.y = kf_y_.q_est(0); 78 | 79 | obstacle_.velocity.x = kf_x_.q_est(1); 80 | obstacle_.velocity.y = kf_y_.q_est(1); 81 | 82 | obstacle_.radius = kf_r_.q_est(0); 83 | 84 | fade_counter_ = s_fade_counter_size_; 85 | } 86 | 87 | void updateState() { 88 | kf_x_.predictState(); 89 | kf_y_.predictState(); 90 | kf_r_.predictState(); 91 | 92 | kf_x_.correctState(); 93 | kf_y_.correctState(); 94 | kf_r_.correctState(); 95 | 96 | obstacle_.center.x = kf_x_.q_est(0); 97 | obstacle_.center.y = kf_y_.q_est(0); 98 | 99 | obstacle_.velocity.x = kf_x_.q_est(1); 100 | obstacle_.velocity.y = kf_y_.q_est(1); 101 | 102 | obstacle_.radius = kf_r_.q_est(0); 103 | 104 | fade_counter_--; 105 | } 106 | 107 | static void setSamplingTime(double tp) { 108 | s_sampling_time_ = tp; 109 | } 110 | 111 | static void setCounterSize(int size) { 112 | s_fade_counter_size_ = size; 113 | } 114 | 115 | static void setCovariances(double process_var, double process_rate_var, double measurement_var) { 116 | s_process_variance_ = process_var; 117 | s_process_rate_variance_ = process_rate_var; 118 | s_measurement_variance_ = measurement_var; 119 | } 120 | 121 | bool hasFaded() const { return ((fade_counter_ <= 0) ? true : false); } 122 | const CircleObstacle& getObstacle() const { return obstacle_; } 123 | const KalmanFilter& getKFx() const { return kf_x_; } 124 | const KalmanFilter& getKFy() const { return kf_y_; } 125 | const KalmanFilter& getKFr() const { return kf_r_; } 126 | 127 | private: 128 | void initKF() { 129 | kf_x_.A(0, 1) = s_sampling_time_; 130 | kf_y_.A(0, 1) = s_sampling_time_; 131 | kf_r_.A(0, 1) = s_sampling_time_; 132 | 133 | kf_x_.C(0, 0) = 1.0; 134 | kf_y_.C(0, 0) = 1.0; 135 | kf_r_.C(0, 0) = 1.0; 136 | 137 | kf_x_.R(0, 0) = s_measurement_variance_; 138 | kf_y_.R(0, 0) = s_measurement_variance_; 139 | kf_r_.R(0, 0) = s_measurement_variance_; 140 | 141 | kf_x_.Q(0, 0) = s_process_variance_; 142 | kf_r_.Q(0, 0) = s_process_variance_; 143 | kf_y_.Q(0, 0) = s_process_variance_; 144 | 145 | kf_x_.Q(1, 1) = s_process_rate_variance_; 146 | kf_y_.Q(1, 1) = s_process_rate_variance_; 147 | kf_r_.Q(1, 1) = s_process_rate_variance_; 148 | 149 | kf_x_.q_pred(0) = obstacle_.center.x; 150 | kf_r_.q_pred(0) = obstacle_.radius; 151 | kf_y_.q_pred(0) = obstacle_.center.y; 152 | 153 | kf_x_.q_pred(1) = obstacle_.velocity.x; 154 | kf_y_.q_pred(1) = obstacle_.velocity.y; 155 | 156 | kf_x_.q_est(0) = obstacle_.center.x; 157 | kf_r_.q_est(0) = obstacle_.radius; 158 | kf_y_.q_est(0) = obstacle_.center.y; 159 | 160 | kf_x_.q_est(1) = obstacle_.velocity.x; 161 | kf_y_.q_est(1) = obstacle_.velocity.y; 162 | } 163 | 164 | CircleObstacle obstacle_; 165 | 166 | KalmanFilter kf_x_; 167 | KalmanFilter kf_y_; 168 | KalmanFilter kf_r_; 169 | 170 | int fade_counter_; 171 | 172 | // Common variables 173 | static int s_fade_counter_size_; 174 | static double s_sampling_time_; 175 | static double s_process_variance_; 176 | static double s_process_rate_variance_; 177 | static double s_measurement_variance_; 178 | }; 179 | 180 | } 181 | -------------------------------------------------------------------------------- /launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | [-3.0, -2.5, -2.5, -1.0, -1.0, -0.5, 2.5, 0.2, 2.0, 4.5, 4.0, 1.5] 84 | [1.5, 0.0, -2.5, 3.0, 1.0, -4.0, -3.0, -0.9, 0.0, 0.0, 2.0, 2.0] 85 | [0.5, 0.5, 1.5, 0.5, 0.7, 0.5, 1.5, 0.7, 0.7, 1.0, 0.5, 1.0] 86 | [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 87 | [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /launch/nodelets.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | [-3.0, -2.5, -2.5, -1.0, -1.0, -0.5, 2.5, 0.2, 2.0, 4.5, 4.0, 1.5] 75 | [1.5, 0.0, -2.5, 3.0, 1.0, -4.0, -3.0, -0.9, 0.0, 0.0, 2.0, 2.0] 76 | [0.5, 0.5, 1.5, 0.5, 0.7, 0.5, 1.5, 0.7, 0.7, 1.0, 0.5, 1.0] 77 | [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 78 | [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 79 | 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /launch/nodes.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | [-3.0, -2.5, -2.5, -1.0, -1.0, -0.5, 2.5, 0.2, 2.0, 4.5, 4.0, 1.5] 73 | [1.5, 0.0, -2.5, 3.0, 1.0, -4.0, -3.0, -0.9, 0.0, 0.0, 2.0, 2.0] 74 | [0.5, 0.5, 1.5, 0.5, 0.7, 0.5, 1.5, 0.7, 0.7, 1.0, 0.5, 1.0] 75 | [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 76 | [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 77 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /msg/CircleObstacle.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point center # Central point [m] 2 | geometry_msgs/Vector3 velocity # Linear velocity [m/s] 3 | float64 radius # Radius with added margin [m] 4 | float64 true_radius # True measured radius [m] 5 | -------------------------------------------------------------------------------- /msg/Obstacles.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | obstacle_detector/SegmentObstacle[] segments 4 | obstacle_detector/CircleObstacle[] circles 5 | -------------------------------------------------------------------------------- /msg/SegmentObstacle.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point first_point # First point of the segment [m] 2 | geometry_msgs/Point last_point # Last point of the segment [m] 3 | -------------------------------------------------------------------------------- /nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | Scans Merger Nodelet 8 | 9 | 10 | 11 | 14 | 15 | Obstacle Extractor Nodelet 16 | 17 | 18 | 19 | 22 | 23 | Obstacle Tracker Nodelet 24 | 25 | 26 | 27 | 30 | 31 | Obstacle Publisher Nodelet 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | obstacle_detector 4 | 1.0.0 5 | Detect obstacles in form of line segments and circles from 2D laser scans. 6 | Mateusz Przybyla 7 | Mateusz Przybyla 8 | BSD 9 | 10 | catkin 11 | message_generation 12 | message_runtime 13 | 14 | tf 15 | rviz 16 | roscpp 17 | roslaunch 18 | nodelet 19 | std_msgs 20 | std_srvs 21 | sensor_msgs 22 | geometry_msgs 23 | laser_geometry 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /resources/ObstacleDetector.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tysik/obstacle_detector/bd59d405b2284f05c46f7b99cb9cb4247934e2ff/resources/ObstacleDetector.pdf -------------------------------------------------------------------------------- /resources/Using hokuyo_node with udev.txt: -------------------------------------------------------------------------------- 1 | How to use multiple Hokuyo laser scanners and determine which is which? 2 | ---- 3 | 4 | When you plug in the usb Hokuyo laser scanner into computer running Ubuntu, it shows up as a link in /dev/ folders (e.g. /dev/ttyACM0). If you have two or more laser scanners connected, you cannot be sure which link is dedicated to which device. In order to make sure that the Hokuyo laser scanners connected to the computer are being properly identified, one can make use of the getID node provided by the hokuyo_node package and the udev rules. To do so, take the following steps: 5 | 6 | 1. Create a file with an udev rule in the folder /etc/udev/rules.d: 7 | 8 | sudo gedit /etc/udev/rules.d/47-hokuyo.rules 9 | 10 | (the name and number is arbitrary but there should not be two rules with the same number). 11 | 12 | 2. Fill the file with the following rule (just paste the following in the file): 13 | 14 | SUBSYSTEMS=="usb", KERNEL=="ttyACM[0-9]*", ACTION=="add", ATTRS{idVendor}=="15d1", ATTRS{idProduct}=="0000", ATTRS{manufacturer}=="Hokuyo Data Flex for USB", ATTRS{product}=="URG-Series USB Driver", MODE="666", PROGRAM="/etc/ros/run.sh hokuyo_node getID %N q", SYMLINK+="sensors/hokuyo_%c", GROUP="dialout" 15 | 16 | (mind that the user should be a member of dialout group: sudo adduser $USER dialout). 17 | 18 | 3. Create a file named run.sh in the /etc/ros/ folder and provide it with executable rights: 19 | 20 | sudo touch /etc/ros/run.sh 21 | sudo chmod +x /etc/ros/run.sh 22 | 23 | 4. Fill the file with the following: 24 | 25 | #!/bin/sh 26 | . /opt/ros/hydro/setup.sh 27 | rosrun $@ 28 | 29 | (change the distribution of ROS to the one that you use - it was Hydromedusa in my case). 30 | 31 | 5. Refresh the udev rules list with: 32 | 33 | sudo udevadm control --reload-rules 34 | 35 | From now on, after plugging in the usb Hokuyo laser scanner, the /dev/ folder should not only contain ttyACMn links, but also /sensors/hokuyo_ links, with which you can unambiguously determine which device are you using. Good luck! 36 | 37 | -------------------------------------------------------------------------------- /resources/obstacle_detector.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 931 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Views 12 | Expanded: 13 | - /Current View1 14 | Name: Views 15 | Splitter Ratio: 0.5 16 | - Class: rviz/Time 17 | Experimental: false 18 | Name: Time 19 | SyncMode: 0 20 | SyncSource: Merged PCL 21 | - Class: Scans Merger 22 | Name: Scans Merger 23 | - Class: Obstacle Extractor 24 | Name: Obstacle Extractor 25 | - Class: Obstacle Tracker 26 | Name: Obstacle Tracker 27 | - Class: Obstacle Publisher 28 | Name: Obstacle Publisher 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.0299999993 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: map 49 | Value: true 50 | - Alpha: 1 51 | Autocompute Intensity Bounds: true 52 | Autocompute Value Bounds: 53 | Max Value: 10 54 | Min Value: -10 55 | Value: true 56 | Axis: Z 57 | Channel Name: intensity 58 | Class: rviz/LaserScan 59 | Color: 255; 0; 0 60 | Color Transformer: FlatColor 61 | Decay Time: 0 62 | Enabled: false 63 | Invert Rainbow: false 64 | Max Color: 255; 255; 255 65 | Max Intensity: 4096 66 | Min Color: 0; 0; 0 67 | Min Intensity: 0 68 | Name: Front Scanner 69 | Position Transformer: XYZ 70 | Queue Size: 10 71 | Selectable: false 72 | Size (Pixels): 3 73 | Size (m): 0.00999999978 74 | Style: Points 75 | Topic: /front_scan 76 | Unreliable: true 77 | Use Fixed Frame: true 78 | Use rainbow: true 79 | Value: false 80 | - Alpha: 1 81 | Autocompute Intensity Bounds: true 82 | Autocompute Value Bounds: 83 | Max Value: 10 84 | Min Value: -10 85 | Value: true 86 | Axis: Z 87 | Channel Name: intensity 88 | Class: rviz/LaserScan 89 | Color: 255; 255; 255 90 | Color Transformer: FlatColor 91 | Decay Time: 0 92 | Enabled: false 93 | Invert Rainbow: false 94 | Max Color: 255; 255; 255 95 | Max Intensity: 4096 96 | Min Color: 0; 0; 0 97 | Min Intensity: 0 98 | Name: Rear Scanner 99 | Position Transformer: XYZ 100 | Queue Size: 10 101 | Selectable: false 102 | Size (Pixels): 3 103 | Size (m): 0.00999999978 104 | Style: Points 105 | Topic: /rear_scan 106 | Unreliable: true 107 | Use Fixed Frame: true 108 | Use rainbow: true 109 | Value: false 110 | - Alpha: 1 111 | Autocompute Intensity Bounds: true 112 | Autocompute Value Bounds: 113 | Max Value: 10 114 | Min Value: -10 115 | Value: true 116 | Axis: Z 117 | Channel Name: intensity 118 | Class: rviz/LaserScan 119 | Color: 85; 170; 255 120 | Color Transformer: FlatColor 121 | Decay Time: 0 122 | Enabled: true 123 | Invert Rainbow: false 124 | Max Color: 255; 255; 255 125 | Max Intensity: 4096 126 | Min Color: 0; 0; 0 127 | Min Intensity: 0 128 | Name: Merged Scan 129 | Position Transformer: XYZ 130 | Queue Size: 10 131 | Selectable: false 132 | Size (Pixels): 5 133 | Size (m): 0.00999999978 134 | Style: Points 135 | Topic: /scan 136 | Unreliable: true 137 | Use Fixed Frame: true 138 | Use rainbow: true 139 | Value: true 140 | - Alpha: 1 141 | Autocompute Intensity Bounds: true 142 | Autocompute Value Bounds: 143 | Max Value: 10 144 | Min Value: -10 145 | Value: true 146 | Axis: Z 147 | Channel Name: intensity 148 | Class: rviz/PointCloud 149 | Color: 85; 170; 255 150 | Color Transformer: FlatColor 151 | Decay Time: 0 152 | Enabled: true 153 | Invert Rainbow: false 154 | Max Color: 255; 255; 255 155 | Max Intensity: 4096 156 | Min Color: 0; 0; 0 157 | Min Intensity: 0 158 | Name: Merged PCL 159 | Position Transformer: XYZ 160 | Queue Size: 10 161 | Selectable: false 162 | Size (Pixels): 5 163 | Size (m): 0.00999999978 164 | Style: Points 165 | Topic: /pcl 166 | Unreliable: true 167 | Use Fixed Frame: true 168 | Use rainbow: true 169 | Value: true 170 | - Circles color: 170; 0; 0 171 | Class: Obstacles 172 | Enabled: true 173 | Margin color: 0; 170; 0 174 | Name: Obstacles 175 | Opacity: 0.75 176 | Segments color: 170; 170; 0 177 | Segments thickness: 0.0299999993 178 | Topic: /obstacles 179 | Unreliable: true 180 | Value: true 181 | - Class: rviz/TF 182 | Enabled: true 183 | Frame Timeout: 15 184 | Frames: 185 | All Enabled: true 186 | front_scanner: 187 | Value: true 188 | map: 189 | Value: true 190 | rear_scanner: 191 | Value: true 192 | robot: 193 | Value: true 194 | Marker Scale: 1 195 | Name: TF 196 | Show Arrows: true 197 | Show Axes: true 198 | Show Names: true 199 | Tree: 200 | map: 201 | robot: 202 | front_scanner: 203 | {} 204 | rear_scanner: 205 | {} 206 | Update Interval: 0 207 | Value: true 208 | Enabled: true 209 | Global Options: 210 | Background Color: 48; 48; 48 211 | Fixed Frame: map 212 | Frame Rate: 30 213 | Name: root 214 | Tools: 215 | - Class: rviz/Interact 216 | Hide Inactive Objects: true 217 | - Class: rviz/MoveCamera 218 | - Class: rviz/Select 219 | - Class: rviz/FocusCamera 220 | - Class: rviz/Measure 221 | - Class: rviz/SetInitialPose 222 | Topic: /initialpose 223 | - Class: rviz/SetGoal 224 | Topic: /move_base_simple/goal 225 | - Class: rviz/PublishPoint 226 | Single click: true 227 | Topic: /clicked_point 228 | Value: true 229 | Views: 230 | Current: 231 | Angle: 0 232 | Class: rviz/TopDownOrtho 233 | Enable Stereo Rendering: 234 | Stereo Eye Separation: 0.0599999987 235 | Stereo Focal Distance: 1 236 | Swap Stereo Eyes: false 237 | Value: false 238 | Name: Current View 239 | Near Clip Distance: 0.00999999978 240 | Scale: 77.9355011 241 | Target Frame: map 242 | Value: TopDownOrtho (rviz) 243 | X: 0 244 | Y: 0 245 | Saved: ~ 246 | Window Geometry: 247 | Displays: 248 | collapsed: false 249 | Height: 1056 250 | Hide Left Dock: false 251 | Hide Right Dock: false 252 | Obstacle Extractor: 253 | collapsed: false 254 | Obstacle Publisher: 255 | collapsed: false 256 | Obstacle Tracker: 257 | collapsed: false 258 | QMainWindow State: 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 259 | Scans Merger: 260 | collapsed: false 261 | Selection: 262 | collapsed: false 263 | Time: 264 | collapsed: false 265 | Views: 266 | collapsed: false 267 | Width: 1855 268 | X: 65 269 | Y: 24 270 | -------------------------------------------------------------------------------- /resources/play.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tysik/obstacle_detector/bd59d405b2284f05c46f7b99cb9cb4247934e2ff/resources/play.png -------------------------------------------------------------------------------- /resources/scans_demo.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tysik/obstacle_detector/bd59d405b2284f05c46f7b99cb9cb4247934e2ff/resources/scans_demo.bag -------------------------------------------------------------------------------- /resources/stop.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tysik/obstacle_detector/bd59d405b2284f05c46f7b99cb9cb4247934e2ff/resources/stop.png -------------------------------------------------------------------------------- /rviz_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | Scans Merger Panel. 8 | 9 | 10 | 11 | 14 | 15 | Obstacle Extractor Panel. 16 | 17 | 18 | 19 | 22 | 23 | Obstacle Tracker Panel. 24 | 25 | 26 | 27 | 30 | 31 | Obstacle Publisher Panel. 32 | 33 | 34 | 35 | 38 | 39 | Obstacles Display 40 | 41 | obstacle_detector/Obstacles 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /src/displays/circle_visual.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include "obstacle_detector/displays/circle_visual.h" 37 | 38 | namespace obstacles_display 39 | { 40 | 41 | CircleVisual::CircleVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node) { 42 | scene_manager_ = scene_manager; 43 | frame_node_1_ = parent_node->createChildSceneNode(); 44 | frame_node_2_ = parent_node->createChildSceneNode(); 45 | 46 | obstacle_.reset(new rviz::Shape(rviz::Shape::Cylinder, scene_manager_, frame_node_1_)); 47 | margin_.reset(new rviz::Shape(rviz::Shape::Cylinder, scene_manager_, frame_node_2_)); 48 | } 49 | 50 | CircleVisual::~CircleVisual() { 51 | scene_manager_->destroySceneNode(frame_node_1_); 52 | scene_manager_->destroySceneNode(frame_node_2_); 53 | } 54 | 55 | void CircleVisual::setData(const obstacle_detector::CircleObstacle& circle) { 56 | Ogre::Vector3 pos(circle.center.x, circle.center.y, 0.25); 57 | obstacle_->setPosition(pos); 58 | 59 | Ogre::Vector3 true_scale(2.0 * circle.true_radius, 0.1, 2.0 * circle.true_radius); 60 | obstacle_->setScale(true_scale); 61 | 62 | Ogre::Vector3 pos2(circle.center.x, circle.center.y, 0.1); 63 | margin_->setPosition(pos2); 64 | 65 | Ogre::Vector3 scale(2.0 * circle.radius, 0.2, 2.0 * circle.radius); 66 | margin_->setScale(scale); 67 | 68 | Ogre::Vector3 dir(Ogre::Real(1.0), Ogre::Real(0.0), Ogre::Real(0.0)); 69 | Ogre::Radian angle(Ogre::Real(M_PI_2)); 70 | Ogre::Quaternion q(angle, dir); 71 | obstacle_->setOrientation(q); 72 | margin_->setOrientation(q); 73 | } 74 | 75 | void CircleVisual::setFramePosition(const Ogre::Vector3& position) { 76 | frame_node_1_->setPosition(position); 77 | frame_node_2_->setPosition(position); 78 | } 79 | 80 | void CircleVisual::setFrameOrientation(const Ogre::Quaternion& orientation) { 81 | frame_node_1_->setOrientation(orientation); 82 | frame_node_2_->setOrientation(orientation); 83 | } 84 | 85 | void CircleVisual::setMainColor(float r, float g, float b, float a) { 86 | obstacle_->setColor(r, g, b, a); 87 | } 88 | 89 | void CircleVisual::setMarginColor(float r, float g, float b, float a) { 90 | margin_->setColor(r, g, b, a); 91 | } 92 | 93 | } // end namespace obstacles_display 94 | 95 | -------------------------------------------------------------------------------- /src/displays/obstacles_display.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include "obstacle_detector/displays/obstacles_display.h" 37 | 38 | namespace obstacles_display 39 | { 40 | 41 | ObstaclesDisplay::ObstaclesDisplay() { 42 | circle_color_property_ = new rviz::ColorProperty("Circles color", QColor(170, 0, 0), "Color of circles.", this, SLOT(updateCircleColor())); 43 | margin_color_property_ = new rviz::ColorProperty("Margin color", QColor(0, 170, 0), "Color of margin added around circles.", this, SLOT(updateCircleColor())); 44 | segment_color_property_ = new rviz::ColorProperty("Segments color", QColor(170, 170, 0), "Color of segments.", this, SLOT(updateSegmentColor())); 45 | alpha_property_ = new rviz::FloatProperty("Opacity", 0.75, "Value 0,0 is fully transparent, 1,0 is fully opaque.", this, SLOT(updateAlpha())); 46 | thickness_property_ = new rviz::FloatProperty("Segments thickness", 0.03f, "Width of the segments in meters.", this, SLOT(updateThickness())); 47 | } 48 | 49 | void ObstaclesDisplay::onInitialize() { 50 | MessageFilterDisplay::onInitialize(); 51 | } 52 | 53 | ObstaclesDisplay::~ObstaclesDisplay() {} 54 | 55 | void ObstaclesDisplay::reset() { 56 | MessageFilterDisplay::reset(); 57 | circle_visuals_.clear(); 58 | segment_visuals_.clear(); 59 | } 60 | 61 | void ObstaclesDisplay::updateCircleColor() { 62 | float alpha = alpha_property_->getFloat(); 63 | Ogre::ColourValue main_color = circle_color_property_->getOgreColor(); 64 | Ogre::ColourValue margin_color = margin_color_property_->getOgreColor(); 65 | 66 | for (auto& c : circle_visuals_) { 67 | c->setMainColor(main_color.r, main_color.g, main_color.b, alpha); 68 | c->setMarginColor(margin_color.r, margin_color.g, margin_color.b, alpha); 69 | } 70 | } 71 | 72 | void ObstaclesDisplay::updateSegmentColor() { 73 | float alpha = alpha_property_->getFloat(); 74 | Ogre::ColourValue color = segment_color_property_->getOgreColor(); 75 | 76 | for (auto& s : segment_visuals_) 77 | s->setColor(color.r, color.g, color.b, alpha); 78 | } 79 | 80 | void ObstaclesDisplay::updateAlpha() { 81 | updateCircleColor(); 82 | updateSegmentColor(); 83 | } 84 | 85 | void ObstaclesDisplay::updateThickness() { 86 | float width = thickness_property_->getFloat(); 87 | 88 | for (auto& s : segment_visuals_) 89 | s->setWidth(width); 90 | } 91 | 92 | void ObstaclesDisplay::processMessage(const obstacle_detector::Obstacles::ConstPtr& obstacles_msg) { 93 | circle_visuals_.clear(); 94 | segment_visuals_.clear(); 95 | 96 | Ogre::Quaternion orientation; 97 | Ogre::Vector3 position; 98 | if (!context_->getFrameManager()->getTransform(obstacles_msg->header.frame_id, obstacles_msg->header.stamp, position, orientation)) { 99 | ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", obstacles_msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); 100 | return; 101 | } 102 | 103 | for (const auto& circle : obstacles_msg->circles) { 104 | boost::shared_ptr visual; 105 | visual.reset(new CircleVisual(context_->getSceneManager(), scene_node_)); 106 | 107 | visual->setData(circle); 108 | visual->setFramePosition(position); 109 | visual->setFrameOrientation(orientation); 110 | 111 | circle_visuals_.push_back(visual); 112 | } 113 | 114 | for (const auto& segment : obstacles_msg->segments) { 115 | boost::shared_ptr visual; 116 | visual.reset(new SegmentVisual(context_->getSceneManager(), scene_node_)); 117 | 118 | visual->setData(segment); 119 | visual->setFramePosition(position); 120 | visual->setFrameOrientation(orientation); 121 | 122 | segment_visuals_.push_back(visual); 123 | } 124 | 125 | updateAlpha(); 126 | updateThickness(); 127 | } 128 | 129 | } // end namespace obstacles_display 130 | 131 | #include 132 | PLUGINLIB_EXPORT_CLASS(obstacles_display::ObstaclesDisplay, rviz::Display) 133 | 134 | -------------------------------------------------------------------------------- /src/displays/segment_visual.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include "obstacle_detector/displays/segment_visual.h" 37 | 38 | namespace obstacles_display 39 | { 40 | 41 | SegmentVisual::SegmentVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node) { 42 | scene_manager_ = scene_manager; 43 | frame_node_ = parent_node->createChildSceneNode(); 44 | 45 | line_.reset(new rviz::BillboardLine(scene_manager_, frame_node_)); 46 | } 47 | 48 | SegmentVisual::~SegmentVisual() { 49 | scene_manager_->destroySceneNode(frame_node_); 50 | } 51 | 52 | void SegmentVisual::setData(const obstacle_detector::SegmentObstacle& segment) { 53 | Ogre::Vector3 p1(segment.first_point.x, segment.first_point.y, 0.0); 54 | Ogre::Vector3 p2(segment.last_point.x, segment.last_point.y, 0.0); 55 | line_->addPoint(p1); 56 | line_->addPoint(p2); 57 | } 58 | 59 | void SegmentVisual::setFramePosition(const Ogre::Vector3& position) { 60 | frame_node_->setPosition(position); 61 | } 62 | 63 | void SegmentVisual::setFrameOrientation(const Ogre::Quaternion& orientation) { 64 | frame_node_->setOrientation(orientation); 65 | } 66 | 67 | void SegmentVisual::setColor(float r, float g, float b, float a) { 68 | line_->setColor(r, g, b, a); 69 | } 70 | 71 | void SegmentVisual::setWidth(float w) { 72 | line_->setLineWidth(w); 73 | } 74 | 75 | } // end namespace obstacles_display 76 | 77 | -------------------------------------------------------------------------------- /src/nodelets/obstacle_extractor_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include 37 | #include 38 | 39 | #include "obstacle_detector/obstacle_extractor.h" 40 | 41 | namespace obstacle_detector 42 | { 43 | 44 | class ObstacleExtractorNodelet : public nodelet::Nodelet 45 | { 46 | public: 47 | virtual void onInit() { 48 | ros::NodeHandle nh = getNodeHandle(); 49 | ros::NodeHandle nh_local = getPrivateNodeHandle(); 50 | 51 | try { 52 | NODELET_INFO("[Obstacle Extractor]: Initializing nodelet"); 53 | obstacle_extractor_ = std::shared_ptr(new ObstacleExtractor(nh, nh_local)); 54 | } 55 | catch (const char* s) { 56 | NODELET_FATAL_STREAM("[Obstacle Extractor]: " << s); 57 | } 58 | catch (...) { 59 | NODELET_FATAL_STREAM("[Obstacle Extractor]: Unexpected error"); 60 | } 61 | } 62 | 63 | virtual ~ObstacleExtractorNodelet() { 64 | NODELET_INFO("[Obstacle Extractor]: Shutdown"); 65 | } 66 | 67 | private: 68 | std::shared_ptr obstacle_extractor_; 69 | }; 70 | 71 | } // namespace obstacle_detector 72 | 73 | #include 74 | PLUGINLIB_EXPORT_CLASS(obstacle_detector::ObstacleExtractorNodelet, nodelet::Nodelet) 75 | -------------------------------------------------------------------------------- /src/nodelets/obstacle_publisher_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include 37 | #include 38 | 39 | #include "obstacle_detector/obstacle_publisher.h" 40 | 41 | namespace obstacle_detector 42 | { 43 | 44 | class ObstaclePublisherNodelet : public nodelet::Nodelet 45 | { 46 | public: 47 | virtual void onInit() { 48 | ros::NodeHandle nh = getNodeHandle(); 49 | ros::NodeHandle nh_local = getPrivateNodeHandle(); 50 | 51 | try { 52 | NODELET_INFO("[Obstacle Publisher]: Initializing nodelet"); 53 | obstacle_publisher_ = std::shared_ptr(new ObstaclePublisher(nh, nh_local)); 54 | } 55 | catch (const char* s) { 56 | NODELET_FATAL_STREAM("[Obstacle Publisher]: " << s); 57 | } 58 | catch (...) { 59 | NODELET_FATAL_STREAM("[Obstacle Publisher]: Unexpected error"); 60 | } 61 | } 62 | 63 | virtual ~ObstaclePublisherNodelet() { 64 | NODELET_INFO("[Obstacle Publisher]: Shutdown"); 65 | } 66 | 67 | private: 68 | std::shared_ptr obstacle_publisher_; 69 | }; 70 | 71 | } // namespace obstacle_detector 72 | 73 | #include 74 | PLUGINLIB_EXPORT_CLASS(obstacle_detector::ObstaclePublisherNodelet, nodelet::Nodelet) 75 | -------------------------------------------------------------------------------- /src/nodelets/obstacle_tracker_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include 37 | #include 38 | 39 | #include "obstacle_detector/obstacle_tracker.h" 40 | 41 | namespace obstacle_detector 42 | { 43 | 44 | class ObstacleTrackerNodelet : public nodelet::Nodelet 45 | { 46 | public: 47 | virtual void onInit() { 48 | ros::NodeHandle nh = getNodeHandle(); 49 | ros::NodeHandle nh_local = getPrivateNodeHandle(); 50 | 51 | try { 52 | NODELET_INFO("[Obstacle Tracker]: Initializing nodelet"); 53 | obstacle_tracker_ = std::shared_ptr(new ObstacleTracker(nh, nh_local)); 54 | } 55 | catch (const char* s) { 56 | NODELET_FATAL_STREAM("[Obstacle Tracker]: " << s); 57 | } 58 | catch (...) { 59 | NODELET_FATAL_STREAM("[Obstacle Tracker]: Unexpected error"); 60 | } 61 | } 62 | 63 | virtual ~ObstacleTrackerNodelet() { 64 | NODELET_INFO("[Obstacle Tracker]: Shutdown"); 65 | } 66 | 67 | private: 68 | std::shared_ptr obstacle_tracker_; 69 | }; 70 | 71 | } // namespace obstacle_detector 72 | 73 | #include 74 | PLUGINLIB_EXPORT_CLASS(obstacle_detector::ObstacleTrackerNodelet, nodelet::Nodelet) 75 | -------------------------------------------------------------------------------- /src/nodelets/scans_merger_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include 37 | #include 38 | 39 | #include "obstacle_detector/scans_merger.h" 40 | 41 | namespace obstacle_detector 42 | { 43 | 44 | class ScansMergerNodelet : public nodelet::Nodelet 45 | { 46 | public: 47 | virtual void onInit() { 48 | ros::NodeHandle nh = getNodeHandle(); 49 | ros::NodeHandle nh_local = getPrivateNodeHandle(); 50 | 51 | try { 52 | NODELET_INFO("[Scans Merger]: Initializing nodelet"); 53 | scans_merger_ = std::shared_ptr(new ScansMerger(nh, nh_local)); 54 | } 55 | catch (const char* s) { 56 | NODELET_FATAL_STREAM("[Scans Merger]: " << s); 57 | } 58 | catch (...) { 59 | NODELET_FATAL_STREAM("[Scans Merger]: Unexpected error"); 60 | } 61 | } 62 | 63 | virtual ~ScansMergerNodelet() { 64 | NODELET_INFO("[Scans Merger]: Shutdown"); 65 | } 66 | 67 | private: 68 | std::shared_ptr scans_merger_; 69 | }; 70 | 71 | } // namespace obstacle_detector 72 | 73 | #include 74 | PLUGINLIB_EXPORT_CLASS(obstacle_detector::ScansMergerNodelet, nodelet::Nodelet) 75 | -------------------------------------------------------------------------------- /src/nodes/obstacle_extractor_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include "obstacle_detector/obstacle_extractor.h" 37 | 38 | using namespace obstacle_detector; 39 | 40 | int main(int argc, char** argv) { 41 | ros::init(argc, argv, "obstacle_extractor", ros::init_options::NoRosout); 42 | ros::NodeHandle nh(""); 43 | ros::NodeHandle nh_local("~"); 44 | 45 | try { 46 | ROS_INFO("[Obstacle Extractor]: Initializing node"); 47 | ObstacleExtractor od(nh, nh_local); 48 | ros::spin(); 49 | } 50 | catch (const char* s) { 51 | ROS_FATAL_STREAM("[Obstacle Extractor]: " << s); 52 | } 53 | catch (...) { 54 | ROS_FATAL_STREAM("[Obstacle Extractor]: Unexpected error"); 55 | } 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /src/nodes/obstacle_publisher_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include "obstacle_detector/obstacle_publisher.h" 37 | 38 | using namespace obstacle_detector; 39 | 40 | int main(int argc, char** argv) { 41 | ros::init(argc, argv, "obstacle_publisher", ros::init_options::NoRosout); 42 | ros::NodeHandle nh(""); 43 | ros::NodeHandle nh_local("~"); 44 | 45 | try { 46 | ROS_INFO("[Obstacle Publisher]: Initializing node"); 47 | ObstaclePublisher op(nh, nh_local); 48 | ros::spin(); 49 | } 50 | catch (const char* s) { 51 | ROS_FATAL_STREAM("[Obstacle Publisher]: " << s); 52 | } 53 | catch (...) { 54 | ROS_FATAL_STREAM("[Obstacle Publisher]: Unexpected error"); 55 | } 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /src/nodes/obstacle_tracker_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include "obstacle_detector/obstacle_tracker.h" 37 | 38 | using namespace obstacle_detector; 39 | 40 | int main(int argc, char** argv) { 41 | ros::init(argc, argv, "obstacle_tracker", ros::init_options::NoRosout); 42 | ros::NodeHandle nh(""); 43 | ros::NodeHandle nh_local("~"); 44 | 45 | try { 46 | ROS_INFO("[Obstacle Tracker]: Initializing node"); 47 | ObstacleTracker ot(nh, nh_local); 48 | ros::spin(); 49 | } 50 | catch (const char* s) { 51 | ROS_FATAL_STREAM("[Obstacle Tracker]: " << s); 52 | } 53 | catch (...) { 54 | ROS_FATAL_STREAM("[Obstacle Tracker]: Unexpected error"); 55 | } 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /src/nodes/scans_merger_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include "obstacle_detector/scans_merger.h" 37 | 38 | using namespace obstacle_detector; 39 | 40 | int main(int argc, char** argv) { 41 | ros::init(argc, argv, "scans_merger", ros::init_options::NoRosout); 42 | ros::NodeHandle nh(""); 43 | ros::NodeHandle nh_local("~"); 44 | 45 | try { 46 | ROS_INFO("[Scans Merger]: Initializing node"); 47 | ScansMerger sm(nh, nh_local); 48 | ros::spin(); 49 | } 50 | catch (const char* s) { 51 | ROS_FATAL_STREAM("[Scans Merger]: " << s); 52 | } 53 | catch (...) { 54 | ROS_FATAL_STREAM("[Scans Merger]: Unexpected error"); 55 | } 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /src/obstacle_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include "obstacle_detector/obstacle_publisher.h" 37 | 38 | using namespace std; 39 | using namespace obstacle_detector; 40 | 41 | ObstaclePublisher::ObstaclePublisher(ros::NodeHandle& nh, ros::NodeHandle& nh_local) : nh_(nh), nh_local_(nh_local) { 42 | p_active_ = false; 43 | t_ = 0.0; 44 | 45 | timer_ = nh_.createTimer(ros::Duration(1.0), &ObstaclePublisher::timerCallback, this, false, false); 46 | params_srv_ = nh_local_.advertiseService("params", &ObstaclePublisher::updateParams, this); 47 | initialize(); 48 | } 49 | 50 | ObstaclePublisher::~ObstaclePublisher() { 51 | nh_local_.deleteParam("active"); 52 | nh_local_.deleteParam("reset"); 53 | 54 | nh_local_.deleteParam("fusion_example"); 55 | nh_local_.deleteParam("fission_example"); 56 | 57 | nh_local_.deleteParam("loop_rate"); 58 | nh_local_.deleteParam("radius_margin"); 59 | 60 | nh_local_.deleteParam("x_vector"); 61 | nh_local_.deleteParam("y_vector"); 62 | nh_local_.deleteParam("r_vector"); 63 | 64 | nh_local_.deleteParam("vx_vector"); 65 | nh_local_.deleteParam("vy_vector"); 66 | 67 | nh_local_.deleteParam("frame_id"); 68 | } 69 | 70 | bool ObstaclePublisher::updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { 71 | bool prev_active = p_active_; 72 | 73 | nh_local_.param("active", p_active_, true); 74 | nh_local_.param("reset", p_reset_, false); 75 | 76 | nh_local_.param("fusion_example", p_fusion_example_, false); 77 | nh_local_.param("fission_example", p_fission_example_, false); 78 | 79 | nh_local_.param("loop_rate", p_loop_rate_, 10.0); 80 | nh_local_.param("radius_margin", p_radius_margin_, 0.25); 81 | 82 | nh_local_.getParam("x_vector", p_x_vector_); 83 | nh_local_.getParam("y_vector", p_y_vector_); 84 | nh_local_.getParam("r_vector", p_r_vector_); 85 | 86 | nh_local_.getParam("vx_vector", p_vx_vector_); 87 | nh_local_.getParam("vy_vector", p_vy_vector_); 88 | 89 | nh_local_.param("frame_id", p_frame_id_, string("map")); 90 | 91 | p_sampling_time_ = 1.0 / p_loop_rate_; 92 | timer_.setPeriod(ros::Duration(p_sampling_time_), false); 93 | 94 | if (p_active_ != prev_active) { 95 | if (p_active_) { 96 | obstacle_pub_ = nh_.advertise("obstacles", 10); 97 | timer_.start(); 98 | } 99 | else { 100 | obstacle_pub_.shutdown(); 101 | timer_.stop(); 102 | } 103 | } 104 | 105 | obstacles_.header.frame_id = p_frame_id_; 106 | obstacles_.circles.clear(); 107 | 108 | if (p_x_vector_.size() != p_y_vector_.size() || p_x_vector_.size() != p_r_vector_.size() || 109 | p_x_vector_.size() != p_vx_vector_.size() || p_x_vector_.size() != p_vy_vector_.size()) 110 | return false; 111 | 112 | for (int idx = 0; idx < p_x_vector_.size(); ++idx) { 113 | CircleObstacle circle; 114 | circle.center.x = p_x_vector_[idx]; 115 | circle.center.y = p_y_vector_[idx]; 116 | circle.radius = p_r_vector_[idx]; 117 | circle.true_radius = p_r_vector_[idx] - p_radius_margin_;; 118 | 119 | circle.velocity.x = p_vx_vector_[idx]; 120 | circle.velocity.y = p_vy_vector_[idx]; 121 | 122 | obstacles_.circles.push_back(circle); 123 | } 124 | 125 | if (p_reset_) 126 | reset(); 127 | 128 | return true; 129 | } 130 | 131 | void ObstaclePublisher::timerCallback(const ros::TimerEvent& e) { 132 | t_ += p_sampling_time_; 133 | 134 | calculateObstaclesPositions(p_sampling_time_); 135 | 136 | if (p_fusion_example_) 137 | fusionExample(t_); 138 | else if (p_fission_example_) 139 | fissionExample(t_); 140 | 141 | if (obstacles_.circles.size() > 0) 142 | publishObstacles(); 143 | } 144 | 145 | void ObstaclePublisher::calculateObstaclesPositions(double dt) { 146 | for (auto& circ : obstacles_.circles) { 147 | circ.center.x += circ.velocity.x * dt; 148 | circ.center.y += circ.velocity.y * dt; 149 | } 150 | } 151 | 152 | void ObstaclePublisher::fusionExample(double t) { 153 | CircleObstacle circ1, circ2; 154 | 155 | obstacles_.circles.clear(); 156 | 157 | if (t < 5.0) { 158 | circ1.center.x = -1.20 + 0.2 * t; 159 | circ1.center.y = 0.0; 160 | circ1.radius = 0.20; 161 | 162 | circ2.center.x = 1.20 - 0.2 * t; 163 | circ2.center.y = 0.0; 164 | circ2.radius = 0.20; 165 | 166 | obstacles_.circles.push_back(circ1); 167 | obstacles_.circles.push_back(circ2); 168 | } 169 | else if (t < 15.0) { 170 | circ1.center.x = 0.0; 171 | circ1.center.y = 0.0; 172 | circ1.radius = 0.20 + 0.20 * exp(-(t - 5.0) / 1.0); 173 | 174 | obstacles_.circles.push_back(circ1); 175 | } 176 | else if (t > 20.0) 177 | reset(); 178 | 179 | circ1.true_radius = circ1.radius; 180 | circ2.true_radius = circ2.radius; 181 | } 182 | 183 | void ObstaclePublisher::fissionExample(double t) { 184 | CircleObstacle circ1, circ2; 185 | 186 | obstacles_.circles.clear(); 187 | 188 | if (t < 5.0) { 189 | circ1.center.x = 0.0; 190 | circ1.center.y = 0.0; 191 | circ1.radius = 0.20; 192 | 193 | obstacles_.circles.push_back(circ1); 194 | } 195 | else if (t < 6.0) { 196 | circ1.center.x = 0.0; 197 | circ1.center.y = 0.0; 198 | circ1.radius = 0.20 + 0.20 * (1.0 - exp(-(t - 5.0) / 1.0)); 199 | 200 | obstacles_.circles.push_back(circ1); 201 | } 202 | else if (t < 16.0){ 203 | circ1.center.x = -0.20 - 0.2 * (t - 6.0); 204 | circ1.center.y = 0.0; 205 | circ1.radius = 0.20; 206 | 207 | circ2.center.x = 0.20 + 0.2 * (t - 6.0); 208 | circ2.center.y = 0.0; 209 | circ2.radius = 0.20; 210 | 211 | obstacles_.circles.push_back(circ1); 212 | obstacles_.circles.push_back(circ2); 213 | } 214 | else if (t > 20.0) 215 | reset(); 216 | 217 | circ1.true_radius = circ1.radius; 218 | circ2.true_radius = circ2.radius; 219 | } 220 | 221 | void ObstaclePublisher::publishObstacles() { 222 | obstacle_detector::ObstaclesPtr obstacles_msg(new obstacle_detector::Obstacles); 223 | *obstacles_msg = obstacles_; 224 | 225 | obstacles_msg->header.stamp = ros::Time::now(); 226 | obstacle_pub_.publish(obstacles_msg); 227 | } 228 | 229 | void ObstaclePublisher::reset() { 230 | t_ = 0.0; 231 | p_reset_ = false; 232 | nh_local_.setParam("reset", false); 233 | } 234 | -------------------------------------------------------------------------------- /src/panels/obstacle_publisher_panel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include "obstacle_detector/panels/obstacle_publisher_panel.h" 37 | 38 | using namespace obstacle_detector; 39 | using namespace std; 40 | 41 | ObstaclePublisherPanel::ObstaclePublisherPanel(QWidget* parent) : rviz::Panel(parent), nh_(""), nh_local_("obstacle_publisher") { 42 | params_cli_ = nh_local_.serviceClient("params"); 43 | getParams(); 44 | 45 | activate_checkbox_ = new QCheckBox("On/Off"); 46 | fusion_example_checkbox_ = new QCheckBox("Fusion"); 47 | fission_example_checkbox_ = new QCheckBox("Fission"); 48 | 49 | obstacles_list_ = new QListWidget(); 50 | add_button_ = new QPushButton("Add obstacle"); 51 | remove_button_ = new QPushButton("Remove selected"); 52 | reset_button_ = new QPushButton("Reset time"); 53 | 54 | x_input_ = new QLineEdit(); 55 | y_input_ = new QLineEdit(); 56 | r_input_ = new QLineEdit(); 57 | vx_input_ = new QLineEdit(); 58 | vy_input_ = new QLineEdit(); 59 | frame_input_ = new QLineEdit(); 60 | 61 | obstacles_list_->setSelectionMode(QAbstractItemView::MultiSelection); 62 | frame_input_->setAlignment(Qt::AlignRight); 63 | 64 | QFrame* lines[5]; 65 | for (auto& line : lines) { 66 | line = new QFrame(); 67 | line->setFrameShape(QFrame::HLine); 68 | line->setFrameShadow(QFrame::Sunken); 69 | } 70 | 71 | QSpacerItem* margin = new QSpacerItem(1, 1, QSizePolicy::Expanding, QSizePolicy::Fixed); 72 | 73 | QHBoxLayout* demos_layout = new QHBoxLayout; 74 | demos_layout->addWidget(fusion_example_checkbox_); 75 | demos_layout->addWidget(fission_example_checkbox_); 76 | 77 | QGroupBox* demos_box = new QGroupBox("Demos:"); 78 | demos_box->setLayout(demos_layout); 79 | 80 | QHBoxLayout* target_frame_layout = new QHBoxLayout; 81 | target_frame_layout->addItem(margin); 82 | target_frame_layout->addWidget(new QLabel("Target frame:")); 83 | target_frame_layout->addWidget(frame_input_, 0, Qt::AlignLeft); 84 | target_frame_layout->addItem(margin); 85 | 86 | QGroupBox* frames_box = new QGroupBox("Frames:"); 87 | frames_box->setLayout(target_frame_layout); 88 | 89 | QHBoxLayout* xyr_layout = new QHBoxLayout; 90 | xyr_layout->addItem(margin); 91 | xyr_layout->addWidget(new QLabel("x:")); 92 | xyr_layout->addWidget(x_input_); 93 | xyr_layout->addWidget(new QLabel("m, ")); 94 | xyr_layout->addWidget(new QLabel("y:")); 95 | xyr_layout->addWidget(y_input_); 96 | xyr_layout->addWidget(new QLabel("m, ")); 97 | xyr_layout->addWidget(new QLabel("r:")); 98 | xyr_layout->addWidget(r_input_); 99 | xyr_layout->addWidget(new QLabel("m")); 100 | xyr_layout->addItem(margin); 101 | 102 | QHBoxLayout* vxvy_layout = new QHBoxLayout; 103 | vxvy_layout->addItem(margin); 104 | vxvy_layout->addWidget(new QLabel("vx:")); 105 | vxvy_layout->addWidget(vx_input_); 106 | vxvy_layout->addWidget(new QLabel("m/s, ")); 107 | vxvy_layout->addWidget(new QLabel("vy:")); 108 | vxvy_layout->addWidget(vy_input_); 109 | vxvy_layout->addWidget(new QLabel("m/s")); 110 | vxvy_layout->addItem(margin); 111 | 112 | QVBoxLayout* obst_params_layout = new QVBoxLayout; 113 | obst_params_layout->addWidget(obstacles_list_); 114 | obst_params_layout->addWidget(remove_button_); 115 | obst_params_layout->addWidget(lines[2]); 116 | obst_params_layout->addLayout(xyr_layout); 117 | obst_params_layout->addLayout(vxvy_layout); 118 | obst_params_layout->addWidget(add_button_, Qt::AlignCenter); 119 | obst_params_layout->addWidget(lines[3]); 120 | obst_params_layout->addWidget(reset_button_, Qt::AlignCenter); 121 | 122 | QGroupBox* obst_box = new QGroupBox("Obstacles:"); 123 | obst_box->setLayout(obst_params_layout); 124 | 125 | QVBoxLayout* layout = new QVBoxLayout; 126 | layout->addWidget(activate_checkbox_); 127 | layout->addWidget(lines[0]); 128 | layout->addWidget(demos_box); 129 | layout->addWidget(lines[1]); 130 | layout->addWidget(frames_box); 131 | layout->addWidget(lines[4]); 132 | layout->addWidget(obst_box); 133 | layout->setAlignment(layout, Qt::AlignCenter); 134 | setLayout(layout); 135 | 136 | connect(activate_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); 137 | connect(fusion_example_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); 138 | connect(fission_example_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); 139 | connect(frame_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 140 | 141 | connect(add_button_, SIGNAL(clicked()), this, SLOT(addObstacle())); 142 | connect(remove_button_, SIGNAL(clicked()), this, SLOT(removeObstacles())); 143 | connect(reset_button_, SIGNAL(clicked()), this, SLOT(reset())); 144 | 145 | evaluateParams(); 146 | } 147 | 148 | void ObstaclePublisherPanel::processInputs() { 149 | verifyInputs(); 150 | setParams(); 151 | evaluateParams(); 152 | notifyParamsUpdate(); 153 | } 154 | 155 | void ObstaclePublisherPanel::addObstacle() { 156 | verifyInputs(); 157 | 158 | if (r_ > 0.0) { 159 | p_x_vector_.push_back(x_); 160 | p_y_vector_.push_back(y_); 161 | p_r_vector_.push_back(r_); 162 | 163 | p_vx_vector_.push_back(vx_); 164 | p_vy_vector_.push_back(vy_); 165 | 166 | setParams(); 167 | evaluateParams(); 168 | notifyParamsUpdate(); 169 | } 170 | } 171 | 172 | void ObstaclePublisherPanel::removeObstacles() { 173 | QModelIndexList indexes = obstacles_list_->selectionModel()->selectedIndexes(); 174 | 175 | vector index_list; 176 | for (QModelIndex index : indexes) 177 | index_list.push_back(index.row()); 178 | 179 | sort(index_list.begin(), index_list.end(), greater()); 180 | 181 | for (int idx : index_list) { 182 | p_x_vector_.erase(p_x_vector_.begin() + idx); 183 | p_y_vector_.erase(p_y_vector_.begin() + idx); 184 | p_r_vector_.erase(p_r_vector_.begin() + idx); 185 | 186 | p_vx_vector_.erase(p_vx_vector_.begin() + idx); 187 | p_vy_vector_.erase(p_vy_vector_.begin() + idx); 188 | 189 | delete obstacles_list_items_[idx]; 190 | obstacles_list_items_.erase(obstacles_list_items_.begin() + idx); 191 | } 192 | 193 | setParams(); 194 | evaluateParams(); 195 | notifyParamsUpdate(); 196 | } 197 | 198 | void ObstaclePublisherPanel::reset() { 199 | p_reset_ = true; 200 | 201 | processInputs(); 202 | 203 | p_reset_ = false; 204 | } 205 | 206 | void ObstaclePublisherPanel::verifyInputs() { 207 | p_active_ = activate_checkbox_->isChecked(); 208 | p_fusion_example_ = fusion_example_checkbox_->isChecked(); 209 | p_fission_example_ = fission_example_checkbox_->isChecked(); 210 | 211 | try { x_ = boost::lexical_cast(x_input_->text().toStdString()); } 212 | catch(boost::bad_lexical_cast &) { x_ = 0.0; x_input_->setText("0.0"); } 213 | 214 | try { y_ = boost::lexical_cast(y_input_->text().toStdString()); } 215 | catch(boost::bad_lexical_cast &) { y_ = 0.0; y_input_->setText("0.0"); } 216 | 217 | try { r_ = boost::lexical_cast(r_input_->text().toStdString()); } 218 | catch(boost::bad_lexical_cast &) { r_ = 0.0; r_input_->setText("0.0"); } 219 | 220 | try { vx_ = boost::lexical_cast(vx_input_->text().toStdString()); } 221 | catch(boost::bad_lexical_cast &) { vx_ = 0.0; vx_input_->setText("0.0"); } 222 | 223 | try { vy_ = boost::lexical_cast(vy_input_->text().toStdString()); } 224 | catch(boost::bad_lexical_cast &) { vy_ = 0.0; vy_input_->setText("0.0"); } 225 | 226 | p_frame_id_ = frame_input_->text().toStdString(); 227 | } 228 | 229 | void ObstaclePublisherPanel::setParams() { 230 | nh_local_.setParam("active", p_active_); 231 | nh_local_.setParam("reset", p_reset_); 232 | 233 | nh_local_.setParam("fusion_example", p_fusion_example_); 234 | nh_local_.setParam("fission_example", p_fission_example_); 235 | 236 | nh_local_.setParam("x_vector", p_x_vector_); 237 | nh_local_.setParam("y_vector", p_y_vector_); 238 | nh_local_.setParam("r_vector", p_r_vector_); 239 | 240 | nh_local_.setParam("vx_vector", p_vx_vector_); 241 | nh_local_.setParam("vy_vector", p_vy_vector_); 242 | 243 | nh_local_.setParam("frame_id", p_frame_id_); 244 | } 245 | 246 | void ObstaclePublisherPanel::getParams() { 247 | p_active_ = nh_local_.param("active", false); 248 | p_reset_ = nh_local_.param("reset", false); 249 | 250 | p_fusion_example_ = nh_local_.param("fusion_example", false); 251 | p_fission_example_ = nh_local_.param("fission_example", false); 252 | 253 | nh_local_.getParam("x_vector", p_x_vector_); 254 | nh_local_.getParam("y_vector", p_y_vector_); 255 | nh_local_.getParam("r_vector", p_r_vector_); 256 | 257 | nh_local_.getParam("vx_vector", p_vx_vector_); 258 | nh_local_.getParam("vy_vector", p_vy_vector_); 259 | 260 | p_frame_id_ = nh_local_.param("frame_id", std::string("")); 261 | } 262 | 263 | void ObstaclePublisherPanel::evaluateParams() { 264 | activate_checkbox_->setChecked(p_active_); 265 | fusion_example_checkbox_->setEnabled(p_active_); 266 | fission_example_checkbox_->setEnabled(p_active_); 267 | 268 | fusion_example_checkbox_->setChecked(p_fusion_example_); 269 | fission_example_checkbox_->setChecked(p_fission_example_); 270 | 271 | frame_input_->setEnabled(p_active_); 272 | frame_input_->setText(QString::fromStdString(p_frame_id_)); 273 | 274 | add_button_->setEnabled(p_active_); 275 | remove_button_->setEnabled(p_active_); 276 | reset_button_->setEnabled(p_active_); 277 | 278 | x_input_->setEnabled(p_active_); 279 | y_input_->setEnabled(p_active_); 280 | r_input_->setEnabled(p_active_); 281 | 282 | vx_input_->setEnabled(p_active_); 283 | vy_input_->setEnabled(p_active_); 284 | 285 | obstacles_list_->setEnabled(p_active_); 286 | 287 | if (p_x_vector_.size() < p_y_vector_.size() || p_x_vector_.size() < p_r_vector_.size() || 288 | p_x_vector_.size() < p_vx_vector_.size() || p_x_vector_.size() < p_vy_vector_.size()) 289 | return; 290 | 291 | for (QListWidgetItem* item : obstacles_list_items_) 292 | delete item; 293 | 294 | obstacles_list_items_.clear(); 295 | obstacles_list_->clear(); 296 | 297 | for (int idx = 0; idx < p_x_vector_.size(); ++idx) { 298 | QListWidgetItem* item = new QListWidgetItem; 299 | item->setText(QString::number(idx + 1) + ". " + 300 | "[x: " + QString::number(p_x_vector_[idx]) + "m] " + 301 | "[y: " + QString::number(p_y_vector_[idx]) + "m] " + 302 | "[r: " + QString::number(p_r_vector_[idx]) + "m] " + 303 | "[vx: " + QString::number(p_vx_vector_[idx]) + "m/s] " + 304 | "[vy: " + QString::number(p_vy_vector_[idx]) + "m/s]"); 305 | obstacles_list_items_.push_back(item); 306 | obstacles_list_->insertItem(idx, item); 307 | } 308 | } 309 | 310 | void ObstaclePublisherPanel::notifyParamsUpdate() { 311 | std_srvs::Empty empty; 312 | if (!params_cli_.call(empty)) { 313 | p_active_ = false; 314 | setParams(); 315 | evaluateParams(); 316 | } 317 | } 318 | 319 | void ObstaclePublisherPanel::save(rviz::Config config) const { 320 | rviz::Panel::save(config); 321 | } 322 | 323 | void ObstaclePublisherPanel::load(const rviz::Config& config) { 324 | rviz::Panel::load(config); 325 | } 326 | 327 | #include 328 | PLUGINLIB_EXPORT_CLASS(obstacle_detector::ObstaclePublisherPanel, rviz::Panel) 329 | -------------------------------------------------------------------------------- /src/panels/obstacle_tracker_panel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include "obstacle_detector/panels/obstacle_tracker_panel.h" 37 | 38 | using namespace obstacle_detector; 39 | using namespace std; 40 | 41 | ObstacleTrackerPanel::ObstacleTrackerPanel(QWidget* parent) : rviz::Panel(parent), nh_(""), nh_local_("obstacle_tracker") { 42 | params_cli_ = nh_local_.serviceClient("params"); 43 | getParams(); 44 | 45 | activate_checkbox_ = new QCheckBox("On/Off"); 46 | 47 | tracking_duration_input_ = new QLineEdit(); 48 | loop_rate_input_ = new QLineEdit(); 49 | min_corr_cost_input_ = new QLineEdit(); 50 | std_corr_dev_input_ = new QLineEdit(); 51 | process_var_input_ = new QLineEdit(); 52 | process_rate_var_input_ = new QLineEdit(); 53 | measure_var_input_ = new QLineEdit(); 54 | 55 | tracking_duration_input_->setAlignment(Qt::AlignRight); 56 | loop_rate_input_->setAlignment(Qt::AlignRight); 57 | min_corr_cost_input_->setAlignment(Qt::AlignRight); 58 | std_corr_dev_input_->setAlignment(Qt::AlignRight); 59 | process_var_input_->setAlignment(Qt::AlignRight); 60 | process_rate_var_input_->setAlignment(Qt::AlignRight); 61 | measure_var_input_->setAlignment(Qt::AlignRight); 62 | 63 | QSpacerItem* margin = new QSpacerItem(1, 1, QSizePolicy::Expanding, QSizePolicy::Fixed); 64 | 65 | QFrame* lines[4]; 66 | for (auto& line : lines) { 67 | line = new QFrame(); 68 | line->setFrameShape(QFrame::HLine); 69 | line->setFrameShadow(QFrame::Sunken); 70 | } 71 | 72 | QGroupBox* tracking_box = new QGroupBox("General:"); 73 | QGridLayout* tracking_layout = new QGridLayout; 74 | tracking_layout->addWidget(new QLabel("Tracking rate:"), 0, 0, Qt::AlignRight); 75 | tracking_layout->addWidget(loop_rate_input_, 0, 1, Qt::AlignLeft); 76 | tracking_layout->addWidget(new QLabel("Hz"), 0, 2, Qt::AlignLeft); 77 | tracking_layout->addWidget(new QLabel("Tracking duration:"), 1, 0, Qt::AlignRight); 78 | tracking_layout->addWidget(tracking_duration_input_, 1, 1, Qt::AlignLeft); 79 | tracking_layout->addWidget(new QLabel("s"), 1, 2, Qt::AlignLeft); 80 | tracking_box->setLayout(tracking_layout); 81 | 82 | QGroupBox* corr_box = new QGroupBox("Correspondence:"); 83 | QGridLayout* corr_layout = new QGridLayout; 84 | corr_layout->addWidget(new QLabel("Minimal cost:"), 0, 0, Qt::AlignRight); 85 | corr_layout->addWidget(min_corr_cost_input_, 0, 1, Qt::AlignLeft); 86 | corr_layout->addWidget(new QLabel("m"), 0, 2, Qt::AlignLeft); 87 | corr_layout->addWidget(new QLabel("Standard deviation:"), 1, 0, Qt::AlignRight); 88 | corr_layout->addWidget(std_corr_dev_input_, 1, 1, Qt::AlignLeft); 89 | corr_layout->addWidget(new QLabel("m"), 1, 2, Qt::AlignLeft); 90 | corr_box->setLayout(corr_layout); 91 | 92 | QGroupBox* kf_box = new QGroupBox("Kalman Filter:"); 93 | QGridLayout* kf_layout = new QGridLayout; 94 | kf_layout->addWidget(new QLabel("Process variance:"), 0, 0, Qt::AlignRight); 95 | kf_layout->addWidget(process_var_input_, 0, 1, Qt::AlignLeft); 96 | kf_layout->addWidget(new QLabel("m2"), 0, 2, Qt::AlignLeft); 97 | kf_layout->addWidget(new QLabel("Process rate variance:"), 1, 0, Qt::AlignRight); 98 | kf_layout->addWidget(process_rate_var_input_, 1, 1, Qt::AlignLeft); 99 | kf_layout->addWidget(new QLabel("(m/s)2"), 1, 2, Qt::AlignLeft); 100 | kf_layout->addWidget(new QLabel("Measurement variance:"), 2, 0, Qt::AlignRight); 101 | kf_layout->addWidget(measure_var_input_, 2, 1, Qt::AlignLeft); 102 | kf_layout->addWidget(new QLabel("m2"), 2, 2, Qt::AlignLeft); 103 | kf_box->setLayout(kf_layout); 104 | 105 | QVBoxLayout* layout = new QVBoxLayout; 106 | layout->addWidget(activate_checkbox_); 107 | layout->addWidget(lines[0]); 108 | layout->addWidget(tracking_box); 109 | layout->addWidget(lines[1]); 110 | layout->addWidget(corr_box); 111 | layout->addWidget(lines[2]); 112 | layout->addWidget(kf_box); 113 | layout->setAlignment(layout, Qt::AlignCenter); 114 | setLayout(layout); 115 | 116 | connect(activate_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); 117 | 118 | connect(tracking_duration_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 119 | connect(loop_rate_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 120 | connect(min_corr_cost_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 121 | connect(std_corr_dev_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 122 | connect(process_var_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 123 | connect(process_rate_var_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 124 | connect(measure_var_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 125 | 126 | evaluateParams(); 127 | } 128 | 129 | void ObstacleTrackerPanel::processInputs() { 130 | verifyInputs(); 131 | setParams(); 132 | evaluateParams(); 133 | notifyParamsUpdate(); 134 | } 135 | 136 | void ObstacleTrackerPanel::verifyInputs() { 137 | p_active_ = activate_checkbox_->isChecked(); 138 | 139 | try { p_loop_rate_ = boost::lexical_cast(loop_rate_input_->text().toStdString()); } 140 | catch(boost::bad_lexical_cast &) { p_loop_rate_ = 1.0; loop_rate_input_->setText("1.0"); } 141 | 142 | try { p_tracking_duration_ = boost::lexical_cast(tracking_duration_input_->text().toStdString()); } 143 | catch(boost::bad_lexical_cast &) { p_tracking_duration_ = 0.0; tracking_duration_input_->setText("0.0"); } 144 | 145 | try { p_min_correspondence_cost_ = boost::lexical_cast(min_corr_cost_input_->text().toStdString()); } 146 | catch(boost::bad_lexical_cast &) { p_min_correspondence_cost_ = 0.0; min_corr_cost_input_->setText("0.0"); } 147 | 148 | try { p_std_correspondence_dev_ = boost::lexical_cast(std_corr_dev_input_->text().toStdString()); } 149 | catch(boost::bad_lexical_cast &) { p_std_correspondence_dev_ = 0.0; std_corr_dev_input_->setText("0.0"); } 150 | 151 | try { p_process_variance_ = boost::lexical_cast(process_var_input_->text().toStdString()); } 152 | catch(boost::bad_lexical_cast &) { p_process_variance_ = 0.0; process_var_input_->setText("0.0"); } 153 | 154 | try { p_process_rate_variance_ = boost::lexical_cast(process_rate_var_input_->text().toStdString()); } 155 | catch(boost::bad_lexical_cast &) { p_process_rate_variance_ = 0.0; process_rate_var_input_->setText("0.0"); } 156 | 157 | try { p_measurement_variance_ = boost::lexical_cast(measure_var_input_->text().toStdString()); } 158 | catch(boost::bad_lexical_cast &) { p_measurement_variance_ = 0.0; measure_var_input_->setText("0.0"); } 159 | } 160 | 161 | void ObstacleTrackerPanel::setParams() { 162 | nh_local_.setParam("active", p_active_); 163 | 164 | nh_local_.setParam("loop_rate", p_loop_rate_); 165 | nh_local_.setParam("tracking_duration", p_tracking_duration_); 166 | nh_local_.setParam("min_correspondence_cost", p_min_correspondence_cost_); 167 | nh_local_.setParam("std_correspondence_dev", p_std_correspondence_dev_); 168 | nh_local_.setParam("process_variance", p_process_variance_); 169 | nh_local_.setParam("process_rate_variance", p_process_rate_variance_); 170 | nh_local_.setParam("measurement_variance", p_measurement_variance_); 171 | } 172 | 173 | void ObstacleTrackerPanel::getParams() { 174 | p_active_ = nh_local_.param("active", false); 175 | 176 | p_loop_rate_ = nh_local_.param("loop_rate", 0.0); 177 | p_tracking_duration_ = nh_local_.param("tracking_duration", 0.0); 178 | p_min_correspondence_cost_ = nh_local_.param("min_correspondence_cost", 0.0); 179 | p_std_correspondence_dev_ = nh_local_.param("std_correspondence_dev", 0.0); 180 | p_process_variance_ = nh_local_.param("process_variance", 0.0); 181 | p_process_rate_variance_ = nh_local_.param("process_rate_variance", 0.0); 182 | p_measurement_variance_ = nh_local_.param("measurement_variance", 0.0); 183 | } 184 | 185 | void ObstacleTrackerPanel::evaluateParams() { 186 | activate_checkbox_->setChecked(p_active_); 187 | 188 | loop_rate_input_->setEnabled(p_active_); 189 | loop_rate_input_->setText(QString::number(p_loop_rate_)); 190 | 191 | tracking_duration_input_->setEnabled(p_active_); 192 | tracking_duration_input_->setText(QString::number(p_tracking_duration_)); 193 | 194 | min_corr_cost_input_->setEnabled(p_active_); 195 | min_corr_cost_input_->setText(QString::number(p_min_correspondence_cost_)); 196 | 197 | std_corr_dev_input_->setEnabled(p_active_); 198 | std_corr_dev_input_->setText(QString::number(p_std_correspondence_dev_)); 199 | 200 | process_var_input_->setEnabled(p_active_); 201 | process_var_input_->setText(QString::number(p_process_variance_)); 202 | 203 | process_rate_var_input_->setEnabled(p_active_); 204 | process_rate_var_input_->setText(QString::number(p_process_rate_variance_)); 205 | 206 | measure_var_input_->setEnabled(p_active_); 207 | measure_var_input_->setText(QString::number(p_measurement_variance_)); 208 | } 209 | 210 | void ObstacleTrackerPanel::notifyParamsUpdate() { 211 | std_srvs::Empty empty; 212 | if (!params_cli_.call(empty)) { 213 | p_active_ = false; 214 | setParams(); 215 | evaluateParams(); 216 | } 217 | } 218 | 219 | void ObstacleTrackerPanel::save(rviz::Config config) const { 220 | rviz::Panel::save(config); 221 | } 222 | 223 | void ObstacleTrackerPanel::load(const rviz::Config& config) { 224 | rviz::Panel::load(config); 225 | } 226 | 227 | #include 228 | PLUGINLIB_EXPORT_CLASS(obstacle_detector::ObstacleTrackerPanel, rviz::Panel) 229 | -------------------------------------------------------------------------------- /src/panels/scans_merger_panel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include "obstacle_detector/panels/scans_merger_panel.h" 37 | 38 | using namespace obstacle_detector; 39 | using namespace std; 40 | 41 | ScansMergerPanel::ScansMergerPanel(QWidget* parent) : rviz::Panel(parent), nh_(""), nh_local_("scans_merger") { 42 | params_cli_ = nh_local_.serviceClient("params"); 43 | getParams(); 44 | 45 | activate_checkbox_ = new QCheckBox("On/Off"); 46 | scan_checkbox_ = new QCheckBox("Publish scan"); 47 | pcl_checkbox_ = new QCheckBox("Publish PCL"); 48 | 49 | n_input_ = new QLineEdit(); 50 | r_min_input_ = new QLineEdit(); 51 | r_max_input_ = new QLineEdit(); 52 | x_min_input_ = new QLineEdit(); 53 | x_max_input_ = new QLineEdit(); 54 | y_min_input_ = new QLineEdit(); 55 | y_max_input_ = new QLineEdit(); 56 | fixed_frame_id_input_ = new QLineEdit(); 57 | target_frame_id_input_ = new QLineEdit(); 58 | 59 | n_input_->setAlignment(Qt::AlignRight); 60 | r_min_input_->setAlignment(Qt::AlignRight); 61 | r_max_input_->setAlignment(Qt::AlignRight); 62 | x_min_input_->setAlignment(Qt::AlignRight); 63 | x_max_input_->setAlignment(Qt::AlignRight); 64 | y_min_input_->setAlignment(Qt::AlignRight); 65 | y_max_input_->setAlignment(Qt::AlignRight); 66 | fixed_frame_id_input_->setAlignment(Qt::AlignRight); 67 | target_frame_id_input_->setAlignment(Qt::AlignRight); 68 | 69 | QFrame* lines[5]; 70 | for (auto& line : lines) { 71 | line = new QFrame(); 72 | line->setFrameShape(QFrame::HLine); 73 | line->setFrameShadow(QFrame::Sunken); 74 | } 75 | 76 | QSpacerItem* margin = new QSpacerItem(1, 1, QSizePolicy::Expanding, QSizePolicy::Fixed); 77 | 78 | QHBoxLayout* scan_pcl_layout = new QHBoxLayout; 79 | scan_pcl_layout->addItem(margin); 80 | scan_pcl_layout->addWidget(scan_checkbox_); 81 | scan_pcl_layout->addItem(margin); 82 | scan_pcl_layout->addWidget(pcl_checkbox_); 83 | scan_pcl_layout->addItem(margin); 84 | 85 | QHBoxLayout* beams_num_layout = new QHBoxLayout; 86 | beams_num_layout->addItem(margin); 87 | beams_num_layout->addWidget(new QLabel("Number of beams:")); 88 | beams_num_layout->addWidget(n_input_); 89 | beams_num_layout->addItem(margin); 90 | 91 | QHBoxLayout* ranges_lim_layout = new QHBoxLayout; 92 | ranges_lim_layout->addItem(margin); 93 | ranges_lim_layout->addWidget(new QLabel("rmin:"), 0, Qt::AlignRight); 94 | ranges_lim_layout->addWidget(r_min_input_); 95 | ranges_lim_layout->addWidget(new QLabel("m "), 0, Qt::AlignLeft); 96 | ranges_lim_layout->addWidget(new QLabel("rmax:"), 0, Qt::AlignRight); 97 | ranges_lim_layout->addWidget(r_max_input_); 98 | ranges_lim_layout->addWidget(new QLabel("m"), 0, Qt::AlignLeft); 99 | ranges_lim_layout->addItem(margin); 100 | 101 | QVBoxLayout* beams_layout = new QVBoxLayout; 102 | beams_layout->addLayout(beams_num_layout); 103 | beams_layout->addLayout(ranges_lim_layout); 104 | 105 | QGroupBox* scan_box = new QGroupBox("Beams limits:"); 106 | scan_box->setLayout(beams_layout); 107 | 108 | QHBoxLayout* x_lim_layout = new QHBoxLayout; 109 | x_lim_layout->addItem(margin); 110 | x_lim_layout->addWidget(new QLabel("xmin:"), 0, Qt::AlignRight); 111 | x_lim_layout->addWidget(x_min_input_); 112 | x_lim_layout->addWidget(new QLabel("m "), 0, Qt::AlignLeft); 113 | x_lim_layout->addWidget(new QLabel("xmax:"), 0, Qt::AlignRight); 114 | x_lim_layout->addWidget(x_max_input_); 115 | x_lim_layout->addWidget(new QLabel("m"), 0, Qt::AlignLeft); 116 | x_lim_layout->addItem(margin); 117 | 118 | QHBoxLayout* y_lim_layout = new QHBoxLayout; 119 | y_lim_layout->addItem(margin); 120 | y_lim_layout->addWidget(new QLabel("ymin:"), 0, Qt::AlignRight); 121 | y_lim_layout->addWidget(y_min_input_); 122 | y_lim_layout->addWidget(new QLabel("m "), 0, Qt::AlignLeft); 123 | y_lim_layout->addWidget(new QLabel("ymax:"), 0, Qt::AlignRight); 124 | y_lim_layout->addWidget(y_max_input_); 125 | y_lim_layout->addWidget(new QLabel("m"), 0, Qt::AlignLeft); 126 | y_lim_layout->addItem(margin); 127 | 128 | QVBoxLayout* xy_lim_layout = new QVBoxLayout; 129 | xy_lim_layout->addLayout(x_lim_layout); 130 | xy_lim_layout->addLayout(y_lim_layout); 131 | 132 | QGroupBox* xy_lim_box = new QGroupBox("Coordinates limits:"); 133 | xy_lim_box->setLayout(xy_lim_layout); 134 | 135 | QHBoxLayout* fixed_frame_layout = new QHBoxLayout; 136 | fixed_frame_layout->addItem(margin); 137 | fixed_frame_layout->addWidget(new QLabel("Fixed frame:")); 138 | fixed_frame_layout->addWidget(fixed_frame_id_input_, 0, Qt::AlignLeft); 139 | fixed_frame_layout->addItem(margin); 140 | 141 | QHBoxLayout* target_frame_layout = new QHBoxLayout; 142 | target_frame_layout->addItem(margin); 143 | target_frame_layout->addWidget(new QLabel("Target frame:")); 144 | target_frame_layout->addWidget(target_frame_id_input_, 0, Qt::AlignLeft); 145 | target_frame_layout->addItem(margin); 146 | 147 | QVBoxLayout* frames_layout = new QVBoxLayout; 148 | frames_layout->addLayout(fixed_frame_layout); 149 | frames_layout->addLayout(target_frame_layout); 150 | 151 | QGroupBox* frames_box = new QGroupBox("Frames:"); 152 | frames_box->setLayout(frames_layout); 153 | 154 | QVBoxLayout* layout = new QVBoxLayout; 155 | layout->addWidget(activate_checkbox_); 156 | layout->addWidget(lines[0]); 157 | layout->addLayout(scan_pcl_layout); 158 | layout->addWidget(lines[1]); 159 | layout->addWidget(scan_box); 160 | layout->addWidget(lines[2]); 161 | layout->addWidget(xy_lim_box); 162 | layout->addWidget(lines[3]); 163 | layout->addWidget(frames_box); 164 | layout->setAlignment(layout, Qt::AlignCenter); 165 | setLayout(layout); 166 | 167 | connect(activate_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); 168 | connect(scan_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); 169 | connect(pcl_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); 170 | 171 | connect(n_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 172 | connect(r_min_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 173 | connect(r_max_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 174 | connect(x_min_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 175 | connect(x_max_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 176 | connect(y_min_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 177 | connect(y_max_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 178 | connect(fixed_frame_id_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 179 | connect(target_frame_id_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); 180 | 181 | evaluateParams(); 182 | } 183 | 184 | void ScansMergerPanel::processInputs() { 185 | verifyInputs(); 186 | setParams(); 187 | evaluateParams(); 188 | notifyParamsUpdate(); 189 | } 190 | 191 | void ScansMergerPanel::verifyInputs() { 192 | p_active_ = activate_checkbox_->isChecked(); 193 | p_publish_scan_ = scan_checkbox_->isChecked(); 194 | p_publish_pcl_ = pcl_checkbox_->isChecked(); 195 | 196 | try { p_ranges_num_ = boost::lexical_cast(n_input_->text().toStdString()); } 197 | catch(boost::bad_lexical_cast &) { p_ranges_num_ = 0; n_input_->setText("0"); } 198 | 199 | try { p_min_scanner_range_ = boost::lexical_cast(r_min_input_->text().toStdString()); } 200 | catch(boost::bad_lexical_cast &) { p_min_scanner_range_ = 0.0; r_min_input_->setText("0.0"); } 201 | 202 | try { p_max_scanner_range_ = boost::lexical_cast(r_max_input_->text().toStdString()); } 203 | catch(boost::bad_lexical_cast &) { p_max_scanner_range_ = 0.0; r_max_input_->setText("0.0"); } 204 | 205 | try { p_min_x_range_ = boost::lexical_cast(x_min_input_->text().toStdString()); } 206 | catch(boost::bad_lexical_cast &) { p_min_x_range_ = 0.0; x_min_input_->setText("0.0"); } 207 | 208 | try { p_max_x_range_ = boost::lexical_cast(x_max_input_->text().toStdString()); } 209 | catch(boost::bad_lexical_cast &) { p_max_x_range_ = 0.0; x_max_input_->setText("0.0"); } 210 | 211 | try { p_min_y_range_ = boost::lexical_cast(y_min_input_->text().toStdString()); } 212 | catch(boost::bad_lexical_cast &) { p_min_y_range_ = 0.0; y_min_input_->setText("0.0"); } 213 | 214 | try { p_max_y_range_ = boost::lexical_cast(y_max_input_->text().toStdString()); } 215 | catch(boost::bad_lexical_cast &) { p_max_y_range_ = 0.0; y_max_input_->setText("0.0"); } 216 | 217 | p_fixed_frame_id_ = fixed_frame_id_input_->text().toStdString(); 218 | p_target_frame_id_ = target_frame_id_input_->text().toStdString(); 219 | } 220 | 221 | void ScansMergerPanel::setParams() { 222 | nh_local_.setParam("active", p_active_); 223 | nh_local_.setParam("publish_scan", p_publish_scan_); 224 | nh_local_.setParam("publish_pcl", p_publish_pcl_); 225 | 226 | nh_local_.setParam("ranges_num", p_ranges_num_); 227 | 228 | nh_local_.setParam("min_scanner_range", p_min_scanner_range_); 229 | nh_local_.setParam("max_scanner_range", p_max_scanner_range_); 230 | 231 | nh_local_.setParam("min_x_range", p_min_x_range_); 232 | nh_local_.setParam("max_x_range", p_max_x_range_); 233 | nh_local_.setParam("min_y_range", p_min_y_range_); 234 | nh_local_.setParam("max_y_range", p_max_y_range_); 235 | 236 | nh_local_.setParam("fixed_frame_id", p_fixed_frame_id_); 237 | nh_local_.setParam("target_frame_id", p_target_frame_id_); 238 | } 239 | 240 | void ScansMergerPanel::getParams() { 241 | p_active_ = nh_local_.param("active", false); 242 | p_publish_scan_ = nh_local_.param("publish_scan", false); 243 | p_publish_pcl_ = nh_local_.param("publish_pcl", false); 244 | 245 | p_ranges_num_ = nh_local_.param("ranges_num", 0); 246 | 247 | p_min_scanner_range_ = nh_local_.param("min_scanner_range", 0.0); 248 | p_max_scanner_range_ = nh_local_.param("max_scanner_range", 0.0); 249 | 250 | p_min_x_range_ = nh_local_.param("min_x_range", 0.0); 251 | p_max_x_range_ = nh_local_.param("max_x_range", 0.0); 252 | p_min_y_range_ = nh_local_.param("min_y_range", 0.0); 253 | p_max_y_range_ = nh_local_.param("max_y_range", 0.0); 254 | 255 | p_fixed_frame_id_ = nh_local_.param("fixed_frame_id", std::string("")); 256 | p_target_frame_id_ = nh_local_.param("target_frame_id", std::string("")); 257 | } 258 | 259 | void ScansMergerPanel::evaluateParams() { 260 | activate_checkbox_->setChecked(p_active_); 261 | 262 | scan_checkbox_->setEnabled(p_active_); 263 | pcl_checkbox_->setEnabled(p_active_); 264 | 265 | scan_checkbox_->setChecked(p_publish_scan_); 266 | pcl_checkbox_->setChecked(p_publish_pcl_); 267 | 268 | n_input_->setEnabled(p_active_); 269 | r_min_input_->setEnabled(p_active_); 270 | r_max_input_->setEnabled(p_active_); 271 | x_min_input_->setEnabled(p_active_); 272 | x_max_input_->setEnabled(p_active_); 273 | y_min_input_->setEnabled(p_active_); 274 | y_max_input_->setEnabled(p_active_); 275 | fixed_frame_id_input_->setEnabled(p_active_); 276 | target_frame_id_input_->setEnabled(p_active_); 277 | 278 | n_input_->setText(QString::number(p_ranges_num_)); 279 | r_min_input_->setText(QString::number(p_min_scanner_range_)); 280 | r_max_input_->setText(QString::number(p_max_scanner_range_)); 281 | x_min_input_->setText(QString::number(p_min_x_range_)); 282 | x_max_input_->setText(QString::number(p_max_x_range_)); 283 | y_min_input_->setText(QString::number(p_min_y_range_)); 284 | y_max_input_->setText(QString::number(p_max_y_range_)); 285 | 286 | fixed_frame_id_input_->setText(QString::fromStdString(p_fixed_frame_id_)); 287 | target_frame_id_input_->setText(QString::fromStdString(p_target_frame_id_)); 288 | } 289 | 290 | void ScansMergerPanel::notifyParamsUpdate() { 291 | std_srvs::Empty empty; 292 | if (!params_cli_.call(empty)) { 293 | p_active_ = false; 294 | setParams(); 295 | evaluateParams(); 296 | } 297 | } 298 | 299 | void ScansMergerPanel::save(rviz::Config config) const { 300 | rviz::Panel::save(config); 301 | } 302 | 303 | void ScansMergerPanel::load(const rviz::Config& config) { 304 | rviz::Panel::load(config); 305 | } 306 | 307 | #include 308 | PLUGINLIB_EXPORT_CLASS(obstacle_detector::ScansMergerPanel, rviz::Panel) 309 | -------------------------------------------------------------------------------- /src/scans_merger.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Poznan University of Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Poznan University of Technology nor the names 16 | * of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /* 33 | * Author: Mateusz Przybyla 34 | */ 35 | 36 | #include "obstacle_detector/scans_merger.h" 37 | 38 | using namespace obstacle_detector; 39 | using namespace std; 40 | 41 | ScansMerger::ScansMerger(ros::NodeHandle& nh, ros::NodeHandle& nh_local) : nh_(nh), nh_local_(nh_local) { 42 | p_active_ = false; 43 | 44 | front_scan_received_ = false; 45 | rear_scan_received_ = false; 46 | 47 | front_scan_error_ = false; 48 | rear_scan_error_ = false; 49 | 50 | params_srv_ = nh_local_.advertiseService("params", &ScansMerger::updateParams, this); 51 | 52 | initialize(); 53 | } 54 | 55 | ScansMerger::~ScansMerger() { 56 | nh_local_.deleteParam("active"); 57 | nh_local_.deleteParam("publish_scan"); 58 | nh_local_.deleteParam("publish_pcl"); 59 | 60 | nh_local_.deleteParam("ranges_num"); 61 | 62 | nh_local_.deleteParam("min_scanner_range"); 63 | nh_local_.deleteParam("max_scanner_range"); 64 | 65 | nh_local_.deleteParam("min_x_range"); 66 | nh_local_.deleteParam("max_x_range"); 67 | nh_local_.deleteParam("min_y_range"); 68 | nh_local_.deleteParam("max_y_range"); 69 | 70 | nh_local_.deleteParam("fixed_frame_id"); 71 | nh_local_.deleteParam("target_frame_id"); 72 | } 73 | 74 | bool ScansMerger::updateParams(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { 75 | bool prev_active = p_active_; 76 | 77 | nh_local_.param("active", p_active_, true); 78 | nh_local_.param("publish_scan", p_publish_scan_, false); 79 | nh_local_.param("publish_pcl", p_publish_pcl_, true); 80 | 81 | nh_local_.param("ranges_num", p_ranges_num_, 1000); 82 | 83 | nh_local_.param("min_scanner_range", p_min_scanner_range_, 0.05); 84 | nh_local_.param("max_scanner_range", p_max_scanner_range_, 10.0); 85 | 86 | nh_local_.param("min_x_range", p_min_x_range_, -10.0); 87 | nh_local_.param("max_x_range", p_max_x_range_, 10.0); 88 | nh_local_.param("min_y_range", p_min_y_range_, -10.0); 89 | nh_local_.param("max_y_range", p_max_y_range_, 10.0); 90 | 91 | nh_local_.param("fixed_frame_id", p_fixed_frame_id_, "map"); 92 | nh_local_.param("target_frame_id", p_target_frame_id_, "robot"); 93 | 94 | if (p_active_ != prev_active) { 95 | if (p_active_) { 96 | front_scan_sub_ = nh_.subscribe("front_scan", 10, &ScansMerger::frontScanCallback, this); 97 | rear_scan_sub_ = nh_.subscribe("rear_scan", 10, &ScansMerger::rearScanCallback, this); 98 | scan_pub_ = nh_.advertise("scan", 10); 99 | pcl_pub_ = nh_.advertise("pcl", 10); 100 | } 101 | else { 102 | front_scan_sub_.shutdown(); 103 | rear_scan_sub_.shutdown(); 104 | scan_pub_.shutdown(); 105 | pcl_pub_.shutdown(); 106 | } 107 | } 108 | 109 | return true; 110 | } 111 | 112 | void ScansMerger::frontScanCallback(const sensor_msgs::LaserScan::ConstPtr front_scan) { 113 | try { 114 | tf_ls_.waitForTransform(front_scan->header.frame_id, p_fixed_frame_id_, 115 | front_scan->header.stamp + ros::Duration().fromSec(front_scan->ranges.size() * front_scan->time_increment), ros::Duration(0.05)); 116 | projector_.transformLaserScanToPointCloud(p_fixed_frame_id_, *front_scan, front_pcl_, tf_ls_); 117 | } 118 | catch (tf::TransformException& ex) { 119 | front_scan_error_ = true; 120 | return; 121 | } 122 | 123 | front_scan_received_ = true; 124 | front_scan_error_ = false; 125 | 126 | if (rear_scan_received_ || rear_scan_error_) 127 | publishMessages(); 128 | else 129 | rear_scan_error_ = true; 130 | } 131 | 132 | void ScansMerger::rearScanCallback(const sensor_msgs::LaserScan::ConstPtr rear_scan) { 133 | try { 134 | tf_ls_.waitForTransform(rear_scan->header.frame_id, p_fixed_frame_id_, 135 | rear_scan->header.stamp + ros::Duration().fromSec(rear_scan->ranges.size() * rear_scan->time_increment), ros::Duration(0.05)); 136 | projector_.transformLaserScanToPointCloud(p_fixed_frame_id_, *rear_scan, rear_pcl_, tf_ls_); 137 | } 138 | catch (tf::TransformException& ex) { 139 | rear_scan_error_ = true; 140 | return; 141 | } 142 | 143 | rear_scan_received_ = true; 144 | rear_scan_error_ = false; 145 | 146 | if (front_scan_received_ || front_scan_error_) 147 | publishMessages(); 148 | else 149 | front_scan_error_ = true; 150 | } 151 | 152 | void ScansMerger::publishMessages() { 153 | ros::Time now = ros::Time::now(); 154 | 155 | vector ranges; 156 | vector points; 157 | sensor_msgs::PointCloud new_front_pcl, new_rear_pcl; 158 | 159 | ranges.assign(p_ranges_num_, nanf("")); // Assign nan values 160 | 161 | if (!front_scan_error_) { 162 | try { 163 | tf_ls_.waitForTransform(p_target_frame_id_, now, front_pcl_.header.frame_id, front_pcl_.header.stamp, p_fixed_frame_id_, ros::Duration(0.05)); 164 | tf_ls_.transformPointCloud(p_target_frame_id_, now, front_pcl_, p_fixed_frame_id_, new_front_pcl); 165 | } 166 | catch (tf::TransformException& ex) { 167 | return; 168 | } 169 | 170 | for (auto& point : new_front_pcl.points) { 171 | if (point.x > p_min_x_range_ && point.x < p_max_x_range_ && 172 | point.y > p_min_y_range_ && point.y < p_max_y_range_) { 173 | 174 | double range = sqrt(pow(point.x, 2.0) + pow(point.y, 2.0)); 175 | 176 | if (range > p_min_scanner_range_ && range < p_max_scanner_range_) { 177 | if (p_publish_pcl_) { 178 | points.push_back(point); 179 | } 180 | 181 | if (p_publish_scan_) { 182 | double angle = atan2(point.y, point.x); 183 | 184 | size_t idx = static_cast(p_ranges_num_ * (angle + M_PI) / (2.0 * M_PI)); 185 | if (isnan(ranges[idx]) || range < ranges[idx]) 186 | ranges[idx] = range; 187 | } 188 | } 189 | } 190 | } 191 | } 192 | 193 | if (!rear_scan_error_) { 194 | try { 195 | tf_ls_.waitForTransform(p_target_frame_id_, now, rear_pcl_.header.frame_id, rear_pcl_.header.stamp, p_fixed_frame_id_, ros::Duration(0.05)); 196 | tf_ls_.transformPointCloud(p_target_frame_id_, now, rear_pcl_, p_fixed_frame_id_, new_rear_pcl); 197 | } 198 | catch (tf::TransformException& ex) { 199 | return; 200 | } 201 | 202 | for (auto& point : new_rear_pcl.points) { 203 | if (point.x > p_min_x_range_ && point.x < p_max_x_range_ && 204 | point.y > p_min_y_range_ && point.y < p_max_y_range_) { 205 | 206 | double range = sqrt(pow(point.x, 2.0) + pow(point.y, 2.0)); 207 | 208 | if (range > p_min_scanner_range_ && range < p_max_scanner_range_) { 209 | if (p_publish_pcl_) { 210 | points.push_back(point); 211 | } 212 | 213 | if (p_publish_scan_) { 214 | double angle = atan2(point.y, point.x); 215 | 216 | size_t idx = static_cast(p_ranges_num_ * (angle + M_PI) / (2.0 * M_PI)); 217 | if (isnan(ranges[idx]) || range < ranges[idx]) 218 | ranges[idx] = range; 219 | } 220 | } 221 | } 222 | } 223 | } 224 | 225 | if (p_publish_scan_) { 226 | sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan); 227 | 228 | scan_msg->header.frame_id = p_target_frame_id_; 229 | scan_msg->header.stamp = now; 230 | scan_msg->angle_min = -M_PI; 231 | scan_msg->angle_max = M_PI; 232 | scan_msg->angle_increment = 2.0 * M_PI / (p_ranges_num_ - 1); 233 | scan_msg->time_increment = 0.0; 234 | scan_msg->scan_time = 0.1; 235 | scan_msg->range_min = p_min_scanner_range_; 236 | scan_msg->range_max = p_max_scanner_range_; 237 | scan_msg->ranges.assign(ranges.begin(), ranges.end()); 238 | 239 | scan_pub_.publish(scan_msg); 240 | } 241 | 242 | if (p_publish_pcl_) { 243 | sensor_msgs::PointCloudPtr pcl_msg(new sensor_msgs::PointCloud); 244 | 245 | pcl_msg->header.frame_id = p_target_frame_id_; 246 | pcl_msg->header.stamp = now; 247 | pcl_msg->points.assign(points.begin(), points.end()); 248 | 249 | pcl_pub_.publish(pcl_msg); 250 | } 251 | 252 | front_scan_received_ = false; 253 | rear_scan_received_ = false; 254 | } 255 | --------------------------------------------------------------------------------