├── .gitignore ├── README.md ├── collvoid ├── CMakeLists.txt └── package.xml ├── collvoid_amcl ├── CMakeLists.txt ├── cfg │ └── AMCL.cfg ├── examples │ ├── amcl_diff.launch │ └── amcl_omni.launch ├── include │ └── amcl │ │ ├── map │ │ └── map.h │ │ ├── pf │ │ ├── eig3.h │ │ ├── pf.h │ │ ├── pf_kdtree.h │ │ ├── pf_pdf.h │ │ └── pf_vector.h │ │ └── sensors │ │ ├── amcl_laser.h │ │ ├── amcl_odom.h │ │ └── amcl_sensor.h ├── package.xml └── src │ ├── amcl │ ├── map │ │ ├── map.c │ │ ├── map_cspace.cpp │ │ ├── map_draw.c │ │ ├── map_range.c │ │ └── map_store.c │ ├── pf │ │ ├── eig3.c │ │ ├── pf.c │ │ ├── pf_draw.c │ │ ├── pf_kdtree.c │ │ ├── pf_pdf.c │ │ └── pf_vector.c │ └── sensors │ │ ├── amcl_laser.cpp │ │ ├── amcl_odom.cpp │ │ └── amcl_sensor.cpp │ └── amcl_node.cpp ├── collvoid_controller ├── CMakeLists.txt ├── goals.yaml ├── package.xml └── src │ └── collvoid_controller │ ├── __init__.py │ ├── active_collision_avoidance.py │ ├── collvoid_visualizer.py │ ├── controller.py │ ├── controller_robots.py │ ├── detect_obstacles.py │ ├── people_tracker.py │ ├── position_share_controller.py │ └── watchdog.py ├── collvoid_dwa_local_planner ├── CHANGELOG.rst ├── CMakeLists.txt ├── blp_plugin.xml ├── cfg │ └── DWAPlanner.cfg ├── include │ └── collvoid_dwa_local_planner │ │ ├── dwa_planner.h │ │ ├── dwa_planner_ros.h │ │ ├── goal_alignment_cost_function.h │ │ └── path_alignment_cost_function.h ├── package.xml └── src │ ├── dwa_planner.cpp │ ├── dwa_planner_ros.cpp │ ├── goal_alignment_cost_function.cpp │ └── path_alignment_cost_function.cpp ├── collvoid_local_planner ├── CMakeLists.txt ├── blp_plugin.xml ├── cfg │ └── Collvoid.cfg ├── include │ └── collvoid_local_planner │ │ ├── Agent.h │ │ ├── ROSAgent.h │ │ ├── Utils.h │ │ ├── Vector2.h │ │ ├── clearpath.h │ │ ├── collvoid_local_planner.h │ │ ├── collvoid_publishers.h │ │ ├── collvoid_scoring_function.h │ │ ├── me_publisher.h │ │ └── orca.h ├── package.xml ├── src │ ├── Agent.cpp │ ├── ROSAgent.cpp │ ├── clearpath.cpp │ ├── collvoid_local_planner.cpp │ ├── collvoid_publishers.cpp │ ├── collvoid_scoring_function.cpp │ ├── helper.cpp │ ├── me_publisher.cpp │ └── orca.cpp └── srv │ └── GetCollvoidTwist.srv ├── collvoid_msgs ├── CMakeLists.txt ├── msg │ ├── AggregatedPoseTwist.msg │ ├── PoseArrayWeighted.msg │ ├── PoseTwistWithCovariance.msg │ └── collvoid_debug.msg └── package.xml ├── collvoid_simple_global_planner ├── CMakeLists.txt ├── bgp_plugin.xml ├── include │ └── collvoid_simple_global_planner │ │ └── collvoid_simple_global_planner.h ├── package.xml └── src │ └── collvoid_simple_global_planner.cpp ├── collvoid_srvs ├── CMakeLists.txt ├── package.xml └── srv │ ├── GetMe.srv │ ├── GetNeighbors.srv │ └── GetObstacles.srv ├── collvoid_stage ├── CMakeLists.txt ├── bags │ ├── .eval_bags │ └── evaluateArrows.py ├── launch │ ├── amcl_diff_multi.launch │ ├── amcl_diff_single.launch │ ├── amcl_omni_multi.launch │ ├── amcl_omni_single.launch │ ├── move_base_collvoid.launch │ ├── move_base_collvoid_legacy.launch │ ├── move_base_dwa.launch │ ├── move_base_dwa_simple.launch │ ├── sim_single_pr2.launch │ ├── sim_single_turtle.launch │ ├── sim_single_turtle_dwa.launch │ └── sim_test.launch ├── multi_view.rviz ├── package.xml ├── params │ ├── collvoid_common.yaml │ ├── collvoid_config.yaml │ ├── costmap_common_turtle.yaml │ ├── me_publisher_turtle.yaml │ ├── move_base_params_turtle.yaml │ └── stage_params.yaml ├── scripts │ ├── create.py │ ├── create_random_goals.py │ ├── exp │ │ ├── evaluate.py │ │ ├── evaluate_real.py │ │ ├── evaluate_spataps.py │ │ ├── run_experiments.py │ │ ├── run_localization_with_radius_and_noerror.py │ │ └── run_only_ground_truth.py │ └── vo_based_avoidance.py ├── setup.py ├── single_view.rviz └── world │ ├── 10x10_map.pgm │ ├── 10x10_map.yaml │ ├── 10x10_template.world │ ├── 5x5_map.pgm │ ├── 5x5_map.yaml │ ├── 5x5_template.world │ ├── maplarge.pgm │ ├── maplarge.yaml │ ├── robot.png │ ├── swarmlab_map.pgm │ ├── swarmlab_map.yaml │ ├── swarmlab_single_pr2.world │ ├── swarmlab_single_turtle.world │ ├── swarmlab_template.world │ └── willow_full.world ├── collvoid_turtlebot ├── CMakeLists.txt ├── goals_tb_lab.yaml ├── launch │ ├── amcl_turtlebot.launch │ ├── controller.launch │ ├── gmapping.launch │ ├── hokuyo.launch │ ├── includes │ │ ├── laser_detectors.launch │ │ ├── lcm_controller.launch │ │ ├── lcm_turtlebot.launch │ │ ├── leg_detectors.launch │ │ ├── move_base_collvoid.launch │ │ ├── move_base_collvoid_legacy.launch │ │ └── move_base_dwa.launch │ ├── minimal.launch │ ├── tracking_on_robot.launch │ ├── turtlebot.launch │ ├── turtlebot_master.launch │ └── unused │ │ └── master_sync.launch ├── map │ ├── map.pgm │ ├── map.yaml │ ├── smartlab_arena.pgm │ └── smartlab_arena.yaml ├── package.xml ├── params │ ├── collvoid_common.yaml │ ├── collvoid_config.yaml │ ├── costmap_common_params.yaml │ ├── costmap_common_turtle.yaml │ ├── me_publisher_turtle.yaml │ ├── move_base_params_turtle.yaml │ ├── multimaster_config.yaml │ └── stage_params.yaml ├── scripts │ ├── lcm_relay_share.py │ ├── lcm_ros_relay.py │ └── make_goals.py └── world │ ├── aamas.pgm │ ├── aamas.yaml │ ├── map.pgm │ ├── map.yaml │ ├── maplarge.pgm │ ├── maplarge.yaml │ ├── robot.png │ ├── smartlab_single_turtle.world │ ├── swarmlab.pgm │ ├── swarmlab.yaml │ ├── swarmlab_rev2012.pgm │ └── swarmlab_rev2012.yaml ├── collvoid_wiggle_recovery ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── collvoid_wiggle_recovery │ │ └── collvoid_wiggle_recovery.h ├── package.xml ├── rotate_plugin.xml └── src │ └── collvoid_wiggle_recovery.cpp └── experimental ├── CATKIN_IGNORE ├── collvoid_pr2 ├── CMakeLists.txt ├── ROS_NOBUILD ├── launch │ ├── pr2.launch │ ├── pr2_master.launch │ └── pr2_single.launch ├── mainpage.dox ├── package.xml ├── params │ ├── config.yaml │ ├── params_created.yaml │ └── params_pr2.yaml ├── pr2_nav.vcg └── world │ ├── maplarge.pgm │ └── maplarge.yaml ├── pacman_controller ├── CMakeLists.txt ├── ROS_NOBUILD ├── launch │ ├── create_map.launch │ └── run_game.launch ├── mainpage.dox ├── map_create.vcg ├── meshes │ ├── blue.jpg │ ├── blue2.jpg │ ├── catchable.jpg │ ├── catchable2.jpg │ ├── dead.dae │ ├── dead.jpg │ ├── dead2.jpg │ ├── ghost_blue.dae │ ├── ghost_catchable.dae │ ├── ghost_pink.dae │ ├── ghost_red.dae │ ├── pacman.dae │ ├── pink.jpg │ ├── pink2.jpg │ ├── red.jpg │ ├── red2.jpg │ └── texture4.jpg ├── package.xml ├── params │ ├── ghost_homes.yaml │ ├── map_willow.yaml │ ├── params_created.yaml │ └── params_turtle.yaml ├── pc_view.vcg ├── src │ └── pacman_controller │ │ ├── __init__.py │ │ ├── controller_ghosts.py │ │ ├── controller_pacman.py │ │ ├── create_map.py │ │ ├── game_controller.py │ │ ├── game_engine.py │ │ └── pacman_controller.py └── world │ ├── maplarge.pgm │ ├── maplarge.yaml │ └── willow_full.world └── pacman_turtles ├── CMakeLists.txt ├── ROS_NOBUILD ├── launch ├── ghost.launch ├── master.launch ├── pacman.launch ├── pacman_master.launch ├── player.launch ├── player_withoutmap.launch └── turtlebot_master.launch ├── mainpage.dox ├── package.xml ├── pacman.vcg ├── params ├── config.yaml └── pacman.yaml └── world ├── maplarge.pgm └── maplarge.yaml /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | bin/ 3 | lib/ 4 | msg_gen/ 5 | srv_gen/ 6 | msg/*Action.msg 7 | msg/*ActionFeedback.msg 8 | msg/*ActionGoal.msg 9 | msg/*ActionResult.msg 10 | msg/*Feedback.msg 11 | msg/*Goal.msg 12 | msg/*Result.msg 13 | msg/_*.py 14 | 15 | # Generated by dynamic reconfigure 16 | *.cfgc 17 | /cfg/cpp/ 18 | /cfg/*.py 19 | 20 | # Ignore generated docs 21 | *.dox 22 | *.wikidoc 23 | 24 | # eclipse stuff 25 | .project 26 | .cproject 27 | 28 | # qcreator stuff 29 | CMakeLists.txt.user 30 | 31 | srv/_*.py 32 | *.pcd 33 | *.pyc 34 | qtcreator-* 35 | *.user 36 | 37 | /planning/cfg 38 | /planning/docs 39 | /planning/src 40 | 41 | *~ 42 | 43 | # Emacs 44 | .#* 45 | 46 | # Catkin custom files 47 | CATKIN_IGNORE 48 | 49 | *_created.yaml 50 | *_created.launch 51 | *_created.world 52 | 53 | # intellij files 54 | .idea -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # collvoid 2 | ROS multi_robot_collision_avoidance 3 | 4 | Visit http://wiki.ros.org/multi_robot_collision_avoidance 5 | 6 | -------------------------------------------------------------------------------- /collvoid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(collvoid) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /collvoid/package.xml: -------------------------------------------------------------------------------- 1 | 2 | collvoid 3 | 1.0.0 4 | Multi Robot Collision Avoidance 5 | Maintained by Daniel Claes 6 | 7 | BSD 8 | proprietary license (RVO2 library) 9 | 10 | http://ros.org/wiki/multi_robot_collision_avoidance 11 | 12 | 13 | Maintained by Daniel Claes 14 | 15 | 16 | catkin 17 | 18 | 19 | 20 | 21 | 22 | collvoid_amcl 23 | collvoid_controller 24 | collvoid_dwa_local_planner 25 | pacman_turtles 26 | pacman_controller 27 | collvoid_pr2 28 | collvoid_stage 29 | collvoid_turtlebot 30 | collvoid_local_planner 31 | collvoid_simple_global_planner 32 | collvoid_msgs 33 | collvoid_wiggle_recovery 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /collvoid_amcl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(collvoid_amcl) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | collvoid_msgs 7 | rosbag 8 | roscpp 9 | tf 10 | dynamic_reconfigure 11 | nav_msgs 12 | ) 13 | 14 | find_package(Boost REQUIRED) 15 | 16 | # dynamic reconfigure 17 | generate_dynamic_reconfigure_options( 18 | cfg/AMCL.cfg 19 | ) 20 | 21 | catkin_package( 22 | CATKIN_DEPENDS 23 | collvoid_msgs 24 | rosbag 25 | roscpp 26 | dynamic_reconfigure 27 | tf 28 | INCLUDE_DIRS include 29 | LIBRARIES amcl_sensors amcl_map amcl_pf 30 | ) 31 | 32 | include_directories(include/amcl include/amcl/map include/amcl/sensors include/amcl/pf) 33 | include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 34 | 35 | add_library(collvoid_amcl_pf 36 | src/amcl/pf/pf.c 37 | src/amcl/pf/pf_kdtree.c 38 | src/amcl/pf/pf_pdf.c 39 | src/amcl/pf/pf_vector.c 40 | src/amcl/pf/eig3.c 41 | src/amcl/pf/pf_draw.c) 42 | 43 | add_library(collvoid_amcl_map 44 | src/amcl/map/map.c 45 | src/amcl/map/map_cspace.cpp 46 | src/amcl/map/map_range.c 47 | src/amcl/map/map_store.c 48 | src/amcl/map/map_draw.c) 49 | 50 | add_library(collvoid_amcl_sensors 51 | src/amcl/sensors/amcl_sensor.cpp 52 | src/amcl/sensors/amcl_odom.cpp 53 | src/amcl/sensors/amcl_laser.cpp) 54 | target_link_libraries(collvoid_amcl_sensors collvoid_amcl_map collvoid_amcl_pf) 55 | 56 | 57 | add_executable(collvoid_amcl 58 | src/amcl_node.cpp) 59 | add_dependencies(collvoid_amcl collvoid_amcl_gencfg) 60 | add_dependencies(collvoid_amcl collvoid_msgs_generate_messages_cpp) 61 | 62 | target_link_libraries(collvoid_amcl 63 | collvoid_amcl_sensors collvoid_amcl_map collvoid_amcl_pf 64 | ${Boost_LIBRARIES} 65 | ${catkin_LIBRARIES} 66 | ) 67 | 68 | install( TARGETS 69 | collvoid_amcl collvoid_amcl_sensors collvoid_amcl_map collvoid_amcl_pf 70 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 71 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 72 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 73 | ) 74 | 75 | install(DIRECTORY include/amcl/ 76 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 77 | ) 78 | 79 | install(DIRECTORY examples/ 80 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples 81 | ) 82 | -------------------------------------------------------------------------------- /collvoid_amcl/examples/amcl_diff.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /collvoid_amcl/examples/amcl_omni.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /collvoid_amcl/include/amcl/pf/eig3.h: -------------------------------------------------------------------------------- 1 | 2 | /* Eigen-decomposition for symmetric 3x3 real matrices. 3 | Public domain, copied from the public domain Java library JAMA. */ 4 | 5 | #ifndef _eig_h 6 | 7 | /* Symmetric matrix A => eigenvectors in columns of V, corresponding 8 | eigenvalues in d. */ 9 | void eigen_decomposition(double A[3][3], double V[3][3], double d[3]); 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /collvoid_amcl/include/amcl/pf/pf_kdtree.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: KD tree functions 23 | * Author: Andrew Howard 24 | * Date: 18 Dec 2002 25 | * CVS: $Id: pf_kdtree.h 6532 2008-06-11 02:45:56Z gbiggs $ 26 | *************************************************************************/ 27 | 28 | #ifndef PF_KDTREE_H 29 | #define PF_KDTREE_H 30 | 31 | #ifdef INCLUDE_RTKGUI 32 | #include "rtk.h" 33 | #endif 34 | 35 | 36 | // Info for a node in the tree 37 | typedef struct pf_kdtree_node 38 | { 39 | // Depth in the tree 40 | int leaf, depth; 41 | 42 | // Pivot dimension and value 43 | int pivot_dim; 44 | double pivot_value; 45 | 46 | // The key for this node 47 | int key[3]; 48 | 49 | // The value for this node 50 | double value; 51 | 52 | // The cluster label (leaf nodes) 53 | int cluster; 54 | 55 | // Child nodes 56 | struct pf_kdtree_node *children[2]; 57 | 58 | } pf_kdtree_node_t; 59 | 60 | 61 | // A kd tree 62 | typedef struct 63 | { 64 | // Cell size 65 | double size[3]; 66 | 67 | // The root node of the tree 68 | pf_kdtree_node_t *root; 69 | 70 | // The number of nodes in the tree 71 | int node_count, node_max_count; 72 | pf_kdtree_node_t *nodes; 73 | 74 | // The number of leaf nodes in the tree 75 | int leaf_count; 76 | 77 | } pf_kdtree_t; 78 | 79 | #ifdef __cplusplus 80 | extern "C" { 81 | #endif 82 | 83 | 84 | // Create a tree 85 | pf_kdtree_t *pf_kdtree_alloc(int max_size); 86 | 87 | // Destroy a tree 88 | void pf_kdtree_free(pf_kdtree_t *self); 89 | 90 | // Clear all entries from the tree 91 | void pf_kdtree_clear(pf_kdtree_t *self); 92 | 93 | // Insert a pose into the tree 94 | void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value); 95 | 96 | // Cluster the leaves in the tree 97 | void pf_kdtree_cluster(pf_kdtree_t *self); 98 | 99 | // Determine the probability estimate for the given pose 100 | double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose); 101 | 102 | // Determine the cluster label for the given pose 103 | int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose); 104 | 105 | #ifdef __cplusplus 106 | } 107 | #endif 108 | 109 | #ifdef INCLUDE_RTKGUI 110 | 111 | // Draw the tree 112 | extern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig); 113 | 114 | #endif 115 | 116 | #endif 117 | -------------------------------------------------------------------------------- /collvoid_amcl/include/amcl/pf/pf_vector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Vector functions 23 | * Author: Andrew Howard 24 | * Date: 10 Dec 2002 25 | * CVS: $Id: pf_vector.h 6345 2008-04-17 01:36:39Z gerkey $ 26 | *************************************************************************/ 27 | 28 | #ifndef PF_VECTOR_H 29 | #define PF_VECTOR_H 30 | 31 | #ifdef __cplusplus 32 | extern "C" { 33 | #endif 34 | 35 | #include 36 | 37 | // The basic vector 38 | typedef struct 39 | { 40 | double v[3]; 41 | } pf_vector_t; 42 | 43 | 44 | // The basic matrix 45 | typedef struct 46 | { 47 | double m[3][3]; 48 | } pf_matrix_t; 49 | 50 | 51 | // Return a zero vector 52 | pf_vector_t pf_vector_zero(); 53 | 54 | // Check for NAN or INF in any component 55 | int pf_vector_finite(pf_vector_t a); 56 | 57 | // Print a vector 58 | void pf_vector_fprintf(pf_vector_t s, FILE *file, const char *fmt); 59 | 60 | // Simple vector addition 61 | pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b); 62 | 63 | // Simple vector subtraction 64 | pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b); 65 | 66 | // Transform from local to global coords (a + b) 67 | pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b); 68 | 69 | // Transform from global to local coords (a - b) 70 | pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b); 71 | 72 | 73 | // Return a zero matrix 74 | pf_matrix_t pf_matrix_zero(); 75 | 76 | // Check for NAN or INF in any component 77 | int pf_matrix_finite(pf_matrix_t a); 78 | 79 | // Print a matrix 80 | void pf_matrix_fprintf(pf_matrix_t s, FILE *file, const char *fmt); 81 | 82 | // Compute the matrix inverse. Will also return the determinant, 83 | // which should be checked for underflow (indicated singular matrix). 84 | //pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det); 85 | 86 | // Decompose a covariance matrix [a] into a rotation matrix [r] and a 87 | // diagonal matrix [d] such that a = r * d * r^T. 88 | void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a); 89 | 90 | #ifdef __cplusplus 91 | } 92 | #endif 93 | 94 | #endif 95 | -------------------------------------------------------------------------------- /collvoid_amcl/include/amcl/sensors/amcl_odom.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey et al. 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of the GNU Lesser General Public 7 | * License as published by the Free Software Foundation; either 8 | * version 2.1 of the License, or (at your option) any later version. 9 | * 10 | * This library is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | * Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public 16 | * License along with this library; if not, write to the Free Software 17 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 18 | * 19 | */ 20 | /////////////////////////////////////////////////////////////////////////// 21 | // 22 | // Desc: Odometry sensor model for AMCL 23 | // Author: Andrew Howard 24 | // Date: 17 Aug 2003 25 | // CVS: $Id: amcl_odom.h 4135 2007-08-23 19:58:48Z gerkey $ 26 | // 27 | /////////////////////////////////////////////////////////////////////////// 28 | 29 | #ifndef AMCL_ODOM_H 30 | #define AMCL_ODOM_H 31 | 32 | #include "amcl_sensor.h" 33 | #include "../pf/pf_pdf.h" 34 | 35 | namespace amcl 36 | { 37 | 38 | typedef enum 39 | { 40 | ODOM_MODEL_DIFF, 41 | ODOM_MODEL_OMNI, 42 | ODOM_MODEL_DIFF_CORRECTED, 43 | ODOM_MODEL_OMNI_CORRECTED 44 | } odom_model_t; 45 | 46 | // Odometric sensor data 47 | class AMCLOdomData : public AMCLSensorData 48 | { 49 | // Odometric pose 50 | public: pf_vector_t pose; 51 | 52 | // Change in odometric pose 53 | public: pf_vector_t delta; 54 | }; 55 | 56 | 57 | // Odometric sensor model 58 | class AMCLOdom : public AMCLSensor 59 | { 60 | // Default constructor 61 | public: AMCLOdom(); 62 | 63 | public: void SetModelDiff(double alpha1, 64 | double alpha2, 65 | double alpha3, 66 | double alpha4); 67 | 68 | public: void SetModelOmni(double alpha1, 69 | double alpha2, 70 | double alpha3, 71 | double alpha4, 72 | double alpha5); 73 | 74 | public: void SetModel( odom_model_t type, 75 | double alpha1, 76 | double alpha2, 77 | double alpha3, 78 | double alpha4, 79 | double alpha5 = 0 ); 80 | 81 | // Update the filter based on the action model. Returns true if the filter 82 | // has been updated. 83 | public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data); 84 | 85 | // Current data timestamp 86 | private: double time; 87 | 88 | // Model type 89 | private: odom_model_t model_type; 90 | 91 | // Drift parameters 92 | private: double alpha1, alpha2, alpha3, alpha4, alpha5; 93 | }; 94 | 95 | 96 | } 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /collvoid_amcl/include/amcl/sensors/amcl_sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey et al. 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of the GNU Lesser General Public 7 | * License as published by the Free Software Foundation; either 8 | * version 2.1 of the License, or (at your option) any later version. 9 | * 10 | * This library is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | * Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public 16 | * License along with this library; if not, write to the Free Software 17 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 18 | */ 19 | /////////////////////////////////////////////////////////////////////////// 20 | // 21 | // Desc: Adaptive Monte-Carlo localization 22 | // Author: Andrew Howard 23 | // Date: 6 Feb 2003 24 | // CVS: $Id: amcl_sensor.h 6443 2008-05-15 19:46:11Z gerkey $ 25 | // 26 | /////////////////////////////////////////////////////////////////////////// 27 | 28 | #ifndef AMCL_SENSOR_H 29 | #define AMCL_SENSOR_H 30 | 31 | #include "../pf/pf.h" 32 | 33 | namespace amcl 34 | { 35 | 36 | // Forward declarations 37 | class AMCLSensorData; 38 | 39 | 40 | // Base class for all AMCL sensors 41 | class AMCLSensor 42 | { 43 | // Default constructor 44 | public: AMCLSensor(); 45 | 46 | // Default destructor 47 | public: virtual ~AMCLSensor(); 48 | 49 | // Update the filter based on the action model. Returns true if the filter 50 | // has been updated. 51 | public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data); 52 | 53 | // Initialize the filter based on the sensor model. Returns true if the 54 | // filter has been initialized. 55 | public: virtual bool InitSensor(pf_t *pf, AMCLSensorData *data); 56 | 57 | // Update the filter based on the sensor model. Returns true if the 58 | // filter has been updated. 59 | public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data); 60 | 61 | // Flag is true if this is the action sensor 62 | public: bool is_action; 63 | 64 | // Action pose (action sensors only) 65 | public: pf_vector_t pose; 66 | 67 | // AMCL Base 68 | //protected: AdaptiveMCL & AMCL; 69 | 70 | #ifdef INCLUDE_RTKGUI 71 | // Setup the GUI 72 | public: virtual void SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig); 73 | 74 | // Finalize the GUI 75 | public: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig); 76 | 77 | // Draw sensor data 78 | public: virtual void UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data); 79 | #endif 80 | }; 81 | 82 | 83 | 84 | // Base class for all AMCL sensor measurements 85 | class AMCLSensorData 86 | { 87 | // Pointer to sensor that generated the data 88 | public: AMCLSensor *sensor; 89 | virtual ~AMCLSensorData() {} 90 | 91 | // Data timestamp 92 | public: double time; 93 | }; 94 | 95 | } 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /collvoid_amcl/package.xml: -------------------------------------------------------------------------------- 1 | 2 | collvoid_amcl 3 | 1.12.13 4 | 5 |

6 | amcl is a probabilistic localization system for a robot moving in 7 | 2D. It implements the adaptive (or KLD-sampling) Monte Carlo 8 | localization approach (as described by Dieter Fox), which uses a 9 | particle filter to track the pose of a robot against a known map. 10 |

11 |

12 | This node is derived, with thanks, from Andrew Howard's excellent 13 | 'amcl' Player driver. 14 |

15 | 16 | http://wiki.ros.org/amcl 17 | Brian P. Gerkey 18 | contradict@gmail.com 19 | David V. Lu!! 20 | Michael Ferguson 21 | LGPL 22 | 23 | catkin 24 | 25 | rosbag 26 | collvoid_msgs 27 | dynamic_reconfigure 28 | message_filters 29 | nav_msgs 30 | roscpp 31 | rostest 32 | std_srvs 33 | tf 34 | 35 | rosbag 36 | collvoid_msgs 37 | roscpp 38 | dynamic_reconfigure 39 | tf 40 | nav_msgs 41 | 42 | map_server 43 | 44 | -------------------------------------------------------------------------------- /collvoid_amcl/src/amcl/map/map.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Global map (grid-based) 23 | * Author: Andrew Howard 24 | * Date: 6 Feb 2003 25 | * CVS: $Id: map.c 1713 2003-08-23 04:03:43Z inspectorg $ 26 | **************************************************************************/ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "map.h" 35 | 36 | 37 | // Create a new map 38 | map_t *map_alloc(void) 39 | { 40 | map_t *map; 41 | 42 | map = (map_t*) malloc(sizeof(map_t)); 43 | 44 | // Assume we start at (0, 0) 45 | map->origin_x = 0; 46 | map->origin_y = 0; 47 | 48 | // Make the size odd 49 | map->size_x = 0; 50 | map->size_y = 0; 51 | map->scale = 0; 52 | 53 | // Allocate storage for main map 54 | map->cells = (map_cell_t*) NULL; 55 | 56 | return map; 57 | } 58 | 59 | 60 | // Destroy a map 61 | void map_free(map_t *map) 62 | { 63 | free(map->cells); 64 | free(map); 65 | return; 66 | } 67 | 68 | 69 | // Get the cell at the given point 70 | map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa) 71 | { 72 | int i, j; 73 | map_cell_t *cell; 74 | 75 | i = MAP_GXWX(map, ox); 76 | j = MAP_GYWY(map, oy); 77 | 78 | if (!MAP_VALID(map, i, j)) 79 | return NULL; 80 | 81 | cell = map->cells + MAP_INDEX(map, i, j); 82 | return cell; 83 | } 84 | 85 | -------------------------------------------------------------------------------- /collvoid_amcl/src/amcl/sensors/amcl_sensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /////////////////////////////////////////////////////////////////////////// 22 | // 23 | // Desc: AMCL sensor 24 | // Author: Andrew Howard 25 | // Date: 6 Feb 2003 26 | // CVS: $Id: amcl_sensor.cc 7057 2008-10-02 00:44:06Z gbiggs $ 27 | // 28 | /////////////////////////////////////////////////////////////////////////// 29 | 30 | 31 | #include "amcl_sensor.h" 32 | 33 | using namespace amcl; 34 | 35 | //////////////////////////////////////////////////////////////////////////////// 36 | // Default constructor 37 | AMCLSensor::AMCLSensor() 38 | { 39 | return; 40 | } 41 | 42 | AMCLSensor::~AMCLSensor() 43 | { 44 | } 45 | 46 | //////////////////////////////////////////////////////////////////////////////// 47 | // Apply the action model 48 | bool AMCLSensor::UpdateAction(pf_t *pf, AMCLSensorData *data) 49 | { 50 | return false; 51 | } 52 | 53 | 54 | //////////////////////////////////////////////////////////////////////////////// 55 | // Initialize the filter 56 | bool AMCLSensor::InitSensor(pf_t *pf, AMCLSensorData *data) 57 | { 58 | return false; 59 | } 60 | 61 | 62 | //////////////////////////////////////////////////////////////////////////////// 63 | // Apply the sensor model 64 | bool AMCLSensor::UpdateSensor(pf_t *pf, AMCLSensorData *data) 65 | { 66 | return false; 67 | } 68 | 69 | 70 | #ifdef INCLUDE_RTKGUI 71 | 72 | //////////////////////////////////////////////////////////////////////////////// 73 | // Setup the GUI 74 | void AMCLSensor::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) 75 | { 76 | return; 77 | } 78 | 79 | 80 | //////////////////////////////////////////////////////////////////////////////// 81 | // Shutdown the GUI 82 | void AMCLSensor::ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) 83 | { 84 | return; 85 | } 86 | 87 | 88 | //////////////////////////////////////////////////////////////////////////////// 89 | // Draw sensor data 90 | void AMCLSensor::UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data) 91 | { 92 | return; 93 | } 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /collvoid_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html 2 | # Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html 3 | cmake_minimum_required(VERSION 2.8.3) 4 | project(collvoid_controller) 5 | 6 | find_package(catkin REQUIRED COMPONENTS rospy std_msgs tf geometry_msgs std_srvs collvoid_local_planner) 7 | 8 | # catkin_python_setup() 9 | catkin_package( 10 | DEPENDS rospy std_msgs tf geometry_msgs std_srvs collvoid_local_planner 11 | CATKIN_DEPENDS # TODO 12 | INCLUDE_DIRS # TODO include 13 | LIBRARIES # TODO 14 | ) -------------------------------------------------------------------------------- /collvoid_controller/goals.yaml: -------------------------------------------------------------------------------- 1 | turtlebot-red: 2 | goals: 3 | x: [-2.7, -0.7] 4 | y: [2.4, 4.4] 5 | ang: [7.068, 3.93] 6 | turtlebot-green: 7 | goals: 8 | x: [-2.7, -0.7] 9 | y: [2.4, 4.4] 10 | ang: [7.068, 3.93] 11 | turtlebot-yellow: 12 | goals: 13 | x: [-0.7, -2.7] 14 | y: [2.4, 4.4] 15 | ang: [2.356, 5.497787] 16 | turtlebot-white: 17 | goals: 18 | x: [-0.7, -2.7] 19 | y: [2.4, 4.4] 20 | ang: [2.356, 5.497787] 21 | 22 | 1turtlebot-red: 23 | goals: 24 | x: [-1.3, -1.3] 25 | y: [4.0, 0.0] 26 | ang: [-1.71, 1.71] 27 | 28 | turtlebot-red_bak: 29 | goals: 30 | x: [-0.5, 2.5] 31 | y: [0.5, -2.5] 32 | ang: [-0.78, 2.38] 33 | 34 | 1turtlebot-white: 35 | goals: 36 | x: [-0.5, 2.5] 37 | y: [-2.5, 0.5] 38 | ang: [0.78, -2.38] 39 | 40 | 1turtlebot-yellow: 41 | goals: 42 | x: [-0.5, 2.5] 43 | y: [0.5, -2.5] 44 | ang: [-0.78, 2.38] 45 | 46 | 1turtlebot-green: 47 | goals: 48 | x: [-0.5, 2.5] 49 | y: [-2.5, 0.5] 50 | ang: [0.78, -2.38] 51 | 52 | turtlebot4: 53 | goals: 54 | x: [ 1.2, -1.2] 55 | y: [ 1.2, -1.2] 56 | ang: [-2.356, 0.785] 57 | 58 | turtlebot5: 59 | goals: 60 | x: [ 1.2, -1.2] 61 | y: [ -1.2, 1.2] 62 | ang: [2.356, -0.785] 63 | 64 | turtlebot6: 65 | goals: 66 | x: [ -1.2, 1.2] 67 | y: [ 1.2, -1.2] 68 | ang: [-0.785, 2.356] 69 | 70 | turtlebot7: 71 | goals: 72 | x: [ -1.2, 1.2] 73 | y: [ -1.2, 1.2] 74 | ang: [0.785, -2.356] 75 | 76 | prm1: 77 | goals: 78 | x: [ -1.2, 1.2] 79 | y: [ -1.2, -1.2] 80 | ang: [0.785, -2.356] 81 | 82 | pri1: 83 | goals: 84 | x: [ 3.6, 1.2] 85 | y: [ 2.4, 0.0] 86 | ang: [-2.356, 0.785] 87 | 88 | prk1: 89 | goals: 90 | x: [ -1.2, 1.2] 91 | y: [ -1.2, -1.2] 92 | ang: [0.785, -2.356] 93 | -------------------------------------------------------------------------------- /collvoid_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | collvoid_controller 3 | 1.0.0 4 | collvoid_controller 5 | Daniel Claes 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/collvoid_controller 10 | 11 | 12 | Daniel Claes 13 | 14 | 15 | catkin 16 | 17 | 18 | rospy 19 | std_msgs 20 | tf 21 | geometry_msgs 22 | std_srvs 23 | collvoid_local_planner 24 | 25 | 26 | rospy 27 | std_msgs 28 | tf 29 | geometry_msgs 30 | std_srvs 31 | collvoid_local_planner 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /collvoid_controller/src/collvoid_controller/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_controller/src/collvoid_controller/__init__.py -------------------------------------------------------------------------------- /collvoid_controller/src/collvoid_controller/active_collision_avoidance.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | __author__ = 'danielclaes' 3 | import rospy 4 | from collvoid_local_planner.srv import GetCollvoidTwist, GetCollvoidTwistRequest 5 | from geometry_msgs.msg import Twist, PoseStamped 6 | from std_srvs.srv import Empty, EmptyResponse 7 | from socket import gethostname 8 | import tf 9 | import tf.transformations 10 | 11 | GLOBAL_FRAME = '/map' 12 | 13 | 14 | class ActiveCollisionAvoidanceController(object): 15 | def __init__(self): 16 | self.hostname = rospy.get_namespace() 17 | self.active = False 18 | if self.hostname == "/": 19 | self.hostname = gethostname() 20 | self.tf_listener = tf.TransformListener() 21 | rospy.sleep(0.5) 22 | self.move_back_to_start = True 23 | self.home_pose = None 24 | self.base_link = rospy.get_param("~base_frame_id", "/base_link") 25 | rospy.loginfo("Name: %s", self.hostname) 26 | self.get_twist_srv = rospy.ServiceProxy("get_collvoid_twist", GetCollvoidTwist) 27 | self.twist_publisher = rospy.Publisher("cmd_vel", Twist, queue_size=1) 28 | rospy.Service("toggle_active_collvoid", Empty, self.toggle_active_collvoid) 29 | 30 | def toggle_active_collvoid(self, req): 31 | self.home_pose = self.get_own_pose() 32 | self.active = not self.active 33 | return EmptyResponse() 34 | 35 | def get_own_pose(self): 36 | own_pose = PoseStamped() 37 | own_pose.header.frame_id = self.base_link 38 | own_pose.header.stamp = rospy.Time.now() 39 | own_pose.pose.orientation.w = 1 40 | try: 41 | self.tf_listener.waitForTransform(GLOBAL_FRAME, self.base_link, own_pose.header.stamp, rospy.Duration(0.2)) 42 | result = self.tf_listener.transformPose(GLOBAL_FRAME, own_pose) 43 | except tf.Exception as e: 44 | rospy.logwarn("%s: could not transform pose, %s", self.hostname, e) 45 | return None 46 | return result 47 | 48 | def get_twist(self): 49 | req = GetCollvoidTwistRequest() 50 | if self.move_back_to_start: 51 | req.goal = self.home_pose 52 | else: 53 | req.goal = self.get_own_pose() 54 | if req.goal is None: 55 | return Twist() 56 | try: 57 | res = self.get_twist_srv(req) 58 | except rospy.ServiceException as e: 59 | rospy.logwarn("%s could not get twist %s", self.hostname, e) 60 | return Twist() 61 | return res.twist 62 | 63 | def spin(self): 64 | r = rospy.Rate(10) 65 | while not rospy.is_shutdown(): 66 | if self.active: 67 | self.twist_publisher.publish(self.get_twist()) 68 | r.sleep() 69 | 70 | 71 | if __name__ == '__main__': 72 | rospy.init_node('active_collision_avoidance') 73 | active_collvoid = ActiveCollisionAvoidanceController() 74 | active_collvoid.spin() 75 | -------------------------------------------------------------------------------- /collvoid_controller/src/collvoid_controller/people_tracker.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from geometry_msgs.msg import PoseStamped, PolygonStamped, Point32 3 | 4 | __author__ = 'danielclaes' 5 | import rospy 6 | import tf 7 | from spencer_tracking_msgs.msg import TrackedPersons, TrackedPerson 8 | from collvoid_msgs.msg import AggregatedPoseTwist, PoseTwistWithCovariance 9 | from math import sin, cos, pi 10 | 11 | global_frame = "/map" 12 | radius = 0.4 13 | division = 4 14 | 15 | class PeopleTracker(object): 16 | def __init__(self): 17 | self.detected = [] 18 | self.people_pub = rospy.Publisher('people', AggregatedPoseTwist, queue_size=1) 19 | self.tf_listener = tf.TransformListener() 20 | self.footprint = PolygonStamped() 21 | num_points = 32 22 | for i in xrange(num_points): 23 | p = Point32() 24 | p.x = radius * cos((2. * pi * i) / num_points) 25 | p.y = radius * sin((2. * pi * i) / num_points) 26 | self.footprint.polygon.points.append(p) 27 | rospy.sleep(0.5) 28 | rospy.Subscriber('tracked_persons', TrackedPersons, self.detected_persons) 29 | 30 | def detected_persons(self, msg): 31 | """ 32 | :param msg: 33 | :type msg: TrackedPersons 34 | :return: 35 | """ 36 | publish_msg = AggregatedPoseTwist() 37 | publish_msg.header.stamp = msg.header.stamp 38 | publish_msg.header.frame_id = global_frame 39 | for track in msg.tracks: 40 | person = PoseTwistWithCovariance() 41 | person.header.stamp = msg.header.stamp 42 | person.header.frame_id = global_frame 43 | person.controlled = False 44 | person.twist = track.twist 45 | person.twist.twist.linear.x /= division 46 | person.twist.twist.linear.y /= division 47 | person.twist.twist.angular.z /= division 48 | person.holo_robot = True 49 | person.robot_id = "person_" + str(track.track_id) 50 | person.footprint = self.footprint 51 | person.radius = radius 52 | pose = PoseStamped() 53 | pose.header = msg.header 54 | pose.pose = track.pose.pose 55 | pose = self.transform_pose_to_global(pose) 56 | person.pose.pose = pose.pose 57 | person.pose.covariance = track.pose.covariance 58 | if pose is None: 59 | continue 60 | publish_msg.posetwists.append(person) 61 | self.people_pub.publish(publish_msg) 62 | 63 | def transform_pose_to_global(self, pose): 64 | try: 65 | self.tf_listener.waitForTransform(pose.header.frame_id, global_frame, pose.header.stamp, rospy.Duration(0.1)) 66 | result = self.tf_listener.transformPose(global_frame, pose) 67 | except tf.Exception as e: 68 | rospy.logwarn("Peopletracker: could not transform pose, %s", e) 69 | return None 70 | return result 71 | 72 | 73 | if __name__ == '__main__': 74 | rospy.init_node('people_tracker') 75 | tracker = PeopleTracker() 76 | rospy.spin() 77 | 78 | -------------------------------------------------------------------------------- /collvoid_dwa_local_planner/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package collvoid_dwa_local_planner 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.11.5 (2014-01-30) 6 | ------------------- 7 | * Fix for `#5 `_ 8 | * Parameter to sum scores in DWA Planner 9 | * Keep consistent allignment_cost scale 10 | Use the configured alignment cost instead of resetting to 1.0. This 11 | condition (farther from goal than forward_point_distance) is probably 12 | met at the start of navigation, it seems this cost should be obeyed 13 | until the robot gets close, then ignored completely. 14 | * Cheat factor for end of DWA Plans 15 | * Change maintainer from Hersh to Lu 16 | 17 | 1.11.4 (2013-09-27) 18 | ------------------- 19 | * Package URL Updates 20 | * Changed new Odom-Helper::initialize() function to setOdomTopic(). 21 | * some more corrections for the pointcloud object bug 22 | -------------------------------------------------------------------------------- /collvoid_dwa_local_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(collvoid_dwa_local_planner) 3 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 4 | 5 | find_package(catkin REQUIRED 6 | COMPONENTS 7 | base_local_planner 8 | collvoid_local_planner 9 | collvoid_srvs 10 | cmake_modules 11 | costmap_2d 12 | dynamic_reconfigure 13 | nav_core 14 | nav_msgs 15 | pluginlib 16 | pcl_conversions 17 | roscpp 18 | tf 19 | ) 20 | 21 | find_package(Eigen REQUIRED) 22 | find_package(PCL REQUIRED) 23 | include_directories( 24 | include 25 | ${catkin_INCLUDE_DIRS} 26 | ${EIGEN_INCLUDE_DIRS} 27 | ${PCL_INCLUDE_DIRS} 28 | ) 29 | add_definitions(${EIGEN_DEFINITIONS}) 30 | 31 | link_directories(${catkin_LIBRARY_DIRS}) 32 | 33 | # dynamic reconfigure 34 | generate_dynamic_reconfigure_options( 35 | cfg/DWAPlanner.cfg 36 | ) 37 | 38 | catkin_package( 39 | INCLUDE_DIRS include 40 | LIBRARIES collvoid_dwa_local_planner 41 | CATKIN_DEPENDS 42 | dynamic_reconfigure 43 | pluginlib 44 | roscpp 45 | ) 46 | 47 | add_library(collvoid_dwa_local_planner 48 | src/dwa_planner.cpp 49 | src/dwa_planner_ros.cpp 50 | src/goal_alignment_cost_function.cpp 51 | src/path_alignment_cost_function.cpp 52 | include/collvoid_dwa_local_planner/ 53 | ) 54 | target_link_libraries(collvoid_dwa_local_planner base_local_planner ${catkin_LIBRARIES}) 55 | add_dependencies(collvoid_dwa_local_planner ${catkin_EXPORTED_TARGETS}) 56 | add_dependencies(collvoid_dwa_local_planner collvoid_local_planner) 57 | add_dependencies(collvoid_dwa_local_planner collvoid_scoring_function) 58 | 59 | 60 | install(TARGETS collvoid_dwa_local_planner 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | ) 63 | 64 | install(FILES blp_plugin.xml 65 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 66 | ) 67 | 68 | install(DIRECTORY include/${PROJECT_NAME}/ 69 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 70 | PATTERN ".svn" EXCLUDE 71 | ) 72 | -------------------------------------------------------------------------------- /collvoid_dwa_local_planner/blp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of a local planner using either a DWA approach based on configuration parameters. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /collvoid_dwa_local_planner/cfg/DWAPlanner.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # DWA Planner configuration 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, bool_t 5 | from local_planner_limits import add_generic_localplanner_params 6 | 7 | gen = ParameterGenerator() 8 | 9 | 10 | # This unusual line allows to reuse existing parameter definitions 11 | # that concern all localplanners 12 | add_generic_localplanner_params(gen) 13 | 14 | 15 | gen.add("sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0) 16 | gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0) 17 | gen.add("angular_sim_granularity", double_t, 0, "The granularity with which to check for collisions for rotations in radians", 0.1, 0) 18 | 19 | gen.add("path_distance_bias", double_t, 0, "The weight for the path distance part of the cost function", 32.0, 0.0) 20 | gen.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 24.0, 0.0) 21 | 22 | gen.add("collvoid_scale", double_t, 0, "The weight for the collvoid part of the cost function", 24.0, 0.0) 23 | gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0.0) 24 | 25 | gen.add("stop_time_buffer", double_t, 0, "The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds", 0.2, 0) 26 | gen.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0) 27 | gen.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn before oscillation flags are reset, in radians", 0.2, 0) 28 | 29 | gen.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325) 30 | 31 | gen.add("scaling_speed", double_t, 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0) 32 | gen.add("max_scaling_factor", double_t, 0, "The maximum factor to scale the robot's footprint by", 0.2, 0) 33 | 34 | gen.add("vx_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 3, 1) 35 | gen.add("vy_samples", int_t, 0, "The number of samples to use when exploring the y velocity space", 10, 1) 36 | gen.add("vth_samples", int_t, 0, "The number of samples to use when exploring the theta velocity space", 20, 1) 37 | 38 | gen.add("use_dwa", bool_t, 0, "Use dynamic window approach to constrain sampling velocities to small window.", True) 39 | 40 | gen.add("goal_sq_dist", double_t, 0, "The sq_dist where heading criticts kicks in.", 2, 0) 41 | gen.add("heading_bias", double_t, 0, "The weight for the heading bias.", 16.0, 0) 42 | gen.add("path_scale", double_t, 0, "The path alignment scale", 0.1, 0, 1) 43 | 44 | 45 | gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration.", False) 46 | 47 | exit(gen.generate("collvoid_dwa_local_planner", "collvoid_dwa_local_planner", "DWAPlanner")) 48 | -------------------------------------------------------------------------------- /collvoid_dwa_local_planner/include/collvoid_dwa_local_planner/goal_alignment_cost_function.h: -------------------------------------------------------------------------------- 1 | /* 2 | * 3 | * Copyright (C) 2014 Samir Benmendil 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | * 18 | */ 19 | 20 | #ifndef GOALALIGNMENTCOSTFUNCTION_H 21 | #define GOALALIGNMENTCOSTFUNCTION_H 22 | 23 | #include 24 | #include 25 | 26 | namespace collvoid_dwa_local_planner 27 | { 28 | 29 | using namespace base_local_planner; 30 | 31 | class GoalAlignmentCostFunction : public TrajectoryCostFunction 32 | { 33 | public: 34 | GoalAlignmentCostFunction() {} 35 | ~GoalAlignmentCostFunction() {} 36 | 37 | bool prepare() { 38 | return true; 39 | } 40 | double scoreTrajectory(Trajectory &traj); 41 | void setGoalPose(const geometry_msgs::PoseStamped &goalPose) { goalPose_ = goalPose; } 42 | 43 | private: 44 | geometry_msgs::PoseStamped goalPose_; 45 | }; 46 | } 47 | 48 | #endif // GOALALIGNMENTCOSTFUNCTION_H 49 | -------------------------------------------------------------------------------- /collvoid_dwa_local_planner/include/collvoid_dwa_local_planner/path_alignment_cost_function.h: -------------------------------------------------------------------------------- 1 | #ifndef PATHALIGMENTCOSTFUNCTION_H 2 | #define PATHALIGMENTCOSTFUNCTION_H 3 | 4 | #include 5 | 6 | namespace collvoid_dwa_local_planner 7 | { 8 | 9 | using namespace base_local_planner; 10 | 11 | class PathAlignmentCostFunction : public TrajectoryCostFunction 12 | { 13 | public: 14 | PathAlignmentCostFunction() {} 15 | ~PathAlignmentCostFunction() {} 16 | 17 | bool prepare() { return true; } 18 | 19 | double scoreTrajectory(Trajectory &traj); 20 | }; 21 | } 22 | 23 | #endif // PATHALIGMENTCOSTFUNCTION_H 24 | -------------------------------------------------------------------------------- /collvoid_dwa_local_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | collvoid_dwa_local_planner 3 | 1.11.6 4 | 5 | 6 | This package provides an implementation of the Dynamic Window Approach to 7 | local robot navigation on a plane. Given a global plan to follow and a 8 | costmap, the local planner produces velocity commands to send to a mobile 9 | base. This package supports any robot who's footprint can be represented as 10 | a convex polygon or cicrle, and exposes its configuration as ROS parameters 11 | that can be set in a launch file. The parameters for this planner are also 12 | dynamically reconfigurable. This package's ROS wrapper adheres to the 13 | BaseLocalPlanner interface specified in the nav_core package. 14 | 15 | 16 | Eitan Marder-Eppstein 17 | contradict@gmail.com 18 | David V. Lu!! 19 | BSD 20 | http://wiki.ros.org/collvoid_dwa_local_planner 21 | 22 | catkin 23 | cmake_modules 24 | 25 | base_local_planner 26 | costmap_2d 27 | collvoid_local_planner 28 | collvoid_srvs 29 | 30 | dynamic_reconfigure 31 | eigen 32 | nav_core 33 | nav_msgs 34 | pluginlib 35 | pcl_conversions 36 | roscpp 37 | tf 38 | 39 | base_local_planner 40 | collvoid_local_planner 41 | collvoid_srvs 42 | costmap_2d 43 | dynamic_reconfigure 44 | eigen 45 | nav_core 46 | nav_msgs 47 | pluginlib 48 | roscpp 49 | tf 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /collvoid_dwa_local_planner/src/goal_alignment_cost_function.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * 3 | * Copyright (C) 2014 Samir Benmendil 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | * 18 | */ 19 | 20 | #include "collvoid_dwa_local_planner/goal_alignment_cost_function.h" 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | namespace collvoid_dwa_local_planner 27 | { 28 | double GoalAlignmentCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj) 29 | { 30 | if (traj.getPointsSize() < 1) return 0; 31 | double goalHeading = tf::getYaw(goalPose_.pose.orientation); 32 | double x, y, th; 33 | traj.getEndpoint(x, y, th); 34 | double delta_th = fabs(angles::shortest_angular_distance(th, goalHeading)); 35 | return delta_th / M_PI; 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /collvoid_dwa_local_planner/src/path_alignment_cost_function.cpp: -------------------------------------------------------------------------------- 1 | #include "collvoid_dwa_local_planner/path_alignment_cost_function.h" 2 | 3 | #include 4 | 5 | namespace collvoid_dwa_local_planner 6 | { 7 | double PathAlignmentCostFunction::scoreTrajectory(Trajectory &traj) 8 | { 9 | return fabs(traj.yv_ ); 10 | } 11 | } 12 | -------------------------------------------------------------------------------- /collvoid_local_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html 2 | # Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html 3 | cmake_minimum_required(VERSION 2.8.3) 4 | project(collvoid_local_planner) 5 | # Load catkin and all dependencies required for this package 6 | find_package(catkin REQUIRED COMPONENTS 7 | roslib roscpp collvoid_msgs collvoid_srvs visualization_msgs laser_geometry tf angles base_local_planner pluginlib nav_core actionlib move_base_msgs std_srvs dynamic_reconfigure amcl) 8 | 9 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 10 | 11 | include_directories( 12 | include 13 | ${catkin_INCLUDE_DIRS} 14 | ) 15 | 16 | 17 | generate_dynamic_reconfigure_options(cfg/Collvoid.cfg) 18 | 19 | add_service_files( 20 | FILES 21 | GetCollvoidTwist.srv 22 | ) 23 | 24 | # Generate added messages and services with any dependencies listed here 25 | generate_messages( 26 | DEPENDENCIES 27 | geometry_msgs 28 | ) 29 | 30 | include_directories(${Boost_INCLUDE_DIRS}) 31 | 32 | 33 | catkin_package( 34 | DEPENDS 35 | CATKIN_DEPENDS 36 | roscpp collvoid_msgs visualization_msgs laser_geometry tf angles base_local_planner pluginlib nav_core actionlib move_base_msgs std_srvs dynamic_reconfigure amcl 37 | INCLUDE_DIRS include 38 | LIBRARIES collvoid_local_planner collvoid_scoring_function 39 | ) 40 | 41 | 42 | add_library(collvoid_scoring_function 43 | src/Agent.cpp 44 | src/collvoid_scoring_function.cpp 45 | src/orca.cpp 46 | src/clearpath.cpp 47 | src/collvoid_publishers.cpp 48 | include/collvoid_local_planner/) 49 | add_dependencies(collvoid_scoring_function ${${PROJECT_NAME}_EXPORTED_TARGETS}) 50 | add_dependencies(collvoid_scoring_function ${catkin_EXPORTED_TARGETS}) 51 | target_link_libraries(collvoid_scoring_function 52 | ${catkin_LIBRARIES} 53 | ${Boost_LIBRARIES} 54 | ) 55 | 56 | 57 | add_library(collvoid_local_planner src/collvoid_local_planner.cpp src/Agent.cpp src/ROSAgent.cpp src/orca.cpp src/collvoid_publishers.cpp src/clearpath.cpp) 58 | add_dependencies(collvoid_local_planner ${${PROJECT_NAME}_EXPORTED_TARGETS}) 59 | add_dependencies(collvoid_local_planner ${catkin_EXPORTED_TARGETS}) 60 | target_link_libraries(collvoid_local_planner 61 | ${catkin_LIBRARIES} 62 | ${Boost_LIBRARIES} 63 | ) 64 | 65 | 66 | add_executable(MePublisher src/me_publisher.cpp src/clearpath.cpp src/collvoid_publishers.cpp) 67 | target_link_libraries(MePublisher 68 | ${catkin_LIBRARIES} 69 | ${Boost_LIBRARIES}) 70 | add_dependencies(MePublisher ${${PROJECT_NAME}_EXPORTED_TARGETS}) 71 | add_dependencies(MePublisher ${catkin_EXPORTED_TARGETS}) 72 | 73 | 74 | add_executable(ROSAgent src/collvoid_local_planner.cpp src/Agent.cpp src/ROSAgent.cpp src/orca.cpp src/collvoid_publishers.cpp src/clearpath.cpp) 75 | target_link_libraries(ROSAgent 76 | ${catkin_LIBRARIES} 77 | ${Boost_LIBRARIES}) 78 | add_dependencies(ROSAgent ${${PROJECT_NAME}_EXPORTED_TARGETS}) 79 | add_dependencies(ROSAgent ${catkin_EXPORTED_TARGETS}) 80 | 81 | -------------------------------------------------------------------------------- /collvoid_local_planner/blp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of VO paradigm for local planning. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /collvoid_local_planner/include/collvoid_local_planner/collvoid_publishers.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Daniel Claes, Maastricht University 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Maastricht University nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef COLLVOID_PUBLISHERS_H 31 | #define COLLVOID_PUBLISHERS_H 32 | 33 | /* #include */ 34 | #include 35 | #include 36 | #include 37 | #include "collvoid_local_planner/Vector2.h" 38 | #include "collvoid_local_planner/Agent.h" 39 | 40 | namespace collvoid { 41 | 42 | #define MAX_POINTS_ 400 43 | 44 | void publishHoloSpeed(Vector2 pos, Vector2 vel, std::string target_frame, std::string name_space, ros::Publisher speed_pub); 45 | 46 | void publishVOs(Vector2& pos, const std::vector& truncated_vos, bool use_truncation, std::string target_frame, std::string name_space, ros::Publisher vo_pub); 47 | 48 | void publishPoints(Vector2& pos, const std::vector& points, std::string target_frame, std::string name_space, ros::Publisher samples_pub); 49 | void publishOrcaLines(const std::vector& orca_lines, Vector2& position, std::string target_frame, std::string name_space, ros::Publisher line_pub); 50 | 51 | void publishNeighborPositionsBare(std::vector& neighbors, std::string target_frame, std::string name_space, ros::Publisher neighbors_pub); 52 | 53 | void publishMePosition(double radius, tf::Stamped global_pose, std::string target_frame, std::string name_space, ros::Publisher me_pub); 54 | 55 | void fillMarkerWithParams(visualization_msgs::MarkerArray &marker, double radius, Vector2 position, double yaw, std::string base_frame, 56 | std::string name_space); 57 | void publishObstacleLines(const std::vector& obstacles_lines, std::string target_frame, std::string name_space, ros::Publisher line_pub); 58 | 59 | } 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /collvoid_local_planner/include/collvoid_local_planner/collvoid_scoring_function.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by danielclaes on 22/06/15. 3 | // 4 | 5 | #ifndef COLLVOID_LOCAL_PLANNER_COLLVOID_SCORING_FUNCTION_H 6 | #define COLLVOID_LOCAL_PLANNER_COLLVOID_SCORING_FUNCTION_H 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include "collvoid_local_planner/Vector2.h" 17 | #include "collvoid_local_planner/Utils.h" 18 | #include "collvoid_local_planner/Agent.h" 19 | #include "collvoid_local_planner/collvoid_publishers.h" 20 | 21 | 22 | using namespace collvoid; 23 | 24 | namespace collvoid_scoring_function 25 | { 26 | 27 | using namespace base_local_planner; 28 | 29 | class CollvoidScoringFunction : public TrajectoryCostFunction 30 | { 31 | public: 32 | CollvoidScoringFunction() {}; 33 | ~CollvoidScoringFunction() {}; 34 | 35 | void init(ros::NodeHandle nh); 36 | bool prepare(); 37 | double scoreTrajectory(Trajectory &traj); 38 | 39 | private: 40 | bool getMe(); 41 | bool getNeighbors(); 42 | bool compareNeighborsPositions(const AgentPtr &agent1, const AgentPtr &agent2); 43 | bool compareVectorPosition(const collvoid::Vector2 &v1, const collvoid::Vector2 &v2); 44 | 45 | AgentPtr createAgentFromMsg(collvoid_msgs::PoseTwistWithCovariance &msg); 46 | 47 | AgentPtr me_; 48 | bool use_truncation_, use_polygon_footprint_; 49 | double trunc_time_, max_dist_vo_; 50 | ros::Publisher vo_pub_, neighbors_pub_, samples_pub_; 51 | ros::ServiceClient get_me_srv_, get_neighbors_srv_; 52 | 53 | std::vector points; 54 | }; 55 | } 56 | 57 | 58 | #endif //COLLVOID_LOCAL_PLANNER_COLLVOID_SCORING_FUNCTION_H 59 | -------------------------------------------------------------------------------- /collvoid_local_planner/include/collvoid_local_planner/me_publisher.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by danielclaes on 23/06/15. 3 | // 4 | 5 | #ifndef COLLVOID_LOCAL_PLANNER_ME_PUBLISHER_H 6 | #define COLLVOID_LOCAL_PLANNER_ME_PUBLISHER_H 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "collvoid_local_planner/clearpath.h" 21 | #include "collvoid_local_planner/Utils.h" 22 | #include "collvoid_local_planner/Vector2.h" 23 | #include "collvoid_local_planner/collvoid_publishers.h" 24 | 25 | 26 | using namespace collvoid; 27 | 28 | class MePublisher { 29 | public: 30 | MePublisher(); 31 | virtual ~MePublisher() { }; 32 | void init(ros::NodeHandle private_nh, tf::TransformListener *tf); 33 | 34 | private: 35 | void computeNewMinkowskiFootprint(); 36 | void computeNewLocUncertainty(); 37 | void amclPoseArrayWeightedCallback(const collvoid_msgs::PoseArrayWeighted::ConstPtr &msg); 38 | void odomCallback(const nav_msgs::Odometry::ConstPtr &msg); 39 | void publishMePoseTwist(); 40 | bool compareConvexHullPointsPosition(const ConvexHullPoint &p1, const ConvexHullPoint &p2); 41 | void setMinkowskiFootprintVector2(geometry_msgs::PolygonStamped minkowski_footprint); 42 | geometry_msgs::PolygonStamped createFootprintMsgFromVector2(const std::vector &footprint); 43 | void getFootprint(ros::NodeHandle private_nh); 44 | bool getMeCB(collvoid_srvs::GetMe::Request &req, collvoid_srvs::GetMe::Response &res); 45 | 46 | bool getGlobalPose(tf::Stamped &global_pose, std::string target_frame, const ros::Time stamp); 47 | bool createMeMsg(collvoid_msgs::PoseTwistWithCovariance &me_msg, std::string target_frame); 48 | 49 | 50 | //Agent description 51 | std::string my_id_; 52 | std::string base_frame_, global_frame_; 53 | bool use_polygon_footprint_, holo_robot_, controlled_; 54 | Vector2 holo_velocity_; 55 | 56 | double uninflated_robot_radius_, radius_, cur_loc_unc_radius_; 57 | 58 | std::vector minkowski_footprint_; 59 | geometry_msgs::PolygonStamped footprint_msg_; 60 | 61 | 62 | double eps_; //localization epsilon 63 | 64 | 65 | 66 | double publish_me_period_; 67 | 68 | ros::Time last_seen_, last_time_me_published_; 69 | ros::Publisher position_share_pub_, polygon_pub_, me_pub_; 70 | ros::Subscriber odom_sub_, particle_sub_; 71 | ros::ServiceServer server_; 72 | tf::TransformListener *tf_; 73 | 74 | geometry_msgs::Twist twist_; 75 | 76 | boost::mutex convex_lock_; 77 | std::vector > pose_array_weighted_; 78 | 79 | }; 80 | 81 | #endif //COLLVOID_LOCAL_PLANNER_ME_PUBLISHER_H 82 | -------------------------------------------------------------------------------- /collvoid_local_planner/srv/GetCollvoidTwist.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped goal 2 | --- 3 | geometry_msgs/Twist twist -------------------------------------------------------------------------------- /collvoid_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(collvoid_msgs) 4 | find_package(catkin REQUIRED COMPONENTS geometry_msgs nav_msgs) 5 | 6 | add_message_files( 7 | FILES 8 | AggregatedPoseTwist.msg 9 | collvoid_debug.msg 10 | PoseTwistWithCovariance.msg 11 | PoseArrayWeighted.msg 12 | ) 13 | 14 | generate_messages( 15 | DEPENDENCIES geometry_msgs nav_msgs 16 | ) 17 | 18 | catkin_package( 19 | DEPENDS geometry_msgs nav_msgs 20 | CATKIN_DEPENDS # TODO 21 | INCLUDE_DIRS # TODO include 22 | LIBRARIES # TODO 23 | ) -------------------------------------------------------------------------------- /collvoid_msgs/msg/AggregatedPoseTwist.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | collvoid_msgs/PoseTwistWithCovariance[] posetwists -------------------------------------------------------------------------------- /collvoid_msgs/msg/PoseArrayWeighted.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose[] poses 3 | float32[] weights -------------------------------------------------------------------------------- /collvoid_msgs/msg/PoseTwistWithCovariance.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string robot_id 3 | float32 radius 4 | bool holo_robot 5 | bool controlled 6 | geometry_msgs/Vector3 holonomic_velocity 7 | geometry_msgs/PoseWithCovariance pose 8 | geometry_msgs/TwistWithCovariance twist 9 | geometry_msgs/PolygonStamped footprint 10 | -------------------------------------------------------------------------------- /collvoid_msgs/msg/collvoid_debug.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int32 run 3 | geometry_msgs/Twist cmd_vel 4 | geometry_msgs/Vector3Stamped holo_vel 5 | nav_msgs/Odometry odom 6 | nav_msgs/Odometry ground_truth 7 | geometry_msgs/PoseStamped located_pose 8 | float32 loc_error 9 | float32 radius_uncertainty -------------------------------------------------------------------------------- /collvoid_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | collvoid_msgs 3 | 1.0.0 4 | collvoid_msg 5 | Daniel Claes 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/collvoid_msg 10 | 11 | 12 | Daniel Claes 13 | 14 | 15 | catkin 16 | 17 | 18 | geometry_msgs 19 | nav_msgs 20 | 21 | 22 | geometry_msgs 23 | nav_msgs 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /collvoid_simple_global_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html 2 | # Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html 3 | cmake_minimum_required(VERSION 2.8.3) 4 | project(collvoid_simple_global_planner) 5 | # Load catkin and all dependencies required for this package 6 | find_package(catkin REQUIRED COMPONENTS roscpp pluginlib nav_core costmap_2d base_local_planner tf angles) 7 | 8 | 9 | include_directories( 10 | include 11 | ${catkin_INCLUDE_DIRS} 12 | SYSTEM 13 | ) 14 | 15 | add_library(${PROJECT_NAME} src/collvoid_simple_global_planner.cpp) 16 | 17 | include_directories(${Boost_INCLUDE_DIRS}) 18 | target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) 19 | 20 | catkin_package( 21 | DEPENDS roscpp pluginlib nav_core costmap_2d base_local_planner tf angles 22 | CATKIN_DEPENDS # TODO 23 | INCLUDE_DIRS # TODO include 24 | LIBRARIES # TODO 25 | ) -------------------------------------------------------------------------------- /collvoid_simple_global_planner/bgp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /collvoid_simple_global_planner/include/collvoid_simple_global_planner/collvoid_simple_global_planner.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Daniel Claes 36 | *********************************************************************/ 37 | #ifndef COLLVOID_SIMPLE_GLOBAL_PLANNER_H_ 38 | #define COLLVOID_SIMPLE_GLOBAL_PLANNER_H_ 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | 47 | 48 | namespace collvoid_simple_global_planner{ 49 | class CollvoidSimpleGlobalPlanner : public nav_core::BaseGlobalPlanner { 50 | public: 51 | CollvoidSimpleGlobalPlanner(); 52 | CollvoidSimpleGlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros); 53 | void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros); 54 | bool makePlan(const geometry_msgs::PoseStamped& start, 55 | const geometry_msgs::PoseStamped& goal, std::vector& plan); 56 | }; 57 | }; 58 | #endif 59 | -------------------------------------------------------------------------------- /collvoid_simple_global_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | collvoid_simple_global_planner 3 | 1.0.0 4 | collvoid_simple_global_planner 5 | Daniel Claes 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/collvoid_simple_global_planner 10 | 11 | 12 | Daniel Claes 13 | 14 | 15 | catkin 16 | 17 | 18 | roscpp 19 | pluginlib 20 | nav_core 21 | costmap_2d 22 | base_local_planner 23 | tf 24 | angles 25 | 26 | 27 | roscpp 28 | pluginlib 29 | nav_core 30 | costmap_2d 31 | base_local_planner 32 | tf 33 | angles 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /collvoid_srvs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | collvoid_srvs 4 | 0.0.0 5 | The collvoid_srvs package 6 | 7 | 8 | 9 | 10 | danielclaes 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 | catkin 43 | collvoid_msgs 44 | geometry_msgs 45 | message_generation 46 | std_srvs 47 | collvoid_msgs 48 | geometry_msgs 49 | message_runtime 50 | std_srvs 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /collvoid_srvs/srv/GetMe.srv: -------------------------------------------------------------------------------- 1 | --- 2 | collvoid_msgs/PoseTwistWithCovariance me -------------------------------------------------------------------------------- /collvoid_srvs/srv/GetNeighbors.srv: -------------------------------------------------------------------------------- 1 | --- 2 | collvoid_msgs/PoseTwistWithCovariance[] neighbors -------------------------------------------------------------------------------- /collvoid_srvs/srv/GetObstacles.srv: -------------------------------------------------------------------------------- 1 | --- 2 | geometry_msgs/PolygonStamped[] obstacles -------------------------------------------------------------------------------- /collvoid_stage/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(collvoid_stage) 3 | 4 | find_package(catkin REQUIRED COMPONENTS collvoid_local_planner) 5 | #catkin_python_setup() 6 | 7 | catkin_package( 8 | DEPENDS collvoid_local_planner 9 | CATKIN_DEPENDS # TODO 10 | INCLUDE_DIRS # TODO include 11 | LIBRARIES # TODO 12 | ) -------------------------------------------------------------------------------- /collvoid_stage/bags/.eval_bags: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_stage/bags/.eval_bags -------------------------------------------------------------------------------- /collvoid_stage/launch/amcl_diff_multi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /collvoid_stage/launch/amcl_diff_single.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /collvoid_stage/launch/amcl_omni_multi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /collvoid_stage/launch/amcl_omni_single.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /collvoid_stage/launch/move_base_collvoid.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | - 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /collvoid_stage/launch/move_base_collvoid_legacy.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | - 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /collvoid_stage/launch/move_base_dwa.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | - 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /collvoid_stage/launch/move_base_dwa_simple.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | - 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /collvoid_stage/launch/sim_single_pr2.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 | -------------------------------------------------------------------------------- /collvoid_stage/launch/sim_single_turtle.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /collvoid_stage/launch/sim_single_turtle_dwa.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /collvoid_stage/package.xml: -------------------------------------------------------------------------------- 1 | 2 | collvoid_stage 3 | 1.0.0 4 | collvoid_stage 5 | Daniel Claes 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/collvoid_stage 10 | 11 | 12 | Daniel Claes 13 | 14 | 15 | catkin 16 | 17 | 18 | collvoid_local_planner 19 | 20 | 21 | stage 22 | collvoid_local_planner 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /collvoid_stage/params/collvoid_common.yaml: -------------------------------------------------------------------------------- 1 | ## For collvoid_scoring_function and me publisher 2 | global_frame_id: map 3 | # base_frame: /robot_0/base_link 4 | max_dist_vo: 1. #for scoring function scale 5 | trunc_time: 10.0 6 | 7 | use_truncation: true 8 | use_polygon_footprint: true # true #footprint or radius 9 | 10 | -------------------------------------------------------------------------------- /collvoid_stage/params/collvoid_config.yaml: -------------------------------------------------------------------------------- 1 | 2 | CollvoidLocalPlanner: 3 | use_obstacles: true 4 | use_dwa_scoring: true 5 | prune_plan: true 6 | 7 | time_to_holo: 0.4 8 | min_error_holo: 0.02 9 | max_error_holo: 0.10 10 | left_pref: -0.05 11 | 12 | type_vo: 0 #HRVO = 0, RVO = 1, VO = 2 13 | orca: false #orca or VO 14 | clearpath: true #clearpath or sampling 15 | num_samples: 400 #num samples 16 | new_sampling: true 17 | use_truncation: true #truncate vos -------------------------------------------------------------------------------- /collvoid_stage/params/costmap_common_turtle.yaml: -------------------------------------------------------------------------------- 1 | map_type: costmap 2 | #Set the tolerance we're willing to have for tf transforms 3 | transform_tolerance: 0.3 4 | 5 | #footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] 6 | robot_radius: 0.22 7 | 8 | footprint_padding: 0.0 9 | 10 | static_layer: 11 | enabled: true 12 | 13 | obstacle_layer: 14 | enabled: true 15 | max_obstacle_height: 2.0 16 | origin_z: 0 17 | z_resolution: 2 18 | z_voxels: 1 19 | unknown_threshold: 15 20 | mark_threshold: 0 21 | combination_method: 1 22 | track_unknown_space: true #true needed for disabling global path planning through unknown space 23 | obstacle_range: 2.5 24 | raytrace_range: 3.0 25 | publish_voxel_map: true 26 | observation_sources: base_scan #python_node 27 | base_scan: 28 | data_type: LaserScan 29 | topic: base_scan 30 | expected_update_rate: 0.2 31 | observation_persistence: 0.0 32 | marking: true 33 | clearing: true 34 | min_obstacle_height: -0.1 35 | max_obstacle_height: 2. 36 | 37 | stationary_robots: 38 | enabled: true 39 | max_obstacle_height: 2.0 40 | obstacle_range: 2 41 | raytrace_range: 4.0 42 | publish_voxel_map: true 43 | 44 | observation_sources: python_node python_node_clearing 45 | python_node_clearing: 46 | data_type: LaserScan 47 | topic: clearing_scan 48 | observation_persistence: 0.0 49 | marking: false 50 | clearing: true 51 | 52 | python_node: 53 | data_type: PointCloud2 54 | topic: stationary_robots 55 | observation_persistence: 0.0 56 | marking: true 57 | clearing: false 58 | -------------------------------------------------------------------------------- /collvoid_stage/params/me_publisher_turtle.yaml: -------------------------------------------------------------------------------- 1 | collvoid: 2 | holo_robot: false 3 | controlled: true 4 | eps: 0.7 5 | publish_me_frequency: 10 6 | -------------------------------------------------------------------------------- /collvoid_stage/params/stage_params.yaml: -------------------------------------------------------------------------------- 1 | /noise_std: 0.0 2 | /covariance: 0.001 3 | /use_sim_time: true 4 | -------------------------------------------------------------------------------- /collvoid_stage/scripts/exp/run_localization_with_radius_and_noerror.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import os 4 | import sys 5 | from subprocess import * 6 | import time 7 | 8 | if __name__ == '__main__': 9 | 10 | # WAIT_FOR_INIT = 10 11 | # MAX_TIME = 40 12 | # RUNS = 2 //set Runs in Watchdog.py in controller 13 | SHUTDOWN_TIME = 10 14 | # MAX_EXPERIMENT_TIME = (WAIT_FOR_INIT + MAX_TIME + 5) * RUNS 15 | 16 | # print "sleep time is: %d"%MAX_EXPERIMENT_TIME 17 | 18 | circ_size = 1.8 # constant 19 | 20 | for num_robots in [8, 7, 6, 5, 4, 3, 2]: 21 | for radius_scaling in [True, False]: 22 | for add_noise in [False]: 23 | 24 | bag_fname = "collvoid_" + str(num_robots) + "_True_" + "rad_scale_" + str(radius_scaling) + "noise_" + str(add_noise) +".bag" 25 | 26 | # print "#robots: %d\t radiuslocalization: %s\t bag-file-name: %s"%(num_robots, str(localization), bag_fname) 27 | 28 | cmd = "./CreateLaunchAndWorld.py" 29 | args = " -n " + str(num_robots) + " -s " + str(circ_size) + " -f " + str(bag_fname) 30 | 31 | if radius_scaling: 32 | args += " -R" 33 | 34 | args += " -x" #add -R for radiusscaling 35 | 36 | if add_noise: 37 | args += " -N" 38 | 39 | 40 | print 'calling "%s" ...'%(cmd) 41 | retcode = call(cmd + args, shell=True) 42 | if retcode < 0: 43 | print "child process was terminated by signal", -retcode 44 | sys.exit(-1) 45 | else: 46 | print "child process returned", retcode 47 | 48 | cmd = "roslaunch" # collvoid_sim control.launch" 49 | args = "launch/control.launch" 50 | print 'calling "%s" ...'%(cmd) 51 | popen = Popen([cmd, args],universal_newlines=True,stdout=None,stderr=PIPE) 52 | done = False 53 | while not done: 54 | output = popen.stderr.readline()[:-1] 55 | print output 56 | 57 | if "I AM DONE" in output: 58 | done = True; 59 | 60 | popen.terminate() 61 | print "-" * 80 62 | 63 | time.sleep(SHUTDOWN_TIME) 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /collvoid_stage/scripts/exp/run_only_ground_truth.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import os 4 | import sys 5 | from subprocess import * 6 | import time 7 | 8 | if __name__ == '__main__': 9 | 10 | # WAIT_FOR_INIT = 10 11 | # MAX_TIME = 40 12 | # RUNS = 2 //set Runs in Watchdog.py in controller 13 | SHUTDOWN_TIME = 10 14 | # MAX_EXPERIMENT_TIME = (WAIT_FOR_INIT + MAX_TIME + 5) * RUNS 15 | 16 | # print "sleep time is: %d"%MAX_EXPERIMENT_TIME 17 | 18 | circ_size = 1.8 # constant 19 | 20 | for num_robots in [8, 7, 6, 5, 4, 3, 2]: 21 | for localization in [False]: 22 | bag_fname = "collvoid_" + str(num_robots) + "_" + str(localization) + "no_rad_scale.bag" 23 | 24 | print "#robots: %d\t localization: %s\t bag-file-name: %s"%(num_robots, str(localization), bag_fname) 25 | 26 | cmd = "./CreateLaunchAndWorld.py" 27 | args = " -n " + str(num_robots) + " -s " + str(circ_size) + " -f " + str(bag_fname) 28 | if not localization: 29 | args += " -l" 30 | 31 | args += " -x" #add -R for radiusscaling 32 | 33 | 34 | 35 | print 'calling "%s" ...'%(cmd) 36 | retcode = call(cmd + args, shell=True) 37 | if retcode < 0: 38 | print "child process was terminated by signal", -retcode 39 | sys.exit(-1) 40 | else: 41 | print "child process returned", retcode 42 | 43 | cmd = "roslaunch" # collvoid_sim control.launch" 44 | args = "launch/control.launch" 45 | print 'calling "%s" ...'%(cmd) 46 | # with open("log.txt","r+") as myFile: 47 | popen = Popen([cmd, args],universal_newlines=True,stdout=None,stderr=PIPE) 48 | done = False 49 | while not done: 50 | output = popen.stderr.readline()[:-1] 51 | #if popen.stdout.readline()[:-1]=='': 52 | # break 53 | # for output in popen.stdout: 54 | # output = popen.stdout.readline() 55 | print output 56 | 57 | if "I AM DONE" in output: 58 | done = True; 59 | #popen.stdout.flush() 60 | # time.sleep(0.1) 61 | 62 | 63 | popen.terminate() 64 | print "-" * 80 65 | 66 | time.sleep(SHUTDOWN_TIME) 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /collvoid_stage/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | ## don't do this unless you want a globally visible script 8 | # scripts=['bin/myscript'], 9 | packages=['PYTHON_PACKAGE_NAME'], 10 | package_dir={'': 'scripts'} 11 | ) 12 | 13 | setup(**d) 14 | -------------------------------------------------------------------------------- /collvoid_stage/world/10x10_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_stage/world/10x10_map.pgm -------------------------------------------------------------------------------- /collvoid_stage/world/10x10_map.yaml: -------------------------------------------------------------------------------- 1 | image: 10x10_map.pgm 2 | resolution: 0.050000 3 | origin: [-5.25000, -5.250000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /collvoid_stage/world/10x10_template.world: -------------------------------------------------------------------------------- 1 | resolution 0.02 # set the resolution of the underlying raytrace model in meters 2 | interval_sim 100 # simulation timestep in milliseconds 3 | #interval_real 100 4 | 5 | window 6 | ( 7 | size [ 958 1179 ] 8 | scale 61.748 9 | ) 10 | 11 | define map model 12 | ( 13 | color "gray30" 14 | boundary 0 15 | gui_nose 0 16 | gui_grid 0 17 | gui_move 0 18 | gui_outline 0 19 | obstacle_return 1 20 | gripper_return 0 21 | fiducial_return 0 22 | ) 23 | 24 | map 25 | ( 26 | name "10x10_map" 27 | size [10.500 10.500 0.500] 28 | pose [0.00 0.0 0.000 0.000] 29 | bitmap "10x10_map.pgm" 30 | ) 31 | 32 | 33 | define hokuyo ranger 34 | ( 35 | sensor( 36 | range [0.0 4.0] 37 | fov 270 38 | samples 270 39 | size [ 0.05 0.05 0.1 ] 40 | ) 41 | color "black" 42 | #ctrl "lasernoise" 43 | ) 44 | 45 | define topurg ranger 46 | ( 47 | sensor( 48 | range [0.0 30.0] 49 | fov 270.25 50 | samples 800 51 | size [ 0.05 0.05 0.1 ] 52 | ) 53 | # generic model properties 54 | color "black" 55 | ) 56 | 57 | 58 | define roomba position 59 | ( 60 | size [0.330 0.330 0.200] 61 | origin [0.000 0.000 0.000 0.000] 62 | gui_nose 1 63 | 64 | # This block approximates the circular shape of a Roomba 65 | topurg( pose [0.000 0.000 0.100 0.000] ) 66 | drive "diff" 67 | bitmap "robot.png" 68 | ) 69 | 70 | 71 | define youbot position 72 | ( 73 | size [0.650 0.350 0.300] 74 | origin [0.000 0.000 0.000 0.000] 75 | gui_nose 1 76 | drive "omni" 77 | wheelbase 0.25 78 | topurg(pose [ 0.000 0.000 0.100 0.000 ]) 79 | ) 80 | 81 | define obst model 82 | ( 83 | size [0.400 0.300 0.400] 84 | origin [0.000 0.000 0.000 0.000] 85 | gui_nose 1 86 | ) 87 | 88 | #obst( pose [ -2.108 3.821 0.000 -95.374 ] name "food" color "red") 89 | # add here robots 90 | #omni_robot( pose [ 8.164 4.633 0.000 -10.755 ] name "robot_0" color "dark slate blue") 91 | #roomba( pose [ -2.755 -1.040 0.000 15.000 ] name "robot_4" color "royal blue") 92 | #roomba( pose [ -2.590 -1.358 0.000 75.000 ] name "robot_5" color "LightSkyBlue") 93 | -------------------------------------------------------------------------------- /collvoid_stage/world/5x5_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_stage/world/5x5_map.pgm -------------------------------------------------------------------------------- /collvoid_stage/world/5x5_map.yaml: -------------------------------------------------------------------------------- 1 | image: 10x10_map.pgm 2 | resolution: 0.0250000 3 | origin: [-2.625000, -2.6250000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /collvoid_stage/world/5x5_template.world: -------------------------------------------------------------------------------- 1 | resolution 0.02 # set the resolution of the underlying raytrace model in meters 2 | interval_sim 100 # simulation timestep in milliseconds 3 | #interval_real 100 4 | 5 | window 6 | ( 7 | size [ 958 1179 ] 8 | scale 61.748 9 | ) 10 | 11 | define map model 12 | ( 13 | color "gray30" 14 | boundary 0 15 | gui_nose 0 16 | gui_grid 0 17 | gui_move 0 18 | gui_outline 0 19 | obstacle_return 1 20 | gripper_return 0 21 | fiducial_return 0 22 | ) 23 | 24 | map 25 | ( 26 | name "5x5_map" 27 | size [5.2500 5.2500 0.500] 28 | pose [0.00 0.0 0.000 0.000] 29 | bitmap "10x10_map.pgm" 30 | ) 31 | 32 | 33 | define hokuyo ranger 34 | ( 35 | sensor( 36 | range [0.0 4.0] 37 | fov 200 38 | samples 270 39 | size [ 0.05 0.05 0.1 ] 40 | ) 41 | color "black" 42 | #ctrl "lasernoise" 43 | ) 44 | 45 | define topurg ranger 46 | ( 47 | sensor( 48 | range [0.0 30.0] 49 | fov 270.25 50 | samples 800 51 | size [ 0.05 0.05 0.1 ] 52 | ) 53 | # generic model properties 54 | color "black" 55 | ) 56 | 57 | 58 | define roomba position 59 | ( 60 | size [0.330 0.330 0.200] 61 | origin [0.000 0.000 0.000 0.000] 62 | gui_nose 1 63 | 64 | # This block approximates the circular shape of a Roomba 65 | hokuyo( pose [0.000 0.000 0.100 0.000] ) 66 | drive "diff" 67 | bitmap "robot.png" 68 | ) 69 | 70 | 71 | define youbot position 72 | ( 73 | size [0.650 0.350 0.300] 74 | origin [0.000 0.000 0.000 0.000] 75 | gui_nose 1 76 | drive "omni" 77 | wheelbase 0.25 78 | topurg(pose [ 0.000 0.000 0.100 0.000 ]) 79 | ) 80 | 81 | define obst model 82 | ( 83 | size [0.400 0.400 0.400] 84 | origin [0.000 0.000 0.000 0.000] 85 | gui_nose 1 86 | ) 87 | 88 | #obst( pose [ 0 0 0.000 -95.374 ] name "obst_0" color "red") 89 | #obst( pose [ 0 0 0.000 -95.374 ] name "obst_1" color "red") 90 | # add here robots 91 | #omni_robot( pose [ 8.164 4.633 0.000 -10.755 ] name "robot_0" color "dark slate blue") 92 | #roomba( pose [ -2.755 -1.040 0.000 15.000 ] name "robot_4" color "royal blue") 93 | #roomba( pose [ -2.590 -1.358 0.000 75.000 ] name "robot_5" color "LightSkyBlue") 94 | -------------------------------------------------------------------------------- /collvoid_stage/world/maplarge.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_stage/world/maplarge.pgm -------------------------------------------------------------------------------- /collvoid_stage/world/maplarge.yaml: -------------------------------------------------------------------------------- 1 | image: maplarge.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /collvoid_stage/world/robot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_stage/world/robot.png -------------------------------------------------------------------------------- /collvoid_stage/world/swarmlab_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_stage/world/swarmlab_map.pgm -------------------------------------------------------------------------------- /collvoid_stage/world/swarmlab_map.yaml: -------------------------------------------------------------------------------- 1 | image: swarmlab_map.pgm 2 | resolution: 0.050000 3 | origin: [-5.4500000, -8.8750000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /collvoid_stage/world/swarmlab_single_pr2.world: -------------------------------------------------------------------------------- 1 | resolution 0.02 # set the resolution of the underlying raytrace model in meters 2 | interval_sim 100 # simulation timestep in milliseconds 3 | #interval_real 100 4 | 5 | window 6 | ( 7 | size [ 800.000 600.000 ] 8 | scale 28.806 9 | ) 10 | 11 | define map model 12 | ( 13 | color "gray30" 14 | boundary 0 15 | gui_nose 0 16 | gui_grid 0 17 | gui_move 0 18 | gui_outline 0 19 | obstacle_return 1 20 | gripper_return 0 21 | fiducial_return 0 22 | laser_return 1 23 | ) 24 | 25 | map 26 | ( 27 | name "swarmlab_map" 28 | size [10.9 17.75 0.5] 29 | pose [0 0 0 90] 30 | bitmap "swarmlab_map.pgm" 31 | ) 32 | 33 | 34 | define hokuyo ranger 35 | ( 36 | sensor( 37 | range [0.0 4.0] 38 | fov 270 39 | samples 270 40 | size [ 0.05 0.05 0.1 ] 41 | ) 42 | color "black" 43 | #ctrl "lasernoise" 44 | ) 45 | 46 | define roomba position 47 | ( 48 | size [0.33 0.33 0.2] 49 | origin [0 0 0 0] 50 | gui_nose 1 51 | 52 | # This block approximates the circular shape of a Roomba 53 | hokuyo( pose [0 0 0.1 0] ) 54 | drive "diff" 55 | bitmap "robot.png" 56 | ) 57 | 58 | define topurg ranger 59 | ( 60 | sensor( 61 | range[ 0.0 30.0 ] 62 | fov 270.25 63 | samples 800 64 | size [ 0.05 0.05 0.1 ] 65 | 66 | ) 67 | # generic model properties 68 | color "black" 69 | ) 70 | 71 | define diff_robot position 72 | ( 73 | size [0.34 0.34 0.3] 74 | origin [0 0 0 0] 75 | gui_nose 1 76 | drive "diff" 77 | wheelbase 0.25 78 | topurg(pose [ 0.0 0.000 0 0.000 ]) 79 | bitmap "robot.png" 80 | 81 | ) 82 | 83 | define omni_robot position 84 | ( 85 | size [0.34 0.34 0.3] 86 | origin [0 0 0 0] 87 | gui_nose 1 88 | drive "omni" 89 | wheelbase 0.25 90 | topurg(pose [ 0.0 0.000 0 0.000 ]) 91 | ) 92 | 93 | define pr2 position 94 | ( 95 | size [0.65 0.65 0.1] 96 | origin [0 0 0 0] 97 | gui_nose 1 98 | 99 | topurg( pose [0.275 0 0.1 0] ) 100 | drive "omni" 101 | 102 | ) 103 | 104 | 105 | define stick position 106 | ( 107 | size [0.6 0.4 0.1] 108 | origin [0 0 0 0] 109 | gui_nose 1 110 | 111 | 112 | hokuyo( pose [0.125 0 0.1 0] ) 113 | drive "omni" 114 | 115 | ) 116 | define obst model 117 | ( 118 | size [0.4 0.4 0.4] 119 | origin [0 0 0 0] 120 | gui_nose 1 121 | ) 122 | 123 | #obst( pose [ 0.0 0.0 0 45.000000 ] name "block" color "black") 124 | 125 | # robots added here automatically 126 | pr2( pose [ -0.0 0.0 0 0.000000 ] name "robot_0" color "blue") 127 | -------------------------------------------------------------------------------- /collvoid_stage/world/swarmlab_single_turtle.world: -------------------------------------------------------------------------------- 1 | resolution 0.02 # set the resolution of the underlying raytrace model in meters 2 | interval_sim 100 # simulation timestep in milliseconds 3 | #interval_real 100 4 | 5 | window 6 | ( 7 | size [ 800.000 600.000 ] 8 | scale 28.806 9 | ) 10 | 11 | define map model 12 | ( 13 | color "gray30" 14 | boundary 0 15 | gui_nose 0 16 | gui_grid 0 17 | gui_move 0 18 | gui_outline 0 19 | obstacle_return 0 20 | gripper_return 0 21 | fiducial_return 0 22 | laser_return 1 23 | ) 24 | 25 | map 26 | ( 27 | name "swarmlab_map" 28 | size [10.9 17.75 0.5] 29 | pose [0 0 0 0] 30 | bitmap "swarmlab_map.pgm" 31 | ) 32 | 33 | 34 | define hokuyo ranger 35 | ( 36 | sensor( 37 | range [0.0 4.0] 38 | fov 270 39 | samples 270 40 | size [ 0.05 0.05 0.1 ] 41 | ) 42 | color "black" 43 | #ctrl "lasernoise" 44 | ) 45 | 46 | define roomba position 47 | ( 48 | size [0.33 0.33 0.2] 49 | origin [0 0 0 0] 50 | gui_nose 1 51 | 52 | # This block approximates the circular shape of a Roomba 53 | hokuyo( pose [0 0 0.1 0] ) 54 | drive "diff" 55 | bitmap "robot.png" 56 | ) 57 | 58 | define topurg ranger 59 | ( 60 | sensor( 61 | range[ 0.0 30.0 ] 62 | fov 270.25 63 | samples 800 64 | size [ 0.05 0.05 0.1 ] 65 | 66 | ) 67 | # generic model properties 68 | color "black" 69 | ) 70 | 71 | define diff_robot position 72 | ( 73 | size [0.34 0.34 0.3] 74 | origin [0 0 0 0] 75 | gui_nose 1 76 | drive "diff" 77 | wheelbase 0.25 78 | topurg(pose [ 0.0 0.000 0 0.000 ]) 79 | bitmap "robot.png" 80 | 81 | ) 82 | 83 | define omni_robot position 84 | ( 85 | size [0.34 0.34 0.3] 86 | origin [0 0 0 0] 87 | gui_nose 1 88 | drive "omni" 89 | wheelbase 0.25 90 | topurg(pose [ 0.0 0.000 0 0.000 ]) 91 | ) 92 | 93 | define pr2 position 94 | ( 95 | size [0.65 0.65 0.1] 96 | origin [0 0 0 0] 97 | gui_nose 1 98 | 99 | topurg( pose [0.275 0 0.1 0] ) 100 | drive "omni" 101 | 102 | ) 103 | 104 | 105 | define stick position 106 | ( 107 | size [0.6 0.4 0.1] 108 | origin [0 0 0 0] 109 | gui_nose 1 110 | 111 | 112 | hokuyo( pose [0.125 0 0.1 0] ) 113 | drive "omni" 114 | 115 | ) 116 | define obst model 117 | ( 118 | size [0.4 0.4 0.4] 119 | origin [0 0 0 0] 120 | gui_nose 1 121 | ) 122 | 123 | #obst( pose [ 0.0 0.0 0 45.000000 ] name "block" color "black") 124 | 125 | # robots added here automatically 126 | roomba( pose [ -0.0 0.0 0 0.000000 ] name "robot_0" color "blue") 127 | -------------------------------------------------------------------------------- /collvoid_stage/world/swarmlab_template.world: -------------------------------------------------------------------------------- 1 | resolution 0.02 # set the resolution of the underlying raytrace model in meters 2 | interval_sim 50 # simulation timestep in milliseconds 3 | #interval_real 100 4 | 5 | window 6 | ( 7 | size [ 800.000 600.000 ] 8 | scale 28.806 9 | ) 10 | 11 | define map model 12 | ( 13 | color "gray30" 14 | boundary 0 15 | gui_nose 0 16 | gui_grid 0 17 | gui_move 0 18 | gui_outline 0 19 | obstacle_return 1 20 | gripper_return 0 21 | fiducial_return 0 22 | ) 23 | 24 | map 25 | ( 26 | name "swarmlab_map" 27 | size [10.9 17.75 0.5] 28 | pose [0 0 0 0] 29 | bitmap "swarmlab_map.pgm" 30 | ) 31 | 32 | 33 | define hokuyo ranger 34 | ( 35 | sensor( 36 | range [0.0 4.0] 37 | fov 270 38 | samples 270 39 | size [ 0.05 0.05 0.1 ] 40 | ) 41 | color "black" 42 | #ctrl "lasernoise" 43 | ) 44 | 45 | define roomba position 46 | ( 47 | size [0.33 0.33 0.2] 48 | origin [0 0 0 0] 49 | gui_nose 1 50 | 51 | # This block approximates the circular shape of a Roomba 52 | hokuyo( pose [0 0 0.1 0] ) 53 | drive "diff" 54 | bitmap "robot.png" 55 | ) 56 | 57 | define topurg ranger 58 | ( 59 | sensor( 60 | range [0.0 30.0] 61 | fov 270.25 62 | samples 800 63 | size [ 0.05 0.05 0.1 ] 64 | ) 65 | # generic model properties 66 | color "black" 67 | ) 68 | 69 | define diff_robot position 70 | ( 71 | size [0.34 0.34 0.3] 72 | origin [0 0 0 0] 73 | gui_nose 1 74 | drive "diff" 75 | wheelbase 0.25 76 | topurg(pose [ 0.0 0.000 0 0.000 ]) 77 | bitmap "robot.png" 78 | 79 | ) 80 | 81 | define omni_robot position 82 | ( 83 | size [0.34 0.34 0.3] 84 | origin [0 0 0 0] 85 | gui_nose 1 86 | drive "omni" 87 | wheelbase 0.25 88 | topurg(pose [ 0.0 0.000 0 0.000 ]) 89 | ) 90 | 91 | define pr2 position 92 | ( 93 | size [0.50 0.45 0.1] 94 | origin [0 0 0 0] 95 | gui_nose 1 96 | topurg( pose [0.275 0 0.1 0] ) 97 | drive "omni" 98 | ) 99 | 100 | 101 | define stick position 102 | ( 103 | size [0.6 0.4 0.1] 104 | origin [0 0 0 0] 105 | gui_nose 1 106 | 107 | hokuyo( pose [0.125 0 0.1 0] ) 108 | drive "omni" 109 | 110 | ) 111 | define obst model 112 | ( 113 | size [0.4 0.4 0.4] 114 | origin [0 0 0 0] 115 | gui_nose 1 116 | ) 117 | 118 | #obst( pose [ 0.0 0.0 0 45.000000 ] name "block" color "black") 119 | 120 | # robots added here automatically 121 | -------------------------------------------------------------------------------- /collvoid_stage/world/willow_full.world: -------------------------------------------------------------------------------- 1 | resolution 0.02 # set the resolution of the underlying raytrace model in meters 2 | interval_sim 100 # simulation timestep in milliseconds 3 | #interval_real 100 4 | 5 | window 6 | ( 7 | size [ 800.000 600.000 ] 8 | scale 28.806 9 | ) 10 | 11 | define map model 12 | ( 13 | color "gray30" 14 | boundary 0 15 | gui_nose 0 16 | gui_grid 0 17 | gui_move 0 18 | gui_outline 0 19 | obstacle_return 1 20 | gripper_return 0 21 | fiducial_return 0 22 | laser_return 1 23 | ) 24 | 25 | map 26 | ( 27 | name "wilow_map" 28 | size [99.200 99.200 0.500] 29 | pose [0 0 0 0] 30 | origin [-0.400 -0.400 0 0] 31 | 32 | bitmap "maplarge.pgm" 33 | ) 34 | 35 | define hokuyo ranger 36 | ( 37 | sensor( 38 | range [0.0 4.0] 39 | fov 270 40 | samples 270 41 | size [ 0.05 0.05 0.1 ] 42 | ) 43 | color "black" 44 | #ctrl "lasernoise" 45 | ) 46 | 47 | define roomba position 48 | ( 49 | size [0.33 0.33 0.2] 50 | origin [0 0 0 0] 51 | gui_nose 1 52 | 53 | # This block approximates the circular shape of a Roomba 54 | hokuyo( pose [0 0 0.1 0] ) 55 | drive "diff" 56 | bitmap "robot.png" 57 | ) 58 | 59 | define topurg ranger 60 | ( 61 | sensor( 62 | range[ 0.0 30.0 ] 63 | fov 270.25 64 | samples 800 65 | size [ 0.05 0.05 0.1 ] 66 | 67 | ) 68 | # generic model properties 69 | color "black" 70 | ) 71 | 72 | define diff_robot position 73 | ( 74 | size [0.34 0.34 0.3] 75 | origin [0 0 0 0] 76 | gui_nose 1 77 | drive "diff" 78 | wheelbase 0.25 79 | topurg(pose [ 0.0 0.000 0 0.000 ]) 80 | bitmap "robot.png" 81 | 82 | ) 83 | 84 | define omni_robot position 85 | ( 86 | size [0.34 0.34 0.3] 87 | origin [0 0 0 0] 88 | gui_nose 1 89 | drive "omni" 90 | wheelbase 0.25 91 | topurg(pose [ 0.0 0.000 0 0.000 ]) 92 | ) 93 | 94 | define pr2 position 95 | ( 96 | size [0.65 0.65 0.1] 97 | origin [0 0 0 0] 98 | gui_nose 1 99 | 100 | topurg( pose [0.275 0 0.1 0] ) 101 | drive "omni" 102 | 103 | ) 104 | 105 | 106 | define stick position 107 | ( 108 | size [0.6 0.4 0.1] 109 | origin [0 0 0 0] 110 | gui_nose 1 111 | 112 | 113 | hokuyo( pose [0.125 0 0.1 0] ) 114 | drive "omni" 115 | 116 | ) 117 | define obst model 118 | ( 119 | size [0.4 0.4 0.4] 120 | origin [0 0 0 0] 121 | gui_nose 1 122 | ) 123 | 124 | #obst( pose [ 0.0 0.0 0 45.000000 ] name "block" color "black") 125 | 126 | # robots added here automatically 127 | pr2( pose [ 6.023 3.057 0 4.807 ] name "robot_0" color "dark slate blue") 128 | 129 | -------------------------------------------------------------------------------- /collvoid_turtlebot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(collvoid_turtlebot) 3 | find_package(catkin REQUIRED ) 4 | 5 | catkin_package( 6 | DEPENDS # TODO add dependencies 7 | CATKIN_DEPENDS # TODO 8 | INCLUDE_DIRS # TODO include 9 | LIBRARIES # TODO 10 | ) -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/amcl_turtlebot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/gmapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/hokuyo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/includes/laser_detectors.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/includes/lcm_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/includes/lcm_turtlebot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/includes/leg_detectors.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/includes/move_base_collvoid.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | - 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/includes/move_base_collvoid_legacy.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | - 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/includes/move_base_dwa.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 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/minimal.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 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/tracking_on_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/turtlebot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/turtlebot_master.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 | -------------------------------------------------------------------------------- /collvoid_turtlebot/launch/unused/master_sync.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /collvoid_turtlebot/map/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_turtlebot/map/map.pgm -------------------------------------------------------------------------------- /collvoid_turtlebot/map/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-12.200000, -17.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /collvoid_turtlebot/map/smartlab_arena.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_turtlebot/map/smartlab_arena.pgm -------------------------------------------------------------------------------- /collvoid_turtlebot/map/smartlab_arena.yaml: -------------------------------------------------------------------------------- 1 | image: smartlab_arena.pgm 2 | resolution: 0.050000 3 | origin: [-15.400000, -17.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /collvoid_turtlebot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | collvoid_turtlebot 3 | 1.0.0 4 | collvoid_turtlebot 5 | Daniel Claes 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/collvoid_turtlebot 10 | 11 | 12 | Daniel Claes 13 | 14 | 15 | catkin 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /collvoid_turtlebot/params/collvoid_common.yaml: -------------------------------------------------------------------------------- 1 | ## For collvoid_scoring_function and me publisher 2 | global_frame_id: map 3 | # base_frame: /robot_0/base_link 4 | max_dist_vo: 1. #for scoring function scale 5 | trunc_time: 10.0 6 | 7 | use_truncation: true 8 | use_polygon_footprint: true # true #footprint or radius 9 | 10 | -------------------------------------------------------------------------------- /collvoid_turtlebot/params/collvoid_config.yaml: -------------------------------------------------------------------------------- 1 | 2 | CollvoidLocalPlanner: 3 | use_obstacles: true 4 | use_dwa_scoring: true 5 | prune_plan: true 6 | 7 | time_to_holo: 0.4 8 | min_error_holo: 0.02 9 | max_error_holo: 0.10 10 | left_pref: -0.05 11 | 12 | type_vo: 0 #HRVO = 0, RVO = 1, VO = 2 13 | orca: false #orca or VO 14 | clearpath: true #clearpath or sampling 15 | num_samples: 400 #num samples 16 | new_sampling: true 17 | use_truncation: true #truncate vos -------------------------------------------------------------------------------- /collvoid_turtlebot/params/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | max_obstacle_height: 0.60 # assume something like an arm is mounted on top of the robot 2 | 3 | # Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation) 4 | robot_radius: 0.20 # distance a circular robot should be clear of the obstacle (kobuki: 0.18) 5 | # footprint: [[x0, y0], [x1, y1], ... [xn, yn]] # if the robot is not circular 6 | 7 | map_type: voxel 8 | 9 | obstacle_layer: 10 | enabled: true 11 | max_obstacle_height: 0.6 12 | origin_z: 0.0 13 | z_resolution: 0.2 14 | z_voxels: 2 15 | unknown_threshold: 15 16 | mark_threshold: 0 17 | combination_method: 1 18 | track_unknown_space: true #true needed for disabling global path planning through unknown space 19 | obstacle_range: 2.5 20 | raytrace_range: 3.0 21 | origin_z: 0.0 22 | z_resolution: 0.2 23 | z_voxels: 2 24 | publish_voxel_map: false 25 | observation_sources: scan #bump 26 | scan: 27 | sensor_frame: /laser 28 | data_type: LaserScan 29 | topic: scan_filtered 30 | marking: true 31 | clearing: true 32 | min_obstacle_height: 0.0 33 | max_obstacle_height: 0.35 34 | bump: 35 | data_type: PointCloud2 36 | topic: mobile_base/sensors/bumper_pointcloud 37 | marking: true 38 | clearing: false 39 | min_obstacle_height: 0.0 40 | max_obstacle_height: 0.15 41 | # for debugging only, let's you see the entire voxel grid 42 | 43 | #cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns 44 | inflation_layer: 45 | enabled: true 46 | cost_scaling_factor: 5.0 # exponential rate at which the obstacle cost drops off (default: 10) 47 | inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths. 48 | 49 | static_layer: 50 | enabled: true 51 | 52 | 53 | -------------------------------------------------------------------------------- /collvoid_turtlebot/params/costmap_common_turtle.yaml: -------------------------------------------------------------------------------- 1 | map_type: costmap 2 | #Set the tolerance we're willing to have for tf transforms 3 | transform_tolerance: 0.3 4 | 5 | #footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] 6 | robot_radius: 0.22 7 | 8 | footprint_padding: 0.0 9 | 10 | static_layer: 11 | enabled: true 12 | 13 | obstacle_layer: 14 | enabled: true 15 | max_obstacle_height: 2.0 16 | origin_z: 0 17 | z_resolution: 2 18 | z_voxels: 1 19 | unknown_threshold: 15 20 | mark_threshold: 0 21 | combination_method: 1 22 | track_unknown_space: true #true needed for disabling global path planning through unknown space 23 | obstacle_range: 2.5 24 | raytrace_range: 3.0 25 | publish_voxel_map: true 26 | observation_sources: scan #bump 27 | scan: 28 | sensor_frame: laser 29 | data_type: LaserScan 30 | topic: scan_filtered 31 | marking: true 32 | clearing: true 33 | min_obstacle_height: 0.0 34 | max_obstacle_height: 0.35 35 | 36 | stationary_robots: 37 | enabled: true 38 | max_obstacle_height: 2.0 39 | obstacle_range: 2 40 | raytrace_range: 4.0 41 | publish_voxel_map: true 42 | 43 | observation_sources: python_node python_node_clearing 44 | python_node_clearing: 45 | data_type: LaserScan 46 | topic: clearing_scan 47 | observation_persistence: 0.0 48 | marking: false 49 | clearing: true 50 | 51 | python_node: 52 | data_type: PointCloud2 53 | topic: stationary_robots 54 | observation_persistence: 0.0 55 | marking: true 56 | clearing: false 57 | -------------------------------------------------------------------------------- /collvoid_turtlebot/params/me_publisher_turtle.yaml: -------------------------------------------------------------------------------- 1 | collvoid: 2 | holo_robot: false 3 | controlled: true 4 | eps: 0.7 5 | publish_me_frequency: 10 6 | -------------------------------------------------------------------------------- /collvoid_turtlebot/params/multimaster_config.yaml: -------------------------------------------------------------------------------- 1 | sync_topics: [/position_share, /commands_robot] 2 | sync_remote_node: False -------------------------------------------------------------------------------- /collvoid_turtlebot/params/stage_params.yaml: -------------------------------------------------------------------------------- 1 | /noise_std: 0.0 2 | /covariance: 0.001 3 | /use_sim_time: true 4 | -------------------------------------------------------------------------------- /collvoid_turtlebot/scripts/lcm_relay_share.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | __author__ = 'daniel' 3 | 4 | import rospy 5 | import importlib 6 | from socket import gethostname 7 | 8 | import lcm 9 | import StringIO 10 | 11 | # from msg import communication_msg 12 | 13 | lc = None 14 | topic = '/position_share' 15 | lcm_topic ='tb' 16 | msg_name = 'PoseTwistWithCovariance' 17 | package = 'collvoid_msgs.msg' 18 | 19 | module = importlib.import_module(package) 20 | TYPE = getattr(module, msg_name) 21 | 22 | 23 | name = None 24 | lcm_sub = None 25 | pub = None 26 | 27 | 28 | def init_globals(): 29 | global lc, name, pub, lcm_sub 30 | lc = lcm.LCM("udpm://224.1.1.1:5007?ttl=2") 31 | name = gethostname() 32 | print name 33 | #lcm_sub = lc.subscribe(name, udp_callback) 34 | #if 'tb' in name: 35 | lcm_sub = lc.subscribe(lcm_topic, udp_callback) 36 | pub = rospy.Publisher(topic, TYPE, queue_size=1) 37 | rospy.Subscriber(topic, TYPE, handle_msg) 38 | 39 | 40 | def udp_callback(channel, data): 41 | msg = TYPE() 42 | msg.deserialize(data) 43 | if msg.robot_id != name: 44 | pub.publish(msg) 45 | 46 | 47 | def handle_msg(msg): 48 | if msg.robot_id != name: 49 | return 50 | send(msg) 51 | 52 | 53 | def main(): 54 | rospy.init_node('share_lcm_relay') 55 | init_globals() 56 | 57 | while not rospy.is_shutdown(): 58 | lc.handle() 59 | lc.unsubscribe(lcm_sub) 60 | 61 | 62 | def send(msg, repeats=1): 63 | buff = StringIO.StringIO() 64 | msg.serialize(buff) 65 | for i in xrange(repeats): 66 | lc.publish(lcm_topic, buff.getvalue()) 67 | 68 | 69 | if __name__ == '__main__': 70 | main() 71 | -------------------------------------------------------------------------------- /collvoid_turtlebot/scripts/lcm_ros_relay.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | __author__ = 'daniel' 3 | 4 | import rospy 5 | import importlib 6 | from socket import gethostname 7 | 8 | import lcm 9 | import StringIO 10 | 11 | # from msg import communication_msg 12 | 13 | lc = None 14 | lcm_channel = None 15 | TYPE = None 16 | name = None 17 | lcm_sub = None 18 | pub = None 19 | id = None 20 | module_type = None 21 | 22 | 23 | def init_globals(): 24 | global lc, name, pub, lcm_sub, lcm_channel, TYPE, module_type, id 25 | lc = lcm.LCM("udpm://224.1.1.1:5007?ttl=2") 26 | name = gethostname() 27 | # lcm_sub = lc.subscribe(name, udp_callback) 28 | # if 'tb' in name: 29 | topic = rospy.get_param('~topic') 30 | lcm_channel = rospy.get_param('~lcm_channel', topic) 31 | 32 | msg_package = rospy.get_param('~msg_package') 33 | msg_name = rospy.get_param('~msg_name') 34 | module_type = rospy.get_param('~module_type') 35 | 36 | module = importlib.import_module(msg_package) 37 | TYPE = getattr(module, msg_name) 38 | id = rospy.get_param('~id', 'robot_id') 39 | 40 | if module_type in ['receiver', 'transceiver']: 41 | lcm_sub = lc.subscribe(lcm_channel, udp_callback) 42 | pub = rospy.Publisher(topic, TYPE, queue_size=1) 43 | 44 | if module_type in ['sender', 'transceiver']: 45 | rospy.Subscriber(topic, TYPE, handle_msg) 46 | 47 | 48 | def udp_callback(channel, data): 49 | msg = TYPE() 50 | msg.deserialize(data) 51 | if module_type == 'transceiver' and getattr(msg, id) == name: 52 | return 53 | pub.publish(msg) 54 | 55 | 56 | def handle_msg(msg): 57 | if module_type == 'transceiver' and getattr(msg, id) != name: 58 | return 59 | send(msg) 60 | 61 | 62 | def main(): 63 | rospy.init_node('share_lcm_relay') 64 | init_globals() 65 | while not rospy.is_shutdown(): 66 | lc.handle() 67 | lc.unsubscribe(lcm_sub) 68 | 69 | 70 | def send(msg, repeats=1): 71 | buff = StringIO.StringIO() 72 | msg.serialize(buff) 73 | for i in xrange(repeats): 74 | lc.publish(lcm_channel, buff.getvalue()) 75 | 76 | 77 | if __name__ == '__main__': 78 | main() 79 | -------------------------------------------------------------------------------- /collvoid_turtlebot/scripts/make_goals.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import math 4 | import sys 5 | import getopt 6 | import commands 7 | 8 | def create_world_file(argv): 9 | offset = 0 10 | try: 11 | opts, args= getopt.getopt(argv, "hx:y:n:s:o:", ["help","x=","y=","num=","size=","offset="]) 12 | except getopt.GetoptError: 13 | print 'make_goals.py -x -y -n ' 14 | sys.exit(2) 15 | for opt, arg in opts: 16 | if opt == '-h': 17 | print 'make_goals.py -n -s <-h> <-l> <-x> <-f> bagfile' 18 | sys.exit(2) 19 | elif opt in ("-n","--num"): 20 | num = int(arg) 21 | elif opt in ("-o","--offset"): 22 | offset = int(arg) 23 | elif opt in ("-x","--x"): 24 | x = float(arg) 25 | elif opt in ("-y","--y"): 26 | y = float(arg) 27 | elif opt in ("-s","--size"): 28 | size = float(arg) 29 | if not num or not x or not y or not x or not size: 30 | print 'make_goals.py -n -s <-h> <-l> <-x> <-f> bagfile' 31 | sys.exit(2) 32 | 33 | create_goal_file(size,num,x,y, offset) 34 | 35 | def create_goal_file(circleSize, numRobots, centerX, centerY, offset=0): 36 | yamlWrite = open('goals_created.yaml','w') 37 | 38 | angle = 360.0 / numRobots 39 | for x in range(numRobots): 40 | angleX = offset + x * angle 41 | posX = circleSize*math.cos(angleX/360*2*math.pi) 42 | posY = circleSize*math.sin(angleX/360*2*math.pi) 43 | 44 | yamlWrite.write('tb_{0}:\n'.format(x)) 45 | yamlWrite.write(' goals:\n') 46 | yamlWrite.write(' x: [{0:f}, {1:f}]\n'.format(centerX+posX, centerX-posX)) 47 | yamlWrite.write(' y: [{0:f}, {1:f}]\n'.format(centerY+posY, centerY-posY)) 48 | yamlWrite.write(' ang: [{0:f}, {1:f}]\n'.format((angleX-180) / 360.0 * 2 * math.pi,(angleX) / 360.0 * 2 * math.pi)) 49 | 50 | yamlWrite.close() 51 | 52 | 53 | if __name__ == "__main__": 54 | create_world_file(sys.argv[1:]) 55 | 56 | -------------------------------------------------------------------------------- /collvoid_turtlebot/world/aamas.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_turtlebot/world/aamas.pgm -------------------------------------------------------------------------------- /collvoid_turtlebot/world/aamas.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-20.000000, -32.800000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /collvoid_turtlebot/world/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_turtlebot/world/map.pgm -------------------------------------------------------------------------------- /collvoid_turtlebot/world/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-5.5000000, -5.500000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /collvoid_turtlebot/world/maplarge.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_turtlebot/world/maplarge.pgm -------------------------------------------------------------------------------- /collvoid_turtlebot/world/maplarge.yaml: -------------------------------------------------------------------------------- 1 | image: maplarge.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /collvoid_turtlebot/world/robot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_turtlebot/world/robot.png -------------------------------------------------------------------------------- /collvoid_turtlebot/world/smartlab_single_turtle.world: -------------------------------------------------------------------------------- 1 | resolution 0.02 # set the resolution of the underlying raytrace model in meters 2 | interval_sim 100 # simulation timestep in milliseconds 3 | #interval_real 100 4 | 5 | window 6 | ( 7 | size [ 800.000 600.000 ] 8 | scale 28.806 9 | ) 10 | 11 | define map model 12 | ( 13 | color "gray30" 14 | boundary 0 15 | gui_nose 0 16 | gui_grid 0 17 | gui_move 0 18 | gui_outline 0 19 | obstacle_return 1 20 | gripper_return 0 21 | fiducial_return 0 22 | laser_return 1 23 | ) 24 | 25 | map 26 | ( 27 | name "map" 28 | size [10.5 10.5 0.5] 29 | pose [0 0 0 90] 30 | bitmap "map.pgm" 31 | ) 32 | 33 | 34 | define hokuyo ranger 35 | ( 36 | sensor( 37 | range [0.0 4.0] 38 | fov 270 39 | samples 270 40 | size [ 0.05 0.05 0.1 ] 41 | ) 42 | color "black" 43 | ctrl "lasernoise" 44 | ) 45 | 46 | define roomba position 47 | ( 48 | size [0.33 0.33 0.2] 49 | origin [0 0 0 0] 50 | gui_nose 1 51 | 52 | # This block approximates the circular shape of a Roomba 53 | hokuyo( pose [0 0 0.1 0] ) 54 | drive "diff" 55 | bitmap "robot.png" 56 | ) 57 | 58 | define topurg ranger 59 | ( 60 | sensor( 61 | range[ 0.0 30.0 ] 62 | fov 270.25 63 | samples 800 64 | size [ 0.05 0.05 0.1 ] 65 | 66 | ) 67 | # generic model properties 68 | color "black" 69 | ) 70 | 71 | define diff_robot position 72 | ( 73 | size [0.34 0.34 0.3] 74 | origin [0 0 0 0] 75 | gui_nose 1 76 | drive "diff" 77 | wheelbase 0.25 78 | topurg(pose [ 0.0 0.000 0 0.000 ]) 79 | bitmap "robot.png" 80 | 81 | ) 82 | 83 | define omni_robot position 84 | ( 85 | size [0.34 0.34 0.3] 86 | origin [0 0 0 0] 87 | gui_nose 1 88 | drive "omni" 89 | wheelbase 0.25 90 | topurg(pose [ 0.0 0.000 0 0.000 ]) 91 | ) 92 | 93 | define pr2 position 94 | ( 95 | size [0.65 0.65 0.1] 96 | origin [0 0 0 0] 97 | gui_nose 1 98 | 99 | topurg( pose [0.275 0 0.1 0] ) 100 | drive "omni" 101 | 102 | ) 103 | 104 | 105 | define stick position 106 | ( 107 | size [0.6 0.4 0.1] 108 | origin [0 0 0 0] 109 | gui_nose 1 110 | 111 | 112 | hokuyo( pose [0.125 0 0.1 0] ) 113 | drive "omni" 114 | 115 | ) 116 | define obst model 117 | ( 118 | size [0.4 0.4 0.4] 119 | origin [0 0 0 0] 120 | gui_nose 1 121 | ) 122 | 123 | #obst( pose [ 0.0 0.0 0 45.000000 ] name "block" color "black") 124 | 125 | # robots added here automatically 126 | roomba( pose [ -0.0 0.0 0 0.000000 ] name "robot_0" color "blue") 127 | -------------------------------------------------------------------------------- /collvoid_turtlebot/world/swarmlab.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_turtlebot/world/swarmlab.pgm -------------------------------------------------------------------------------- /collvoid_turtlebot/world/swarmlab.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-1.000000, -1.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /collvoid_turtlebot/world/swarmlab_rev2012.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/collvoid_turtlebot/world/swarmlab_rev2012.pgm -------------------------------------------------------------------------------- /collvoid_turtlebot/world/swarmlab_rev2012.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-5.4500000, -8.8750000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /collvoid_wiggle_recovery/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package collvoid_wiggle_recovery 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.12.13 (2016-08-15) 6 | -------------------- 7 | 8 | 1.12.12 (2016-06-24) 9 | -------------------- 10 | 11 | 1.12.11 (2016-06-08) 12 | -------------------- 13 | 14 | 1.12.10 (2016-05-27) 15 | -------------------- 16 | 17 | 1.12.9 (2016-05-26) 18 | ------------------- 19 | 20 | 1.12.8 (2016-05-16) 21 | ------------------- 22 | 23 | 1.12.7 (2016-01-05) 24 | ------------------- 25 | 26 | 1.12.6 (2016-01-02) 27 | ------------------- 28 | 29 | 1.12.5 (2015-10-29) 30 | ------------------- 31 | 32 | 1.12.4 (2015-06-03) 33 | ------------------- 34 | 35 | 1.12.3 (2015-04-30) 36 | ------------------- 37 | 38 | 1.12.2 (2015-03-31) 39 | ------------------- 40 | 41 | 1.12.1 (2015-03-14) 42 | ------------------- 43 | 44 | 1.12.0 (2015-02-04) 45 | ------------------- 46 | * update maintainer email 47 | * Contributors: Michael Ferguson 48 | 49 | 1.11.15 (2015-02-03) 50 | -------------------- 51 | * Add ARCHIVE_DESTINATION for static builds 52 | * Contributors: Gary Servin 53 | 54 | 1.11.14 (2014-12-05) 55 | -------------------- 56 | 57 | 1.11.13 (2014-10-02) 58 | -------------------- 59 | 60 | 1.11.12 (2014-10-01) 61 | -------------------- 62 | 63 | 1.11.11 (2014-07-23) 64 | -------------------- 65 | 66 | 1.11.10 (2014-06-25) 67 | -------------------- 68 | 69 | 1.11.9 (2014-06-10) 70 | ------------------- 71 | 72 | 1.11.8 (2014-05-21) 73 | ------------------- 74 | 75 | 1.11.7 (2014-05-21) 76 | ------------------- 77 | * update build to find eigen using cmake_modules 78 | * Contributors: Michael Ferguson 79 | 80 | 1.11.4 (2013-09-27) 81 | ------------------- 82 | * Package URL Updates 83 | -------------------------------------------------------------------------------- /collvoid_wiggle_recovery/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(collvoid_wiggle_recovery) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | cmake_modules 7 | roscpp 8 | tf 9 | costmap_2d 10 | nav_core 11 | pluginlib 12 | base_local_planner 13 | ) 14 | 15 | find_package(Eigen REQUIRED) 16 | include_directories( 17 | include 18 | ${catkin_INCLUDE_DIRS} 19 | ${EIGEN_INCLUDE_DIRS} 20 | ) 21 | add_definitions(${EIGEN_DEFINITIONS}) 22 | 23 | catkin_package( 24 | INCLUDE_DIRS include 25 | LIBRARIES collvoid_wiggle_recovery 26 | CATKIN_DEPENDS 27 | roscpp 28 | pluginlib 29 | ) 30 | 31 | add_library(collvoid_wiggle_recovery src/collvoid_wiggle_recovery.cpp) 32 | target_link_libraries(collvoid_wiggle_recovery ${catkin_LIBRARIES}) 33 | 34 | install(TARGETS collvoid_wiggle_recovery 35 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 36 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 37 | ) 38 | 39 | install(FILES rotate_plugin.xml 40 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 41 | ) 42 | 43 | -------------------------------------------------------------------------------- /collvoid_wiggle_recovery/package.xml: -------------------------------------------------------------------------------- 1 | 2 | collvoid_wiggle_recovery 3 | 1.12.13 4 | 5 | 6 | This package provides a recovery behavior for the navigation stack that attempts to clear space by performing a 360 degree rotation of the robot. 7 | 8 | 9 | Eitan Marder-Eppstein 10 | contradict@gmail.com 11 | David V. Lu!! 12 | Michael Ferguson 13 | BSD 14 | http://wiki.ros.org/collvoid_wiggle_recovery 15 | 16 | catkin 17 | 18 | cmake_modules 19 | roscpp 20 | tf 21 | costmap_2d 22 | nav_core 23 | pluginlib 24 | eigen 25 | base_local_planner 26 | 27 | roscpp 28 | tf 29 | costmap_2d 30 | nav_core 31 | pluginlib 32 | eigen 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /collvoid_wiggle_recovery/rotate_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A recovery behavior that performs a 360 degree in-place rotation to attempt to clear out space. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /experimental/CATKIN_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/CATKIN_IGNORE -------------------------------------------------------------------------------- /experimental/collvoid_pr2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html 2 | # Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html 3 | cmake_minimum_required(VERSION 2.8.3) 4 | project(collvoid_pr2) 5 | # Load catkin and all dependencies required for this package 6 | # TODO: remove all from COMPONENTS that are not catkin packages. 7 | find_package(catkin REQUIRED ) 8 | 9 | # include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) 10 | # CATKIN_MIGRATION: removed during catkin migration 11 | # cmake_minimum_required(VERSION 2.4.6) 12 | 13 | # CATKIN_MIGRATION: removed during catkin migration 14 | # include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 15 | 16 | # Set the build type. Options are: 17 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 18 | # Debug : w/ debug symbols, w/o optimization 19 | # Release : w/o debug symbols, w/ optimization 20 | # RelWithDebInfo : w/ debug symbols, w/ optimization 21 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 22 | #set(ROS_BUILD_TYPE RelWithDebInfo) 23 | 24 | 25 | # CATKIN_MIGRATION: removed during catkin migration 26 | # rosbuild_init() 27 | 28 | #set the default path for built executables to the "bin" directory 29 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 30 | #set the default path for built libraries to the "lib" directory 31 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 32 | 33 | #uncomment if you have defined messages 34 | #add_message_files( 35 | # FILES 36 | # TODO: List your msg files here 37 | #) 38 | #uncomment if you have defined services 39 | #add_service_files( 40 | # FILES 41 | # TODO: List your msg files here 42 | #) 43 | 44 | #common commands for building c++ executables and libraries 45 | #add_library(${PROJECT_NAME} src/example.cpp) 46 | #target_link_libraries(${PROJECT_NAME} another_library) 47 | # 48 | # CATKIN_MIGRATION: removed during catkin migration 49 | # rosbuild_add_boost_directories() 50 | #find_package(Boost REQUIRED COMPONENTS thread) 51 | #include_directories(${Boost_INCLUDE_DIRS}) 52 | #target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) 53 | #add_executable(example examples/example.cpp) 54 | #target_link_libraries(example ${PROJECT_NAME}) 55 | ## Generate added messages and services with any dependencies listed here 56 | generate_messages( 57 | #TODO DEPENDENCIES geometry_msgs std_msgs 58 | ) 59 | 60 | # catkin_package parameters: http://ros.org/doc/groovy/api/catkin/html/dev_guide/generated_cmake_api.html#catkin-package 61 | # TODO: fill in what other packages will need to use this package 62 | catkin_package( 63 | DEPENDS # TODO add dependencies 64 | CATKIN_DEPENDS # TODO 65 | INCLUDE_DIRS # TODO include 66 | LIBRARIES # TODO 67 | ) -------------------------------------------------------------------------------- /experimental/collvoid_pr2/ROS_NOBUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/collvoid_pr2/ROS_NOBUILD -------------------------------------------------------------------------------- /experimental/collvoid_pr2/launch/pr2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /experimental/collvoid_pr2/launch/pr2_master.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /experimental/collvoid_pr2/launch/pr2_single.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /experimental/collvoid_pr2/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b collvoid_pr2 is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /experimental/collvoid_pr2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | collvoid_pr2 3 | 1.0.0 4 | collvoid_pr2 5 | Daniel Claes 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/collvoid_pr2 10 | 11 | 12 | 13 | catkin 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /experimental/collvoid_pr2/params/config.yaml: -------------------------------------------------------------------------------- 1 | local_pubs: [position_share] 2 | foreign_pubs: [position_share,commands_robot] 3 | -------------------------------------------------------------------------------- /experimental/collvoid_pr2/params/params_created.yaml: -------------------------------------------------------------------------------- 1 | /simulation_mode: false 2 | /use_ground_truth: false 3 | /scale_radius: yes 4 | /init_guess_noise_std: 0.0 5 | -------------------------------------------------------------------------------- /experimental/collvoid_pr2/world/maplarge.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/collvoid_pr2/world/maplarge.pgm -------------------------------------------------------------------------------- /experimental/collvoid_pr2/world/maplarge.yaml: -------------------------------------------------------------------------------- 1 | image: maplarge.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /experimental/pacman_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html 2 | # Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html 3 | cmake_minimum_required(VERSION 2.8.3) 4 | project(pacman_controller) 5 | # Load catkin and all dependencies required for this package 6 | # TODO: remove all from COMPONENTS that are not catkin packages. 7 | find_package(catkin REQUIRED COMPONENTS rospy roscpp std_msgs std_srvs visualization_msgs geometry_msgs collvoid_msgs collvoid_local_planner tf actionlib move_base_msgs) 8 | 9 | # include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) 10 | # CATKIN_MIGRATION: removed during catkin migration 11 | # cmake_minimum_required(VERSION 2.4.6) 12 | 13 | # CATKIN_MIGRATION: removed during catkin migration 14 | # include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 15 | 16 | # Set the build type. Options are: 17 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 18 | # Debug : w/ debug symbols, w/o optimization 19 | # Release : w/o debug symbols, w/ optimization 20 | # RelWithDebInfo : w/ debug symbols, w/ optimization 21 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 22 | #set(ROS_BUILD_TYPE RelWithDebInfo) 23 | 24 | 25 | # CATKIN_MIGRATION: removed during catkin migration 26 | # rosbuild_init() 27 | 28 | #set the default path for built executables to the "bin" directory 29 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 30 | #set the default path for built libraries to the "lib" directory 31 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 32 | 33 | #uncomment if you have defined messages 34 | #add_message_files( 35 | # FILES 36 | # TODO: List your msg files here 37 | #) 38 | #uncomment if you have defined services 39 | #add_service_files( 40 | # FILES 41 | # TODO: List your msg files here 42 | #) 43 | 44 | #common commands for building c++ executables and libraries 45 | #add_library(${PROJECT_NAME} src/example.cpp) 46 | #target_link_libraries(${PROJECT_NAME} another_library) 47 | # 48 | # CATKIN_MIGRATION: removed during catkin migration 49 | # rosbuild_add_boost_directories() 50 | #find_package(Boost REQUIRED COMPONENTS thread) 51 | #include_directories(${Boost_INCLUDE_DIRS}) 52 | #target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) 53 | #add_executable(example examples/example.cpp) 54 | #target_link_libraries(example ${PROJECT_NAME}) 55 | ## Generate added messages and services with any dependencies listed here 56 | generate_messages( 57 | #TODO DEPENDENCIES geometry_msgs std_msgs 58 | ) 59 | 60 | # catkin_package parameters: http://ros.org/doc/groovy/api/catkin/html/dev_guide/generated_cmake_api.html#catkin-package 61 | # TODO: fill in what other packages will need to use this package 62 | catkin_package( 63 | DEPENDS rospy roscpp std_msgs std_srvs visualization_msgs geometry_msgs collvoid_msgs collvoid_local_planner tf actionlib move_base_msgs 64 | CATKIN_DEPENDS # TODO 65 | INCLUDE_DIRS # TODO include 66 | LIBRARIES # TODO 67 | ) -------------------------------------------------------------------------------- /experimental/pacman_controller/ROS_NOBUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_controller/ROS_NOBUILD -------------------------------------------------------------------------------- /experimental/pacman_controller/launch/create_map.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 | -------------------------------------------------------------------------------- /experimental/pacman_controller/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pacman_controller is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /experimental/pacman_controller/meshes/blue.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_controller/meshes/blue.jpg -------------------------------------------------------------------------------- /experimental/pacman_controller/meshes/blue2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_controller/meshes/blue2.jpg -------------------------------------------------------------------------------- /experimental/pacman_controller/meshes/catchable.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_controller/meshes/catchable.jpg -------------------------------------------------------------------------------- /experimental/pacman_controller/meshes/catchable2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_controller/meshes/catchable2.jpg -------------------------------------------------------------------------------- /experimental/pacman_controller/meshes/dead.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_controller/meshes/dead.jpg -------------------------------------------------------------------------------- /experimental/pacman_controller/meshes/dead2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_controller/meshes/dead2.jpg -------------------------------------------------------------------------------- /experimental/pacman_controller/meshes/pink.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_controller/meshes/pink.jpg -------------------------------------------------------------------------------- /experimental/pacman_controller/meshes/pink2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_controller/meshes/pink2.jpg -------------------------------------------------------------------------------- /experimental/pacman_controller/meshes/red.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_controller/meshes/red.jpg -------------------------------------------------------------------------------- /experimental/pacman_controller/meshes/red2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_controller/meshes/red2.jpg -------------------------------------------------------------------------------- /experimental/pacman_controller/meshes/texture4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_controller/meshes/texture4.jpg -------------------------------------------------------------------------------- /experimental/pacman_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | pacman_controller 3 | 1.0.0 4 | pacman_controller 5 | Daniel Claes 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/pacman_controller 10 | 11 | 12 | Daniel Claes 13 | 14 | 15 | catkin 16 | 17 | 18 | rospy 19 | roscpp 20 | std_msgs 21 | std_srvs 22 | visualization_msgs 23 | geometry_msgs 24 | collvoid_msgs 25 | collvoid_local_planner 26 | tf 27 | actionlib 28 | move_base_msgs 29 | 30 | 31 | rospy 32 | roscpp 33 | std_msgs 34 | std_srvs 35 | visualization_msgs 36 | geometry_msgs 37 | collvoid_msgs 38 | collvoid_local_planner 39 | tf 40 | actionlib 41 | move_base_msgs 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /experimental/pacman_controller/params/ghost_homes.yaml: -------------------------------------------------------------------------------- 1 | ghost_red: 2 | home: 24 3 | sleep_time: 0.0 4 | 5 | ghost_pink: 6 | home: 25 7 | sleep_time: 5.0 8 | 9 | ghost_blue: 10 | home: 26 11 | sleep_time: 10.0 12 | 13 | pacman: 14 | home: 13 15 | sleep_time: 0.0 16 | 17 | score_loc: 18 | x: 5.0 19 | y: 5.0 20 | 21 | info_loc: 22 | x: 0.0 23 | y: 0.0 24 | 25 | power_ups: 26 | nodes: [1,4,12,9,19] -------------------------------------------------------------------------------- /experimental/pacman_controller/params/params_created.yaml: -------------------------------------------------------------------------------- 1 | /use_sim_time: true 2 | /simulation_mode: true 3 | /use_ground_truth: false 4 | /scale_radius: true 5 | /init_guess_noise_std: 0.0 6 | /num_robots: 6 7 | robot_0: 8 | goals: 9 | x: [-3.472792] 10 | y: [3.272792] 11 | ang: [2.356194] 12 | robot_1: 13 | goals: 14 | x: [-3.938666] 15 | y: [1.534126] 16 | ang: [3.403392] 17 | robot_2: 18 | goals: 19 | x: [-2.665874] 20 | y: [0.261334] 21 | ang: [4.450590] 22 | robot_3: 23 | goals: 24 | x: [-0.927208] 25 | y: [0.727208] 26 | ang: [5.497787] 27 | robot_4: 28 | goals: 29 | x: [-0.461334] 30 | y: [2.465874] 31 | ang: [6.544985] 32 | robot_5: 33 | goals: 34 | x: [-1.734126] 35 | y: [3.738666] 36 | ang: [7.592182] 37 | -------------------------------------------------------------------------------- /experimental/pacman_controller/src/pacman_controller/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_controller/src/pacman_controller/__init__.py -------------------------------------------------------------------------------- /experimental/pacman_controller/world/maplarge.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_controller/world/maplarge.pgm -------------------------------------------------------------------------------- /experimental/pacman_controller/world/maplarge.yaml: -------------------------------------------------------------------------------- 1 | image: maplarge.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /experimental/pacman_controller/world/willow_full.world: -------------------------------------------------------------------------------- 1 | resolution 0.02 # set the resolution of the underlying raytrace model in meters 2 | interval_sim 100 # simulation timestep in milliseconds 3 | #interval_real 100 4 | 5 | window 6 | ( 7 | size [ 800.000 600.000 ] 8 | scale 28.806 9 | ) 10 | 11 | define map model 12 | ( 13 | color "gray30" 14 | boundary 0 15 | gui_nose 0 16 | gui_grid 0 17 | gui_move 0 18 | gui_outline 0 19 | obstacle_return 1 20 | gripper_return 0 21 | fiducial_return 0 22 | laser_return 1 23 | ) 24 | 25 | map 26 | ( 27 | name "wilow_map" 28 | size [99.200 99.200 0.500] 29 | pose [0 0 0 0] 30 | origin [-0.400 -0.400 0 0] 31 | 32 | bitmap "maplarge.pgm" 33 | ) 34 | 35 | 36 | define hokuyo laser 37 | ( 38 | range_max 4.0 39 | fov 270.25 40 | samples 1081 41 | color "black" 42 | size [ 0.050 0.050 0.100 ] 43 | ) 44 | 45 | define roomba position 46 | ( 47 | size [0.330 0.330 0.100] 48 | origin [0 0 0 0] 49 | gui_nose 1 50 | 51 | # This block approximates the circular shape of a Roomba 52 | block 53 | ( 54 | points 16 55 | point[0] [ 0.225 0.000 ] 56 | point[1] [ 0.208 0.086 ] 57 | point[2] [ 0.159 0.159 ] 58 | point[3] [ 0.086 0.208 ] 59 | point[4] [ 0.000 0.225 ] 60 | point[5] [ -0.086 0.208 ] 61 | point[6] [ -0.159 0.159 ] 62 | point[7] [ -0.208 0.086 ] 63 | point[8] [ -0.225 0.000 ] 64 | point[9] [ -0.208 -0.086 ] 65 | point[10] [ -0.159 -0.159 ] 66 | point[11] [ -0.086 -0.208 ] 67 | point[12] [ -0.000 -0.225 ] 68 | point[13] [ 0.086 -0.208 ] 69 | point[14] [ 0.159 -0.159 ] 70 | point[15] [ 0.208 -0.086 ] 71 | ) 72 | 73 | hokuyo( pose [0 0 0.100 0] ) 74 | drive "diff" 75 | 76 | ) 77 | 78 | define topurg laser 79 | ( 80 | range_max 30.0 81 | fov 270.25 82 | samples 1081 83 | # generic model properties 84 | color "black" 85 | size [ 0.05 0.05 0.1 ] 86 | ) 87 | 88 | define diff_robot position 89 | ( 90 | size [0.34 0.34 0.3] 91 | origin [0 0 0 0] 92 | gui_nose 1 93 | drive "diff" 94 | wheelbase 0.25 95 | topurg(pose [ 0.0 0.000 0 0.000 ]) 96 | ) 97 | 98 | define omni_robot position 99 | ( 100 | size [0.34 0.34 0.3] 101 | origin [0 0 0 0] 102 | gui_nose 1 103 | drive "omni" 104 | wheelbase 0.25 105 | topurg(pose [ 0.0 0.000 0 0.000 ]) 106 | ) 107 | 108 | define pr2 position 109 | ( 110 | size [0.55 0.55 0.1] 111 | origin [0 0 0 0] 112 | gui_nose 1 113 | 114 | # This block approximates the shape of a PR2 115 | block 116 | ( 117 | points 5 118 | point[0] [ -0.325 0.325 ] 119 | point[1] [ -0.325 -0.325 ] 120 | point[2] [ 0.325 -0.325 ] 121 | point[3] [ 0.46 0.0 ] 122 | 123 | point[4] [ 0.325 0.325 ] 124 | 125 | ) 126 | 127 | topurg( pose [0.275 0 0.1 0] ) 128 | drive "omni" 129 | 130 | ) 131 | 132 | 133 | # robots added here automatically 134 | roomba( pose [ 6.023 3.057 0 4.807 ] name "robot_0" color "black") 135 | roomba( pose [ -1.459 1.068 0 2.509 ] name "robot_1" color "SlateGray") 136 | roomba( pose [ -1.488 -0.557 0 4.503 ] name "robot_2" color "LightGrey") 137 | roomba( pose [ -1.199 -1.679 0 4.574 ] name "robot_3" color "dark slate blue") 138 | -------------------------------------------------------------------------------- /experimental/pacman_turtles/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html 2 | # Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html 3 | cmake_minimum_required(VERSION 2.8.3) 4 | project(pacman_turtles) 5 | # Load catkin and all dependencies required for this package 6 | # TODO: remove all from COMPONENTS that are not catkin packages. 7 | find_package(catkin REQUIRED ) 8 | 9 | # include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) 10 | # CATKIN_MIGRATION: removed during catkin migration 11 | # cmake_minimum_required(VERSION 2.4.6) 12 | 13 | # CATKIN_MIGRATION: removed during catkin migration 14 | # include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 15 | 16 | # Set the build type. Options are: 17 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 18 | # Debug : w/ debug symbols, w/o optimization 19 | # Release : w/o debug symbols, w/ optimization 20 | # RelWithDebInfo : w/ debug symbols, w/ optimization 21 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 22 | #set(ROS_BUILD_TYPE RelWithDebInfo) 23 | 24 | 25 | # CATKIN_MIGRATION: removed during catkin migration 26 | # rosbuild_init() 27 | 28 | #set the default path for built executables to the "bin" directory 29 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 30 | #set the default path for built libraries to the "lib" directory 31 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 32 | 33 | #uncomment if you have defined messages 34 | #add_message_files( 35 | # FILES 36 | # TODO: List your msg files here 37 | #) 38 | #uncomment if you have defined services 39 | #add_service_files( 40 | # FILES 41 | # TODO: List your msg files here 42 | #) 43 | 44 | #common commands for building c++ executables and libraries 45 | #add_library(${PROJECT_NAME} src/example.cpp) 46 | #target_link_libraries(${PROJECT_NAME} another_library) 47 | # 48 | # CATKIN_MIGRATION: removed during catkin migration 49 | # rosbuild_add_boost_directories() 50 | #find_package(Boost REQUIRED COMPONENTS thread) 51 | #include_directories(${Boost_INCLUDE_DIRS}) 52 | #target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) 53 | #add_executable(example examples/example.cpp) 54 | #target_link_libraries(example ${PROJECT_NAME}) 55 | ## Generate added messages and services with any dependencies listed here 56 | generate_messages( 57 | #TODO DEPENDENCIES geometry_msgs std_msgs 58 | ) 59 | 60 | # catkin_package parameters: http://ros.org/doc/groovy/api/catkin/html/dev_guide/generated_cmake_api.html#catkin-package 61 | # TODO: fill in what other packages will need to use this package 62 | catkin_package( 63 | DEPENDS # TODO add dependencies 64 | CATKIN_DEPENDS # TODO 65 | INCLUDE_DIRS # TODO include 66 | LIBRARIES # TODO 67 | ) -------------------------------------------------------------------------------- /experimental/pacman_turtles/ROS_NOBUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_turtles/ROS_NOBUILD -------------------------------------------------------------------------------- /experimental/pacman_turtles/launch/ghost.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /experimental/pacman_turtles/launch/master.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /experimental/pacman_turtles/launch/pacman.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /experimental/pacman_turtles/launch/pacman_master.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /experimental/pacman_turtles/launch/player.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /experimental/pacman_turtles/launch/player_withoutmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /experimental/pacman_turtles/launch/turtlebot_master.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /experimental/pacman_turtles/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pacman_turtles is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /experimental/pacman_turtles/package.xml: -------------------------------------------------------------------------------- 1 | 2 | pacman_turtles 3 | 1.0.0 4 | pacman_turtles 5 | Daniel Claes 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/pacman_turtles 10 | 11 | 12 | Daniel Claes 13 | 14 | 15 | catkin 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /experimental/pacman_turtles/params/config.yaml: -------------------------------------------------------------------------------- 1 | local_pubs: [position_share] 2 | foreign_pubs: [position_share,commands_robot,state] 3 | -------------------------------------------------------------------------------- /experimental/pacman_turtles/params/pacman.yaml: -------------------------------------------------------------------------------- 1 | local_pubs: [position_share] 2 | foreign_pubs: [position_share,commands_robot,state,/move_base_simple/goal] 3 | -------------------------------------------------------------------------------- /experimental/pacman_turtles/world/maplarge.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daenny/collvoid/efadbab62b6e2f05f9069baf4416e6420e568946/experimental/pacman_turtles/world/maplarge.pgm -------------------------------------------------------------------------------- /experimental/pacman_turtles/world/maplarge.yaml: -------------------------------------------------------------------------------- 1 | image: maplarge.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | --------------------------------------------------------------------------------