model://chassis/meshes/chassisV5.dae
diff --git a/src/models_pkg/lidar/lidar.xacro b/src/models_pkg/lidar/lidar.xacro
new file mode 100644
index 0000000..e9bc59e
--- /dev/null
+++ b/src/models_pkg/lidar/lidar.xacro
@@ -0,0 +1,54 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Red
+
+
+ 0 0 0 0 0 0
+ true
+ 10
+
+
+
+ 360
+ -3.14
+ 3.14
+
+
+
+ 0.3
+ 12
+
+
+
+
+ ~/out:=scan
+
+ sensor_msgs/LaserScan
+ laser_frame
+
+
+
+
+
\ No newline at end of file
diff --git a/src/models_pkg/rcCar_assembly/model.sdf b/src/models_pkg/rcCar_assembly/model.sdf
index 12d4b03..1c9c800 100644
--- a/src/models_pkg/rcCar_assembly/model.sdf
+++ b/src/models_pkg/rcCar_assembly/model.sdf
@@ -309,13 +309,7 @@
-
-
- chassis::link
- lidar::lidar_link
- 0 0 0 0 -0 0
-
-
+
0.26
0.162
diff --git a/src/models_pkg/rcCar_assembly/model.urdf b/src/models_pkg/rcCar_assembly/model.urdf
index 783c7e8..00f4bfc 100644
--- a/src/models_pkg/rcCar_assembly/model.urdf
+++ b/src/models_pkg/rcCar_assembly/model.urdf
@@ -5,28 +5,28 @@
-
+
-
+
-
+
-
+
@@ -60,7 +60,7 @@
-
+
diff --git a/src/models_pkg/rcCar_assembly/test.urdf b/src/models_pkg/rcCar_assembly/test.urdf
new file mode 100644
index 0000000..a2a5d2f
--- /dev/null
+++ b/src/models_pkg/rcCar_assembly/test.urdf
@@ -0,0 +1,244 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/models_pkg/rcCar_assembly_REC/model.sdf b/src/models_pkg/rcCar_assembly_REC/model.sdf
index 2fe2e4c..3006377 100644
--- a/src/models_pkg/rcCar_assembly_REC/model.sdf
+++ b/src/models_pkg/rcCar_assembly_REC/model.sdf
@@ -385,7 +385,7 @@
0.0
0.0
- /
+ automobile
diff --git a/src/obstacle_detector/CMakeLists.txt b/src/obstacle_detector/CMakeLists.txt
new file mode 100644
index 0000000..d2d2ba1
--- /dev/null
+++ b/src/obstacle_detector/CMakeLists.txt
@@ -0,0 +1,130 @@
+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 std_srvs geometry_msgs sensor_msgs tf laser_geometry 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 scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui
+ CATKIN_DEPENDS roscpp nodelet rviz std_msgs std_srvs geometry_msgs sensor_msgs tf laser_geometry message_runtime
+)
+
+include_directories(include ${catkin_INCLUDE_DIRS} ${ARMADILLO_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
+
+#
+# Build libs
+#
+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(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/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 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 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 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/src/obstacle_detector/README.md b/src/obstacle_detector/README.md
new file mode 100644
index 0000000..ecc8cfb
--- /dev/null
+++ b/src/obstacle_detector/README.md
@@ -0,0 +1,270 @@
+# 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.
+
+-----------------------
+
+ 
+
+ 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
+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 unchanged (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.
+
+The input laser scans are firstly rectified to incorporate the motion of the scanner in time (see `laser_geometry` package). Next, two PCLs obtained from the previous step are synchronized and transformed into the target coordinate frame at the current point in time.
+
+-----------------------
+
+ 
+
+ Fig. 2. Visual example of `scans_merger` output.
+
+
+-----------------------
+
+The resulting messages contain geometric data described with respect to a specific coordinate frame (e.g. `robot`). Assuming that the coordinate frames attached to two laser scanners are called `front_scanner` and `rear_scanner`, both transformation from `robot` frame to `front_scanner` frame and from `robot` frame to `rear_scanner` frame must be provided. The node allows to artificially restrict measured points to some rectangular region around the `robot` frame as well as to limit the resulting laser scan range. The points falling behind this region will be discarded.
+
+Even if only one laser scanner is used, the node can be useful for simple data pre-processing, e.g. rectification, range restriction or recalculation of points to a different coordinate frame. The node uses the following set of local parameters:
+
+* `~active` (`bool`, default: `true`) - active/sleep mode,
+* `~publish_scan` (`bool`, default: `false`) - publish the merged laser scan message,
+* `~publish_pcl` (`bool`, default: `true`) - publish the merged point cloud message,
+* `~ranges_num` (`int`, default: `1000`) - number of ranges (circular sectors) contained in the 360 deg laser scan message,
+* `~min_scanner_range` (`double`, default: `0.05`) - minimal allowable range value for produced laser scan message,
+* `~max_scanner_range` (`double`, default: `10.0`) - maximal allowable range value for produced laser scan message,
+* `~min_x_range` (`double`, default: `-10.0`) - limitation for points coordinates (points with coordinates behind these limitations will be discarded),
+* `~max_x_range` (`double`, default: `10.0`) - as above,
+* `~min_y_range` (`double`, default: `-10.0`) - as above,
+* `~max_y_range` (`double`, default: `10.0`) - as above,
+* `~fixed_frame_id` (`string`, default: `map`) - name of the fixed coordinate frame used for scan rectification in time,
+* `~target_frame_id` (`string`, default: `map`) - name of the coordinate frame used as the origin for the produced laser scan or point cloud.
+
+In the original implementation, the `~target_frame_id` is the `robot` frame. However, with a moving robot, this causes the Kalman filters later on to think that the points are moving even though the robot is moving w. r. t. the map. That's why we enhance the point cloud with "range" (distance) information in the `scans_merger` node. This means that even though the origin of the target frame does not match the lidar scan origin, we still preserve the distance of each point to the scanner that produced the point. For that reason, it is absolutely essential to run the scans through the `scans_merger` node, even if not merging scans and to set `publish_pcl` to `true`.
+
+The package comes with Rviz panel for this 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 PCL message must be ordered in the angular fashion, because the algorithm exploits the polar nature of laser scanners.
+
+-----------------------
+
+ 
+
+ Fig. 4. Visual example of `obstacle_extractor` output.
+
+
+-----------------------
+
+The input points are firstly grouped into subsets and marked as visible or not (if a group is in front of neighbouring groups, it is visible. Otherwise it is assumed to be occluded). The algorithm extracts segments from each points subset. Next, the segments are checked for possible merging between each other. The circular obstacles are then extracted from segments and also merged if possible. Resulting set of obstacles can be transformed to a dedicated coordinate frame.
+
+The node is configurable with the following set of local parameters:
+
+* `~active` (`bool`, default: `true`) - active/sleep mode,
+* `~use_scan` (`bool`, default: `false`) - use laser scan messages,
+* `~use_pcl` (`bool`, default: `true`) - use point cloud messages (if both scan and pcl are chosen, scans will have priority),
+* `~use_split_and_merge` (`bool`, default: `true`) - choose wether to use Iterative End Point Fit (false) or Split And Merge (true) algorithm to detect segments,
+* `~circles_from_visibles` (`bool`, default: `true`) - detect circular obstacles only from fully visible (not occluded) segments,
+* `~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,
+* `~min_group_points` (`int`, default: `5`) - minimum number of points comprising a group to be further processed,
+* `~max_group_distance` (`double`, default: `0.1`) - if the distance between two points is greater than this value, start a new group,
+* `~distance_proportion` (`double`, default: `0.00628`) - enlarge the allowable distance between points proportionally to the range of point (use scan angle increment in radians),
+* `~max_split_distance` (`double`, default: `0.2`) - if a point in group lays further from a leading line than this value, split the group,
+* `~max_merge_separation` (`double`, default: `0.2`) - if distance between obstacles is smaller than this value, consider merging them,
+* `~max_merge_spread` (`double`, default: `0.2`) - merge two segments if all of their extreme points lay closer to the leading line than this value,
+* `~max_circle_radius` (`double`, default: `0.6`) - if a circle would have greater radius than this value, skip it,
+* `~radius_enlargement` (`double`, default: `0.25`) - artificially enlarge the circles radius by this value,
+* `~frame_id` (`string`, default: `map`) - name of the coordinate frame used as origin for produced obstacles (used only if `transform_coordinates` flag is set to true).
+
+The package comes with Rviz panel for this node.
+
+-----------------------
+
+ 
+
+ Fig. 5. Rviz panel for the `obstacle_extractor` node.
+
+
+-----------------------
+
+### 1.3. The obstacle_tracker node
+
+This node tracks and filters the circular and line segment 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 to both, the circular obstacles and line segments. The tracked obstacles are supplemented with additional information on their velocity.
+
+-----------------------
+
+ 
+
+ 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),
+* `~frame_id` (`string`, default: `map`) - name of the coordinate frame in which the obstacles are described,
+
+The package comes with Rviz panel for this node.
+
+-----------------------
+
+ 
+
+ Fig. 7. Rviz panel for the `obstacle_tracker` node.
+
+
+-----------------------
+
+### 1.4. The obstacle_publisher node
+
+The auxiliary node which allows to publish a set of virtual, circular obstacles in the form of message of type `obstacles_detector/Obstacles` under topic `obstacles`. The node is mostly used for off-line tests. The following parameters are used to configure the node:
+
+* `~active` (`bool`, default: `true`) - active/sleep mode,
+* `~reset` (`bool`, default: `false`) - reset time for obstacles motion calculation (used by dedicated Rviz panel),
+* `~fusion_example` (`bool`, default: `false`) - produce obstacles showing fusion,
+* `~fission_example` (`bool`, default: `false`) - produce obstacles showing fission,
+* `~radius_margin` (`double`, default: `0.25`) - artificially enlarge the circles radius by this value in meters,
+* `~loop_rate` (`double`, default: `10.0`) - the main loop rate in Hz,
+* `~frame_id` (`string`, default: `map`) - name of the coordinate frame in which the obstacles are described.
+
+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.
+
+-----------------------
+
+ 
+
+ Fig. 8. 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.
+
+## 5. Other interesting forks of this repository
+
+- Lots of improvements:
+
+ https://github.com/6RiverSystems/obstacle_detector/tree/6RS
+
+ Melodic:
+
+ https://github.com/6RiverSystems/obstacle_detector/tree/6RS-melodic
+ https://github.com/6RiverSystems/obstacle_detector/tree/feature/lidar-tracking
+
+- Rectangular obstacles:
+
+ https://github.com/tysik/obstacle_detector/compare/master...icsl-Jeon:obstacle_detector:master
+
+- Dynamic map subtraction, robot base velocity compensation:
+
+ https://github.com/tysik/obstacle_detector/compare/master...bangyii:obstacle_detector:master
+
+- Fix start/end of laser bug:
+
+ https://github.com/tysik/obstacle_detector/compare/master...seiga-k:obstacle_detector:master
+
+- Obstacles to PoseArray conversion for better Visualization:
+
+ https://github.com/tysik/obstacle_detector/compare/master...gaoyangu:obstacle_detector:rikirobot
+
+- More obstacle information (can it move?) publishing:
+
+ https://github.com/tysik/obstacle_detector/compare/master...Xvdgeest:obstacle_detector:master
+
+- Publishing of Polygons in addition to Segments:
+
+ https://github.com/tysik/obstacle_detector/compare/master...FelixTFD:obstacle_detector:master
+
+- Different circle merging logic:
+
+ https://github.com/tysik/obstacle_detector/compare/master...ljburtz:obstacle_detector:circle_merge_logic
+
+- Cone detection:
+
+ https://github.com/tysik/obstacle_detector/compare/master...Magnusgaertner:obstacle_detector:master
+
+Author:
+_Mateusz Przybyla_
+
diff --git a/src/obstacle_detector/icons/classes/Obstacle Extractor.png b/src/obstacle_detector/icons/classes/Obstacle Extractor.png
new file mode 100644
index 0000000..bf97282
Binary files /dev/null and b/src/obstacle_detector/icons/classes/Obstacle Extractor.png differ
diff --git a/src/obstacle_detector/icons/classes/Obstacle Publisher.png b/src/obstacle_detector/icons/classes/Obstacle Publisher.png
new file mode 100644
index 0000000..61a4902
Binary files /dev/null and b/src/obstacle_detector/icons/classes/Obstacle Publisher.png differ
diff --git a/src/obstacle_detector/icons/classes/Obstacle Tracker 2.png b/src/obstacle_detector/icons/classes/Obstacle Tracker 2.png
new file mode 100644
index 0000000..cf74075
Binary files /dev/null and b/src/obstacle_detector/icons/classes/Obstacle Tracker 2.png differ
diff --git a/src/obstacle_detector/icons/classes/Obstacle Tracker.png b/src/obstacle_detector/icons/classes/Obstacle Tracker.png
new file mode 100644
index 0000000..4c1c767
Binary files /dev/null and b/src/obstacle_detector/icons/classes/Obstacle Tracker.png differ
diff --git a/src/obstacle_detector/icons/classes/Obstacles.png b/src/obstacle_detector/icons/classes/Obstacles.png
new file mode 100644
index 0000000..ed91d08
Binary files /dev/null and b/src/obstacle_detector/icons/classes/Obstacles.png differ
diff --git a/src/obstacle_detector/icons/classes/Scans Merger.png b/src/obstacle_detector/icons/classes/Scans Merger.png
new file mode 100644
index 0000000..b1dff2c
Binary files /dev/null and b/src/obstacle_detector/icons/classes/Scans Merger.png differ
diff --git a/src/obstacle_detector/include/obstacle_detector/displays/circle_visual.h b/src/obstacle_detector/include/obstacle_detector/displays/circle_visual.h
new file mode 100644
index 0000000..22011c7
--- /dev/null
+++ b/src/obstacle_detector/include/obstacle_detector/displays/circle_visual.h
@@ -0,0 +1,80 @@
+/*
+ * 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
+#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 setFramePose(const Ogre::Vector3& position, 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_;
+ boost::shared_ptr velocity_;
+ rviz::MovableText* text_;
+
+ Ogre::Vector3 center_position_;
+
+ Ogre::SceneNode* frame_node_obstacle_;
+ Ogre::SceneNode* frame_node_margin_;
+ Ogre::SceneNode* frame_node_velocity_;
+ Ogre::SceneNode* frame_node_text_;
+ Ogre::SceneManager* scene_manager_;
+};
+
+} // end namespace obstacles_display
diff --git a/src/obstacle_detector/include/obstacle_detector/displays/obstacles_display.h b/src/obstacle_detector/include/obstacle_detector/displays/obstacles_display.h
new file mode 100644
index 0000000..7538234
--- /dev/null
+++ b/src/obstacle_detector/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/src/obstacle_detector/include/obstacle_detector/displays/segment_visual.h b/src/obstacle_detector/include/obstacle_detector/displays/segment_visual.h
new file mode 100644
index 0000000..3807b6f
--- /dev/null
+++ b/src/obstacle_detector/include/obstacle_detector/displays/segment_visual.h
@@ -0,0 +1,79 @@
+/*
+ * 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
+#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 setFramePose(const Ogre::Vector3& position, const Ogre::Quaternion& orientation);
+ void setColor(float r, float g, float b, float a);
+ void setWidth(float w);
+
+private:
+ boost::shared_ptr line_;
+ boost::shared_ptr velocity_;
+ rviz::MovableText* text_;
+
+ Ogre::Vector3 center_position_;
+
+ Ogre::SceneNode* frame_node_line_;
+ Ogre::SceneNode* frame_node_velocity_;
+ Ogre::SceneNode* frame_node_text_;
+ Ogre::SceneManager* scene_manager_;
+};
+
+} // end namespace obstacles_display
diff --git a/src/obstacle_detector/include/obstacle_detector/obstacle_extractor.h b/src/obstacle_detector/include/obstacle_detector/obstacle_extractor.h
new file mode 100644
index 0000000..0e10ff7
--- /dev/null
+++ b/src/obstacle_detector/include/obstacle_detector/obstacle_extractor.h
@@ -0,0 +1,124 @@
+/*
+ * 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);
+ ~ObstacleExtractor();
+
+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_circles_from_visibles_;
+ 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/src/obstacle_detector/include/obstacle_detector/obstacle_publisher.h b/src/obstacle_detector/include/obstacle_detector/obstacle_publisher.h
new file mode 100644
index 0000000..eb10f3b
--- /dev/null
+++ b/src/obstacle_detector/include/obstacle_detector/obstacle_publisher.h
@@ -0,0 +1,93 @@
+/*
+ * 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);
+ ~ObstaclePublisher();
+
+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/src/obstacle_detector/include/obstacle_detector/obstacle_tracker.h b/src/obstacle_detector/include/obstacle_detector/obstacle_tracker.h
new file mode 100644
index 0000000..856bfe5
--- /dev/null
+++ b/src/obstacle_detector/include/obstacle_detector/obstacle_tracker.h
@@ -0,0 +1,128 @@
+/*
+ * 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 "obstacle_detector/utilities/tracked_circle_obstacle.h"
+#include "obstacle_detector/utilities/tracked_segment_obstacle.h"
+#include "obstacle_detector/utilities/math_utilities.h"
+
+namespace obstacle_detector
+{
+
+class ObstacleTracker {
+public:
+ ObstacleTracker(ros::NodeHandle& nh, ros::NodeHandle& nh_local);
+ ~ObstacleTracker();
+
+private:
+ bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
+ void timerCallback(const ros::TimerEvent&);
+ void obstaclesCallback(const obstacle_detector::Obstacles::ConstPtr new_obstacles);
+ void obstaclesCallbackCircles(const obstacle_detector::Obstacles::ConstPtr new_obstacles);
+ void obstaclesCallbackSegments(const obstacle_detector::Obstacles::ConstPtr new_obstacles);
+ void odomCallback(const nav_msgs::Odometry::ConstPtr &msg);
+
+ void initialize() { std_srvs::Empty empt; updateParams(empt.request, empt.response); }
+
+ double obstacleCostFunction(const CircleObstacle& new_obstacle, const CircleObstacle& old_obstacle);
+ double obstacleCostFunction(const SegmentObstacle& new_obstacle, const SegmentObstacle& old_obstacle);
+ void calculateCostMatrix(const std::vector& new_obstacles, arma::mat& cost_matrix);
+ void calculateCostMatrix(const std::vector& new_obstacles, arma::mat& cost_matrix);
+ void calculateRowMinIndices(const arma::mat& cost_matrix, std::vector& row_min_indices, const int T, const int U);
+ void calculateColMinIndices(const arma::mat& cost_matrix, std::vector& col_min_indices, const int T, const int U);
+
+ 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 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
-From new parkour:
+# From new parkour:
-From added RVIZ:
-
+# From added RVIZ:
+# Obstacle detection
+
+
+
+# Traffic lights plugin:
+
+
+
# BFMC Simulator Project
The project contains the entire Gazebo simulator.
@@ -25,10 +133,23 @@ From KOU-Mekatronom team:
- Added urdf and lidar.sdf
- It has laser_scan now topic name is ```/automobile/scan``` for bostacle_detection.
- Added TF2 package the tf tree visualization ```frames.pdf```
+- Added traffic lights publisher, ```src/sim_pkg/launch/sublaunchers/traffic_lights.launch```
+- In your main code you need to subscribe ``` automobile/trafficlight/master,slave,start topics```
Tips on how to install and work on it, can be found in the
-
## The documentation is available in details here:
[Documentation](https://bosch-future-mobility-challenge-documentation.readthedocs-hosted.com/data/simulator.html)
+
+This project includes the algorithms has been made from KOU-Mekatronom::
+- It has robot_localization package, you can fuse the gps and IMU data easily.
+- Robot_localization package config path is ```src/example/config/ekf_localization.yaml```
+- Added urdf and lidar.sdf
+- It has laser_scan now topic name is ```/automobile/scan``` for bostacle_detection.
+- Added TF2 package the tf tree visualization ```frames.pdf```
+- Added traffic lights publisher, ```src/sim_pkg/launch/sublaunchers/traffic_lights.launch```
+- In your main code you need to subscribe ``` automobile/trafficlight/master,slave,start topics```
+
+# Contributors
+[Mehmet Baha Dursun](https://github.com/Renbago)
diff --git a/autonomus_vehicle.docx.pdf b/autonomus_vehicle.docx.pdf
new file mode 100644
index 0000000..735c7e3
Binary files /dev/null and b/autonomus_vehicle.docx.pdf differ
diff --git a/obstacle_detection.png b/obstacle_detection.png
new file mode 100644
index 0000000..f517814
Binary files /dev/null and b/obstacle_detection.png differ
diff --git a/rviz.png b/rviz.png
index b3b00f5..f517814 100644
Binary files a/rviz.png and b/rviz.png differ
diff --git a/src/depends/install_deps.sh b/src/depends/install_deps.sh
new file mode 100644
index 0000000..b7f9c76
--- /dev/null
+++ b/src/depends/install_deps.sh
@@ -0,0 +1,54 @@
+#!/bin/bash
+set -e
+REPO_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )/../" && pwd )"
+TMP_DIR="/tmp"
+
+install_ipopt() {
+ echo "Prepare to install IPOPT ..."
+ IPOPT_URL="https://e.coding.net/tmp-code/ipopt-3.12.4.git"
+ sudo apt-get -y install \
+ gfortran \
+ cmake \
+ build-essential \
+ gcc \
+ g++
+ sudo ldconfig
+ if ( ldconfig -p | grep libipopt ); then
+ echo "Ipopt is already installed......."
+ else
+ echo "Start installing Ipopt, version: 3.12.4 .........."
+ pwd
+ cd $TMP_DIR
+ pwd
+ rm -rf ipopt-3.12.4 && git clone "$IPOPT_URL" && cd ipopt-3.12.4
+ # configure,build and install the IPOPT
+ echo "Configuring and building IPOPT ..."
+ ./configure --prefix /usr/local
+ make -j$(nproc)
+ make test
+ sudo make install
+ if (grep 'export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH' $HOME/.bashrc); then
+ echo "LD_LIBRARY_PATH has been set."
+ else
+ echo 'export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH' >> $HOME/.bashrc
+ fi
+ sudo ldconfig
+ echo "IPOPT installed successfully"
+ source $HOME/.bashrc
+ fi
+ cd $REPO_DIR
+}
+
+main() {
+ #sudo apt-get update
+ install_ipopt
+ # install_cppad
+ # install_benchmark
+ # install_glog
+ # install_gflags
+ # install_grid_map
+ # install_osqp_eigen
+ # clone_other_ros_pkgs
+}
+
+main
\ No newline at end of file
diff --git a/src/example/CMakeLists.txt b/src/example/CMakeLists.txt
index f8dce85..df9ace0 100644
--- a/src/example/CMakeLists.txt
+++ b/src/example/CMakeLists.txt
@@ -1,18 +1,92 @@
cmake_minimum_required(VERSION 3.0.2)
project(example)
+# Ensure C++17 is used
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_WARN_DEPRECATED OFF)
+set(CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION OFF)
+
+# Standard C++ settings
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-std=c++17 -Wall -Wextra)
+endif()
+
+
+# Find required packages
+find_package(Casadi REQUIRED)
+find_package(pugixml REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+message(STATUS "Casadi include dir: ${Casadi_INCLUDE_DIRS}")
+message(STATUS "Casadi libraries: ${Casadi_LIBRARIES}")
+
+# Find catkin packages
find_package(catkin REQUIRED COMPONENTS
- rospy
+ geometry_msgs
std_msgs
+ rospy
+ roscpp
+ sensor_msgs
utils
+ cv_bridge
+ image_transport
+ message_generation
)
+# Add message files
+add_message_files(
+ FILES
+ MekatronomYolo.msg
+)
+
+# Generate messages
+generate_messages(
+ DEPENDENCIES
+ std_msgs
+)
+
+# Catkin specific configuration
catkin_package(
-# INCLUDE_DIRS include
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+ CATKIN_DEPENDS geometry_msgs std_msgs rospy utils roscpp sensor_msgs message_runtime
+ DEPENDS Eigen3
)
+# Include directories
include_directories(
-# include
+ include
${catkin_INCLUDE_DIRS}
+ ${OpenCV_INCLUDE_DIRS}
+ ${EIGEN3_INCLUDE_DIR}
+)
+
+# Add executable
+add_executable(mpc_node scripts/mpc_node.cpp scripts/MpcNode.cpp)
+add_dependencies(mpc_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+# Link libraries
+target_link_libraries(mpc_node
+ ${catkin_LIBRARIES}
+ casadi
+ ${Casadi_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+ pugixml
+)
+
+# Installation
+install(TARGETS mpc_node
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
+# Optionally install config and launch directories
+# install(DIRECTORY config launch
+# DESTINATION share/${PROJECT_NAME}
+# )
diff --git a/src/example/config/Competition_track_graph.graphml b/src/example/config/Competition_track_graph.graphml
new file mode 100644
index 0000000..5f51a09
--- /dev/null
+++ b/src/example/config/Competition_track_graph.graphml
@@ -0,0 +1,3788 @@
+
+