diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..9a65e4c --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +CMakeLists.txt.user +*~ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..6356a7f --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,138 @@ +cmake_minimum_required(VERSION 2.8.3) +project(obstacle_detector) + +set(CMAKE_CXX_FLAGS "-std=c++11 -fpermissive ${CMAKE_CXX_FLAGS} -Wfatal-errors\ ") + +find_package(catkin REQUIRED COMPONENTS roscpp roslaunch nodelet rviz std_msgs nav_msgs geometry_msgs sensor_msgs std_srvs tf message_generation) +find_package(Armadillo REQUIRED) +find_package(Boost 1.54.0 REQUIRED system) + +add_message_files(FILES CircleObstacle.msg SegmentObstacle.msg Obstacles.msg) +generate_messages(DEPENDENCIES std_msgs geometry_msgs) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES scan_rectifier scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui + CATKIN_DEPENDS roscpp nodelet rviz std_msgs nav_msgs geometry_msgs sensor_msgs std_srvs tf message_runtime +) + +include_directories(include ${catkin_INCLUDE_DIRS} ${ARMADILLO_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) + +# +# Build libs +# +add_library(scan_rectifier src/scan_rectifier.cpp) +target_link_libraries(scan_rectifier ${catkin_LIBRARIES}) +add_dependencies(scan_rectifier ${catkin_EXPORTED_TARGETS}) + +add_library(scans_merger src/scans_merger.cpp) +target_link_libraries(scans_merger ${catkin_LIBRARIES}) +add_dependencies(scans_merger ${catkin_EXPORTED_TARGETS}) + +add_library(obstacle_extractor src/obstacle_extractor.cpp) +target_link_libraries(obstacle_extractor ${catkin_LIBRARIES} ${ARMADILLO_LIBRARIES}) +add_dependencies(obstacle_extractor ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) + +add_library(obstacle_tracker src/obstacle_tracker.cpp) +target_link_libraries(obstacle_tracker ${catkin_LIBRARIES} ${ARMADILLO_LIBRARIES}) +add_dependencies(obstacle_tracker ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) + +add_library(obstacle_publisher src/obstacle_publisher.cpp) +target_link_libraries(obstacle_publisher ${catkin_LIBRARIES}) +add_dependencies(obstacle_publisher ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) + +# +# Build nodes +# +add_executable(scan_rectifier_node src/nodes/scan_rectifier_node.cpp) +target_link_libraries(scan_rectifier_node scan_rectifier) + +add_executable(scans_merger_node src/nodes/scans_merger_node.cpp) +target_link_libraries(scans_merger_node scans_merger) + +add_executable(obstacle_extractor_node src/nodes/obstacle_extractor_node.cpp) +target_link_libraries(obstacle_extractor_node obstacle_extractor) + +add_executable(obstacle_tracker_node src/nodes/obstacle_tracker_node.cpp) +target_link_libraries(obstacle_tracker_node obstacle_tracker) + +add_executable(obstacle_publisher_node src/nodes/obstacle_publisher_node.cpp) +target_link_libraries(obstacle_publisher_node obstacle_publisher) + +# +# Build nodelets +# +add_library(${PROJECT_NAME}_nodelets + src/nodelets/scan_rectifier_nodelet.cpp + src/nodelets/scans_merger_nodelet.cpp + src/nodelets/obstacle_extractor_nodelet.cpp + src/nodelets/obstacle_tracker_nodelet.cpp + src/nodelets/obstacle_publisher_nodelet.cpp) +target_link_libraries(${PROJECT_NAME}_nodelets scan_rectifier scans_merger obstacle_extractor obstacle_tracker obstacle_publisher) + +# +# Build rviz plugins +# +set(CMAKE_AUTOMOC ON) + +if(rviz_QT_VERSION VERSION_LESS "5") + message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) + include(${QT_USE_FILE}) +else() + message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) + set(QT_LIBRARIES Qt5::Widgets) +endif() + +add_definitions(-DQT_NO_KEYWORDS) + +add_library(${PROJECT_NAME}_gui + src/displays/obstacles_display.cpp include/${PROJECT_NAME}/displays/obstacles_display.h + src/displays/circle_visual.cpp include/${PROJECT_NAME}/displays/circle_visual.h + src/displays/segment_visual.cpp include/${PROJECT_NAME}/displays/segment_visual.h + # + src/panels/scans_merger_panel.cpp include/${PROJECT_NAME}/panels/scans_merger_panel.h + src/panels/obstacle_extractor_panel.cpp include/${PROJECT_NAME}/panels/obstacle_extractor_panel.h + src/panels/obstacle_tracker_panel.cpp include/${PROJECT_NAME}/panels/obstacle_tracker_panel.h + src/panels/obstacle_publisher_panel.cpp include/${PROJECT_NAME}/panels/obstacle_publisher_panel.h) +target_link_libraries(${PROJECT_NAME}_gui ${QT_LIBRARIES} ${catkin_LIBRARIES}) +add_dependencies(${PROJECT_NAME}_gui ${catkin_EXPORTED_TARGETS}) + +# +# Install libraries +# +install(TARGETS scan_rectifier scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + +# +# Install nodes +# +install(TARGETS scan_rectifier_node scans_merger_node obstacle_extractor_node obstacle_tracker_node obstacle_publisher_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +# +# Install header files +# +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +# +# Install nodelet and rviz plugins description +# +install(FILES nodelet_plugins.xml rviz_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +# +# Install launch files +# +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +# +# Create folders and copy resources +# +file(MAKE_DIRECTORY $ENV{HOME}/.local/share/icons/robor) +file(COPY resources/play.png resources/stop.png DESTINATION $ENV{HOME}/.local/share/icons/robor) diff --git a/README.md b/README.md new file mode 100644 index 0000000..e6f1384 --- /dev/null +++ b/README.md @@ -0,0 +1,216 @@ +# The obstacle_detector package + +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. + +The package requires [Armadillo C++](http://arma.sourceforge.net) library for compilation and runtime. + +----------------------- +

+ Visual example of obstacle detector output. +
+ Fig. 1. Visual example of obstacle detector output. +

+----------------------- + +1. The nodes and nodelets + 1.1 The scans_merger + 1.2 The obstacle_extractor + 1.3 The obstacle_tracker + 1.4 The obstacle_publisher + 1.5 The obstacle_recorder +2. The messages +3. Launch files +4. The displays + +## 1. The nodes and nodelets + +The package provides separate nodes/nodelets to perform separate tasks. In general solution the data is processed in a following manner: + +`two laser scans` -> `scans merger` -> `merged scan or pcl` -> `obstacle extractor` -> `obstacles` -> `obstacle tracker` -> `refined obstacles` + +For some scenarios the pure obstacle extraction directly from a laser scan (without tracking) might be sufficient. + +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. + +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. + +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 unchanges (cf. launch files for examples). + +### 1.1. The scans_merger node + +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. + +----------------------- +

+ Visual example of obstacle detector output +
+ Fig. 2. Visual example of scans merging with coordinates restrictions. +

+----------------------- + +The resulting messages contain geometric data described with respect to a specific coordinate frame (e.g. `scanners_base`). Assuming that the coordinate frames attached to two laser scanners are called `front_scanner` and `rear_scanner`, both transformation from `scanners_base` frame to `front_scanner` frame and from `scanners_base` frame to `rear_scanner` frame must be provided. The node allows to artificially restrict measured points to some rectangular region around the `scanners_base` frame as well as to limit the resulting laser scan range. The points falling behind this region or ranges excluded from the limit will be discarded. + +Even if only one laser scanner is used, the node can be useful for simple data pre-processing, e.g. range restriction or recalculation of points to a different coordinate frame. The node uses the following set of local parameters: + +* `~active` (`bool`, default: `true`) - active/sleep mode, +* `~publish_scan` (`bool`, default: `true`) - publish the merged laser scan message, +* `~publish_pcl` (`bool`, default: `false`) - publish the merged point cloud message, +* `~ranges_num` (`int`, default: `1000`) - number of ranges (circular sectors) contained in the 360 deg laser scan message, +* `~min_scanner_range` (`double`, default: `0.05`) - minimal allowable range value for produced laser scan message, +* `~max_scanner_range` (`double`, default: `10.0`) - maximal allowable range value for produced laser scan message, +* `~max_x_range` (`double`, default: `10.0`) - limitation for points coordinates (points with coordinates behind these limitations will be discarded), +* `~min_x_range` (`double`, default: `-10.0`) - as above, +* `~max_y_range` (`double`, default: `10.0`) - as above, +* `~min_y_range` (`double`, default: `-10.0`) - as above, +* `~frame_id` (`string`, default: `scanners_base`) - name of the coordinate frame used as the origin for the produced laser scan or point cloud. + +The package comes with Rviz panel for this node. + +----------------------- +

+ Rviz panel for the scans_merger node. +
+ Fig. 3. Rviz panel for the scans_merger node. +

+----------------------- + +### 1.2. The obstacle_extractor node + +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 point cloud message must be ordered in the angular fashion, because the algorithm exploits the polar nature of laser scanners. + +----------------------- +

+ Visual example of obstacle extractor output. +
+ Fig. 4. Visual example of obstacle extractor output. +

+----------------------- + +The input points are firstly grouped into subsets. The algorithm extracts segments from each points subset. Next, the segments are checked for possible merging between each other. The circular obstacles are extracted from segments and also checked for possible merging. Resulting set of obstacles can be described with respect to coordinate frame provided by the input messages or to other, custom coordinate frame. + +The node is configurable with the following set of local parameters: + +* `~active` (`bool`, default: `true`) - active/sleep mode, +* `~use_scan` (`bool`, default: `true`) - use laser scan messages, +* `~use_pcl` (`bool`, default: `false`) - use point cloud messages (if both scan and pcl are chosen, scans will have priority), +* `~discard_converted_segments` (`bool`, default: `true`) - do not publish segments, from which the circles were spawned, +* `~transform_coordinates` (`bool`, default: `true`) - transform the coordinates of obstacles to a frame described with `frame_id` parameter, +* `~frame_id` (`string`, default: `world`) - name of coordinate frame used as origin for produced obstacles (used only if `transform_coordinates` flag is set to true). + +The following set of local parameters is dedicated to the algorithm itself: + +* `~use_split_and_merge` (`bool`, default: `false`) - choose wether to use Iterative End Point Fit (false) or Split And Merge (true) algorithm to detect segments, +* `~min_group_points` (`int`, default: `5`) - minimum number of points comprising a group to be further processed, +* `~max_group_distance` (`double`, default: `0.100`) - if the distance between two points is greater than this value, start a new group, +* `~distance_proportion` (`double`, default: `0.006136`) - enlarge the allowable distance between points proportionally to the range of point (use scan angle increment in radians), +* `~max_split_distance` (`double`, default: `0.070`) - if a point in group lays further from a leading line than this value, split the group, +* `~max_merge_separation` (`double`, default: `0.150`) - if distance between obstacles is smaller than this value, consider merging them, +* `~max_merge_spread` (`double`, default: `0.070`) - merge two segments if all of their extreme points lay closer to the leading line than this value, +* `~max_circle_radius` (`double`, default: `0.500`) - if a circle would have greater radius than this value, skip it, +* `~radius_enlargement` (`double`, default: `0.100`) - artificially enlarge the circles radius by this value. + +The package comes with Rviz panel for this node. + +----------------------- +

+ Rviz panel for the obstacle_detector node. +
+ Fig. 5. Rviz panel for the obstacle_detector node. +

+----------------------- + +### 1.3. The obstacle_tracker node + +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. + +----------------------- +

+ Visual example of obstacle tracker output. +
+ Fig. 6. Visual example of obstacle tracker output. +

+----------------------- + +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: + +* `~active` (`bool`, default: `true`) - active/sleep mode, +* `~copy_segments` (`bool`, default: `true`) - copy detected segments into tracked obstacles message, +* `~loop_rate` (`double`, default: `100.0`) - the main loop rate in Hz, +* `~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), +* `~min_correspondence_cost` (`double`, default `0.3`) - a threshold for correspondence test, +* `~std_correspondence_dev` (`double`, default `0.15`) - (experimental) standard deviation of the position ellipse in the correspondence test, +* `~process_variance` (`double`, default `0.01`) - variance of obstacles position and radius (parameter of Kalman Filter), +* `~process_rate_variance` (`double`, default `0.1`) - variance of rate of change of obstacles values (parameter of Kalman Filter), +* `~measurement_variance` (`double`, default `1.0`) - variance of measured obstacles values (parameter of Kalman Filter). + +The package comes with Rviz panel for this node. + +----------------------- +

+ Rviz panel for the obstacle_tracker node. +
+ Fig. 7. Rviz panel for the obstacle_tracker node. +

+----------------------- + +### 1.4. The obstacle_publisher node + +The auxiliary node which allows to publishes 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: + +* `~active` (`bool`, default: `false`) - active/sleep mode, +* `~reset` (`bool`, default: `false`) - reset time for obstacles motion calculation, +* `~loop_rate` (`double`, default: `10.0`) - the main loop rate in Hz. + +The following parameters are used to provide the node with a set of obstacles: + +* `~x_vector` (`std::vector`, default: `[]`) - the array of x-coordinates of obstacles center points, +* `~y_vector` (`std::vector`, default: `[]`) - the array of y-coordinates of obstacles center points, +* `~r_vector` (`std::vector`, default: `[]`) - the array of obstacles radii, +* `~x_vector` (`std::vector`, default: `[]`) - the array of velocities of obstacles center points in x direction, +* `~y_vector` (`std::vector`, default: `[]`) - the array of velocities of obstacles center points in y direction. + +The package comes with Rviz panel for this node. + +----------------------- +

+ Rviz panel for the obstacle_publisher node. +
+ Fig. 9. Rviz panel for the obstacle_publisher node. +

+----------------------- + +## 2. The messages + +The package provides three custom message types. All of their numerical values are provided in SI units. + +* `CircleObstacle` + - `geometry_msgs/Point center` - center of circular obstacle, + - `geometry_msgs/Vector3 velocity` - linear velocity of circular obstacle, + - `float64 radius` - radius of circular obstacle with added safety margin, + - `float64 true_radius` - measured radius of obstacle without the safety margin. +* `SegmentObstacle` + - `geometry_msgs/Point first_point` - first point of the segment (in counter-clockwise direction), + - `geometry_msgs/Point last_point` - end point of the segment. +* `Obstacles` + - `Header header` + - `obstacle_detector/SegmentObstacle[] segments` + - `obstacle_detector/CircleObstacle[] circles` + +## 3. The launch files + +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. +* `demo.launch` - Plays a rosbag with recorded scans and starts all of the nodes with Rviz configured with appropriate panels. +* `nodes_example.launch` - Runs all of the nodes with their parameters set to default values. +* `nodelets_example.launch` - Runs all of the nodelets with their parameters set to default values. + +## 4. The displays + +For better visual effects, appropriate Rviz display for `Obstacles` messages was prepared. Via its properties, one can change the colors of the obstacles. + +## In progress + +The current work focuses on preparing a scans rectifier node which can correct the scans in the moving frames. + +Author: +_Mateusz Przybyla_ + diff --git a/icons/classes/Obstacle Extractor.png b/icons/classes/Obstacle Extractor.png new file mode 100644 index 0000000..bf97282 Binary files /dev/null and b/icons/classes/Obstacle Extractor.png differ diff --git a/icons/classes/Obstacle Publisher.png b/icons/classes/Obstacle Publisher.png new file mode 100644 index 0000000..61a4902 Binary files /dev/null and b/icons/classes/Obstacle Publisher.png differ diff --git a/icons/classes/Obstacle Tracker.png b/icons/classes/Obstacle Tracker.png new file mode 100644 index 0000000..4c1c767 Binary files /dev/null and b/icons/classes/Obstacle Tracker.png differ diff --git a/icons/classes/Obstacles.png b/icons/classes/Obstacles.png new file mode 100644 index 0000000..ed91d08 Binary files /dev/null and b/icons/classes/Obstacles.png differ diff --git a/icons/classes/Scans Merger.png b/icons/classes/Scans Merger.png new file mode 100644 index 0000000..b1dff2c Binary files /dev/null and b/icons/classes/Scans Merger.png differ diff --git a/include/obstacle_detector/displays/circle_visual.h b/include/obstacle_detector/displays/circle_visual.h new file mode 100644 index 0000000..35de349 --- /dev/null +++ b/include/obstacle_detector/displays/circle_visual.h @@ -0,0 +1,73 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#ifndef Q_MOC_RUN +#include + +#include +#include +#include + +#include +#endif + +namespace obstacles_display +{ + +class CircleVisual +{ +public: + CircleVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node); + + virtual ~CircleVisual(); + + void setData(const obstacle_detector::CircleObstacle& circle); + void setFramePosition(const Ogre::Vector3& position); + void setFrameOrientation(const Ogre::Quaternion& orientation); + void setMainColor(float r, float g, float b, float a); + void setMarginColor(float r, float g, float b, float a); + +private: + boost::shared_ptr obstacle_; + boost::shared_ptr margin_; + + Ogre::SceneNode* frame_node_1_; + Ogre::SceneNode* frame_node_2_; + Ogre::SceneManager* scene_manager_; +}; + +} // end namespace obstacles_display diff --git a/include/obstacle_detector/displays/obstacles_display.h b/include/obstacle_detector/displays/obstacles_display.h new file mode 100644 index 0000000..7538234 --- /dev/null +++ b/include/obstacle_detector/displays/obstacles_display.h @@ -0,0 +1,88 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#ifndef Q_MOC_RUN +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include "obstacle_detector/displays/circle_visual.h" +#include "obstacle_detector/displays/segment_visual.h" +#endif + +namespace obstacles_display +{ + +class ObstaclesDisplay: public rviz::MessageFilterDisplay +{ +Q_OBJECT +public: + ObstaclesDisplay(); + virtual ~ObstaclesDisplay(); + +protected: + virtual void onInitialize(); + virtual void reset(); + +private Q_SLOTS: + void updateCircleColor(); + void updateSegmentColor(); + void updateAlpha(); + void updateThickness(); + +private: + void processMessage(const obstacle_detector::Obstacles::ConstPtr& obstacles_msg); + + std::vector< boost::shared_ptr > circle_visuals_; + std::vector< boost::shared_ptr > segment_visuals_; + + rviz::ColorProperty* circle_color_property_; + rviz::ColorProperty* margin_color_property_; + rviz::ColorProperty* segment_color_property_; + rviz::FloatProperty* alpha_property_; + rviz::FloatProperty* thickness_property_; +}; + +} // end namespace obstacles_display diff --git a/include/obstacle_detector/displays/segment_visual.h b/include/obstacle_detector/displays/segment_visual.h new file mode 100644 index 0000000..8605658 --- /dev/null +++ b/include/obstacle_detector/displays/segment_visual.h @@ -0,0 +1,72 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#ifndef Q_MOC_RUN +#include + +#include +#include +#include + +#include +#include +#endif + +namespace obstacles_display +{ + +class SegmentVisual +{ +public: + SegmentVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node); + + virtual ~SegmentVisual(); + + void setData(const obstacle_detector::SegmentObstacle& segment); + void setFramePosition(const Ogre::Vector3& position); + void setFrameOrientation(const Ogre::Quaternion& orientation); + void setColor(float r, float g, float b, float a); + void setWidth(float w); + +private: + boost::shared_ptr line_; + + Ogre::SceneNode* frame_node_; + Ogre::SceneManager* scene_manager_; +}; + +} // end namespace obstacles_display diff --git a/include/obstacle_detector/obstacle_extractor.h b/include/obstacle_detector/obstacle_extractor.h new file mode 100644 index 0000000..d23e586 --- /dev/null +++ b/include/obstacle_detector/obstacle_extractor.h @@ -0,0 +1,121 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "obstacle_detector/utilities/point.h" +#include "obstacle_detector/utilities/segment.h" +#include "obstacle_detector/utilities/circle.h" +#include "obstacle_detector/utilities/point_set.h" + +namespace obstacle_detector +{ + +class ObstacleExtractor +{ +public: + ObstacleExtractor(ros::NodeHandle& nh, ros::NodeHandle& nh_local); + +private: + bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + void scanCallback(const sensor_msgs::LaserScan::ConstPtr scan_msg); + void pclCallback(const sensor_msgs::PointCloud::ConstPtr pcl_msg); + + void initialize() { std_srvs::Empty empt; updateParams(empt.request, empt.response); } + + void processPoints(); + void groupPoints(); + void publishObstacles(); + + void detectSegments(const PointSet& point_set); + void mergeSegments(); + bool compareSegments(const Segment& s1, const Segment& s2, Segment& merged_segment); + bool checkSegmentsProximity(const Segment& s1, const Segment& s2); + bool checkSegmentsCollinearity(const Segment& segment, const Segment& s1, const Segment& s2); + + void detectCircles(); + void mergeCircles(); + bool compareCircles(const Circle& c1, const Circle& c2, Circle& merged_circle); + + ros::NodeHandle nh_; + ros::NodeHandle nh_local_; + + ros::Subscriber scan_sub_; + ros::Subscriber pcl_sub_; + ros::Publisher obstacles_pub_; + ros::ServiceServer params_srv_; + + ros::Time stamp_; + std::string base_frame_id_; + tf::TransformListener tf_listener_; + + std::list input_points_; + std::list segments_; + std::list circles_; + + // Parameters + bool p_active_; + bool p_use_scan_; + bool p_use_pcl_; + bool p_use_split_and_merge_; + bool p_discard_converted_segments_; + bool p_transform_coordinates_; + + int p_min_group_points_; + + double p_distance_proportion_; + double p_max_group_distance_; + double p_max_split_distance_; + double p_max_merge_separation_; + double p_max_merge_spread_; + double p_max_circle_radius_; + double p_radius_enlargement_; + + double p_min_x_limit_; + double p_max_x_limit_; + double p_min_y_limit_; + double p_max_y_limit_; + + std::string p_frame_id_; +}; + +} // namespace obstacle_detector diff --git a/include/obstacle_detector/obstacle_publisher.h b/include/obstacle_detector/obstacle_publisher.h new file mode 100644 index 0000000..e7d762e --- /dev/null +++ b/include/obstacle_detector/obstacle_publisher.h @@ -0,0 +1,92 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include +#include +#include + +namespace obstacle_detector +{ + +class ObstaclePublisher +{ +public: + ObstaclePublisher(ros::NodeHandle &nh, ros::NodeHandle &nh_local); + +private: + bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + void timerCallback(const ros::TimerEvent& e); + + void initialize() { std_srvs::Empty empt; updateParams(empt.request, empt.response); } + + void calculateObstaclesPositions(double dt); + void fusionExample(double t); + void fissionExample(double t); + void publishObstacles(); + void reset(); + + ros::NodeHandle nh_; + ros::NodeHandle nh_local_; + + ros::Publisher obstacle_pub_; + ros::ServiceServer params_srv_; + ros::Timer timer_; + + obstacle_detector::Obstacles obstacles_; + double t_; + + // Parameters + bool p_active_; + bool p_reset_; + bool p_fusion_example_; + bool p_fission_example_; + + double p_loop_rate_; + double p_sampling_time_; + double p_radius_margin_; + + std::vector p_x_vector_; + std::vector p_y_vector_; + std::vector p_r_vector_; + + std::vector p_vx_vector_; + std::vector p_vy_vector_; + + std::string p_frame_id_; +}; + +} // namespace obstacle_detector diff --git a/include/obstacle_detector/obstacle_tracker.h b/include/obstacle_detector/obstacle_tracker.h new file mode 100644 index 0000000..8503bb7 --- /dev/null +++ b/include/obstacle_detector/obstacle_tracker.h @@ -0,0 +1,109 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "obstacle_detector/utilities/tracked_obstacle.h" +#include "obstacle_detector/utilities/math_utilities.h" + +namespace obstacle_detector +{ + +class ObstacleTracker { +public: + ObstacleTracker(ros::NodeHandle& nh, ros::NodeHandle& nh_local); + +private: + bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + void timerCallback(const ros::TimerEvent&); + void obstaclesCallback(const obstacle_detector::Obstacles::ConstPtr new_obstacles); + + void initialize() { std_srvs::Empty empt; updateParams(empt.request, empt.response); } + + double obstacleCostFunction(const CircleObstacle& new_obstacle, const CircleObstacle& old_obstacle); + void calculateCostMatrix(const std::vector& new_obstacles, arma::mat& cost_matrix); + void calculateRowMinIndices(const arma::mat& cost_matrix, std::vector& row_min_indices); + void calculateColMinIndices(const arma::mat& cost_matrix, std::vector& col_min_indices); + + bool fusionObstacleUsed(const int idx, const std::vector& col_min_indices, const std::vector& used_new, const std::vector& used_old); + bool fusionObstaclesCorrespond(const int idx, const int jdx, const std::vector& col_min_indices, const std::vector& used_old); + bool fissionObstacleUsed(const int idx, const int T, const std::vector& row_min_indices, const std::vector& used_new, const std::vector& used_old); + bool fissionObstaclesCorrespond(const int idx, const int jdx, const std::vector& row_min_indices, const std::vector& used_new); + + void fuseObstacles(const std::vector& fusion_indices, const std::vector& col_min_indices, + std::vector& new_tracked, const Obstacles::ConstPtr& new_obstacles); + void fissureObstacle(const std::vector& fission_indices, const std::vector& row_min_indices, + std::vector& new_tracked, const Obstacles::ConstPtr& new_obstacles); + + void updateObstacles(); + void publishObstacles(); + + ros::NodeHandle nh_; + ros::NodeHandle nh_local_; + + ros::Subscriber obstacles_sub_; + ros::Publisher obstacles_pub_; + ros::ServiceServer params_srv_; + ros::Timer timer_; + + double radius_margin_; + obstacle_detector::Obstacles obstacles_; + + std::vector tracked_obstacles_; + std::vector untracked_obstacles_; + + // Parameters + bool p_active_; + bool p_copy_segments_; + + double p_tracking_duration_; + double p_loop_rate_; + double p_sampling_time_; + double p_sensor_rate_; + double p_min_correspondence_cost_; + double p_std_correspondence_dev_; + double p_process_variance_; + double p_process_rate_variance_; + double p_measurement_variance_; +}; + +} // namespace obstacle_detector diff --git a/include/obstacle_detector/panels/obstacle_extractor_panel.h b/include/obstacle_detector/panels/obstacle_extractor_panel.h new file mode 100644 index 0000000..fb618cc --- /dev/null +++ b/include/obstacle_detector/panels/obstacle_extractor_panel.h @@ -0,0 +1,119 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace obstacle_detector +{ + +class ObstacleExtractorPanel : public rviz::Panel +{ +Q_OBJECT +public: + ObstacleExtractorPanel(QWidget* parent = 0); + + virtual void load(const rviz::Config& config); + virtual void save(rviz::Config config) const; + +private Q_SLOTS: + void processInputs(); + +private: + void verifyInputs(); + void setParams(); + void getParams(); + void evaluateParams(); + void notifyParamsUpdate(); + +private: + QCheckBox* activate_checkbox_; + QCheckBox* use_scan_checkbox_; + QCheckBox* use_pcl_checkbox_; + QCheckBox* use_split_merge_checkbox_; + QCheckBox* discard_segments_checkbox_; + QCheckBox* transform_coords_checkbox_; + + QLineEdit* min_n_input_; + QLineEdit* dist_prop_input_; + QLineEdit* group_dist_input_; + QLineEdit* split_dist_input_; + QLineEdit* merge_sep_input_; + QLineEdit* merge_spread_input_; + QLineEdit* max_radius_input_; + QLineEdit* radius_enl_input_; + QLineEdit* frame_id_input_; + + ros::NodeHandle nh_; + ros::NodeHandle nh_local_; + + ros::ServiceClient params_cli_; + + // Parameters + int p_min_group_points_; + + bool p_active_; + bool p_use_scan_; + bool p_use_pcl_; + bool p_use_split_and_merge_; + bool p_discard_converted_segments_; + bool p_transform_coordinates_; + + double p_distance_proportion_; + double p_max_group_distance_; + + double p_max_split_distance_; + double p_max_merge_separation_; + double p_max_merge_spread_; + double p_max_circle_radius_; + double p_radius_enlargement_; + + std::string p_frame_id_; +}; + +} // namespace obstacle_detector diff --git a/include/obstacle_detector/panels/obstacle_publisher_panel.h b/include/obstacle_detector/panels/obstacle_publisher_panel.h new file mode 100644 index 0000000..b421222 --- /dev/null +++ b/include/obstacle_detector/panels/obstacle_publisher_panel.h @@ -0,0 +1,123 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace obstacle_detector +{ + +class ObstaclePublisherPanel : public rviz::Panel +{ +Q_OBJECT +public: + ObstaclePublisherPanel(QWidget* parent = 0); + + virtual void load(const rviz::Config& config); + virtual void save(rviz::Config config) const; + +private Q_SLOTS: + void processInputs(); + void addObstacle(); + void removeObstacles(); + void reset(); + +private: + void verifyInputs(); + void setParams(); + void getParams(); + void evaluateParams(); + void notifyParamsUpdate(); + +private: + QCheckBox* activate_checkbox_; + QCheckBox* fusion_example_checkbox_; + QCheckBox* fission_example_checkbox_; + + QListWidget* obstacles_list_; + std::vector obstacles_list_items_; + + QPushButton* add_button_; + QPushButton* remove_button_; + QPushButton* reset_button_; + + QLineEdit* x_input_; + QLineEdit* y_input_; + QLineEdit* r_input_; + + QLineEdit* vx_input_; + QLineEdit* vy_input_; + + ros::NodeHandle nh_; + ros::NodeHandle nh_local_; + + ros::ServiceClient params_cli_; + + double x_, y_, r_, vx_, vy_; + + // Parameters + bool p_active_; + bool p_reset_; + bool p_fusion_example_; + bool p_fission_example_; + + double p_loop_rate_; + + std::vector p_x_vector_; + std::vector p_y_vector_; + std::vector p_r_vector_; + + std::vector p_vx_vector_; + std::vector p_vy_vector_; + + std::string p_frame_id_; +}; + +} diff --git a/include/obstacle_detector/panels/obstacle_tracker_panel.h b/include/obstacle_detector/panels/obstacle_tracker_panel.h new file mode 100644 index 0000000..2ffd7de --- /dev/null +++ b/include/obstacle_detector/panels/obstacle_tracker_panel.h @@ -0,0 +1,102 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace obstacle_detector +{ + +class ObstacleTrackerPanel : public rviz::Panel +{ +Q_OBJECT +public: + ObstacleTrackerPanel(QWidget* parent = 0); + + virtual void load(const rviz::Config& config); + virtual void save(rviz::Config config) const; + +private Q_SLOTS: + void processInputs(); + +private: + void verifyInputs(); + void setParams(); + void getParams(); + void evaluateParams(); + void notifyParamsUpdate(); + +private: + QCheckBox* activate_checkbox_; + + QLineEdit* tracking_duration_input_; + QLineEdit* loop_rate_input_; + QLineEdit* min_corr_cost_input_; + QLineEdit* std_corr_dev_input_; + QLineEdit* process_var_input_; + QLineEdit* process_rate_var_input_; + QLineEdit* measure_var_input_; + + ros::NodeHandle nh_; + ros::NodeHandle nh_local_; + + ros::ServiceClient params_cli_; + + // Parameters + bool p_active_; + + double p_tracking_duration_; + double p_loop_rate_; + double p_min_correspondence_cost_; + double p_std_correspondence_dev_; + double p_process_variance_; + double p_process_rate_variance_; + double p_measurement_variance_; +}; + +} diff --git a/include/obstacle_detector/panels/scans_merger_panel.h b/include/obstacle_detector/panels/scans_merger_panel.h new file mode 100644 index 0000000..5d3448d --- /dev/null +++ b/include/obstacle_detector/panels/scans_merger_panel.h @@ -0,0 +1,111 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace obstacle_detector +{ + +class ScansMergerPanel : public rviz::Panel +{ +Q_OBJECT +public: + ScansMergerPanel(QWidget* parent = 0); + + virtual void load(const rviz::Config& config); + virtual void save(rviz::Config config) const; + +private Q_SLOTS: + void processInputs(); + +private: + void verifyInputs(); + void setParams(); + void getParams(); + void evaluateParams(); + void notifyParamsUpdate(); + +private: + QCheckBox* activate_checkbox_; + QCheckBox* scan_checkbox_; + QCheckBox* pcl_checkbox_; + + QLineEdit* n_input_; + QLineEdit* r_min_input_; + QLineEdit* r_max_input_; + + QLineEdit* x_min_input_; + QLineEdit* x_max_input_; + QLineEdit* y_min_input_; + QLineEdit* y_max_input_; + + QLineEdit* frame_id_input_; + + ros::NodeHandle nh_; + ros::NodeHandle nh_local_; + + ros::ServiceClient params_cli_; + + // Parameters + int p_ranges_num_; + + bool p_active_; + bool p_publish_scan_; + bool p_publish_pcl_; + + double p_min_scanner_range_; + double p_max_scanner_range_; + double p_max_x_range_; + double p_min_x_range_; + double p_max_y_range_; + double p_min_y_range_; + + std::string p_frame_id_; +}; + +} // namespace obstacle_detector diff --git a/include/obstacle_detector/scan_rectifier.h b/include/obstacle_detector/scan_rectifier.h new file mode 100644 index 0000000..736fbf0 --- /dev/null +++ b/include/obstacle_detector/scan_rectifier.h @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace obstacle_detector +{ + +class ScanRectifier +{ +public: + ScanRectifier(ros::NodeHandle& nh, ros::NodeHandle& nh_local); + +private: + bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + void scanCallback(const sensor_msgs::LaserScan::ConstPtr scan); + void odomCallback(const nav_msgs::Odometry::ConstPtr odom_msg); + + void initialize() { std_srvs::Empty empt; updateParams(empt.request, empt.response); } + + ros::NodeHandle nh_; + ros::NodeHandle nh_local_; + + ros::Subscriber scan_sub_; + ros::Subscriber odom_sub_; + ros::Publisher scan_pub_; + ros::ServiceServer params_srv_; + + tf::TransformListener tf_; + + boost::circular_buffer odoms_; + sensor_msgs::LaserScan scan_prototype_; + geometry_msgs::Pose2D scanner_in_base_tf_; + + int num_ranges_; + std::vector ranges_; + std::vector points_; + + // Parameters + bool p_active_; + double p_scan_rate_; + double p_odom_rate_; + int p_odom2scan_ratio_; +}; + +} // namespace obstacle_detector diff --git a/include/obstacle_detector/scans_merger.h b/include/obstacle_detector/scans_merger.h new file mode 100644 index 0000000..bda0199 --- /dev/null +++ b/include/obstacle_detector/scans_merger.h @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace obstacle_detector +{ + +class ScansMerger +{ +public: + ScansMerger(ros::NodeHandle& nh, ros::NodeHandle& nh_local); + +private: + bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + void frontScanCallback(const sensor_msgs::LaserScan::ConstPtr front_scan); + void rearScanCallback(const sensor_msgs::LaserScan::ConstPtr rear_scan); + + void initialize() { std_srvs::Empty empt; updateParams(empt.request, empt.response); } + + void publishScan(); + void publishPCL(); + void publishAll(); + + ros::NodeHandle nh_; + ros::NodeHandle nh_local_; + + ros::ServiceServer params_srv_; + + ros::Subscriber front_scan_sub_; + ros::Subscriber rear_scan_sub_; + ros::Publisher scan_pub_; + ros::Publisher pcl_pub_; + + bool front_scan_received_; + bool rear_scan_received_; + bool front_scan_error_; + bool rear_scan_error_; + + ros::Time stamp_; + + std::string front_scan_frame_id_; + std::string rear_scan_frame_id_; + + geometry_msgs::Pose2D front_tf_; + geometry_msgs::Pose2D rear_tf_; + + std::vector ranges_; + std::vector points_; + + // Parameters + bool p_active_; + bool p_publish_scan_; + bool p_publish_pcl_; + + int p_ranges_num_; + + double p_min_scanner_range_; + double p_max_scanner_range_; + double p_max_x_range_; + double p_min_x_range_; + double p_max_y_range_; + double p_min_y_range_; + + std::string p_frame_id_; +}; + +} // namespace obstacle_detector diff --git a/include/obstacle_detector/utilities/circle.h b/include/obstacle_detector/utilities/circle.h new file mode 100644 index 0000000..614284c --- /dev/null +++ b/include/obstacle_detector/utilities/circle.h @@ -0,0 +1,69 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include "obstacle_detector/utilities/point.h" +#include "obstacle_detector/utilities/segment.h" + +namespace obstacle_detector +{ + +class Circle +{ +public: + Circle(const Point& p = Point(), const double r = 0.0) : center(p), radius(r) { } + + /* + * Create a circle by taking the segment as a base of equilateral + * triangle. The circle is circumscribed on this triangle. + */ + Circle(const Segment& s) { + radius = 0.5773502 * s.length(); // sqrt(3)/3 * length + center = (s.first_point + s.last_point - radius * s.normal()) / 2.0; + point_sets = s.point_sets; + } + + double distanceTo(const Point& p) { return (p - center).length() - radius; } + + friend std::ostream& operator<<(std::ostream& out, const Circle& c) + { out << "C: " << c.center << ", R: " << c.radius; return out; } + + Point center; + double radius; + std::vector point_sets; +}; + +} // namespace obstacle_detector diff --git a/include/obstacle_detector/utilities/figure_fitting.h b/include/obstacle_detector/utilities/figure_fitting.h new file mode 100644 index 0000000..0161742 --- /dev/null +++ b/include/obstacle_detector/utilities/figure_fitting.h @@ -0,0 +1,203 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include + +#include "obstacle_detector/utilities/point.h" +#include "obstacle_detector/utilities/segment.h" +#include "obstacle_detector/utilities/circle.h" + +namespace obstacle_detector +{ + +/* + * Returns a total best fit approximation of + * segment based on given point set. The equation + * used for fitting is given by + * Ax + By = -C + * and the A, B, C parameters are normalized. + */ +Segment fitSegment(const PointSet& point_set) { + static int num_calls = 0; + num_calls++; + + int N = point_set.num_points; + assert(N >= 2); + + arma::mat input = arma::mat(N, 2).zeros(); // [x_i, y_i] + arma::vec output = arma::vec(N).ones(); // [-C] + arma::vec params = arma::vec(2).zeros(); // [A ; B] + + PointIterator point = point_set.begin; + for (int i = 0; i < N; ++i) { + input(i, 0) = point->x; + input(i, 1) = point->y; + std::advance(point, 1); + } + + // Find A and B coefficients from linear regression (assuming C = -1.0) + params = arma::pinv(input) * output; + + double A, B, C; + A = params(0); + B = params(1); + C = -1.0; + + // Find end points + Point p1 = *point_set.begin; + Point p2 = *point_set.end; + + Segment segment(p1, p2); + segment.point_sets.push_back(point_set); + + double D = (A * A + B * B); + + // Project end points on the line + if (D > 0.0) { + Point projected_p1, projected_p2; + + projected_p1.x = ( B * B * p1.x - A * B * p1.y - A * C) / D; + projected_p1.y = (-A * B * p1.x + A * A * p1.y - B * C) / D; + + projected_p2.x = ( B * B * p2.x - A * B * p2.y - A * C) / D; + projected_p2.y = (-A * B * p2.x + A * A * p2.y - B * C) / D; + + segment.first_point = projected_p1; + segment.last_point = projected_p2; + } + + return segment; +} + +Segment fitSegment(const std::vector& point_sets) { + static int num_calls = 0; + num_calls++; + + int N = 0; + for (PointSet ps : point_sets) + N += ps.num_points; + + assert(N >= 2); + + arma::mat input = arma::mat(N, 2).zeros(); // [x_i, y_i] + arma::vec output = arma::vec(N).ones(); // [-C] + arma::vec params = arma::vec(2).zeros(); // [A ; B] + + int n = 0; + for (PointSet ps : point_sets) { + PointIterator point = ps.begin; + for (int i = 0; i < ps.num_points; ++i) { + input(i + n, 0) = point->x; + input(i + n, 1) = point->y; + std::advance(point, 1); + } + + n += ps.num_points; + } + + // Find A and B coefficients from linear regression (assuming C = -1.0) + params = arma::pinv(input) * output; + + double A, B, C; + A = params(0); + B = params(1); + C = -1.0; + + // Find end points + Point p1 = *point_sets.front().begin; + Point p2 = *point_sets.back().end; + + Segment segment(p1, p2); + segment.point_sets = point_sets; + + double D = (A * A + B * B); + + // Project end points on the line + if (D > 0.0) { + Point projected_p1, projected_p2; + + projected_p1.x = ( B * B * p1.x - A * B * p1.y - A * C) / D; + projected_p1.y = (-A * B * p1.x + A * A * p1.y - B * C) / D; + + projected_p2.x = ( B * B * p2.x - A * B * p2.y - A * C) / D; + projected_p2.y = (-A * B * p2.x + A * A * p2.y - B * C) / D; + + segment.first_point = projected_p1; + segment.last_point = projected_p2; + } + + return segment; +} + +/* + * Returns a total best fit approximation of + * a circle based on given point set. The equation + * used for fitting is given by + * a1 * x + a2 * y + a3 = -(x^2 + y^2) + * where parameters a1, a2, a3 are obtained from + * circle equation + * (x-x0)^2 + (y-y0)^2 = r^2. + */ +Circle fitCircle(const std::list& point_set) +{ + int N = point_set.size(); + assert(N >= 3); + + arma::mat input = arma::mat(N, 3).zeros(); // [x_i, y_i, 1] + arma::vec output = arma::vec(N).zeros(); // [-(x_i^2 + y_i^2)] + arma::vec params = arma::vec(3).zeros(); // [a_1 ; a_2 ; a_3] + + int i = 0; + for (const Point& point : point_set) { + input(i, 0) = point.x; + input(i, 1) = point.y; + input(i, 2) = 1.0; + + output(i) = -(pow(point.x, 2) + pow(point.y, 2)); + i++; + } + + // Find a_1, a_2 and a_3 coefficients from linear regression + params = arma::pinv(input) * output; + + Point center(-params(0) / 2.0, -params(1) / 2.0); + double radius = sqrt((params(0) * params(0) + params(1) * params(1)) / 4.0 - params(2)); + + return Circle(center, radius); +} + +} // namespace obstacle_detector diff --git a/include/obstacle_detector/utilities/kalman.h b/include/obstacle_detector/utilities/kalman.h new file mode 100644 index 0000000..879af16 --- /dev/null +++ b/include/obstacle_detector/utilities/kalman.h @@ -0,0 +1,108 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include + +class KalmanFilter +{ +public: + KalmanFilter(uint dim_in, uint dim_out, uint dim_state) : l(dim_in), m(dim_out), n(dim_state) { + using arma::mat; + using arma::vec; + + A = mat(n,n).eye(); + B = mat(n,l).zeros(); + C = mat(m,n).zeros(); + + Q = mat(n,n).eye(); + R = mat(m,m).eye(); + P = mat(n,n).eye(); + + K = mat(n,m).eye(); + I = arma::eye(n,n); + + u = vec(l).zeros(); + q_pred = vec(n).zeros(); + q_est = vec(n).zeros(); + y = vec(m).zeros(); + } + + void predictState() { + q_pred = A * q_est + B * u; + P = A * P * trans(A) + Q; + } + + void correctState() { + K = P * trans(C) * inv(C * P * trans(C) + R); + q_est = q_pred + K * (y - C * q_pred); + P = (I - K * C) * P; + } + + void updateState() { + predictState(); + correctState(); + } + +public: + // System matrices: + arma::mat A; // State + arma::mat B; // Input + arma::mat C; // Output + + // Covariance matrices: + arma::mat Q; // Process + arma::mat R; // Measurement + arma::mat P; // Estimate error + + // Kalman gain matrix: + arma::mat K; + + // Identity matrix + arma::mat I; + + // Signals: + arma::vec u; // Input + arma::vec q_pred; // Predicted state + arma::vec q_est; // Estimated state + arma::vec y; // Measurement + +private: + // Dimensions: + uint l; // Input + uint m; // Output + uint n; // State +}; diff --git a/include/obstacle_detector/utilities/math_utilities.h b/include/obstacle_detector/utilities/math_utilities.h new file mode 100644 index 0000000..ce61970 --- /dev/null +++ b/include/obstacle_detector/utilities/math_utilities.h @@ -0,0 +1,101 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include +#include +#include + +#include "obstacle_detector/utilities/point.h" + +namespace obstacle_detector +{ + +inline double signum(double x) { return (x < 0.0) ? -1.0 : 1.0; } +inline double abs(double x) { return (x < 0.0) ? -x : x; } +inline double max(double x, double y) { return (x > y) ? x : y; } + +inline double length(const geometry_msgs::Point& point) { + return sqrt(point.x * point.x + point.y * point.y); +} + +inline double squaredLength(const geometry_msgs::Point& point) { + return point.x * point.x + point.y * point.y; +} + +inline double length(const geometry_msgs::Vector3& vec) { + return sqrt(vec.x * vec.x + vec.y * vec.y); +} + +inline double squaredLength(const geometry_msgs::Vector3& vec) { + return vec.x * vec.x + vec.y * vec.y; +} + +inline geometry_msgs::Point transformPoint(const geometry_msgs::Point& point, double x, double y, double theta) { + geometry_msgs::Point p; + + p.x = point.x * cos(theta) - point.y * sin(theta) + x; + p.y = point.x * sin(theta) + point.y * cos(theta) + y; + + return p; +} + +inline geometry_msgs::Point32 transformPoint(const geometry_msgs::Point32& point, double x, double y, double theta) { + geometry_msgs::Point32 p; + + p.x = point.x * cos(theta) - point.y * sin(theta) + x; + p.y = point.x * sin(theta) + point.y * cos(theta) + y; + + return p; +} + +inline Point transformPoint(const Point point, double x, double y, double theta) { + Point p; + + p.x = point.x * cos(theta) - point.y * sin(theta) + x; + p.y = point.x * sin(theta) + point.y * cos(theta) + y; + + return p; +} + +inline bool checkPointInLimits(const geometry_msgs::Point32& p, double x_min, double x_max, double y_min, double y_max) { + if ((p.x > x_max) || (p.x < x_min) || (p.y > y_max) || (p.y < y_min)) + return false; + else + return true; +} + +} // namespace obstacle_detector diff --git a/include/obstacle_detector/utilities/point.h b/include/obstacle_detector/utilities/point.h new file mode 100644 index 0000000..f325242 --- /dev/null +++ b/include/obstacle_detector/utilities/point.h @@ -0,0 +1,90 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include +#include + +namespace obstacle_detector +{ + +class Point +{ +public: + Point(double x = 0.0, double y = 0.0) : x(x), y(y) {} + Point(const Point& p) : x(p.x), y(p.y) {} + static Point fromPoolarCoords(const double r, const double phi) { return Point(r * cos(phi), r * sin(phi)); } + + double length() const { return sqrt(pow(x, 2.0) + pow(y, 2.0)); } + double lengthSquared() const { return pow(x, 2.0) + pow(y, 2.0); } + double angle() const { return atan2(y, x); } + double angleDeg() const { return 180.0 * atan2(y, x) / M_PI; } + double dot(const Point& p) const { return x * p.x + y * p.y; } + double cross(const Point& p) const { return x * p.y - y * p.x; } + + Point normalized() { return (length() > 0.0) ? *this / length() : *this; } + Point reflected(const Point& normal) const { return *this - 2.0 * normal * (normal.dot(*this)); } + Point perpendicular() const { return Point(-y, x); } + + friend Point operator+ (const Point& p1, const Point& p2) { return Point(p1.x + p2.x, p1.y + p2.y); } + friend Point operator- (const Point& p1, const Point& p2) { return Point(p1.x - p2.x, p1.y - p2.y); } + friend Point operator* (const double f, const Point& p) { return Point(f * p.x, f * p.y); } + friend Point operator* (const Point& p, const double f) { return Point(f * p.x, f * p.y); } + friend Point operator/ (const Point& p, const double f) { return (f != 0.0) ? Point(p.x / f, p.y / f) : Point(); } + + Point operator- () { return Point(-x, -y); } + Point operator+ () { return Point( x, y); } + + Point& operator= (const Point& p) { if (this != &p) { x = p.x; y = p.y; } return *this; } + Point& operator+= (const Point& p) { x += p.x; y += p.y; return *this; } + Point& operator-= (const Point& p) { x -= p.x; y -= p.y; return *this; } + + friend bool operator== (const Point& p1, const Point& p2) { return (p1.x == p2.x && p1.y == p2.y); } + friend bool operator!= (const Point& p1, const Point& p2) { return !(p1 == p2); } + friend bool operator< (const Point& p1, const Point& p2) { return (p1.lengthSquared() < p2.lengthSquared()); } + friend bool operator<= (const Point& p1, const Point& p2) { return (p1.lengthSquared() <= p2.lengthSquared()); } + friend bool operator> (const Point& p1, const Point& p2) { return (p1.lengthSquared() > p2.lengthSquared()); } + friend bool operator>= (const Point& p1, const Point& p2) { return (p1.lengthSquared() >= p2.lengthSquared()); } + friend bool operator! (const Point& p1) { return (p1.x == 0.0 && p1.y == 0.0); } + + friend std::ostream& operator<<(std::ostream& out, const Point& p) + { out << "(" << p.x << ", " << p.y << ")"; return out; } + + double x; + double y; +}; + +} // namespace obstacle_detector diff --git a/include/obstacle_detector/utilities/point_set.h b/include/obstacle_detector/utilities/point_set.h new file mode 100644 index 0000000..57ab58d --- /dev/null +++ b/include/obstacle_detector/utilities/point_set.h @@ -0,0 +1,57 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include + +#include "obstacle_detector/utilities/point.h" + +namespace obstacle_detector +{ + +typedef std::list::iterator PointIterator; + +class PointSet +{ +public: + PointSet() { num_points = 0; } + + PointIterator begin, end; // The iterators point to the list of points existing somewhere else + int num_points; +}; + +} // namespace obstacle_detector + diff --git a/include/obstacle_detector/utilities/segment.h b/include/obstacle_detector/utilities/segment.h new file mode 100644 index 0000000..26ba730 --- /dev/null +++ b/include/obstacle_detector/utilities/segment.h @@ -0,0 +1,121 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include + +#include "obstacle_detector/utilities/point.h" +#include "obstacle_detector/utilities/point_set.h" + +namespace obstacle_detector +{ + +class Segment +{ +public: + Segment(const Point& p1 = Point(), const Point& p2 = Point()) { + // Swap if not counter-clockwise + if (p1.cross(p2) > 0.0) + first_point = p1, last_point = p2; + else + first_point = p2, last_point = p1; + } + + double length() const { + return (last_point - first_point).length(); + } + + double lengthSquared() const { + return (last_point - first_point).lengthSquared(); + } + + Point normal() const { + return (last_point - first_point).perpendicular().normalized(); + } + + Point projection(const Point& p) const { + Point a = last_point - first_point; + Point b = p - first_point; + return first_point + a.dot(b) * a / a.lengthSquared(); + } + + Point trueProjection(const Point& p) const { + Point a = last_point - first_point; + Point b = p - first_point; + Point c = p - last_point; + + double t = a.dot(b) / a.lengthSquared(); + + if (t < 0.0) + return (first_point); + else if (t > 1.0) + return (last_point); + else + return first_point + a.dot(b) * a / a.lengthSquared(); + } + + double distanceTo(const Point& p) const { + return (p - projection(p)).length(); + } + + double trueDistanceTo(const Point& p) const { + Point a = last_point - first_point; + Point b = p - first_point; + Point c = p - last_point; + + double t = a.dot(b) / a.lengthSquared(); + + if (t < 0.0) + return b.length(); + else if (t > 1.0) + return c.length(); + + Point projection = first_point + t * a; + return (p - projection).length(); + } + + + friend std::ostream& operator<<(std::ostream& out, const Segment& s) { + out << "p1: " << s.first_point << ", p2: " << s.last_point; + return out; + } + + Point first_point; + Point last_point; + std::vector point_sets; +}; + +} // namespace obstacle_detector diff --git a/include/obstacle_detector/utilities/tracked_obstacle.h b/include/obstacle_detector/utilities/tracked_obstacle.h new file mode 100644 index 0000000..67dfc2a --- /dev/null +++ b/include/obstacle_detector/utilities/tracked_obstacle.h @@ -0,0 +1,180 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#pragma once + +#include +#include "obstacle_detector/utilities/kalman.h" + +namespace obstacle_detector +{ + +class TrackedObstacle { +public: + TrackedObstacle(const CircleObstacle& obstacle) : obstacle_(obstacle), kf_x_(0, 1, 2), kf_y_(0, 1, 2), kf_r_(0, 1, 2) { + fade_counter_ = s_fade_counter_size_; + initKF(); + } + + void predictState() { + kf_x_.predictState(); + kf_y_.predictState(); + kf_r_.predictState(); + + obstacle_.center.x = kf_x_.q_pred(0); + obstacle_.center.y = kf_y_.q_pred(0); + + obstacle_.velocity.x = kf_x_.q_pred(1); + obstacle_.velocity.y = kf_y_.q_pred(1); + + obstacle_.radius = kf_r_.q_pred(0); + + fade_counter_--; + } + + void correctState(const CircleObstacle& new_obstacle) { + kf_x_.y(0) = new_obstacle.center.x; + kf_y_.y(0) = new_obstacle.center.y; + kf_r_.y(0) = new_obstacle.radius; + + kf_x_.correctState(); + kf_y_.correctState(); + kf_r_.correctState(); + + obstacle_.center.x = kf_x_.q_est(0); + obstacle_.center.y = kf_y_.q_est(0); + + obstacle_.velocity.x = kf_x_.q_est(1); + obstacle_.velocity.y = kf_y_.q_est(1); + + obstacle_.radius = kf_r_.q_est(0); + + fade_counter_ = s_fade_counter_size_; + } + + void updateState() { + kf_x_.predictState(); + kf_y_.predictState(); + kf_r_.predictState(); + + kf_x_.correctState(); + kf_y_.correctState(); + kf_r_.correctState(); + + obstacle_.center.x = kf_x_.q_est(0); + obstacle_.center.y = kf_y_.q_est(0); + + obstacle_.velocity.x = kf_x_.q_est(1); + obstacle_.velocity.y = kf_y_.q_est(1); + + obstacle_.radius = kf_r_.q_est(0); + + fade_counter_--; + } + + static void setSamplingTime(double tp) { + s_sampling_time_ = tp; + } + + static void setCounterSize(int size) { + s_fade_counter_size_ = size; + } + + static void setCovariances(double process_var, double process_rate_var, double measurement_var) { + s_process_variance_ = process_var; + s_process_rate_variance_ = process_rate_var; + s_measurement_variance_ = measurement_var; + } + + bool hasFaded() const { return ((fade_counter_ <= 0) ? true : false); } + const CircleObstacle& getObstacle() const { return obstacle_; } + const KalmanFilter& getKFx() const { return kf_x_; } + const KalmanFilter& getKFy() const { return kf_y_; } + const KalmanFilter& getKFr() const { return kf_r_; } + +private: + void initKF() { + kf_x_.A(0, 1) = s_sampling_time_; + kf_y_.A(0, 1) = s_sampling_time_; + kf_r_.A(0, 1) = s_sampling_time_; + + kf_x_.C(0, 0) = 1.0; + kf_y_.C(0, 0) = 1.0; + kf_r_.C(0, 0) = 1.0; + + kf_x_.R(0, 0) = s_measurement_variance_; + kf_y_.R(0, 0) = s_measurement_variance_; + kf_r_.R(0, 0) = s_measurement_variance_; + + kf_x_.Q(0, 0) = s_process_variance_; + kf_r_.Q(0, 0) = s_process_variance_; + kf_y_.Q(0, 0) = s_process_variance_; + + kf_x_.Q(1, 1) = s_process_rate_variance_; + kf_y_.Q(1, 1) = s_process_rate_variance_; + kf_r_.Q(1, 1) = s_process_rate_variance_; + + kf_x_.q_pred(0) = obstacle_.center.x; + kf_r_.q_pred(0) = obstacle_.radius; + kf_y_.q_pred(0) = obstacle_.center.y; + + kf_x_.q_pred(1) = obstacle_.velocity.x; + kf_y_.q_pred(1) = obstacle_.velocity.y; + + kf_x_.q_est(0) = obstacle_.center.x; + kf_r_.q_est(0) = obstacle_.radius; + kf_y_.q_est(0) = obstacle_.center.y; + + kf_x_.q_est(1) = obstacle_.velocity.x; + kf_y_.q_est(1) = obstacle_.velocity.y; + } + + CircleObstacle obstacle_; + + KalmanFilter kf_x_; + KalmanFilter kf_y_; + KalmanFilter kf_r_; + + int fade_counter_; + + // Common variables + static int s_fade_counter_size_; + static double s_sampling_time_; + static double s_process_variance_; + static double s_process_rate_variance_; + static double s_measurement_variance_; +}; + +} diff --git a/launch/demo.launch b/launch/demo.launch new file mode 100644 index 0000000..26085fd --- /dev/null +++ b/launch/demo.launch @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [-1.0, 0.0, 1.0] + [0.0, -1.0, 0.0] + [0.5, 0.5, 0.5] + [0.0, 0.0, 0.0] + [0.0, 0.0, 0.0] + + + + + + + + + diff --git a/launch/nodelets.launch b/launch/nodelets.launch new file mode 100644 index 0000000..902f7cf --- /dev/null +++ b/launch/nodelets.launch @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [-1.0, 0.0, 1.0] + [0.0, -1.0, 0.0] + [0.5, 0.5, 0.5] + [0.0, 0.0, 0.0] + [0.0, 0.0, 0.0] + + + + diff --git a/launch/nodes.launch b/launch/nodes.launch new file mode 100644 index 0000000..5248fa0 --- /dev/null +++ b/launch/nodes.launch @@ -0,0 +1,109 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [-3.0, -2.5, -2.5, -1.0, -1.0, -0.5, 2.5, 0.2, 2.0, 4.5, 4.0, 1.5] + [1.5, 0.0, -2.5, 3.0, 1.0, -4.0, -3.0, -0.9, 0.0, 0.0, 2.0, 2.0] + [0.5, 0.5, 1.5, 0.5, 0.7, 0.5, 1.5, 0.7, 0.7, 1.0, 0.5, 1.0] + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + + + diff --git a/msg/CircleObstacle.msg b/msg/CircleObstacle.msg new file mode 100644 index 0000000..aaa10bd --- /dev/null +++ b/msg/CircleObstacle.msg @@ -0,0 +1,4 @@ +geometry_msgs/Point center # Central point [m] +geometry_msgs/Vector3 velocity # Linear velocity [m/s] +float64 radius # Radius with added margin [m] +float64 true_radius # True measured radius [m] diff --git a/msg/Obstacles.msg b/msg/Obstacles.msg new file mode 100644 index 0000000..ae63b42 --- /dev/null +++ b/msg/Obstacles.msg @@ -0,0 +1,4 @@ +Header header + +obstacle_detector/SegmentObstacle[] segments +obstacle_detector/CircleObstacle[] circles diff --git a/msg/SegmentObstacle.msg b/msg/SegmentObstacle.msg new file mode 100644 index 0000000..3b79ab7 --- /dev/null +++ b/msg/SegmentObstacle.msg @@ -0,0 +1,2 @@ +geometry_msgs/Point first_point # First point of the segment [m] +geometry_msgs/Point last_point # Last point of the segment [m] diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml new file mode 100644 index 0000000..b9c17e2 --- /dev/null +++ b/nodelet_plugins.xml @@ -0,0 +1,43 @@ + + + + + Scan Rectifier Nodelet + + + + + + Scans Merger Nodelet + + + + + + Obstacle Extractor Nodelet + + + + + + Obstacle Tracker Nodelet + + + + + + Obstacle Publisher Nodelet + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..f774997 --- /dev/null +++ b/package.xml @@ -0,0 +1,29 @@ + + + obstacle_detector + 1.0.0 + Detect obstacles in form of line segments and circles from 2D laser scans. + Mateusz Przybyla + Mateusz Przybyla + BSD + + catkin + message_generation + message_runtime + + tf + rviz + roscpp + roslaunch + nodelet + std_msgs + nav_msgs + sensor_msgs + geometry_msgs + std_srvs + + + + + + diff --git a/resources/ObstacleDetector.pdf b/resources/ObstacleDetector.pdf new file mode 100644 index 0000000..a284f2b Binary files /dev/null and b/resources/ObstacleDetector.pdf differ diff --git a/resources/Using hokuyo_node with udev.txt b/resources/Using hokuyo_node with udev.txt new file mode 100644 index 0000000..22ee924 --- /dev/null +++ b/resources/Using hokuyo_node with udev.txt @@ -0,0 +1,36 @@ +How to use multiple Hokuyo laser scanners and determine which is which? +---- + +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: + +1. Create a file with an udev rule in the folder /etc/udev/rules.d: + +sudo gedit /etc/udev/rules.d/47-hokuyo.rules + +(the name and number is arbitrary but there should not be two rules with the same number). + +2. Fill the file with the following rule (just paste the following in the file): + +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" + +(mind that the user should be a member of dialout group: sudo adduser $USER dialout). + +3. Create a file named run.sh in the /etc/ros/ folder and provide it with executable rights: + +sudo touch /etc/ros/run.sh +sudo chmod +x /etc/ros/run.sh + +4. Fill the file with the following: + +#!/bin/sh +. /opt/ros/hydro/setup.sh +rosrun $@ + +(change the distribution of ROS to the one that you use - it was Hydromedusa in my case). + +5. Refresh the udev rules list with: + +sudo udevadm control --reload-rules + +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! + diff --git a/resources/obstacle_detector.rviz b/resources/obstacle_detector.rviz new file mode 100644 index 0000000..f91754a --- /dev/null +++ b/resources/obstacle_detector.rviz @@ -0,0 +1,272 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 931 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Merged Scan + - Class: Scans Merger + Name: Scans Merger + - Class: Obstacle Extractor + Name: Obstacle Extractor + - Class: Obstacle Tracker + Name: Obstacle Tracker + - Class: Obstacle Publisher + Name: Obstacle Publisher +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: map + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Front Scanner + Position Transformer: XYZ + Queue Size: 10 + Selectable: false + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /front_scan + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Rear Scanner + Position Transformer: XYZ + Queue Size: 10 + Selectable: false + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /rear_scan + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Merged Scan + Position Transformer: XYZ + Queue Size: 10 + Selectable: false + Size (Pixels): 5 + Size (m): 0.00999999978 + Style: Points + Topic: /scan + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud + Color: 255; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Merged PCL + Position Transformer: XYZ + Queue Size: 10 + Selectable: false + Size (Pixels): 5 + Size (m): 0.00999999978 + Style: Points + Topic: /pcl + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: false + - Circles color: 170; 0; 0 + Class: Obstacles + Enabled: true + Margin color: 0; 170; 0 + Name: Obstacles + Opacity: 0.75 + Segments color: 170; 170; 0 + Segments thickness: 0.0299999993 + Topic: /obstacles + Unreliable: true + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base: + Value: true + front_scanner: + Value: true + map: + Value: true + rear_scanner: + Value: true + scanner_base: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + base: + scanner_base: + front_scanner: + {} + rear_scanner: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Angle: 0 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Name: Current View + Near Clip Distance: 0.00999999978 + Scale: 62.1297035 + Target Frame: map + Value: TopDownOrtho (rviz) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + Obstacle Extractor: + collapsed: false + Obstacle Publisher: + collapsed: false + Obstacle Tracker: + collapsed: false + QMainWindow State: 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 + Scans Merger: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Views: + collapsed: false + Width: 1855 + X: 65 + Y: 24 diff --git a/resources/play.png b/resources/play.png new file mode 100644 index 0000000..1a7d96a Binary files /dev/null and b/resources/play.png differ diff --git a/resources/scans_demo.bag b/resources/scans_demo.bag new file mode 100644 index 0000000..27c2fc8 Binary files /dev/null and b/resources/scans_demo.bag differ diff --git a/resources/stop.png b/resources/stop.png new file mode 100644 index 0000000..dbdbb34 Binary files /dev/null and b/resources/stop.png differ diff --git a/rviz_plugins.xml b/rviz_plugins.xml new file mode 100644 index 0000000..bfd46f4 --- /dev/null +++ b/rviz_plugins.xml @@ -0,0 +1,44 @@ + + + + + Scans Merger Panel. + + + + + + Obstacle Extractor Panel. + + + + + + Obstacle Tracker Panel. + + + + + + Obstacle Publisher Panel. + + + + + + Obstacles Display + + obstacle_detector/Obstacles + + + diff --git a/src/displays/circle_visual.cpp b/src/displays/circle_visual.cpp new file mode 100644 index 0000000..6d71a2f --- /dev/null +++ b/src/displays/circle_visual.cpp @@ -0,0 +1,94 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/displays/circle_visual.h" + +namespace obstacles_display +{ + +CircleVisual::CircleVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node) { + scene_manager_ = scene_manager; + frame_node_1_ = parent_node->createChildSceneNode(); + frame_node_2_ = parent_node->createChildSceneNode(); + + obstacle_.reset(new rviz::Shape(rviz::Shape::Cylinder, scene_manager_, frame_node_1_)); + margin_.reset(new rviz::Shape(rviz::Shape::Cylinder, scene_manager_, frame_node_2_)); +} + +CircleVisual::~CircleVisual() { + scene_manager_->destroySceneNode(frame_node_1_); + scene_manager_->destroySceneNode(frame_node_2_); +} + +void CircleVisual::setData(const obstacle_detector::CircleObstacle& circle) { + Ogre::Vector3 pos(circle.center.x, circle.center.y, 0.25); + obstacle_->setPosition(pos); + + Ogre::Vector3 true_scale(2.0 * circle.true_radius, 0.1, 2.0 * circle.true_radius); + obstacle_->setScale(true_scale); + + Ogre::Vector3 pos2(circle.center.x, circle.center.y, 0.1); + margin_->setPosition(pos2); + + Ogre::Vector3 scale(2.0 * circle.radius, 0.2, 2.0 * circle.radius); + margin_->setScale(scale); + + Ogre::Vector3 dir(Ogre::Real(1.0), Ogre::Real(0.0), Ogre::Real(0.0)); + Ogre::Radian angle(Ogre::Real(M_PI_2)); + Ogre::Quaternion q(angle, dir); + obstacle_->setOrientation(q); + margin_->setOrientation(q); +} + +void CircleVisual::setFramePosition(const Ogre::Vector3& position) { + frame_node_1_->setPosition(position); + frame_node_2_->setPosition(position); +} + +void CircleVisual::setFrameOrientation(const Ogre::Quaternion& orientation) { + frame_node_1_->setOrientation(orientation); + frame_node_2_->setOrientation(orientation); +} + +void CircleVisual::setMainColor(float r, float g, float b, float a) { + obstacle_->setColor(r, g, b, a); +} + +void CircleVisual::setMarginColor(float r, float g, float b, float a) { + margin_->setColor(r, g, b, a); +} + +} // end namespace obstacles_display + diff --git a/src/displays/obstacles_display.cpp b/src/displays/obstacles_display.cpp new file mode 100644 index 0000000..c905c4b --- /dev/null +++ b/src/displays/obstacles_display.cpp @@ -0,0 +1,133 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/displays/obstacles_display.h" + +namespace obstacles_display +{ + +ObstaclesDisplay::ObstaclesDisplay() { + circle_color_property_ = new rviz::ColorProperty("Circles color", QColor(170, 0, 0), "Color of circles.", this, SLOT(updateCircleColor())); + margin_color_property_ = new rviz::ColorProperty("Margin color", QColor(0, 170, 0), "Color of margin added around circles.", this, SLOT(updateCircleColor())); + segment_color_property_ = new rviz::ColorProperty("Segments color", QColor(170, 170, 0), "Color of segments.", this, SLOT(updateSegmentColor())); + alpha_property_ = new rviz::FloatProperty("Opacity", 0.75, "Value 0,0 is fully transparent, 1,0 is fully opaque.", this, SLOT(updateAlpha())); + thickness_property_ = new rviz::FloatProperty("Segments thickness", 0.03f, "Width of the segments in meters.", this, SLOT(updateThickness())); +} + +void ObstaclesDisplay::onInitialize() { + MessageFilterDisplay::onInitialize(); +} + +ObstaclesDisplay::~ObstaclesDisplay() {} + +void ObstaclesDisplay::reset() { + MessageFilterDisplay::reset(); + circle_visuals_.clear(); + segment_visuals_.clear(); +} + +void ObstaclesDisplay::updateCircleColor() { + float alpha = alpha_property_->getFloat(); + Ogre::ColourValue main_color = circle_color_property_->getOgreColor(); + Ogre::ColourValue margin_color = margin_color_property_->getOgreColor(); + + for (auto& c : circle_visuals_) { + c->setMainColor(main_color.r, main_color.g, main_color.b, alpha); + c->setMarginColor(margin_color.r, margin_color.g, margin_color.b, alpha); + } +} + +void ObstaclesDisplay::updateSegmentColor() { + float alpha = alpha_property_->getFloat(); + Ogre::ColourValue color = segment_color_property_->getOgreColor(); + + for (auto& s : segment_visuals_) + s->setColor(color.r, color.g, color.b, alpha); +} + +void ObstaclesDisplay::updateAlpha() { + updateCircleColor(); + updateSegmentColor(); +} + +void ObstaclesDisplay::updateThickness() { + float width = thickness_property_->getFloat(); + + for (auto& s : segment_visuals_) + s->setWidth(width); +} + +void ObstaclesDisplay::processMessage(const obstacle_detector::Obstacles::ConstPtr& obstacles_msg) { + circle_visuals_.clear(); + segment_visuals_.clear(); + + Ogre::Quaternion orientation; + Ogre::Vector3 position; + if (!context_->getFrameManager()->getTransform(obstacles_msg->header.frame_id, obstacles_msg->header.stamp, position, orientation)) { + ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", obstacles_msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); + return; + } + + for (const auto& circle : obstacles_msg->circles) { + boost::shared_ptr visual; + visual.reset(new CircleVisual(context_->getSceneManager(), scene_node_)); + + visual->setData(circle); + visual->setFramePosition(position); + visual->setFrameOrientation(orientation); + + circle_visuals_.push_back(visual); + } + + for (const auto& segment : obstacles_msg->segments) { + boost::shared_ptr visual; + visual.reset(new SegmentVisual(context_->getSceneManager(), scene_node_)); + + visual->setData(segment); + visual->setFramePosition(position); + visual->setFrameOrientation(orientation); + + segment_visuals_.push_back(visual); + } + + updateAlpha(); + updateThickness(); +} + +} // end namespace obstacles_display + +#include +PLUGINLIB_EXPORT_CLASS(obstacles_display::ObstaclesDisplay, rviz::Display) + diff --git a/src/displays/segment_visual.cpp b/src/displays/segment_visual.cpp new file mode 100644 index 0000000..1921ff9 --- /dev/null +++ b/src/displays/segment_visual.cpp @@ -0,0 +1,76 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/displays/segment_visual.h" + +namespace obstacles_display +{ + +SegmentVisual::SegmentVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node) { + scene_manager_ = scene_manager; + frame_node_ = parent_node->createChildSceneNode(); + + line_.reset(new rviz::BillboardLine(scene_manager_, frame_node_)); +} + +SegmentVisual::~SegmentVisual() { + scene_manager_->destroySceneNode(frame_node_); +} + +void SegmentVisual::setData(const obstacle_detector::SegmentObstacle& segment) { + Ogre::Vector3 p1(segment.first_point.x, segment.first_point.y, 0.0); + Ogre::Vector3 p2(segment.last_point.x, segment.last_point.y, 0.0); + line_->addPoint(p1); + line_->addPoint(p2); +} + +void SegmentVisual::setFramePosition(const Ogre::Vector3& position) { + frame_node_->setPosition(position); +} + +void SegmentVisual::setFrameOrientation(const Ogre::Quaternion& orientation) { + frame_node_->setOrientation(orientation); +} + +void SegmentVisual::setColor(float r, float g, float b, float a) { + line_->setColor(r, g, b, a); +} + +void SegmentVisual::setWidth(float w) { + line_->setLineWidth(w); +} + +} // end namespace obstacles_display + diff --git a/src/nodelets/obstacle_extractor_nodelet.cpp b/src/nodelets/obstacle_extractor_nodelet.cpp new file mode 100644 index 0000000..230b3ae --- /dev/null +++ b/src/nodelets/obstacle_extractor_nodelet.cpp @@ -0,0 +1,67 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include +#include + +#include "obstacle_detector/obstacle_extractor.h" + +namespace obstacle_detector +{ + +class ObstacleExtractorNodelet : public nodelet::Nodelet +{ +public: + virtual void onInit() { + NODELET_INFO("Initializing Obstacle Extractor Nodelet"); + ros::NodeHandle nh = getMTNodeHandle(); + ros::NodeHandle nh_local = getPrivateNodeHandle(); + + try { + obstacle_extractor_ = std::shared_ptr(new ObstacleExtractor(nh, nh_local)); + } + catch (std::string s) { + NODELET_ERROR_STREAM(s); + } + } + +private: + std::shared_ptr obstacle_extractor_; +}; + +} // namespace obstacle_detector + +#include +PLUGINLIB_EXPORT_CLASS(obstacle_detector::ObstacleExtractorNodelet, nodelet::Nodelet) diff --git a/src/nodelets/obstacle_publisher_nodelet.cpp b/src/nodelets/obstacle_publisher_nodelet.cpp new file mode 100644 index 0000000..feebc83 --- /dev/null +++ b/src/nodelets/obstacle_publisher_nodelet.cpp @@ -0,0 +1,67 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include +#include + +#include "obstacle_detector/obstacle_publisher.h" + +namespace obstacle_detector +{ + +class ObstaclePublisherNodelet : public nodelet::Nodelet +{ +public: + virtual void onInit() { + NODELET_INFO("Initializing Obstacle Publisher Nodelet"); + ros::NodeHandle nh = getMTNodeHandle(); + ros::NodeHandle nh_local = getPrivateNodeHandle(); + + try { + obstacle_publisher_ = std::shared_ptr(new ObstaclePublisher(nh, nh_local)); + } + catch (std::string s) { + NODELET_ERROR_STREAM(s); + } + } + +private: + std::shared_ptr obstacle_publisher_; +}; + +} // namespace obstacle_detector + +#include +PLUGINLIB_EXPORT_CLASS(obstacle_detector::ObstaclePublisherNodelet, nodelet::Nodelet) diff --git a/src/nodelets/obstacle_tracker_nodelet.cpp b/src/nodelets/obstacle_tracker_nodelet.cpp new file mode 100644 index 0000000..415b39c --- /dev/null +++ b/src/nodelets/obstacle_tracker_nodelet.cpp @@ -0,0 +1,67 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include +#include + +#include "obstacle_detector/obstacle_tracker.h" + +namespace obstacle_detector +{ + +class ObstacleTrackerNodelet : public nodelet::Nodelet +{ +public: + virtual void onInit() { + NODELET_INFO("Initializing Obstacle Tracker Nodelet"); + ros::NodeHandle nh = getMTNodeHandle(); + ros::NodeHandle nh_local = getPrivateNodeHandle(); + + try { + obstacle_tracker_ = std::shared_ptr(new ObstacleTracker(nh, nh_local)); + } + catch (std::string s) { + NODELET_ERROR_STREAM(s); + } + } + +private: + std::shared_ptr obstacle_tracker_; +}; + +} // namespace obstacle_detector + +#include +PLUGINLIB_EXPORT_CLASS(obstacle_detector::ObstacleTrackerNodelet, nodelet::Nodelet) diff --git a/src/nodelets/scan_rectifier_nodelet.cpp b/src/nodelets/scan_rectifier_nodelet.cpp new file mode 100644 index 0000000..a95376c --- /dev/null +++ b/src/nodelets/scan_rectifier_nodelet.cpp @@ -0,0 +1,67 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include +#include + +#include "obstacle_detector/scan_rectifier.h" + +namespace obstacle_detector +{ + +class ScanRectifierNodelet : public nodelet::Nodelet +{ +public: + virtual void onInit() { + NODELET_INFO("Initializing Scan Rectifier Nodelet"); + ros::NodeHandle nh = getMTNodeHandle(); + ros::NodeHandle nh_local = getPrivateNodeHandle(); + + try { + scan_rectifier_ = std::shared_ptr(new ScanRectifier(nh, nh_local)); + } + catch(std::string s) { + NODELET_ERROR_STREAM(s); + } + } + +private: + std::shared_ptr scan_rectifier_; +}; + +} // namespace obstacle_detector + +#include +PLUGINLIB_EXPORT_CLASS(obstacle_detector::ScanRectifierNodelet, nodelet::Nodelet) diff --git a/src/nodelets/scans_merger_nodelet.cpp b/src/nodelets/scans_merger_nodelet.cpp new file mode 100644 index 0000000..4d44dc9 --- /dev/null +++ b/src/nodelets/scans_merger_nodelet.cpp @@ -0,0 +1,67 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include +#include + +#include "obstacle_detector/scans_merger.h" + +namespace obstacle_detector +{ + +class ScansMergerNodelet : public nodelet::Nodelet +{ +public: + virtual void onInit() { + NODELET_INFO("Initializing Scans Merger Nodelet"); + ros::NodeHandle nh = getMTNodeHandle(); + ros::NodeHandle nh_local = getPrivateNodeHandle(); + + try { + scans_merger_ = std::shared_ptr(new ScansMerger(nh, nh_local)); + } + catch (std::string s) { + NODELET_ERROR_STREAM(s); + } + } + +private: + std::shared_ptr scans_merger_; +}; + +} // namespace obstacle_detector + +#include +PLUGINLIB_EXPORT_CLASS(obstacle_detector::ScansMergerNodelet, nodelet::Nodelet) diff --git a/src/nodes/obstacle_extractor_node.cpp b/src/nodes/obstacle_extractor_node.cpp new file mode 100644 index 0000000..23c8eff --- /dev/null +++ b/src/nodes/obstacle_extractor_node.cpp @@ -0,0 +1,54 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/obstacle_extractor.h" + +using namespace obstacle_detector; + +int main(int argc, char** argv) { + ros::init(argc, argv, "obstacle_extractor", ros::init_options::NoRosout); + ros::NodeHandle nh(""); + ros::NodeHandle nh_local("~"); + + try { + ObstacleExtractor od(nh, nh_local); + ros::spin(); + } + catch (std::string s) { + ROS_ERROR_STREAM(s); + } + + return 0; +} diff --git a/src/nodes/obstacle_publisher_node.cpp b/src/nodes/obstacle_publisher_node.cpp new file mode 100644 index 0000000..ef84c1c --- /dev/null +++ b/src/nodes/obstacle_publisher_node.cpp @@ -0,0 +1,54 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/obstacle_publisher.h" + +using namespace obstacle_detector; + +int main(int argc, char** argv) { + ros::init(argc, argv, "obstacle_publisher", ros::init_options::NoRosout); + ros::NodeHandle nh(""); + ros::NodeHandle nh_local("~"); + + try { + ObstaclePublisher op(nh, nh_local); + ros::spin(); + } + catch (std::string s) { + ROS_ERROR_STREAM(s); + } + + return 0; +} diff --git a/src/nodes/obstacle_tracker_node.cpp b/src/nodes/obstacle_tracker_node.cpp new file mode 100644 index 0000000..2bc0da4 --- /dev/null +++ b/src/nodes/obstacle_tracker_node.cpp @@ -0,0 +1,54 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/obstacle_tracker.h" + +using namespace obstacle_detector; + +int main(int argc, char** argv) { + ros::init(argc, argv, "obstacle_tracker", ros::init_options::NoRosout); + ros::NodeHandle nh(""); + ros::NodeHandle nh_local("~"); + + try { + ObstacleTracker ot(nh, nh_local); + ros::spin(); + } + catch (std::string s) { + ROS_ERROR_STREAM(s); + } + + return 0; +} diff --git a/src/nodes/scan_rectifier_node.cpp b/src/nodes/scan_rectifier_node.cpp new file mode 100644 index 0000000..dce3649 --- /dev/null +++ b/src/nodes/scan_rectifier_node.cpp @@ -0,0 +1,54 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/scan_rectifier.h" + +using namespace obstacle_detector; + +int main(int argc, char** argv) { + ros::init(argc, argv, "scan_rectifier", ros::init_options::NoRosout); + ros::NodeHandle nh(""); + ros::NodeHandle nh_local("~"); + + try { + ScanRectifier sr(nh, nh_local); + ros::spin(); + } + catch(std::string s) { + ROS_ERROR_STREAM(s); + } + + return 0; +} diff --git a/src/nodes/scans_merger_node.cpp b/src/nodes/scans_merger_node.cpp new file mode 100644 index 0000000..0eb12e0 --- /dev/null +++ b/src/nodes/scans_merger_node.cpp @@ -0,0 +1,54 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/scans_merger.h" + +using namespace obstacle_detector; + +int main(int argc, char** argv) { + ros::init(argc, argv, "scans_merger", ros::init_options::NoRosout); + ros::NodeHandle nh(""); + ros::NodeHandle nh_local("~"); + + try { + ScansMerger sm(nh, nh_local); + ros::spin(); + } + catch (std::string s) { + ROS_ERROR_STREAM(s); + } + + return 0; +} diff --git a/src/obstacle_extractor.cpp b/src/obstacle_extractor.cpp new file mode 100644 index 0000000..8c7c9cf --- /dev/null +++ b/src/obstacle_extractor.cpp @@ -0,0 +1,404 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/obstacle_extractor.h" +#include "obstacle_detector/utilities/figure_fitting.h" +#include "obstacle_detector/utilities/math_utilities.h" + +using namespace std; +using namespace obstacle_detector; + +ObstacleExtractor::ObstacleExtractor(ros::NodeHandle& nh, ros::NodeHandle& nh_local) : nh_(nh), nh_local_(nh_local) { + p_active_ = false; + + params_srv_ = nh_local_.advertiseService("params", &ObstacleExtractor::updateParams, this); + initialize(); +} + +bool ObstacleExtractor::updateParams(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { + bool prev_active = p_active_; + + nh_local_.param("active", p_active_, true); + nh_local_.param("use_scan", p_use_scan_, true); + nh_local_.param("use_pcl", p_use_pcl_, false); + nh_local_.param("use_split_and_merge", p_use_split_and_merge_, false); + nh_local_.param("discard_converted_segments", p_discard_converted_segments_, true); + nh_local_.param("transform_coordinates", p_transform_coordinates_, true); + + nh_local_.param("min_group_points", p_min_group_points_, 5); + + nh_local_.param("max_group_distance", p_max_group_distance_, 0.100); + nh_local_.param("distance_proportion", p_distance_proportion_, 0.006136); + nh_local_.param("max_split_distance", p_max_split_distance_, 0.070); + nh_local_.param("max_merge_separation", p_max_merge_separation_, 0.150); + nh_local_.param("max_merge_spread", p_max_merge_spread_, 0.070); + nh_local_.param("max_circle_radius", p_max_circle_radius_, 0.500); + nh_local_.param("radius_enlargement", p_radius_enlargement_, 0.100); + + nh_local_.param("min_x_limit", p_min_x_limit_, -10.0); + nh_local_.param("max_x_limit", p_max_x_limit_, 10.0); + nh_local_.param("min_y_limit", p_min_y_limit_, -10.0); + nh_local_.param("max_y_limit", p_max_y_limit_, 10.0); + + nh_local_.param("frame_id", p_frame_id_, "map"); + + if (p_active_ != prev_active) { + if (p_active_) { + if (p_use_scan_) + scan_sub_ = nh_.subscribe("scan", 10, &ObstacleExtractor::scanCallback, this); + else if (p_use_pcl_) + pcl_sub_ = nh_.subscribe("pcl", 10, &ObstacleExtractor::pclCallback, this); + + obstacles_pub_ = nh_.advertise("raw_obstacles", 10); + } + else { + // Send empty message + obstacle_detector::ObstaclesPtr obstacles_msg(new obstacle_detector::Obstacles); + obstacles_msg->header.frame_id = p_frame_id_; + obstacles_msg->header.stamp = ros::Time::now(); + obstacles_pub_.publish(obstacles_msg); + + scan_sub_.shutdown(); + pcl_sub_.shutdown(); + obstacles_pub_.shutdown(); + } + } + + return true; +} + +void ObstacleExtractor::scanCallback(const sensor_msgs::LaserScan::ConstPtr scan_msg) { + base_frame_id_ = scan_msg->header.frame_id; + stamp_ = scan_msg->header.stamp; + + double phi = scan_msg->angle_min; + + for (const float r : scan_msg->ranges) { + if (r >= scan_msg->range_min && r <= scan_msg->range_max) + input_points_.push_back(Point::fromPoolarCoords(r, phi)); + + phi += scan_msg->angle_increment; + } + + processPoints(); +} + +void ObstacleExtractor::pclCallback(const sensor_msgs::PointCloud::ConstPtr pcl_msg) { + base_frame_id_ = pcl_msg->header.frame_id; + stamp_ = pcl_msg->header.stamp; + + for (const geometry_msgs::Point32& point : pcl_msg->points) + input_points_.push_back(Point(point.x, point.y)); + + processPoints(); +} + +void ObstacleExtractor::processPoints() { + segments_.clear(); + circles_.clear(); + + groupPoints(); // Grouping points simultaneously detects segments + mergeSegments(); + + detectCircles(); + mergeCircles(); + + publishObstacles(); + + input_points_.clear(); +} + +void ObstacleExtractor::groupPoints() { + PointSet point_set; + + for (PointIterator point = input_points_.begin(); point != input_points_.end(); ++point) { + if (point_set.num_points != 0) { + double r = (*point).length(); + + if ((*point - *point_set.end).lengthSquared() > pow(p_max_group_distance_ + r * p_distance_proportion_, 2.0)) { + detectSegments(point_set); + point_set.num_points = -1; + advance(point, -1); + } + else + point_set.end = point; + } + else { + point_set.begin = point; + point_set.end = point; + } + + point_set.num_points++; + } + + detectSegments(point_set); // Check the last point set too! +} + +void ObstacleExtractor::detectSegments(const PointSet& point_set) { + if (point_set.num_points < p_min_group_points_) + return; + + Segment segment(*point_set.begin, *point_set.end); // Use Iterative End Point Fit + + if (p_use_split_and_merge_) + segment = fitSegment(point_set); + + PointIterator set_divider; + double max_distance = 0.0; + double distance = 0.0; + + int split_index = 0; // Natural index of splitting point (counting from 1) + int point_index = 0; // Natural index of current point in the set + + // Seek the point of division + for (PointIterator point = point_set.begin; point != point_set.end; ++point) { + ++point_index; + + if ((distance = segment.distanceTo(*point)) >= max_distance) { + double r = (*point).length(); + + if (distance > p_max_split_distance_ + r * p_distance_proportion_) { + max_distance = distance; + set_divider = point; + split_index = point_index; + } + } + } + + // Split the set only if the sub-groups are not 'small' + if (max_distance > 0.0 && split_index > p_min_group_points_ && split_index < point_set.num_points - p_min_group_points_) { + set_divider = input_points_.insert(set_divider, *set_divider); // Clone the dividing point for each group + + PointSet subset1, subset2; + subset1.begin = point_set.begin; + subset1.end = set_divider; + subset1.num_points = split_index; + + subset2.begin = ++set_divider; + subset2.end = point_set.end; + subset2.num_points = point_set.num_points - split_index; + + detectSegments(subset1); + detectSegments(subset2); + } else { // Add the segment + if (!p_use_split_and_merge_) + segment = fitSegment(point_set); + + segments_.push_back(segment); + } +} + +void ObstacleExtractor::mergeSegments() { + for (auto i = segments_.begin(); i != segments_.end(); ++i) { + for (auto j = i; j != segments_.end(); ++j) { + Segment merged_segment; + + if (compareSegments(*i, *j, merged_segment)) { + auto temp_itr = segments_.insert(i, merged_segment); + segments_.erase(i); + segments_.erase(j); + i = --temp_itr; + break; + } + } + } +} + +bool ObstacleExtractor::compareSegments(const Segment& s1, const Segment& s2, Segment& merged_segment) { + if (&s1 == &s2) + return false; + + // Segments must be provided counter-clockwise + if (s1.first_point.cross(s2.first_point) < 0.0) + return compareSegments(s2, s1, merged_segment); + + if (checkSegmentsProximity(s1, s2)) { + vector point_sets; + point_sets.insert(point_sets.end(), s1.point_sets.begin(), s1.point_sets.end()); + point_sets.insert(point_sets.end(), s2.point_sets.begin(), s2.point_sets.end()); + + Segment segment = fitSegment(point_sets); + + if (checkSegmentsCollinearity(segment, s1, s2)) { + merged_segment = segment; + return true; + } + } + + return false; +} + +bool ObstacleExtractor::checkSegmentsProximity(const Segment& s1, const Segment& s2) { + return (s1.trueDistanceTo(s2.first_point) < p_max_merge_separation_ || + s1.trueDistanceTo(s2.last_point) < p_max_merge_separation_ || + s2.trueDistanceTo(s1.first_point) < p_max_merge_separation_ || + s2.trueDistanceTo(s1.last_point) < p_max_merge_separation_); +} + +bool ObstacleExtractor::checkSegmentsCollinearity(const Segment& segment, const Segment& s1, const Segment& s2) { + return (segment.distanceTo(s1.first_point) < p_max_merge_spread_ && + segment.distanceTo(s1.last_point) < p_max_merge_spread_ && + segment.distanceTo(s2.first_point) < p_max_merge_spread_ && + segment.distanceTo(s2.last_point) < p_max_merge_spread_); +} + +void ObstacleExtractor::detectCircles() { + for (auto segment = segments_.begin(); segment != segments_.end(); ++segment) { + Circle circle(*segment); + circle.radius += p_radius_enlargement_; + + if (circle.radius < p_max_circle_radius_) { + circles_.push_back(circle); + + if (p_discard_converted_segments_) { + segment = segments_.erase(segment); + --segment; + } + } + } +} + +void ObstacleExtractor::mergeCircles() { + for (auto i = circles_.begin(); i != circles_.end(); ++i) { + for (auto j = i; j != circles_.end(); ++j) { + Circle merged_circle; + + if (compareCircles(*i, *j, merged_circle)) { + auto temp_itr = circles_.insert(i, merged_circle); + circles_.erase(i); + circles_.erase(j); + i = --temp_itr; + break; + } + } + } +} + +bool ObstacleExtractor::compareCircles(const Circle& c1, const Circle& c2, Circle& merged_circle) { + if (&c1 == &c2) + return false; + + // If circle c1 is fully inside c2 - merge and leave as c2 + if (c2.radius - c1.radius >= (c2.center - c1.center).length()) { + merged_circle = c2; + return true; + } + + // If circle c2 is fully inside c1 - merge and leave as c1 + if (c1.radius - c2.radius >= (c2.center - c1.center).length()) { + merged_circle = c1; + return true; + } + + // If circles intersect and are 'small' - merge + if (c1.radius + c2.radius >= (c2.center - c1.center).length()) { + Point center = c1.center + (c2.center - c1.center) * c1.radius / (c1.radius + c2.radius); + double radius = (c1.center - center).length() + c1.radius; + + Circle circle(center, radius); + circle.radius += max(c1.radius, c2.radius); + + if (circle.radius < p_max_circle_radius_) { + circle.point_sets.insert(circle.point_sets.end(), c1.point_sets.begin(), c1.point_sets.end()); + circle.point_sets.insert(circle.point_sets.end(), c2.point_sets.begin(), c2.point_sets.end()); + merged_circle = circle; + return true; + } + } + + return false; +} + +void ObstacleExtractor::publishObstacles() { + obstacle_detector::ObstaclesPtr obstacles_msg(new obstacle_detector::Obstacles); + obstacles_msg->header.stamp = stamp_; + + if (p_transform_coordinates_) { + tf::StampedTransform transform; + + try { + tf_listener_.waitForTransform(p_frame_id_, base_frame_id_, stamp_, ros::Duration(10.0)); + tf_listener_.lookupTransform(p_frame_id_, base_frame_id_, stamp_, transform); + } + catch (tf::TransformException ex) { + ROS_ERROR_STREAM(ex.what()); + return; + } + + obstacles_msg->header.frame_id = p_frame_id_; + + tf::Vector3 origin = transform.getOrigin(); + double theta = tf::getYaw(transform.getRotation()); + + for (Segment& s : segments_) { + s.first_point = transformPoint(s.first_point, origin.x(), origin.y(), theta); + s.last_point = transformPoint(s.last_point, origin.x(), origin.y(), theta); + } + + for (Circle& c : circles_) + c.center = transformPoint(c.center, origin.x(), origin.y(), theta); + } + else + obstacles_msg->header.frame_id = base_frame_id_; + + + for (const Segment& s : segments_) { + SegmentObstacle segment; + + segment.first_point.x = s.first_point.x; + segment.first_point.y = s.first_point.y; + segment.last_point.x = s.last_point.x; + segment.last_point.y = s.last_point.y; + + obstacles_msg->segments.push_back(segment); + } + + for (const Circle& c : circles_) { + if (c.center.x > p_min_x_limit_ && c.center.x < p_max_x_limit_ && c.center.y > p_min_y_limit_ && c.center.y < p_max_y_limit_) { + CircleObstacle circle; + + circle.center.x = c.center.x; + circle.center.y = c.center.y; + circle.velocity.x = 0.0; + circle.velocity.y = 0.0; + circle.radius = c.radius; + circle.true_radius = c.radius - p_radius_enlargement_; + + obstacles_msg->circles.push_back(circle); + } + } + + obstacles_pub_.publish(obstacles_msg); +} diff --git a/src/obstacle_publisher.cpp b/src/obstacle_publisher.cpp new file mode 100644 index 0000000..2e0a9ad --- /dev/null +++ b/src/obstacle_publisher.cpp @@ -0,0 +1,211 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/obstacle_publisher.h" + +using namespace std; +using namespace obstacle_detector; + +ObstaclePublisher::ObstaclePublisher(ros::NodeHandle& nh, ros::NodeHandle& nh_local) : nh_(nh), nh_local_(nh_local) { + p_active_ = false; + t_ = 0.0; + + timer_ = nh_.createTimer(ros::Duration(1.0), &ObstaclePublisher::timerCallback, this, false, false); + params_srv_ = nh_local_.advertiseService("params", &ObstaclePublisher::updateParams, this); + initialize(); +} + +bool ObstaclePublisher::updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { + bool prev_active = p_active_; + + nh_local_.param("active", p_active_, false); + nh_local_.param("reset", p_reset_, false); + nh_local_.param("fusion_example", p_fusion_example_, false); + nh_local_.param("fission_example", p_fission_example_, false); + + nh_local_.param("loop_rate", p_loop_rate_, 10.0); + nh_local_.param("radius_margin", p_radius_margin_, 0.25); + + nh_local_.getParam("x_vector", p_x_vector_); + nh_local_.getParam("y_vector", p_y_vector_); + nh_local_.getParam("r_vector", p_r_vector_); + + nh_local_.getParam("vx_vector", p_vx_vector_); + nh_local_.getParam("vy_vector", p_vy_vector_); + + nh_local_.getParam("frame_id", p_frame_id_); + + p_sampling_time_ = 1.0 / p_loop_rate_; + timer_.setPeriod(ros::Duration(p_sampling_time_), false); + + if (p_active_ != prev_active) { + if (p_active_) { + obstacle_pub_ = nh_.advertise("obstacles", 10); + timer_.start(); + } + else { + obstacle_pub_.shutdown(); + timer_.stop(); + } + } + + obstacles_.header.frame_id = p_frame_id_; + obstacles_.circles.clear(); + + if (p_x_vector_.size() != p_y_vector_.size() || p_x_vector_.size() != p_r_vector_.size() || + p_x_vector_.size() != p_vx_vector_.size() || p_x_vector_.size() != p_vy_vector_.size()) + return false; + + for (int idx = 0; idx < p_x_vector_.size(); ++idx) { + CircleObstacle circle; + circle.center.x = p_x_vector_[idx]; + circle.center.y = p_y_vector_[idx]; + circle.radius = p_r_vector_[idx]; + circle.true_radius = p_r_vector_[idx] - p_radius_margin_;; + + circle.velocity.x = p_vx_vector_[idx]; + circle.velocity.y = p_vy_vector_[idx]; + + obstacles_.circles.push_back(circle); + } + + if (p_reset_) + reset(); + + return true; +} + +void ObstaclePublisher::timerCallback(const ros::TimerEvent& e) { + t_ += p_sampling_time_; + + calculateObstaclesPositions(p_sampling_time_); + + if (p_fusion_example_) + fusionExample(t_); + else if (p_fission_example_) + fissionExample(t_); + + publishObstacles(); +} + +void ObstaclePublisher::calculateObstaclesPositions(double dt) { + for (auto& circ : obstacles_.circles) { + circ.center.x += circ.velocity.x * dt; + circ.center.y += circ.velocity.y * dt; + } +} + +void ObstaclePublisher::fusionExample(double t) { + CircleObstacle circ1, circ2; + + obstacles_.circles.clear(); + + if (t < 5.0) { + circ1.center.x = -1.20 + 0.2 * t; + circ1.center.y = 0.0; + circ1.radius = 0.20; + + circ2.center.x = 1.20 - 0.2 * t; + circ2.center.y = 0.0; + circ2.radius = 0.20; + + obstacles_.circles.push_back(circ1); + obstacles_.circles.push_back(circ2); + } + else if (t < 15.0) { + circ1.center.x = 0.0; + circ1.center.y = 0.0; + circ1.radius = 0.20 + 0.20 * exp(-(t - 5.0) / 1.0); + + obstacles_.circles.push_back(circ1); + } + else if (t > 20.0) + reset(); + + circ1.true_radius = circ1.radius; + circ2.true_radius = circ2.radius; +} + +void ObstaclePublisher::fissionExample(double t) { + CircleObstacle circ1, circ2; + + obstacles_.circles.clear(); + + if (t < 5.0) { + circ1.center.x = 0.0; + circ1.center.y = 0.0; + circ1.radius = 0.20; + + obstacles_.circles.push_back(circ1); + } + else if (t < 6.0) { + circ1.center.x = 0.0; + circ1.center.y = 0.0; + circ1.radius = 0.20 + 0.20 * (1.0 - exp(-(t - 5.0) / 1.0)); + + obstacles_.circles.push_back(circ1); + } + else if (t < 16.0){ + circ1.center.x = -0.20 - 0.2 * (t - 6.0); + circ1.center.y = 0.0; + circ1.radius = 0.20; + + circ2.center.x = 0.20 + 0.2 * (t - 6.0); + circ2.center.y = 0.0; + circ2.radius = 0.20; + + obstacles_.circles.push_back(circ1); + obstacles_.circles.push_back(circ2); + } + else if (t > 20.0) + reset(); + + circ1.true_radius = circ1.radius; + circ2.true_radius = circ2.radius; +} + +void ObstaclePublisher::publishObstacles() { + obstacle_detector::ObstaclesPtr obstacles_msg(new obstacle_detector::Obstacles); + *obstacles_msg = obstacles_; + + obstacles_msg->header.stamp = ros::Time::now(); + obstacle_pub_.publish(obstacles_msg); +} + +void ObstaclePublisher::reset() { + t_ = 0.0; + p_reset_ = false; + nh_local_.setParam("reset", false); +} diff --git a/src/obstacle_tracker.cpp b/src/obstacle_tracker.cpp new file mode 100644 index 0000000..d90c852 --- /dev/null +++ b/src/obstacle_tracker.cpp @@ -0,0 +1,475 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/obstacle_tracker.h" + +using namespace obstacle_detector; +using namespace arma; +using namespace std; + +ObstacleTracker::ObstacleTracker(ros::NodeHandle& nh, ros::NodeHandle& nh_local) : nh_(nh), nh_local_(nh_local) { + p_active_ = false; + + timer_ = nh_.createTimer(ros::Duration(1.0), &ObstacleTracker::timerCallback, this, false, false); + params_srv_ = nh_local_.advertiseService("params", &ObstacleTracker::updateParams, this); + + ROS_INFO_STREAM("[Obstacle Tracker]: Waiting for first obstacles."); + boost::shared_ptr obstacles; + obstacles = ros::topic::waitForMessage("raw_obstacles", nh_); + obstacles_.header.frame_id = obstacles->header.frame_id; + ROS_INFO_STREAM("[Obstacle Tracker]: Acquired first obstacles."); + + initialize(); +} + +bool ObstacleTracker::updateParams(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { + bool prev_active = p_active_; + + nh_local_.param("active", p_active_, true); + nh_local_.param("copy_segments", p_copy_segments_, true); + + nh_local_.param("loop_rate", p_loop_rate_, 100.0); + p_sampling_time_ = 1.0 / p_loop_rate_; + p_sensor_rate_ = 10.0; // 10 Hz for Hokuyo + + nh_local_.param("tracking_duration", p_tracking_duration_, 2.0); + nh_local_.param("min_correspondence_cost", p_min_correspondence_cost_, 0.3); + nh_local_.param("std_correspondence_dev", p_std_correspondence_dev_, 0.15); + nh_local_.param("process_variance", p_process_variance_, 0.01); + nh_local_.param("process_rate_variance", p_process_rate_variance_, 0.1); + nh_local_.param("measurement_variance", p_measurement_variance_, 1.0); + + TrackedObstacle::setSamplingTime(p_sampling_time_); + TrackedObstacle::setCounterSize(static_cast(p_loop_rate_ * p_tracking_duration_)); + TrackedObstacle::setCovariances(p_process_variance_, p_process_rate_variance_, p_measurement_variance_); + + timer_.setPeriod(ros::Duration(p_sampling_time_), false); + + if (p_active_ != prev_active) { + if (p_active_) { + obstacles_sub_ = nh_.subscribe("raw_obstacles", 10, &ObstacleTracker::obstaclesCallback, this); + obstacles_pub_ = nh_.advertise("tracked_obstacles", 10); + timer_.start(); + } + else { + // Send empty message + obstacle_detector::ObstaclesPtr obstacles_msg(new obstacle_detector::Obstacles); + obstacles_msg->header.frame_id = obstacles_.header.frame_id; + obstacles_msg->header.stamp = ros::Time::now(); + obstacles_pub_.publish(obstacles_msg); + + obstacles_sub_.shutdown(); + obstacles_pub_.shutdown(); + + tracked_obstacles_.clear(); + untracked_obstacles_.clear(); + + timer_.stop(); + } + } + + return true; +} + +void ObstacleTracker::timerCallback(const ros::TimerEvent&) { + updateObstacles(); + publishObstacles(); +} + +void ObstacleTracker::obstaclesCallback(const obstacle_detector::Obstacles::ConstPtr new_obstacles) { + if (new_obstacles->circles.size() > 0) + radius_margin_ = new_obstacles->circles[0].radius - new_obstacles->circles[0].true_radius; + + if (p_copy_segments_) { + obstacles_.segments.clear(); + obstacles_.segments.assign(new_obstacles->segments.begin(), new_obstacles->segments.end()); + } + + int N = new_obstacles->circles.size(); + int T = tracked_obstacles_.size(); + int U = untracked_obstacles_.size(); + + if (T + U == 0) { + untracked_obstacles_.assign(new_obstacles->circles.begin(), new_obstacles->circles.end()); + return; + } + + mat cost_matrix; + calculateCostMatrix(new_obstacles->circles, cost_matrix); + + vector row_min_indices; + calculateRowMinIndices(cost_matrix, row_min_indices); + + vector col_min_indices; + calculateColMinIndices(cost_matrix, col_min_indices); + + vector used_old_obstacles; + vector used_new_obstacles; + + vector new_tracked_obstacles; + vector new_untracked_obstacles; + + // Check for fusion (only tracked obstacles) + for (int i = 0; i < T-1; ++i) { + if (fusionObstacleUsed(i, col_min_indices, used_new_obstacles, used_old_obstacles)) + continue; + + vector fusion_indices; + fusion_indices.push_back(i); + + for (int j = i+1; j < T; ++j) { + if (fusionObstaclesCorrespond(i, j, col_min_indices, used_old_obstacles)) + fusion_indices.push_back(j); + } + + if (fusion_indices.size() > 1) { + fuseObstacles(fusion_indices, col_min_indices, new_tracked_obstacles, new_obstacles); + + // Mark used old and new obstacles + used_old_obstacles.insert(used_old_obstacles.end(), fusion_indices.begin(), fusion_indices.end()); + used_new_obstacles.push_back(col_min_indices[i]); + } + } + + // Check for fission (only tracked obstacles) + for (int i = 0; i < N-1; ++i) { + if (fissionObstacleUsed(i, T, row_min_indices, used_new_obstacles, used_old_obstacles)) + continue; + + vector fission_indices; + fission_indices.push_back(i); + + for (int j = i+1; j < N; ++j) { + if (fissionObstaclesCorrespond(i, j, row_min_indices, used_new_obstacles)) + fission_indices.push_back(j); + } + + if (fission_indices.size() > 1) { + fissureObstacle(fission_indices, row_min_indices, new_tracked_obstacles, new_obstacles); + + // Mark used old and new obstacles + used_old_obstacles.push_back(row_min_indices[i]); + used_new_obstacles.insert(used_new_obstacles.end(), fission_indices.begin(), fission_indices.end()); + } + } + + // Check for other possibilities + for (int n = 0; n < N; ++n) { + if (find(used_new_obstacles.begin(), used_new_obstacles.end(), n) != used_new_obstacles.end()) + continue; + + if (row_min_indices[n] == -1) { + new_untracked_obstacles.push_back(new_obstacles->circles[n]); + } + else if (find(used_old_obstacles.begin(), used_old_obstacles.end(), row_min_indices[n]) == used_old_obstacles.end()) { + if (row_min_indices[n] >= 0 && row_min_indices[n] < T) { + tracked_obstacles_[row_min_indices[n]].correctState(new_obstacles->circles[n]); + } + else if (row_min_indices[n] >= T) { + TrackedObstacle to(untracked_obstacles_[row_min_indices[n] - T]); + to.correctState(new_obstacles->circles[n]); + for (int i = 0; i < static_cast(p_loop_rate_ / p_sensor_rate_); ++i) + to.updateState(); + + new_tracked_obstacles.push_back(to); + } + + used_new_obstacles.push_back(n); + } + } + + // Remove tracked obstacles that are no longer existent due to fusion or fission and insert new ones + // Sort in descending order to remove from back of the list + sort(used_old_obstacles.rbegin(), used_old_obstacles.rend()); + for (int idx : used_old_obstacles) + tracked_obstacles_.erase(tracked_obstacles_.begin() + idx); + + tracked_obstacles_.insert(tracked_obstacles_.end(), new_tracked_obstacles.begin(), new_tracked_obstacles.end()); + + // Remove old untracked obstacles and save new ones + untracked_obstacles_.clear(); + untracked_obstacles_.assign(new_untracked_obstacles.begin(), new_untracked_obstacles.end()); +} + +double ObstacleTracker::obstacleCostFunction(const CircleObstacle& new_obstacle, const CircleObstacle& old_obstacle) { + mat distribution = mat(2, 2).zeros(); + vec relative_position = vec(2).zeros(); + + double cost = 0.0; + double penalty = 1.0; + double tp = 1.0 / p_sensor_rate_; + + double direction = atan2(old_obstacle.velocity.y, old_obstacle.velocity.x); + + geometry_msgs::Point new_center = transformPoint(new_obstacle.center, 0.0, 0.0, -direction); + geometry_msgs::Point old_center = transformPoint(old_obstacle.center, 0.0, 0.0, -direction); + + distribution(0, 0) = pow(p_std_correspondence_dev_, 2.0) + squaredLength(old_obstacle.velocity) * pow(tp, 2.0); + distribution(1, 1) = pow(p_std_correspondence_dev_, 2.0); + + relative_position(0) = new_center.x - old_center.x - tp * length(old_obstacle.velocity); + relative_position(1) = new_center.y - old_center.y; + + cost = sqrt(pow(new_obstacle.center.x - old_obstacle.center.x, 2.0) + pow(new_obstacle.center.y - old_obstacle.center.y, 2.0) + pow(new_obstacle.radius - old_obstacle.radius, 2.0)); + + mat a = -0.5 * trans(relative_position) * distribution * relative_position; + penalty = exp(a(0, 0)); + + // TODO: Check values for cost/penalty in common situations + // return cost / penalty; + return cost / 1.0; +} + +void ObstacleTracker::calculateCostMatrix(const vector& new_obstacles, mat& cost_matrix) { + /* + * Cost between two obstacles represents their difference. + * The bigger the cost, the less similar they are. + * N rows of cost_matrix represent new obstacles. + * T+U columns of cost matrix represent old tracked and untracked obstacles. + */ + int N = new_obstacles.size(); + int T = tracked_obstacles_.size(); + int U = untracked_obstacles_.size(); + + cost_matrix = mat(N, T + U, fill::zeros); + + for (int n = 0; n < N; ++n) { + for (int t = 0; t < T; ++t) + cost_matrix(n, t) = obstacleCostFunction(new_obstacles[n], tracked_obstacles_[t].getObstacle()); + + for (int u = 0; u < U; ++u) + cost_matrix(n, u + T) = obstacleCostFunction(new_obstacles[n], untracked_obstacles_[u]); + } +} + +void ObstacleTracker::calculateRowMinIndices(const mat& cost_matrix, vector& row_min_indices) { + /* + * Vector of row minimal indices keeps the indices of old obstacles (tracked and untracked) + * that have the minimum cost related to each of new obstacles, i.e. row_min_indices[n] + * keeps the index of old obstacle that has the minimum cost with n-th new obstacle. + */ + int N = cost_matrix.n_rows; + int T = tracked_obstacles_.size(); + int U = untracked_obstacles_.size(); + + row_min_indices.assign(N, -1); // Minimum index -1 means no correspondence has been found + + for (int n = 0; n < N; ++n) { + double min_cost = p_min_correspondence_cost_; + + for (int t = 0; t < T; ++t) { + if (cost_matrix(n, t) < min_cost) { + min_cost = cost_matrix(n, t); + row_min_indices[n] = t; + } + } + + for (int u = 0; u < U; ++u) { + if (cost_matrix(n, u + T) < min_cost) { + min_cost = cost_matrix(n, u + T); + row_min_indices[n] = u + T; + } + } + } +} + +void ObstacleTracker::calculateColMinIndices(const mat& cost_matrix, vector& col_min_indices) { + /* + * Vector of column minimal indices keeps the indices of new obstacles that has the minimum + * cost related to each of old (tracked and untracked) obstacles, i.e. col_min_indices[i] + * keeps the index of new obstacle that has the minimum cost with i-th old obstacle. + */ + int N = cost_matrix.n_rows; + int T = tracked_obstacles_.size(); + int U = untracked_obstacles_.size(); + + col_min_indices.assign(T + U, -1); // Minimum index -1 means no correspondence has been found + + for (int t = 0; t < T; ++t) { + double min_cost = p_min_correspondence_cost_; + + for (int n = 0; n < N; ++n) { + if (cost_matrix(n, t) < min_cost) { + min_cost = cost_matrix(n, t); + col_min_indices[t] = n; + } + } + } + + for (int u = 0; u < U; ++u) { + double min_cost = p_min_correspondence_cost_; + + for (int n = 0; n < N; ++n) { + if (cost_matrix(n, u + T) < min_cost) { + min_cost = cost_matrix(n, u + T); + col_min_indices[u + T] = n; + } + } + } +} + +bool ObstacleTracker::fusionObstacleUsed(const int idx, const vector &col_min_indices, const vector &used_new, const vector &used_old) { + /* + * This function returns true if: + * - idx-th old obstacle was already used + * - obstacle to which idx-th old obstacle corresponds was already used + * - there is no corresponding obstacle + */ + + return (find(used_old.begin(), used_old.end(), idx) != used_old.end() || + find(used_new.begin(), used_new.end(), col_min_indices[idx]) != used_new.end() || + col_min_indices[idx] < 0); +} + +bool ObstacleTracker::fusionObstaclesCorrespond(const int idx, const int jdx, const vector& col_min_indices, const vector& used_old) { + /* + * This function returns true if: + * - both old obstacles correspond to the same new obstacle + * - jdx-th old obstacle was not yet used + */ + + return (col_min_indices[idx] == col_min_indices[jdx] && + find(used_old.begin(), used_old.end(), jdx) == used_old.end()); +} + +bool ObstacleTracker::fissionObstacleUsed(const int idx, const int T, const vector& row_min_indices, const vector& used_new, const vector& used_old) { + /* + * This function returns true if: + * - idx-th new obstacle was already used + * - obstacle to which idx-th new obstacle corresponds was already used + * - there is no corresponding obstacle + * - obstacle to which idx-th new obstacle corresponds is untracked + */ + + return (find(used_new.begin(), used_new.end(), idx) != used_new.end() || + find(used_old.begin(), used_old.end(), row_min_indices[idx]) != used_old.end() || + row_min_indices[idx] < 0 || + row_min_indices[idx] >= T); +} + +bool ObstacleTracker::fissionObstaclesCorrespond(const int idx, const int jdx, const vector& row_min_indices, const vector& used_new) { + /* + * This function returns true if: + * - both new obstacles correspond to the same old obstacle + * - jdx-th new obstacle was not yet used + */ + + return (row_min_indices[idx] == row_min_indices[jdx] && + find(used_new.begin(), used_new.end(), jdx) == used_new.end()); +} + +void ObstacleTracker::fuseObstacles(const vector& fusion_indices, const vector &col_min_indices, + vector& new_tracked, const Obstacles::ConstPtr& new_obstacles) { + CircleObstacle c; + + double sum_var_x = 0.0; + double sum_var_y = 0.0; + double sum_var_vx = 0.0; + double sum_var_vy = 0.0; + double sum_var_r = 0.0; + + for (int idx : fusion_indices) { + c.center.x += tracked_obstacles_[idx].getObstacle().center.x / tracked_obstacles_[idx].getKFx().P(0,0); + c.center.y += tracked_obstacles_[idx].getObstacle().center.y / tracked_obstacles_[idx].getKFy().P(0,0); + c.velocity.x += tracked_obstacles_[idx].getObstacle().velocity.x / tracked_obstacles_[idx].getKFx().P(1,1); + c.velocity.y += tracked_obstacles_[idx].getObstacle().velocity.y / tracked_obstacles_[idx].getKFy().P(1,1); + c.radius += tracked_obstacles_[idx].getObstacle().radius / tracked_obstacles_[idx].getKFr().P(0,0); + + sum_var_x += 1.0 / tracked_obstacles_[idx].getKFx().P(0,0); + sum_var_y += 1.0 / tracked_obstacles_[idx].getKFy().P(0,0); + sum_var_vx += 1.0 / tracked_obstacles_[idx].getKFx().P(1,1); + sum_var_vy += 1.0 / tracked_obstacles_[idx].getKFy().P(1,1); + sum_var_r += 1.0 / tracked_obstacles_[idx].getKFr().P(0,0); + } + + c.center.x /= sum_var_x; + c.center.y /= sum_var_y; + c.velocity.x /= sum_var_vx; + c.velocity.y /= sum_var_vy; + c.radius /= sum_var_r; + + TrackedObstacle to(c); + to.correctState(new_obstacles->circles[col_min_indices[fusion_indices.front()]]); + for (int i = 0; i < static_cast(p_loop_rate_ / p_sensor_rate_); ++i) + to.updateState(); + + new_tracked.push_back(to); +} + +void ObstacleTracker::fissureObstacle(const vector& fission_indices, const vector& row_min_indices, + vector& new_tracked, const Obstacles::ConstPtr& new_obstacles) { + // For each new obstacle taking part in fission create a tracked obstacle from the original old one and update it with the new one + for (int idx : fission_indices) { + TrackedObstacle to = tracked_obstacles_[row_min_indices[idx]]; + to.correctState(new_obstacles->circles[idx]); + for (int i = 0; i < static_cast(p_loop_rate_ / p_sensor_rate_); ++i) + to.updateState(); + + new_tracked.push_back(to); + } +} + +void ObstacleTracker::updateObstacles() { + for (int i = 0; i < tracked_obstacles_.size(); ++i) { + if (!tracked_obstacles_[i].hasFaded()) + tracked_obstacles_[i].updateState(); + else + tracked_obstacles_.erase(tracked_obstacles_.begin() + i--); + } +} + +void ObstacleTracker::publishObstacles() { + obstacle_detector::ObstaclesPtr obstacles_msg(new obstacle_detector::Obstacles); + + obstacles_.circles.clear(); + + for (auto& tracked_obstacle : tracked_obstacles_) { + CircleObstacle ob = tracked_obstacle.getObstacle(); + ob.true_radius = ob.radius - radius_margin_; + obstacles_.circles.push_back(ob); + } + + *obstacles_msg = obstacles_; + obstacles_msg->header.stamp = ros::Time::now(); + + obstacles_pub_.publish(obstacles_msg); +} + +// Ugly initialization of static members of tracked obstacles... +int TrackedObstacle::s_fade_counter_size_ = 0; +double TrackedObstacle::s_sampling_time_ = 100.0; +double TrackedObstacle::s_process_variance_ = 0.0; +double TrackedObstacle::s_process_rate_variance_ = 0.0; +double TrackedObstacle::s_measurement_variance_ = 0.0; diff --git a/src/panels/obstacle_extractor_panel.cpp b/src/panels/obstacle_extractor_panel.cpp new file mode 100644 index 0000000..ad25192 --- /dev/null +++ b/src/panels/obstacle_extractor_panel.cpp @@ -0,0 +1,318 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/panels/obstacle_extractor_panel.h" + +using namespace obstacle_detector; +using namespace std; + +ObstacleExtractorPanel::ObstacleExtractorPanel(QWidget* parent) : rviz::Panel(parent), nh_(""), nh_local_("obstacle_extractor") { + params_cli_ = nh_local_.serviceClient("params"); + getParams(); + + activate_checkbox_ = new QCheckBox("On/Off"); + use_scan_checkbox_ = new QCheckBox("Use scan"); + use_pcl_checkbox_ = new QCheckBox("Use PCL"); + use_split_merge_checkbox_ = new QCheckBox("Use split and merge"); + discard_segments_checkbox_ = new QCheckBox("Discard segments"); + transform_coords_checkbox_ = new QCheckBox("Transform coordinates"); + + min_n_input_ = new QLineEdit(); + dist_prop_input_ = new QLineEdit(); + group_dist_input_ = new QLineEdit(); + split_dist_input_ = new QLineEdit(); + merge_sep_input_ = new QLineEdit(); + merge_spread_input_ = new QLineEdit(); + max_radius_input_ = new QLineEdit(); + radius_enl_input_ = new QLineEdit(); + frame_id_input_ = new QLineEdit(); + + min_n_input_->setAlignment(Qt::AlignRight); + dist_prop_input_->setAlignment(Qt::AlignRight); + group_dist_input_->setAlignment(Qt::AlignRight); + split_dist_input_->setAlignment(Qt::AlignRight); + merge_sep_input_->setAlignment(Qt::AlignRight); + merge_spread_input_->setAlignment(Qt::AlignRight); + max_radius_input_->setAlignment(Qt::AlignRight); + radius_enl_input_->setAlignment(Qt::AlignRight); + frame_id_input_->setAlignment(Qt::AlignRight); + + QFrame* lines[5]; + for (auto& line : lines) { + line = new QFrame(); + line->setFrameShape(QFrame::HLine); + line->setFrameShadow(QFrame::Sunken); + } + + QSpacerItem* margin = new QSpacerItem(1, 1, QSizePolicy::Expanding, QSizePolicy::Fixed); + + QHBoxLayout* layout_1 = new QHBoxLayout; + layout_1->addItem(margin); + layout_1->addWidget(use_scan_checkbox_); + layout_1->addItem(margin); + layout_1->addWidget(use_pcl_checkbox_); + layout_1->addItem(margin); + + QGroupBox* segmentation_box = new QGroupBox("Segmentation:"); + QGridLayout* layout_2 = new QGridLayout; + layout_2->addWidget(new QLabel("Nmin:"), 0, 0, Qt::AlignRight); + layout_2->addWidget(min_n_input_, 0, 1); + layout_2->addWidget(new QLabel(" "), 0, 2); + layout_2->addWidget(new QLabel("dp:"), 0, 3, Qt::AlignRight); + layout_2->addWidget(dist_prop_input_, 0, 4); + layout_2->addWidget(new QLabel(""), 0, 5); + + layout_2->addWidget(new QLabel("dgroup:"), 1, 0, Qt::AlignRight); + layout_2->addWidget(group_dist_input_, 1, 1); + layout_2->addWidget(new QLabel("m "), 1, 2, Qt::AlignLeft); + layout_2->addWidget(new QLabel("dsplit:"), 1, 3, Qt::AlignRight); + layout_2->addWidget(split_dist_input_, 1, 4); + layout_2->addWidget(new QLabel("m"), 1, 5, Qt::AlignLeft); + + layout_2->addWidget(new QLabel("dsep:"), 2, 0, Qt::AlignRight); + layout_2->addWidget(merge_sep_input_, 2, 1); + layout_2->addWidget(new QLabel("m "), 2, 2, Qt::AlignLeft); + layout_2->addWidget(new QLabel("dspread:"), 2, 3, Qt::AlignRight); + layout_2->addWidget(merge_spread_input_, 2, 4); + layout_2->addWidget(new QLabel("m"), 2, 5, Qt::AlignLeft); + + layout_2->addWidget(use_split_merge_checkbox_, 3, 0, 1, 6, Qt::AlignCenter); + segmentation_box->setLayout(layout_2); + + QGroupBox* circle_box = new QGroupBox("Circularization:"); + QGridLayout* layout_3 = new QGridLayout; + layout_3->addWidget(new QLabel("rmax:"), 0, 0, Qt::AlignRight); + layout_3->addWidget(max_radius_input_, 0, 1); + layout_3->addWidget(new QLabel("m "), 0, 2, Qt::AlignLeft); + layout_3->addWidget(new QLabel("rmargin:"), 0, 3, Qt::AlignRight); + layout_3->addWidget(radius_enl_input_, 0, 4); + layout_3->addWidget(new QLabel("m"), 0, 5, Qt::AlignLeft); + + layout_3->addWidget(discard_segments_checkbox_, 1, 0, 1, 6, Qt::AlignCenter); + circle_box->setLayout(layout_3); + + QGroupBox* frame_box = new QGroupBox("Frames:"); + QGridLayout* layout_4 = new QGridLayout; + layout_4->addItem(margin, 0, 0, 2, 1); + layout_4->addWidget(new QLabel("Frame ID:"), 0, 1, Qt::AlignRight); + layout_4->addWidget(frame_id_input_, 0, 2, Qt::AlignLeft); + layout_4->addWidget(transform_coords_checkbox_, 1, 1, 1, 2, Qt::AlignCenter); + layout_4->addItem(margin, 0, 3, 2, 1); + frame_box->setLayout(layout_4); + + QVBoxLayout* layout = new QVBoxLayout; + layout->addWidget(activate_checkbox_); + layout->addWidget(lines[0]); + layout->addLayout(layout_1); + layout->addWidget(lines[1]); + layout->addWidget(segmentation_box); + layout->addWidget(lines[2]); + layout->addWidget(circle_box); + layout->addWidget(lines[3]); + layout->addWidget(frame_box); + layout->setAlignment(layout, Qt::AlignCenter); + setLayout(layout); + + connect(activate_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); + connect(use_scan_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); + connect(use_pcl_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); + connect(use_split_merge_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); + connect(discard_segments_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); + connect(transform_coords_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); + + connect(min_n_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(dist_prop_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(group_dist_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(split_dist_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(merge_sep_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(merge_spread_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(max_radius_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(radius_enl_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(frame_id_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + + evaluateParams(); +} + +void ObstacleExtractorPanel::processInputs() { + verifyInputs(); + setParams(); + evaluateParams(); + notifyParamsUpdate(); +} + +void ObstacleExtractorPanel::verifyInputs() { + p_active_ = activate_checkbox_->isChecked(); + p_use_scan_ = use_scan_checkbox_->isChecked(); + p_use_pcl_ = use_pcl_checkbox_->isChecked(); + + p_use_split_and_merge_ = use_split_merge_checkbox_->isChecked(); + p_discard_converted_segments_ = discard_segments_checkbox_->isChecked(); + p_transform_coordinates_ = transform_coords_checkbox_->isChecked(); + + try { p_min_group_points_ = boost::lexical_cast(min_n_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_min_group_points_ = 0; min_n_input_->setText("0"); } + + try { p_distance_proportion_ = boost::lexical_cast(dist_prop_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_distance_proportion_ = 0.0; dist_prop_input_->setText("0.0"); } + + try { p_max_group_distance_ = boost::lexical_cast(group_dist_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_max_group_distance_ = 0.0; group_dist_input_->setText("0.0"); } + + try { p_max_split_distance_ = boost::lexical_cast(split_dist_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_max_split_distance_ = 0.0; split_dist_input_->setText("0.0"); } + + try { p_max_merge_separation_ = boost::lexical_cast(merge_sep_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_max_merge_separation_ = 0.0; merge_sep_input_->setText("0.0"); } + + try { p_max_merge_spread_ = boost::lexical_cast(merge_spread_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_max_merge_spread_ = 0.0; merge_spread_input_->setText("0.0"); } + + try { p_max_circle_radius_ = boost::lexical_cast(max_radius_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_max_circle_radius_ = 0.0; max_radius_input_->setText("0.0"); } + + try { p_radius_enlargement_ = boost::lexical_cast(radius_enl_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_radius_enlargement_ = 0.0; radius_enl_input_->setText("0.0"); } + + p_frame_id_ = frame_id_input_->text().toStdString(); +} + +void ObstacleExtractorPanel::setParams() { + nh_local_.setParam("min_group_points", p_min_group_points_); + + nh_local_.setParam("active", p_active_); + nh_local_.setParam("use_scan", p_use_scan_); + nh_local_.setParam("use_pcl", p_use_pcl_); + nh_local_.setParam("use_split_and_merge", p_use_split_and_merge_); + nh_local_.setParam("discard_converted_segments", p_discard_converted_segments_); + nh_local_.setParam("transform_coordinates", p_transform_coordinates_); + + nh_local_.setParam("max_group_distance", p_max_group_distance_); + nh_local_.setParam("distance_proportion", p_distance_proportion_); + nh_local_.setParam("max_split_distance", p_max_split_distance_); + + nh_local_.setParam("max_merge_separation", p_max_merge_separation_); + nh_local_.setParam("max_merge_spread", p_max_merge_spread_); + nh_local_.setParam("max_circle_radius", p_max_circle_radius_); + nh_local_.setParam("radius_enlargement", p_radius_enlargement_); + + nh_local_.setParam("frame_id", p_frame_id_); +} + +void ObstacleExtractorPanel::getParams() { + nh_local_.param("min_group_points", p_min_group_points_, 5); + + nh_local_.param("active", p_active_, true); + nh_local_.param("use_scan", p_use_scan_, true); + nh_local_.param("use_pcl", p_use_pcl_, false); + nh_local_.param("use_split_and_merge", p_use_split_and_merge_, false); + nh_local_.param("discard_converted_segments", p_discard_converted_segments_, true); + nh_local_.param("transform_coordinates", p_transform_coordinates_, true); + + nh_local_.param("max_group_distance", p_max_group_distance_, 0.100); + nh_local_.param("distance_proportion", p_distance_proportion_, 0.006136); + nh_local_.param("max_split_distance", p_max_split_distance_, 0.070); + nh_local_.param("max_merge_separation", p_max_merge_separation_, 0.150); + nh_local_.param("max_merge_spread", p_max_merge_spread_, 0.070); + nh_local_.param("max_circle_radius", p_max_circle_radius_, 0.300); + nh_local_.param("radius_enlargement", p_radius_enlargement_, 0.030); + + nh_local_.param("frame_id", p_frame_id_, "world"); +} + +void ObstacleExtractorPanel::evaluateParams() { + activate_checkbox_->setChecked(p_active_); + + use_scan_checkbox_->setEnabled(p_active_); + use_scan_checkbox_->setChecked(p_use_scan_); + + use_pcl_checkbox_->setEnabled(p_active_); + use_pcl_checkbox_->setChecked(p_use_pcl_); + + use_split_merge_checkbox_->setEnabled(p_active_); + use_split_merge_checkbox_->setChecked(p_use_split_and_merge_); + + discard_segments_checkbox_->setEnabled(p_active_); + discard_segments_checkbox_->setChecked(p_discard_converted_segments_); + + transform_coords_checkbox_->setEnabled(p_active_); + transform_coords_checkbox_->setChecked(p_transform_coordinates_); + + min_n_input_->setEnabled(p_active_); + min_n_input_->setText(QString::number(p_min_group_points_)); + + dist_prop_input_->setEnabled(p_active_); + dist_prop_input_->setText(QString::number(p_distance_proportion_)); + + group_dist_input_->setEnabled(p_active_); + group_dist_input_->setText(QString::number(p_max_group_distance_)); + + split_dist_input_->setEnabled(p_active_); + split_dist_input_->setText(QString::number(p_max_split_distance_)); + + merge_sep_input_->setEnabled(p_active_); + merge_sep_input_->setText(QString::number(p_max_merge_separation_)); + + merge_spread_input_->setEnabled(p_active_); + merge_spread_input_->setText(QString::number(p_max_merge_spread_)); + + max_radius_input_->setEnabled(p_active_); + max_radius_input_->setText(QString::number(p_max_circle_radius_)); + + radius_enl_input_->setEnabled(p_active_); + radius_enl_input_->setText(QString::number(p_radius_enlargement_)); + + frame_id_input_->setEnabled(p_active_); + frame_id_input_->setText(QString::fromStdString(p_frame_id_)); +} + +void ObstacleExtractorPanel::notifyParamsUpdate() { + std_srvs::Empty empty; + if (!params_cli_.call(empty)) { + p_active_ = false; + setParams(); + evaluateParams(); + } +} + +void ObstacleExtractorPanel::save(rviz::Config config) const { + rviz::Panel::save(config); +} + +void ObstacleExtractorPanel::load(const rviz::Config& config) { + rviz::Panel::load(config); +} + +#include +PLUGINLIB_EXPORT_CLASS(obstacle_detector::ObstacleExtractorPanel, rviz::Panel) diff --git a/src/panels/obstacle_publisher_panel.cpp b/src/panels/obstacle_publisher_panel.cpp new file mode 100644 index 0000000..ce09a10 --- /dev/null +++ b/src/panels/obstacle_publisher_panel.cpp @@ -0,0 +1,313 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/panels/obstacle_publisher_panel.h" + +using namespace obstacle_detector; +using namespace std; + +ObstaclePublisherPanel::ObstaclePublisherPanel(QWidget* parent) : rviz::Panel(parent), nh_(""), nh_local_("obstacle_publisher") { + params_cli_ = nh_local_.serviceClient("params"); + getParams(); + + activate_checkbox_ = new QCheckBox("On/Off"); + fusion_example_checkbox_ = new QCheckBox("Fusion"); + fission_example_checkbox_ = new QCheckBox("Fission"); + + obstacles_list_ = new QListWidget(); + add_button_ = new QPushButton("Add obstacle"); + remove_button_ = new QPushButton("Remove selected"); + reset_button_ = new QPushButton("Reset time"); + + x_input_ = new QLineEdit(); + y_input_ = new QLineEdit(); + r_input_ = new QLineEdit(); + vx_input_ = new QLineEdit(); + vy_input_ = new QLineEdit(); + + obstacles_list_->setSelectionMode(QAbstractItemView::MultiSelection); + + QFrame* lines[4]; + for (auto& line : lines) { + line = new QFrame(); + line->setFrameShape(QFrame::HLine); + line->setFrameShadow(QFrame::Sunken); + } + + QSpacerItem* margin = new QSpacerItem(1, 1, QSizePolicy::Expanding, QSizePolicy::Fixed); + + QHBoxLayout* demos_layout = new QHBoxLayout; + demos_layout->addWidget(fusion_example_checkbox_); + demos_layout->addWidget(fission_example_checkbox_); + + QGroupBox* demos_box = new QGroupBox("Demos:"); + demos_box->setLayout(demos_layout); + + QHBoxLayout* xyr_layout = new QHBoxLayout; + xyr_layout->addItem(margin); + xyr_layout->addWidget(new QLabel("x:")); + xyr_layout->addWidget(x_input_); + xyr_layout->addWidget(new QLabel("m, ")); + xyr_layout->addWidget(new QLabel("y:")); + xyr_layout->addWidget(y_input_); + xyr_layout->addWidget(new QLabel("m, ")); + xyr_layout->addWidget(new QLabel("r:")); + xyr_layout->addWidget(r_input_); + xyr_layout->addWidget(new QLabel("m")); + xyr_layout->addItem(margin); + + QHBoxLayout* vxvy_layout = new QHBoxLayout; + vxvy_layout->addItem(margin); + vxvy_layout->addWidget(new QLabel("vx:")); + vxvy_layout->addWidget(vx_input_); + vxvy_layout->addWidget(new QLabel("m/s, ")); + vxvy_layout->addWidget(new QLabel("vy:")); + vxvy_layout->addWidget(vy_input_); + vxvy_layout->addWidget(new QLabel("m/s")); + vxvy_layout->addItem(margin); + + QVBoxLayout* obst_params_layout = new QVBoxLayout; + obst_params_layout->addWidget(obstacles_list_); + obst_params_layout->addWidget(remove_button_); + obst_params_layout->addWidget(lines[2]); + obst_params_layout->addLayout(xyr_layout); + obst_params_layout->addLayout(vxvy_layout); + obst_params_layout->addWidget(add_button_, Qt::AlignCenter); + obst_params_layout->addWidget(lines[3]); + obst_params_layout->addWidget(reset_button_, Qt::AlignCenter); + + QGroupBox* obst_box = new QGroupBox("Obstacles:"); + obst_box->setLayout(obst_params_layout); + + QVBoxLayout* layout = new QVBoxLayout; + layout->addWidget(activate_checkbox_); + layout->addWidget(lines[0]); + layout->addWidget(demos_box); + layout->addWidget(lines[1]); + layout->addWidget(obst_box); + layout->setAlignment(layout, Qt::AlignCenter); + setLayout(layout); + + connect(activate_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); + connect(fusion_example_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); + connect(fission_example_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); + + connect(add_button_, SIGNAL(clicked()), this, SLOT(addObstacle())); + connect(remove_button_, SIGNAL(clicked()), this, SLOT(removeObstacles())); + connect(reset_button_, SIGNAL(clicked()), this, SLOT(reset())); + + evaluateParams(); +} + +void ObstaclePublisherPanel::processInputs() { + verifyInputs(); + setParams(); + evaluateParams(); + notifyParamsUpdate(); +} + +void ObstaclePublisherPanel::addObstacle() { + verifyInputs(); + + if (r_ > 0.0) { + p_x_vector_.push_back(x_); + p_y_vector_.push_back(y_); + p_r_vector_.push_back(r_); + + p_vx_vector_.push_back(vx_); + p_vy_vector_.push_back(vy_); + + setParams(); + evaluateParams(); + notifyParamsUpdate(); + } +} + +void ObstaclePublisherPanel::removeObstacles() { + QModelIndexList indexes = obstacles_list_->selectionModel()->selectedIndexes(); + + vector index_list; + for (QModelIndex index : indexes) + index_list.push_back(index.row()); + + sort(index_list.begin(), index_list.end(), greater()); + + for (int idx : index_list) { + p_x_vector_.erase(p_x_vector_.begin() + idx); + p_y_vector_.erase(p_y_vector_.begin() + idx); + p_r_vector_.erase(p_r_vector_.begin() + idx); + + p_vx_vector_.erase(p_vx_vector_.begin() + idx); + p_vy_vector_.erase(p_vy_vector_.begin() + idx); + + delete obstacles_list_items_[idx]; + obstacles_list_items_.erase(obstacles_list_items_.begin() + idx); + } + + setParams(); + evaluateParams(); + notifyParamsUpdate(); +} + +void ObstaclePublisherPanel::reset() { + p_reset_ = true; + + processInputs(); + + p_reset_ = false; +} + +void ObstaclePublisherPanel::verifyInputs() { + p_active_ = activate_checkbox_->isChecked(); + p_fusion_example_ = fusion_example_checkbox_->isChecked(); + p_fission_example_ = fission_example_checkbox_->isChecked(); + + try { x_ = boost::lexical_cast(x_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { x_ = 0.0; x_input_->setText("0.0"); } + + try { y_ = boost::lexical_cast(y_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { y_ = 0.0; y_input_->setText("0.0"); } + + try { r_ = boost::lexical_cast(r_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { r_ = 0.0; r_input_->setText("0.0"); } + + try { vx_ = boost::lexical_cast(vx_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { vx_ = 0.0; vx_input_->setText("0.0"); } + + try { vy_ = boost::lexical_cast(vy_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { vy_ = 0.0; vy_input_->setText("0.0"); } + +// p_frame_id_ = +} + +void ObstaclePublisherPanel::setParams() { + nh_local_.setParam("active", p_active_); + nh_local_.setParam("reset", p_reset_); + nh_local_.setParam("fusion_example", p_fusion_example_); + nh_local_.setParam("fission_example", p_fission_example_); + + nh_local_.setParam("loop_rate", p_loop_rate_); + + nh_local_.setParam("x_vector", p_x_vector_); + nh_local_.setParam("y_vector", p_y_vector_); + nh_local_.setParam("r_vector", p_r_vector_); + + nh_local_.setParam("vx_vector", p_vx_vector_); + nh_local_.setParam("vy_vector", p_vy_vector_); + + nh_local_.setParam("frame_id", p_frame_id_); +} + +void ObstaclePublisherPanel::getParams() { + nh_local_.param("active", p_active_, true); + nh_local_.param("reset", p_reset_, false); + nh_local_.param("fusion_example", p_fusion_example_, false); + nh_local_.param("fission_example", p_fission_example_, false); + + nh_local_.param("loop_rate", p_loop_rate_, 10.0); + + nh_local_.getParam("x_vector", p_x_vector_); + nh_local_.getParam("y_vector", p_y_vector_); + nh_local_.getParam("r_vector", p_r_vector_); + + nh_local_.getParam("vx_vector", p_vx_vector_); + nh_local_.getParam("vy_vector", p_vy_vector_); + + nh_local_.getParam("frame_id", p_frame_id_); +} + +void ObstaclePublisherPanel::evaluateParams() { + activate_checkbox_->setChecked(p_active_); + fusion_example_checkbox_->setEnabled(p_active_); + fission_example_checkbox_->setEnabled(p_active_); + + fusion_example_checkbox_->setChecked(p_fusion_example_); + fission_example_checkbox_->setChecked(p_fission_example_); + + add_button_->setEnabled(p_active_); + remove_button_->setEnabled(p_active_); + reset_button_->setEnabled(p_active_); + + x_input_->setEnabled(p_active_); + y_input_->setEnabled(p_active_); + r_input_->setEnabled(p_active_); + + vx_input_->setEnabled(p_active_); + vy_input_->setEnabled(p_active_); + + obstacles_list_->setEnabled(p_active_); + + if (p_x_vector_.size() < p_y_vector_.size() || p_x_vector_.size() < p_r_vector_.size() || + p_x_vector_.size() < p_vx_vector_.size() || p_x_vector_.size() < p_vy_vector_.size()) + return; + + for (QListWidgetItem* item : obstacles_list_items_) + delete item; + + obstacles_list_items_.clear(); + obstacles_list_->clear(); + + for (int idx = 0; idx < p_x_vector_.size(); ++idx) { + QListWidgetItem* item = new QListWidgetItem; + item->setText(QString::number(idx + 1) + ". " + + "[x: " + QString::number(p_x_vector_[idx]) + "m] " + + "[y: " + QString::number(p_y_vector_[idx]) + "m] " + + "[r: " + QString::number(p_r_vector_[idx]) + "m] " + + "[vx: " + QString::number(p_vx_vector_[idx]) + "m/s] " + + "[vy: " + QString::number(p_vy_vector_[idx]) + "m/s]"); + obstacles_list_items_.push_back(item); + obstacles_list_->insertItem(idx, item); + } +} + +void ObstaclePublisherPanel::notifyParamsUpdate() { + std_srvs::Empty empty; + if (!params_cli_.call(empty)) { + p_active_ = false; + setParams(); + evaluateParams(); + } +} + +void ObstaclePublisherPanel::save(rviz::Config config) const { + rviz::Panel::save(config); +} + +void ObstaclePublisherPanel::load(const rviz::Config& config) { + rviz::Panel::load(config); +} + +#include +PLUGINLIB_EXPORT_CLASS(obstacle_detector::ObstaclePublisherPanel, rviz::Panel) diff --git a/src/panels/obstacle_tracker_panel.cpp b/src/panels/obstacle_tracker_panel.cpp new file mode 100644 index 0000000..3e5f515 --- /dev/null +++ b/src/panels/obstacle_tracker_panel.cpp @@ -0,0 +1,228 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/panels/obstacle_tracker_panel.h" + +using namespace obstacle_detector; +using namespace std; + +ObstacleTrackerPanel::ObstacleTrackerPanel(QWidget* parent) : rviz::Panel(parent), nh_(""), nh_local_("obstacle_tracker") { + params_cli_ = nh_local_.serviceClient("params"); + getParams(); + + activate_checkbox_ = new QCheckBox("On/Off"); + + tracking_duration_input_ = new QLineEdit(); + loop_rate_input_ = new QLineEdit(); + min_corr_cost_input_ = new QLineEdit(); + std_corr_dev_input_ = new QLineEdit(); + process_var_input_ = new QLineEdit(); + process_rate_var_input_ = new QLineEdit(); + measure_var_input_ = new QLineEdit(); + + tracking_duration_input_->setAlignment(Qt::AlignRight); + loop_rate_input_->setAlignment(Qt::AlignRight); + min_corr_cost_input_->setAlignment(Qt::AlignRight); + std_corr_dev_input_->setAlignment(Qt::AlignRight); + process_var_input_->setAlignment(Qt::AlignRight); + process_rate_var_input_->setAlignment(Qt::AlignRight); + measure_var_input_->setAlignment(Qt::AlignRight); + + QSpacerItem* margin = new QSpacerItem(1, 1, QSizePolicy::Expanding, QSizePolicy::Fixed); + + QFrame* lines[4]; + for (auto& line : lines) { + line = new QFrame(); + line->setFrameShape(QFrame::HLine); + line->setFrameShadow(QFrame::Sunken); + } + + QGroupBox* tracking_box = new QGroupBox("General:"); + QGridLayout* tracking_layout = new QGridLayout; + tracking_layout->addWidget(new QLabel("Tracking rate:"), 0, 0, Qt::AlignRight); + tracking_layout->addWidget(loop_rate_input_, 0, 1, Qt::AlignLeft); + tracking_layout->addWidget(new QLabel("Hz"), 0, 2, Qt::AlignLeft); + tracking_layout->addWidget(new QLabel("Tracking duration:"), 1, 0, Qt::AlignRight); + tracking_layout->addWidget(tracking_duration_input_, 1, 1, Qt::AlignLeft); + tracking_layout->addWidget(new QLabel("s"), 1, 2, Qt::AlignLeft); + tracking_box->setLayout(tracking_layout); + + QGroupBox* corr_box = new QGroupBox("Correspondence:"); + QGridLayout* corr_layout = new QGridLayout; + corr_layout->addWidget(new QLabel("Minimal cost:"), 0, 0, Qt::AlignRight); + corr_layout->addWidget(min_corr_cost_input_, 0, 1, Qt::AlignLeft); + corr_layout->addWidget(new QLabel("m"), 0, 2, Qt::AlignLeft); + corr_layout->addWidget(new QLabel("Standard deviation:"), 1, 0, Qt::AlignRight); + corr_layout->addWidget(std_corr_dev_input_, 1, 1, Qt::AlignLeft); + corr_layout->addWidget(new QLabel("m"), 1, 2, Qt::AlignLeft); + corr_box->setLayout(corr_layout); + + QGroupBox* kf_box = new QGroupBox("Kalman Filter:"); + QGridLayout* kf_layout = new QGridLayout; + kf_layout->addWidget(new QLabel("Process variance:"), 0, 0, Qt::AlignRight); + kf_layout->addWidget(process_var_input_, 0, 1, Qt::AlignLeft); + kf_layout->addWidget(new QLabel("m2"), 0, 2, Qt::AlignLeft); + kf_layout->addWidget(new QLabel("Process rate variance:"), 1, 0, Qt::AlignRight); + kf_layout->addWidget(process_rate_var_input_, 1, 1, Qt::AlignLeft); + kf_layout->addWidget(new QLabel("(m/s)2"), 1, 2, Qt::AlignLeft); + kf_layout->addWidget(new QLabel("Measurement variance:"), 2, 0, Qt::AlignRight); + kf_layout->addWidget(measure_var_input_, 2, 1, Qt::AlignLeft); + kf_layout->addWidget(new QLabel("m2"), 2, 2, Qt::AlignLeft); + kf_box->setLayout(kf_layout); + + QVBoxLayout* layout = new QVBoxLayout; + layout->addWidget(activate_checkbox_); + layout->addWidget(lines[0]); + layout->addWidget(tracking_box); + layout->addWidget(lines[1]); + layout->addWidget(corr_box); + layout->addWidget(lines[2]); + layout->addWidget(kf_box); + layout->setAlignment(layout, Qt::AlignCenter); + setLayout(layout); + + connect(activate_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); + + connect(tracking_duration_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(loop_rate_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(min_corr_cost_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(std_corr_dev_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(process_var_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(process_rate_var_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(measure_var_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + + evaluateParams(); +} + +void ObstacleTrackerPanel::processInputs() { + verifyInputs(); + setParams(); + evaluateParams(); + notifyParamsUpdate(); +} + +void ObstacleTrackerPanel::verifyInputs() { + p_active_ = activate_checkbox_->isChecked(); + + try { p_loop_rate_ = boost::lexical_cast(loop_rate_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_loop_rate_ = 1.0; loop_rate_input_->setText("1.0"); } + + try { p_tracking_duration_ = boost::lexical_cast(tracking_duration_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_tracking_duration_ = 0.0; tracking_duration_input_->setText("0.0"); } + + try { p_min_correspondence_cost_ = boost::lexical_cast(min_corr_cost_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_min_correspondence_cost_ = 0.0; min_corr_cost_input_->setText("0.0"); } + + try { p_std_correspondence_dev_ = boost::lexical_cast(std_corr_dev_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_std_correspondence_dev_ = 0.0; std_corr_dev_input_->setText("0.0"); } + + try { p_process_variance_ = boost::lexical_cast(process_var_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_process_variance_ = 0.0; process_var_input_->setText("0.0"); } + + try { p_process_rate_variance_ = boost::lexical_cast(process_rate_var_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_process_rate_variance_ = 0.0; process_rate_var_input_->setText("0.0"); } + + try { p_measurement_variance_ = boost::lexical_cast(measure_var_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_measurement_variance_ = 0.0; measure_var_input_->setText("0.0"); } +} + +void ObstacleTrackerPanel::setParams() { + nh_local_.setParam("active", p_active_); + + nh_local_.setParam("loop_rate", p_loop_rate_); + nh_local_.setParam("tracking_duration", p_tracking_duration_); + nh_local_.setParam("min_correspondence_cost", p_min_correspondence_cost_); + nh_local_.setParam("std_correspondence_dev", p_std_correspondence_dev_); + nh_local_.setParam("process_variance", p_process_variance_); + nh_local_.setParam("process_rate_variance", p_process_rate_variance_); + nh_local_.setParam("measurement_variance", p_measurement_variance_); +} + +void ObstacleTrackerPanel::getParams() { + nh_local_.param("active", p_active_, true); + + nh_local_.param("loop_rate", p_loop_rate_, 100.0); + nh_local_.param("tracking_duration", p_tracking_duration_, 2.0); + nh_local_.param("min_correspondence_cost", p_min_correspondence_cost_, 0.3); + nh_local_.param("std_correspondence_dev", p_std_correspondence_dev_, 0.15); + nh_local_.param("process_variance", p_process_variance_, 0.001); + nh_local_.param("process_rate_variance", p_process_rate_variance_, 0.01); + nh_local_.param("measurement_variance", p_measurement_variance_, 1.0); +} + +void ObstacleTrackerPanel::evaluateParams() { + activate_checkbox_->setChecked(p_active_); + + loop_rate_input_->setEnabled(p_active_); + loop_rate_input_->setText(QString::number(p_loop_rate_)); + + tracking_duration_input_->setEnabled(p_active_); + tracking_duration_input_->setText(QString::number(p_tracking_duration_)); + + min_corr_cost_input_->setEnabled(p_active_); + min_corr_cost_input_->setText(QString::number(p_min_correspondence_cost_)); + + std_corr_dev_input_->setEnabled(p_active_); + std_corr_dev_input_->setText(QString::number(p_std_correspondence_dev_)); + + process_var_input_->setEnabled(p_active_); + process_var_input_->setText(QString::number(p_process_variance_)); + + process_rate_var_input_->setEnabled(p_active_); + process_rate_var_input_->setText(QString::number(p_process_rate_variance_)); + + measure_var_input_->setEnabled(p_active_); + measure_var_input_->setText(QString::number(p_measurement_variance_)); +} + +void ObstacleTrackerPanel::notifyParamsUpdate() { + std_srvs::Empty empty; + if (!params_cli_.call(empty)) { + p_active_ = false; + setParams(); + evaluateParams(); + } +} + +void ObstacleTrackerPanel::save(rviz::Config config) const { + rviz::Panel::save(config); +} + +void ObstacleTrackerPanel::load(const rviz::Config& config) { + rviz::Panel::load(config); +} + +#include +PLUGINLIB_EXPORT_CLASS(obstacle_detector::ObstacleTrackerPanel, rviz::Panel) diff --git a/src/panels/scans_merger_panel.cpp b/src/panels/scans_merger_panel.cpp new file mode 100644 index 0000000..bdd0915 --- /dev/null +++ b/src/panels/scans_merger_panel.cpp @@ -0,0 +1,287 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/panels/scans_merger_panel.h" + +using namespace obstacle_detector; +using namespace std; + +ScansMergerPanel::ScansMergerPanel(QWidget* parent) : rviz::Panel(parent), nh_(""), nh_local_("scans_merger") { + params_cli_ = nh_local_.serviceClient("params"); + getParams(); + + activate_checkbox_ = new QCheckBox("On/Off"); + scan_checkbox_ = new QCheckBox("Publish scan"); + pcl_checkbox_ = new QCheckBox("Publish PCL"); + + n_input_ = new QLineEdit(); + r_min_input_ = new QLineEdit(); + r_max_input_ = new QLineEdit(); + x_min_input_ = new QLineEdit(); + x_max_input_ = new QLineEdit(); + y_min_input_ = new QLineEdit(); + y_max_input_ = new QLineEdit(); + frame_id_input_ = new QLineEdit(); + + n_input_->setAlignment(Qt::AlignRight); + r_min_input_->setAlignment(Qt::AlignRight); + r_max_input_->setAlignment(Qt::AlignRight); + x_min_input_->setAlignment(Qt::AlignRight); + x_max_input_->setAlignment(Qt::AlignRight); + y_min_input_->setAlignment(Qt::AlignRight); + y_max_input_->setAlignment(Qt::AlignRight); + frame_id_input_->setAlignment(Qt::AlignRight); + + QFrame* lines[5]; + for (auto& line : lines) { + line = new QFrame(); + line->setFrameShape(QFrame::HLine); + line->setFrameShadow(QFrame::Sunken); + } + + QSpacerItem* margin = new QSpacerItem(1, 1, QSizePolicy::Expanding, QSizePolicy::Fixed); + + QHBoxLayout* layout_1 = new QHBoxLayout; + layout_1->addItem(margin); + layout_1->addWidget(scan_checkbox_); + layout_1->addItem(margin); + layout_1->addWidget(pcl_checkbox_); + layout_1->addItem(margin); + + QHBoxLayout* layout_2 = new QHBoxLayout; + layout_2->addItem(margin); + layout_2->addWidget(new QLabel("Number of ranges:")); + layout_2->addWidget(n_input_); + layout_2->addItem(margin); + + QHBoxLayout* layout_3 = new QHBoxLayout; + layout_3->addItem(margin); + layout_3->addWidget(new QLabel("rmin:"), 0, Qt::AlignRight); + layout_3->addWidget(r_min_input_); + layout_3->addWidget(new QLabel("m "), 0, Qt::AlignLeft); + layout_3->addWidget(new QLabel("rmax:"), 0, Qt::AlignRight); + layout_3->addWidget(r_max_input_); + layout_3->addWidget(new QLabel("m"), 0, Qt::AlignLeft); + layout_3->addItem(margin); + + QVBoxLayout* ranges_layout = new QVBoxLayout; + ranges_layout->addLayout(layout_2); + ranges_layout->addLayout(layout_3); + + QGroupBox* scan_box = new QGroupBox("Ranges limits:"); + scan_box->setLayout(ranges_layout); + + QHBoxLayout* layout_4 = new QHBoxLayout; + layout_4->addItem(margin); + layout_4->addWidget(new QLabel("xmin:"), 0, Qt::AlignRight); + layout_4->addWidget(x_min_input_); + layout_4->addWidget(new QLabel("m "), 0, Qt::AlignLeft); + layout_4->addWidget(new QLabel("xmax:"), 0, Qt::AlignRight); + layout_4->addWidget(x_max_input_); + layout_4->addWidget(new QLabel("m"), 0, Qt::AlignLeft); + layout_4->addItem(margin); + + QHBoxLayout* layout_5 = new QHBoxLayout; + layout_5->addItem(margin); + layout_5->addWidget(new QLabel("ymin:"), 0, Qt::AlignRight); + layout_5->addWidget(y_min_input_); + layout_5->addWidget(new QLabel("m "), 0, Qt::AlignLeft); + layout_5->addWidget(new QLabel("ymax:"), 0, Qt::AlignRight); + layout_5->addWidget(y_max_input_); + layout_5->addWidget(new QLabel("m"), 0, Qt::AlignLeft); + layout_5->addItem(margin); + + QVBoxLayout* coords_layout = new QVBoxLayout; + coords_layout->addLayout(layout_4); + coords_layout->addLayout(layout_5); + + QGroupBox* coords_box = new QGroupBox("Coordinates limits:"); + coords_box->setLayout(coords_layout); + + QHBoxLayout* layout_6 = new QHBoxLayout; + layout_6->addItem(margin); + layout_6->addWidget(new QLabel("Frame ID:")); + layout_6->addWidget(frame_id_input_, 0, Qt::AlignLeft); + layout_6->addItem(margin); + + QGroupBox* frame_box = new QGroupBox("Frames:"); + frame_box->setLayout(layout_6); + + QVBoxLayout* layout = new QVBoxLayout; + layout->addWidget(activate_checkbox_); + layout->addWidget(lines[0]); + layout->addLayout(layout_1); + layout->addWidget(lines[1]); + layout->addWidget(scan_box); + layout->addWidget(lines[2]); + layout->addWidget(coords_box); + layout->addWidget(lines[3]); + layout->addWidget(frame_box); + layout->setAlignment(layout, Qt::AlignCenter); + setLayout(layout); + + connect(activate_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); + connect(scan_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); + connect(pcl_checkbox_, SIGNAL(clicked()), this, SLOT(processInputs())); + + connect(n_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(r_min_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(r_max_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(x_min_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(x_max_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(y_min_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(y_max_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + connect(frame_id_input_, SIGNAL(editingFinished()), this, SLOT(processInputs())); + + evaluateParams(); +} + +void ScansMergerPanel::processInputs() { + verifyInputs(); + setParams(); + evaluateParams(); + notifyParamsUpdate(); +} + +void ScansMergerPanel::verifyInputs() { + p_active_ = activate_checkbox_->isChecked(); + p_publish_scan_ = scan_checkbox_->isChecked(); + p_publish_pcl_ = pcl_checkbox_->isChecked(); + + try { p_ranges_num_ = boost::lexical_cast(n_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_ranges_num_ = 0; n_input_->setText("0"); } + + try { p_min_scanner_range_ = boost::lexical_cast(r_min_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_min_scanner_range_ = 0.0; r_min_input_->setText("0.0"); } + + try { p_max_scanner_range_ = boost::lexical_cast(r_max_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_max_scanner_range_ = 0.0; r_max_input_->setText("0.0"); } + + try { p_min_x_range_ = boost::lexical_cast(x_min_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_min_x_range_ = 0.0; x_min_input_->setText("0.0"); } + + try { p_max_x_range_ = boost::lexical_cast(x_max_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_max_x_range_ = 0.0; x_max_input_->setText("0.0"); } + + try { p_min_y_range_ = boost::lexical_cast(y_min_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_min_y_range_ = 0.0; y_min_input_->setText("0.0"); } + + try { p_max_y_range_ = boost::lexical_cast(y_max_input_->text().toStdString()); } + catch(boost::bad_lexical_cast &) { p_max_y_range_ = 0.0; y_max_input_->setText("0.0"); } + + p_frame_id_ = frame_id_input_->text().toStdString(); +} + +void ScansMergerPanel::setParams() { + nh_local_.setParam("ranges_num", p_ranges_num_); + + nh_local_.setParam("active", p_active_); + nh_local_.setParam("publish_scan", p_publish_scan_); + nh_local_.setParam("publish_pcl", p_publish_pcl_); + + nh_local_.setParam("min_scanner_range", p_min_scanner_range_); + nh_local_.setParam("max_scanner_range", p_max_scanner_range_); + nh_local_.setParam("max_x_range", p_max_x_range_); + nh_local_.setParam("min_x_range", p_min_x_range_); + nh_local_.setParam("max_y_range", p_max_y_range_); + nh_local_.setParam("min_y_range", p_min_y_range_); + + nh_local_.setParam("frame_id", p_frame_id_); +} + +void ScansMergerPanel::getParams() { + nh_local_.param("ranges_num", p_ranges_num_, 1000); + + nh_local_.param("active", p_active_, true); + nh_local_.param("publish_scan", p_publish_scan_, true); + nh_local_.param("publish_pcl", p_publish_pcl_, true); + + nh_local_.param("min_scanner_range", p_min_scanner_range_, 0.05); + nh_local_.param("max_scanner_range", p_max_scanner_range_, 10.0); + nh_local_.param("max_x_range", p_max_x_range_, 10.0); + nh_local_.param("min_x_range", p_min_x_range_, -10.0); + nh_local_.param("max_y_range", p_max_y_range_, 10.0); + nh_local_.param("min_y_range", p_min_y_range_, -10.0); + + nh_local_.param("frame_id", p_frame_id_, "scanner_base"); +} + +void ScansMergerPanel::evaluateParams() { + activate_checkbox_->setChecked(p_active_); + + scan_checkbox_->setEnabled(p_active_); + pcl_checkbox_->setEnabled(p_active_); + + scan_checkbox_->setChecked(p_publish_scan_); + pcl_checkbox_->setChecked(p_publish_pcl_); + + n_input_->setEnabled(p_active_); + r_min_input_->setEnabled(p_active_); + r_max_input_->setEnabled(p_active_); + x_min_input_->setEnabled(p_active_); + x_max_input_->setEnabled(p_active_); + y_min_input_->setEnabled(p_active_); + y_max_input_->setEnabled(p_active_); + frame_id_input_->setEnabled(p_active_); + + n_input_->setText(QString::number(p_ranges_num_)); + r_min_input_->setText(QString::number(p_min_scanner_range_)); + r_max_input_->setText(QString::number(p_max_scanner_range_)); + x_min_input_->setText(QString::number(p_min_x_range_)); + x_max_input_->setText(QString::number(p_max_x_range_)); + y_min_input_->setText(QString::number(p_min_y_range_)); + y_max_input_->setText(QString::number(p_max_y_range_)); + frame_id_input_->setText(QString::fromStdString(p_frame_id_)); +} + +void ScansMergerPanel::notifyParamsUpdate() { + std_srvs::Empty empty; + if (!params_cli_.call(empty)) { + p_active_ = false; + setParams(); + evaluateParams(); + } +} + +void ScansMergerPanel::save(rviz::Config config) const { + rviz::Panel::save(config); +} + +void ScansMergerPanel::load(const rviz::Config& config) { + rviz::Panel::load(config); +} + +#include +PLUGINLIB_EXPORT_CLASS(obstacle_detector::ScansMergerPanel, rviz::Panel) diff --git a/src/scan_rectifier.cpp b/src/scan_rectifier.cpp new file mode 100644 index 0000000..e259be4 --- /dev/null +++ b/src/scan_rectifier.cpp @@ -0,0 +1,200 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/scan_rectifier.h" + +using namespace obstacle_detector; +using namespace std; + +ScanRectifier::ScanRectifier(ros::NodeHandle& nh, ros::NodeHandle& nh_local) : nh_(nh), nh_local_(nh_local) { + p_active_ = false; + params_srv_ = nh_local_.advertiseService("params", &ScanRectifier::updateParams, this); + +// ROS_INFO_STREAM("[Scan Rectifier]: Waiting for first " << p_odom2scan_ratio_ << " odometry messages."); +// boost::shared_ptr first_odom; +// for (int i = 0; i < p_odom2scan_ratio_; ++i) { +// first_odom = ros::topic::waitForMessage("odom", nh_); +// odoms_.push_back(*first_odom); +// } +// ROS_INFO_STREAM("[Scan Rectifier]: Acquired first " << p_odom2scan_ratio_ << " odometry messages."); + + ROS_INFO_STREAM("[Scan Rectifier]: Waiting for first odometry message."); + boost::shared_ptr first_odom; + first_odom = ros::topic::waitForMessage("odom", nh_); + odoms_.push_back(*first_odom); + ROS_INFO_STREAM("[Scan Rectifier]: Acquired first odometry message."); + + ROS_INFO_STREAM("[Scan Rectifier]: Waiting for first scan message."); + boost::shared_ptr first_scan; + first_scan = ros::topic::waitForMessage("scan", nh_); + scan_prototype_ = *first_scan; + num_ranges_ = scan_prototype_.ranges.size(); + scan_prototype_.ranges.clear(); + ROS_INFO_STREAM("[Scan Rectifier]: Acquired first laser scan."); + + try { + ROS_INFO_STREAM("[Scan Rectifier]: Waiting for transformations."); + tf::StampedTransform transform; + ros::Time now = ros::Time::now(); + tf_.waitForTransform(first_odom->child_frame_id, first_scan->header.frame_id, now, ros::Duration(10.0)); + tf_.lookupTransform(first_odom->child_frame_id, first_scan->header.frame_id, now, transform); + + scanner_in_base_tf_.x = transform.getOrigin().getX(); + scanner_in_base_tf_.y = transform.getOrigin().getY(); + scanner_in_base_tf_.theta = tf::getYaw(transform.getRotation()); + ROS_INFO_STREAM("[Scan Rectifier]: Acquired transformations."); + } + catch (tf::TransformException ex) { + throw ex.what(); + } + + initialize(); +} + +bool ScanRectifier::updateParams(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { + bool prev_active = p_active_; + + nh_local_.param("active", p_active_, true); + + nh_local_.param("scan_rate", p_scan_rate_, 10.0); + nh_local_.param("odom_rate", p_odom_rate_, 100.0); + + p_odom2scan_ratio_ = static_cast(p_odom_rate_ / p_scan_rate_); + + odoms_.resize(p_odom2scan_ratio_); + + if (p_active_ != prev_active) { + if (p_active_) { + scan_sub_ = nh_.subscribe("scan", 10, &ScanRectifier::scanCallback, this); + odom_sub_ = nh_.subscribe("odom", 10, &ScanRectifier::odomCallback, this); + scan_pub_ = nh_.advertise("rect_scan", 10); + } + else { + scan_sub_.shutdown(); + odom_sub_.shutdown(); + scan_pub_.shutdown(); + } + } + + return true; +} + +void ScanRectifier::scanCallback(const sensor_msgs::LaserScan::ConstPtr scan_msg) { + points_.clear(); + + double t = 0.0; + double theta = 0.0; + double phi = scan_msg->angle_min; + + double dt = scan_msg->time_increment; + double dphi = scan_msg->angle_increment; + double dt_offset = scan_msg->header.stamp.toSec() - odoms_[0].header.stamp.toSec(); + + int idx_offset = 0; + if (dt_offset > 0.0) + idx_offset = static_cast(dt_offset * p_odom_rate_); + + // Temporary velocities in robot coordinate frame + double u, v, w; + u = v = w = 0.0; + + // Corrections of point coordinates resulting from motion prediction + double x_cor, y_cor; + x_cor = y_cor = 0.0; + + geometry_msgs::Point32 point; + for (const float r : scan_msg->ranges) { + if (r > scan_msg->range_min && r < scan_msg->range_max) { + int cur_odom_idx = static_cast(p_odom2scan_ratio_ * t / scan_msg->scan_time) + idx_offset; + if (cur_odom_idx >= odoms_.size()) + cur_odom_idx = odoms_.size() - 1; + + u = odoms_[cur_odom_idx].twist.twist.linear.x; + v = odoms_[cur_odom_idx].twist.twist.linear.y; + w = odoms_[cur_odom_idx].twist.twist.angular.z; + + double v_s_x = u - scanner_in_base_tf_.y * w; + double v_s_y = v + scanner_in_base_tf_.x * w; + + // TODO: Rotate velocities into scanner frame + + if (w != 0.0) { + x_cor = (v_s_x * sin(theta) + v_s_y * (cos(theta) - 1.0)) / w; + y_cor = (v_s_x * (1.0 - cos(theta)) + v_s_y * sin(theta)) / w; + } + else { + x_cor = v_s_x * t; + y_cor = v_s_y * t; + } + + point.x = r * cos(phi + theta) + x_cor; + point.y = r * sin(phi + theta) + y_cor; + + points_.push_back(point); + } + + t += dt; + phi += dphi; + theta += w * dt; + } + + // Prepare and publish scan + sensor_msgs::LaserScanPtr rect_scan_msg(new sensor_msgs::LaserScan(scan_prototype_)); + + ranges_.assign(num_ranges_, 10.0f * scan_prototype_.range_max); + + for (auto& point : points_) { + float angle = atan2(point.y, point.x); + float range = sqrt(pow(point.x, 2.0) + pow(point.y, 2.0)); +\ + int idx = static_cast(num_ranges_ * (angle - scan_prototype_.angle_min) / (scan_prototype_.angle_max - scan_prototype_.angle_min)); + + if (ranges_[idx] > range) + ranges_[idx] = range; + } + + for (int jdx = 0; jdx < num_ranges_; ++jdx) + if (ranges_[jdx] < scan_prototype_.range_min || ranges_[jdx] > scan_prototype_.range_max) + ranges_[jdx] = nan(""); + + rect_scan_msg->header.stamp = scan_msg->header.stamp; + rect_scan_msg->ranges = ranges_; + + scan_pub_.publish(rect_scan_msg); +} + +void ScanRectifier::odomCallback(const nav_msgs::Odometry::ConstPtr odom_msg) { + odoms_.push_back(*odom_msg); +} diff --git a/src/scans_merger.cpp b/src/scans_merger.cpp new file mode 100644 index 0000000..def8f44 --- /dev/null +++ b/src/scans_merger.cpp @@ -0,0 +1,248 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Poznan University of Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Poznan University of Technology nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Mateusz Przybyla + */ + +#include "obstacle_detector/scans_merger.h" +#include "obstacle_detector/utilities/math_utilities.h" + +using namespace obstacle_detector; +using namespace std; + +ScansMerger::ScansMerger(ros::NodeHandle& nh, ros::NodeHandle& nh_local) : nh_(nh), nh_local_(nh_local) { + p_active_ = false; + + front_scan_received_ = false; + rear_scan_received_ = false; + + front_scan_error_ = false; + rear_scan_error_ = false; + + params_srv_ = nh_local_.advertiseService("params", &ScansMerger::updateParams, this); + + ROS_INFO_STREAM("[Scans Merger]: Waiting for first scans."); + boost::shared_ptr scan; + + scan = ros::topic::waitForMessage("front_scan", nh_); + front_scan_frame_id_ = scan->header.frame_id; + + scan = ros::topic::waitForMessage("rear_scan", nh_); + rear_scan_frame_id_ = scan->header.frame_id; + ROS_INFO_STREAM("[Scans Merger]: Acquired first scans."); + + initialize(); +} + +bool ScansMerger::updateParams(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { + bool prev_active = p_active_; + + nh_local_.param("active", p_active_, true); + nh_local_.param("publish_scan", p_publish_scan_, true); + nh_local_.param("publish_pcl", p_publish_pcl_, false); + + nh_local_.param("ranges_num", p_ranges_num_, 1000); + + nh_local_.param("min_scanner_range", p_min_scanner_range_, 0.05); + nh_local_.param("max_scanner_range", p_max_scanner_range_, 10.0); + nh_local_.param("max_x_range", p_max_x_range_, 10.0); + nh_local_.param("min_x_range", p_min_x_range_, -10.0); + nh_local_.param("max_y_range", p_max_y_range_, 10.0); + nh_local_.param("min_y_range", p_min_y_range_, -10.0); + + nh_local_.param("frame_id", p_frame_id_, "scanner_base"); + + try { + ROS_INFO_STREAM("[Scans Merger]: Waiting for transformations."); + tf::TransformListener tf; + tf::StampedTransform transform; + ros::Time now = ros::Time::now(); + + tf.waitForTransform(p_frame_id_, front_scan_frame_id_, now, ros::Duration(10.0)); + tf.lookupTransform(p_frame_id_, front_scan_frame_id_, now, transform); + + front_tf_.x = transform.getOrigin().getX(); + front_tf_.y = transform.getOrigin().getY(); + front_tf_.theta = tf::getYaw(transform.getRotation()); + + tf.waitForTransform(p_frame_id_, rear_scan_frame_id_, now, ros::Duration(10.0)); + tf.lookupTransform(p_frame_id_, rear_scan_frame_id_, now, transform); + + rear_tf_.x = transform.getOrigin().getX(); + rear_tf_.y = transform.getOrigin().getY(); + rear_tf_.theta = tf::getYaw(transform.getRotation()); + ROS_INFO_STREAM("[Scans Merger]: Acquired transformations."); + } + catch (tf::TransformException ex) { + throw ex.what(); + } + + if (p_active_ != prev_active) { + if (p_active_) { + front_scan_sub_ = nh_.subscribe("front_scan", 10, &ScansMerger::frontScanCallback, this); + rear_scan_sub_ = nh_.subscribe("rear_scan", 10, &ScansMerger::rearScanCallback, this); + scan_pub_ = nh_.advertise("scan", 10); + pcl_pub_ = nh_.advertise("pcl", 10); + } + else { + front_scan_sub_.shutdown(); + rear_scan_sub_.shutdown(); + scan_pub_.shutdown(); + pcl_pub_.shutdown(); + } + } + + return true; +} + +void ScansMerger::frontScanCallback(const sensor_msgs::LaserScan::ConstPtr front_scan) { + if (front_scan_received_) + points_.clear(); + + if (!rear_scan_received_ || rear_scan_error_) + stamp_ = front_scan->header.stamp; + + geometry_msgs::Point32 local_point, base_point; + float phi = front_scan->angle_min; + + for (const float r : front_scan->ranges) { + if (r > front_scan->range_min && r < front_scan->range_max) { + local_point.x = r * cos(phi); + local_point.y = r * sin(phi); + + base_point = transformPoint(local_point, front_tf_.x, front_tf_.y, front_tf_.theta); + + if (checkPointInLimits(base_point, p_min_x_range_, p_max_x_range_, p_min_y_range_, p_max_y_range_)) + points_.push_back(base_point); + } + + phi += front_scan->angle_increment; + } + + front_scan_received_ = true; + front_scan_error_ = false; + + if (rear_scan_received_ || rear_scan_error_) + publishAll(); + else + rear_scan_error_ = true; +} + +void ScansMerger::rearScanCallback(const sensor_msgs::LaserScan::ConstPtr rear_scan) { + if (rear_scan_received_) + points_.clear(); + + if (!front_scan_received_ || front_scan_error_) + stamp_ = rear_scan->header.stamp; + + geometry_msgs::Point32 local_point, base_point; + float phi = rear_scan->angle_min; + + for (const float r : rear_scan->ranges) { + if (r > rear_scan->range_min && r < rear_scan->range_max) { + local_point.x = r * cos(phi); + local_point.y = r * sin(phi); + + base_point = transformPoint(local_point, rear_tf_.x, rear_tf_.y, rear_tf_.theta); + + if (checkPointInLimits(base_point, p_min_x_range_, p_max_x_range_, p_min_y_range_, p_max_y_range_)) + points_.push_back(base_point); + } + + phi += rear_scan->angle_increment; + } + + rear_scan_received_ = true; + rear_scan_error_ = false; + + if (front_scan_received_ || front_scan_error_) + publishAll(); + else + front_scan_error_ = true; +} + +void ScansMerger::publishScan() { + sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan); + + scan_msg->header.frame_id = p_frame_id_; + scan_msg->header.stamp = stamp_; + scan_msg->angle_min = -M_PI; + scan_msg->angle_max = M_PI; + scan_msg->angle_increment = 2.0 * M_PI / (p_ranges_num_ - 1); + scan_msg->time_increment = 0.0; + scan_msg->scan_time = 0.1; + scan_msg->range_min = p_min_scanner_range_; + scan_msg->range_max = p_max_scanner_range_; + + ranges_.assign(p_ranges_num_, 2.0 * p_max_scanner_range_); + + for (auto& point : points_) { + float angle = atan2(point.y, point.x); + float range = sqrt(pow(point.x, 2.0) + pow(point.y, 2.0)); + + int idx = static_cast(p_ranges_num_ * (angle + M_PI) / (2.0 * M_PI)); + + if (ranges_[idx] > range) + ranges_[idx] = range; + } + + for (int jdx = 0; jdx < ranges_.size(); ++jdx) + if (ranges_[jdx] < p_min_scanner_range_ || ranges_[jdx] > p_max_scanner_range_) + ranges_[jdx] = nan(""); + + scan_msg->ranges = ranges_; + + scan_pub_.publish(scan_msg); +} + +void ScansMerger::publishPCL() { + sensor_msgs::PointCloudPtr pcl_msg(new sensor_msgs::PointCloud); + + pcl_msg->header.frame_id = p_frame_id_; + pcl_msg->header.stamp = ros::Time::now(); + pcl_msg->points = points_; + + pcl_pub_.publish(pcl_msg); +} + +void ScansMerger::publishAll() { + if (p_publish_scan_) + publishScan(); + + if (p_publish_pcl_) + publishPCL(); + + front_scan_received_ = false; + rear_scan_received_ = false; + + points_.clear(); +}