├── .travis.yml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── bagfile └── time.bag ├── build_docker.sh ├── config ├── controller.yaml ├── diff_drive.yaml ├── dynamic_avoidance.rviz ├── dynamic_avoidance.yaml ├── fwdis.yaml └── urdf.rviz ├── docker-compose.yml ├── docker ├── Dockerfile └── ros_entrypoint.sh ├── include └── dynamic_obstacle_avoidance_planner │ ├── avoidance_path_planner.h │ ├── dynamic_local_costmap_generator.h │ ├── local_costmap_generator.h │ ├── measurer.h │ ├── obstacle_tracker_kf.h │ ├── static_local_costmap_generator.h │ ├── tf_to_obstacles.h │ └── velocity_arrow_to_obstacle_converter.h ├── launch ├── avoidance_path_planner.launch ├── collision_detector.launch ├── diff_drive.launch ├── diff_drive_mpc.launch ├── diff_drive_sim.launch ├── dynamic_local_costmap.launch ├── dynamic_obstacle_avoidance_core.launch ├── for_mocap_experiment.launch ├── for_rviz_simulation.launch ├── fwdis.launch ├── fwdis_mpc.launch ├── load_param_diff_drive.launch ├── load_param_dynamic_avoidance.launch ├── obstacle_trajectory_predictor_kf.launch ├── path_to_local_goal.launch ├── robot_predictor.launch └── tf_to_obstacles.launch ├── package.xml ├── run_docker.sh ├── run_docker_with_vicon.sh ├── scripts ├── evaluator.py ├── log4csv.py ├── old_test_scripts │ ├── dynamic_avoidance_test.sh │ ├── dynamic_avoidance_test_docker.sh │ ├── dynamic_avoidance_test_docker_robot.sh │ ├── dynamic_avoidance_test_fwdis.sh │ ├── dynamic_avoidance_test_fwdis_docker.sh │ ├── dynamic_avoidance_test_fwdis_docker_robot.sh │ ├── dynamic_avoidance_test_vicon.sh │ ├── dynamic_avoidance_test_vicon_fwdis.sh │ ├── gazebo_dwa_sim.sh │ ├── gazebo_sim.sh │ └── mpc_test.sh ├── sim_3dof.py ├── sub_laser.py └── test_robomech.py └── src ├── avoidance_path_planner.cpp ├── base_costmap_tf_publisher.cpp ├── collision_detector.cpp ├── demo └── obstacle_tracker_kf_demo.cpp ├── diff_drive_mpc.cpp ├── diff_drive_sim.cpp ├── dynamic_local_costmap_generator.cpp ├── fwdis_mpc.cpp ├── holonomic_dwa.cpp ├── local_costmap_generator.cpp ├── measurer.cpp ├── obstacle_tracker_kf.cpp ├── odom_to_tf.cpp ├── path_to_local_goal.cpp ├── robot_predictor.cpp ├── sfm_obstacle_simulator.cpp ├── static_local_costmap_generator.cpp ├── tf_to_obstacles.cpp ├── trajectory_logger.cpp ├── velocity_arrow_to_obstacle_converter.cpp └── waypoints_test.cpp /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | 3 | services: 4 | - docker 5 | 6 | dist: xenial 7 | 8 | env: 9 | DOCKER_COMPOSE_VERSION: 1.8.0 10 | 11 | before_install: 12 | - sudo apt-get update 13 | - sudo apt-get -y -q -o Dpkg::Options::="--force-confnew" install docker-ce 14 | - sudo rm /usr/local/bin/docker-compose 15 | - curl -L https://github.com/docker/compose/releases/download/${DOCKER_COMPOSE_VERSION}/docker-compose-`uname -s`-`uname -m` > docker-compose 16 | - chmod +x docker-compose 17 | - sudo mv docker-compose /usr/local/bin 18 | - docker-compose up -d 19 | 20 | script: 21 | - docker-compose run master /bin/bash -c "cd catkin_ws && catkin_make" 22 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(dynamic_obstacle_avoidance_planner) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | 8 | string(ASCII 27 ESC) 9 | 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | tf 15 | cmake_modules 16 | pcl_ros 17 | ) 18 | 19 | find_package(Eigen3 REQUIRED COMPONENTS system) 20 | find_package(PkgConfig REQUIRED) 21 | pkg_check_modules(IPOPT ipopt) 22 | 23 | if(${IPOPT_FOUND}) 24 | message("-- ${ESC}[32mIPOPT was found!!!${ESC}[0m") 25 | else() 26 | message("-- ${ESC}[31mIPOPT was NOT found!!!${ESC}[0m") 27 | message("-- ${ESC}[031mMPC nodes will not be built${ESC}[0m") 28 | endif() 29 | 30 | catkin_package( 31 | INCLUDE_DIRS include 32 | LIBRARIES obstacle_tracker_kf 33 | # CATKIN_DEPENDS roscpp rospy std_msgs message_runtime 34 | # DEPENDS system_lib 35 | ) 36 | 37 | include_directories( 38 | include 39 | ${catkin_INCLUDE_DIRS} 40 | ${Eigen3_INCLUDE_DIRS} 41 | ${IPOPT_INCLUDE_DIRS} 42 | ) 43 | 44 | add_library(obstacle_tracker_kf 45 | src/obstacle_tracker_kf.cpp 46 | ) 47 | add_executable(path_to_local_goal src/path_to_local_goal.cpp) 48 | add_executable(diff_drive_sim src/diff_drive_sim.cpp) 49 | add_executable(robot_predictor src/robot_predictor.cpp) 50 | add_executable(dynamic_local_costmap_generator src/dynamic_local_costmap_generator.cpp) 51 | add_executable(avoidance_path_planner src/avoidance_path_planner.cpp) 52 | add_executable(waypoints_test src/waypoints_test.cpp) 53 | add_executable(base_costmap_tf_publisher src/base_costmap_tf_publisher.cpp) 54 | add_executable(collision_detector src/collision_detector.cpp) 55 | add_executable(trajectory_logger src/trajectory_logger.cpp) 56 | add_executable(velocity_arrow_to_obstacle_converter src/velocity_arrow_to_obstacle_converter.cpp) 57 | add_executable(local_costmap_generator src/local_costmap_generator.cpp) 58 | if(${IPOPT_FOUND}) 59 | add_executable(diff_drive_mpc src/diff_drive_mpc.cpp) 60 | add_executable(fwdis_mpc src/fwdis_mpc.cpp) 61 | endif() 62 | add_executable(tf_to_obstacles src/tf_to_obstacles.cpp) 63 | add_executable(static_local_costmap_generator src/static_local_costmap_generator.cpp) 64 | add_executable(obstacle_tracker_kf_demo src/demo/obstacle_tracker_kf_demo.cpp) 65 | add_executable(sfm_obstacle_simulator src/sfm_obstacle_simulator.cpp) 66 | add_executable(measurer src/measurer.cpp) 67 | 68 | target_link_libraries(path_to_local_goal 69 | ${catkin_LIBRARIES} 70 | ) 71 | target_link_libraries(diff_drive_sim 72 | ${catkin_LIBRARIES} 73 | ) 74 | target_link_libraries(robot_predictor 75 | ${catkin_LIBRARIES} 76 | ) 77 | target_link_libraries(dynamic_local_costmap_generator 78 | ${catkin_LIBRARIES} 79 | obstacle_tracker_kf 80 | ) 81 | target_link_libraries(avoidance_path_planner 82 | ${catkin_LIBRARIES} 83 | ) 84 | target_link_libraries(waypoints_test 85 | ${catkin_LIBRARIES} 86 | ) 87 | target_link_libraries(base_costmap_tf_publisher 88 | ${catkin_LIBRARIES} 89 | ) 90 | target_link_libraries(collision_detector 91 | ${catkin_LIBRARIES} 92 | ) 93 | target_link_libraries(trajectory_logger 94 | ${catkin_LIBRARIES} 95 | ) 96 | target_link_libraries(velocity_arrow_to_obstacle_converter 97 | ${catkin_LIBRARIES} 98 | ) 99 | target_link_libraries(local_costmap_generator 100 | ${catkin_LIBRARIES} 101 | ) 102 | if(${IPOPT_FOUND}) 103 | target_link_libraries(diff_drive_mpc 104 | ${catkin_LIBRARIES} 105 | ${IPOPT_LIBRARIES} 106 | ) 107 | target_link_libraries(fwdis_mpc 108 | ${catkin_LIBRARIES} 109 | ${IPOPT_LIBRARIES} 110 | ) 111 | endif() 112 | target_link_libraries(tf_to_obstacles 113 | ${catkin_LIBRARIES} 114 | ) 115 | target_link_libraries(static_local_costmap_generator 116 | ${catkin_LIBRARIES} 117 | ) 118 | target_link_libraries(obstacle_tracker_kf_demo 119 | ${catkin_LIBRARIES} 120 | obstacle_tracker_kf 121 | ) 122 | target_link_libraries(sfm_obstacle_simulator 123 | ${catkin_LIBRARIES} 124 | ) 125 | target_link_libraries(measurer 126 | ${catkin_LIBRARIES} 127 | ) 128 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, amsl 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # dynamic_obstacle_avoidance_planner 2 | [![Build Status](https://travis-ci.org/amslabtech/dynamic_obstacle_avoidance_planner.svg?branch=master)](https://travis-ci.org/amslabtech/dynamic_obstacle_avoidance_planner) 3 | 4 | Collision prediction based dynamic obstacle avoidance planner 5 | 6 | ## Environment 7 | - Ubuntu16.04 or Ubuntu18.04 8 | 9 | ## Requirement 10 | - Docker 11 | 12 | Using docker is recommended. If you don't want to use docker, see Dockerfile and Reference(below) to setup. 13 | 14 | ## Install and Build 15 | ``` 16 | $ cd your/workspace 17 | $ git clone https://github.com/amslabtech/dynamic_obstacle_avoidance_planner.git 18 | $ ./build.sh 19 | ``` 20 | 21 | ## How to Use 22 | ``` 23 | $ run_docker.sh 24 | $ cd catkin_ws && catkin_make 25 | 26 | open another terminal 27 | 28 | for collision prediction, local costmap generation, and avoidance path planning 29 | $ docker exec -it ros_mpc bash -c "source /ros_entrypoint.sh && roslaunch dynamic_obstacle_avoidance_planner dynamic_obstacle_avoidance_core.launch" 30 | ``` 31 | 32 | ## Reference 33 | - https://github.com/coin-or/Ipopt 34 | - https://coin-or.github.io/CppAD/doc/cppad.htm 35 | -------------------------------------------------------------------------------- /bagfile/time.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/amslabtech/dynamic_obstacle_avoidance_planner/e8d3a883f917cb247529204ab8ebb591247bae69/bagfile/time.bag -------------------------------------------------------------------------------- /build_docker.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker build -t amslabtech/ros_mpc ./docker 4 | -------------------------------------------------------------------------------- /config/controller.yaml: -------------------------------------------------------------------------------- 1 | fwdis: 2 | joint_state_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 50 5 | 6 | front_right_steering_joint: 7 | type: velocity_controllers/JointPositionController 8 | joint: front_right_steering_joint 9 | pid: {p: 5.0, i: 0.0, d: 0.0} 10 | 11 | front_left_steering_joint: 12 | type: velocity_controllers/JointPositionController 13 | joint: front_left_steering_joint 14 | pid: {p: 5.0, i: 0.0, d: 0.0} 15 | 16 | rear_right_steering_joint: 17 | type: velocity_controllers/JointPositionController 18 | joint: rear_right_steering_joint 19 | pid: {p: 5.0, i: 0.0, d: 0.0} 20 | 21 | rear_left_steering_joint: 22 | type: velocity_controllers/JointPositionController 23 | joint: rear_left_steering_joint 24 | pid: {p: 5.0, i: 0.0, d: 0.0} 25 | 26 | front_right_wheel_joint: 27 | type: velocity_controllers/JointVelocityController 28 | joint: front_right_wheel_joint 29 | 30 | front_left_wheel_joint: 31 | type: velocity_controllers/JointVelocityController 32 | joint: front_left_wheel_joint 33 | 34 | rear_right_wheel_joint: 35 | type: velocity_controllers/JointVelocityController 36 | joint: rear_right_wheel_joint 37 | 38 | rear_left_wheel_joint: 39 | type: velocity_controllers/JointVelocityController 40 | joint: rear_left_wheel_joint 41 | -------------------------------------------------------------------------------- /config/diff_drive.yaml: -------------------------------------------------------------------------------- 1 | diff_drive: 2 | MAX_VELOCITY: 0.6 #[m/s] 3 | MAX_ACCELERATION: 1.0 #[m/ss] 4 | MAX_ANGULAR_ACCELERATION: 1.0 #[rad/ss] 5 | MAX_ANGULAR_VELOCITY: 0.8 #[rad/s] 6 | MAX_WHEEL_ANGULAR_ACCELERATION: 17.5 #[rad/ss] 7 | MAX_WHEEL_ANGULAR_VELOCITY: 30.0 #[rad/s] 8 | ROBOT_FRAME: "base_link" 9 | TREAD: 0.5 #[m] 10 | WHEEL_RADIUS: 0.0775 #[m] 11 | -------------------------------------------------------------------------------- /config/dynamic_avoidance.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /TF1/Frames1 9 | Splitter Ratio: 0.5 10 | Tree Height: 775 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Toolbars: 31 | toolButtonStyle: 2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: false 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 30 51 | Reference Frame: 52 | Value: false 53 | - Class: rviz/TF 54 | Enabled: false 55 | Frame Timeout: 15 56 | Frames: 57 | All Enabled: false 58 | Marker Scale: 1 59 | Name: TF 60 | Show Arrows: false 61 | Show Axes: true 62 | Show Names: true 63 | Tree: 64 | {} 65 | Update Interval: 0 66 | Value: false 67 | - Alpha: 1 68 | Arrow Length: 0.0500000007 69 | Axes Length: 0.300000012 70 | Axes Radius: 0.00999999978 71 | Class: rviz/PoseArray 72 | Color: 255; 25; 0 73 | Enabled: true 74 | Head Length: 0 75 | Head Radius: 0.0299999993 76 | Name: PoseArray 77 | Shaft Length: 0.100000001 78 | Shaft Radius: 0.0500000007 79 | Shape: Arrow (3D) 80 | Topic: /local_costmap_generator/dynamic_local_costmap_generator/obstacles_predicted_path 81 | Unreliable: false 82 | Value: true 83 | - Alpha: 1 84 | Arrow Length: 0.0500000007 85 | Axes Length: 0.300000012 86 | Axes Radius: 0.00999999978 87 | Class: rviz/PoseArray 88 | Color: 0; 170; 0 89 | Enabled: true 90 | Head Length: 0 91 | Head Radius: 0.0500000007 92 | Name: PoseArray 93 | Shaft Length: 0.100000001 94 | Shaft Radius: 0.0500000007 95 | Shape: Arrow (3D) 96 | Topic: /robot_predicted_path 97 | Unreliable: false 98 | Value: true 99 | - Alpha: 0.699999988 100 | Class: rviz/Map 101 | Color Scheme: map 102 | Draw Behind: false 103 | Enabled: true 104 | Name: Map 105 | Topic: /local_costmap 106 | Unreliable: false 107 | Use Timestamp: false 108 | Value: true 109 | - Alpha: 1 110 | Buffer Length: 1 111 | Class: rviz/Path 112 | Color: 0; 0; 255 113 | Enabled: true 114 | Head Diameter: 0.300000012 115 | Head Length: 0.200000003 116 | Length: 0.300000012 117 | Line Style: Billboards 118 | Line Width: 0.100000001 119 | Name: Path 120 | Offset: 121 | X: 0 122 | Y: 0 123 | Z: 0 124 | Pose Color: 255; 85; 255 125 | Pose Style: None 126 | Radius: 0.0299999993 127 | Shaft Diameter: 0.100000001 128 | Shaft Length: 0.100000001 129 | Topic: /intermediate_path 130 | Unreliable: false 131 | Value: true 132 | - Alpha: 1 133 | Arrow Length: 0.300000012 134 | Axes Length: 0.300000012 135 | Axes Radius: 0.00999999978 136 | Class: rviz/PoseArray 137 | Color: 255; 25; 0 138 | Enabled: false 139 | Head Length: 0.0700000003 140 | Head Radius: 0.0299999993 141 | Name: PoseArray 142 | Shaft Length: 0.230000004 143 | Shaft Radius: 0.00999999978 144 | Shape: Arrow (Flat) 145 | Topic: /waypoints 146 | Unreliable: false 147 | Value: false 148 | - Alpha: 1 149 | Buffer Length: 1 150 | Class: rviz/Path 151 | Color: 170; 0; 0 152 | Enabled: true 153 | Head Diameter: 0.300000012 154 | Head Length: 0.200000003 155 | Length: 0.300000012 156 | Line Style: Billboards 157 | Line Width: 0.0299999993 158 | Name: Path 159 | Offset: 160 | X: 0 161 | Y: 0 162 | Z: 0 163 | Pose Color: 255; 85; 255 164 | Pose Style: None 165 | Radius: 0.0299999993 166 | Shaft Diameter: 0.100000001 167 | Shaft Length: 0.100000001 168 | Topic: /intermediate_path2 169 | Unreliable: false 170 | Value: true 171 | - Alpha: 1 172 | Arrow Length: 0.300000012 173 | Axes Length: 0.300000012 174 | Axes Radius: 0.00999999978 175 | Class: rviz/PoseArray 176 | Color: 255; 25; 0 177 | Enabled: true 178 | Head Length: 0.0700000003 179 | Head Radius: 0.0299999993 180 | Name: PoseArray 181 | Shaft Length: 0.230000004 182 | Shaft Radius: 0.00999999978 183 | Shape: Arrow (Flat) 184 | Topic: /mpc_path 185 | Unreliable: false 186 | Value: true 187 | - Alpha: 1 188 | Buffer Length: 1 189 | Class: rviz/Path 190 | Color: 0; 0; 255 191 | Enabled: false 192 | Head Diameter: 0.300000012 193 | Head Length: 0.200000003 194 | Length: 0.300000012 195 | Line Style: Billboards 196 | Line Width: 0.100000001 197 | Name: Path 198 | Offset: 199 | X: 0 200 | Y: 0 201 | Z: 0 202 | Pose Color: 255; 85; 255 203 | Pose Style: None 204 | Radius: 0.0299999993 205 | Shaft Diameter: 0.100000001 206 | Shaft Length: 0.100000001 207 | Topic: /trajectory/robot 208 | Unreliable: false 209 | Value: false 210 | - Alpha: 1 211 | Buffer Length: 1 212 | Class: rviz/Path 213 | Color: 170; 0; 0 214 | Enabled: false 215 | Head Diameter: 0.300000012 216 | Head Length: 0.200000003 217 | Length: 0.300000012 218 | Line Style: Billboards 219 | Line Width: 0.100000001 220 | Name: Path 221 | Offset: 222 | X: 0 223 | Y: 0 224 | Z: 0 225 | Pose Color: 255; 85; 255 226 | Pose Style: None 227 | Radius: 0.0299999993 228 | Shaft Diameter: 0.100000001 229 | Shaft Length: 0.100000001 230 | Topic: /trajectory/obs0 231 | Unreliable: false 232 | Value: false 233 | - Class: rviz/MarkerArray 234 | Enabled: false 235 | Marker Topic: /markers/obs 236 | Name: MarkerArray 237 | Namespaces: 238 | {} 239 | Queue Size: 100 240 | Value: false 241 | - Class: rviz/MarkerArray 242 | Enabled: false 243 | Marker Topic: /markers/robot 244 | Name: MarkerArray 245 | Namespaces: 246 | {} 247 | Queue Size: 100 248 | Value: false 249 | - Class: rviz/Marker 250 | Enabled: true 251 | Marker Topic: /marker/robot 252 | Name: Marker 253 | Namespaces: 254 | robot: true 255 | Queue Size: 1 256 | Value: true 257 | - Class: rviz/MarkerArray 258 | Enabled: true 259 | Marker Topic: /marker/obs 260 | Name: MarkerArray 261 | Namespaces: 262 | obs0: true 263 | obs1: true 264 | Queue Size: 100 265 | Value: true 266 | - Class: rviz/Marker 267 | Enabled: false 268 | Marker Topic: /lines 269 | Name: Marker 270 | Namespaces: 271 | {} 272 | Queue Size: 100 273 | Value: false 274 | - Alpha: 1 275 | Autocompute Intensity Bounds: true 276 | Autocompute Value Bounds: 277 | Max Value: 10 278 | Min Value: -10 279 | Value: true 280 | Axis: Z 281 | Channel Name: intensity 282 | Class: rviz/LaserScan 283 | Color: 255; 255; 255 284 | Color Transformer: Intensity 285 | Decay Time: 0 286 | Enabled: true 287 | Invert Rainbow: false 288 | Max Color: 255; 255; 255 289 | Max Intensity: 4096 290 | Min Color: 0; 0; 0 291 | Min Intensity: 0 292 | Name: LaserScan 293 | Position Transformer: XYZ 294 | Queue Size: 10 295 | Selectable: true 296 | Size (Pixels): 3 297 | Size (m): 0.00999999978 298 | Style: Points 299 | Topic: /raycasting/scan 300 | Unreliable: false 301 | Use Fixed Frame: true 302 | Use rainbow: true 303 | Value: true 304 | Enabled: true 305 | Global Options: 306 | Background Color: 48; 48; 48 307 | Default Light: true 308 | Fixed Frame: map 309 | Frame Rate: 30 310 | Name: root 311 | Tools: 312 | - Class: rviz/Interact 313 | Hide Inactive Objects: true 314 | - Class: rviz/MoveCamera 315 | - Class: rviz/Select 316 | - Class: rviz/FocusCamera 317 | - Class: rviz/Measure 318 | - Class: rviz/SetInitialPose 319 | Topic: /initialpose 320 | - Class: rviz/SetGoal 321 | Topic: /move_base_simple/goal 322 | - Class: rviz/PublishPoint 323 | Single click: true 324 | Topic: /clicked_point 325 | Value: true 326 | Views: 327 | Current: 328 | Class: rviz/Orbit 329 | Distance: 22.5789642 330 | Enable Stereo Rendering: 331 | Stereo Eye Separation: 0.0599999987 332 | Stereo Focal Distance: 1 333 | Swap Stereo Eyes: false 334 | Value: false 335 | Focal Point: 336 | X: 4.32752609 337 | Y: -0.498573571 338 | Z: 0.00433291215 339 | Focal Shape Fixed Size: false 340 | Focal Shape Size: 0.0500000007 341 | Invert Z Axis: false 342 | Name: Current View 343 | Near Clip Distance: 0.00999999978 344 | Pitch: 1.55479634 345 | Target Frame: 346 | Value: Orbit (rviz) 347 | Yaw: 3.14039135 348 | Saved: ~ 349 | Window Geometry: 350 | Displays: 351 | collapsed: false 352 | Height: 1056 353 | Hide Left Dock: false 354 | Hide Right Dock: false 355 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 356 | Selection: 357 | collapsed: false 358 | Time: 359 | collapsed: false 360 | Tool Properties: 361 | collapsed: false 362 | Views: 363 | collapsed: false 364 | Width: 1920 365 | X: 0 366 | Y: 24 367 | -------------------------------------------------------------------------------- /config/dynamic_avoidance.yaml: -------------------------------------------------------------------------------- 1 | dynamic_avoidance: 2 | VREF: 0.5 #[m/s] 3 | PREDICTION_TIME: 2.5 #[s] 4 | RESOLUTION: 0.10 #[s] 5 | RADIUS: 0.6 #[m] 6 | MAX_ANGULAR_ACCELERATION: 3.14 #[rad/ss] 7 | MAX_ANGULAR_VELOCITY: 1.0 #[rad/s] 8 | ROBOT_FRAME: "base_link" 9 | OBSTACLES_FRAME: "obs" 10 | WORLD_FRAME: "map" 11 | VELOCITY_TOPIC_NAME: "/velocity" 12 | INTERMEDIATE_PATH_TOPIC_NAME: "/intermediate_path" 13 | -------------------------------------------------------------------------------- /config/fwdis.yaml: -------------------------------------------------------------------------------- 1 | fwdis: 2 | WHEEL_RADIUS: 0.100 #[m] 3 | WHEEL_BASE: 0.400 #[m] 4 | TREAD: 0.400 #[m] 5 | MAX_WHEEL_ANGULAR_VELOCITY: 30 #[rad/s] 6 | MAX_WHEEL_ANGULAR_ACCELERATION: 60 #[rad/ss] 7 | MAX_STEERING_ANGLE: 2.094395 #[rad] # 120[deg] 8 | ROBOT_FRAME: "base_link" 9 | MAX_VELOCITY: 1.5 #[m/s] 10 | MAX_ACCELERATION: 2.0 #[m/ss] 11 | MAX_ANGULAR_VELOCITY: 2.0 #[rad/s] 12 | MAX_ANGULAR_ACCELERATION: 2.0 #[rad/ss] 13 | 14 | -------------------------------------------------------------------------------- /config/urdf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1/Frames1 10 | - /TF1/Tree1 11 | Splitter Ratio: 0.5 12 | Tree Height: 745 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679016 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 0.5 54 | Class: rviz/RobotModel 55 | Collision Enabled: false 56 | Enabled: true 57 | Links: 58 | All Links Enabled: true 59 | Expand Joint Details: false 60 | Expand Link Details: true 61 | Expand Tree: false 62 | Link Tree Style: Links in Alphabetic Order 63 | base_footprint: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | base_link: 68 | Alpha: 1 69 | Show Axes: true 70 | Show Trail: false 71 | Value: true 72 | front_left_steering_link: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | front_left_wheel_link: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | front_right_steering_link: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | front_right_wheel_link: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | rear_left_steering_link: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | rear_left_wheel_link: 98 | Alpha: 1 99 | Show Axes: false 100 | Show Trail: false 101 | Value: true 102 | rear_right_steering_link: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | Value: true 107 | rear_right_wheel_link: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | Value: true 112 | Name: RobotModel 113 | Robot Description: robot_description 114 | TF Prefix: "" 115 | Update Interval: 0 116 | Value: true 117 | Visual Enabled: true 118 | - Class: rviz/TF 119 | Enabled: true 120 | Frame Timeout: 15 121 | Frames: 122 | All Enabled: false 123 | base_footprint: 124 | Value: false 125 | base_link: 126 | Value: true 127 | front_left_steering_link: 128 | Value: false 129 | front_left_wheel_link: 130 | Value: false 131 | front_right_steering_link: 132 | Value: false 133 | front_right_wheel_link: 134 | Value: false 135 | odom: 136 | Value: true 137 | rear_left_steering_link: 138 | Value: false 139 | rear_left_wheel_link: 140 | Value: false 141 | rear_right_steering_link: 142 | Value: false 143 | rear_right_wheel_link: 144 | Value: false 145 | Marker Scale: 1 146 | Name: TF 147 | Show Arrows: true 148 | Show Axes: true 149 | Show Names: true 150 | Tree: 151 | odom: 152 | base_footprint: 153 | base_link: 154 | front_left_steering_link: 155 | front_left_wheel_link: 156 | {} 157 | front_right_steering_link: 158 | front_right_wheel_link: 159 | {} 160 | rear_left_steering_link: 161 | rear_left_wheel_link: 162 | {} 163 | rear_right_steering_link: 164 | rear_right_wheel_link: 165 | {} 166 | Update Interval: 0 167 | Value: true 168 | - Alpha: 1 169 | Autocompute Intensity Bounds: true 170 | Autocompute Value Bounds: 171 | Max Value: 0 172 | Min Value: 0 173 | Value: true 174 | Axis: Z 175 | Channel Name: intensity 176 | Class: rviz/LaserScan 177 | Color: 255; 255; 255 178 | Color Transformer: Intensity 179 | Decay Time: 0 180 | Enabled: true 181 | Invert Rainbow: false 182 | Max Color: 255; 255; 255 183 | Max Intensity: 0 184 | Min Color: 0; 0; 0 185 | Min Intensity: 0 186 | Name: LaserScan 187 | Position Transformer: XYZ 188 | Queue Size: 10 189 | Selectable: true 190 | Size (Pixels): 3 191 | Size (m): 0.00999999978 192 | Style: Points 193 | Topic: /scan 194 | Unreliable: false 195 | Use Fixed Frame: true 196 | Use rainbow: true 197 | Value: true 198 | - Alpha: 1 199 | Axes Length: 1 200 | Axes Radius: 0.100000001 201 | Class: rviz/Pose 202 | Color: 255; 25; 0 203 | Enabled: true 204 | Head Length: 0.300000012 205 | Head Radius: 0.100000001 206 | Name: Pose 207 | Shaft Length: 1 208 | Shaft Radius: 0.0500000007 209 | Shape: Arrow 210 | Topic: /move_base_simple/goal 211 | Unreliable: false 212 | Value: true 213 | - Alpha: 1 214 | Axes Length: 1 215 | Axes Radius: 0.100000001 216 | Class: rviz/Pose 217 | Color: 255; 25; 0 218 | Enabled: false 219 | Head Length: 0.300000012 220 | Head Radius: 0.100000001 221 | Name: Pose 222 | Shaft Length: 1 223 | Shaft Radius: 0.0500000007 224 | Shape: Arrow 225 | Topic: /fwdis/local_goal/debug 226 | Unreliable: false 227 | Value: false 228 | - Alpha: 1 229 | Arrow Length: 0.300000012 230 | Axes Length: 0.300000012 231 | Axes Radius: 0.00999999978 232 | Class: rviz/PoseArray 233 | Color: 255; 25; 0 234 | Enabled: true 235 | Head Length: 0.0700000003 236 | Head Radius: 0.0299999993 237 | Name: PoseArray 238 | Shaft Length: 0.230000004 239 | Shaft Radius: 0.00999999978 240 | Shape: Arrow (Flat) 241 | Topic: /fwdis/local_path 242 | Unreliable: false 243 | Value: true 244 | - Alpha: 1 245 | Buffer Length: 1 246 | Class: rviz/Path 247 | Color: 25; 255; 0 248 | Enabled: true 249 | Head Diameter: 0.300000012 250 | Head Length: 0.200000003 251 | Length: 0.300000012 252 | Line Style: Lines 253 | Line Width: 0.0299999993 254 | Name: Path 255 | Offset: 256 | X: 0 257 | Y: 0 258 | Z: 0 259 | Pose Color: 255; 85; 255 260 | Pose Style: None 261 | Radius: 0.0299999993 262 | Shaft Diameter: 0.100000001 263 | Shaft Length: 0.100000001 264 | Topic: /path 265 | Unreliable: false 266 | Value: true 267 | Enabled: true 268 | Global Options: 269 | Background Color: 48; 48; 48 270 | Default Light: true 271 | Fixed Frame: odom 272 | Frame Rate: 30 273 | Name: root 274 | Tools: 275 | - Class: rviz/Interact 276 | Hide Inactive Objects: true 277 | - Class: rviz/MoveCamera 278 | - Class: rviz/Select 279 | - Class: rviz/FocusCamera 280 | - Class: rviz/Measure 281 | - Class: rviz/SetInitialPose 282 | Topic: /initialpose 283 | - Class: rviz/SetGoal 284 | Topic: /move_base_simple/goal 285 | - Class: rviz/PublishPoint 286 | Single click: true 287 | Topic: /clicked_point 288 | Value: true 289 | Views: 290 | Current: 291 | Class: rviz/Orbit 292 | Distance: 17.1310329 293 | Enable Stereo Rendering: 294 | Stereo Eye Separation: 0.0599999987 295 | Stereo Focal Distance: 1 296 | Swap Stereo Eyes: false 297 | Value: false 298 | Focal Point: 299 | X: 0.41126892 300 | Y: 0.0142520647 301 | Z: 0.32518965 302 | Focal Shape Fixed Size: true 303 | Focal Shape Size: 0.0500000007 304 | Invert Z Axis: false 305 | Name: Current View 306 | Near Clip Distance: 0.00999999978 307 | Pitch: 0.414797664 308 | Target Frame: 309 | Value: Orbit (rviz) 310 | Yaw: 5.31855726 311 | Saved: ~ 312 | Window Geometry: 313 | Displays: 314 | collapsed: false 315 | Height: 1026 316 | Hide Left Dock: false 317 | Hide Right Dock: false 318 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000378fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000378000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000378fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000378000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000064f0000003efc0100000002fb0000000800540069006d006501000000000000064f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003ca0000037800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 319 | Selection: 320 | collapsed: false 321 | Time: 322 | collapsed: false 323 | Tool Properties: 324 | collapsed: false 325 | Views: 326 | collapsed: false 327 | Width: 1615 328 | X: 3905 329 | Y: 24 330 | -------------------------------------------------------------------------------- /docker-compose.yml: -------------------------------------------------------------------------------- 1 | version: '2' 2 | services: 3 | master: 4 | build: ./docker 5 | volumes: 6 | - ".:/root/catkin_ws/src/this_repository" 7 | container_name: master 8 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:kinetic-ros-base 2 | 3 | SHELL ["/bin/bash", "-c"] 4 | 5 | RUN apt-get update 6 | 7 | RUN apt-get install -y sudo\ 8 | wget\ 9 | lsb-release\ 10 | mesa-utils \ 11 | gfortran \ 12 | && rm -rf /var/lib/apt/lists/* 13 | 14 | RUN echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-latest.list \ 15 | && wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add - 16 | 17 | RUN apt-get update 18 | 19 | RUN apt-get install -y ros-kinetic-pcl-ros* \ 20 | && rm -rf /var/lib/apt/lists/* 21 | 22 | WORKDIR /root 23 | 24 | RUN mkdir -p catkin_ws/src 25 | 26 | RUN cd catkin_ws/src && source /opt/ros/kinetic/setup.bash; catkin_init_workspace 27 | 28 | RUN cd catkin_ws && source /opt/ros/kinetic/setup.bash; catkin_make 29 | 30 | RUN cd /root && echo source /root/catkin_ws/devel/setup.bash >> .bashrc 31 | 32 | ENV ROS_PACKAGE_PATH=/root/catkin_ws:$ROS_PACKAGE_PATH 33 | 34 | ENV ROS_WORKSPACE=/root/catkin_ws 35 | 36 | # install ipopt 37 | ARG IPOPT="Ipopt-3.12.1" 38 | 39 | RUN wget https://www.coin-or.org/download/source/Ipopt/${IPOPT}.tgz && tar -xvzf ${IPOPT}.tgz && rm ${IPOPT}.tgz 40 | 41 | RUN mkdir ${IPOPT}/build 42 | 43 | WORKDIR /root/${IPOPT}/ThirdParty/Blas 44 | 45 | RUN /bin/bash ./get.Blas 46 | 47 | RUN mkdir -p build 48 | 49 | WORKDIR /root/${IPOPT}/ThirdParty/Blas/build 50 | 51 | RUN ../configure --prefix=/usr/local --disable-shared --with-pic && make install 52 | 53 | WORKDIR /root/${IPOPT}/ThirdParty/Lapack 54 | 55 | RUN ./get.Lapack 56 | 57 | RUN mkdir -p build 58 | 59 | WORKDIR /root/${IPOPT}/ThirdParty/Lapack/build 60 | 61 | RUN ../configure --prefix=/usr/local --disable-shared --with-pic \ 62 | --with-blas="/usr/local/lib/libcoinblas.a -lgfortran" 63 | 64 | RUN make install 65 | 66 | WORKDIR /root/${IPOPT}/ThirdParty/Mumps 67 | 68 | RUN ./get.Mumps 69 | 70 | WORKDIR /root/${IPOPT}/ThirdParty/Metis 71 | 72 | RUN ./get.Metis 73 | 74 | WORKDIR /root/${IPOPT}/ThirdParty/ASL 75 | 76 | RUN ./get.ASL 77 | 78 | WORKDIR /root/${IPOPT}/build 79 | 80 | RUN ../configure --prefix=/usr/local --enable-static CXX=g++ CC=gcc F77=gfortran 81 | 82 | RUN make && make test && make install 83 | 84 | # install cppad 85 | RUN apt-get update && apt-get install cppad \ 86 | && rm -rf /var/lib/apt/lists/* 87 | 88 | RUN ln -s /usr/include/eigen3/Eigen /usr/include/Eigen 89 | 90 | WORKDIR /root 91 | 92 | COPY ./ros_entrypoint.sh / 93 | 94 | CMD ["/ros_entrypoint.sh"] 95 | -------------------------------------------------------------------------------- /docker/ros_entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | ldconfig 6 | 7 | source /opt/ros/kinetic/setup.bash 8 | source ~/catkin_ws/devel/setup.bash 9 | 10 | exec "$@" 11 | -------------------------------------------------------------------------------- /include/dynamic_obstacle_avoidance_planner/avoidance_path_planner.h: -------------------------------------------------------------------------------- 1 | #ifndef __AVOIDANCE_PATH_PLANNER_H 2 | #define __AVOIDANCE_PATH_PLANNER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | class AvoidancePathPlanner 14 | { 15 | public: 16 | AvoidancePathPlanner(void); 17 | 18 | void map_callback(const nav_msgs::OccupancyGridConstPtr&); 19 | void waypoints_callback(const geometry_msgs::PoseArrayConstPtr&); 20 | double get_difference(const nav_msgs::Path&, const nav_msgs::Path&); 21 | bool is_contained(const std::vector&, int); 22 | double calculate_astar(const geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, nav_msgs::Path&); 23 | void intersection_on_map(geometry_msgs::PoseStamped&, geometry_msgs::PoseStamped&); 24 | // from dynamic_local_costmap.cpp 25 | bool predict_intersection(const geometry_msgs::Pose&, const geometry_msgs::Pose&, const geometry_msgs::Pose&, const geometry_msgs::Pose&); 26 | // from dynamic_local_costmap.cpp 27 | void predict_intersection_point(const geometry_msgs::Pose&, const geometry_msgs::Pose&, const geometry_msgs::Pose&, const geometry_msgs::Pose&, geometry_msgs::PoseStamped&); 28 | int get_distance_to_global_path(int, int); 29 | inline int get_i_from_x(double); 30 | inline int get_j_from_y(double); 31 | inline int get_index(double, double); 32 | inline int get_heuristic(int, int); 33 | void process(void); 34 | 35 | class Cell 36 | { 37 | public: 38 | Cell(void) 39 | { 40 | is_wall = false; 41 | cost = 0; 42 | step = 0; 43 | sum = -1; 44 | parent_index = -1; 45 | } 46 | 47 | int cost;// attribute cost 48 | int step;// move cost 49 | int sum;// cost + step + heuristic 50 | int parent_index; 51 | bool is_wall; 52 | }; 53 | 54 | private: 55 | double MARGIN_WALL; 56 | std::string INTERMEDIATE_PATH_TOPIC_NAME; 57 | double HZ = 10; 58 | 59 | ros::NodeHandle nh; 60 | ros::NodeHandle local_nh; 61 | ros::Publisher path_pub; 62 | // path2用 63 | ros::Publisher path2_pub; 64 | ros::Subscriber map_sub; 65 | ros::Subscriber waypoints_sub; 66 | geometry_msgs::PoseArray waypoints; 67 | geometry_msgs::PoseStamped waypoint0;// from 68 | geometry_msgs::PoseStamped waypoint1;// to 69 | nav_msgs::OccupancyGrid local_costmap; 70 | nav_msgs::Path path; 71 | nav_msgs::Path path2;// for comparing 72 | nav_msgs::Path previous_path; 73 | // for a* 74 | std::vector cells; 75 | std::vector open_list; 76 | std::vector close_list; 77 | 78 | bool waypoints_received = false; 79 | bool map_received = false; 80 | 81 | }; 82 | #endif// __AVOIDANCE_PATH_PLANNER_H 83 | -------------------------------------------------------------------------------- /include/dynamic_obstacle_avoidance_planner/dynamic_local_costmap_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef __DYNAMIC_LOCAL_COSTMAP_GENERATOR_H 2 | #define __DYNAMIC_LOCAL_COSTMAP_GENERATOR_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include "dynamic_obstacle_avoidance_planner/obstacle_tracker_kf.h" 17 | 18 | class DynamicLocalCostmapGenerator 19 | { 20 | public: 21 | DynamicLocalCostmapGenerator(void); 22 | 23 | void process(void); 24 | void robot_path_callback(const geometry_msgs::PoseArrayConstPtr&); 25 | void obstacle_pose_callback(const geometry_msgs::PoseArrayConstPtr&); 26 | void setup_map(void); 27 | bool predict_intersection(geometry_msgs::Pose, geometry_msgs::Pose, geometry_msgs::Pose, geometry_msgs::Pose); 28 | void predict_intersection_point(geometry_msgs::Pose, geometry_msgs::Pose, geometry_msgs::Pose, geometry_msgs::Pose, geometry_msgs::PoseStamped&); 29 | bool predict_approaching(geometry_msgs::Pose, geometry_msgs::Pose); 30 | bool predict_approaching(geometry_msgs::Pose&, geometry_msgs::Pose&, geometry_msgs::Pose&); 31 | void set_cost_with_velocity(geometry_msgs::PoseStamped&, geometry_msgs::Twist&, geometry_msgs::Twist&); 32 | inline int get_i_from_x(double); 33 | inline int get_j_from_y(double); 34 | inline int get_index(double, double); 35 | inline int get_distance_grid(int, int, int, int); 36 | 37 | private: 38 | double PREDICTION_TIME;// [s], trafjectory prediction time 39 | double HZ; 40 | double DT;// [s] 41 | int PREDICTION_STEP; 42 | double MAP_WIDTH;// [m] 43 | double RESOLUTION;// [m] 44 | double RADIUS;// radius for collision check[m] 45 | double COST_COLLISION; 46 | double MIN_COST; 47 | double MAX_COST; 48 | std::string WORLD_FRAME; 49 | std::string OBS_FRAME; 50 | std::string ROBOT_FRAME; 51 | 52 | ros::NodeHandle nh; 53 | ros::NodeHandle local_nh; 54 | ros::Publisher costmap_pub; 55 | ros::Publisher obstacles_predicted_path_pub; 56 | ros::Subscriber robot_predicted_path_sub; 57 | ros::Subscriber obstacle_pose_sub; 58 | tf::TransformListener listener; 59 | 60 | geometry_msgs::PoseArray robot_path; 61 | geometry_msgs::PoseArray obstacle_paths; 62 | geometry_msgs::PoseArray obstacle_pose; 63 | nav_msgs::OccupancyGrid local_costmap; 64 | int obs_num; 65 | ObstacleTrackerKF tracker; 66 | }; 67 | 68 | #endif// __DYNAMIC_LOCAL_COSTMAP_GENERATOR_H 69 | -------------------------------------------------------------------------------- /include/dynamic_obstacle_avoidance_planner/local_costmap_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef __LOCAL_COSTMAP_GENERATOR_H 2 | #define __LOCAL_COSTMAP_GENERATOR_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | class LocalCostmapGenerator 18 | { 19 | public: 20 | LocalCostmapGenerator(void); 21 | void cloud_callback(const sensor_msgs::PointCloud2ConstPtr&); 22 | void dynamic_callback(const nav_msgs::OccupancyGridConstPtr&); 23 | void process(void); 24 | inline int get_i_from_x(double); 25 | inline int get_j_from_y(double); 26 | inline int get_index(double, double); 27 | 28 | private: 29 | double HZ; 30 | double RADIUS; 31 | double RADIUS_GRID; 32 | std::string ROBOT_FRAME; 33 | std::string WORLD_FRAME; 34 | 35 | ros::NodeHandle nh; 36 | ros::NodeHandle local_nh; 37 | ros::Publisher map_pub; 38 | ros::Subscriber cloud_sub; 39 | ros::Subscriber dynamic_sub; 40 | tf::TransformListener listener; 41 | tf::StampedTransform _transform; 42 | geometry_msgs::TransformStamped transform; 43 | pcl::PointCloud::Ptr cloud_ptr; 44 | bool local_costmap_subscribed; 45 | bool cloud_updated; 46 | double map_max_limit_x; 47 | double map_min_limit_x; 48 | double map_max_limit_y; 49 | double map_min_limit_y; 50 | int max_index; 51 | nav_msgs::OccupancyGrid local_costmap; 52 | }; 53 | 54 | #endif// __LOCAL_COSTMAP_GENERATOR_H 55 | -------------------------------------------------------------------------------- /include/dynamic_obstacle_avoidance_planner/measurer.h: -------------------------------------------------------------------------------- 1 | #ifndef __MEASURER_H 2 | #define __MEASURER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | class Measurer 12 | { 13 | public: 14 | Measurer(void); 15 | 16 | void process(void); 17 | void pose_callback(const geometry_msgs::PoseStampedConstPtr&); 18 | void show_results(void); 19 | 20 | protected: 21 | double GOAL_X; 22 | double GOAL_Y; 23 | double GOAL_TOLERANCE; 24 | 25 | ros::NodeHandle nh; 26 | ros::NodeHandle local_nh; 27 | 28 | ros::Publisher result_pub; 29 | ros::Subscriber pose_sub; 30 | 31 | bool first_callback_flag; 32 | double start_time; 33 | double last_time; 34 | double traveled_distance; 35 | Eigen::Vector2d goal; 36 | }; 37 | 38 | #endif// __MEASURER_H 39 | -------------------------------------------------------------------------------- /include/dynamic_obstacle_avoidance_planner/obstacle_tracker_kf.h: -------------------------------------------------------------------------------- 1 | #ifndef __OBSTACLE_TRACKER_KF_H 2 | #define __OBSTACLE_TRACKER_KF_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | class KalmanFilter 10 | { 11 | public: 12 | KalmanFilter(void); 13 | 14 | Eigen::Matrix4d get_f(double);// transition model 15 | Eigen::Matrix4d get_q(double);// transition noise 16 | 17 | double sigma_a; 18 | private: 19 | }; 20 | 21 | class Obstacle 22 | { 23 | public: 24 | Obstacle(void); 25 | Obstacle(const Obstacle&); 26 | Obstacle(const Eigen::Vector2d&); 27 | 28 | void update(const Eigen::Vector2d&); 29 | void predict(void); 30 | void predict(double); 31 | Eigen::Vector2d get_position(void); 32 | double calculate_likelihood(void); 33 | 34 | Eigen::Vector2d position; 35 | Eigen::Vector4d x; 36 | Eigen::Matrix4d p; 37 | Eigen::Matrix h; 38 | double likelihood; 39 | double lifetime; 40 | double age; 41 | double not_observed_time; 42 | private: 43 | void initialize(void); 44 | 45 | Eigen::Matrix2d r; 46 | KalmanFilter kf; 47 | double last_time; 48 | }; 49 | 50 | class ObstacleTrackerKF 51 | { 52 | public: 53 | ObstacleTrackerKF(void); 54 | 55 | std::map obstacles; 56 | 57 | void set_obstacles_pose(const geometry_msgs::PoseArray&); 58 | void get_velocities(std::vector&); 59 | void get_poses(std::vector&); 60 | 61 | private: 62 | bool associate_obstacles(const std::vector&); 63 | double get_distance(const Obstacle&, const Eigen::Vector2d&); 64 | int get_id_from_index(int); 65 | int get_new_id(void); 66 | bool solve_hungarian_method(Eigen::MatrixXi&); 67 | void update_tracking(const std::vector&); 68 | 69 | double SAME_OBSTACLE_THRESHOLD; 70 | double ERASE_LIKELIHOOD_THREHSOLD; 71 | double NOT_OBSERVED_TIME_THRESHOLD; 72 | double DEFAULT_LIFE_TIME; 73 | std::vector candidates; 74 | }; 75 | 76 | #endif// __OBSTACLE_TRACKER_KF_H 77 | -------------------------------------------------------------------------------- /include/dynamic_obstacle_avoidance_planner/static_local_costmap_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef __STATIC_LOCAL_COSTMAP_GENERATOR_H 2 | #define __STATIC_LOCAL_COSTMAP_GENERATOR_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | class StaticLocalCostmapGenerator 18 | { 19 | public: 20 | StaticLocalCostmapGenerator(void); 21 | void cloud_callback(const sensor_msgs::PointCloud2ConstPtr&); 22 | void process(void); 23 | inline int get_i_from_x(double); 24 | inline int get_j_from_y(double); 25 | inline int get_index(double, double); 26 | 27 | private: 28 | double HZ; 29 | double RADIUS; 30 | double RADIUS_GRID; 31 | double MAP_WIDTH; 32 | double RESOLUTION; 33 | std::string ROBOT_FRAME; 34 | std::string WORLD_FRAME; 35 | 36 | ros::NodeHandle nh; 37 | ros::NodeHandle local_nh; 38 | ros::Publisher map_pub; 39 | ros::Subscriber cloud_sub; 40 | ros::Subscriber dynamic_sub; 41 | tf::TransformListener listener; 42 | tf::StampedTransform _transform; 43 | geometry_msgs::TransformStamped transform; 44 | pcl::PointCloud::Ptr cloud_ptr; 45 | bool cloud_updated; 46 | nav_msgs::OccupancyGrid local_costmap; 47 | }; 48 | 49 | #endif// __STATIC_LOCAL_COSTMAP_GENERATOR_H 50 | -------------------------------------------------------------------------------- /include/dynamic_obstacle_avoidance_planner/tf_to_obstacles.h: -------------------------------------------------------------------------------- 1 | #ifndef __TF_TO_OBSTACLES_H 2 | #define __TF_TO_OBSTACLES_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | class TFToObstacles 14 | { 15 | public: 16 | TFToObstacles(void); 17 | void process(void); 18 | 19 | private: 20 | std::string WORLD_FRAME; 21 | std::string OBS_PREFIX; 22 | double HZ; 23 | bool NOISE_ENABLED; 24 | 25 | ros::NodeHandle nh; 26 | ros::NodeHandle local_nh; 27 | ros::Publisher obstacles_pose_pub; 28 | tf::TransformListener listener; 29 | 30 | std::random_device seed; 31 | std::mt19937 engine; 32 | std::normal_distribution<> dist; 33 | 34 | }; 35 | 36 | #endif// __TF_TO_OBSTACLES_H 37 | -------------------------------------------------------------------------------- /include/dynamic_obstacle_avoidance_planner/velocity_arrow_to_obstacle_converter.h: -------------------------------------------------------------------------------- 1 | #ifndef __VELOCITY_ARROW_TO_OBSTACLE_CONVERTER 2 | #define __VELOCITY_ARROW_TO_OBSTACLE_CONVERTER 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | class VelocityArrowToObstacleConverter 15 | { 16 | public: 17 | VelocityArrowToObstacleConverter(void); 18 | void waypoint_callback(const geometry_msgs::PoseArrayConstPtr&); 19 | void velocity_arrow_callback(const visualization_msgs::MarkerArrayConstPtr&); 20 | void process(void); 21 | 22 | private: 23 | double PREDICTION_TIME; 24 | int PREDICTION_STEP; 25 | std::string WORLD_FRAME; 26 | std::string OBS_FRAME; 27 | double HZ; 28 | double INTERVAL; 29 | 30 | ros::NodeHandle nh; 31 | ros::NodeHandle local_nh; 32 | ros::Publisher obs_num_pub; 33 | ros::Publisher predicted_paths_pub; 34 | ros::Subscriber velocity_arrow_sub; 35 | geometry_msgs::PoseArray predicted_paths; 36 | bool velocity_arrow_updated; 37 | tf::TransformBroadcaster br; 38 | tf::StampedTransform _transform; 39 | visualization_msgs::MarkerArray velocity_arrows; 40 | geometry_msgs::TransformStamped transform; 41 | std::vector velocities; 42 | geometry_msgs::PoseArray obs_poses; 43 | std_msgs::Int32 obs_num; 44 | }; 45 | 46 | #endif// __VELOCITY_ARROW_TO_OBSTACLE_CONVERTER 47 | -------------------------------------------------------------------------------- /launch/avoidance_path_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /launch/collision_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /launch/diff_drive.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /launch/diff_drive_mpc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/diff_drive_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/dynamic_local_costmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /launch/dynamic_obstacle_avoidance_core.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /launch/for_mocap_experiment.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /launch/for_rviz_simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /launch/fwdis.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /launch/fwdis_mpc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /launch/load_param_diff_drive.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/load_param_dynamic_avoidance.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/obstacle_trajectory_predictor_kf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /launch/path_to_local_goal.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /launch/robot_predictor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/tf_to_obstacles.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dynamic_obstacle_avoidance_planner 4 | 0.0.0 5 | The dynamic_obstacle_avoidance_planner package 6 | 7 | 8 | 9 | 10 | amsl 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | tf 56 | cmake_modules 57 | roscpp 58 | rospy 59 | std_msgs 60 | cmake_modules 61 | roscpp 62 | rospy 63 | std_msgs 64 | tf 65 | cmake_modules 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /run_docker.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "=== run_docker ===" 4 | 5 | xhost +local:docker 6 | 7 | docker run -it --rm \ 8 | --env=QT_X11_NO_MITSHM=1 \ 9 | --env=DISPLAY=$DISPLAY \ 10 | --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 11 | --volume="${PWD}:/root/catkin_ws/src/dynamic_obstacle_avoidance_planner" \ 12 | --net='host' \ 13 | --name="ros_mpc" \ 14 | amslabtech/ros_mpc \ 15 | bash 16 | 17 | -------------------------------------------------------------------------------- /run_docker_with_vicon.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "=== run_docker ===" 4 | 5 | xhost +local:docker 6 | 7 | docker run -it --rm \ 8 | --env=QT_X11_NO_MITSHM=1 \ 9 | --env=DISPLAY=$DISPLAY \ 10 | --env=ROS_MASTER_URI=http://192.168.10.102:11311 \ 11 | --env=ROS_IP=192.168.0.245 \ 12 | --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 13 | --volume="${PWD}:/root/catkin_ws/src/avoidance_path_planner" \ 14 | --net='host' \ 15 | --name="ros_mpc" \ 16 | amslabtech/ros_mpc 17 | -------------------------------------------------------------------------------- /scripts/evaluator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from geometry_msgs.msg import Twist 5 | from geometry_msgs.msg import PoseStamped 6 | from geometry_msgs.msg import PoseArray 7 | from nav_msgs.msg import Path 8 | import tf 9 | 10 | import math as m 11 | 12 | obs_paths = PoseArray() 13 | obs_poses = PoseArray() 14 | PREDICTION_TIME = 0 15 | VELOCITY_TOPIC_NAME = "" 16 | last_velocity = Twist() 17 | start_flag = False 18 | started_flag = False 19 | intermediate_path = Path() 20 | INTERMEDIATE_PATH_TOPIC_NAME = "" 21 | goal = PoseStamped() 22 | transformed_goal = PoseStamped() 23 | goal_subscribed = False 24 | 25 | HZ = 10.0 26 | 27 | def obs_callback(data): 28 | global obs_paths 29 | global obs_poses 30 | obs_paths = PoseArray() 31 | obs_poses = PoseArray() 32 | obs_poses.header = data.header 33 | PREDICTION_STEP = int(PREDICTION_TIME * HZ + 1) 34 | obs_paths = data 35 | _size = len(obs_paths.poses) 36 | obs_num = int(_size / PREDICTION_STEP) 37 | for i in range(obs_num): 38 | obs_poses.poses.append(obs_paths.poses[i * PREDICTION_STEP]) 39 | print(PREDICTION_TIME) 40 | print(PREDICTION_STEP) 41 | print(_size) 42 | print(obs_num) 43 | 44 | def velocity_callback(data): 45 | global last_velocity 46 | global start_flag 47 | if(last_velocity.linear.x == 0.0 and data.linear.x > 0.0): 48 | start_flag = True 49 | last_velocity = data 50 | 51 | def path_callback(data): 52 | #print("=== path_callback ===") 53 | global intermediate_path 54 | intermediate_path = data 55 | _size = len(intermediate_path.poses) 56 | print(_size) 57 | if(_size > 0): 58 | global goal 59 | goal = intermediate_path.poses[_size - 1] 60 | global goal_subscribed 61 | goal_subscribed = True 62 | 63 | def is_goal(x, y): 64 | dx = x - transformed_goal.pose.position.x 65 | dy = y - transformed_goal.pose.position.y 66 | dist = m.sqrt(dx**2 + dy**2) 67 | if(dist < 0.3): 68 | return True 69 | else: 70 | return False 71 | 72 | def process(): 73 | print("=== evaluator ===") 74 | 75 | listener = tf.TransformListener() 76 | 77 | r = rospy.Rate(HZ) 78 | 79 | ave_velocity = 0 80 | min_distance = 2.0 81 | 82 | last_r_x = 0 83 | last_r_y = 0 84 | 85 | count = 0.0 86 | 87 | first = True 88 | ROBOT_FRAME = rospy.get_param("/dynamic_avoidance/ROBOT_FRAME") 89 | OBS_FRAME = rospy.get_param("/dynamic_avoidance/OBSTACLES_FRAME") 90 | WORLD_FRAME = rospy.get_param("/dynamic_avoidance/WORLD_FRAME") 91 | global VELOCITY_TOPIC_NAME 92 | VELOCITY_TOPIC_NAME = rospy.get_param("/dynamic_avoidance/VELOCITY_TOPIC_NAME") 93 | global PREDICTION_TIME 94 | PREDICTION_TIME = rospy.get_param("/dynamic_avoidance/PREDICTION_TIME") 95 | global INTERMEDIATE_PATH_TOPIC_NAME 96 | INTERMEDIATE_PATH_TOPIC_NAME = rospy.get_param("/dynamic_avoidance/INTERMEDIATE_PATH_TOPIC_NAME") 97 | 98 | rospy.Subscriber("/predicted_paths", PoseArray, obs_callback) 99 | rospy.Subscriber(VELOCITY_TOPIC_NAME, Twist, velocity_callback) 100 | rospy.Subscriber(INTERMEDIATE_PATH_TOPIC_NAME, Path, path_callback) 101 | obs_pub = rospy.Publisher("/evalation/obs", PoseArray, queue_size=1) 102 | 103 | time_to_goal = rospy.get_time() 104 | 105 | transformed = False 106 | 107 | 108 | while not rospy.is_shutdown(): 109 | try: 110 | (trans, rot)= listener.lookupTransform(WORLD_FRAME, ROBOT_FRAME, rospy.Time(0)) 111 | #(_trans, _rot)= listener.lookupTransform(WORLD_FRAME, "local_costmap", rospy.Time(0)) 112 | transformed = True 113 | except(tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 114 | continue 115 | print(transformed) 116 | print(goal_subscribed) 117 | if(transformed and goal_subscribed): 118 | robot_x = trans[0] 119 | robot_y = trans[1] 120 | global transformed_goal 121 | transformed_goal = listener.transformPose(WORLD_FRAME, goal) 122 | 123 | global started_flag 124 | if(start_flag and not started_flag): 125 | time_to_goal = rospy.get_time() 126 | started_flag = True 127 | current_time = rospy.get_time() 128 | if(not first): 129 | temp_distance = 2 130 | temp_min_distance = 2 131 | for obs in obs_poses.poses: 132 | temp_distance = m.sqrt((robot_x-obs.position.x)**2 + (robot_y-obs.position.y)**2) 133 | if(temp_min_distance > temp_distance): 134 | temp_min_distance = temp_distance 135 | interval = current_time - last_time 136 | velocity = m.sqrt((robot_x-last_r_x)**2 + (robot_y-last_r_y)**2) / interval 137 | ave_velocity = (count * ave_velocity + velocity) / (count + 1.0) 138 | count += 1.0 139 | if(temp_min_distance < min_distance): 140 | min_distance = temp_min_distance 141 | 142 | last_r_x = robot_x 143 | last_r_y = robot_y 144 | first = False 145 | last_time = current_time 146 | obs_pub.publish(obs_poses) 147 | if(is_goal(robot_x, robot_y)): 148 | time_to_goal = rospy.get_time() - time_to_goal 149 | print("time_to_goal=" + str(time_to_goal)) 150 | return 151 | print("closest_distance=" + str(min_distance)) 152 | print("average_velocity=" + str(ave_velocity)) 153 | r.sleep() 154 | 155 | if __name__=='__main__': 156 | rospy.init_node('evaluator') 157 | try: 158 | process() 159 | except rospy.ROSInterruptException: pass 160 | -------------------------------------------------------------------------------- /scripts/log4csv.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #! coding:utf-8 3 | 4 | import rospy 5 | from geometry_msgs.msg import Twist 6 | from geometry_msgs.msg import PoseStamped 7 | from std_msgs.msg import Empty 8 | import tf 9 | 10 | import math as m 11 | import numpy as np 12 | import os 13 | 14 | HZ = 20.0 15 | 16 | FILE_NAME = 'ex5.csv' 17 | ROBOT_FRAME = '/vicon/base_link/base_link' 18 | WORLD_FRAME = '/world' 19 | VELOCITY_TOPIC_NAME = '/fwdis/velocity' 20 | 21 | class Log4csv: 22 | def __init__(self): 23 | self.vel = Twist() 24 | self.path = os.getcwd() + '/' + FILE_NAME 25 | self.first_flag = True 26 | self.p_0 = PoseStamped() 27 | self.theta_0 = 0 28 | 29 | def vel_callback(self, data): 30 | self.vel = data 31 | if self.first_flag: 32 | self.first_time = rospy.get_time() 33 | self.first_flag = False 34 | (trans, rot) = self.listener.lookupTransform(WORLD_FRAME, ROBOT_FRAME, rospy.Time(0)) 35 | self.p_0.pose.position.x = trans[0] 36 | self.p_0.pose.position.y = trans[1] 37 | self.p_0.pose.orientation.x = rot[0] 38 | self.p_0.pose.orientation.y = rot[1] 39 | self.p_0.pose.orientation.z = rot[2] 40 | self.p_0.pose.orientation.w = rot[3] 41 | _, _, self.theta_0 = tf.transformations.euler_from_quaternion(rot) 42 | else: 43 | try: 44 | (trans, rot) = self.listener.lookupTransform(WORLD_FRAME, ROBOT_FRAME, rospy.Time(0)) 45 | with open(self.path, 'a') as f: 46 | x = trans[0] * m.cos(-self.theta_0) - trans[1] * m.sin(-self.theta_0) 47 | y = trans[0] * m.sin(-self.theta_0) + trans[1] * m.cos(-self.theta_0) 48 | sentence = (str(rospy.get_time() - self.first_time) + ',' 49 | + str(x) + ',' 50 | + str(y) + ',' 51 | + str(self.vel.linear.x) + ',' 52 | + str(self.vel.linear.y) + ',' 53 | + str(self.vel.angular.z) 54 | + '\n' 55 | ) 56 | f.write(sentence) 57 | print(sentence) 58 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 59 | print('exception') 60 | 61 | def process(self): 62 | rospy.init_node('log4csv') 63 | self.vel_sub = rospy.Subscriber(VELOCITY_TOPIC_NAME, Twist, self.vel_callback, queue_size=1) 64 | self.listener = tf.TransformListener() 65 | self.first_time = rospy.get_time() 66 | print('=== log4csv ===') 67 | rospy.spin() 68 | 69 | if __name__=='__main__': 70 | log4csv = Log4csv() 71 | try: 72 | log4csv.process() 73 | except rospy.ROSInterruptException: pass 74 | -------------------------------------------------------------------------------- /scripts/old_test_scripts/dynamic_avoidance_test.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gnome-terminal -e "/opt/ros/kinetic/bin/roscore" --geometry=45x12+0+0 & 4 | sleep 1.0s 5 | 6 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_dynamic_avoidance.launch" --geometry=45x12+0+0 & 7 | sleep 1.0s 8 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_diff_drive.launch" --geometry=45x12+0+0 & 9 | sleep 1.0s 10 | 11 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun rviz rviz -d ../config/dynamic_avoidance.rviz" --geometry=45x12+475+0 & 12 | sleep 0.5s 13 | 14 | # robot sim 15 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner sim_3dof.py" --geometry=45x12+895+0 & 16 | sleep 0.1s 17 | 18 | # obstacle sim 19 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner diff_drive_sim.launch" --geometry=45x12+1315+0 & 20 | sleep 0.1s 21 | 22 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner obstacle_trajectory_predictor_kf" --geometry=45x12+0+250 & 23 | sleep 0.1s 24 | 25 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner robot_predictor.launch" --geometry=45x12+475+250 & 26 | sleep 0.1s 27 | 28 | # if no localization 29 | WORLD_FRAME=$(/opt/ros/${ROS_DISTRO}/bin/rosparam get /dynamic_avoidance/WORLD_FRAME) 30 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun tf static_transform_publisher 0 0 0 0 0 0 $WORLD_FRAME odom 100" --geometry=45x12+895+250 & 31 | sleep 0.1s 32 | 33 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner dynamic_local_costmap.launch" --geometry=45x12+1315+250 & 34 | sleep 0.1s 35 | 36 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner avoidance_path_planner.launch" --geometry=45x12+0+476 & 37 | sleep 0.1s 38 | 39 | # for test 40 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner waypoints_test" --geometry=45x12+475+476 & 41 | sleep 0.1s 42 | 43 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner diff_drive_mpc.launch" --geometry=45x12+895+476 & 44 | sleep 0.1s 45 | 46 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner base_costmap_tf_publisher" --geometry=45x12+1315+476 & 47 | sleep 0.1s 48 | 49 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner collision_detector.launch" --geometry=45x12+0+702 & 50 | sleep 0.1s 51 | 52 | #gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner trajectory_logger" --geometry=45x12+475+702 & 53 | sleep 0.1s 54 | -------------------------------------------------------------------------------- /scripts/old_test_scripts/dynamic_avoidance_test_docker.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gnome-terminal -e "/opt/ros/${ROS_DISTRO}/bin/roscore" --geometry=45x12+0+0 & 4 | sleep 1.0s 5 | 6 | gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_diff_drive.launch'" --geometry=45x12+0+0 & 7 | sleep 1.0s 8 | 9 | gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_dynamic_avoidance.launch'" --geometry=45x12+0+0 & 10 | sleep 1.0s 11 | 12 | gnome-terminal -e "/opt/ros/${ROS_DISTRO}/bin/rosrun rviz rviz -d ../config/dynamic_avoidance.rviz" --geometry=45x12+475+0 & 13 | sleep 0.5s 14 | 15 | # robot sim 16 | gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner sim_3dof.py'" --geometry=45x12+895+0 & 17 | sleep 0.1s 18 | 19 | # obstacle sim 20 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner diff_drive_sim.launch'" --geometry=45x12+1315+0 & 21 | sleep 0.1s 22 | 23 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner obstacle_trajectory_predictor_kf'" --geometry=45x12+0+250 & 24 | sleep 0.1s 25 | 26 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner robot_predictor.launch'" --geometry=45x12+475+250 & 27 | sleep 0.1s 28 | 29 | WORLD_FRAME=$(/opt/ros/${ROS_DISTRO}/bin/rosparam get /dynamic_avoidance/WORLD_FRAME) 30 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun tf static_transform_publisher 0 0 0 0 0 0 $WORLD_FRAME odom 100'" --geometry=45x12+895+250 & 31 | sleep 0.1s 32 | 33 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner dynamic_local_costmap.launch'" --geometry=45x12+1315+250 & 34 | sleep 0.1s 35 | 36 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner avoidance_path_planner.launch'" --geometry=45x12+0+476 & 37 | sleep 0.1s 38 | 39 | # for test 40 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner waypoints_test'" --geometry=45x12+475+476 & 41 | sleep 0.1s 42 | 43 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner diff_drive_mpc.launch'" --geometry=45x12+895+476 & 44 | sleep 0.1s 45 | 46 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner base_costmap_tf_publisher'" --geometry=45x12+1315+476 & 47 | sleep 0.1s 48 | 49 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner collision_detector.launch'" --geometry=45x12+0+702 & 50 | sleep 0.1s 51 | 52 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rorun dynamic_obstacle_avoidance_planner trajectory_logger'" --geometry=45x12+475+702 & 53 | sleep 0.1s 54 | -------------------------------------------------------------------------------- /scripts/old_test_scripts/dynamic_avoidance_test_docker_robot.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gnome-terminal -e "/opt/ros/${ROS_DISTRO}/bin/roscore" --geometry=45x12+0+0 & 4 | sleep 1.0s 5 | 6 | gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_diff_drive.launch'" --geometry=45x12+0+0 & 7 | sleep 1.0s 8 | 9 | gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_dynamic_avoidance.launch'" --geometry=45x12+0+0 & 10 | sleep 1.0s 11 | 12 | gnome-terminal -e "/opt/ros/${ROS_DISTRO}/bin/rosrun rviz rviz -d ../config/dynamic_avoidance.rviz" --geometry=45x12+475+0 & 13 | sleep 0.5s 14 | 15 | # robot sim 16 | #gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner sim_3dof.py'" --geometry=45x12+895+0 & 17 | sleep 0.1s 18 | 19 | # obstacle sim 20 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner diff_drive_sim.launch'" --geometry=45x12+1315+0 & 21 | sleep 0.1s 22 | 23 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner obstacle_trajectory_predictor_kf'" --geometry=45x12+0+250 & 24 | sleep 0.1s 25 | 26 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner robot_predictor.launch'" --geometry=45x12+475+250 & 27 | sleep 0.1s 28 | 29 | #WORLD_FRAME=$(/opt/ros/${ROS_DISTRO}/bin/rosparam get /dynamic_avoidance/WORLD_FRAME) 30 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun tf static_transform_publisher 0 0 0 0 0 0 $WORLD_FRAME odom 100'" --geometry=45x12+895+250 & 31 | #sleep 0.1s 32 | 33 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner dynamic_local_costmap.launch'" --geometry=45x12+1315+250 & 34 | sleep 0.1s 35 | 36 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner avoidance_path_planner.launch'" --geometry=45x12+0+476 & 37 | sleep 0.1s 38 | 39 | # for test 40 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner waypoints_test'" --geometry=45x12+475+476 & 41 | sleep 0.1s 42 | 43 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner diff_drive_mpc.launch'" --geometry=45x12+895+476 & 44 | sleep 0.1s 45 | 46 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner base_costmap_tf_publisher'" --geometry=45x12+1315+476 & 47 | sleep 0.1s 48 | 49 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner collision_detector.launch'" --geometry=45x12+0+702 & 50 | sleep 0.1s 51 | 52 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rorun dynamic_obstacle_avoidance_planner trajectory_logger'" --geometry=45x12+475+702 & 53 | sleep 0.1s 54 | 55 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner velocity_arrow_to_obstacle_converter'" --geometry=45x12+475+702 & 56 | sleep 0.1s 57 | -------------------------------------------------------------------------------- /scripts/old_test_scripts/dynamic_avoidance_test_fwdis.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gnome-terminal -e "/opt/ros/kinetic/bin/roscore" --geometry=45x12+0+0 & 4 | sleep 1.0s 5 | 6 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_dynamic_avoidance.launch" --geometry=45x12+0+0 & 7 | sleep 1.0s 8 | 9 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_fwdis.launch" --geometry=45x12+0+0 & 10 | sleep 1.0s 11 | 12 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun rviz rviz -d ../config/dynamic_avoidance.rviz" --geometry=45x12+475+0 & 13 | #sleep 0.5s 14 | 15 | # robot sim 16 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner sim_3dof.py" --geometry=45x12+895+0 & 17 | sleep 0.1s 18 | 19 | # obstacle sim 20 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner diff_drive_sim.launch" --geometry=45x12+1315+0 & 21 | sleep 0.1s 22 | 23 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner obstacle_trajectory_predictor_kf" --geometry=45x12+0+250 & 24 | sleep 0.1s 25 | 26 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner robot_predictor.launch" --geometry=45x12+475+250 & 27 | sleep 0.1s 28 | 29 | # if no localization 30 | WORLD_FRAME=$(/opt/ros/${ROS_DISTRO}/bin/rosparam get /dynamic_avoidance/WORLD_FRAME) 31 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun tf static_transform_publisher 0 0 0 0 0 0 $WORLD_FRAME odom 100" --geometry=45x12+895+250 & 32 | sleep 0.1s 33 | 34 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner dynamic_local_costmap.launch" --geometry=45x12+1315+250 & 35 | sleep 0.1s 36 | 37 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner avoidance_path_planner.launch" --geometry=45x12+0+476 & 38 | sleep 0.1s 39 | 40 | # for test 41 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner waypoints_test" --geometry=45x12+475+476 & 42 | sleep 0.1s 43 | 44 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner fwdis_mpc.launch" --geometry=45x12+895+476 & 45 | sleep 0.1s 46 | 47 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner base_costmap_tf_publisher" --geometry=45x12+1315+476 & 48 | sleep 0.1s 49 | 50 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner collision_detector.launch" --geometry=45x12+0+702 & 51 | sleep 0.1s 52 | 53 | #gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner trajectory_logger" --geometry=45x12+475+702 & 54 | sleep 0.1s 55 | -------------------------------------------------------------------------------- /scripts/old_test_scripts/dynamic_avoidance_test_fwdis_docker.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gnome-terminal -e "/opt/ros/${ROS_DISTRO}/bin/roscore" --geometry=45x12+0+0 & 4 | sleep 1.0s 5 | 6 | gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_fwdis.launch'" --geometry=45x12+0+0 & 7 | sleep 1.0s 8 | 9 | gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_dynamic_avoidance.launch'" --geometry=45x12+0+0 & 10 | sleep 1.0s 11 | 12 | gnome-terminal -e "/opt/ros/${ROS_DISTRO}/bin/rosrun rviz rviz -d ../config/dynamic_avoidance.rviz" --geometry=45x12+475+0 & 13 | sleep 0.5s 14 | 15 | # robot sim 16 | gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner sim_3dof.py'" --geometry=45x12+895+0 & 17 | sleep 0.1s 18 | 19 | # obstacle sim 20 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner diff_drive_sim.launch'" --geometry=45x12+1315+0 & 21 | sleep 0.1s 22 | 23 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner obstacle_trajectory_predictor_kf'" --geometry=45x12+0+250 & 24 | sleep 0.1s 25 | 26 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner robot_predictor.launch'" --geometry=45x12+475+250 & 27 | sleep 0.1s 28 | 29 | WORLD_FRAME=$(/opt/ros/${ROS_DISTRO}/bin/rosparam get /dynamic_avoidance/WORLD_FRAME) 30 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun tf static_transform_publisher 0 0 0 0 0 0 $WORLD_FRAME odom 100'" --geometry=45x12+895+250 & 31 | sleep 0.1s 32 | 33 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner dynamic_local_costmap.launch'" --geometry=45x12+1315+250 & 34 | sleep 0.1s 35 | 36 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner avoidance_path_planner.launch'" --geometry=45x12+0+476 & 37 | sleep 0.1s 38 | 39 | # for test 40 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner waypoints_test'" --geometry=45x12+475+476 & 41 | sleep 0.1s 42 | 43 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner fwdis_mpc.launch'" --geometry=45x12+895+476 & 44 | sleep 0.1s 45 | 46 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner base_costmap_tf_publisher'" --geometry=45x12+1315+476 & 47 | sleep 0.1s 48 | 49 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner collision_detector.launch'" --geometry=45x12+0+702 & 50 | sleep 0.1s 51 | 52 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rorun dynamic_obstacle_avoidance_planner trajectory_logger'" --geometry=45x12+475+702 & 53 | sleep 0.1s 54 | -------------------------------------------------------------------------------- /scripts/old_test_scripts/dynamic_avoidance_test_fwdis_docker_robot.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gnome-terminal -e "/opt/ros/${ROS_DISTRO}/bin/roscore" --geometry=45x12+0+0 & 4 | sleep 1.0s 5 | 6 | gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_fwdis.launch'" --geometry=45x12+0+0 & 7 | sleep 1.0s 8 | 9 | gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_dynamic_avoidance.launch'" --geometry=45x12+0+0 & 10 | sleep 1.0s 11 | 12 | gnome-terminal -e "/opt/ros/${ROS_DISTRO}/bin/rosrun rviz rviz -d ../config/dynamic_avoidance.rviz" --geometry=45x12+475+0 & 13 | sleep 0.5s 14 | 15 | # robot sim 16 | #gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner sim_3dof.py'" --geometry=45x12+895+0 & 17 | sleep 0.1s 18 | 19 | # obstacle sim 20 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner diff_drive_sim.launch'" --geometry=45x12+1315+0 & 21 | sleep 0.1s 22 | 23 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner obstacle_trajectory_predictor_kf'" --geometry=45x12+0+250 & 24 | sleep 0.1s 25 | 26 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner robot_predictor.launch'" --geometry=45x12+475+250 & 27 | sleep 0.1s 28 | 29 | #WORLD_FRAME=$(/opt/ros/${ROS_DISTRO}/bin/rosparam get /dynamic_avoidance/WORLD_FRAME) 30 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun tf static_transform_publisher 0 0 0 0 0 0 $WORLD_FRAME odom 100'" --geometry=45x12+895+250 & 31 | #sleep 0.1s 32 | 33 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner dynamic_local_costmap.launch'" --geometry=45x12+1315+250 & 34 | sleep 0.1s 35 | 36 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner avoidance_path_planner.launch'" --geometry=45x12+0+476 & 37 | sleep 0.1s 38 | 39 | # for test 40 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner waypoints_test'" --geometry=45x12+475+476 & 41 | sleep 0.1s 42 | 43 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner fwdis_mpc.launch'" --geometry=45x12+895+476 & 44 | sleep 0.1s 45 | 46 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner base_costmap_tf_publisher'" --geometry=45x12+1315+476 & 47 | sleep 0.1s 48 | 49 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner collision_detector.launch'" --geometry=45x12+0+702 & 50 | sleep 0.1s 51 | 52 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rorun dynamic_obstacle_avoidance_planner trajectory_logger'" --geometry=45x12+475+702 & 53 | sleep 0.1s 54 | 55 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner velocity_arrow_to_obstacle_converter'" --geometry=45x12+475+702 & 56 | sleep 0.1s 57 | -------------------------------------------------------------------------------- /scripts/old_test_scripts/dynamic_avoidance_test_vicon.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gnome-terminal -e "/opt/ros/${ROS_DISTRO}/bin/roscore" --geometry=45x12+0+0 & 4 | sleep 1.0s 5 | 6 | gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_diff_drive.launch'" --geometry=45x12+0+0 & 7 | sleep 1.0s 8 | 9 | gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_dynamic_avoidance.launch'" --geometry=45x12+0+0 & 10 | sleep 1.0s 11 | 12 | gnome-terminal -e "/opt/ros/${ROS_DISTRO}/bin/rosrun rviz rviz -d ../config/dynamic_avoidance.rviz" --geometry=45x12+475+0 & 13 | sleep 0.5s 14 | 15 | # robot sim 16 | #gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner sim_3dof.py'" --geometry=45x12+895+0 & 17 | sleep 0.1s 18 | 19 | # obstacle sim 20 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner diff_drive_sim.launch'" --geometry=45x12+1315+0 & 21 | sleep 0.1s 22 | 23 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner obstacle_trajectory_predictor_kf'" --geometry=45x12+0+250 & 24 | sleep 0.1s 25 | 26 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner robot_predictor.launch'" --geometry=45x12+475+250 & 27 | sleep 0.1s 28 | 29 | #WORLD_FRAME=$(/opt/ros/${ROS_DISTRO}/bin/rosparam get /dynamic_avoidance/WORLD_FRAME) 30 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun tf static_transform_publisher 0 0 0 0 0 0 $WORLD_FRAME odom 100'" --geometry=45x12+895+250 & 31 | #sleep 0.1s 32 | 33 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner dynamic_local_costmap.launch'" --geometry=45x12+1315+250 & 34 | sleep 0.1s 35 | 36 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner avoidance_path_planner.launch'" --geometry=45x12+0+476 & 37 | sleep 0.1s 38 | 39 | # for test 40 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner waypoints_test'" --geometry=45x12+475+476 & 41 | sleep 0.1s 42 | 43 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner diff_drive_mpc.launch'" --geometry=45x12+895+476 & 44 | sleep 0.1s 45 | 46 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner base_costmap_tf_publisher'" --geometry=45x12+1315+476 & 47 | sleep 0.1s 48 | 49 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner collision_detector.launch'" --geometry=45x12+0+702 & 50 | sleep 0.1s 51 | 52 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rorun dynamic_obstacle_avoidance_planner trajectory_logger'" --geometry=45x12+475+702 & 53 | sleep 0.1s 54 | 55 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner velocity_arrow_to_obstacle_converter'" --geometry=45x12+475+702 & 56 | #sleep 0.1s 57 | -------------------------------------------------------------------------------- /scripts/old_test_scripts/dynamic_avoidance_test_vicon_fwdis.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gnome-terminal -e "/opt/ros/${ROS_DISTRO}/bin/roscore" --geometry=45x12+0+0 & 4 | sleep 1.0s 5 | 6 | gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_fwdis.launch'" --geometry=45x12+0+0 & 7 | sleep 1.0s 8 | 9 | gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner load_param_dynamic_avoidance.launch'" --geometry=45x12+0+0 & 10 | sleep 1.0s 11 | 12 | gnome-terminal -e "/opt/ros/${ROS_DISTRO}/bin/rosrun rviz rviz -d ../config/dynamic_avoidance.rviz" --geometry=45x12+475+0 & 13 | sleep 0.5s 14 | 15 | # robot sim 16 | #gnome-terminal -e "docker exec -it ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner sim_3dof.py'" --geometry=45x12+895+0 & 17 | sleep 0.1s 18 | 19 | # obstacle sim 20 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner diff_drive_sim.launch'" --geometry=45x12+1315+0 & 21 | sleep 0.1s 22 | 23 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner obstacle_trajectory_predictor_kf'" --geometry=45x12+0+250 & 24 | sleep 0.1s 25 | 26 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner robot_predictor.launch'" --geometry=45x12+475+250 & 27 | sleep 0.1s 28 | 29 | #WORLD_FRAME=$(/opt/ros/${ROS_DISTRO}/bin/rosparam get /dynamic_avoidance/WORLD_FRAME) 30 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun tf static_transform_publisher 0 0 0 0 0 0 $WORLD_FRAME odom 100'" --geometry=45x12+895+250 & 31 | #sleep 0.1s 32 | 33 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner dynamic_local_costmap.launch'" --geometry=45x12+1315+250 & 34 | sleep 0.1s 35 | 36 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner avoidance_path_planner.launch'" --geometry=45x12+0+476 & 37 | sleep 0.1s 38 | 39 | # for test 40 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner waypoints_test'" --geometry=45x12+475+476 & 41 | sleep 0.1s 42 | 43 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner fwdis_mpc.launch'" --geometry=45x12+895+476 & 44 | sleep 0.1s 45 | 46 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner base_costmap_tf_publisher'" --geometry=45x12+1315+476 & 47 | sleep 0.1s 48 | 49 | gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner collision_detector.launch'" --geometry=45x12+0+702 & 50 | sleep 0.1s 51 | 52 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rorun dynamic_obstacle_avoidance_planner trajectory_logger'" --geometry=45x12+475+702 & 53 | sleep 0.1s 54 | 55 | #gnome-terminal -e "docker exec ros_mpc /bin/bash -c 'ldconfig && source /root/catkin_ws/devel/setup.bash && /opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner velocity_arrow_to_obstacle_converter'" --geometry=45x12+475+702 & 56 | #sleep 0.1s 57 | -------------------------------------------------------------------------------- /scripts/old_test_scripts/gazebo_dwa_sim.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gnome-terminal -e "/opt/ros/kinetic/bin/roscore" --geometry=40x12+0+0 4 | 5 | sleep 1s 6 | 7 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner gazebo.launch" --geometry=40x12+450+0 8 | 9 | sleep 1s 10 | 11 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner rviz.launch" --geometry=40x12+450+300 12 | 13 | sleep 1s 14 | 15 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner fwdis_controller" --geometry=40x12+850+0 16 | 17 | sleep 1s 18 | 19 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner fwdis_gazebo" --geometry=40x12+1250+300 20 | 21 | sleep 1s 22 | 23 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner control.launch" --geometry=40x12+0+300 24 | 25 | sleep 1s 26 | 27 | 28 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch --screen dynamic_obstacle_avoidance_planner holonomic_dwa.launch" --geometry=40x12+850+300 29 | 30 | sleep 1s 31 | -------------------------------------------------------------------------------- /scripts/old_test_scripts/gazebo_sim.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gnome-terminal -e "/opt/ros/kinetic/bin/roscore" --geometry=40x12+0+0 4 | 5 | sleep 1s 6 | 7 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner gazebo.launch" --geometry=40x12+450+0 8 | 9 | sleep 1s 10 | 11 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner rviz.launch" --geometry=40x12+450+300 12 | 13 | sleep 1s 14 | 15 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner fwdis_controller" --geometry=40x12+850+0 16 | 17 | sleep 1s 18 | 19 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner fwdis_gazebo" --geometry=40x12+1250+300 20 | 21 | sleep 1s 22 | 23 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner control.launch" --geometry=40x12+0+300 24 | 25 | sleep 1s 26 | 27 | 28 | #gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch --screen dynamic_obstacle_avoidance_planner holonomic_dwa.launch" --geometry=40x12+850+300 29 | 30 | sleep 1s 31 | -------------------------------------------------------------------------------- /scripts/old_test_scripts/mpc_test.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gnome-terminal -e "/opt/ros/kinetic/bin/roscore" --geometry=40x12+0+0 4 | 5 | sleep 1s 6 | 7 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner mpc_path_tracker" --geometry=40x12+485+0 8 | 9 | gnome-terminal -e "/opt/ros/kinetic/bin/roslaunch dynamic_obstacle_avoidance_planner sin_curve.launch" --geometry=40x12+900+0 10 | 11 | gnome-terminal -e "/opt/ros/kinetic/bin/rosrun dynamic_obstacle_avoidance_planner sim_3dof.py" --geometry=40x12+1235+0 12 | -------------------------------------------------------------------------------- /scripts/sim_3dof.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from geometry_msgs.msg import Twist 5 | from geometry_msgs.msg import PoseStamped 6 | from geometry_msgs.msg import Quaternion 7 | from nav_msgs.msg import Odometry 8 | from std_msgs.msg import Empty 9 | import tf 10 | 11 | import math as m 12 | 13 | # global variables 14 | pose = PoseStamped() 15 | velocity = Twist() 16 | stop_flag = False 17 | MAX_ACCELERATION = 0 18 | MAX_YAWRATE = 0 19 | MAX_D_YAWRATE = 0 20 | 21 | HZ = 20.0 22 | 23 | def velocity_callback(data): 24 | # print("velocity callback") 25 | global velocity 26 | velocity = data 27 | # print(velocity) 28 | 29 | def stop_callback(data): 30 | global stop_flag 31 | stop_flag = True 32 | 33 | def process(): 34 | global MAX_ACCELERATION 35 | global MAX_YAWRATE 36 | global MAX_D_YAWRATE 37 | ROBOT_FRAME = rospy.get_param("/dynamic_avoidance/ROBOT_FRAME") 38 | VELOCITY_TOPIC_NAME = rospy.get_param("/dynamic_avoidance/VELOCITY_TOPIC_NAME") 39 | MAX_ACCELERATION = rospy.get_param("~MAX_ACCELERATION", 1.0) 40 | MAX_YAWRATE = rospy.get_param("/dynamic_avoidance/MAX_ANGULAR_VELOCITY", 1.0) 41 | MAX_D_YAWRATE = rospy.get_param("/dynamic_avoidance/MAX_ANGULAR_ACCCELERATION", 3.14) 42 | INIT_X = rospy.get_param("~INIT_X", 0) 43 | INIT_Y = rospy.get_param("~INIT_Y", 0) 44 | INIT_YAW = rospy.get_param("~INIT_YAW", 0) 45 | 46 | print(MAX_ACCELERATION) 47 | print(MAX_YAWRATE) 48 | print(MAX_D_YAWRATE) 49 | print(INIT_X) 50 | print(INIT_Y) 51 | print(INIT_YAW) 52 | 53 | odom_pub = rospy.Publisher("/odom", Odometry, queue_size=1) 54 | pose_pub = rospy.Publisher("pose", PoseStamped, queue_size=1) 55 | vel_pub = rospy.Publisher("/velocity/debug", Twist, queue_size=1) 56 | rospy.Subscriber(VELOCITY_TOPIC_NAME, Twist, velocity_callback) 57 | rospy.Subscriber("/stop", Empty, stop_callback) 58 | 59 | print("=== sim 3dof ===") 60 | 61 | br = tf.TransformBroadcaster() 62 | 63 | pose.pose.position.x = INIT_X 64 | pose.pose.position.y = INIT_Y 65 | pose.pose.orientation = Quaternion(*(tf.transformations.quaternion_from_euler(0, 0, INIT_YAW))) 66 | pose.header.frame_id = ROBOT_FRAME 67 | 68 | global velocity 69 | velocity.linear.x = 0 70 | velocity.linear.y = 0 71 | velocity.angular.z = 0 72 | 73 | r = rospy.Rate(HZ) 74 | 75 | cmd = Twist() 76 | while not rospy.is_shutdown(): 77 | if stop_flag: 78 | velocity.linear.x = 0 79 | velocity.linear.y = 0 80 | 81 | print("velocity") 82 | # print(velocity) 83 | dt = 1. / HZ 84 | acc_x = (velocity.linear.x - cmd.linear.x) / dt 85 | # print(acc_x) 86 | acc_x = max(-MAX_ACCELERATION, min(MAX_ACCELERATION, acc_x)) 87 | # print(acc_x) 88 | acc_y = (velocity.linear.y - cmd.linear.y) / dt 89 | acc_y = max(-MAX_ACCELERATION, min(MAX_ACCELERATION, acc_y)) 90 | cmd.linear.x += acc_x * dt 91 | cmd.linear.y += acc_y * dt 92 | ang_acc_z = (velocity.angular.z - cmd.angular.z) / dt 93 | # print(ang_acc_z) 94 | ang_acc_z = max(-MAX_D_YAWRATE, min(MAX_D_YAWRATE, ang_acc_z)) 95 | # print(ang_acc_z) 96 | cmd.angular.z += ang_acc_z * dt 97 | print(cmd) 98 | vel_pub.publish(cmd) 99 | 100 | q_list = [pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w] 101 | (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(q_list) 102 | q = tf.transformations.quaternion_from_euler(roll, pitch, yaw + cmd.angular.z / HZ) 103 | pose.pose.orientation.x = q[0] 104 | pose.pose.orientation.y = q[1] 105 | pose.pose.orientation.z = q[2] 106 | pose.pose.orientation.w = q[3] 107 | pose.pose.position.x += cmd.linear.x * m.cos(yaw) / HZ - cmd.linear.y * m.sin(yaw) / HZ 108 | pose.pose.position.y += cmd.linear.x * m.sin(yaw) / HZ + cmd.linear.y * m.cos(yaw) / HZ 109 | br.sendTransform((pose.pose.position.x, pose.pose.position.y, 0), (pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w), rospy.Time.now(), ROBOT_FRAME, "odom") 110 | odom = Odometry() 111 | odom.header.frame_id = "odom" 112 | odom.header.stamp = rospy.Time.now() 113 | odom.child_frame_id = ROBOT_FRAME 114 | odom.pose.pose = pose.pose 115 | odom.twist.twist = cmd 116 | odom_pub.publish(odom) 117 | pose.header = odom.header 118 | print(pose) 119 | pose_pub.publish(pose) 120 | r.sleep() 121 | 122 | if __name__=='__main__': 123 | rospy.init_node('sim_3dof') 124 | try: 125 | process() 126 | except rospy.ROSInterruptException: pass 127 | -------------------------------------------------------------------------------- /scripts/sub_laser.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from sensor_msgs.msg import LaserScan 5 | 6 | HZ = 10.0 7 | 8 | laser = LaserScan() 9 | min_pooling_laser = LaserScan() 10 | 11 | def laser_callback(data): 12 | global laser 13 | laser = data 14 | min_pooling_laser = laser 15 | 16 | def process(): 17 | laser_pub = rospy.Publisher("/min_pooling_laser", LaserScan, queue_size=10) 18 | rospy.Subscriber('/scan', LaserScan, laser_callback) 19 | 20 | print("=== sub laser ===") 21 | 22 | r = rospy.Rate(HZ) 23 | 24 | while not rospy.is_shutdown(): 25 | min_pooling_laser.ranges = [] 26 | for i in (len(laser.ranges) / 81 - 1): 27 | l = min(laser.ranges[i*81:(i+1)*81]) 28 | min_pooling_laser.ranges.append(l) 29 | laser_pub.publish(min_pooling_laser) 30 | 31 | r.sleep() 32 | 33 | if __name__=='__main__': 34 | rospy.init_node('sub_laser') 35 | try: 36 | process() 37 | except rospy.ROSInterruptException: pass 38 | -------------------------------------------------------------------------------- /scripts/test_robomech.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #! coding:utf-8 3 | 4 | import rospy 5 | from geometry_msgs.msg import Twist 6 | from geometry_msgs.msg import PoseStamped 7 | from std_msgs.msg import Empty 8 | import tf 9 | 10 | import math as m 11 | import numpy as np 12 | from pyquaternion import Quaternion as q 13 | 14 | velocity = Twist() 15 | 16 | HZ = 20.0 17 | 18 | ROBOT = "DD" 19 | #ROBOT = "FWDIS" 20 | 21 | def process(): 22 | ROBOT_FRAME = rospy.get_param("/dynamic_avoidance/ROBOT_FRAME") 23 | WORLD_FRAME = rospy.get_param("/dynamic_avoidance/WORLD_FRAME") 24 | VELOCITY_TOPIC_NAME = rospy.get_param("/dynamic_avoidance/VELOCITY_TOPIC_NAME") 25 | 26 | vel_pub = rospy.Publisher(VELOCITY_TOPIC_NAME, Twist) 27 | 28 | print("=== test robomech ===") 29 | 30 | velocity.linear.x = 0 31 | velocity.linear.y = 0 32 | velocity.angular.z = 0 33 | 34 | r = rospy.Rate(HZ) 35 | 36 | current_pose = np.empty(3) 37 | current_orientation = np.empty(4) 38 | current_quaternion = q(current_orientation) 39 | last_pose = np.empty(3) 40 | last_orientation = np.empty(4) 41 | last_quaternion = q(last_orientation) 42 | first_flag = True 43 | listener = tf.TransformListener() 44 | 45 | current_time = rospy.get_time() 46 | 47 | v_robot = 0 48 | omega = 0 49 | vel_flag = False 50 | flag_time = 0 51 | w = 0 52 | theta_n = 0 53 | theta_n_max = m.pi / 2.0 54 | 55 | while not rospy.is_shutdown(): 56 | try: 57 | last_pose = current_pose 58 | last_orientation = current_orientation 59 | last_quaternion = current_quaternion 60 | (current_pose, current_orientation) = listener.lookupTransform(WORLD_FRAME, ROBOT_FRAME, rospy.Time(0)) 61 | current_pose = np.reshape(np.array(current_pose), (3)) 62 | current_quaternion = q(np.array([current_orientation[3], current_orientation[0], current_orientation[1], current_orientation[2]])) 63 | current_quaternion = current_quaternion.normalised 64 | last_time = current_time 65 | current_time = rospy.get_time() 66 | dt = current_time - last_time 67 | 68 | if not first_flag: 69 | print("process") 70 | _, _, current_yaw = tf.transformations.euler_from_quaternion(np.reshape(current_orientation, (4))) 71 | _, _, last_yaw = tf.transformations.euler_from_quaternion(np.reshape(last_orientation, (4))) 72 | d_base = current_pose - last_pose 73 | 74 | #print(current_orientation, _last_orientation) 75 | #print(current_quaternion) 76 | d_quaternion = current_quaternion.inverse * last_quaternion 77 | d_quaternion = d_quaternion.normalised 78 | v_base = d_base / dt 79 | #print(v_base) 80 | v_robot = last_quaternion.inverse.rotate(v_base) 81 | dyaw = current_yaw - last_yaw 82 | omega = dyaw / dt 83 | print(v_robot) 84 | print(omega) 85 | #print(d_quaternion.radians / dt) 86 | V_MAX = 0.5 87 | if ROBOT == "DD": 88 | velocity.linear.x = V_MAX 89 | if (v_robot[0] > 0.5) and (not vel_flag): 90 | vel_flag = True 91 | flag_time = rospy.get_time() 92 | elif vel_flag: 93 | time = rospy.get_time() - flag_time 94 | if time > 1.0: 95 | print(time, "[s]") 96 | W_MAX = 0.8 97 | DW = 1.0 98 | w += DW / HZ 99 | if w < -W_MAX: 100 | w = -W_MAX 101 | elif w > W_MAX: 102 | w = W_MAX 103 | velocity.angular.z = w 104 | vel_pub.publish(velocity) 105 | print(velocity) 106 | print("DD") 107 | elif ROBOT == "FWDIS": 108 | W_MAX = 3 109 | current_v = m.sqrt(v_robot[0] * v_robot[0] + v_robot[1] * v_robot[1]) 110 | velocity.linear.x = V_MAX 111 | if (current_v > 0.5) and (not vel_flag): 112 | vel_flag = True 113 | flag_time = rospy.get_time() 114 | elif vel_flag: 115 | time = rospy.get_time() - flag_time 116 | if time > 1.0: 117 | print(time, "[s]") 118 | theta_n += W_MAX / HZ 119 | if theta_n < -theta_n_max: 120 | theta_n = -theta_n_max 121 | elif theta_n > theta_n_max: 122 | theta_n = theta_n_max 123 | velocity.linear.x = V_MAX * m.cos(theta_n) 124 | velocity.linear.y = V_MAX * m.sin(theta_n) 125 | vel_pub.publish(velocity) 126 | print(velocity) 127 | print(theta_n) 128 | print("FWDIS") 129 | else: 130 | first_flag = False 131 | print("first") 132 | 133 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 134 | #print("exception") 135 | continue 136 | 137 | r.sleep() 138 | 139 | if __name__=='__main__': 140 | rospy.init_node('test_robomech') 141 | try: 142 | process() 143 | except rospy.ROSInterruptException: pass 144 | -------------------------------------------------------------------------------- /src/base_costmap_tf_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | std::string WORLD_FRAME; 7 | std::string ROBOT_FRAME; 8 | 9 | int main(int argc, char** argv) 10 | { 11 | ros::init(argc, argv, "base_costmap_tf_publisher_tf_publisher"); 12 | ros::NodeHandle nh; 13 | 14 | ros::NodeHandle local_nh("~"); 15 | local_nh.getParam("/dynamic_avoidance/ROBOT_FRAME", ROBOT_FRAME); 16 | local_nh.getParam("/dynamic_avoidance/WORLD_FRAME", WORLD_FRAME); 17 | 18 | tf::TransformBroadcaster br; 19 | 20 | geometry_msgs::TransformStamped base_costmap_tf_publisher; 21 | 22 | tf::TransformListener listener; 23 | 24 | base_costmap_tf_publisher.header.frame_id = WORLD_FRAME; 25 | base_costmap_tf_publisher.header.stamp = ros::Time::now(); 26 | base_costmap_tf_publisher.child_frame_id = "local_costmap"; 27 | //base_costmap_tf_publisher.transform.translation.x = 0; 28 | //base_costmap_tf_publisher.transform.translation.y = 0; 29 | base_costmap_tf_publisher.transform.rotation = tf::createQuaternionMsgFromYaw(0); 30 | //br.sendTransform(base_costmap_tf_publisher); 31 | 32 | ros::Rate loop_rate(100); 33 | 34 | std::cout << "base_link to local_costmap" << std::endl; 35 | 36 | while(ros::ok()){ 37 | bool transformed = false; 38 | tf::StampedTransform transform; 39 | try{ 40 | listener.lookupTransform(WORLD_FRAME, ROBOT_FRAME, ros::Time(0), transform); 41 | transformed = true; 42 | }catch(tf::TransformException ex){ 43 | std::cout << ex.what() << std::endl; 44 | } 45 | if(transformed){ 46 | base_costmap_tf_publisher.header.stamp = ros::Time::now(); 47 | base_costmap_tf_publisher.transform.translation.x = transform.getOrigin().getX(); 48 | base_costmap_tf_publisher.transform.translation.y = transform.getOrigin().getY(); 49 | //base_costmap_tf_publisher.transform.rotation = tf::createQuaternionMsgFromYaw(-tf::getYaw(transform.getRotation())); 50 | base_costmap_tf_publisher.transform.rotation = tf::createQuaternionMsgFromYaw(0); 51 | br.sendTransform(base_costmap_tf_publisher); 52 | } 53 | 54 | loop_rate.sleep(); 55 | ros::spinOnce(); 56 | } 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /src/collision_detector.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | class CollisionDetector 12 | { 13 | public: 14 | CollisionDetector(void); 15 | 16 | void process(void); 17 | bool detect_collision(geometry_msgs::Pose, geometry_msgs::Pose); 18 | geometry_msgs::PoseArray get_obstacles(void); 19 | 20 | private: 21 | double RADIUS; 22 | std::string WORLD_FRAME; 23 | std::string ROBOT_FRAME; 24 | std::string OBS_PREFIX; 25 | 26 | ros::NodeHandle nh; 27 | ros::NodeHandle local_nh; 28 | 29 | ros::Publisher collision_pub; 30 | 31 | tf::TransformListener listener; 32 | 33 | double min_distance; 34 | }; 35 | 36 | CollisionDetector::CollisionDetector(void) 37 | :local_nh("~") 38 | { 39 | local_nh.param("/dynamic_avoidance/RADIUS", RADIUS, {0.6}); 40 | local_nh.param("/dynamic_avoidance/ROBOT_FRAME", ROBOT_FRAME, {"base_link"}); 41 | local_nh.param("/dynamic_avoidance/WORLD_FRAME", WORLD_FRAME, {"world"}); 42 | local_nh.param("/dynamic_avoidance/OBSTACLES_FRAME", OBS_PREFIX, {"obs"}); 43 | 44 | collision_pub = nh.advertise("/collision_info", 1); 45 | 46 | min_distance = 1e6; 47 | } 48 | 49 | void CollisionDetector::process(void) 50 | { 51 | geometry_msgs::PoseStamped current_robot; 52 | current_robot.header.frame_id = WORLD_FRAME; 53 | 54 | ros::Rate loop_rate(10); 55 | 56 | std::cout << "=== collision detector ===" << std::endl; 57 | 58 | unsigned int collision_count = 0; 59 | 60 | std::map collision_list; 61 | 62 | while(ros::ok()){ 63 | bool transformed_flag = false; 64 | static Eigen::Vector3d last_robot_position = Eigen::Vector3d::Zero(); 65 | double robot_speed = 0; 66 | try{ 67 | static double last_transform_time = ros::Time::now().toSec(); 68 | tf::StampedTransform transform; 69 | listener.lookupTransform(WORLD_FRAME, ROBOT_FRAME, ros::Time(0), transform); 70 | current_robot.header.stamp = transform.stamp_; 71 | double transform_time = current_robot.header.stamp.toSec(); 72 | tf::poseTFToMsg(transform, current_robot.pose); 73 | Eigen::Vector3d robot_position(current_robot.pose.position.x, current_robot.pose.position.y, current_robot.pose.position.z); 74 | double dt = transform_time - last_transform_time; 75 | if(dt > 0.0){ 76 | robot_speed = (robot_position - last_robot_position).norm() / dt; 77 | } 78 | last_robot_position = robot_position; 79 | last_transform_time = transform_time; 80 | transformed_flag = true; 81 | }catch(tf::TransformException& ex){ 82 | std::cout << ex.what() << std::endl; 83 | } 84 | if(transformed_flag){ 85 | geometry_msgs::PoseArray obstacles = get_obstacles(); 86 | int obs_num = obstacles.poses.size(); 87 | // std::cout << "obs num:" << obs_num << std::endl; 88 | for(int i=0;itrue; 128 | */ 129 | double dx = p0.position.x - p1.position.x; 130 | double dy = p0.position.y - p1.position.y; 131 | double distance = sqrt(dx*dx + dy*dy); 132 | if(distance < min_distance){ 133 | min_distance = distance; 134 | std::cout << "min distance: " << min_distance << "[m]" << std::endl; 135 | } 136 | if(distance <= RADIUS){ 137 | std::cout << distance << "[m]" << std::endl; 138 | return true; 139 | } 140 | return false; 141 | } 142 | 143 | geometry_msgs::PoseArray CollisionDetector::get_obstacles(void) 144 | { 145 | geometry_msgs::PoseArray obstacles; 146 | tf::FrameGraph::Request req; 147 | tf::FrameGraph::Response res; 148 | if(listener.getFrames(req, res)){ 149 | std::vector obs_names; 150 | while(1){ 151 | // std::cout << res.dot_graph << std::endl; 152 | int position = res.dot_graph.find("-> \"" + OBS_PREFIX); 153 | // std::cout << "position: " << position << std::endl; 154 | if(position != std::string::npos){ 155 | position = res.dot_graph.find(OBS_PREFIX); 156 | res.dot_graph.erase(0, position); 157 | int double_quotation_pos = res.dot_graph.find("\""); 158 | std::string obs_name = res.dot_graph.substr(0, double_quotation_pos); 159 | res.dot_graph.erase(0, double_quotation_pos); 160 | // std::cout << obs_name << std::endl; 161 | obs_names.emplace_back(obs_name); 162 | }else{ 163 | // std::cout << OBS_PREFIX << " was not found" << std::endl; 164 | break; 165 | } 166 | } 167 | obstacles.header.frame_id = WORLD_FRAME; 168 | obstacles.header.stamp = ros::Time::now(); 169 | for(const auto& obs_name : obs_names){ 170 | try{ 171 | tf::StampedTransform transform; 172 | listener.lookupTransform(WORLD_FRAME, obs_name, ros::Time(0), transform); 173 | obstacles.header.stamp = transform.stamp_; 174 | geometry_msgs::Pose pose; 175 | pose.position.x = transform.getOrigin().x(); 176 | pose.position.y = transform.getOrigin().y(); 177 | pose.position.z = transform.getOrigin().z(); 178 | tf::quaternionTFToMsg(transform.getRotation(), pose.orientation); 179 | obstacles.poses.push_back(pose); 180 | }catch(tf::TransformException& ex){ 181 | std::cout << ex.what() << std::endl; 182 | } 183 | } 184 | }else{ 185 | std::cout << "cannot get frames" << std::endl; 186 | } 187 | return obstacles; 188 | } 189 | -------------------------------------------------------------------------------- /src/demo/obstacle_tracker_kf_demo.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "dynamic_obstacle_avoidance_planner/obstacle_tracker_kf.h" 9 | 10 | class ObstacleTrackerKFDemo 11 | { 12 | public: 13 | ObstacleTrackerKFDemo(void); 14 | 15 | void obstacles_callback(const geometry_msgs::PoseArrayConstPtr&); 16 | void process(void); 17 | 18 | private: 19 | std::string FIXED_FRAME; 20 | 21 | ros::NodeHandle nh; 22 | ros::NodeHandle local_nh; 23 | ros::Publisher velocity_arrows_pub; 24 | ros::Subscriber obstacles_sub; 25 | 26 | tf::TransformListener listener; 27 | 28 | ObstacleTrackerKF tracker; 29 | }; 30 | 31 | ObstacleTrackerKFDemo::ObstacleTrackerKFDemo(void) 32 | :local_nh("~") 33 | { 34 | velocity_arrows_pub = local_nh.advertise("velocity_arrows", 1); 35 | obstacles_sub = nh.subscribe("/dynamic_obstacles", 1, &ObstacleTrackerKFDemo::obstacles_callback, this); 36 | 37 | local_nh.param("FIXED_FRAME", FIXED_FRAME, {"/odom"}); 38 | } 39 | 40 | void ObstacleTrackerKFDemo::obstacles_callback(const geometry_msgs::PoseArrayConstPtr& msg) 41 | { 42 | std::cout << "obstacles callback" << std::endl; 43 | geometry_msgs::PoseArray obstacle_pose; 44 | obstacle_pose = *msg; 45 | try{ 46 | for(auto& p : obstacle_pose.poses){ 47 | geometry_msgs::PoseStamped p_; 48 | p_.header = obstacle_pose.header; 49 | p_.pose = p; 50 | listener.transformPose(FIXED_FRAME, p_, p_); 51 | p = p_.pose; 52 | } 53 | obstacle_pose.header.frame_id = FIXED_FRAME; 54 | }catch(tf::TransformException ex){ 55 | std::cout << ex.what() << std::endl; 56 | return; 57 | } 58 | tracker.set_obstacles_pose(obstacle_pose); 59 | std::vector poses; 60 | tracker.get_poses(poses); 61 | std::vector velocities; 62 | tracker.get_velocities(velocities); 63 | 64 | static int last_obs_num = 0; 65 | int obs_num = poses.size(); 66 | visualization_msgs::MarkerArray velocity_arrows; 67 | for(int i=0;iheader.stamp; 70 | v_arrow.header.frame_id = FIXED_FRAME; 71 | v_arrow.id = i; 72 | v_arrow.ns = "obstacle_tracker_kf_demo"; 73 | v_arrow.type = visualization_msgs::Marker::ARROW; 74 | v_arrow.action = visualization_msgs::Marker::ADD; 75 | v_arrow.lifetime = ros::Duration(); 76 | v_arrow.pose.position.x = poses[i](0); 77 | v_arrow.pose.position.y = poses[i](1); 78 | v_arrow.pose.position.z = poses[i](2); 79 | double direction = atan2(velocities[i](1), velocities[i](0)); 80 | v_arrow.pose.orientation = tf::createQuaternionMsgFromYaw(direction); 81 | v_arrow.scale.x = velocities[i].norm(); 82 | v_arrow.scale.y = v_arrow.scale.z = 0.3; 83 | v_arrow.color.r = 1.0; 84 | v_arrow.color.a = 1.0; 85 | velocity_arrows.markers.push_back(v_arrow); 86 | } 87 | for(int i=obs_num;i 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | std::vector obs_list; 11 | 12 | const double DT = 0.1;//[s] 13 | const double HZ = 100; 14 | 15 | int NUM; 16 | 17 | std::string WORLD_FRAME; 18 | std::string OBS_FRAME; 19 | 20 | double get_yaw(geometry_msgs::Quaternion); 21 | void set_pose(int, double, double, double); 22 | void update(int, double, double); 23 | 24 | int main(int argc, char** argv) 25 | { 26 | ros::init(argc, argv, "diff_drive_sim"); 27 | ros::NodeHandle nh; 28 | ros::NodeHandle local_nh("~"); 29 | 30 | local_nh.getParam("/dynamic_avoidance/WORLD_FRAME", WORLD_FRAME); 31 | local_nh.getParam("/dynamic_avoidance/OBSTACLES_FRAME", OBS_FRAME); 32 | 33 | NUM = 5; 34 | 35 | obs_list.resize(NUM); 36 | 37 | tf::TransformBroadcaster obs_broadcaster; 38 | 39 | // 初期位置設定 40 | for(int i=0;i dist(0.0, 0.20); 94 | obs_list[index].header.stamp = ros::Time::now(); 95 | double yaw = tf::getYaw(obs_list[index].transform.rotation); 96 | v += dist(engine); 97 | omega += dist(engine); 98 | obs_list[index].transform.translation.x += v * cos(yaw) / HZ; 99 | obs_list[index].transform.translation.y += v * sin(yaw) / HZ; 100 | obs_list[index].transform.rotation = tf::createQuaternionMsgFromYaw(yaw + omega / HZ); 101 | } 102 | -------------------------------------------------------------------------------- /src/holonomic_dwa.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | double MAX_VELOCITY; 11 | double MAX_ACCELERATION; 12 | double MAX_ANGULAR_VELOCITY; 13 | double MAX_ANGULAR_ACCELERATION; 14 | double VELOCITY_RESOLUTION; 15 | double ANGULAR_VELOCITY_RESOLUTION; 16 | double INTERVAL; 17 | double DT; 18 | double SIMULATE_TIME; 19 | double ROBOT_RADIUS; 20 | double GOAL_XY_TOLERANCE; 21 | double GOAL_YAW_TOLERANCE; 22 | 23 | double ALPHA; 24 | double BETA; 25 | double GAMMA; 26 | 27 | double GOAL_DISTANCE; 28 | 29 | double window_vx_max = MAX_VELOCITY; 30 | double window_vx_min = -MAX_VELOCITY; 31 | double window_vy_max = MAX_VELOCITY; 32 | double window_vy_min = -MAX_VELOCITY; 33 | double window_omega_max = MAX_ANGULAR_VELOCITY; 34 | double window_omega_min = -MAX_ANGULAR_VELOCITY; 35 | 36 | geometry_msgs::PoseStamped _goal; 37 | geometry_msgs::PoseStamped goal; 38 | geometry_msgs::Twist velocity; 39 | std::vector > > cost; 40 | geometry_msgs::PoseArray local_path; 41 | nav_msgs::Path global_path; 42 | geometry_msgs::PoseStamped odometry; 43 | 44 | void path_callback(const nav_msgs::PathConstPtr& msg) 45 | { 46 | global_path = *msg; 47 | } 48 | 49 | void calculate_dynamic_window(void); 50 | void generate_paths(void); 51 | double evaluate_distance_to_local_goal(nav_msgs::Path); 52 | double evaluate_distance_to_global_path(nav_msgs::Path); 53 | double calcurate_distance(geometry_msgs::Pose, geometry_msgs::Pose); 54 | 55 | geometry_msgs::Twist get_velocity(void); 56 | 57 | int main(int argc, char** argv) 58 | { 59 | ros::init(argc, argv, "holonomic_dwa"); 60 | 61 | ros::NodeHandle nh; 62 | 63 | ros::NodeHandle local_nh("~"); 64 | 65 | local_nh.getParam("MAX_VELOCITY", MAX_VELOCITY); 66 | local_nh.getParam("MAX_ACCELERATION", MAX_ACCELERATION); 67 | local_nh.getParam("MAX_ANGULAR_VELOCITY", MAX_ANGULAR_VELOCITY); 68 | local_nh.getParam("MAX_ANGULAR_ACCELERATION", MAX_ANGULAR_ACCELERATION); 69 | local_nh.getParam("VELOCITY_RESOLUTION", VELOCITY_RESOLUTION); 70 | local_nh.getParam("ANGULAR_VELOCITY_RESOLUTION", ANGULAR_VELOCITY_RESOLUTION); 71 | local_nh.getParam("INTERVAL", INTERVAL); 72 | local_nh.getParam("DT", DT); 73 | local_nh.getParam("SIMULATE_TIME", SIMULATE_TIME); 74 | local_nh.getParam("ROBOT_RADIUS", ROBOT_RADIUS); 75 | local_nh.getParam("GOAL_XY_TOLERANCE", GOAL_XY_TOLERANCE); 76 | local_nh.getParam("GOAL_YAW_TOLERANCE", GOAL_YAW_TOLERANCE); 77 | local_nh.getParam("ALPHA", ALPHA); 78 | local_nh.getParam("BETA", BETA); 79 | local_nh.getParam("GAMMA", GAMMA); 80 | local_nh.getParam("GOAL_DISTANCE", GOAL_DISTANCE); 81 | 82 | std::cout << MAX_VELOCITY << std::endl; 83 | std::cout << MAX_ACCELERATION << std::endl; 84 | std::cout << MAX_ANGULAR_VELOCITY << std::endl; 85 | std::cout << MAX_ANGULAR_ACCELERATION << std::endl; 86 | std::cout << VELOCITY_RESOLUTION << std::endl; 87 | std::cout << ANGULAR_VELOCITY_RESOLUTION << std::endl; 88 | std::cout << INTERVAL << std::endl; 89 | std::cout << DT << std::endl; 90 | std::cout << SIMULATE_TIME << std::endl; 91 | std::cout << ROBOT_RADIUS << std::endl; 92 | std::cout << GOAL_XY_TOLERANCE << std::endl; 93 | std::cout << GOAL_YAW_TOLERANCE << std::endl; 94 | std::cout << ALPHA << std::endl; 95 | std::cout << BETA << std::endl; 96 | std::cout << GAMMA << std::endl; 97 | std::cout << GOAL_DISTANCE << std::endl; 98 | 99 | ros::Publisher velocity_pub = nh.advertise("/fwdis/velocity", 100); 100 | 101 | ros::Publisher path_pub = nh.advertise("/fwdis/local_path", 100); 102 | 103 | ros::Publisher goal_pub = nh.advertise("/fwdis/local_goal", 100); 104 | 105 | ros::Subscriber path_sub = nh.subscribe("/path", 100, path_callback); 106 | 107 | tf::TransformListener listener; 108 | tf::StampedTransform _transform; 109 | geometry_msgs::TransformStamped transform; 110 | 111 | velocity.linear.x = 0; 112 | velocity.linear.y = 0; 113 | velocity.angular.z = 0; 114 | 115 | _goal.header.frame_id = "odom"; 116 | _goal.pose.position.x = 1; 117 | _goal.pose.orientation.z = 1; 118 | 119 | local_path.header.frame_id = "base_link"; 120 | 121 | ros::Rate loop_rate(10); 122 | 123 | while(ros::ok()){ 124 | bool transformed = false; 125 | try{ 126 | listener.lookupTransform("odom", "base_link", ros::Time(0), _transform); 127 | tf::transformStampedTFToMsg(_transform, transform); 128 | odometry.header = transform.header; 129 | odometry.pose.position.x = transform.transform.translation.x; 130 | odometry.pose.position.y = transform.transform.translation.y; 131 | odometry.pose.position.z = transform.transform.translation.z; 132 | odometry.pose.orientation = transform.transform.rotation; 133 | 134 | transformed = true; 135 | }catch(tf::TransformException ex){ 136 | ROS_ERROR("%s", ex.what()); 137 | ros::Duration(1.0).sleep(); 138 | } 139 | if(!global_path.poses.empty() && transformed){ 140 | // local goal を決める部分 141 | try{ 142 | for(int i=global_path.poses.size()-1;i>=0;i--){ 143 | if(calcurate_distance(global_path.poses[i].pose, odometry.pose) < GOAL_DISTANCE){ 144 | _goal = global_path.poses[i]; 145 | break; 146 | } 147 | } 148 | }catch(tf::TransformException ex){ 149 | ROS_ERROR("%s", ex.what()); 150 | ros::Duration(1.0).sleep(); 151 | } 152 | try{ 153 | listener.transformPose("base_link", ros::Time(0), _goal, "odom", goal); 154 | }catch(tf::TransformException ex){ 155 | std::cout << ex.what() << std::endl; 156 | ros::Duration(1.0).sleep(); 157 | } 158 | 159 | goal_pub.publish(goal); 160 | //std::cout << goal << std::endl; 161 | calculate_dynamic_window(); 162 | generate_paths(); 163 | velocity = get_velocity(); 164 | double distance = sqrt(goal.pose.position.x * goal.pose.position.x + goal.pose.position.y * goal.pose.position.y); 165 | if(distance < GOAL_XY_TOLERANCE){ 166 | std::cout << "stop" << std::endl; 167 | velocity.linear.x = 0; 168 | velocity.linear.y = 0; 169 | } 170 | velocity_pub.publish(velocity); 171 | 172 | local_path.poses.clear(); 173 | geometry_msgs::Pose pose; 174 | pose.position.x = 0; 175 | pose.position.y = 0; 176 | pose.orientation = tf::createQuaternionMsgFromYaw(0); 177 | int step_time = SIMULATE_TIME / (10 * DT); 178 | for(int t=0;t MAX_VELOCITY){ 202 | window_vx_max = MAX_VELOCITY; 203 | } 204 | window_vx_min = _velocity.linear.x - INTERVAL * MAX_ACCELERATION; 205 | if(window_vx_min < -MAX_VELOCITY){ 206 | window_vx_min = -MAX_VELOCITY; 207 | } 208 | window_vy_max = _velocity.linear.y + INTERVAL * MAX_ACCELERATION; 209 | if(window_vy_max > MAX_VELOCITY){ 210 | window_vy_max = MAX_VELOCITY; 211 | } 212 | window_vy_min = _velocity.linear.y - INTERVAL * MAX_ACCELERATION; 213 | if(window_vy_min < -MAX_VELOCITY){ 214 | window_vy_min = -MAX_VELOCITY; 215 | } 216 | window_omega_max = _velocity.angular.z + INTERVAL * MAX_ANGULAR_ACCELERATION; 217 | if(window_omega_max > MAX_ANGULAR_VELOCITY){ 218 | window_omega_max = MAX_ANGULAR_VELOCITY; 219 | } 220 | window_omega_min = _velocity.angular.z - INTERVAL * MAX_ANGULAR_ACCELERATION; 221 | if(window_omega_min < -MAX_ANGULAR_VELOCITY){ 222 | window_omega_min = -MAX_ANGULAR_VELOCITY; 223 | } 224 | } 225 | 226 | void generate_paths(void) 227 | { 228 | std::cout << "generate paths" << std::endl; 229 | int step_vx = (window_vx_max - window_vx_min) / VELOCITY_RESOLUTION + 1; 230 | int step_vy = (window_vy_max - window_vy_min) / VELOCITY_RESOLUTION + 1; 231 | int step_omega = (window_omega_max - window_omega_min) / ANGULAR_VELOCITY_RESOLUTION + 1; 232 | int step_time = SIMULATE_TIME / DT; 233 | 234 | /* 235 | std::cout << window_vx_max << ", "; 236 | std::cout << window_vx_min << std::endl; 237 | std::cout << window_vy_max << ", "; 238 | std::cout << window_vy_min << std::endl; 239 | std::cout << window_omega_max << ", "; 240 | std::cout << window_omega_min << std::endl; 241 | std::cout << step_vx << ", "; 242 | std::cout << step_vy << ", "; 243 | std::cout << step_omega << std::endl; 244 | */ 245 | 246 | cost.clear(); 247 | cost.resize(step_vx); 248 | for(int i=0;i MAX_VELOCITY * MAX_VELOCITY){ 263 | cost[i][j][k] = -1; 264 | continue; 265 | } 266 | nav_msgs::Path path; 267 | path.header.frame_id = "base_link"; 268 | geometry_msgs::PoseStamped pose; 269 | pose.header.frame_id = "base_link"; 270 | pose.pose.position.x = 0; 271 | pose.pose.position.y = 0; 272 | pose.pose.orientation = tf::createQuaternionMsgFromYaw(0); 273 | for(int t=0;t 0){ 315 | if(cost[i][j][k] < min_cost){ 316 | min_i = i; 317 | min_j = j; 318 | min_k = k; 319 | min_cost = cost[i][j][k]; 320 | } 321 | } 322 | } 323 | } 324 | } 325 | 326 | std::cout << min_i << ", " << min_j << ", " << min_k << std::endl; 327 | 328 | _velocity.linear.x = window_vx_min + min_i * VELOCITY_RESOLUTION; 329 | _velocity.linear.y = window_vy_min + min_j * VELOCITY_RESOLUTION; 330 | _velocity.angular.z = window_omega_min + min_k * ANGULAR_VELOCITY_RESOLUTION; 331 | return _velocity; 332 | } 333 | 334 | double calcurate_distance(geometry_msgs::Pose a, geometry_msgs::Pose b) 335 | { 336 | return sqrt((a.position.x - b.position.x) * (a.position.x - b.position.x) + (a.position.y - b.position.y) * (a.position.y - b.position.y)); 337 | } 338 | 339 | double evaluate_distance_to_global_path(nav_msgs::Path local_path) 340 | { 341 | double min_distance = 100; 342 | int local_path_length = local_path.poses.size() - 1; 343 | for(int i=0;i("/local_costmap", 1); 12 | cloud_sub = nh.subscribe("/cluster/human/removed", 1, &LocalCostmapGenerator::cloud_callback, this); 13 | dynamic_sub = nh.subscribe("dynamic_local_costmap", 1, &LocalCostmapGenerator::dynamic_callback, this); 14 | cloud_ptr = pcl::PointCloud::Ptr(new pcl::PointCloud); 15 | local_costmap_subscribed = false; 16 | cloud_updated = false; 17 | 18 | std::cout << "=== local_costmap ===" << std::endl; 19 | std::cout << "RADIUS: " << RADIUS << std::endl; 20 | std::cout << "ROBOT_FRAME: " << ROBOT_FRAME << std::endl; 21 | std::cout << "WORLD_FRAME: " << WORLD_FRAME << std::endl; 22 | std::cout << "HZ: " << HZ << std::endl; 23 | } 24 | 25 | void LocalCostmapGenerator::process(void) 26 | { 27 | ros::Rate loop_rate(HZ); 28 | 29 | while(ros::ok()){ 30 | if(local_costmap_subscribed){ 31 | std::cout << "=== local_costmap ===" << std::endl; 32 | if(cloud_updated){ 33 | nav_msgs::OccupancyGrid _map; 34 | _map.data.resize(max_index); 35 | pcl_ros::transformPointCloud(local_costmap.header.frame_id, *cloud_ptr, *cloud_ptr, listener); 36 | int size = cloud_ptr->points.size(); 37 | std::cout << "cloud size: " << size << std::endl; 38 | std::cout << "max index: " << max_index << std::endl; 39 | for(int i=0;ipoints[i].x < map_max_limit_x && cloud_ptr->points[i].x > map_min_limit_x && cloud_ptr->points[i].y < map_max_limit_y && cloud_ptr->points[i].y > map_min_limit_y){ 41 | int index = get_index(cloud_ptr->points[i].x, cloud_ptr->points[i].y); 42 | if(index < max_index){ 43 | _map.data[index] = 100; 44 | } 45 | } 46 | } 47 | for(int i=0;i("/result", 1); 7 | pose_sub = nh.subscribe("/pose", 1, &Measurer::pose_callback, this); 8 | 9 | local_nh.param("GOAL_X", GOAL_X, {10.0}); 10 | local_nh.param("GOAL_Y", GOAL_Y, {0.0}); 11 | local_nh.param("GOAL_TOLERANCE", GOAL_TOLERANCE, {0.5}); 12 | 13 | first_callback_flag = true; 14 | start_time = 0; 15 | last_time = 0; 16 | traveled_distance = 0; 17 | goal << GOAL_X, GOAL_Y; 18 | // std::cout << GOAL_X << ", " << GOAL_Y << ", " << GOAL_TOLERANCE << std::endl; 19 | // std::cout << goal << std::endl; 20 | } 21 | 22 | void Measurer::process(void) 23 | { 24 | ros::spin(); 25 | } 26 | 27 | void Measurer::pose_callback(const geometry_msgs::PoseStampedConstPtr& msg) 28 | { 29 | static Eigen::Vector2d last_pose(msg->pose.position.x, msg->pose.position.y); 30 | Eigen::Vector2d current_pose(msg->pose.position.x, msg->pose.position.y); 31 | 32 | last_time = msg->header.stamp.toSec(); 33 | if(!first_callback_flag){ 34 | traveled_distance += (current_pose - last_pose).norm(); 35 | }else{ 36 | first_callback_flag = false; 37 | start_time = msg->header.stamp.toSec(); 38 | traveled_distance = 0; 39 | } 40 | last_pose = current_pose; 41 | // std::cout << "cb:" << std::endl; 42 | // std::cout << current_pose.transpose() << std::endl; 43 | // std::cout << goal.transpose() << std::endl; 44 | if((current_pose - goal).norm() < GOAL_TOLERANCE){ 45 | std::cout << "goal!!!" << std::endl; 46 | std_msgs::Float64MultiArray result; 47 | result.data.push_back(traveled_distance); 48 | result.data.push_back(last_time - start_time); 49 | result_pub.publish(result); 50 | ros::spinOnce(); 51 | ros::shutdown(); 52 | } 53 | } 54 | 55 | void Measurer::show_results(void) 56 | { 57 | std::cout << "---" << std::endl; 58 | std::cout << "traveled distance: " << traveled_distance << "[m]" << std::endl; 59 | std::cout << "traveling time: " << last_time - start_time << "[s]" << std::endl; 60 | } 61 | 62 | int main(int argc, char** argv) 63 | { 64 | ros::init(argc, argv, "measurer"); 65 | Measurer m; 66 | m.process(); 67 | return 0; 68 | } 69 | -------------------------------------------------------------------------------- /src/obstacle_tracker_kf.cpp: -------------------------------------------------------------------------------- 1 | #include "dynamic_obstacle_avoidance_planner/obstacle_tracker_kf.h" 2 | 3 | KalmanFilter::KalmanFilter(void) 4 | { 5 | sigma_a = 0.1; 6 | } 7 | 8 | Eigen::Matrix4d KalmanFilter::get_f(double dt) 9 | { 10 | Eigen::Matrix4d f; 11 | f << 1.0, 0.0, dt, 0.0, 12 | 0.0, 1.0, 0.0, dt, 13 | 0.0, 0.0, 1.0, 0.0, 14 | 0.0, 0.0, 0.0, 1.0; 15 | return f; 16 | } 17 | 18 | Eigen::Matrix4d KalmanFilter::get_q(double dt) 19 | { 20 | Eigen::Matrix g; 21 | g << dt * dt / 2.0, 0.0, 22 | 0.0, dt * dt / 2.0, 23 | dt, 0.0, 24 | 0.0, dt; 25 | Eigen::Matrix4d q = sigma_a * sigma_a * g * g.transpose(); 26 | return q; 27 | } 28 | 29 | Obstacle::Obstacle(void) 30 | { 31 | x = Eigen::Vector4d::Zero(); 32 | 33 | initialize(); 34 | } 35 | 36 | Obstacle::Obstacle(const Obstacle& obstacle) 37 | { 38 | x = obstacle.x; 39 | p = obstacle.p; 40 | r = obstacle.r; 41 | h = obstacle.h; 42 | likelihood = obstacle.likelihood; 43 | last_time = obstacle.last_time; 44 | lifetime = obstacle.lifetime; 45 | age = obstacle.age; 46 | not_observed_time = obstacle.not_observed_time; 47 | } 48 | 49 | Obstacle::Obstacle(const Eigen::Vector2d& position) 50 | { 51 | x << position(0), position(1), 0.0, 0.0; 52 | 53 | initialize(); 54 | } 55 | 56 | void Obstacle::initialize(void) 57 | { 58 | p << 1e1, 0.0, 0.0, 0.0, 59 | 0.0, 1e1, 0.0, 0.0, 60 | 0.0, 0.0, 1e1, 0.0, 61 | 0.0, 0.0, 0.0, 1e1; 62 | 63 | h << 1.0, 0.0, 0.0, 0.0, 64 | 0.0, 1.0, 0.0, 0.0; 65 | 66 | r << 1e-4, 0.0, 67 | 0.0, 1e-4; 68 | 69 | last_time = ros::Time::now().toSec(); 70 | likelihood = 1.0; 71 | lifetime = 1; 72 | age = 0; 73 | not_observed_time = 0; 74 | } 75 | 76 | Eigen::Vector2d Obstacle::get_position(void) 77 | { 78 | return x.segment(0, 2); 79 | } 80 | 81 | void Obstacle::update(const Eigen::Vector2d& z) 82 | { 83 | // std::cout << "update" << std::endl; 84 | // std::cout << "Z:\n" << z << std::endl; 85 | Eigen::Vector2d e = z - h * x; 86 | Eigen::Matrix2d s = h * p * h.transpose() + r; 87 | Eigen::Matrix k = p * h.transpose() * s.inverse(); 88 | // std::cout << "K:\n" << k << std::endl; 89 | x = x + k * e; 90 | // std::cout << "X:\n" << x << std::endl; 91 | p = (Eigen::Matrix4d::Identity() - k * h) * p; 92 | // std::cout << "P:\n" << p << std::endl; 93 | 94 | not_observed_time = 0; 95 | } 96 | 97 | void Obstacle::predict(void) 98 | { 99 | // std::cout << "predict" << std::endl; 100 | double current_time = ros::Time::now().toSec(); 101 | double dt = current_time - last_time; 102 | last_time = current_time; 103 | age += dt; 104 | not_observed_time += dt; 105 | // std::cout << "age: " << age << std::endl; 106 | // std::cout << "not_observed_time: " << not_observed_time << std::endl; 107 | 108 | Eigen::Matrix4d f = kf.get_f(dt); 109 | x = f * x; 110 | // std::cout << "X:\n" << x << std::endl;; 111 | Eigen::Matrix4d q = kf.get_q(dt); 112 | p = f * p * f.transpose() + q; 113 | // std::cout << "P:\n" << p << std::endl;; 114 | } 115 | 116 | void Obstacle::predict(double dt) 117 | { 118 | // std::cout << "predict" << std::endl; 119 | age += dt; 120 | not_observed_time += dt; 121 | // std::cout << "age: " << age << std::endl; 122 | // std::cout << "not_observed_time: " << not_observed_time << std::endl; 123 | 124 | Eigen::Matrix4d f = kf.get_f(dt); 125 | x = f * x; 126 | // std::cout << "X:\n" << x << std::endl;; 127 | Eigen::Matrix4d q = kf.get_q(dt); 128 | p = f * p * f.transpose() + q; 129 | // std::cout << "P:\n" << p << std::endl;; 130 | } 131 | 132 | double Obstacle::calculate_likelihood(void) 133 | { 134 | Eigen::Matrix2d m = p.block<2, 2>(0, 0);// up left 2x2 135 | Eigen::EigenSolver es(m); 136 | if(!es.info()){ 137 | Eigen::Vector2d e_values = es.eigenvalues().real(); 138 | const double CHI2 = 9.21034;// chi-square, 99% 139 | double a, b;// ellipse parameter 140 | if(e_values(0) > e_values(1)){ 141 | a = sqrt(CHI2 * e_values(0)); 142 | b = sqrt(CHI2 * e_values(1)); 143 | }else{ 144 | a = sqrt(CHI2 * e_values(1)); 145 | b = sqrt(CHI2 * e_values(0)); 146 | } 147 | if(a * b > 1e-5){ 148 | likelihood = 1.0 / (a * b); 149 | }else{ 150 | likelihood = 1e3; 151 | } 152 | }else{ 153 | std::cout << "Eigen solver error: " << es.info() << std::endl; 154 | } 155 | return likelihood; 156 | } 157 | 158 | ObstacleTrackerKF::ObstacleTrackerKF(void) 159 | :SAME_OBSTACLE_THRESHOLD(0.8), ERASE_LIKELIHOOD_THREHSOLD(0.8) 160 | , NOT_OBSERVED_TIME_THRESHOLD(3.0), DEFAULT_LIFE_TIME(5.0) 161 | { 162 | obstacles.clear(); 163 | } 164 | 165 | void ObstacleTrackerKF::set_obstacles_pose(const geometry_msgs::PoseArray& pose_array) 166 | { 167 | std::cout << "tracking " << obstacles.size() << " obstacles" << std::endl; 168 | std::cout << "observed " << pose_array.poses.size() << " obstacles" << std::endl; 169 | std::vector observed_obstacles; 170 | observed_obstacles.reserve(pose_array.poses.size()); 171 | for(const auto& pose : pose_array.poses){ 172 | Eigen::Vector2d position; 173 | position << pose.position.x, pose.position.y; 174 | observed_obstacles.push_back(position); 175 | } 176 | bool is_suceeded = associate_obstacles(observed_obstacles); 177 | if(!is_suceeded){ 178 | std::cout << "\033[31m=====================\nfailed to associate obstacles\n=====================\033[0m" << std::endl; 179 | observed_obstacles.clear(); 180 | associate_obstacles(observed_obstacles); 181 | } 182 | update_tracking(observed_obstacles); 183 | 184 | std::cout << "--- predict ---" << std::endl; 185 | auto it = obstacles.begin(); 186 | while(it != obstacles.end()){ 187 | it->second.predict(); 188 | std::cout << "not observed: " << it->second.not_observed_time << std::endl; 189 | std::cout << "age: " << it->second.age << std::endl; 190 | std::cout << "likelihood: " << it->second.calculate_likelihood() << std::endl; 191 | if(it->second.age < 0.0){ 192 | std::cout << "\033[31mobstacle " << it->first << " was erased\033[0m" << std::endl; 193 | it = obstacles.erase(it); 194 | continue; 195 | } 196 | if((it->second.calculate_likelihood() > ERASE_LIKELIHOOD_THREHSOLD) && (it->second.not_observed_time < NOT_OBSERVED_TIME_THRESHOLD)){ 197 | it->second.lifetime = DEFAULT_LIFE_TIME; 198 | ++it; 199 | }else{ 200 | if(it->second.lifetime > it->second.age){ 201 | // ??? 202 | ++it; 203 | std::cout << "\033[33mthe likelihood of this obstacle is small but not old enough to be erased\033[0m" << std::endl; 204 | }else{ 205 | std::cout << "\033[31mobstacle " << it->first << " was erased\033[0m" << std::endl; 206 | it = obstacles.erase(it); 207 | } 208 | } 209 | } 210 | } 211 | 212 | void ObstacleTrackerKF::get_velocities(std::vector& velocities) 213 | { 214 | for(auto it=obstacles.begin();it!=obstacles.end();++it){ 215 | Eigen::Vector3d v; 216 | v << it->second.x(2), it->second.x(3), 0; 217 | velocities.push_back(v); 218 | } 219 | } 220 | 221 | void ObstacleTrackerKF::get_poses(std::vector& poses) 222 | { 223 | for(auto it=obstacles.begin();it!=obstacles.end();++it){ 224 | Eigen::Vector3d p; 225 | p << it->second.x(0), it->second.x(1), 0; 226 | poses.push_back(p); 227 | } 228 | } 229 | 230 | bool ObstacleTrackerKF::associate_obstacles(const std::vector& observed_obstacles) 231 | { 232 | std::cout << "--- associate obstacles ---" << std::endl; 233 | std::cout << "observed obstacles:" << std::endl; 234 | for(const auto& oo : observed_obstacles){ 235 | std::cout << oo.transpose() << std::endl; 236 | } 237 | int cluster_num = obstacles.size(); 238 | int observed_obstacles_num = observed_obstacles.size(); 239 | 240 | int matrix_size = cluster_num + observed_obstacles_num; 241 | 242 | Eigen::MatrixXi association_matrix(matrix_size, matrix_size); 243 | for(int i=0;ifirst; 290 | } 291 | index_++; 292 | } 293 | return -1; 294 | } 295 | 296 | int ObstacleTrackerKF::get_new_id(void) 297 | { 298 | int new_id = 0; 299 | int obstacles_num = obstacles.size(); 300 | for(int i=0;ifirst){ 304 | used_id_flag = true; 305 | break; 306 | } 307 | } 308 | if(!used_id_flag){ 309 | return new_id; 310 | } 311 | new_id++; 312 | } 313 | return new_id; 314 | } 315 | 316 | bool ObstacleTrackerKF::solve_hungarian_method(Eigen::MatrixXi& matrix) 317 | { 318 | // reference: http://www.prefield.com/algorithm/math/hungarian.html 319 | const double inf = 1e6; 320 | int n = matrix.rows(), p, q; 321 | std::vector fx(n, inf), fy(n, 0); 322 | std::vector x(n, -1), y(n, -1); 323 | for(int i = 0;i < n;++i){ 324 | for(int j = 0;j < n;++j){ 325 | fx[i] = std::min(fx[i], matrix(i, j)); 326 | } 327 | } 328 | const int MAX_ITERATION = 100; 329 | int count = 0; 330 | for(int i = 0;i < n;){ 331 | // std::cout << "count: " << count << std::endl; 332 | if(count > MAX_ITERATION){ 333 | std::cout << "\033[31mafter " << MAX_ITERATION << " times loop, break!!!!!\033[0m" << std::endl; 334 | return false; 335 | } 336 | std::vector t(n, -1), s(n+1, i); 337 | for(p = q = 0;p <= q && x[i] < 0;++p){ 338 | for(int k = s[p], j = 0;j < n && x[i] < 0;++j){ 339 | if(fx[k] + fy[j] == matrix(k, j) && t[j] < 0){ 340 | s[++q] = y[j]; 341 | t[j] = k; 342 | if(s[q] < 0){ 343 | for(p = j;p >= 0;j = p){ 344 | y[j] = k = t[j]; 345 | p = x[k]; 346 | x[k] = j; 347 | } 348 | } 349 | } 350 | } 351 | } 352 | if(x[i] < 0){ 353 | int d = inf; 354 | for(int k = 0;k <= q;++k){ 355 | for(int j = 0;j < n;++j){ 356 | if(t[j] < 0){ 357 | d = std::max(d, fx[s[k]] + fy[j] - matrix(s[k], j)); 358 | } 359 | } 360 | for(int j = 0;j < n;++j){ 361 | fy[j] += (t[j] < 0 ? 0 : d); 362 | } 363 | for(int k = 0;k <= q;++k){ 364 | fx[s[k]] -= d; 365 | } 366 | } 367 | }else{ 368 | ++i; 369 | } 370 | count++; 371 | } 372 | // std::cout << "candidates: " << std::endl; 373 | candidates.resize(n); 374 | for(int i = 0;i < n;++i){ 375 | candidates[i] = get_id_from_index(y[i]); 376 | // std::cout << i << ": " << candidates[i] << std::endl; 377 | // std::cout << "index: " << y[i] << std::endl; 378 | } 379 | return true; 380 | } 381 | 382 | void ObstacleTrackerKF::update_tracking(const std::vector& observed_obstacles) 383 | { 384 | std::cout << "--- update tracking ---" << std::endl; 385 | for(auto it=observed_obstacles.begin();it!=observed_obstacles.end();++it){ 386 | int id = candidates[it - observed_obstacles.begin()]; 387 | auto obstacle_it = obstacles.find(id); 388 | if(obstacle_it != obstacles.end()){ 389 | // this obstacle has already been tracked 390 | obstacle_it->second.update(*it); 391 | }else{ 392 | // new obstacle 393 | Obstacle obstacle(*it); 394 | int new_id = get_new_id(); 395 | obstacles[new_id] = obstacle; 396 | std::cout << "new obstacle (id:" << new_id << ") was added" << std::endl; 397 | } 398 | } 399 | } 400 | -------------------------------------------------------------------------------- /src/odom_to_tf.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | std::string ROBOT_FRAME; 8 | const double INTERVAL = 0.01; 9 | 10 | class OdomToTF 11 | { 12 | public: 13 | OdomToTF(void); 14 | void odom_callback(const nav_msgs::OdometryConstPtr&); 15 | void process(void); 16 | 17 | private: 18 | ros::NodeHandle nh; 19 | ros::Subscriber odom_sub; 20 | tf::TransformBroadcaster odom_broadcaster; 21 | nav_msgs::Odometry odometry; 22 | geometry_msgs::TransformStamped odom_tf; 23 | bool odom_updated; 24 | }; 25 | 26 | int main(int argc, char** argv) 27 | { 28 | ros::init(argc, argv, "odom_to_tf"); 29 | ros::NodeHandle nh; 30 | 31 | OdomToTF odom_to_tf; 32 | odom_to_tf.process(); 33 | } 34 | 35 | OdomToTF::OdomToTF(void) 36 | { 37 | odom_sub = nh.subscribe("/odom", 1, &OdomToTF::odom_callback, this); 38 | odom_updated = false; 39 | } 40 | 41 | void OdomToTF::process(void) 42 | { 43 | ros::Rate loop_rate(1.0 / INTERVAL); 44 | 45 | std::cout << "odom_to_tf" << std::endl; 46 | 47 | while(ros::ok()){ 48 | if(odom_updated){ 49 | odom_updated = false; 50 | odom_tf.header = odometry.header; 51 | odom_tf.child_frame_id = odometry.child_frame_id; 52 | odom_tf.transform.translation.x = odometry.pose.pose.position.x; 53 | odom_tf.transform.translation.y = odometry.pose.pose.position.y; 54 | odom_tf.transform.rotation = odometry.pose.pose.orientation; 55 | odom_tf.header = odometry.header; 56 | } 57 | odom_broadcaster.sendTransform(odom_tf); 58 | 59 | ros::spinOnce(); 60 | loop_rate.sleep(); 61 | 62 | } 63 | } 64 | 65 | void OdomToTF::odom_callback(const nav_msgs::OdometryConstPtr& msg) 66 | { 67 | odometry = *msg; 68 | odom_updated = true; 69 | } 70 | 71 | -------------------------------------------------------------------------------- /src/path_to_local_goal.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | double GOAL_DISTANCE; 8 | 9 | nav_msgs::Path path; 10 | 11 | void path_callback(const nav_msgs::PathConstPtr& msg) 12 | { 13 | path = *msg; 14 | } 15 | 16 | double calcurate_distance(geometry_msgs::Pose, geometry_msgs::Pose); 17 | 18 | int main(int argc, char** argv) 19 | { 20 | ros::init(argc, argv, "path_to_local_goal"); 21 | ros::NodeHandle nh; 22 | ros::NodeHandle local_nh("~"); 23 | 24 | local_nh.getParam("GOAL_DISTANCE", GOAL_DISTANCE); 25 | 26 | ros::Publisher goal_pub = nh.advertise("/goal", 100); 27 | ros::Subscriber path_sub = nh.subscribe("/path", 100, path_callback); 28 | 29 | tf::TransformListener listener; 30 | tf::StampedTransform _transform; 31 | geometry_msgs::TransformStamped transform; 32 | geometry_msgs::PoseStamped odometry; 33 | 34 | ros::Rate loop_rate(10); 35 | 36 | while(ros::ok()){ 37 | bool transformed = false; 38 | try{ 39 | listener.lookupTransform("odom", "base_link", ros::Time(0), _transform); 40 | tf::transformStampedTFToMsg(_transform, transform); 41 | odometry.header = transform.header; 42 | odometry.pose.position.x = transform.transform.translation.x; 43 | odometry.pose.position.y = transform.transform.translation.y; 44 | odometry.pose.position.z = transform.transform.translation.z; 45 | odometry.pose.orientation = transform.transform.rotation; 46 | transformed = true; 47 | }catch(tf::TransformException ex){ 48 | ROS_ERROR("%s", ex.what()); 49 | } 50 | 51 | if(!path.poses.empty() && transformed){ 52 | for(int i=path.poses.size()-1;i>=0;i--){ 53 | if(calcurate_distance(path.poses[i].pose, odometry.pose) < GOAL_DISTANCE){ 54 | goal_pub.publish(path.poses[i]); 55 | break; 56 | } 57 | } 58 | } 59 | 60 | loop_rate.sleep(); 61 | ros::spinOnce(); 62 | } 63 | return 0; 64 | } 65 | 66 | double calcurate_distance(geometry_msgs::Pose a, geometry_msgs::Pose b) 67 | { 68 | return sqrt((a.position.x - b.position.x) * (a.position.x - b.position.x) + (a.position.y - b.position.y) * (a.position.y - b.position.y)); 69 | } 70 | -------------------------------------------------------------------------------- /src/robot_predictor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | double PREDICTION_TIME;// [s], 軌道予測時間 9 | const double DT = 0.1;// [s] 10 | int PREDICTION_STEP; 11 | const double HZ = 10; 12 | std::string ROBOT_FRAME; 13 | std::string WORLD_FRAME; 14 | 15 | double ANGULAR_ACCELERATION = 0.0; 16 | double MAX_ANGULAR_VELOCITY = 0.0; 17 | 18 | geometry_msgs::PoseArray predicted_path; 19 | geometry_msgs::Pose current_pose; 20 | geometry_msgs::Pose previous_pose; 21 | 22 | geometry_msgs::Twist current_velocity; 23 | geometry_msgs::Twist previous_velocity; 24 | 25 | tf::StampedTransform _transform; 26 | 27 | int main(int argc, char** argv) 28 | { 29 | ros::init(argc, argv, "robot_predictor"); 30 | ros::NodeHandle nh; 31 | ros::NodeHandle local_nh("~"); 32 | 33 | local_nh.getParam("/dynamic_avoidance/PREDICTION_TIME", PREDICTION_TIME); 34 | local_nh.getParam("/dynamic_avoidance/MAX_ANGULAR_ACCELERATION", ANGULAR_ACCELERATION); 35 | local_nh.getParam("/dynamic_avoidance/MAX_ANGULAR_VELOCITY", MAX_ANGULAR_VELOCITY); 36 | local_nh.getParam("/dynamic_avoidance/ROBOT_FRAME", ROBOT_FRAME); 37 | local_nh.getParam("/dynamic_avoidance/WORLD_FRAME", WORLD_FRAME); 38 | PREDICTION_STEP = PREDICTION_TIME / DT; 39 | 40 | ros::Publisher predicted_path_pub = nh.advertise("/robot_predicted_path", 100); 41 | 42 | tf::TransformListener listener; 43 | 44 | bool first_transform = true; 45 | 46 | predicted_path.header.frame_id = WORLD_FRAME; 47 | 48 | ros::Rate loop_rate(HZ); 49 | 50 | while(ros::ok()){ 51 | bool transformed = false; 52 | try{ 53 | listener.lookupTransform(WORLD_FRAME, ROBOT_FRAME, ros::Time(0), _transform); 54 | geometry_msgs::TransformStamped transform; 55 | tf::transformStampedTFToMsg(_transform, transform); 56 | geometry_msgs::Pose pose; 57 | pose.position.x = transform.transform.translation.x; 58 | pose.position.y = transform.transform.translation.y; 59 | pose.position.z = transform.transform.translation.z; 60 | pose.orientation = transform.transform.rotation; 61 | current_pose = pose; 62 | transformed = true; 63 | }catch(tf::TransformException ex){ 64 | std::cout << ex.what() << std::endl; 65 | } 66 | if(transformed && first_transform){ 67 | first_transform = false; 68 | }else if(transformed && !first_transform){ 69 | std::cout << "===calculate velocity===" << std::endl; 70 | geometry_msgs::Twist velocity; 71 | velocity.linear.x = (current_pose.position.x - previous_pose.position.x) * HZ; 72 | velocity.linear.y = (current_pose.position.y - previous_pose.position.y) * HZ; 73 | velocity.angular.z = (tf::getYaw(current_pose.orientation) - tf::getYaw(previous_pose.orientation)) * HZ; 74 | current_velocity = velocity; 75 | std::cout << current_velocity << std::endl; 76 | std::cout << "===predict path===" << std::endl; 77 | predicted_path.poses.clear(); 78 | // v=const, omega=0 79 | { 80 | double vx = current_velocity.linear.x; 81 | double vy = current_velocity.linear.y; 82 | double v = sqrt(vx*vx + vy*vy); 83 | double omega = current_velocity.angular.z; 84 | //double yaw = tf::getYaw(current_pose.orientation); 85 | double yaw = atan2(vy, vx); 86 | geometry_msgs::Pose pose; 87 | pose.position = current_pose.position; 88 | pose.orientation = tf::createQuaternionMsgFromYaw(yaw); 89 | predicted_path.poses.push_back(pose); 90 | for(int j=0;j= -MAX_ANGULAR_VELOCITY){ 155 | omega -= ANGULAR_ACCELERATION * DT; 156 | } 157 | predicted_path.poses.push_back(pose); 158 | } 159 | } 160 | std::cout << predicted_path.poses.size() << std::endl; 161 | predicted_path.header.stamp = _transform.stamp_; 162 | predicted_path_pub.publish(predicted_path); 163 | previous_velocity = current_velocity; 164 | previous_pose = current_pose; 165 | } 166 | ros::spinOnce(); 167 | loop_rate.sleep(); 168 | } 169 | return 0; 170 | } 171 | 172 | -------------------------------------------------------------------------------- /src/sfm_obstacle_simulator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | int NUM_OBSTACLE; 12 | double NEIGHBER_RANGE = 5.0; 13 | double LAMBDA = 2.0; 14 | double GAMMA = 0.35; 15 | double N_PRIME = 3.0; 16 | double N = 2.0; 17 | double MAX_VELOCITY = 1.5; 18 | double RELAXATION_TIME = 0.5; 19 | double DESIRED_FORCE_FACTOR; 20 | double SOCIAL_FORCE_FACTOR; 21 | double HZ; 22 | double INITIAL_KEEP_OUT_POSITION_X; 23 | double INITIAL_KEEP_OUT_POSITION_Y; 24 | double INITIAL_KEEP_OUT_RANGE; 25 | 26 | 27 | std::string ROBOT_FRAME; 28 | std::string WORLD_FRAME; 29 | std::string OBS_FRAME; 30 | double SIMULATION_SQUARE_LENGTH; 31 | 32 | class SFMObstacle 33 | { 34 | public: 35 | SFMObstacle(void); 36 | 37 | unsigned int id; 38 | Eigen::Vector3d pose; 39 | Eigen::Vector3d velocity; 40 | Eigen::Vector3d current_goal; 41 | double preferred_speed; 42 | bool dodging_right; 43 | Eigen::Vector3d last_desired_force; 44 | Eigen::Vector3d last_social_force; 45 | private: 46 | }; 47 | 48 | SFMObstacle::SFMObstacle(void) 49 | { 50 | id = 0; 51 | pose = Eigen::Vector3d::Zero(); 52 | velocity = Eigen::Vector3d::Zero(); 53 | current_goal = Eigen::Vector3d::Zero(); 54 | preferred_speed = 1.0; 55 | dodging_right = true; 56 | last_desired_force = Eigen::Vector3d::Zero(); 57 | last_social_force = Eigen::Vector3d::Zero(); 58 | } 59 | 60 | std::ostream& operator<<(std::ostream& out, const SFMObstacle& sfmo) 61 | { 62 | out << "id: " << sfmo.id << "\n" 63 | << "pose: " << sfmo.pose.transpose() << "\n" 64 | << "velocity: " << sfmo.velocity.transpose() << "\n" 65 | << "current speed: " << sfmo.velocity.norm() << "\n" 66 | << "current goal: " << sfmo.current_goal.transpose() << "\n" 67 | << "last desired force: " << sfmo.last_desired_force.transpose() << "\n" 68 | << "last social force: " << sfmo.last_social_force.transpose(); 69 | return out; 70 | } 71 | 72 | Eigen::Vector3d get_next_goal(const SFMObstacle& agent) 73 | { 74 | Eigen::Vector3d new_goal = Eigen::Vector3d::Zero(); 75 | while(1){ 76 | new_goal = Eigen::Vector3d::Random() * SIMULATION_SQUARE_LENGTH * 0.5; 77 | double distance_from_agent = (new_goal - agent.pose).norm(); 78 | if(distance_from_agent > SIMULATION_SQUARE_LENGTH * 0.5){ 79 | return new_goal; 80 | } 81 | } 82 | } 83 | 84 | Eigen::Vector3d get_social_force(const SFMObstacle& agent, const std::vector& obstacles) 85 | { 86 | Eigen::Vector3d force = Eigen::Vector3d::Zero(); 87 | 88 | for(auto obstacle : obstacles){ 89 | if(agent.id == obstacle.id){ 90 | continue; 91 | } 92 | 93 | Eigen::Vector3d diff_vector = obstacle.pose - agent.pose; 94 | double distance = diff_vector.norm(); 95 | if (distance > NEIGHBER_RANGE){ 96 | continue; 97 | } 98 | // e_{ij} 99 | Eigen::Vector3d diff_direction = diff_vector.normalized(); 100 | 101 | double other_angle = atan2(diff_direction(1), diff_direction(0)); 102 | double agent_angle = atan2(agent.velocity(1), agent.velocity(0)); 103 | double angle_fov = other_angle - agent_angle; 104 | angle_fov = atan2(sin(angle_fov), cos(angle_fov)); 105 | if(fabs(angle_fov) > M_PI * 5.0 / 6.0){ 106 | continue; 107 | } 108 | 109 | Eigen::Vector3d velocity_diff = agent.velocity - obstacle.velocity; 110 | 111 | // D_{ij} 112 | Eigen::Vector3d interaction_vector = LAMBDA * velocity_diff + diff_direction; 113 | // t_{ij} 114 | Eigen::Vector3d interaction_direction = interaction_vector.normalized(); 115 | 116 | double interaction_angle = atan2(interaction_direction(1), interaction_direction(0)); 117 | double theta_angle = other_angle - interaction_angle; 118 | theta_angle = atan2(sin(theta_angle), cos(theta_angle)); 119 | 120 | double sign_of_theta = (fabs(theta_angle) == 0.00) ? 0.0 : theta_angle / fabs(theta_angle); 121 | 122 | double b = GAMMA * interaction_vector.norm(); 123 | 124 | double force_velocity_amount = -std::exp(-diff_vector.norm() / b - (N_PRIME * b * theta_angle) * (N_PRIME * b * theta_angle)); 125 | double force_angle_amount = -sign_of_theta * std::exp(-diff_vector.norm() / b - (N * b * theta_angle) * (N * b * theta_angle)); 126 | 127 | Eigen::Vector3d force_velocity = force_velocity_amount * interaction_direction; 128 | 129 | Eigen::Vector3d interaction_direction_normal; 130 | if(agent.dodging_right){ 131 | interaction_direction_normal << -interaction_direction(1), interaction_direction(0), interaction_direction(2); 132 | }else{ 133 | interaction_direction_normal << interaction_direction(1), -interaction_direction(0), interaction_direction(2); 134 | } 135 | 136 | Eigen::Vector3d force_angle = force_angle_amount * interaction_direction_normal; 137 | 138 | force += force_velocity + force_angle; 139 | } 140 | return force; 141 | } 142 | 143 | Eigen::Vector3d get_desired_force(const SFMObstacle& agent) 144 | { 145 | Eigen::Vector3d force = Eigen::Vector3d::Zero(); 146 | // force = (agent.current_goal - agent.pose).normalized(); 147 | Eigen::Vector3d desired_vector = (agent.current_goal - agent.pose).normalized(); 148 | force = (agent.preferred_speed * desired_vector - agent.velocity) / RELAXATION_TIME; 149 | return force; 150 | } 151 | 152 | void set_obs_list(const std::vector& obstacles, std::vector& obs_list) 153 | { 154 | obs_list.clear(); 155 | for(auto& obs : obstacles){ 156 | geometry_msgs::TransformStamped tfs; 157 | tfs.header.stamp = ros::Time::now(); 158 | tfs.header.frame_id = WORLD_FRAME; 159 | tfs.child_frame_id = OBS_FRAME + std::to_string(obs.id); 160 | tfs.transform.translation.x = obs.pose(0); 161 | tfs.transform.translation.y = obs.pose(1); 162 | tfs.transform.rotation = tf::createQuaternionMsgFromYaw(atan2(obs.velocity(1), obs.velocity(0))); 163 | obs_list.push_back(tfs); 164 | } 165 | } 166 | 167 | void simulate_one_step(std::vector& obstacles, double dt) 168 | { 169 | if(dt < 0.0){ 170 | return; 171 | } 172 | // std::cout << "simulate one step" << std::endl; 173 | for(auto& obs : obstacles){ 174 | // std::cout << obs << std::endl; 175 | Eigen::Vector3d relative_goal = obs.current_goal - obs.pose; 176 | if(relative_goal.norm() < 0.5){ 177 | obs.current_goal = Eigen::Vector3d::Random() * SIMULATION_SQUARE_LENGTH * 0.5; 178 | obs.current_goal = get_next_goal(obs); 179 | obs.current_goal(2) = 0; 180 | } 181 | Eigen::Vector3d desired_force = get_desired_force(obs); 182 | obs.last_desired_force = desired_force; 183 | // std::cout << obs.last_desired_force.transpose() << std::endl; 184 | Eigen::Vector3d social_force = get_social_force(obs, obstacles); 185 | obs.last_social_force = social_force; 186 | // std::cout << obs.last_social_force.transpose() << std::endl; 187 | Eigen::Vector3d force = DESIRED_FORCE_FACTOR * desired_force + SOCIAL_FORCE_FACTOR * social_force; 188 | obs.velocity += obs.velocity + force * dt; 189 | // std::cout << obs.velocity.transpose() << std::endl; 190 | double speed = obs.velocity.norm(); 191 | if(speed > obs.preferred_speed){ 192 | obs.velocity = obs.velocity.normalized() * obs.preferred_speed; 193 | } 194 | obs.pose += obs.velocity * dt; 195 | // std::cout << obs.pose.transpose() << std::endl; 196 | } 197 | } 198 | 199 | int main(int argc, char** argv) 200 | { 201 | ros::init(argc, argv, "sfm_obstacle_simulator"); 202 | std::cout << "=== sfm_obstacle_simulator ===" << std::endl; 203 | ros::NodeHandle nh; 204 | 205 | ros::NodeHandle local_nh("~"); 206 | 207 | local_nh.param("/dynamic_avoidance/ROBOT_FRAME", ROBOT_FRAME, {"base_link"}); 208 | local_nh.param("/dynamic_avoidance/WORLD_FRAME", WORLD_FRAME, {"map"}); 209 | local_nh.param("/dynamic_avoidance/OBSTACLES_FRAME", OBS_FRAME, {"obs"}); 210 | local_nh.param("HZ", HZ, {20.0}); 211 | local_nh.param("NUM_OBSTACLE", NUM_OBSTACLE, {20}); 212 | local_nh.param("DESIRED_FORCE_FACTOR", DESIRED_FORCE_FACTOR, {20.0}); 213 | local_nh.param("SOCIAL_FORCE_FACTOR", SOCIAL_FORCE_FACTOR, {100.0}); 214 | local_nh.param("SIMULATION_SQUARE_LENGTH", SIMULATION_SQUARE_LENGTH, {20.0}); 215 | local_nh.param("INITIAL_KEEP_OUT_POSITION_X", INITIAL_KEEP_OUT_POSITION_X, {-10.0}); 216 | local_nh.param("INITIAL_KEEP_OUT_POSITION_Y", INITIAL_KEEP_OUT_POSITION_Y, {0.0}); 217 | local_nh.param("INITIAL_KEEP_OUT_RANGE", INITIAL_KEEP_OUT_RANGE, {3.0}); 218 | int SEED; 219 | local_nh.param("SEED", SEED, {-1}); 220 | 221 | std::cout << "HZ: " << HZ << std::endl; 222 | std::cout << "NUM_OBSTACLE: " << NUM_OBSTACLE << std::endl; 223 | std::cout << "DESIRED_FORCE_FACTOR: " << DESIRED_FORCE_FACTOR << std::endl; 224 | std::cout << "SOCIAL_FORCE_FACTOR: " << SOCIAL_FORCE_FACTOR << std::endl; 225 | std::cout << "SIMULATION_SQUARE_LENGTH: " << SIMULATION_SQUARE_LENGTH << std::endl; 226 | std::cout << "INITIAL_KEEP_OUT_POSITION_X: " << INITIAL_KEEP_OUT_POSITION_X << std::endl; 227 | std::cout << "INITIAL_KEEP_OUT_POSITION_Y: " << INITIAL_KEEP_OUT_POSITION_Y << std::endl; 228 | std::cout << "INITIAL_KEEP_OUT_RANGE: " << INITIAL_KEEP_OUT_RANGE << std::endl; 229 | std::cout << "SEED: " << SEED << std::endl; 230 | 231 | ros::Rate loop_rate(HZ); 232 | 233 | tf::TransformBroadcaster obs_broadcaster; 234 | 235 | // srand((unsigned int)time(0));// for Eigen 236 | // srand((unsigned int)SEED);// for Eigen 237 | srand(SEED < 0 ? (unsigned int)time(0) : (unsigned int)SEED);// for Eigen 238 | std::random_device rnd; 239 | std::mt19937 mt(SEED < 0 ? rnd() : SEED); 240 | std::uniform_real_distribution<> dist(1.0, MAX_VELOCITY); 241 | std::uniform_int_distribution<> dist_bool(0, 1); 242 | 243 | std::vector obstacles; 244 | obstacles.clear(); 245 | // initialize 246 | Eigen::Vector3d init_pos(INITIAL_KEEP_OUT_POSITION_X, INITIAL_KEEP_OUT_POSITION_Y, 0.0); 247 | for(int i=0;i INITIAL_KEEP_OUT_RANGE){ 254 | break; 255 | } 256 | } 257 | o.velocity = Eigen::Vector3d::Random(); 258 | o.velocity(2) = 0; 259 | o.current_goal = Eigen::Vector3d::Random() * SIMULATION_SQUARE_LENGTH * 0.5; 260 | o.current_goal(2) = 0; 261 | o.preferred_speed = dist(mt); 262 | o.dodging_right = dist(mt); 263 | obstacles.push_back(o); 264 | std::cout << o << std::endl; 265 | } 266 | 267 | tf::TransformListener listener; 268 | 269 | while(ros::ok()){ 270 | bool robot_added_flag = false; 271 | try{ 272 | static bool first_tf_flag = true; 273 | static double last_tf_time = 0; 274 | static Eigen::Vector3d last_robot_pose = Eigen::Vector3d::Zero(); 275 | Eigen::Vector3d robot_velocity = Eigen::Vector3d::Zero(); 276 | Eigen::Vector3d robot_pose = Eigen::Vector3d::Zero(); 277 | 278 | geometry_msgs::PoseStamped robot_p; 279 | robot_p.header.frame_id = ROBOT_FRAME; 280 | robot_p.header.stamp = ros::Time(0); 281 | robot_p.pose.orientation = tf::createQuaternionMsgFromYaw(0); 282 | listener.transformPose(WORLD_FRAME, robot_p, robot_p); 283 | robot_pose(0) = robot_p.pose.position.x; 284 | robot_pose(1) = robot_p.pose.position.y; 285 | if(!first_tf_flag){ 286 | double dt = robot_p.header.stamp.toSec() - last_tf_time; 287 | if(dt > 1e-4){ 288 | robot_velocity = (robot_pose - last_robot_pose) / dt; 289 | }else{ 290 | robot_velocity = Eigen::Vector3d::Zero(); 291 | } 292 | }else{ 293 | first_tf_flag = false; 294 | } 295 | SFMObstacle robot_o; 296 | robot_o.id = obstacles.size(); 297 | robot_o.pose = robot_pose; 298 | robot_o.velocity = robot_velocity; 299 | robot_o.current_goal = robot_o.pose + robot_o.velocity * 5.0;// 5[s] after 300 | obstacles.push_back(robot_o); 301 | // std::cout << "robot is added" << std::endl; 302 | // std::cout << robot_o << std::endl; 303 | // std::cout << robot_p.header.stamp.toSec() - last_tf_time << std::endl; 304 | // std::cout << last_robot_pose.transpose() << std::endl; 305 | 306 | robot_added_flag = true; 307 | last_robot_pose = robot_pose; 308 | last_tf_time = robot_p.header.stamp.toSec(); 309 | }catch(tf::TransformException& ex){ 310 | std::cout << ex.what() << std::endl; 311 | } 312 | 313 | simulate_one_step(obstacles, 1 / HZ); 314 | 315 | if(robot_added_flag){ 316 | obstacles.pop_back(); 317 | } 318 | 319 | std::vector obs_list; 320 | obs_list.clear(); 321 | set_obs_list(obstacles, obs_list); 322 | 323 | std::cout << "===" << std::endl; 324 | for(const auto obs : obstacles){ 325 | std::cout << obs << std::endl; 326 | } 327 | 328 | obs_broadcaster.sendTransform(obs_list); 329 | ros::spinOnce(); 330 | loop_rate.sleep(); 331 | } 332 | return 0; 333 | }; 334 | -------------------------------------------------------------------------------- /src/static_local_costmap_generator.cpp: -------------------------------------------------------------------------------- 1 | #include "dynamic_obstacle_avoidance_planner/static_local_costmap_generator.h" 2 | 3 | StaticLocalCostmapGenerator::StaticLocalCostmapGenerator(void) 4 | :local_nh("~") 5 | { 6 | local_nh.getParam("/dynamic_avoidance/ROBOT_FRAME", ROBOT_FRAME); 7 | local_nh.getParam("/dynamic_avoidance/WORLD_FRAME", WORLD_FRAME); 8 | local_nh.getParam("/dynamic_avoidance/RADIUS", RADIUS); 9 | local_nh.param("RADIUS", RADIUS, {0.6}); 10 | local_nh.param("HZ", HZ, {10}); 11 | local_nh.param("MAP_WIDTH", MAP_WIDTH, {20.0}); 12 | local_nh.param("RESOLUTION", RESOLUTION, {0.1}); 13 | 14 | map_pub = nh.advertise("/local_costmap", 1); 15 | cloud_sub = nh.subscribe("/cluster/human/removed", 1, &StaticLocalCostmapGenerator::cloud_callback, this); 16 | cloud_ptr = pcl::PointCloud::Ptr(new pcl::PointCloud); 17 | cloud_updated = false; 18 | 19 | std::cout << "=== local_costmap ===" << std::endl; 20 | std::cout << "RADIUS: " << RADIUS << std::endl; 21 | std::cout << "ROBOT_FRAME: " << ROBOT_FRAME << std::endl; 22 | std::cout << "WORLD_FRAME: " << WORLD_FRAME << std::endl; 23 | std::cout << "HZ: " << HZ << std::endl; 24 | std::cout << "MAP_WIDTH: " << MAP_WIDTH << std::endl; 25 | std::cout << "RESOLUTION: " << RESOLUTION << std::endl; 26 | } 27 | 28 | void StaticLocalCostmapGenerator::process(void) 29 | { 30 | ros::Rate loop_rate(HZ); 31 | 32 | local_costmap.header.frame_id = ROBOT_FRAME; 33 | local_costmap.info.resolution = RESOLUTION; 34 | local_costmap.info.width = MAP_WIDTH / RESOLUTION; 35 | local_costmap.info.height = MAP_WIDTH / RESOLUTION; 36 | local_costmap.info.origin.position.x = -MAP_WIDTH / 2.0; 37 | local_costmap.info.origin.position.y = -MAP_WIDTH / 2.0; 38 | local_costmap.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0); 39 | local_costmap.data.resize(local_costmap.info.width * local_costmap.info.height); 40 | RADIUS_GRID = RADIUS / local_costmap.info.resolution; 41 | for(unsigned int i=0;ipoints.size(); 59 | std::cout << "cloud size: " << cloud_size << std::endl; 60 | std::cout << "MAP_SIZE" << MAP_SIZE << std::endl; 61 | double map_max_limit_x = local_costmap.info.resolution * local_costmap.info.height * 0.5; 62 | double map_min_limit_x = -local_costmap.info.resolution * local_costmap.info.height * 0.5; 63 | double map_max_limit_y = local_costmap.info.resolution * local_costmap.info.width * 0.5; 64 | double map_min_limit_y = -local_costmap.info.resolution * local_costmap.info.width * 0.5; 65 | for(int i=0;ipoints[i].x < map_max_limit_x && cloud_ptr->points[i].x > map_min_limit_x && cloud_ptr->points[i].y < map_max_limit_y && cloud_ptr->points[i].y > map_min_limit_y){ 67 | int index = get_index(cloud_ptr->points[i].x, cloud_ptr->points[i].y); 68 | if(index < MAP_SIZE){ 69 | local_costmap.data[index] = 100; 70 | } 71 | } 72 | } 73 | nav_msgs::OccupancyGrid expanded_costmap = local_costmap; 74 | for(int i=0;i 0){ 76 | int _j = i % local_costmap.info.height; 77 | int _k = i / local_costmap.info.height; 78 | for(unsigned int j=0;j("/dynamic_obstacles", 1); 12 | 13 | std::cout << "=== tf_to_obstacles ===" << std::endl; 14 | std::cout << "WORLD_FRAME: " << WORLD_FRAME << std::endl; 15 | std::cout << "OBS_PREFIX: " << OBS_PREFIX << std::endl; 16 | std::cout << "HZ: " << HZ << std::endl; 17 | std::cout << "NOISE_ENABLED: " << NOISE_ENABLED << std::endl; 18 | } 19 | 20 | void TFToObstacles::process(void) 21 | { 22 | ros::Rate loop_rate(HZ); 23 | 24 | while(ros::ok()){ 25 | std::cout << "=== tf_to_obstacles ===" << std::endl; 26 | tf::FrameGraph::Request req; 27 | tf::FrameGraph::Response res; 28 | if(listener.getFrames(req, res)){ 29 | std::vector obs_names; 30 | while(1){ 31 | // std::cout << res.dot_graph << std::endl; 32 | int position = res.dot_graph.find("-> \"" + OBS_PREFIX); 33 | if(position != std::string::npos){ 34 | position = res.dot_graph.find(OBS_PREFIX); 35 | res.dot_graph.erase(0, position); 36 | int double_quotation_pos = res.dot_graph.find("\""); 37 | std::string obs_name = res.dot_graph.substr(0, double_quotation_pos); 38 | res.dot_graph.erase(0, double_quotation_pos); 39 | std::cout << obs_name << std::endl; 40 | obs_names.emplace_back(obs_name); 41 | }else{ 42 | std::cout << OBS_PREFIX << " was not found" << std::endl; 43 | break; 44 | } 45 | } 46 | geometry_msgs::PoseArray obstacles; 47 | obstacles.header.frame_id = WORLD_FRAME; 48 | obstacles.header.stamp = ros::Time::now(); 49 | for(const auto& obs_name : obs_names){ 50 | try{ 51 | tf::StampedTransform transform; 52 | listener.lookupTransform(WORLD_FRAME, obs_name, ros::Time(0), transform); 53 | obstacles.header.stamp = transform.stamp_; 54 | geometry_msgs::Pose pose; 55 | pose.position.x = transform.getOrigin().x(); 56 | pose.position.y = transform.getOrigin().y(); 57 | pose.position.z = transform.getOrigin().z(); 58 | if(NOISE_ENABLED){ 59 | pose.position.x += dist(engine); 60 | pose.position.y += dist(engine); 61 | pose.position.z += dist(engine); 62 | } 63 | tf::quaternionTFToMsg(transform.getRotation(), pose.orientation); 64 | obstacles.poses.push_back(pose); 65 | }catch(tf::TransformException ex){ 66 | std::cout << ex.what() << std::endl; 67 | } 68 | } 69 | obstacles_pose_pub.publish(obstacles); 70 | }else{ 71 | std::cout << "cannot get frames" << std::endl; 72 | } 73 | loop_rate.sleep(); 74 | ros::spinOnce(); 75 | } 76 | } 77 | 78 | int main(int argc, char** argv) 79 | { 80 | ros::init(argc, argv, "tf_to_obstacles"); 81 | TFToObstacles tf_to_obstacles; 82 | tf_to_obstacles.process(); 83 | return 0; 84 | } 85 | -------------------------------------------------------------------------------- /src/trajectory_logger.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | double HZ = 100.0; 11 | 12 | visualization_msgs::Marker lines; 13 | 14 | std::string ROBOT_FRAME; 15 | std::string OBS_PREFIX; 16 | std::string WORLD_FRAME; 17 | double PREDICTION_TIME; 18 | int PREDICTION_STEP; 19 | const double TRAJ_WIDTH = 0.03; 20 | 21 | typedef std::map NamedTrajectory; 22 | 23 | void path_callback(const geometry_msgs::PoseArrayConstPtr& msg) 24 | { 25 | lines.points.clear(); 26 | geometry_msgs::PoseArray pose_array; 27 | pose_array = *msg; 28 | lines.header = pose_array.header; 29 | lines.color.r = 0; 30 | lines.color.g = 0; 31 | lines.color.b = 1; 32 | lines.color.a = 1; 33 | for(int i=0;i("/marker/robot", 1); 87 | ros::Publisher obs_viz_pub = nh.advertise("/marker/obs", 1); 88 | ros::Publisher robot_trajectory_viz_pub = nh.advertise("/robot_passed_trajectory", 1); 89 | ros::Publisher obs_trajectory_viz_pub = nh.advertise("/obs_passed_trajectory", 1); 90 | 91 | ros::Subscriber robot_path_sub = nh.subscribe("/robot_predicted_path", 1, path_callback); 92 | ros::Publisher robot_line_pub = nh.advertise("/lines", 1); 93 | 94 | visualization_msgs::MarkerArray robot_viz; 95 | visualization_msgs::MarkerArray obs_viz; 96 | visualization_msgs::Marker robot_trajectory_viz; 97 | 98 | lines.type = visualization_msgs::Marker::LINE_LIST; 99 | lines.lifetime = ros::Duration(0.01); 100 | lines.action = visualization_msgs::Marker::ADD; 101 | lines.header.frame_id = WORLD_FRAME; 102 | lines.ns = "lines"; 103 | lines.scale.x = 0.02; 104 | 105 | robot_trajectory_viz.type = visualization_msgs::Marker::LINE_STRIP; 106 | robot_trajectory_viz.lifetime = ros::Duration(); 107 | robot_trajectory_viz.action = visualization_msgs::Marker::ADD; 108 | robot_trajectory_viz.header.frame_id = WORLD_FRAME; 109 | robot_trajectory_viz.ns = "robot_trajectory"; 110 | robot_trajectory_viz.scale.x = TRAJ_WIDTH; 111 | robot_trajectory_viz.color.b = 1.0; 112 | robot_trajectory_viz.color.a = 0.8; 113 | robot_trajectory_viz.pose.orientation.w = 1; 114 | 115 | NamedTrajectory obs_trajectories; 116 | 117 | while(ros::ok()){ 118 | static int count = 0; 119 | 120 | tf::FrameGraph::Request req; 121 | tf::FrameGraph::Response res; 122 | std::vector obs_names; 123 | if(listener.getFrames(req, res)){ 124 | while(1){ 125 | // std::cout << res.dot_graph << std::endl; 126 | int position = res.dot_graph.find("-> \"" + OBS_PREFIX); 127 | if(position != std::string::npos){ 128 | position = res.dot_graph.find(OBS_PREFIX); 129 | res.dot_graph.erase(0, position); 130 | int double_quotation_pos = res.dot_graph.find("\""); 131 | std::string obs_name = res.dot_graph.substr(0, double_quotation_pos); 132 | res.dot_graph.erase(0, double_quotation_pos); 133 | std::cout << obs_name << std::endl; 134 | obs_names.emplace_back(obs_name); 135 | if(obs_trajectories.find(obs_name) == obs_trajectories.end()){ 136 | // obstacle with new name 137 | visualization_msgs::Marker obs_traj_marker; 138 | obs_traj_marker.type = visualization_msgs::Marker::LINE_STRIP; 139 | obs_traj_marker.lifetime = ros::Duration(); 140 | obs_traj_marker.action = visualization_msgs::Marker::ADD; 141 | obs_traj_marker.header.frame_id = WORLD_FRAME; 142 | obs_traj_marker.scale.x = TRAJ_WIDTH; 143 | obs_traj_marker.color.r = 1.0; 144 | obs_traj_marker.color.a = 0.8; 145 | obs_traj_marker.pose.orientation.w = 1; 146 | obs_traj_marker.ns = obs_name; 147 | obs_trajectories[obs_name] = obs_traj_marker; 148 | } 149 | }else{ 150 | std::cout << OBS_PREFIX << " was not found" << std::endl; 151 | break; 152 | } 153 | } 154 | } 155 | try{ 156 | tf::StampedTransform robot_transform; 157 | listener.lookupTransform(WORLD_FRAME, ROBOT_FRAME, ros::Time(0), robot_transform); 158 | geometry_msgs::PoseStamped pose; 159 | tf::poseStampedTFToMsg(tf::Stamped(robot_transform, robot_transform.stamp_, robot_transform.frame_id_), pose); 160 | visualization_msgs::Marker viz; 161 | viz = get_marker(visualization_msgs::Marker::CUBE, 0.6, 0.6, 0.2, 0, 1, 0, pose.pose, WORLD_FRAME, "robot"); 162 | viz.header.stamp = ros::Time::now(); 163 | viz.lifetime = ros::Duration(); 164 | static bool first_flag = true; 165 | robot_viz_pub.publish(viz); 166 | if(!first_flag){ 167 | robot_viz_pub.publish(viz); 168 | } 169 | first_flag = false; 170 | robot_line_pub.publish(lines); 171 | robot_trajectory_viz.points.push_back(pose.pose.position); 172 | robot_trajectory_viz_pub.publish(robot_trajectory_viz); 173 | }catch(tf::TransformException ex){ 174 | std::cout << ex.what() << std::endl; 175 | } 176 | visualization_msgs::MarkerArray obs_markers; 177 | for(const auto& obs_name : obs_names){ 178 | try{ 179 | tf::StampedTransform obs_transform; 180 | listener.lookupTransform(WORLD_FRAME, obs_name, ros::Time(0), obs_transform); 181 | geometry_msgs::PoseStamped pose; 182 | tf::poseStampedTFToMsg(tf::Stamped(obs_transform, obs_transform.stamp_, obs_transform.frame_id_), pose); 183 | visualization_msgs::Marker viz; 184 | viz = get_marker(visualization_msgs::Marker::CUBE, 0.6, 0.6, 0.2, 1, 0, 0, pose.pose, WORLD_FRAME, obs_name); 185 | viz.header.stamp = ros::Time::now(); 186 | viz.lifetime = ros::Duration(); 187 | std::cout << viz << std::endl; 188 | obs_markers.markers.push_back(viz); 189 | obs_trajectories[obs_name].points.push_back(pose.pose.position); 190 | int size = obs_trajectories[obs_name].points.size(); 191 | const int TRAJ_LIMIT = 300; 192 | if(size > TRAJ_LIMIT){ 193 | // erase old trajectory 194 | obs_trajectories[obs_name].points.erase(obs_trajectories[obs_name].points.begin(), obs_trajectories[obs_name].points.begin() + (size - TRAJ_LIMIT)); 195 | } 196 | }catch(tf::TransformException ex){ 197 | std::cout << ex.what() << std::endl; 198 | } 199 | } 200 | obs_viz_pub.publish(obs_markers); 201 | obs_trajectory_viz_pub.publish(get_obs_trajectory_markers(obs_trajectories)); 202 | count++; 203 | count %= 10; 204 | 205 | ros::spinOnce(); 206 | loop_rate.sleep(); 207 | } 208 | return 0; 209 | } 210 | -------------------------------------------------------------------------------- /src/velocity_arrow_to_obstacle_converter.cpp: -------------------------------------------------------------------------------- 1 | #include "dynamic_obstacle_avoidance_planner/velocity_arrow_to_obstacle_converter.h" 2 | 3 | VelocityArrowToObstacleConverter::VelocityArrowToObstacleConverter(void) 4 | :local_nh("~") 5 | { 6 | local_nh.param("/dynamic_avoidance/PREDICTION_TIME", PREDICTION_TIME, {3.5}); 7 | local_nh.param("/dynamic_avoidance/WORLD_FRAME", WORLD_FRAME, {"map"}); 8 | local_nh.param("/dynamic_avoidance/OBSTACLES_FRAME", OBS_FRAME, {"obs"}); 9 | local_nh.param("HZ", HZ, {10}); 10 | INTERVAL = 1.0 / HZ; 11 | PREDICTION_STEP = PREDICTION_TIME / INTERVAL; 12 | 13 | obs_num_pub = nh.advertise("/obs_num", 1); 14 | predicted_paths_pub = nh.advertise("/predicted_paths", 1); 15 | velocity_arrow_sub = nh.subscribe("/velocity_arrows", 1, &VelocityArrowToObstacleConverter::velocity_arrow_callback, this); 16 | velocity_arrow_updated = false; 17 | obs_poses.header.frame_id = WORLD_FRAME; 18 | obs_num.data = 0; 19 | predicted_paths.header.frame_id = WORLD_FRAME; 20 | 21 | std::cout << "=== velocity arrow to obstacle converter ===" << std::endl; 22 | std::cout << "PREDICTION_TIME: " << PREDICTION_TIME << std::endl; 23 | std::cout << "PREDICTION_STEP: " << PREDICTION_STEP << std::endl; 24 | std::cout << "WORLD_FRAME: " << WORLD_FRAME << std::endl; 25 | std::cout << "OBS_FRAME: " << OBS_FRAME << std::endl; 26 | 27 | } 28 | 29 | void VelocityArrowToObstacleConverter::process(void) 30 | { 31 | ros::Rate loop_rate(HZ); 32 | 33 | while(ros::ok()){ 34 | if(velocity_arrow_updated){ 35 | std::cout << "=== velocity arrow to obstacle converter ===" << std::endl; 36 | predicted_paths.poses.clear(); 37 | //std::cout << "obs_num : " << obs_num.data << std::endl; 38 | for(int i=0;i 2 | #include 3 | #include 4 | 5 | std::string WORLD_FRAME; 6 | int main(int argc, char** argv) 7 | { 8 | ros::init(argc, argv, "waypoints_test"); 9 | ros::NodeHandle nh; 10 | 11 | ros::NodeHandle local_nh("~"); 12 | local_nh.getParam("/dynamic_avoidance/WORLD_FRAME", WORLD_FRAME); 13 | 14 | ros::Publisher waypoints_pub = nh.advertise("/waypoints", 100); 15 | 16 | std::cout << "=== waypoints_test ===" << std::endl; 17 | geometry_msgs::PoseArray wp; 18 | wp.header.frame_id = WORLD_FRAME; 19 | wp.poses.resize(2); 20 | wp.poses[0].orientation = wp.poses[1].orientation = tf::createQuaternionMsgFromYaw(0); 21 | wp.poses[0].position.x = 0; 22 | wp.poses[0].position.y = 0; 23 | wp.poses[1].position.x = 10; 24 | wp.poses[1].position.y = 0; 25 | 26 | ros::Rate loop_rate(10); 27 | 28 | while(ros::ok()){ 29 | waypoints_pub.publish(wp); 30 | ros::spinOnce(); 31 | loop_rate.sleep(); 32 | } 33 | return 0; 34 | } 35 | --------------------------------------------------------------------------------