├── .gitignore ├── LICENSE ├── README.md ├── assisted_steering ├── CMakeLists.txt ├── README.md ├── cfg │ └── AssistedSteering.cfg ├── include │ └── assisted_steering │ │ ├── AssistedSteering.h │ │ └── odometry_helper_ros.h ├── launch │ └── assisted_steering.launch ├── package.xml └── src │ ├── AssistedSteering.cpp │ ├── AssistedSteeringNode.cpp │ └── odometry_helper_ros.cpp ├── gmm_sampling ├── CMakeLists.txt ├── README.md ├── include │ └── gmm_sampling │ │ ├── eigenmvn.h │ │ └── gmm_sampling.h ├── launch │ └── approach_gmm_sampling.launch ├── only_one_homotopy_tasks │ ├── 1-front │ │ ├── GMM_mu.txt │ │ ├── GMM_priors.txt │ │ ├── GMM_sigma.txt │ │ └── GMM_vars.txt │ ├── 2-45_right │ │ ├── GMM_mu.txt │ │ ├── GMM_priors.txt │ │ ├── GMM_sigma.txt │ │ └── GMM_vars.txt │ ├── 3-90_right │ │ ├── GMM_mu.txt │ │ ├── GMM_priors.txt │ │ ├── GMM_sigma.txt │ │ └── GMM_vars.txt │ ├── 4-135_right │ │ ├── GMM_mu.txt │ │ ├── GMM_priors.txt │ │ ├── GMM_sigma.txt │ │ └── GMM_vars.txt │ ├── 5-back │ │ ├── GMM_mu.txt │ │ ├── GMM_priors.txt │ │ ├── GMM_sigma.txt │ │ └── GMM_vars.txt │ ├── 6-135_left │ │ ├── GMM_mu.txt │ │ ├── GMM_priors.txt │ │ ├── GMM_sigma.txt │ │ └── GMM_vars.txt │ ├── 7-90_left │ │ ├── GMM_mu.txt │ │ ├── GMM_priors.txt │ │ ├── GMM_sigma.txt │ │ └── GMM_vars.txt │ └── 8-45_left │ │ ├── GMM_mu.txt │ │ ├── GMM_priors.txt │ │ ├── GMM_sigma.txt │ │ └── GMM_vars.txt ├── package.xml ├── src │ ├── gmm_sampling.cpp │ └── gmm_sampling_ros.cpp ├── srv │ ├── GetApproachGMMProb.srv │ ├── GetApproachGMMProbs.srv │ └── GetApproachGMMSamples.srv └── tasks │ ├── 1-front │ ├── GMM_mu.txt │ ├── GMM_priors.txt │ ├── GMM_sigma.txt │ └── GMM_vars.txt │ ├── 2-45_right │ ├── GMM_mu.txt │ ├── GMM_priors.txt │ ├── GMM_sigma.txt │ └── GMM_vars.txt │ ├── 3-90_right │ ├── GMM_mu.txt │ ├── GMM_priors.txt │ ├── GMM_sigma.txt │ └── GMM_vars.txt │ ├── 4-135_right │ ├── GMM_mu.txt │ ├── GMM_priors.txt │ ├── GMM_sigma.txt │ └── GMM_vars.txt │ ├── 5-back │ ├── GMM_mu.txt │ ├── GMM_priors.txt │ ├── GMM_sigma.txt │ └── GMM_vars.txt │ ├── 6-135_left │ ├── GMM_mu.txt │ ├── GMM_priors.txt │ ├── GMM_sigma.txt │ └── GMM_vars.txt │ ├── 7-90_left │ ├── GMM_mu.txt │ ├── GMM_priors.txt │ ├── GMM_sigma.txt │ └── GMM_vars.txt │ └── 8-45_left │ ├── GMM_mu.txt │ ├── GMM_priors.txt │ ├── GMM_sigma.txt │ └── GMM_vars.txt ├── navigation_features ├── .gitignore ├── CMakeLists.txt ├── README.md ├── cfg │ └── nav_features.cfg ├── include │ └── navigation_features │ │ └── nav_features.h ├── package.xml ├── src │ └── nav_features.cpp └── srv │ ├── GetFeatureCount.srv │ ├── InitWeights.srv │ ├── PoseValid.srv │ ├── SetLossCost.srv │ ├── SetScenario.srv │ └── SetWeights.srv ├── path_prediction ├── .gitignore ├── CMakeLists.txt ├── README.md ├── model │ └── my_model.h5 ├── package.xml ├── src │ └── prediction_node.py └── srv │ └── PathPrediction.srv ├── simple_local_planner ├── .gitignore ├── CMakeLists.txt ├── README.md ├── cfg │ └── SimpleLocalPlanner.cfg ├── include │ └── simple_local_planner │ │ ├── costmap_model.h │ │ ├── footprint_helper.h │ │ ├── goal_functions.h │ │ ├── line_iterator.h │ │ ├── odometry_helper_ros.h │ │ ├── pure_planner.h │ │ ├── pure_planner_ros.h │ │ ├── trajectory.h │ │ └── world_model.h ├── package.xml ├── plp_plugin.xml └── src │ ├── costmap_model.cpp │ ├── goal_functions.cpp │ ├── odometry_helper_ros.cpp │ ├── pure_planner.cpp │ ├── pure_planner_ros.cpp │ └── trajectory.cpp ├── upo_decision_making ├── CMakeLists.txt ├── README.md ├── cfg │ ├── centre_sportif_waypoint_defs.yaml │ ├── demo.rviz │ ├── elderly_center_waypoint_defs.yaml │ ├── leds.yaml │ ├── upo_lab_waypoint_defs.yaml │ ├── uva_demo_waypoint_defs.yaml │ ├── uva_lab_waypoint_defs.yaml │ ├── uva_sim_waypoint_defs.yaml │ ├── waypoint_defs.yaml │ └── waypoints_move_base.txt ├── msg │ ├── ControlEvent.msg │ ├── HRIControlMode.msg │ ├── HRIFeedbackEvent.msg │ ├── HRIFeedbackFromInterface.msg │ ├── HRIFeedbackStateInfo.msg │ └── IDArray.msg ├── package.xml └── scripts │ ├── event_manager.py │ ├── hri_common.py │ ├── hri_common.pyc │ ├── led_manager.py │ ├── pilot_interface_bridge.py │ ├── smach_common.py │ ├── smach_common.pyc │ ├── upo_nav_behavior_tester.py │ └── upo_navigation_behavior_fsm.py ├── upo_launchers ├── CMakeLists.txt ├── README.md ├── assisted_steering.launch ├── centre_sportif │ ├── centre_sportif.world │ ├── map.inc │ ├── maps │ │ ├── centre_sportif.pgm │ │ ├── centre_sportif.yaml │ │ ├── waypoint_defs.yaml │ │ ├── yield_map.jpg │ │ ├── yield_map.yaml │ │ └── yield_points.yaml │ └── sim_sportif_nav_behavior.launch ├── cfg │ ├── centre_sportif_waypoint_defs.yaml │ ├── elderly_center_waypoint_defs.yaml │ ├── exp4_sim_waypoints.yaml │ ├── upo_lab_waypoint_defs.yaml │ ├── uva_demo_waypoint_defs.yaml │ ├── uva_lab_waypoint_defs.yaml │ ├── uva_sim_waypoint_defs.yaml │ └── waypoints_move_base.txt ├── elderly_center │ ├── elderly_center.world │ ├── map.inc │ ├── maps │ │ ├── LesArcades-chairs.pgm │ │ ├── LesArcades-chairs.yaml │ │ ├── elderly_center.pgm │ │ ├── elderly_center.yaml │ │ ├── waypoint_defs.yaml │ │ ├── yield_map.jpg │ │ ├── yield_map.pgm │ │ ├── yield_map.yaml │ │ └── yield_points.yaml │ ├── sim_elderly_center_nav_behavior.launch │ ├── upo_navigation.launch │ └── yield_extra │ │ ├── LesArcades-bigdoor.pgm │ │ ├── LesArcades-bigdoor.yaml │ │ ├── LesArcades-yield.jpg │ │ ├── LesArcades-yield.yaml │ │ ├── map.inc │ │ ├── teresa_lesArcades_bigdoor.world │ │ └── yield_points.yaml ├── package.xml ├── params3 │ ├── costmap_common_params.yaml │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ └── navigation_params.yaml ├── params_nav │ ├── costmap_common_params.yaml │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ ├── navigation_params.yaml │ └── navigation_params_costmap.yaml ├── params_sim │ ├── costmap_common_params.yaml │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ └── navigation_params.yaml ├── params_sim2 │ ├── costmap_common_params.yaml │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ └── navigation_params.yaml ├── record_nav.launch ├── rrt_navigation.rviz ├── sim_nav_behavior.launch ├── upo_45 │ ├── map.inc │ ├── maps │ │ ├── upo_45.pgm │ │ ├── upo_45.yaml │ │ ├── upo_45_1.pgm │ │ ├── waypoint_defs.yaml │ │ ├── yield_map.jpg │ │ ├── yield_map.yaml │ │ └── yield_points.yaml │ ├── move_base │ │ ├── navigation_move_base.rviz │ │ ├── params_nav │ │ │ ├── costmap_common_params.yaml │ │ │ ├── global_costmap_params.yaml │ │ │ ├── local_costmap_params.yaml │ │ │ └── navigation_params.yaml │ │ ├── real_mv_upo_lab_optitrack.launch │ │ └── real_nav_upo_lab.launch │ ├── real_rrt_upo_45_optitrack.launch │ ├── sim_upo_45_nav_behavior.launch │ └── upo_45.world ├── upo_lab │ ├── map.inc │ ├── maps │ │ ├── upo45.pgm │ │ ├── upo45.yaml │ │ ├── upo_lab.pgm │ │ ├── upo_lab.yaml │ │ ├── upo_lab_split.pgm │ │ ├── waypoint_defs.yaml │ │ ├── yield_map.jpg │ │ ├── yield_map.yaml │ │ ├── yield_map_empty.jpg │ │ └── yield_points.yaml │ ├── move_base │ │ ├── navigation_move_base.rviz │ │ ├── params_nav │ │ │ ├── costmap_common_params.yaml │ │ │ ├── global_costmap_params.yaml │ │ │ ├── local_costmap_params.yaml │ │ │ └── navigation_params.yaml │ │ ├── real_mv_upo_lab_optitrack.launch │ │ └── real_nav_upo_lab.launch │ ├── real_rrt_upo_lab_optitrack.launch │ ├── sim_upo_lab_nav_behavior.launch │ └── upo_lab.world ├── upo_navigation.rviz ├── upo_rrt_planners.launch ├── upo_rrt_planners3.launch ├── uva_demo │ ├── map.inc │ ├── maps │ │ ├── uva_demo.pgm │ │ ├── uva_demo.yaml │ │ ├── yield_map.jpg │ │ ├── yield_map.yaml │ │ ├── yield_map_empty.jpg │ │ └── yield_points.yaml │ ├── move_base │ │ ├── navigation_move_base.rviz │ │ ├── params_mb │ │ │ ├── costmap_common_params.yaml │ │ │ ├── global_costmap_params.yaml │ │ │ ├── global_costmap_params_sim.yaml │ │ │ ├── local_costmap_params.yaml │ │ │ ├── local_costmap_params_sim.yaml │ │ │ └── navigation_params.yaml │ │ ├── sim_uva_demo_mb.launch │ │ └── uva_demo_mb.launch │ ├── sim_uva_demo_nav_behavior.launch │ ├── uva_demo.world │ ├── uva_demo_navigation.launch │ └── uva_demo_navigation_r6.launch └── uva_lab │ ├── maps │ ├── uva_lab.pgm │ ├── uva_lab.yaml │ ├── yield_map.jpg │ ├── yield_map.yaml │ └── yield_points.yaml │ ├── move_base │ ├── navigation_move_base.rviz │ ├── params_mb │ │ ├── costmap_common_params.yaml │ │ ├── global_costmap_params.yaml │ │ ├── global_costmap_params_sim.yaml │ │ ├── local_costmap_params.yaml │ │ ├── local_costmap_params_sim.yaml │ │ └── navigation_params.yaml │ └── uva_lab_mb.launch │ ├── uva_lab_navigation.launch │ └── uva_lab_navigation_r6.launch ├── upo_local_planner ├── CMakeLists.txt ├── README.md ├── include │ └── upo_local_planner │ │ ├── collision_detection.h │ │ ├── goal_functions.h │ │ ├── odometry_helper_ros.h │ │ ├── trajectory.h │ │ ├── upo_planner.h │ │ └── upo_planner_ros.h ├── package.xml ├── src │ ├── collision_detection.cpp │ ├── goal_functions.cpp │ ├── odometry_helper_ros.cpp │ ├── trajectory.cpp │ ├── upo_planner.cpp │ └── upo_planner_ros.cpp └── ulp_plugin.xml ├── upo_msgs ├── CMakeLists.txt ├── msg │ ├── PersonGoalArrayUPO.msg │ ├── PersonGoalUPO.msg │ ├── PersonPoseArrayUPO.msg │ └── PersonPoseUPO.msg └── package.xml ├── upo_navigation ├── .gitignore ├── CMakeLists.txt ├── README.md ├── include │ └── upo_navigation │ │ ├── costmap_model.h │ │ ├── footprint_helper.h │ │ ├── goal_functions.h │ │ ├── latched_stop_rotate_controller.h │ │ ├── line_iterator.h │ │ ├── local_planner_limits.h │ │ ├── local_planner_util.h │ │ ├── map_cell.h │ │ ├── map_grid.h │ │ ├── map_grid_cost_function.h │ │ ├── map_grid_cost_point.h │ │ ├── map_grid_visualizer.h │ │ ├── obstacle_cost_function.h │ │ ├── odometry_helper_ros.h │ │ ├── oscillation_cost_function.h │ │ ├── planar_laser_scan.h │ │ ├── point_grid.h │ │ ├── pp_controller │ │ └── trajectory_planner_pp.h │ │ ├── prefer_forward_cost_function.h │ │ ├── simple_scored_sampling_planner.h │ │ ├── simple_trajectory_generator.h │ │ ├── trajectory.h │ │ ├── trajectory_cost_function.h │ │ ├── trajectory_inc.h │ │ ├── trajectory_planner.h │ │ ├── trajectory_sample_generator.h │ │ ├── trajectory_search.h │ │ ├── upo_navigation.h │ │ ├── velocity_iterator.h │ │ ├── voxel_grid_model.h │ │ └── world_model.h ├── package.xml ├── src │ ├── costmap_model.cpp │ ├── footprint_helper.cpp │ ├── goal_functions.cpp │ ├── latched_stop_rotate_controller.cpp │ ├── local_planner_limits.py │ ├── local_planner_util.cpp │ ├── map_cell.cpp │ ├── map_grid.cpp │ ├── map_grid_cost_function.cpp │ ├── map_grid_visualizer.cpp │ ├── obstacle_cost_function.cpp │ ├── odometry_helper_ros.cpp │ ├── oscillation_cost_function.cpp │ ├── point_grid.cpp │ ├── pp_controller │ │ └── trajectory_planner_pp.cpp │ ├── prefer_forward_cost_function.cpp │ ├── simple_scored_sampling_planner.cpp │ ├── simple_trajectory_generator.cpp │ ├── trajectory.cpp │ ├── trajectory_planner.cpp │ ├── upo_navigation.cpp │ ├── upo_navigation_node.cpp │ └── voxel_grid_model.cpp ├── srv │ ├── FeatureCounts.srv │ ├── PointCosts.srv │ └── SetWeights.srv └── waypoint_defs.yaml ├── upo_navigation_macro_actions ├── CMakeLists.txt ├── README.md ├── action │ ├── AssistedSteering.action │ ├── NavigateHome.action │ ├── NavigateInteractionTarget.action │ ├── NavigateWaypoint.action │ ├── Wait.action │ ├── WalkSideBySide.action │ └── Yield.action ├── cfg │ └── NavigationMacroActions.cfg ├── include │ └── upo_navigation_macro_actions │ │ ├── Upo_navigation_macro_actions.h │ │ └── Yield.h ├── launch │ ├── teleop_keyboard.launch │ └── upo_navigation_macro_actions.launch ├── package.xml └── src │ ├── Upo_navigation_macro_actions.cpp │ ├── Upo_navigation_macro_actions_node.cpp │ └── Yield.cpp ├── upo_robot_navigation ├── CMakeLists.txt └── package.xml ├── upo_rrt_planners ├── CMakeLists.txt ├── README.md ├── cfg │ └── RRTRosWrapper.cfg ├── include │ └── upo_rrt_planners │ │ ├── Action.h │ │ ├── NearestNeighbors.h │ │ ├── NearestNeighborsFLANN.h │ │ ├── Node.h │ │ ├── RandomNumbers.h │ │ ├── State.h │ │ ├── StateChecker.h │ │ ├── StateSpace.h │ │ ├── planners │ │ ├── Planner.h │ │ ├── control │ │ │ ├── HalfRRTstar.h │ │ │ ├── RRT.h │ │ │ └── RRTstar.h │ │ └── simple │ │ │ ├── SimpleRRT.h │ │ │ └── SimpleRRTstar.h │ │ ├── ros │ │ ├── RRT_ros_wrapper.h │ │ ├── RRT_ros_wrapper2.h │ │ ├── RRT_ros_wrapper3.h │ │ ├── ValidityChecker.h │ │ ├── ValidityChecker2.h │ │ ├── ValidityChecker3.h │ │ └── capture.h │ │ └── steering │ │ └── Steering.h ├── launch │ └── upo_rrt_planners.launch ├── package.xml ├── saved_input │ ├── input_1.jpg │ ├── input_2.jpg │ ├── input_3.jpg │ ├── input_4.jpg │ ├── input_5.jpg │ └── input_6.jpg ├── src │ ├── Action.cpp │ ├── Node.cpp │ ├── RandomNumbers.cpp │ ├── State.cpp │ ├── StateSpace.cpp │ ├── planners │ │ ├── Planner.cpp │ │ ├── control │ │ │ ├── HalfRRTstar.cpp │ │ │ ├── RRT.cpp │ │ │ └── RRTstar.cpp │ │ └── simple │ │ │ ├── SimpleRRT.cpp │ │ │ └── SimpleRRTstar.cpp │ ├── ros │ │ ├── RRT_ros_wrapper.cpp │ │ ├── RRT_ros_wrapper2.cpp │ │ ├── RRT_ros_wrapper3.cpp │ │ ├── ValidityChecker.cpp │ │ ├── ValidityChecker2.cpp │ │ ├── ValidityChecker3.cpp │ │ ├── capture.cpp │ │ ├── ros_wrapper2_node.cpp │ │ ├── ros_wrapper3_node.cpp │ │ └── ros_wrapper_node.cpp │ └── steering │ │ └── Steering.cpp └── srv │ ├── MakePlan.srv │ └── MakePlanCostmap.srv └── upo_social_layer ├── .gitignore ├── CMakeLists.txt ├── README.md ├── cfg └── SocialPlugin.cfg ├── include └── upo_social_layer │ └── SocialLayer.h ├── launch ├── in_global_costmap │ ├── params_sim │ │ ├── costmap_common_params.yaml │ │ ├── global_costmap_params.yaml │ │ ├── local_costmap_params.yaml │ │ └── navigation_params.yaml │ └── sim_navigation2.launch ├── in_global_costmap_all │ ├── params_sim │ │ ├── costmap_common_params.yaml │ │ ├── global_costmap_params.yaml │ │ ├── local_costmap_params.yaml │ │ └── navigation_params.yaml │ └── sim_navigation_all_feat.launch └── in_local_costmap │ ├── params_sim │ ├── costmap_common_params.yaml │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ └── navigation_params.yaml │ └── sim_navigation1.launch ├── package.xml ├── social_layer_plugin.xml ├── src └── SocialLayer.cpp └── srv ├── Cost.srv ├── Features.srv ├── SetDemoPath.srv └── SetGoal.srv /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | 31 | # Others 32 | *~ 33 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, Service Robotics Lab 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of upo_robot_navigation nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /assisted_steering/README.md: -------------------------------------------------------------------------------- 1 | # assisted steering 2 | ROS node used in the TERESA project. It checks the velocity commands that the user sends to the robot. If a possible collision is detected the node tries to find a similar valid command or stops the robot otherwise. 3 | This node is orientated to be used with circular robot footprints and differential kinematics. 4 | The collision checking is based on a laser rangefinder sensor. 5 | 6 | 7 | ## Parameters 8 | 9 | * **ROS topics** 10 | - laser_topic. Name of the topic where the readings of the laser sensor are being published (message type: sensor_msgs/LaserScan). The node will subscribe to this topic. 11 | - odom_topic. Name of the topic where the robot odometry is being published (message type: nav_msgs/Odometry). The node will subscribe to this topic. 12 | - cmdvel_topic. Name of the topic where the user's velocity commands (message type: geometry_msgs/Twist) are being published. The will subscribe to this topic. 13 | - new_cmdvel_topic. Name of the topic where the new valid velocity commands will be published. The robot should be subscribed to this topic. 14 | 15 | * **Robot Configuration Parameters** 16 | - robot_radius. Radius of the robot footprint (m). 17 | - max_lin_vel. Maximum linear velocity (m/s). 18 | - min_ang_vel. Minimum linear velocity (m/s). 19 | - max_lin_acc. Maximum acceleration in translation (m/s^2). 20 | - max_ang_acc. Maximum acceleration in rotation (rad/s^2). 21 | 22 | * **Forward Simulation Parameters** 23 | - time_step. Time (seconds) to expand the robot movement and check for collisions. (default: 1.0). 24 | - granularity. Resolution in meters to split the expanded trayectory and check for collisions (Default: 0.025). 25 | 26 | 27 | 28 | The package is a **work in progress** used in research prototyping. Pull requests and/or issues are highly encouraged. 29 | -------------------------------------------------------------------------------- /assisted_steering/cfg/AssistedSteering.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = 'assisted_steering' 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | 10 | gen.add("max_lin_vel", double_t, 0, "The maximum linear velocity allowed", 0.35, 0.1, 0.6); 11 | gen.add("max_ang_vel", double_t, 0, "The maximum angular velocity allowed", 0.8, 0.3, 1.57); 12 | gen.add("max_lin_acc", double_t, 0, "The maximum linear acceleration allowed", 1.0, 0.5, 2.0); 13 | gen.add("max_ang_acc", double_t, 0, "The maximum angular acceleration allowed", 1.0, 0.5, 3.0); 14 | gen.add("time_step", double_t, 0, "Time (secs) to propagate the predicted robot motion", 1.0, 0.1, 3.0); 15 | gen.add("granularity", double_t, 0, "Distance step to check the proyected trajectories", 0.05, 0.01, 1.0); 16 | gen.add("robot_radius", double_t, 0, "The radius of the inscribed circunference of the footprint of the robot", 0.32, 0.10, 2.0); 17 | gen.add("is_active", bool_t, 0, "Whether or not to activate the assistant", True); 18 | gen.add("ang_vel_inc", double_t, 0, "The step of angular velocity to sample commands", 0.20, 0.0, 1.0); 19 | gen.add("lin_vel_inc", double_t, 0, "The step of linear velocity to sample commands", 0.1, 0.0, 1.0); 20 | 21 | exit(gen.generate(PACKAGE, "assisted_steering", "AssistedSteering")) 22 | -------------------------------------------------------------------------------- /assisted_steering/launch/assisted_steering.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 | -------------------------------------------------------------------------------- /assisted_steering/src/AssistedSteeringNode.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | int main(int argc, char** argv) 6 | { 7 | ros::init(argc, argv, "Assisted Steering node"); 8 | 9 | tf::TransformListener tf(ros::Duration(10)); 10 | 11 | assisted_steering::AssistedSteering steering(&tf); 12 | 13 | ros::spin(); 14 | 15 | return 0; 16 | } 17 | -------------------------------------------------------------------------------- /gmm_sampling/README.md: -------------------------------------------------------------------------------- 1 | # gmm_sampling 2 | A library and its ROS wrapper that loads a set of GMMs defined through text files. Then these GMMs can be consulted trough ROS services to draw samples and costs from them. 3 | Currently, the GMMs included are employed in the macro-action "approaching a target person". 4 | 5 | ## Parameters 6 | 7 | * **gmm_tasks_directory**. Route to the folder where the GMMs are defined for different tasks. 8 | 9 | ## Services 10 | 11 | * **GetApproachGMMSamples**. It accepts as an input the value of the current relative orientation of the robot according to the target person and the number of samples required. It returns the number of samples indicated obtained from the corresponding GMM according to the orientation provided. 12 | * **GetApproachGMMProbs**. It accepts as an input a list of points in the coordinates frame of the target person (x and y coordinates) and returns a vector with the values of the density functions in those points. 13 | 14 | The package is a **work in progress** used in research prototyping. Pull requests and/or issues are highly encouraged. 15 | -------------------------------------------------------------------------------- /gmm_sampling/launch/approach_gmm_sampling.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/1-front/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 2.660155958932e+00 1.604341748585e+00 2 | 1.571755171350e-02 -2.095383885391e-02 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/1-front/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 4.426574705389e-01 5.573425294611e-01 2 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/1-front/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 1.523503161989e-01 -2.000086695706e-03 1.982513253983e-01 5.807082823132e-03 2 | -2.000086695706e-03 9.276876962664e-05 5.807082823132e-03 4.858128980807e-04 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/1-front/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | d a -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/2-45_right/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 2.350401529513e+00 1.105718038555e+00 1.435914442397e+00 3.037379413699e+00 2 | -5.305276615191e-01 -6.427064032219e-02 -2.299604667530e-01 -7.097308713768e-01 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/2-45_right/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 4.514973278444e-01 7.384086542909e-02 3.192233289263e-01 1.554384778002e-01 2 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/2-45_right/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 1.373994037356e-01 -3.717106404937e-02 2.447807280581e-02 -1.260638906516e-02 1.155488258795e-01 -3.316545115709e-02 2.756514308723e-02 -3.307914488824e-03 2 | -3.717106404937e-02 1.086146340685e-02 -1.260638906516e-02 6.572835255278e-03 -3.316545115709e-02 9.645105477891e-03 -3.307914488824e-03 4.869804684017e-04 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/2-45_right/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | distance angle -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/3-90_right/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 2.247477016085e+00 1.488468033539e+00 3.090080799333e+00 2.712089260371e+00 1.045931027814e+00 1.673639916075e+00 2 | -1.161880501090e+00 -6.080985342020e-01 -1.508914516834e+00 -1.426759065107e+00 -2.236740073460e-01 -6.752229264737e-01 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/3-90_right/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 3.002212461028e-01 1.422854654871e-01 1.302952252031e-01 1.202035177837e-01 1.482886477592e-01 1.587058976641e-01 2 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/3-90_right/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 7.286168186684e-02 -3.205934337171e-02 2.532640289804e-02 -3.093659197123e-02 3.510799883695e-02 -3.796117951190e-03 5.690512516290e-02 -9.899513370008e-03 2.206758290746e-02 -1.372023813724e-02 4.940329248764e-02 -4.356824210823e-02 2 | -3.205934337171e-02 2.161600649982e-02 -3.093659197123e-02 4.080412133450e-02 -3.796117951190e-03 4.584344501274e-04 -9.899513370008e-03 2.258722259719e-03 -1.372023813724e-02 1.355984801346e-02 -4.356824210823e-02 3.878521646454e-02 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/3-90_right/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | distance angle -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/4-135_right/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 2.707447682802e+00 1.358780674276e+00 1.085916462433e+00 3.078796872337e+00 1.593658010078e+00 2.101494842405e+00 2 | -2.181639648242e+00 -9.672580250147e-01 -4.682115384991e-01 -2.302114160952e+00 -1.457963767232e+00 -1.930535890679e+00 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/4-135_right/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 1.434364516533e-01 1.389167408594e-01 1.896802030955e-01 9.678690498058e-02 2.016924073407e-01 2.294872920704e-01 2 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/4-135_right/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 5.593629458760e-02 -1.700688567668e-02 1.133571967351e-02 -1.447001317516e-02 2.005136868457e-02 -2.540330191316e-02 4.014630616607e-02 -4.531938509995e-03 3.213286621096e-02 -2.219138427564e-02 7.607325122170e-02 -3.335054840904e-02 2 | -1.700688567668e-02 5.526935972697e-03 -1.447001317516e-02 2.865372534961e-02 -2.540330191316e-02 3.740648894508e-02 -4.531938509995e-03 6.488563450445e-04 -2.219138427564e-02 4.007942559588e-02 -3.335054840904e-02 2.380110357792e-02 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/4-135_right/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | distance angle -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/5-back/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 1.182243478880e+00 2.234014563864e+00 1.603978220070e+00 3.188230000000e+00 2.828659903600e+00 1.313326224370e+00 1.185110122842e+00 2 | -2.085154535296e+00 -2.970366420314e+00 -2.710700374030e+00 3.141420000000e+00 -3.091725614925e+00 -2.611741510236e-01 -1.104478251892e+00 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/5-back/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 1.396257261104e-01 1.132211100248e-01 1.885175293610e-01 1.078748651564e-03 1.047831867686e-01 1.744863769923e-01 2.782873220912e-01 2 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/5-back/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 1.743348671454e-02 -2.226746133473e-02 5.093291206791e-02 -1.718002834200e-02 6.986492126773e-02 -4.771788706128e-02 1.000000000000e-05 0.000000000000e+00 4.526499172217e-02 -6.776768491256e-03 4.992666819568e-02 -2.829181390936e-02 1.928249842481e-02 2.964715695131e-02 2 | -2.226746133473e-02 7.037298526736e-02 -1.718002834200e-02 7.197522827380e-03 -4.771788706128e-02 4.039466887269e-02 0.000000000000e+00 1.000000000000e-05 -6.776768491256e-03 1.103308438455e-03 -2.829181390936e-02 4.207455484030e-02 2.964715695131e-02 1.792754716264e-01 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/5-back/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | distance angle -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/6-135_left/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 2.651673230590e+00 1.249140295933e+00 1.178711743173e+00 3.043117927756e+00 1.509072621351e+00 2.004148268283e+00 2 | 2.352540454770e+00 1.171187491343e+00 3.864439181047e-01 2.414884893304e+00 1.832293693685e+00 2.175834905154e+00 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/6-135_left/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 1.285806377768e-01 2.647298418711e-01 1.860630197444e-01 9.227270956796e-02 1.376492149522e-01 1.907045760875e-01 2 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/6-135_left/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 6.298260114841e-02 8.271109921937e-03 7.565093594327e-03 1.899889115752e-02 1.194643125212e-02 1.395997098321e-02 3.962169840482e-02 9.059688906661e-04 1.718232326217e-02 1.562936023133e-02 5.304035532112e-02 2.289154687349e-02 2 | 8.271109921937e-03 2.307319425815e-03 1.899889115752e-02 9.674963664787e-02 1.395997098321e-02 3.808957813929e-02 9.059688906661e-04 5.095416847423e-05 1.562936023133e-02 2.017736343520e-02 2.289154687349e-02 1.105020821248e-02 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/6-135_left/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | distance angle -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/7-90_left/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 2.818690794480e+00 3.201007640311e+00 1.569452759071e+00 1.297463772548e+00 2.205705111817e+00 2.331069498930e+00 2 | 1.506098901057e+00 1.563315364688e+00 6.858693168207e-01 4.579415568551e-01 1.195227747581e+00 1.438041662753e+00 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/7-90_left/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 1.495511215155e-01 8.306011494686e-02 3.387082009886e-01 8.082534269840e-02 3.177589306915e-01 3.009628915918e-02 2 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/7-90_left/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 2.419276648036e-02 6.530544635282e-03 8.414458370659e-03 1.084904727339e-04 6.778195317336e-02 7.917414117213e-02 2.550405001469e-02 2.264297690783e-02 6.616462220644e-02 4.531090136798e-02 1.533270809147e-02 5.615633637021e-03 2 | 6.530544635282e-03 2.178606743948e-03 1.084904727339e-04 3.853541416657e-05 7.917414117213e-02 9.442028678028e-02 2.264297690783e-02 2.056550466593e-02 4.531090136798e-02 3.235449940054e-02 5.615633637021e-03 2.094091226883e-03 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/7-90_left/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | distance angle -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/8-45_left/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 2.257902511657e+00 1.426869769623e+00 1.664387061236e+00 2.954243176941e+00 2 | 5.270926154830e-01 1.303735594266e-01 1.860958934319e-01 7.432933748136e-01 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/8-45_left/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 3.927173916708e-01 1.962308244253e-01 1.845388301680e-01 2.265129537359e-01 2 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/8-45_left/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 6.897973324989e-02 2.999909120845e-02 4.870131479068e-02 1.580922390655e-02 1.048056339432e-01 4.486397980502e-02 3.467544097779e-02 4.050846809597e-03 2 | 2.999909120845e-02 1.472507680954e-02 1.580922390655e-02 5.266007187243e-03 4.486397980502e-02 1.924856398162e-02 4.050846809597e-03 6.606373720403e-04 3 | -------------------------------------------------------------------------------- /gmm_sampling/only_one_homotopy_tasks/8-45_left/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | d a -------------------------------------------------------------------------------- /gmm_sampling/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gmm_sampling 4 | 0.0.0 5 | The gmm_sampling package 6 | 7 | 8 | 9 | 10 | Noé Pérez 11 | 12 | 13 | 14 | 15 | 16 | BSD 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 | roscpp 44 | message_generation 45 | visualization_msgs 46 | 47 | roscpp 48 | message_runtime 49 | visualization_msgs 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /gmm_sampling/srv/GetApproachGMMProb.srv: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 person_orientation #orientation in the robot frame 4 | --- 5 | float32 prob 6 | -------------------------------------------------------------------------------- /gmm_sampling/srv/GetApproachGMMProbs.srv: -------------------------------------------------------------------------------- 1 | float32[] x 2 | float32[] y 3 | float32 person_orientation #orientation in the robot frame 4 | --- 5 | float32[] probs 6 | -------------------------------------------------------------------------------- /gmm_sampling/srv/GetApproachGMMSamples.srv: -------------------------------------------------------------------------------- 1 | float32 person_orientation #orientation in the robot frame 2 | int32 num_samples 3 | --- 4 | float32[] distances 5 | float32[] orientations 6 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/1-front/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 2.660155958932e+00 1.604341748585e+00 2 | 1.571755171350e-02 -2.095383885391e-02 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/1-front/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 4.426574705389e-01 5.573425294611e-01 2 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/1-front/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 1.523503161989e-01 -2.000086695706e-03 1.982513253983e-01 5.807082823132e-03 2 | -2.000086695706e-03 9.276876962664e-05 5.807082823132e-03 4.858128980807e-04 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/1-front/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | d a -------------------------------------------------------------------------------- /gmm_sampling/tasks/2-45_right/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 2.350401529513e+00 1.105718038555e+00 1.435914442397e+00 3.037379413699e+00 2 | -5.305276615191e-01 -6.427064032219e-02 -2.299604667530e-01 -7.097308713768e-01 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/2-45_right/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 4.514973278444e-01 7.384086542909e-02 3.192233289263e-01 1.554384778002e-01 2 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/2-45_right/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 1.373994037356e-01 -3.717106404937e-02 2.447807280581e-02 -1.260638906516e-02 1.155488258795e-01 -3.316545115709e-02 2.756514308723e-02 -3.307914488824e-03 2 | -3.717106404937e-02 1.086146340685e-02 -1.260638906516e-02 6.572835255278e-03 -3.316545115709e-02 9.645105477891e-03 -3.307914488824e-03 4.869804684017e-04 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/2-45_right/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | distance angle -------------------------------------------------------------------------------- /gmm_sampling/tasks/3-90_right/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 2.247477016085e+00 1.488468033539e+00 3.090080799333e+00 2.712089260371e+00 1.045931027814e+00 1.673639916075e+00 1.416512895913e+00 1.300507731999e+00 1.290885073254e+00 2.532804128755e+00 1.243453405983e+00 1.440965912214e+00 1.039485997541e+00 2 | -1.161880501090e+00 -6.080985342020e-01 -1.508914516834e+00 -1.426759065107e+00 -2.236740073460e-01 -6.752229264737e-01 1.271643831961e+00 2.112281619576e+00 8.540208856030e-01 -1.676814463751e+00 2.911125701260e+00 -2.448566895890e+00 5.268065977294e-01 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/3-90_right/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 3.002212461028e-01 1.422854654871e-01 1.302952252031e-01 1.202035177837e-01 1.482886477592e-01 1.587058976641e-01 9.667784681668e-02 2.374870670006e-01 5.607434109796e-02 2.236589273930e-01 6.066835937484e-02 2.440066877489e-01 8.142677056795e-02 2 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/3-90_right/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 7.286168186684e-02 -3.205934337171e-02 2.532640289804e-02 -3.093659197123e-02 3.510799883695e-02 -3.796117951190e-03 5.690512516290e-02 -9.899513370008e-03 2.206758290746e-02 -1.372023813724e-02 4.940329248764e-02 -4.356824210823e-02 3.960456503951e-04 7.871872006422e-04 3.273972294660e-03 -3.760235895502e-03 3.647163512912e-03 1.128748188730e-03 1.766599836654e-01 4.854514347455e-02 5.250759066793e-03 -3.237304688257e-03 5.898891662889e-02 7.284253168644e-02 1.715315472989e-02 1.203021649384e-02 2 | -3.205934337171e-02 2.161600649982e-02 -3.093659197123e-02 4.080412133450e-02 -3.796117951190e-03 4.584344501274e-04 -9.899513370008e-03 2.258722259719e-03 -1.372023813724e-02 1.355984801346e-02 -4.356824210823e-02 3.878521646454e-02 7.871872006422e-04 7.064503347269e-02 -3.760235895502e-03 2.165425230651e-01 1.128748188730e-03 2.540317371716e-02 4.854514347455e-02 1.518042597102e-02 -3.237304688257e-03 2.192247495310e-02 7.284253168644e-02 1.316272591166e-01 1.203021649384e-02 2.680715990685e-02 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/3-90_right/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | distance angle -------------------------------------------------------------------------------- /gmm_sampling/tasks/4-135_right/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 2.707447682802e+00 1.358780674276e+00 1.085916462433e+00 3.078796872337e+00 1.593658010078e+00 2.101494842405e+00 2.471241760500e+00 1.912486673984e+00 2.992198000845e+00 2.119733681808e+00 1.386175695602e+00 1.378243719877e+00 1.588332869741e+00 2 | -2.181639648242e+00 -9.672580250147e-01 -4.682115384991e-01 -2.302114160952e+00 -1.457963767232e+00 -1.930535890679e+00 -2.443278478418e+00 -2.889232755380e+00 -2.357517832466e+00 -2.579195840164e+00 7.524120542647e-01 2.076784606170e+00 -2.889536021410e+00 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/4-135_right/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 1.434364516533e-01 1.389167408594e-01 1.896802030955e-01 9.678690498058e-02 2.016924073407e-01 2.294872920704e-01 7.134916566009e-02 3.051713854134e-02 9.011108259151e-02 7.801235913293e-02 2.205958267187e-01 4.119501050398e-01 9.746432231555e-02 2 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/4-135_right/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 5.593629458760e-02 -1.700688567668e-02 1.133571967351e-02 -1.447001317516e-02 2.005136868457e-02 -2.540330191316e-02 4.014630616607e-02 -4.531938509995e-03 3.213286621096e-02 -2.219138427564e-02 7.607325122170e-02 -3.335054840904e-02 5.763434127078e-02 8.552622390210e-03 2.531801537332e-02 2.133530477764e-02 4.047085270785e-02 4.290299923652e-03 6.192400414477e-02 1.290120031299e-02 6.870930194785e-02 5.974645236989e-02 1.458186967365e-02 -3.688928654957e-02 1.920899052469e-02 1.896372211955e-02 2 | -1.700688567668e-02 5.526935972697e-03 -1.447001317516e-02 2.865372534961e-02 -2.540330191316e-02 3.740648894508e-02 -4.531938509995e-03 6.488563450445e-04 -2.219138427564e-02 4.007942559588e-02 -3.335054840904e-02 2.380110357792e-02 8.552622390210e-03 1.666358796740e-03 2.133530477764e-02 1.828337864978e-02 4.290299923652e-03 5.058221699213e-04 1.290120031299e-02 5.013160574814e-03 5.974645236989e-02 8.375789424828e-02 -3.688928654957e-02 4.006695341555e-01 1.896372211955e-02 2.093686079412e-02 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/4-135_right/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | distance angle -------------------------------------------------------------------------------- /gmm_sampling/tasks/5-back/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 1.182243478880e+00 2.234014563864e+00 1.603978220070e+00 3.188230000000e+00 2.828659903600e+00 1.313326224370e+00 1.185110122842e+00 2.304594192507e+00 1.346369009693e+00 2.867880958013e+00 1.746108247339e+00 3.173673225806e+00 1.339610933232e+00 1.426028027634e+00 2 | -2.085154535296e+00 -2.970366420314e+00 -2.710700374030e+00 3.141420000000e+00 -3.091725614925e+00 -2.611741510236e-01 -1.104478251892e+00 2.994223019829e+00 2.835214959804e-01 3.112746593870e+00 2.672976681169e+00 -3.133972258065e+00 2.059756706598e+00 1.093184785957e+00 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/5-back/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 1.396257261104e-01 1.132211100248e-01 1.885175293610e-01 1.078748651564e-03 1.047831867686e-01 1.744863769923e-01 2.782873220912e-01 1.133370319572e-01 1.447265951743e-01 8.759838210526e-02 1.664236684648e-01 3.090727816550e-02 1.622580849194e-01 2.947489592135e-01 2 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/5-back/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 1.743348671454e-02 -2.226746133473e-02 5.093291206791e-02 -1.718002834200e-02 6.986492126773e-02 -4.771788706128e-02 1.000000000000e-05 0.000000000000e+00 4.526499172217e-02 -6.776768491256e-03 4.992666819568e-02 -2.829181390936e-02 1.928249842481e-02 2.964715695131e-02 3.771818210343e-02 1.095065800717e-02 2.558620515875e-02 1.830022146543e-02 3.055699594910e-02 3.398241629033e-03 3.956104339440e-02 2.729667971359e-02 1.426883686701e-02 1.378047782518e-04 7.179543570189e-03 1.526717346310e-02 1.234348906757e-02 -2.719072890815e-02 2 | -2.226746133473e-02 7.037298526736e-02 -1.718002834200e-02 7.197522827380e-03 -4.771788706128e-02 4.039466887269e-02 0.000000000000e+00 1.000000000000e-05 -6.776768491256e-03 1.103308438455e-03 -2.829181390936e-02 4.207455484030e-02 2.964715695131e-02 1.792754716264e-01 1.095065800717e-02 3.502701934596e-03 1.830022146543e-02 2.352951695455e-02 3.398241629033e-03 4.420342948635e-04 2.729667971359e-02 2.374666170330e-02 1.378047782518e-04 7.097498522373e-05 1.526717346310e-02 6.098702599570e-02 -2.719072890815e-02 1.403649585715e-01 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/5-back/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | distance angle -------------------------------------------------------------------------------- /gmm_sampling/tasks/6-135_left/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 2.651673230590e+00 1.249140295933e+00 1.178711743173e+00 3.043117927756e+00 1.509072621351e+00 2.004148268283e+00 2.309210068852e+00 1.291312178361e+00 3.022712476448e+00 1.675746665768e+00 1.405646962368e+00 1.243087787507e+00 1.172065648203e+00 2 | 2.352540454770e+00 1.171187491343e+00 3.864439181047e-01 2.414884893304e+00 1.832293693685e+00 2.175834905154e+00 2.628215236827e+00 -2.843946672597e+00 2.450178713316e+00 2.943786737434e+00 -8.391780108823e-01 -2.343899227084e-01 -1.830485003305e+00 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/6-135_left/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 1.285806377768e-01 2.647298418711e-01 1.860630197444e-01 9.227270956796e-02 1.376492149522e-01 1.907045760875e-01 1.647929674626e-01 1.166387177188e-01 8.977227888317e-02 1.072709372726e-01 1.728176245576e-01 9.982862391100e-02 2.488788501942e-01 2 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/6-135_left/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 6.298260114841e-02 8.271109921937e-03 7.565093594327e-03 1.899889115752e-02 1.194643125212e-02 1.395997098321e-02 3.962169840482e-02 9.059688906661e-04 1.718232326217e-02 1.562936023133e-02 5.304035532112e-02 2.289154687349e-02 9.480695984193e-02 -3.008852068678e-02 1.754293268824e-02 -1.756942709114e-02 3.174543610385e-02 -4.323615067125e-03 3.929080112202e-02 -1.821466013141e-02 1.139223944118e-02 -9.334031085912e-03 1.581903754702e-02 -1.541639543556e-02 8.824852220639e-03 2.051304728955e-02 2 | 8.271109921937e-03 2.307319425815e-03 1.899889115752e-02 9.674963664787e-02 1.395997098321e-02 3.808957813929e-02 9.059688906661e-04 5.095416847423e-05 1.562936023133e-02 2.017736343520e-02 2.289154687349e-02 1.105020821248e-02 -3.008852068678e-02 1.032084172808e-02 -1.756942709114e-02 4.136900030744e-02 -4.323615067125e-03 8.522434285047e-04 -1.821466013141e-02 1.245296400757e-02 -9.334031085912e-03 8.862827979216e-02 -1.541639543556e-02 1.836541999842e-02 2.051304728955e-02 2.370716455838e-01 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/6-135_left/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | distance angle -------------------------------------------------------------------------------- /gmm_sampling/tasks/7-90_left/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 2.818690794480e+00 3.201007640311e+00 1.569452759071e+00 1.297463772548e+00 2.205705111817e+00 2.331069498930e+00 2.703394771164e+00 1.335751639553e+00 3.130982597420e+00 1.528595091778e+00 1.223335136289e+00 1.149745019315e+00 2.107354352490e+00 2 | 1.506098901057e+00 1.563315364688e+00 6.858693168207e-01 4.579415568551e-01 1.195227747581e+00 1.438041662753e+00 1.657531347206e+00 -7.768603750555e-01 1.587026732719e+00 2.178680115254e+00 -2.132294845108e+00 2.742589521666e+00 1.845734259986e+00 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/7-90_left/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 1.495511215155e-01 8.306011494686e-02 3.387082009886e-01 8.082534269840e-02 3.177589306915e-01 3.009628915918e-02 7.912882806668e-02 2.042399166411e-01 4.621652360195e-02 1.042129159672e-01 3.272625066243e-01 1.176262125415e-01 1.213130965573e-01 2 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/7-90_left/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 2.419276648036e-02 6.530544635282e-03 8.414458370659e-03 1.084904727339e-04 6.778195317336e-02 7.917414117213e-02 2.550405001469e-02 2.264297690783e-02 6.616462220644e-02 4.531090136798e-02 1.533270809147e-02 5.615633637021e-03 5.439404138178e-02 -9.509681529477e-03 3.483390928827e-02 -4.753737379904e-02 1.487069235036e-02 -1.596968280398e-03 4.715699626005e-02 -2.804788043792e-02 9.346369165350e-03 2.480344381117e-02 9.561239908677e-03 -1.042722512271e-02 9.644445200359e-02 -2.868880772007e-02 2 | 6.530544635282e-03 2.178606743948e-03 1.084904727339e-04 3.853541416657e-05 7.917414117213e-02 9.442028678028e-02 2.264297690783e-02 2.056550466593e-02 4.531090136798e-02 3.235449940054e-02 5.615633637021e-03 2.094091226883e-03 -9.509681529477e-03 2.172834088736e-03 -4.753737379904e-02 1.395193614069e-01 -1.596968280398e-03 2.347484522424e-04 -2.804788043792e-02 2.381066071242e-02 2.480344381117e-02 3.649882718303e-01 -1.042722512271e-02 5.526672670749e-02 -2.868880772007e-02 1.047298012755e-02 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/7-90_left/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | distance angle -------------------------------------------------------------------------------- /gmm_sampling/tasks/8-45_left/GMM_mu.txt: -------------------------------------------------------------------------------- 1 | 2.257902511657e+00 1.426869769623e+00 1.664387061236e+00 2.954243176941e+00 2 | 5.270926154830e-01 1.303735594266e-01 1.860958934319e-01 7.432933748136e-01 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/8-45_left/GMM_priors.txt: -------------------------------------------------------------------------------- 1 | 3.927173916708e-01 1.962308244253e-01 1.845388301680e-01 2.265129537359e-01 2 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/8-45_left/GMM_sigma.txt: -------------------------------------------------------------------------------- 1 | 6.897973324989e-02 2.999909120845e-02 4.870131479068e-02 1.580922390655e-02 1.048056339432e-01 4.486397980502e-02 3.467544097779e-02 4.050846809597e-03 2 | 2.999909120845e-02 1.472507680954e-02 1.580922390655e-02 5.266007187243e-03 4.486397980502e-02 1.924856398162e-02 4.050846809597e-03 6.606373720403e-04 3 | -------------------------------------------------------------------------------- /gmm_sampling/tasks/8-45_left/GMM_vars.txt: -------------------------------------------------------------------------------- 1 | d a -------------------------------------------------------------------------------- /navigation_features/.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | 31 | # Others 32 | *~ 33 | -------------------------------------------------------------------------------- /navigation_features/README.md: -------------------------------------------------------------------------------- 1 | # navigation_features 2 | Package that calculates the feature functions employed by a cost function for robot navigation. 3 | 4 | ## Parameters 5 | 6 | * **Feature set parameters** 7 | 8 | - upo_featureset. Two sets of features are available. 9 | Value '0' for the set of 3 features: goal distance, obstacle distance and Gaussian function over people. 10 | Value '1' for the set of 5 features: goal distance, obstacle distance, Gaussian function in the person front, Gaussian function in the person back and Gaussian function in the person right side. 11 | - w1. Normalized weight for the feature of goal distance. 12 | - w2. Normalized weight for the feature of distance to the closest obstacle. 13 | - w3. If 'upo_featureset' is '0', this value is the normalized weight for the feature of the maximum value of the Gaussian functions deployed over the people. If 'upo_featureset' is '1', the weight corresponds to the feature of the Gaussian function in the front of the people. 14 | - w4. Normalized weight for the feature of the Gaussian function in the back of the people. Only used if 'upo_featureset' is '1'. 15 | - w5. Normalized weight for the feature of the Gaussian function in the right side of the people. Only used if 'upo_featureset' is '1'. 16 | 17 | 18 | * **Gaussian functions parameters** 19 | 20 | - stddev_person_front. Value of the standard deviation in the X axis of the Gaussian function in the front of the person. 21 | - stddev_person_aside. Value of the standard deviation in the Y axis of the Gaussian function in the front of the person. This value is also used to define the standard deviation in X and Y axis of the Gaussian function in the back of the person 22 | - stddev_person_right. Value of the standard deviation in the X axis of the Gausssian function in the right side of the person. 23 | 24 | 25 | * **Parameters for obstacle distance feature** 26 | 27 | - use_laser_projection. Boolean to indicate whether to use a projection of the laser readings onto the map and a distance transform to calculate the feature of the distance to closest obstacle. If not, the ROS costmaps are used. 28 | - pc_topic. Name of the topic to subscribe to with the point cloud of the sensor readings for projection onto the map. 29 | - pc_type. Indicate the type of ROS point cloud message. Value '1' for sensor_msgs/PointCloud, Value '2' for sensor_msgs/PointCloud2. 30 | 31 | 32 | 33 | ## Dependences 34 | 35 | * **upo_msgs** is required. 36 | * **Open_CV** is also needed. 37 | 38 | 39 | The package is a **work in progress** used in research prototyping. Pull requests and/or issues are highly encouraged. 40 | -------------------------------------------------------------------------------- /navigation_features/cfg/nav_features.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = 'navigation_features' 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | 10 | gen.add("upo_featureset", int_t, 0, "The upo feature set to be used.", 0, 0, 1); 11 | #gen.add("use_global_map", bool_t, 0, "Whether or not to use the static ros map", False); 12 | 13 | gen.add("stddev_person_front", double_t, 0, "std dev of the gaussian function in front of the person", 1.2, 0.2, 3.0); 14 | gen.add("stddev_person_aside", double_t, 0, "std dev of the gaussian function aside the person", 0.8, 0.2, 3.0); 15 | gen.add("stddev_person_right", double_t, 0, "std dev of the gaussian function placed to the right of the person", 0.8, 0.2, 3.0); 16 | 17 | gen.add("enable_grouping", bool_t, 0, "Whether or not to group the people detected", False); 18 | gen.add("stddev_group", double_t, 0, "std dev of the rounded gaussian function around a group of people", 0.8, 0.2, 3.0); 19 | gen.add("grouping_distance", double_t, 0, "Max distance in meters between people to consider that they are in the same group", 1.5, 0.5, 2.5); 20 | 21 | gen.add("interaction_target_id", int_t, 0, "Interaction target id", -1, -1, 50000); 22 | gen.add("it_remove_gaussian", bool_t, 0, "Whether or not to remove the Gaussian costs around the interaction target", False); 23 | 24 | exit(gen.generate(PACKAGE, "nav_features", "nav_features")) 25 | -------------------------------------------------------------------------------- /navigation_features/srv/GetFeatureCount.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped[] path 2 | --- 3 | float32[] fc 4 | -------------------------------------------------------------------------------- /navigation_features/srv/InitWeights.srv: -------------------------------------------------------------------------------- 1 | bool random 2 | bool normalize 3 | --- 4 | float32[] weights 5 | -------------------------------------------------------------------------------- /navigation_features/srv/PoseValid.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped pose 2 | --- 3 | bool ok 4 | -------------------------------------------------------------------------------- /navigation_features/srv/SetLossCost.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped[] demo_path 2 | bool use_loss_func 3 | --- 4 | 5 | -------------------------------------------------------------------------------- /navigation_features/srv/SetScenario.srv: -------------------------------------------------------------------------------- 1 | sensor_msgs/PointCloud2 obstacles 2 | upo_msgs/PersonPoseArrayUPO people 3 | geometry_msgs/PoseStamped goal 4 | --- 5 | 6 | -------------------------------------------------------------------------------- /navigation_features/srv/SetWeights.srv: -------------------------------------------------------------------------------- 1 | float32[] weights 2 | --- 3 | -------------------------------------------------------------------------------- /path_prediction/.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | 31 | # Others 32 | *~ 33 | -------------------------------------------------------------------------------- /path_prediction/README.md: -------------------------------------------------------------------------------- 1 | # Path prediction 2 | Package, written in Python, used in navigation with path planning prediction. The predicition is done by a Fully Convolutional Network trained in Keras. The package provide a ROS service that receives a flattened input grid, and return the respective path planning predicition. It's called from the library RRT_ros_wrapper3 (see package upo_rrt_planners). 3 | 4 | 5 | ## Dependences 6 | 7 | * **Keras** is required. 8 | * **Open_CV** is also needed. 9 | 10 | 11 | The package is a **work in progress** used in research prototyping. Pull requests and/or issues are highly encouraged. 12 | -------------------------------------------------------------------------------- /path_prediction/model/my_model.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/path_prediction/model/my_model.h5 -------------------------------------------------------------------------------- /path_prediction/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | path_prediction 4 | 0.0.0 5 | The path_prediction package 6 | 7 | 8 | 9 | 10 | Noé Pérez 11 | 12 | 13 | 14 | 15 | 16 | BSD 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 | cv_bridge 44 | rospy 45 | message_generation 46 | cv_bridge 47 | rospy 48 | message_runtime 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /path_prediction/srv/PathPrediction.srv: -------------------------------------------------------------------------------- 1 | int32[] input 2 | int16 input_rows 3 | int16 input_cols 4 | --- 5 | float32[] prediction 6 | uint8 pred_rows 7 | uint8 pred_cols 8 | -------------------------------------------------------------------------------- /simple_local_planner/.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | 31 | # Others 32 | *~ 33 | -------------------------------------------------------------------------------- /simple_local_planner/README.md: -------------------------------------------------------------------------------- 1 | # simple_local_planner 2 | A controller based on pure pursuit path tracking has been extended to command velocities to the differential robot so as to follow the path smoothly. 3 | This path tracker runs at a frequency of 15Hz and has been extended to perform a collision detection checking similar to the Dynamic Windows Approach algorithm. If the forward projection of the robot movement given by the control law is detected as a possible collision, a valid command is tried to be found by sampling small variations of the given angular velocity. If a possible collision is still detected, a rotation in place in the correct direction is performed in order to avoid very close obstacles. Moreover, a linear decrease of the velocities when approaching the goal has been also added. 4 | 5 | ## Parameters 6 | 7 | * **Robot Configuration Parameters** 8 | - max_trans_acc. Maximum acceleration in translation (m/s^2). 9 | - max_rot_acc. Maximum acceleration in rotation (rad/s^2). 10 | - max_trans_vel. Maximum linear velocity (m/s). 11 | - min_trans_vel. Minimum linear velocity (m/s). 12 | - max_rot_vel. Maximum angular velocity (rad/s). 13 | - min_rot_vel. Minimum angular velocity (rad/s). 14 | - min_in_place_rot_vel. Angular velocity of rotations in place (rad/s). 15 | 16 | * **Goal Tolerance Parameters** 17 | - yaw_goal_tolerance. Tolerance in angular distance (rad) to consider that the goal has been reached. 18 | - xy_goal_tolerance. Tolerance in euclidean distance (m) to consider that the goal has been reached. 19 | - wp_tolerance. Distance (m) from the robot to look for the point of the global plan to follow. 20 | 21 | * **Forward Simulation Parameters** 22 | - sim_time. Time (seconds) to expand the robot movement and check for collisions. (default: 0.5). 23 | - sim_granularity. Resolution in meters to split the expanded trayectory and check for collisions (Default: 0.025). 24 | - angular_sim_granularity. Resolution in radians to split the expanded angular movement and check for collisions (Default: 0.025). 25 | - controller_freq. Frequency of execution in Hz (Default: 15.0). 26 | 27 | 28 | The package is a **work in progress** used in research prototyping. Pull requests and/or issues are highly encouraged. 29 | -------------------------------------------------------------------------------- /simple_local_planner/cfg/SimpleLocalPlanner.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = 'simple_local_planner' 4 | 5 | from math import pi 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator() 10 | 11 | 12 | gen.add("max_trans_acc", double_t, 0, "The translational acceleration limit of the robot", 1.0, 0, 20.0) 13 | gen.add("max_rot_acc", double_t, 0, "The rotational acceleration limit of the robot", 1.0, 0, 20.0) 14 | 15 | 16 | gen.add("max_trans_vel", double_t, 0, "The maximum translation velocity for the robot in m/s", 0.6, 0, 10.0) 17 | gen.add("min_trans_vel", double_t, 0, "The minimum translation velocity for the robot in m/s", 0.1, 0, 10.0) 18 | gen.add("max_rot_vel", double_t, 0, "The absolute value of the maximum rotational velocity for the robot in rad/s", 0.8, 0, 20.0) 19 | gen.add("min_rot_vel", double_t, 0, "The absolute value of the minimum rotational velocity for the robot in rad/s", 0.3, 0, 20.0) 20 | gen.add("min_in_place_rot_vel", double_t, 0, "The absolute value of the minimum in-place rotational velocity the controller will explore", 0.5, 0, 20.0) 21 | 22 | 23 | gen.add("yaw_goal_tolerance", double_t, 0, "Tolerance in angle (rad/s) to reach the goal", 0.12, 0, 3.0) 24 | gen.add("xy_goal_tolerance", double_t, 0, "Tolerance in distance (m) to reach the goal", 0.20, 0, 2.0) 25 | gen.add("wp_tolerance", double_t, 0, "Distance from the robot to the point of the global path to take", 0.5, 0, 2.0) 26 | 27 | 28 | gen.add("sim_time", double_t, 0, "The amount of time to expand the robot movement in seconds", 0.5, 0, 10) 29 | gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0, 5) 30 | gen.add("angular_sim_granularity", double_t, 0, "The distance between simulation points for angular velocity should be small enough that the robot doesn't hit things", 0.025, 0, pi/2) 31 | 32 | gen.add("sample_angular_vels", bool_t, 0, "Whether or not to look for a valid command by sampling angular velocities", True); 33 | 34 | 35 | exit(gen.generate(PACKAGE, "simple_local_planner", "SimpleLocalPlanner")) 36 | -------------------------------------------------------------------------------- /simple_local_planner/plp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of a simple local controller to follow a pre-defined path. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /upo_decision_making/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(upo_decision_making) 3 | 4 | SET ( CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMakeModules ) 5 | 6 | find_package(catkin REQUIRED dynamic_reconfigure 7 | message_generation 8 | actionlib_msgs 9 | actionlib ) 10 | 11 | #find_package( MADP ) 12 | 13 | add_message_files( 14 | FILES 15 | ControlEvent.msg 16 | IDArray.msg 17 | HRIControlMode.msg 18 | HRIFeedbackEvent.msg 19 | HRIFeedbackFromInterface.msg 20 | HRIFeedbackStateInfo.msg 21 | ) 22 | 23 | #add_service_files( 24 | # FILES 25 | # TestPolicy.srv 26 | #) 27 | 28 | ## Generate actions in the 'action' folder 29 | #add_action_files( 30 | # FILES 31 | # InformedLookAround.action 32 | # LookAround.action 33 | # LookForInteractionTargets.action 34 | # MaintainHeadingToInteractionTargets.action 35 | # MaintainInteractionRange.action 36 | # MoveToInteractionRange.action 37 | # StabilizeHeadPose.action 38 | # RepositionForConversation.action 39 | # TurnToInteractionTargets.action 40 | # NNPolicy.action 41 | #) 42 | 43 | generate_messages( 44 | DEPENDENCIES 45 | std_msgs 46 | actionlib_msgs 47 | ) 48 | 49 | #generate_dynamic_reconfigure_options( 50 | # cfg/HRIActionParams.cfg 51 | # cfg/PIDParams.cfg 52 | #) 53 | 54 | catkin_package( 55 | CATKIN_DEPENDS #mdm_library 56 | ) 57 | 58 | include_directories( 59 | include 60 | ${catkin_INCLUDE_DIRS} 61 | ) 62 | #link_directories(/usr/local/lib/) 63 | 64 | #if ( MADP_FOUND ) 65 | # add_executable(mopomdp_action_layer src/mopomdp_action_layer.cpp) 66 | # target_link_libraries(mopomdp_action_layer mdm_action_layer ${catkin_LIBRARIES}) 67 | 68 | # add_executable(mopomdp_control_layer src/mopomdp_control_layer.cpp) 69 | # target_link_libraries(mopomdp_control_layer mdm_control_layer MADPBase ${catkin_LIBRARIES}) 70 | 71 | #endif ( MADP_FOUND ) 72 | -------------------------------------------------------------------------------- /upo_decision_making/README.md: -------------------------------------------------------------------------------- 1 | # upo_decision_making 2 | Python scripts that contains the finite state machine for the navigation macro-actions and the interaction with the corresponding actions defined through the ROS actionlib library. 3 | 4 | The package is a **work in progress** used in research prototyping. Pull requests and/or issues are highly encouraged. 5 | -------------------------------------------------------------------------------- /upo_decision_making/cfg/centre_sportif_waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: -24.179 3 | y: -2.617 4 | theta: -0.592 5 | Base: 6 | x: -24.179 7 | y: -2.617 8 | theta: -0.592 9 | Couloir: 10 | x: -19.15 11 | y: -3.52 12 | theta: -2.062 13 | Salle: 14 | x: -23.54 15 | y: -9.52 16 | theta: -0.508 17 | Salle d'expositions: 18 | x: -30.63 19 | y: -15.57 20 | theta: -2.068 21 | Coin cafe: 22 | x: -16.40 23 | y: -19.31 24 | theta: -2.036 25 | -------------------------------------------------------------------------------- /upo_decision_making/cfg/elderly_center_waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 8.45 3 | y: 17.09 4 | theta: -1.57 5 | cafeteria: 6 | x: 13.61 7 | y: 11.5 8 | theta: 0.0 9 | couloir: 10 | x: 20.84 11 | y: 10.98 12 | theta: -3.13 13 | 14 | -------------------------------------------------------------------------------- /upo_decision_making/cfg/leds.yaml: -------------------------------------------------------------------------------- 1 | nav_plan: [50,250,50] 2 | selected_it: [29, 82, 255] 3 | interaction_mode_color: [29, 90,255] 4 | seek_interaction_color: [200,30,200] 5 | other_it: [6, 216, 255] 6 | manual: [200,200, 200] 7 | nav_home: [255,13,0] 8 | navigate_it_color: [70,26,160] 9 | wsbs_color: [255,124, 0] 10 | wait_color: [140,50,255] 11 | yield_color: [200,160,20] 12 | state_width: 5 13 | state_index: 45 14 | 15 | 16 | -------------------------------------------------------------------------------- /upo_decision_making/cfg/upo_lab_waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 12.9903 3 | y: 17.6948 4 | theta: -2.344 5 | wp1: 6 | x: 11.8 7 | y: 15.067 8 | theta: -0.792 9 | wp2: 10 | x: 9.2027 11 | y: 14.3288 12 | theta: -0.792 13 | wp3: 14 | x: 12.2683 15 | y: 12.0161 16 | theta: 0.783 17 | wp4: 18 | x: 10.3542 19 | y: 5.6311 20 | theta: 1.489 21 | wp5: 22 | x: 9.9458 23 | y: 13.5393 24 | theta: 2.354 25 | wp6: 26 | x: 10.8870 27 | y: 15.5389 28 | theta: 0.813 29 | wp7: 30 | x: 13.0767 31 | y: 16.4580 32 | theta: 2.348 33 | -------------------------------------------------------------------------------- /upo_decision_making/cfg/uva_demo_waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 39.7 3 | y: 17.45 4 | theta: 1.75 5 | lab_corridor: 6 | x: 36.29 7 | y: 25.625 8 | theta: 1.75 9 | automatic_door: 10 | x: 28.0 11 | y: 21.4 12 | theta: 0.4 13 | hall: 14 | x: 37.4 15 | y: 22.7 16 | theta: -1.4 17 | narrow_corridor: 18 | x: 26.5 19 | y: 14.5 20 | theta: 0.4 21 | student_corridor: 22 | x: 44.25 23 | y: 12.25 24 | theta: 1.75 25 | glass_wall: 26 | x: 43.55 27 | y: 20.20 28 | theta: -3.0 29 | #printer: 30 | # x: 11.45 31 | # y: 16.15 32 | # theta: 0.4 33 | -------------------------------------------------------------------------------- /upo_decision_making/cfg/uva_lab_waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 25.95 3 | y: 25.75 4 | theta: 0.79 5 | lab1: 6 | x: 22.25 7 | y: 21.55 8 | theta: 0.79 9 | coffee: 10 | x: 28.25 11 | y: 28.95 12 | theta: 0.79 13 | corridor: 14 | x: 21.7 15 | y: 16.85 16 | theta: -0.60 17 | 18 | -------------------------------------------------------------------------------- /upo_decision_making/cfg/uva_sim_waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 25.95 3 | y: 25.75 4 | theta: 0.79 5 | lab1: 6 | x: 22.25 7 | y: 21.55 8 | theta: 0.79 9 | coffee: 10 | x: 28.25 11 | y: 28.95 12 | theta: 0.79 13 | corridor: 14 | x: 21.7 15 | y: 16.85 16 | theta: -0.60 17 | 18 | -------------------------------------------------------------------------------- /upo_decision_making/cfg/waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | interaction room: 2 | x: 16.63 3 | y: 11.41 4 | theta: 0.0 5 | home: 6 | x: 8.35 7 | y: 11.08 8 | theta: 0.0 9 | #home: 10 | # x: 7.65 11 | # y: 9.0 12 | # theta: 1.57 13 | hall: 14 | x: 26.07 15 | y: 10.65 16 | theta: 0.0 17 | -------------------------------------------------------------------------------- /upo_decision_making/msg/ControlEvent.msg: -------------------------------------------------------------------------------- 1 | #This message encodes control events that are internal to the robot or 2 | #directly related to the execution of its macro-actions 3 | 4 | #standard header, can include frame information 5 | time stamp 6 | 7 | #The type of the event that was detected 8 | uint8 type 9 | 10 | uint8 NAV_GOAL_REACHED=0 11 | uint8 CALL_OVER=1 12 | uint8 LOW_BATT=2 13 | uint8 NAV_TO_WAYPOINT_REQUESTED=3 14 | uint8 NAV_TO_PERSON_REQUESTED=4 15 | uint8 INTERACTION_TARGET_AT_UNSAFE_RANGE=5 16 | uint8 INTERACTION_TARGET_AT_PERSONAL_RANGE=6 17 | uint8 INTERACTION_TARGET_AT_SOCIAL_RANGE=7 18 | uint8 INTERACTION_TARGET_AT_PUBLIC_RANGE=8 19 | uint8 INTERACTION_TARGET_OUT_OF_RANGE=9 20 | uint8 LOST_TRACK_OF_INTERACTION_TARGET=10 21 | uint8 NEW_INTERACTION_TARGET=11 22 | uint8 WALK_WITH_PERSON_REQUESTED=12 23 | uint8 MANUAL_CONTROLLING=13 24 | uint8 HEARING_PROBLEMS_INDICATED=14 25 | uint8 REGAINED_TRACK_OF_INTERACTION_TARGET=15 26 | -------------------------------------------------------------------------------- /upo_decision_making/msg/HRIControlMode.msg: -------------------------------------------------------------------------------- 1 | uint8 mode 2 | 3 | uint8 AUTONOMOUS_MODE=0 4 | uint8 MANUAL_MODE=1 5 | uint8 EMERGENCY_STOP=2 6 | -------------------------------------------------------------------------------- /upo_decision_making/msg/HRIFeedbackEvent.msg: -------------------------------------------------------------------------------- 1 | #This message provides information about instantaneous detections of HRI-specific feedback events 2 | 3 | #standard header, can include frame information 4 | std_msgs/Header header 5 | 6 | #The confidence with which the event was detected 7 | float32 confidence 8 | 9 | #The type of the event that was detected 10 | uint8 type 11 | 12 | 13 | uint8 CONVSUB_TOUCHING_EARS=0 14 | uint8 CONVSUB_LEANING_DETECTION=1 15 | uint8 PILOT_FROWN=3 16 | uint8 BACKGROUND_NOISE_TOO_LOUD=4 17 | -------------------------------------------------------------------------------- /upo_decision_making/msg/HRIFeedbackFromInterface.msg: -------------------------------------------------------------------------------- 1 | #This message encodes feedback information provided by the pilot through the Giraff interface 2 | 3 | #standard header, can include frame information 4 | std_msgs/Header header 5 | 6 | #The type of feedback or command provided by the pilot 7 | uint8 type 8 | 9 | #serialized data 10 | uint8[] data 11 | 12 | # WARNING: DO NOT CHANGE THESE IDs (only add new ones) 13 | # THE INTERFACE PROTOCOL DEPENDS ON THEM 14 | uint8 NAV_TO_WAYPOINT_REQUESTED=0 15 | uint8 NAV_TO_PERSON_REQUESTED=1 16 | uint8 CALL_OVER=2 17 | uint8 WALK_WITH_PERSON_REQUESTED=3 18 | uint8 MANUAL_CONTROL_REQUESTED=4 19 | uint8 AUTONOMOUS_CONTROL_REQUESTED=5 20 | uint8 VOLUME_CHANGE_REQUESTED=6 21 | uint8 HEARING_PROBLEMS_INDICATED=7 22 | -------------------------------------------------------------------------------- /upo_decision_making/msg/HRIFeedbackStateInfo.msg: -------------------------------------------------------------------------------- 1 | string state_name 2 | string info 3 | -------------------------------------------------------------------------------- /upo_decision_making/msg/IDArray.msg: -------------------------------------------------------------------------------- 1 | #This message represents a list of unsigned integer identifiers, 2 | #which can be used, for instance, to encode the desired interaction 3 | #targets 4 | uint16[] ids 5 | -------------------------------------------------------------------------------- /upo_decision_making/scripts/hri_common.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | This file contains utility classes / methods that are common to all HRI actions 4 | """ 5 | 6 | import rospy as rp 7 | import numpy as np 8 | import tf 9 | import re 10 | import math 11 | import yaml 12 | import abc 13 | import threading 14 | import os 15 | import sys 16 | import pdb 17 | from geometry_msgs.msg import TransformStamped, PoseStamped 18 | #from spencer_tracking_msgs.msg import TrackedPersons 19 | #from sensor_msgs.msg import LaserScan 20 | from std_msgs.msg import Int32 21 | 22 | 23 | class WaypointParser(): 24 | def __init__(self, waypoint_defs_path): 25 | with open(waypoint_defs_path, 'r') as f: 26 | wayp_dict = yaml.load(f) 27 | 28 | self.wayp_dict = {} 29 | for (label, wp) in wayp_dict.items(): 30 | x = wp['x'] 31 | y = wp['y'] 32 | theta = wp['theta'] 33 | self.wayp_dict[label] = self.waypoint_to_ps(x, y, theta) 34 | 35 | def get_waypoint(self, label): 36 | return self.wayp_dict[label] 37 | 38 | def get_all_waypoints(self): 39 | return self.wayp_dict 40 | 41 | @staticmethod 42 | def waypoint_to_ps(x, y, theta): 43 | p = PoseStamped() 44 | p.header.stamp = rp.Time.now() 45 | p.header.frame_id = 'map' 46 | p.pose.position.x = x 47 | p.pose.position.y = y 48 | p.pose.orientation.z = math.sin(theta/2.0) 49 | p.pose.orientation.w = math.cos(theta/2.0) 50 | return p 51 | 52 | 53 | def to_stamped_transform(base_frame, target_frame, trans, rot): 54 | """ 55 | Transforms a TF between two frames from (trans, rot) to TransformStamped 56 | """ 57 | tfstamped = TransformStamped() 58 | tfstamped.child_frame_id = target_frame 59 | tfstamped.header.stamp = rp.Time.now() 60 | tfstamped.header.frame_id = base_frame 61 | tfstamped.transform.translation.x = trans[0] 62 | tfstamped.transform.translation.y = trans[1] 63 | tfstamped.transform.translation.z = trans[2] 64 | tfstamped.transform.rotation.x = rot[0] 65 | tfstamped.transform.rotation.y = rot[1] 66 | tfstamped.transform.rotation.z = rot[2] 67 | tfstamped.transform.rotation.w = rot[3] 68 | return tfstamped 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /upo_decision_making/scripts/hri_common.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_decision_making/scripts/hri_common.pyc -------------------------------------------------------------------------------- /upo_decision_making/scripts/smach_common.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy as rp 3 | import smach 4 | import threading 5 | 6 | from upo_decision_making.msg import ControlEvent 7 | 8 | 9 | class EventMonitorState(smach.State): 10 | def __init__(self, outcome_dict): 11 | smach.State.__init__(self, outcomes=outcome_dict.values() + ['aborted']) 12 | self._outcome_dict = outcome_dict 13 | self._ev_type = None 14 | self._sub = rp.Subscriber("behavior_manager/control_events", ControlEvent, self.callback, queue_size=1) 15 | self._thread_ev = threading.Event() 16 | 17 | def callback(self, msg): 18 | if msg.type in self._outcome_dict: 19 | self._ev_type = msg.type 20 | self._thread_ev.set() 21 | 22 | def execute(self, userdata): 23 | while not rp.is_shutdown(): 24 | if self._thread_ev.wait(1): 25 | self._thread_ev.clear() 26 | if self._ev_type in self._outcome_dict: 27 | return self._outcome_dict[self._ev_type] 28 | return 'aborted' 29 | -------------------------------------------------------------------------------- /upo_decision_making/scripts/smach_common.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_decision_making/scripts/smach_common.pyc -------------------------------------------------------------------------------- /upo_launchers/README.md: -------------------------------------------------------------------------------- 1 | # upo_launchers 2 | Package with some configuration files and the launch files to run simulations of the navigation macro-action system. 3 | 4 | Different scenarios are available (centre_sportif, elderly_center, upo_45, upo_lab, uva_demo and uva_lab). To run a simulation of the whole macro-action system for navigation, just move to the folder of the desired scenario and run the launch sim_(scenario_name)_nav_behavior.launch. 5 | 6 | Then, in other terminal, run a tester to launch the different macro-actions: 7 | rosrun upo_decision_making upo_nav_behavior_tester.py 8 | 9 | The navigation goals for each scenario are predefined, and can be consulted in the cfg folder. 10 | 11 | If you are only interested in testing the navigation to a goal, the navigation system without the macro-actions system can be lauched. Run the launch upo_navigation.launch in the folder elderly_center. Regular navigation goals sent through Rviz are accepted. 12 | 13 | 14 | -------------------------------------------------------------------------------- /upo_launchers/assisted_steering.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 | -------------------------------------------------------------------------------- /upo_launchers/centre_sportif/map.inc: -------------------------------------------------------------------------------- 1 | # map.inc - useful setup for a floorplan bitmap 2 | # Authors: Richard Vaughan 3 | # $Id$ 4 | 5 | define floorplan model 6 | ( 7 | # sombre, sensible, artistic 8 | color "gray30" 9 | 10 | # most maps will need a bounding box 11 | boundary 1 12 | 13 | gui_nose 0 14 | gui_grid 0 15 | gui_move 0 16 | gui_outline 0 17 | gripper_return 0 18 | fiducial_return 0 19 | ranger_return 0.5 20 | 21 | obstacle_return 0 22 | ) 23 | 24 | define zone model 25 | ( 26 | color "orange" 27 | size [ 4 4 0.01 ] 28 | 29 | gui_nose 0 30 | gui_grid 0 31 | gui_move 1 32 | gui_outline 0 33 | 34 | # insensible to collision and range sensors 35 | obstacle_return 0 36 | ranger_return -1 # transparent to range sensors 37 | ) 38 | -------------------------------------------------------------------------------- /upo_launchers/centre_sportif/maps/centre_sportif.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/centre_sportif/maps/centre_sportif.pgm -------------------------------------------------------------------------------- /upo_launchers/centre_sportif/maps/centre_sportif.yaml: -------------------------------------------------------------------------------- 1 | image: centre_sportif.pgm 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/centre_sportif/maps/waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 5.8 3 | y: 18.8 4 | theta: -0.785 5 | lunch: 6 | x: 16.75 7 | y: 12.25 8 | theta: 3.13 9 | corridor: 10 | x: 11.7 11 | y: 23.85 12 | theta: -2.09 13 | 14 | -------------------------------------------------------------------------------- /upo_launchers/centre_sportif/maps/yield_map.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/centre_sportif/maps/yield_map.jpg -------------------------------------------------------------------------------- /upo_launchers/centre_sportif/maps/yield_map.yaml: -------------------------------------------------------------------------------- 1 | image: yield_map.jpg 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/centre_sportif/maps/yield_points.yaml: -------------------------------------------------------------------------------- 1 | point1: 2 | x: 8.4 3 | y: 18.5 4 | theta: 0.785 5 | point2: 6 | x: 10.8 7 | y: 16.65 8 | theta: 1.66 9 | point3: 10 | x: 11.1 11 | y: 21.15 12 | theta: -2.09 13 | point4: 14 | x: 10.3 15 | y: 21 16 | theta: -1.92 17 | center1: 18 | x: 10.1 19 | y: 19.15 20 | theta: 0.0 21 | -------------------------------------------------------------------------------- /upo_launchers/centre_sportif/sim_sportif_nav_behavior.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /upo_launchers/cfg/centre_sportif_waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: -24.179 3 | y: -2.617 4 | theta: -0.592 5 | Base: 6 | x: -24.179 7 | y: -2.617 8 | theta: -0.592 9 | Couloir: 10 | x: -19.15 11 | y: -3.52 12 | theta: -2.062 13 | Salle: 14 | x: -23.54 15 | y: -9.52 16 | theta: -0.508 17 | Salle d'expositions: 18 | x: -30.63 19 | y: -15.57 20 | theta: -2.068 21 | Coin cafe: 22 | x: -16.40 23 | y: -19.31 24 | theta: -2.036 25 | -------------------------------------------------------------------------------- /upo_launchers/cfg/elderly_center_waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 8.45 3 | y: 17.09 4 | theta: -1.57 5 | cafeteria: 6 | x: 13.61 7 | y: 11.5 8 | theta: 0.0 9 | couloir: 10 | x: 20.84 11 | y: 10.98 12 | theta: -3.13 13 | 14 | -------------------------------------------------------------------------------- /upo_launchers/cfg/exp4_sim_waypoints.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 3.0 3 | y: -5.0 4 | theta: 0.0 5 | wp1: 6 | x: -5.5 7 | y: 2.0 8 | theta: 0 9 | wp2: 10 | x: -6.0 11 | y: -6.0 12 | theta: 0.96 13 | -------------------------------------------------------------------------------- /upo_launchers/cfg/upo_lab_waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 12.9903 3 | y: 17.6948 4 | theta: -2.344 5 | wp1: 6 | x: 11.8 7 | y: 15.067 8 | theta: -0.792 9 | wp2: 10 | x: 9.2027 11 | y: 14.3288 12 | theta: -0.792 13 | wp3: 14 | x: 12.2683 15 | y: 12.0161 16 | theta: 0.783 17 | wp4: 18 | x: 10.3542 19 | y: 5.6311 20 | theta: 1.489 21 | wp5: 22 | x: 9.9458 23 | y: 13.5393 24 | theta: 2.354 25 | wp6: 26 | x: 10.8870 27 | y: 15.5389 28 | theta: 0.813 29 | wp7: 30 | x: 13.0767 31 | y: 16.4580 32 | theta: 2.348 33 | -------------------------------------------------------------------------------- /upo_launchers/cfg/uva_demo_waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 39.7 3 | y: 17.45 4 | theta: 1.75 5 | lab_corridor: 6 | x: 36.29 7 | y: 25.625 8 | theta: 1.75 9 | automatic_door: 10 | x: 28.0 11 | y: 21.4 12 | theta: 0.4 13 | hall: 14 | x: 37.4 15 | y: 22.7 16 | theta: -1.4 17 | narrow_corridor: 18 | x: 26.5 19 | y: 14.5 20 | theta: 0.4 21 | student_corridor: 22 | x: 44.25 23 | y: 12.25 24 | theta: 1.75 25 | glass_wall: 26 | x: 43.55 27 | y: 20.20 28 | theta: -3.0 29 | #printer: 30 | # x: 11.45 31 | # y: 16.15 32 | # theta: 0.4 33 | -------------------------------------------------------------------------------- /upo_launchers/cfg/uva_lab_waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 25.95 3 | y: 25.75 4 | theta: 0.79 5 | lab1: 6 | x: 22.25 7 | y: 21.55 8 | theta: 0.79 9 | coffee: 10 | x: 28.25 11 | y: 28.95 12 | theta: 0.79 13 | corridor: 14 | x: 21.7 15 | y: 16.85 16 | theta: -0.60 17 | 18 | -------------------------------------------------------------------------------- /upo_launchers/cfg/uva_sim_waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 25.95 3 | y: 25.75 4 | theta: 0.79 5 | lab1: 6 | x: 22.25 7 | y: 21.55 8 | theta: 0.79 9 | coffee: 10 | x: 28.25 11 | y: 28.95 12 | theta: 0.79 13 | corridor: 14 | x: 21.7 15 | y: 16.85 16 | theta: -0.60 17 | 18 | -------------------------------------------------------------------------------- /upo_launchers/elderly_center/map.inc: -------------------------------------------------------------------------------- 1 | # map.inc - useful setup for a floorplan bitmap 2 | # Authors: Richard Vaughan 3 | # $Id$ 4 | 5 | define floorplan model 6 | ( 7 | # sombre, sensible, artistic 8 | color "gray30" 9 | 10 | # most maps will need a bounding box 11 | boundary 1 12 | 13 | gui_nose 0 14 | gui_grid 0 15 | gui_move 0 16 | gui_outline 0 17 | gripper_return 0 18 | fiducial_return 0 19 | ranger_return 0.5 20 | 21 | obstacle_return 0 22 | ) 23 | 24 | define zone model 25 | ( 26 | color "orange" 27 | size [ 4 4 0.01 ] 28 | 29 | gui_nose 0 30 | gui_grid 0 31 | gui_move 1 32 | gui_outline 0 33 | 34 | # insensible to collision and range sensors 35 | obstacle_return 0 36 | ranger_return -1 # transparent to range sensors 37 | ) 38 | -------------------------------------------------------------------------------- /upo_launchers/elderly_center/maps/LesArcades-chairs.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/elderly_center/maps/LesArcades-chairs.pgm -------------------------------------------------------------------------------- /upo_launchers/elderly_center/maps/LesArcades-chairs.yaml: -------------------------------------------------------------------------------- 1 | image: LesArcades-chairs.pgm 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/elderly_center/maps/elderly_center.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/elderly_center/maps/elderly_center.pgm -------------------------------------------------------------------------------- /upo_launchers/elderly_center/maps/elderly_center.yaml: -------------------------------------------------------------------------------- 1 | image: elderly_center.pgm 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/elderly_center/maps/waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 0.0 3 | y: 0.0 4 | theta: 0.0 5 | restarea: 6 | x: 0.0 7 | y: 0.0 8 | theta: 0.0 9 | corridor: 10 | x: 0.0 11 | y: 0.0 12 | theta: 0.0 13 | lab1: 14 | x: 0.0 15 | y: 0.0 16 | theta: 0.0 17 | lab2: 18 | x: 0.0 19 | y: 0.0 20 | theta: 0.0 21 | office: 22 | x: 0.0 23 | y: 0.0 24 | theta: 0.0 25 | -------------------------------------------------------------------------------- /upo_launchers/elderly_center/maps/yield_map.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/elderly_center/maps/yield_map.jpg -------------------------------------------------------------------------------- /upo_launchers/elderly_center/maps/yield_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/elderly_center/maps/yield_map.pgm -------------------------------------------------------------------------------- /upo_launchers/elderly_center/maps/yield_map.yaml: -------------------------------------------------------------------------------- 1 | image: yield_map.jpg 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /upo_launchers/elderly_center/maps/yield_points.yaml: -------------------------------------------------------------------------------- 1 | point1: 2 | x: 20.775 3 | y: 11.975 4 | theta: -0.76 5 | point2: 6 | x: 20.85 7 | y: 10.25 8 | theta: 0.39 9 | point3: 10 | x: 22.835 11 | y: 11.725 12 | theta: -2.75 13 | point4: 14 | x: 22.65 15 | y: 10.1 16 | theta: 2.58 17 | center1: 18 | x: 21.7 19 | y: 10.975 20 | theta: 0.0 21 | -------------------------------------------------------------------------------- /upo_launchers/elderly_center/sim_elderly_center_nav_behavior.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /upo_launchers/elderly_center/yield_extra/LesArcades-bigdoor.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/elderly_center/yield_extra/LesArcades-bigdoor.pgm -------------------------------------------------------------------------------- /upo_launchers/elderly_center/yield_extra/LesArcades-bigdoor.yaml: -------------------------------------------------------------------------------- 1 | image: LesArcades-bigdoor.pgm 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/elderly_center/yield_extra/LesArcades-yield.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/elderly_center/yield_extra/LesArcades-yield.jpg -------------------------------------------------------------------------------- /upo_launchers/elderly_center/yield_extra/LesArcades-yield.yaml: -------------------------------------------------------------------------------- 1 | image: LesArcades-yield.jpg 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/elderly_center/yield_extra/map.inc: -------------------------------------------------------------------------------- 1 | # map.inc - useful setup for a floorplan bitmap 2 | # Authors: Richard Vaughan 3 | # $Id$ 4 | 5 | define floorplan model 6 | ( 7 | # sombre, sensible, artistic 8 | color "gray30" 9 | 10 | # most maps will need a bounding box 11 | boundary 1 12 | 13 | gui_nose 0 14 | gui_grid 0 15 | gui_move 0 16 | gui_outline 0 17 | gripper_return 0 18 | fiducial_return 0 19 | ranger_return 0.5 20 | 21 | obstacle_return 0 22 | ) 23 | 24 | define zone model 25 | ( 26 | color "orange" 27 | size [ 4 4 0.01 ] 28 | 29 | gui_nose 0 30 | gui_grid 0 31 | gui_move 1 32 | gui_outline 0 33 | 34 | # insensible to collision and range sensors 35 | obstacle_return 0 36 | ranger_return -1 # transparent to range sensors 37 | ) 38 | -------------------------------------------------------------------------------- /upo_launchers/elderly_center/yield_extra/yield_points.yaml: -------------------------------------------------------------------------------- 1 | point1: 2 | x: 20.58 3 | y: 11.52 4 | theta: -0.76 5 | point2: 6 | x: 20.23 7 | y: 10.0 8 | theta: 0.39 9 | point3: 10 | x: 23.11 11 | y: 11.32 12 | theta: -2.75 13 | point4: 14 | x: 22.97 15 | y: 9.91 16 | theta: 2.58 17 | -------------------------------------------------------------------------------- /upo_launchers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | upo_launchers 4 | 0.0.0 5 | The upo_launchers package 6 | 7 | 8 | 9 | 10 | upo_launchers 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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /upo_launchers/params3/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | # 2D Costmap parameters 2 | # http://www.ros.org/wiki/costmap_2d 3 | 4 | ### Robot description parameters ### 5 | 6 | # Robot foot print in m (TERESA) 7 | #footprint: [ [0.32, 0], [0.2263, -0.2263], [0, -0.32], [-0.2263, -0.2263], [-0.32, 0], [-0.2263, 0.2263], [0, 0.32], [0.2263, 0.2263]] 8 | footprint: [ [0.345, 0], [0.244, -0.244], [0, -0.345], [-0.244, -0.244], [-0.345, 0], [-0.244, 0.244], [0, 0.345], [0.244, 0.244]] 9 | 10 | # Inscribed and circunscribed radius in m 11 | inscribed_radius: 0.345 12 | circumscribed_radius: 0.345 13 | 14 | # Obstacle inflation in m (circumscribed robot radius) 15 | #inflation_radius: 0.345 16 | 17 | transform_tolerance: 0.5 18 | map_type: costmap 19 | 20 | -------------------------------------------------------------------------------- /upo_launchers/params3/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 2.0 #default=5.0 5 | publish_frequency: 2.0 6 | static_map: true 7 | rolling_window: false 8 | track_unknown_space: true 9 | always_send_full_costmap: false 10 | transform_tolerance: 0.2 11 | resolution: 0.05 #default=0.05 meters/cells 12 | 13 | 14 | plugins: 15 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 16 | #- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 17 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 18 | #- {name: social_layer, type: "social_layer::SocialLayer"} 19 | 20 | 21 | static_layer: 22 | map_topic: /map 23 | unknown_cost_value: 255 #default=-1 24 | trinary_costmap: true 25 | lethal_cost_threshold: 100 #default=100 26 | track_unknown_space: true 27 | use_maximum: false 28 | first_map_only: true #default=false 29 | subscribe_to_updates: false #if true, it subscribes to topic /map_topic_updates 30 | 31 | 32 | obstacle_layer: 33 | obstacle_range: 5.0 34 | max_obstacle_range: 6.0 35 | raytrace_range: 6.3 36 | track_unknown_space: true 37 | transform_tolerance: 0.2 #default=0.2 38 | observation_sources: laserfront_scan_sensor laserback_scan_sensor 39 | #laserfront_scan_sensor: {sensor_frame: base_laser_link_0, data_type: LaserScan, topic: base_scan_0, marking: true, clearing: true, expected_update_rate: 1.0, obstacle_range: 5.0, raytrace_range: 6.3 } 40 | #laserback_scan_sensor: {sensor_frame: base_laser_link_1, data_type: LaserScan, topic: base_scan_1, marking: true, clearing: true, expected_update_rate: 1.0, obstacle_range: 5.0, raytrace_range: 6.3} 41 | 42 | #observation_sources: laser 43 | #laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8, obstacle_range: 5.0, raytrace_range: 6.3} 44 | 45 | 46 | inflation_layer: 47 | inflation_radius: 2.0 48 | cost_scaling_factor: 3.0 49 | 50 | 51 | #social_layer: 52 | # size_x: 5.0 53 | # size_y: 5.0 54 | # all_features: false 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /upo_launchers/params3/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 5.0 5 | publish_frequency: 5.0 #5.0 6 | static_map: false 7 | rolling_window: true 8 | width: 10.0 9 | height: 10.0 10 | resolution: 0.05 #0.025 11 | 12 | 13 | plugins: 14 | #- {name: static_layer, type: "costmap_2d::StaticLayer"} 15 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 16 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 17 | #- {name: social_layer, type: "social_layer::SocialLayer"} 18 | 19 | 20 | #static_layer: 21 | # map_topic: /map 22 | # unknown_cost_value: -1 #default=-1 23 | # trinary_costmap: true 24 | # lethal_cost_threshold: 100 #default=100 25 | # track_unknown_space: false 26 | # use_maximum: false 27 | # first_map_only: false #default=false 28 | # subscribe_to_updates: true #if true, it subscribes to topic /map_topic_updates 29 | 30 | 31 | obstacle_layer: 32 | obstacle_range: 4.9 33 | max_obstacle_range: 4.9 34 | raytrace_range: 5.8 35 | track_unknown_space: false 36 | transform_tolerance: 0.2 #default=0.2 37 | observation_sources: laser #laserfront_scan_sensor #laserback_scan_sensor 38 | #laserfront_scan_sensor: {sensor_frame: base_laser_link_0, data_type: LaserScan, topic: base_scan_0, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 1.0, obstacle_range: 4.95, raytrace_range: 5.8} 39 | #laserback_scan_sensor: {sensor_frame: base_laser_link_1, data_type: LaserScan, topic: base_scan_1, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 1.0, obstacle_range: 4.95, raytrace_range: 5.8} 40 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 41 | 42 | 43 | inflation_layer: 44 | inflation_radius: 0.70 #0.34*2 = 0.68 #0.34*4=1.36 45 | cost_scaling_factor: 10.0 #10.0=default 46 | 47 | 48 | #social_layer: 49 | # size_x: 5.0 50 | # size_y: 5.0 51 | # all_features: true 52 | 53 | -------------------------------------------------------------------------------- /upo_launchers/params_nav/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | # 2D Costmap parameters 2 | # http://www.ros.org/wiki/costmap_2d 3 | 4 | ### Robot description parameters ### 5 | 6 | # Robot foot print in m (TERESA) 7 | #footprint: [ [0.32, 0], [0.2263, -0.2263], [0, -0.32], [-0.2263, -0.2263], [-0.32, 0], [-0.2263, 0.2263], [0, 0.32], [0.2263, 0.2263]] 8 | footprint: [ [0.345, 0], [0.244, -0.244], [0, -0.345], [-0.244, -0.244], [-0.345, 0], [-0.244, 0.244], [0, 0.345], [0.244, 0.244]] 9 | #footprint: [ [0.37, 0], [0.262, -0.262], [0, -0.37], [-0.262, -0.262], [-0.37, 0], [-0.262, 0.262], [0, 0.37], [0.262, 0.262]] 10 | #footprint: [ [0.40, 0], [0.2828, -0.2828], [0, -0.40], [-0.2828, -0.2828], [-0.40, 0], [-0.2828, 0.2828], [0, 0.40], [0.2828, 0.2828]] 11 | 12 | # Inscribed and circunscribed radius in m 13 | inscribed_radius: 0.345 #0.40 #0.37 #0.345 14 | circumscribed_radius: 0.345 #0.40 #0.37 #0.345 15 | 16 | # Obstacle inflation in m (circumscribed robot radius) 17 | #inflation_radius: 0.345 18 | 19 | transform_tolerance: 0.5 20 | map_type: costmap 21 | 22 | 23 | -------------------------------------------------------------------------------- /upo_launchers/params_nav/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 4.0 #default=5.0 5 | publish_frequency: 2.0 6 | static_map: true 7 | rolling_window: false 8 | track_unknown_space: true 9 | always_send_full_costmap: false 10 | transform_tolerance: 0.2 11 | resolution: 0.05 #default=0.05 meters/cells 12 | 13 | 14 | plugins: 15 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 16 | #- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 17 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 18 | #- {name: social_layer, type: "social_layer::SocialLayer"} 19 | 20 | 21 | static_layer: 22 | map_topic: /map 23 | unknown_cost_value: 255 #default=-1 24 | trinary_costmap: true 25 | lethal_cost_threshold: 100 #default=100 26 | track_unknown_space: true 27 | use_maximum: false 28 | first_map_only: true #default=false 29 | subscribe_to_updates: false #if true, it subscribes to topic /map_topic_updates 30 | 31 | 32 | inflation_layer: 33 | inflation_radius: 2.0 34 | cost_scaling_factor: 3.0 35 | 36 | 37 | #social_layer: 38 | # size_x: 5.0 39 | # size_y: 5.0 40 | # all_features: true 41 | 42 | 43 | -------------------------------------------------------------------------------- /upo_launchers/params_nav/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 1.0 #10.0 #8.0 5 | publish_frequency: 2.0 #5.0 6 | static_map: false 7 | rolling_window: true 8 | width: 4.0 #9.0 #10.0 9 | height: 4.0 #9.0 #10.0 10 | resolution: 0.05 #0.025 11 | 12 | 13 | plugins: 14 | #- {name: static_layer, type: "costmap_2d::StaticLayer"} 15 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 16 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 17 | #- {name: social_layer, type: "social_layer::SocialLayer"} 18 | 19 | 20 | 21 | obstacle_layer: 22 | obstacle_range: 2.0 #4.5 23 | max_obstacle_range: 2.0 #4.5 24 | raytrace_range: 2.2 #4.8 25 | track_unknown_space: false 26 | transform_tolerance: 0.2 #default=0.2 27 | ### Sensor management parameters ### 28 | observation_sources: laser #xtion 29 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6} 30 | #xtion: {sensor_frame: base_link, data_type: LaserScan, topic: depthcamscan_node/scanXtion, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8} 31 | 32 | 33 | inflation_layer: 34 | inflation_radius: 0.74 #0.345*2 = 0.68 #0.37*2=0.74 35 | cost_scaling_factor: 10.0 #10.0=default 36 | 37 | -------------------------------------------------------------------------------- /upo_launchers/params_sim/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | # 2D Costmap parameters 2 | # http://www.ros.org/wiki/costmap_2d 3 | 4 | ### Robot description parameters ### 5 | 6 | # Robot foot print in m (TERESA) 7 | #footprint: [ [0.32, 0], [0.2263, -0.2263], [0, -0.32], [-0.2263, -0.2263], [-0.32, 0], [-0.2263, 0.2263], [0, 0.32], [0.2263, 0.2263]] 8 | footprint: [ [0.345, 0], [0.244, -0.244], [0, -0.345], [-0.244, -0.244], [-0.345, 0], [-0.244, 0.244], [0, 0.345], [0.244, 0.244]] 9 | 10 | # Inscribed and circunscribed radius in m 11 | inscribed_radius: 0.345 12 | circumscribed_radius: 0.345 13 | 14 | # Obstacle inflation in m (circumscribed robot radius) 15 | #inflation_radius: 0.345 16 | 17 | transform_tolerance: 0.5 18 | map_type: costmap 19 | 20 | -------------------------------------------------------------------------------- /upo_launchers/params_sim/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 2.0 #default=5.0 5 | publish_frequency: 2.0 6 | static_map: true 7 | rolling_window: false 8 | track_unknown_space: true 9 | always_send_full_costmap: false 10 | transform_tolerance: 0.2 11 | resolution: 0.05 #default=0.05 meters/cells 12 | 13 | 14 | plugins: 15 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 16 | #- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 17 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 18 | #- {name: social_layer, type: "social_layer::SocialLayer"} 19 | 20 | 21 | static_layer: 22 | map_topic: /map 23 | unknown_cost_value: 255 #default=-1 24 | trinary_costmap: true 25 | lethal_cost_threshold: 100 #default=100 26 | track_unknown_space: true 27 | use_maximum: false 28 | first_map_only: true #default=false 29 | subscribe_to_updates: false #if true, it subscribes to topic /map_topic_updates 30 | 31 | 32 | obstacle_layer: 33 | obstacle_range: 5.0 34 | max_obstacle_range: 6.0 35 | raytrace_range: 6.3 36 | track_unknown_space: true 37 | transform_tolerance: 0.2 #default=0.2 38 | observation_sources: laserfront_scan_sensor laserback_scan_sensor 39 | #laserfront_scan_sensor: {sensor_frame: base_laser_link_0, data_type: LaserScan, topic: base_scan_0, marking: true, clearing: true, expected_update_rate: 1.0, obstacle_range: 5.0, raytrace_range: 6.3 } 40 | #laserback_scan_sensor: {sensor_frame: base_laser_link_1, data_type: LaserScan, topic: base_scan_1, marking: true, clearing: true, expected_update_rate: 1.0, obstacle_range: 5.0, raytrace_range: 6.3} 41 | 42 | #observation_sources: laser 43 | #laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8, obstacle_range: 5.0, raytrace_range: 6.3} 44 | 45 | 46 | inflation_layer: 47 | inflation_radius: 2.0 48 | cost_scaling_factor: 3.0 49 | 50 | 51 | #social_layer: 52 | # size_x: 5.0 53 | # size_y: 5.0 54 | # all_features: false 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /upo_launchers/params_sim/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 5.0 5 | publish_frequency: 5.0 #5.0 6 | static_map: false 7 | rolling_window: true 8 | width: 10.0 9 | height: 10.0 10 | resolution: 0.05 #0.025 11 | 12 | 13 | plugins: 14 | #- {name: static_layer, type: "costmap_2d::StaticLayer"} 15 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 16 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 17 | #- {name: social_layer, type: "social_layer::SocialLayer"} 18 | 19 | 20 | #static_layer: 21 | # map_topic: /map 22 | # unknown_cost_value: -1 #default=-1 23 | # trinary_costmap: true 24 | # lethal_cost_threshold: 100 #default=100 25 | # track_unknown_space: false 26 | # use_maximum: false 27 | # first_map_only: false #default=false 28 | # subscribe_to_updates: true #if true, it subscribes to topic /map_topic_updates 29 | 30 | 31 | obstacle_layer: 32 | obstacle_range: 4.9 33 | max_obstacle_range: 4.9 34 | raytrace_range: 5.8 35 | track_unknown_space: false 36 | transform_tolerance: 0.2 #default=0.2 37 | observation_sources: laser #laserfront_scan_sensor #laserback_scan_sensor 38 | #laserfront_scan_sensor: {sensor_frame: base_laser_link_0, data_type: LaserScan, topic: base_scan_0, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 1.0, obstacle_range: 4.95, raytrace_range: 5.8} 39 | #laserback_scan_sensor: {sensor_frame: base_laser_link_1, data_type: LaserScan, topic: base_scan_1, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 1.0, obstacle_range: 4.95, raytrace_range: 5.8} 40 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 41 | 42 | 43 | inflation_layer: 44 | inflation_radius: 0.70 #0.34*2 = 0.68 #0.34*4=1.36 45 | cost_scaling_factor: 10.0 #10.0=default 46 | 47 | 48 | #social_layer: 49 | # size_x: 5.0 50 | # size_y: 5.0 51 | # all_features: true 52 | 53 | -------------------------------------------------------------------------------- /upo_launchers/params_sim2/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | # 2D Costmap parameters 2 | # http://www.ros.org/wiki/costmap_2d 3 | 4 | ### Robot description parameters ### 5 | 6 | # Robot foot print in m (TERESA) 7 | #footprint: [ [0.32, 0], [0.2263, -0.2263], [0, -0.32], [-0.2263, -0.2263], [-0.32, 0], [-0.2263, 0.2263], [0, 0.32], [0.2263, 0.2263]] 8 | footprint: [ [0.345, 0], [0.244, -0.244], [0, -0.345], [-0.244, -0.244], [-0.345, 0], [-0.244, 0.244], [0, 0.345], [0.244, 0.244]] 9 | 10 | # Inscribed and circunscribed radius in m 11 | inscribed_radius: 0.345 12 | circumscribed_radius: 0.345 13 | 14 | # Obstacle inflation in m (circumscribed robot radius) 15 | #inflation_radius: 0.345 16 | 17 | transform_tolerance: 0.5 18 | map_type: costmap 19 | 20 | -------------------------------------------------------------------------------- /upo_launchers/params_sim2/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 2.0 #default=5.0 5 | publish_frequency: 2.0 6 | static_map: true 7 | rolling_window: false 8 | track_unknown_space: true 9 | always_send_full_costmap: false 10 | transform_tolerance: 0.2 11 | resolution: 0.05 #default=0.05 meters/cells 12 | 13 | 14 | plugins: 15 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 16 | #- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 17 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 18 | #- {name: social_layer, type: "social_layer::SocialLayer"} 19 | 20 | 21 | static_layer: 22 | map_topic: /map 23 | unknown_cost_value: 255 #default=-1 24 | trinary_costmap: true 25 | lethal_cost_threshold: 100 #default=100 26 | track_unknown_space: true 27 | use_maximum: false 28 | first_map_only: true #default=false 29 | subscribe_to_updates: false #if true, it subscribes to topic /map_topic_updates 30 | 31 | 32 | obstacle_layer: 33 | obstacle_range: 5.0 34 | max_obstacle_range: 6.0 35 | raytrace_range: 6.3 36 | track_unknown_space: true 37 | transform_tolerance: 0.2 #default=0.2 38 | observation_sources: laserfront_scan_sensor laserback_scan_sensor 39 | #laserfront_scan_sensor: {sensor_frame: base_laser_link_0, data_type: LaserScan, topic: base_scan_0, marking: true, clearing: true, expected_update_rate: 1.0, obstacle_range: 5.0, raytrace_range: 6.3 } 40 | #laserback_scan_sensor: {sensor_frame: base_laser_link_1, data_type: LaserScan, topic: base_scan_1, marking: true, clearing: true, expected_update_rate: 1.0, obstacle_range: 5.0, raytrace_range: 6.3} 41 | 42 | #observation_sources: laser 43 | #laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8, obstacle_range: 5.0, raytrace_range: 6.3} 44 | 45 | 46 | inflation_layer: 47 | inflation_radius: 2.0 48 | cost_scaling_factor: 3.0 49 | 50 | 51 | #social_layer: 52 | # size_x: 5.0 53 | # size_y: 5.0 54 | # all_features: false 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /upo_launchers/params_sim2/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 1.0 5 | publish_frequency: 1.0 #5.0 6 | static_map: false 7 | rolling_window: true 8 | width: 3.0 9 | height: 3.0 10 | resolution: 0.05 #0.025 11 | 12 | 13 | plugins: 14 | #- {name: static_layer, type: "costmap_2d::StaticLayer"} 15 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 16 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 17 | #- {name: social_layer, type: "social_layer::SocialLayer"} 18 | 19 | 20 | #static_layer: 21 | # map_topic: /map 22 | # unknown_cost_value: -1 #default=-1 23 | # trinary_costmap: true 24 | # lethal_cost_threshold: 100 #default=100 25 | # track_unknown_space: false 26 | # use_maximum: false 27 | # first_map_only: false #default=false 28 | # subscribe_to_updates: true #if true, it subscribes to topic /map_topic_updates 29 | 30 | 31 | obstacle_layer: 32 | obstacle_range: 1.2 #4.9 33 | max_obstacle_range: 1.2 #4.9 34 | raytrace_range: 1.3 #5.8 35 | track_unknown_space: false 36 | transform_tolerance: 0.2 #default=0.2 37 | observation_sources: laser #laserfront_scan_sensor #laserback_scan_sensor 38 | #laserfront_scan_sensor: {sensor_frame: base_laser_link_0, data_type: LaserScan, topic: base_scan_0, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 1.0, obstacle_range: 4.95, raytrace_range: 5.8} 39 | #laserback_scan_sensor: {sensor_frame: base_laser_link_1, data_type: LaserScan, topic: base_scan_1, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 1.0, obstacle_range: 4.95, raytrace_range: 5.8} 40 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6} 41 | 42 | 43 | inflation_layer: 44 | inflation_radius: 0.70 #0.34*2 = 0.68 #0.34*4=1.36 45 | cost_scaling_factor: 10.0 #10.0=default 46 | 47 | 48 | #social_layer: 49 | # size_x: 5.0 50 | # size_y: 5.0 51 | # all_features: true 52 | 53 | -------------------------------------------------------------------------------- /upo_launchers/record_nav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/upo_45/map.inc: -------------------------------------------------------------------------------- 1 | # map.inc - useful setup for a floorplan bitmap 2 | # Authors: Richard Vaughan 3 | # $Id$ 4 | 5 | define floorplan model 6 | ( 7 | # sombre, sensible, artistic 8 | color "gray30" 9 | 10 | # most maps will need a bounding box 11 | boundary 1 12 | 13 | gui_nose 0 14 | gui_grid 0 15 | gui_move 0 16 | gui_outline 0 17 | gripper_return 0 18 | fiducial_return 0 19 | ranger_return 0.5 20 | 21 | obstacle_return 0 22 | ) 23 | 24 | define zone model 25 | ( 26 | color "orange" 27 | size [ 4 4 0.01 ] 28 | 29 | gui_nose 0 30 | gui_grid 0 31 | gui_move 1 32 | gui_outline 0 33 | 34 | # insensible to collision and range sensors 35 | obstacle_return 0 36 | ranger_return -1 # transparent to range sensors 37 | ) 38 | -------------------------------------------------------------------------------- /upo_launchers/upo_45/maps/upo_45.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/upo_45/maps/upo_45.pgm -------------------------------------------------------------------------------- /upo_launchers/upo_45/maps/upo_45.yaml: -------------------------------------------------------------------------------- 1 | image: upo_45.pgm 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/upo_45/maps/upo_45_1.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/upo_45/maps/upo_45_1.pgm -------------------------------------------------------------------------------- /upo_launchers/upo_45/maps/waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 8.1 3 | y: 15.25 4 | theta: 0.0 5 | restarea: 6 | x: 10.55 7 | y: 7.3 8 | theta: -1.57 9 | corridor: 10 | x: 14.9 11 | y: 14.8 12 | theta: 0.96 13 | lab1: 14 | x: 10.25 15 | y: 15.2 16 | theta: 0.96 17 | lab2: 18 | x: 13.5 19 | y: 17.8 20 | theta: 0.96 21 | office: 22 | x: 15.8 23 | y: 20.7 24 | theta: 0.96 25 | -------------------------------------------------------------------------------- /upo_launchers/upo_45/maps/yield_map.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/upo_45/maps/yield_map.jpg -------------------------------------------------------------------------------- /upo_launchers/upo_45/maps/yield_map.yaml: -------------------------------------------------------------------------------- 1 | image: yield_map.jpg 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/upo_45/maps/yield_points.yaml: -------------------------------------------------------------------------------- 1 | point1: 2 | x: 11.23 3 | y: 13.07 4 | theta: -1.57 5 | point2: 6 | x: 10.25 7 | y: 12.89 8 | theta: -0.79 9 | point3: 10 | x: 11.88 11 | y: 12.24 12 | theta: -2.85 13 | point4: 14 | x: 11.82 15 | y: 15.84 16 | theta: 0.95 17 | point5: 18 | x: 10.90 19 | y: 16.52 20 | theta: 0.17 21 | point6: 22 | x: 12.09 23 | y: 17.53 24 | theta: -2.36 25 | point7: 26 | x: 12.81 27 | y: 16.58 28 | theta: 2.88 29 | point8: 30 | x: 16.83 31 | y: 17.37 32 | theta: 0.79 33 | point9: 34 | x: 17.63 35 | y: 18.15 36 | theta: 3.13 37 | point10: 38 | x: 17.02 39 | y: 18.81 40 | theta: -1.50 41 | point11: 42 | x: 15.92 43 | y: 18.74 44 | theta: -0.79 45 | -------------------------------------------------------------------------------- /upo_launchers/upo_45/move_base/params_nav/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | # 2D Costmap parameters 2 | # http://www.ros.org/wiki/costmap_2d 3 | 4 | ### Robot description parameters ### 5 | 6 | # Robot foot print in m (TERESA) 7 | footprint: [ [0.32, 0], [0.2263, -0.2263], [0, -0.32], [-0.2263, -0.2263], [-0.32, 0], [-0.2263, 0.2263], [0, 0.32], [0.2263, 0.2263]] 8 | 9 | # Inscribed and circunscribed radius in m 10 | inscribed_radius: 0.32 11 | circumscribed_radius: 0.32 12 | 13 | # Obstacle inflation in m (circumscribed robot radius) 14 | #inflation_radius: 0.32 15 | 16 | transform_tolerance: 0.5 17 | map_type: costmap 18 | 19 | -------------------------------------------------------------------------------- /upo_launchers/upo_45/move_base/params_nav/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 2.0 #default=5.0 5 | publish_frequency: 2.0 6 | static_map: true 7 | rolling_window: false 8 | track_unknown_space: true 9 | always_send_full_costmap: false 10 | transform_tolerance: 0.2 11 | resolution: 0.05 #default=0.05 meters/cells 12 | 13 | 14 | plugins: 15 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 16 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 17 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 18 | #- {name: social_layer, type: "social_layer::SocialLayer"} 19 | 20 | 21 | static_layer: 22 | map_topic: /map 23 | unknown_cost_value: -1 #default=-1 24 | trinary_costmap: true 25 | lethal_cost_threshold: 100 #default=100 26 | track_unknown_space: false 27 | use_maximum: false 28 | first_map_only: true #default=false 29 | subscribe_to_updates: false #if true, it subscribes to topic /map_topic_updates 30 | 31 | 32 | obstacle_layer: 33 | obstacle_range: 4.5 34 | max_obstacle_range: 4.5 35 | raytrace_range: 4.8 36 | track_unknown_space: false 37 | transform_tolerance: 0.2 #default=0.2 38 | ### Sensor management parameters ### 39 | observation_sources: laser xtion 40 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 41 | xtion: {sensor_frame: base_link, data_type: LaserScan, topic: depthcamscan_node/scanXtion, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8} 42 | 43 | 44 | inflation_layer: 45 | inflation_radius: 0.64 #circumscribed robot radius 46 | cost_scaling_factor: 10.0 #3.0 47 | 48 | 49 | #social_layer: 50 | # size_x: 5.0 51 | # size_y: 5.0 52 | # all_features: false 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /upo_launchers/upo_45/move_base/params_nav/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 4.0 5 | publish_frequency: 4.0 6 | static_map: false 7 | rolling_window: true 8 | width: 10.0 9 | height: 10.0 10 | resolution: 0.05 #0.025 11 | 12 | 13 | plugins: 14 | #- {name: static_layer, type: "costmap_2d::StaticLayer"} 15 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 16 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 17 | #- {name: social_layer, type: "social_layer::SocialLayer"} 18 | 19 | 20 | #static_layer: 21 | # map_topic: /map 22 | # unknown_cost_value: -1 #default=-1 23 | # trinary_costmap: true 24 | # lethal_cost_threshold: 100 #default=100 25 | # track_unknown_space: false 26 | # use_maximum: false 27 | # first_map_only: true #default=false 28 | # subscribe_to_updates: true #if true, it subscribes to topic /_updates 29 | 30 | 31 | obstacle_layer: 32 | obstacle_range: 4.5 33 | max_obstacle_range: 4.5 34 | raytrace_range: 4.8 35 | track_unknown_space: false 36 | transform_tolerance: 0.2 #default=0.2 37 | ### Sensor management parameters ### 38 | observation_sources: laser xtion 39 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 40 | xtion: {sensor_frame: base_link, data_type: LaserScan, topic: depthcamscan_node/scanXtion, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8} 41 | 42 | 43 | inflation_layer: 44 | inflation_radius: 0.64 #0.34*2 = 0.68 45 | cost_scaling_factor: 10.0 #10.0=default 46 | 47 | 48 | #social_layer: 49 | # size_x: 5.0 50 | # size_y: 5.0 51 | # all_features: true 52 | 53 | -------------------------------------------------------------------------------- /upo_launchers/upo_45/sim_upo_45_nav_behavior.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /upo_launchers/upo_lab/map.inc: -------------------------------------------------------------------------------- 1 | # map.inc - useful setup for a floorplan bitmap 2 | # Authors: Richard Vaughan 3 | # $Id$ 4 | 5 | define floorplan model 6 | ( 7 | # sombre, sensible, artistic 8 | color "gray30" 9 | 10 | # most maps will need a bounding box 11 | boundary 1 12 | 13 | gui_nose 0 14 | gui_grid 0 15 | gui_move 0 16 | gui_outline 0 17 | gripper_return 0 18 | fiducial_return 0 19 | ranger_return 0.5 20 | 21 | obstacle_return 0 22 | ) 23 | 24 | define zone model 25 | ( 26 | color "orange" 27 | size [ 4 4 0.01 ] 28 | 29 | gui_nose 0 30 | gui_grid 0 31 | gui_move 1 32 | gui_outline 0 33 | 34 | # insensible to collision and range sensors 35 | obstacle_return 0 36 | ranger_return -1 # transparent to range sensors 37 | ) 38 | -------------------------------------------------------------------------------- /upo_launchers/upo_lab/maps/upo45.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/upo_lab/maps/upo45.pgm -------------------------------------------------------------------------------- /upo_launchers/upo_lab/maps/upo45.yaml: -------------------------------------------------------------------------------- 1 | image: upo45.pgm 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/upo_lab/maps/upo_lab.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/upo_lab/maps/upo_lab.pgm -------------------------------------------------------------------------------- /upo_launchers/upo_lab/maps/upo_lab.yaml: -------------------------------------------------------------------------------- 1 | image: upo_lab.pgm 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/upo_lab/maps/upo_lab_split.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/upo_lab/maps/upo_lab_split.pgm -------------------------------------------------------------------------------- /upo_launchers/upo_lab/maps/waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | x: 8.1 3 | y: 15.25 4 | theta: 0.0 5 | restarea: 6 | x: 10.55 7 | y: 7.3 8 | theta: -1.57 9 | corridor: 10 | x: 14.9 11 | y: 14.8 12 | theta: 0.96 13 | lab1: 14 | x: 10.25 15 | y: 15.2 16 | theta: 0.96 17 | lab2: 18 | x: 13.5 19 | y: 17.8 20 | theta: 0.96 21 | office: 22 | x: 15.8 23 | y: 20.7 24 | theta: 0.96 25 | -------------------------------------------------------------------------------- /upo_launchers/upo_lab/maps/yield_map.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/upo_lab/maps/yield_map.jpg -------------------------------------------------------------------------------- /upo_launchers/upo_lab/maps/yield_map.yaml: -------------------------------------------------------------------------------- 1 | image: yield_map_empty.jpg 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/upo_lab/maps/yield_map_empty.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/upo_lab/maps/yield_map_empty.jpg -------------------------------------------------------------------------------- /upo_launchers/upo_lab/maps/yield_points.yaml: -------------------------------------------------------------------------------- 1 | point1: 2 | x: 11.23 3 | y: 13.07 4 | theta: -1.57 5 | point2: 6 | x: 10.25 7 | y: 12.89 8 | theta: -0.79 9 | point3: 10 | x: 11.88 11 | y: 12.24 12 | theta: -2.85 13 | point4: 14 | x: 11.82 15 | y: 15.84 16 | theta: 0.95 17 | point5: 18 | x: 10.90 19 | y: 16.52 20 | theta: 0.17 21 | point6: 22 | x: 12.09 23 | y: 17.53 24 | theta: -2.36 25 | point7: 26 | x: 12.81 27 | y: 16.58 28 | theta: 2.88 29 | point8: 30 | x: 16.83 31 | y: 17.37 32 | theta: 0.79 33 | point9: 34 | x: 17.63 35 | y: 18.15 36 | theta: 3.13 37 | point10: 38 | x: 17.02 39 | y: 18.81 40 | theta: -1.50 41 | point11: 42 | x: 15.92 43 | y: 18.74 44 | theta: -0.79 45 | -------------------------------------------------------------------------------- /upo_launchers/upo_lab/move_base/params_nav/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | # 2D Costmap parameters 2 | # http://www.ros.org/wiki/costmap_2d 3 | 4 | ### Robot description parameters ### 5 | 6 | # Robot foot print in m (TERESA) 7 | footprint: [ [0.32, 0], [0.2263, -0.2263], [0, -0.32], [-0.2263, -0.2263], [-0.32, 0], [-0.2263, 0.2263], [0, 0.32], [0.2263, 0.2263]] 8 | 9 | # Inscribed and circunscribed radius in m 10 | inscribed_radius: 0.32 11 | circumscribed_radius: 0.32 12 | 13 | # Obstacle inflation in m (circumscribed robot radius) 14 | #inflation_radius: 0.32 15 | 16 | transform_tolerance: 0.5 17 | map_type: costmap 18 | 19 | -------------------------------------------------------------------------------- /upo_launchers/upo_lab/move_base/params_nav/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 2.0 #default=5.0 5 | publish_frequency: 2.0 6 | static_map: true 7 | rolling_window: false 8 | track_unknown_space: true 9 | always_send_full_costmap: false 10 | transform_tolerance: 0.2 11 | resolution: 0.05 #default=0.05 meters/cells 12 | 13 | 14 | plugins: 15 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 16 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 17 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 18 | #- {name: social_layer, type: "social_layer::SocialLayer"} 19 | 20 | 21 | static_layer: 22 | map_topic: /map 23 | unknown_cost_value: -1 #default=-1 24 | trinary_costmap: true 25 | lethal_cost_threshold: 100 #default=100 26 | track_unknown_space: false 27 | use_maximum: false 28 | first_map_only: true #default=false 29 | subscribe_to_updates: false #if true, it subscribes to topic /map_topic_updates 30 | 31 | 32 | obstacle_layer: 33 | obstacle_range: 4.5 34 | max_obstacle_range: 4.5 35 | raytrace_range: 4.8 36 | track_unknown_space: false 37 | transform_tolerance: 0.2 #default=0.2 38 | ### Sensor management parameters ### 39 | observation_sources: laser xtion 40 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 41 | xtion: {sensor_frame: base_link, data_type: LaserScan, topic: depthcamscan_node/scanXtion, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8} 42 | 43 | 44 | inflation_layer: 45 | inflation_radius: 0.64 #circumscribed robot radius 46 | cost_scaling_factor: 10.0 #3.0 47 | 48 | 49 | #social_layer: 50 | # size_x: 5.0 51 | # size_y: 5.0 52 | # all_features: false 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /upo_launchers/upo_lab/move_base/params_nav/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 4.0 5 | publish_frequency: 4.0 6 | static_map: false 7 | rolling_window: true 8 | width: 10.0 9 | height: 10.0 10 | resolution: 0.05 #0.025 11 | 12 | 13 | plugins: 14 | #- {name: static_layer, type: "costmap_2d::StaticLayer"} 15 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 16 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 17 | #- {name: social_layer, type: "social_layer::SocialLayer"} 18 | 19 | 20 | #static_layer: 21 | # map_topic: /map 22 | # unknown_cost_value: -1 #default=-1 23 | # trinary_costmap: true 24 | # lethal_cost_threshold: 100 #default=100 25 | # track_unknown_space: false 26 | # use_maximum: false 27 | # first_map_only: true #default=false 28 | # subscribe_to_updates: true #if true, it subscribes to topic /_updates 29 | 30 | 31 | obstacle_layer: 32 | obstacle_range: 4.5 33 | max_obstacle_range: 4.5 34 | raytrace_range: 4.8 35 | track_unknown_space: false 36 | transform_tolerance: 0.2 #default=0.2 37 | ### Sensor management parameters ### 38 | observation_sources: laser xtion 39 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 40 | xtion: {sensor_frame: base_link, data_type: LaserScan, topic: depthcamscan_node/scanXtion, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8} 41 | 42 | 43 | inflation_layer: 44 | inflation_radius: 0.64 #0.34*2 = 0.68 45 | cost_scaling_factor: 10.0 #10.0=default 46 | 47 | 48 | #social_layer: 49 | # size_x: 5.0 50 | # size_y: 5.0 51 | # all_features: true 52 | 53 | -------------------------------------------------------------------------------- /upo_launchers/upo_lab/sim_upo_lab_nav_behavior.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /upo_launchers/uva_demo/map.inc: -------------------------------------------------------------------------------- 1 | # map.inc - useful setup for a floorplan bitmap 2 | # Authors: Richard Vaughan 3 | # $Id$ 4 | 5 | define floorplan model 6 | ( 7 | # sombre, sensible, artistic 8 | color "gray30" 9 | 10 | # most maps will need a bounding box 11 | boundary 1 12 | 13 | gui_nose 0 14 | gui_grid 0 15 | gui_move 0 16 | gui_outline 0 17 | gripper_return 0 18 | fiducial_return 0 19 | ranger_return 0.5 20 | 21 | obstacle_return 0 22 | ) 23 | 24 | define zone model 25 | ( 26 | color "orange" 27 | size [ 4 4 0.01 ] 28 | 29 | gui_nose 0 30 | gui_grid 0 31 | gui_move 1 32 | gui_outline 0 33 | 34 | # insensible to collision and range sensors 35 | obstacle_return 0 36 | ranger_return -1 # transparent to range sensors 37 | ) 38 | -------------------------------------------------------------------------------- /upo_launchers/uva_demo/maps/uva_demo.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/uva_demo/maps/uva_demo.pgm -------------------------------------------------------------------------------- /upo_launchers/uva_demo/maps/uva_demo.yaml: -------------------------------------------------------------------------------- 1 | image: uva_demo.pgm 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/uva_demo/maps/yield_map.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/uva_demo/maps/yield_map.jpg -------------------------------------------------------------------------------- /upo_launchers/uva_demo/maps/yield_map.yaml: -------------------------------------------------------------------------------- 1 | image: yield_map.jpg 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/uva_demo/maps/yield_map_empty.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/uva_demo/maps/yield_map_empty.jpg -------------------------------------------------------------------------------- /upo_launchers/uva_demo/maps/yield_points.yaml: -------------------------------------------------------------------------------- 1 | point1: 2 | x: 36.99 3 | y: 16.6 4 | theta: -2.79 5 | point2: 6 | x: 37.475 7 | y: 15.475 8 | theta: 2.79 9 | point3: 10 | x: 39.925 11 | y: 15.125 12 | theta: 0.35 13 | point4: 14 | x: 33.65 15 | y: 15.825 16 | theta: -0.35 17 | center1: 18 | x: 35.925 19 | y: 15.875 20 | theta: 0.0 21 | -------------------------------------------------------------------------------- /upo_launchers/uva_demo/move_base/params_mb/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | # 2D Costmap parameters 2 | # http://www.ros.org/wiki/costmap_2d 3 | 4 | ### Robot description parameters ### 5 | 6 | # Robot foot print in m (TERESA) 7 | #footprint: [ [0.32, 0], [0.2263, -0.2263], [0, -0.32], [-0.2263, -0.2263], [-0.32, 0], [-0.2263, 0.2263], [0, 0.32], [0.2263, 0.2263]] 8 | footprint: [ [0.345, 0], [0.244, -0.244], [0, -0.345], [-0.244, -0.244], [-0.345, 0], [-0.244, 0.244], [0, 0.345], [0.244, 0.244]] 9 | #footprint: [ [0.37, 0], [0.262, -0.262], [0, -0.37], [-0.262, -0.262], [-0.37, 0], [-0.262, 0.262], [0, 0.37], [0.262, 0.262]] 10 | #footprint: [ [0.40, 0], [0.2828, -0.2828], [0, -0.40], [-0.2828, -0.2828], [-0.40, 0], [-0.2828, 0.2828], [0, 0.40], [0.2828, 0.2828]] 11 | 12 | # Inscribed and circunscribed radius in m 13 | inscribed_radius: 0.345 14 | circumscribed_radius: 0.345 15 | 16 | # Obstacle inflation in m (circumscribed robot radius) 17 | #inflation_radius: 0.345 18 | 19 | transform_tolerance: 0.5 20 | map_type: costmap 21 | 22 | -------------------------------------------------------------------------------- /upo_launchers/uva_demo/move_base/params_mb/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 2.0 #default=5.0 5 | publish_frequency: 2.0 6 | static_map: true 7 | rolling_window: false 8 | track_unknown_space: true 9 | always_send_full_costmap: false 10 | transform_tolerance: 0.2 11 | resolution: 0.05 #default=0.05 meters/cells 12 | 13 | 14 | plugins: 15 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 16 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 17 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 18 | #- {name: social_layer, type: "social_layer::SocialLayer"} 19 | 20 | 21 | static_layer: 22 | map_topic: /map 23 | unknown_cost_value: -1 #default=-1 24 | trinary_costmap: true 25 | lethal_cost_threshold: 100 #default=100 26 | track_unknown_space: false 27 | use_maximum: false 28 | first_map_only: true #default=false 29 | subscribe_to_updates: false #if true, it subscribes to topic /map_topic_updates 30 | 31 | 32 | obstacle_layer: 33 | obstacle_range: 4.5 34 | max_obstacle_range: 4.5 35 | raytrace_range: 4.8 36 | track_unknown_space: false 37 | transform_tolerance: 0.2 #default=0.2 38 | ### Sensor management parameters ### 39 | observation_sources: laser xtion 40 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 41 | xtion: {sensor_frame: base_link, data_type: LaserScan, topic: depthcamscan_node/scanXtion, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8} 42 | 43 | 44 | inflation_layer: 45 | inflation_radius: 0.64 #circumscribed robot radius 46 | cost_scaling_factor: 10.0 #3.0 47 | 48 | 49 | #social_layer: 50 | # size_x: 5.0 51 | # size_y: 5.0 52 | # all_features: false 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /upo_launchers/uva_demo/move_base/params_mb/global_costmap_params_sim.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 2.0 #default=5.0 5 | publish_frequency: 2.0 6 | static_map: true 7 | rolling_window: false 8 | track_unknown_space: true 9 | always_send_full_costmap: false 10 | transform_tolerance: 0.2 11 | resolution: 0.05 #default=0.05 meters/cells 12 | 13 | 14 | plugins: 15 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 16 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 17 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 18 | #- {name: social_layer, type: "social_layer::SocialLayer"} 19 | 20 | 21 | static_layer: 22 | map_topic: /map 23 | unknown_cost_value: -1 #default=-1 24 | trinary_costmap: true 25 | lethal_cost_threshold: 100 #default=100 26 | track_unknown_space: false 27 | use_maximum: false 28 | first_map_only: true #default=false 29 | subscribe_to_updates: false #if true, it subscribes to topic /map_topic_updates 30 | 31 | 32 | obstacle_layer: 33 | obstacle_range: 4.5 34 | max_obstacle_range: 4.5 35 | raytrace_range: 4.8 36 | track_unknown_space: false 37 | transform_tolerance: 0.2 #default=0.2 38 | ### Sensor management parameters ### 39 | observation_sources: laser #xtion 40 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 41 | #xtion: {sensor_frame: base_link, data_type: LaserScan, topic: depthcamscan_node/scanXtion, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8} 42 | 43 | 44 | inflation_layer: 45 | inflation_radius: 0.64 #circumscribed robot radius 46 | cost_scaling_factor: 10.0 #3.0 47 | 48 | 49 | #social_layer: 50 | # size_x: 5.0 51 | # size_y: 5.0 52 | # all_features: false 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /upo_launchers/uva_demo/move_base/params_mb/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 8.0 5 | publish_frequency: 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 10.0 9 | height: 10.0 10 | resolution: 0.05 #0.025 11 | 12 | 13 | plugins: 14 | #- {name: static_layer, type: "costmap_2d::StaticLayer"} 15 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 16 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 17 | #- {name: social_layer, type: "social_layer::SocialLayer"} 18 | 19 | 20 | #static_layer: 21 | # map_topic: /map 22 | # unknown_cost_value: -1 #default=-1 23 | # trinary_costmap: true 24 | # lethal_cost_threshold: 100 #default=100 25 | # track_unknown_space: false 26 | # use_maximum: false 27 | # first_map_only: true #default=false 28 | # subscribe_to_updates: true #if true, it subscribes to topic /_updates 29 | 30 | 31 | obstacle_layer: 32 | obstacle_range: 4.5 33 | max_obstacle_range: 4.5 34 | raytrace_range: 4.8 35 | track_unknown_space: false 36 | transform_tolerance: 0.2 #default=0.2 37 | ### Sensor management parameters ### 38 | observation_sources: laser xtion 39 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 40 | xtion: {sensor_frame: base_link, data_type: LaserScan, topic: depthcamscan_node/scanXtion, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8} 41 | 42 | 43 | inflation_layer: 44 | inflation_radius: 0.64 #0.34*2 = 0.68 45 | cost_scaling_factor: 10.0 #10.0=default 46 | 47 | 48 | #social_layer: 49 | # size_x: 5.0 50 | # size_y: 5.0 51 | # all_features: true 52 | 53 | -------------------------------------------------------------------------------- /upo_launchers/uva_demo/move_base/params_mb/local_costmap_params_sim.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 8.0 5 | publish_frequency: 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 10.0 9 | height: 10.0 10 | resolution: 0.05 #0.025 11 | 12 | 13 | plugins: 14 | #- {name: static_layer, type: "costmap_2d::StaticLayer"} 15 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 16 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 17 | #- {name: social_layer, type: "social_layer::SocialLayer"} 18 | 19 | 20 | #static_layer: 21 | # map_topic: /map 22 | # unknown_cost_value: -1 #default=-1 23 | # trinary_costmap: true 24 | # lethal_cost_threshold: 100 #default=100 25 | # track_unknown_space: false 26 | # use_maximum: false 27 | # first_map_only: true #default=false 28 | # subscribe_to_updates: true #if true, it subscribes to topic /_updates 29 | 30 | 31 | obstacle_layer: 32 | obstacle_range: 4.5 33 | max_obstacle_range: 4.5 34 | raytrace_range: 4.8 35 | track_unknown_space: false 36 | transform_tolerance: 0.2 #default=0.2 37 | ### Sensor management parameters ### 38 | observation_sources: laser #xtion 39 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 40 | #xtion: {sensor_frame: base_link, data_type: LaserScan, topic: depthcamscan_node/scanXtion, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8} 41 | 42 | 43 | inflation_layer: 44 | inflation_radius: 0.64 #0.34*2 = 0.68 45 | cost_scaling_factor: 10.0 #10.0=default 46 | 47 | 48 | #social_layer: 49 | # size_x: 5.0 50 | # size_y: 5.0 51 | # all_features: true 52 | 53 | -------------------------------------------------------------------------------- /upo_launchers/uva_demo/sim_uva_demo_nav_behavior.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /upo_launchers/uva_lab/maps/uva_lab.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/uva_lab/maps/uva_lab.pgm -------------------------------------------------------------------------------- /upo_launchers/uva_lab/maps/uva_lab.yaml: -------------------------------------------------------------------------------- 1 | image: uva_lab.pgm 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/uva_lab/maps/yield_map.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_launchers/uva_lab/maps/yield_map.jpg -------------------------------------------------------------------------------- /upo_launchers/uva_lab/maps/yield_map.yaml: -------------------------------------------------------------------------------- 1 | image: yield_map.jpg 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /upo_launchers/uva_lab/maps/yield_points.yaml: -------------------------------------------------------------------------------- 1 | point1: 2 | x: 8.4 3 | y: 18.5 4 | theta: 0.785 5 | point2: 6 | x: 10.8 7 | y: 16.65 8 | theta: 1.66 9 | point3: 10 | x: 11.1 11 | y: 21.15 12 | theta: -2.09 13 | point4: 14 | x: 10.3 15 | y: 21 16 | theta: -1.92 17 | center1: 18 | x: 10.1 19 | y: 19.15 20 | theta: 0.0 21 | -------------------------------------------------------------------------------- /upo_launchers/uva_lab/move_base/params_mb/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | # 2D Costmap parameters 2 | # http://www.ros.org/wiki/costmap_2d 3 | 4 | ### Robot description parameters ### 5 | 6 | # Robot foot print in m (TERESA) 7 | #footprint: [ [0.32, 0], [0.2263, -0.2263], [0, -0.32], [-0.2263, -0.2263], [-0.32, 0], [-0.2263, 0.2263], [0, 0.32], [0.2263, 0.2263]] 8 | footprint: [ [0.345, 0], [0.244, -0.244], [0, -0.345], [-0.244, -0.244], [-0.345, 0], [-0.244, 0.244], [0, 0.345], [0.244, 0.244]] 9 | #footprint: [ [0.37, 0], [0.262, -0.262], [0, -0.37], [-0.262, -0.262], [-0.37, 0], [-0.262, 0.262], [0, 0.37], [0.262, 0.262]] 10 | #footprint: [ [0.40, 0], [0.2828, -0.2828], [0, -0.40], [-0.2828, -0.2828], [-0.40, 0], [-0.2828, 0.2828], [0, 0.40], [0.2828, 0.2828]] 11 | 12 | # Inscribed and circunscribed radius in m 13 | inscribed_radius: 0.345 14 | circumscribed_radius: 0.345 15 | 16 | # Obstacle inflation in m (circumscribed robot radius) 17 | #inflation_radius: 0.345 18 | 19 | transform_tolerance: 0.5 20 | map_type: costmap 21 | 22 | -------------------------------------------------------------------------------- /upo_launchers/uva_lab/move_base/params_mb/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 2.0 #default=5.0 5 | publish_frequency: 2.0 6 | static_map: true 7 | rolling_window: false 8 | track_unknown_space: true 9 | always_send_full_costmap: false 10 | transform_tolerance: 0.2 11 | resolution: 0.05 #default=0.05 meters/cells 12 | 13 | 14 | plugins: 15 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 16 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 17 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 18 | #- {name: social_layer, type: "social_layer::SocialLayer"} 19 | 20 | 21 | static_layer: 22 | map_topic: /map 23 | unknown_cost_value: -1 #default=-1 24 | trinary_costmap: true 25 | lethal_cost_threshold: 100 #default=100 26 | track_unknown_space: false 27 | use_maximum: false 28 | first_map_only: true #default=false 29 | subscribe_to_updates: false #if true, it subscribes to topic /map_topic_updates 30 | 31 | 32 | obstacle_layer: 33 | obstacle_range: 4.5 34 | max_obstacle_range: 4.5 35 | raytrace_range: 4.8 36 | track_unknown_space: false 37 | transform_tolerance: 0.2 #default=0.2 38 | ### Sensor management parameters ### 39 | observation_sources: laser xtion 40 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 41 | xtion: {sensor_frame: base_link, data_type: LaserScan, topic: depthcamscan_node/scanXtion, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8} 42 | 43 | 44 | inflation_layer: 45 | inflation_radius: 0.64 #circumscribed robot radius 46 | cost_scaling_factor: 10.0 #3.0 47 | 48 | 49 | #social_layer: 50 | # size_x: 5.0 51 | # size_y: 5.0 52 | # all_features: false 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /upo_launchers/uva_lab/move_base/params_mb/global_costmap_params_sim.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 2.0 #default=5.0 5 | publish_frequency: 2.0 6 | static_map: true 7 | rolling_window: false 8 | track_unknown_space: true 9 | always_send_full_costmap: false 10 | transform_tolerance: 0.2 11 | resolution: 0.05 #default=0.05 meters/cells 12 | 13 | 14 | plugins: 15 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 16 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 17 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 18 | #- {name: social_layer, type: "social_layer::SocialLayer"} 19 | 20 | 21 | static_layer: 22 | map_topic: /map 23 | unknown_cost_value: -1 #default=-1 24 | trinary_costmap: true 25 | lethal_cost_threshold: 100 #default=100 26 | track_unknown_space: false 27 | use_maximum: false 28 | first_map_only: true #default=false 29 | subscribe_to_updates: false #if true, it subscribes to topic /map_topic_updates 30 | 31 | 32 | obstacle_layer: 33 | obstacle_range: 4.5 34 | max_obstacle_range: 4.5 35 | raytrace_range: 4.8 36 | track_unknown_space: false 37 | transform_tolerance: 0.2 #default=0.2 38 | ### Sensor management parameters ### 39 | observation_sources: laser #xtion 40 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 41 | #xtion: {sensor_frame: base_link, data_type: LaserScan, topic: depthcamscan_node/scanXtion, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8} 42 | 43 | 44 | inflation_layer: 45 | inflation_radius: 0.64 #circumscribed robot radius 46 | cost_scaling_factor: 10.0 #3.0 47 | 48 | 49 | #social_layer: 50 | # size_x: 5.0 51 | # size_y: 5.0 52 | # all_features: false 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /upo_launchers/uva_lab/move_base/params_mb/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 8.0 5 | publish_frequency: 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 10.0 9 | height: 10.0 10 | resolution: 0.05 #0.025 11 | 12 | 13 | plugins: 14 | #- {name: static_layer, type: "costmap_2d::StaticLayer"} 15 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 16 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 17 | #- {name: social_layer, type: "social_layer::SocialLayer"} 18 | 19 | 20 | #static_layer: 21 | # map_topic: /map 22 | # unknown_cost_value: -1 #default=-1 23 | # trinary_costmap: true 24 | # lethal_cost_threshold: 100 #default=100 25 | # track_unknown_space: false 26 | # use_maximum: false 27 | # first_map_only: true #default=false 28 | # subscribe_to_updates: true #if true, it subscribes to topic /_updates 29 | 30 | 31 | obstacle_layer: 32 | obstacle_range: 4.5 33 | max_obstacle_range: 4.5 34 | raytrace_range: 4.8 35 | track_unknown_space: false 36 | transform_tolerance: 0.2 #default=0.2 37 | ### Sensor management parameters ### 38 | observation_sources: laser xtion 39 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 40 | xtion: {sensor_frame: base_link, data_type: LaserScan, topic: depthcamscan_node/scanXtion, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8} 41 | 42 | 43 | inflation_layer: 44 | inflation_radius: 0.64 #0.34*2 = 0.68 45 | cost_scaling_factor: 10.0 #10.0=default 46 | 47 | 48 | #social_layer: 49 | # size_x: 5.0 50 | # size_y: 5.0 51 | # all_features: true 52 | 53 | -------------------------------------------------------------------------------- /upo_launchers/uva_lab/move_base/params_mb/local_costmap_params_sim.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 8.0 5 | publish_frequency: 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 10.0 9 | height: 10.0 10 | resolution: 0.05 #0.025 11 | 12 | 13 | plugins: 14 | #- {name: static_layer, type: "costmap_2d::StaticLayer"} 15 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 16 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 17 | #- {name: social_layer, type: "social_layer::SocialLayer"} 18 | 19 | 20 | #static_layer: 21 | # map_topic: /map 22 | # unknown_cost_value: -1 #default=-1 23 | # trinary_costmap: true 24 | # lethal_cost_threshold: 100 #default=100 25 | # track_unknown_space: false 26 | # use_maximum: false 27 | # first_map_only: true #default=false 28 | # subscribe_to_updates: true #if true, it subscribes to topic /_updates 29 | 30 | 31 | obstacle_layer: 32 | obstacle_range: 4.5 33 | max_obstacle_range: 4.5 34 | raytrace_range: 4.8 35 | track_unknown_space: false 36 | transform_tolerance: 0.2 #default=0.2 37 | ### Sensor management parameters ### 38 | observation_sources: laser #xtion 39 | laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 40 | #xtion: {sensor_frame: base_link, data_type: LaserScan, topic: depthcamscan_node/scanXtion, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8} 41 | 42 | 43 | inflation_layer: 44 | inflation_radius: 0.64 #0.34*2 = 0.68 45 | cost_scaling_factor: 10.0 #10.0=default 46 | 47 | 48 | #social_layer: 49 | # size_x: 5.0 50 | # size_y: 5.0 51 | # all_features: true 52 | 53 | -------------------------------------------------------------------------------- /upo_local_planner/ulp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of a simple local controller to follow a pre-defined path. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /upo_msgs/msg/PersonGoalArrayUPO.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint32 size 3 | PersonGoalUPO[] personGoals 4 | -------------------------------------------------------------------------------- /upo_msgs/msg/PersonGoalUPO.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint32 id 3 | geometry_msgs/PoseStamped[] goals 4 | float32[] belief 5 | -------------------------------------------------------------------------------- /upo_msgs/msg/PersonPoseArrayUPO.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint32 size 3 | PersonPoseUPO[] personPoses 4 | -------------------------------------------------------------------------------- /upo_msgs/msg/PersonPoseUPO.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint32 id 3 | float64 vel 4 | geometry_msgs/Point position 5 | geometry_msgs/Quaternion orientation 6 | -------------------------------------------------------------------------------- /upo_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | upo_msgs 4 | 0.0.0 5 | The upo_msgs package 6 | 7 | 8 | Noé Pérez 9 | 10 | 11 | 12 | 13 | 14 | BSD 15 | 16 | 17 | 18 | https://github.com/robotics-upo 19 | 20 | 21 | 22 | 23 | Noé Pérez 24 | 25 | 26 | 27 | 28 | catkin 29 | geometry_msgs 30 | roscpp 31 | sensor_msgs 32 | std_msgs 33 | message_generation 34 | geometry_msgs 35 | roscpp 36 | sensor_msgs 37 | std_msgs 38 | message_runtime 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /upo_navigation/.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | 31 | # Others 32 | *~ 33 | -------------------------------------------------------------------------------- /upo_navigation/README.md: -------------------------------------------------------------------------------- 1 | # upo_navigation 2 | This is the main navigation package. 3 | It uses the A* global planner of ROS to plan a first path to the goal. This path is employed to calculate sub-goals in a local area of the robot where the desired RRT planner is used to plan a social path in a dynamic environment. 4 | 5 | ## Parameters 6 | 7 | - rrt_planner_type. Indicates the type of planning used. 8 | Value '1' indicates a regular navigation. If the planner used is RRT*, then the planner cost function will be considered as a weighted linear combination of features (package navigation_features will be used for features calculation). 9 | Value '2' indicates a RRT* navigation using path planning predicition with Fully Convolutional Network. The library RRT_ros_wrapper3 and the package path_prediction will be employed. Only valid for RRT* planner. 10 | 11 | 12 | The package is a **work in progress** used in research prototyping. Pull requests and/or issues are highly encouraged. 13 | -------------------------------------------------------------------------------- /upo_navigation/include/upo_navigation/planar_laser_scan.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 the Willow Garage 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: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef RRT_PLANAR_LASER_SCAN_H_ 38 | #define RRT_PLANAR_LASER_SCAN_H_ 39 | 40 | #include 41 | #include 42 | 43 | namespace upo_nav { 44 | /** 45 | * @class PlanarLaserScan 46 | * @brief Stores a scan from a planar laser that can be used to clear freespace 47 | */ 48 | class PlanarLaserScan { 49 | public: 50 | PlanarLaserScan() {} 51 | geometry_msgs::Point32 origin; 52 | sensor_msgs::PointCloud cloud; 53 | double angle_min, angle_max, angle_increment; 54 | }; 55 | }; 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /upo_navigation/include/upo_navigation/trajectory_inc.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #ifndef TRAJECTORY_INC_H_ 35 | #define TRAJECTORY_INC_H_ 36 | 37 | #include 38 | 39 | #ifndef DBL_MAX /* Max decimal value of a double */ 40 | #define DBL_MAX std::numeric_limits::max() // 1.7976931348623157e+308 41 | #endif 42 | 43 | #ifndef DBL_MIN //Min decimal value of a double 44 | #define DBL_MIN std::numeric_limits::min() // 2.2250738585072014e-308 45 | #endif 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /upo_navigation/src/map_cell.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | 37 | namespace upo_nav{ 38 | 39 | MapCell::MapCell() 40 | : cx(0), cy(0), 41 | target_dist(DBL_MAX), 42 | target_mark(false), 43 | within_robot(false) 44 | {} 45 | 46 | MapCell::MapCell(const MapCell& mc) 47 | : cx(mc.cx), cy(mc.cy), 48 | target_dist(mc.target_dist), 49 | target_mark(mc.target_mark), 50 | within_robot(mc.within_robot) 51 | {} 52 | }; 53 | -------------------------------------------------------------------------------- /upo_navigation/src/prefer_forward_cost_function.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * prefer_forward_cost_function.cpp 3 | * 4 | * Created on: Apr 4, 2012 5 | * Author: tkruse 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | namespace upo_nav { 13 | 14 | 15 | double PreferForwardCostFunction::scoreTrajectory(Trajectory &traj) { 16 | // backward motions bad on a robot without backward sensors 17 | if (traj.xv_ < 0.0) { 18 | return penalty_; 19 | } 20 | // strafing motions also bad on such a robot 21 | if (traj.xv_ < 0.1 && fabs(traj.thetav_) < 0.2) { 22 | return penalty_; 23 | } 24 | // the more we rotate, the less we progress forward 25 | return fabs(traj.thetav_) * 10; 26 | } 27 | 28 | } /* namespace rrt_tr_local_planner */ 29 | -------------------------------------------------------------------------------- /upo_navigation/src/upo_navigation_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * 3 | */ 4 | 5 | #include 6 | 7 | int main(int argc, char** argv){ 8 | ros::init(argc, argv, "upo_navigation_node"); 9 | tf::TransformListener tf(ros::Duration(10)); 10 | 11 | upo_nav::UpoNavigation UpoNav( tf ); 12 | 13 | //ros::MultiThreadedSpinner s; 14 | ros::spin(); 15 | 16 | return(0); 17 | } 18 | -------------------------------------------------------------------------------- /upo_navigation/srv/FeatureCounts.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped[] path 2 | geometry_msgs/PoseStamped goal 3 | --- 4 | float32[] features 5 | -------------------------------------------------------------------------------- /upo_navigation/srv/PointCosts.srv: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | --- 4 | float64 goaldist_cost 5 | float64 inflation_cost 6 | float64 gaussian_cost 7 | -------------------------------------------------------------------------------- /upo_navigation/srv/SetWeights.srv: -------------------------------------------------------------------------------- 1 | float32[] weights 2 | --- 3 | 4 | -------------------------------------------------------------------------------- /upo_navigation/waypoint_defs.yaml: -------------------------------------------------------------------------------- 1 | cafétaria: 2 | x: 15.164 3 | y: 11.773 4 | theta: -0.138 5 | 6 | comptoir: 7 | x: 12.3 8 | y: 11.18 9 | theta: 1.164 10 | 11 | entrée: 12 | x: 21.50 13 | y: 10.83 14 | theta: 2.729 15 | 16 | retour à la base: 17 | x: 8.75 18 | y: 9.1950 19 | theta: 1.434 20 | 21 | home: 22 | x: 8.410 23 | y: 9.366 24 | theta: 1.434 25 | 26 | -------------------------------------------------------------------------------- /upo_navigation_macro_actions/README.md: -------------------------------------------------------------------------------- 1 | # upo_navigation_macro_actions. 2 | A set of navigation macro-actions have been implemented by using the *actionlib* of ROS. This way, the navigation system is employed to perform different actions as reaching a simple goal, approaching a moving person, or walk side-by-side. A launch file to test the macro-actions is inside the launch directory. 3 | 4 | New macro-actions will be added and explained soon. 5 | 6 | The package is a **work in progress** used in research prototyping. Pull requests and/or issues are highly encouraged. 7 | -------------------------------------------------------------------------------- /upo_navigation_macro_actions/action/AssistedSteering.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | geometry_msgs/Twist cmd 3 | --- 4 | # Result 5 | string result 6 | uint32 value 7 | --- 8 | # Feedback 9 | string text 10 | -------------------------------------------------------------------------------- /upo_navigation_macro_actions/action/NavigateHome.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | geometry_msgs/PoseStamped home_pose 3 | --- 4 | # Result 5 | string result 6 | uint32 value 7 | --- 8 | # Feedback. Current robot position 9 | geometry_msgs/PoseStamped base_position 10 | string text 11 | -------------------------------------------------------------------------------- /upo_navigation_macro_actions/action/NavigateInteractionTarget.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | #upo_msgs/PersonPoseUPO it 3 | uint32 it 4 | --- 5 | # Result 6 | string result 7 | uint32 value 8 | --- 9 | # Feedback. Current robot position 10 | geometry_msgs/PoseStamped base_position 11 | string text 12 | -------------------------------------------------------------------------------- /upo_navigation_macro_actions/action/NavigateWaypoint.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | geometry_msgs/PoseStamped target_pose 3 | --- 4 | # Result 5 | string result 6 | uint32 value 7 | --- 8 | # Feedback. Current robot position 9 | geometry_msgs/PoseStamped base_position 10 | string text 11 | -------------------------------------------------------------------------------- /upo_navigation_macro_actions/action/Wait.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | geometry_msgs/PoseStamped target_pose 3 | --- 4 | # Result 5 | --- 6 | # Feedback 7 | string text 8 | -------------------------------------------------------------------------------- /upo_navigation_macro_actions/action/WalkSideBySide.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | #upo_msgs/PersonPoseUPO it 3 | uint32 it 4 | --- 5 | # Result 6 | string result 7 | uint32 value 8 | --- 9 | # Feedback 10 | string text 11 | 12 | 13 | -------------------------------------------------------------------------------- /upo_navigation_macro_actions/action/Yield.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | geometry_msgs/PoseStamped pose 3 | --- 4 | # Result 5 | string result 6 | uint32 value 7 | --- 8 | # Feedback 9 | string text 10 | -------------------------------------------------------------------------------- /upo_navigation_macro_actions/cfg/NavigationMacroActions.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = 'upo_navigation_macro_actions' 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | 10 | gen.add("control_frequency", double_t, 0, "Frequency of the inner control loops of each macro-action", 15.0, 0, 60); 11 | 12 | 13 | gen.add("secs_to_check_block", double_t, 0, "Time (seconds) that the robot is not able to move so as to declare a blocked situation", 9.0, 0.0, 300.0); 14 | gen.add("block_dist", double_t, 0, "minimum distance (m) that the robot should have moved in 'secs_to_check_block' time from its position to determine a blocked situation", 0.4, 0.0, 2.0); 15 | gen.add("secs_to_wait", double_t, 0, "Time (seconds) for waiting action", 8.0, 0.0, 500.0); 16 | 17 | gen.add("social_approaching_type", int_t, 0, "Way of approaching a target", 1, 1, 3); 18 | 19 | gen.add("secs_to_yield", double_t, 0, "Time (seconds) to wait in a yielding position", 8.0, 0.0, 300.0); 20 | 21 | #gen.add("use_leds", bool_t, 0, "Use the robots leds or not", False); 22 | 23 | #gen.add("leds_number", int_t, 0, "Number of total leds in the robot", 60, 1, 100); 24 | 25 | 26 | exit(gen.generate(PACKAGE, "upo_navigation_macro_actions", "NavigationMacroActions")) 27 | -------------------------------------------------------------------------------- /upo_navigation_macro_actions/include/upo_navigation_macro_actions/Yield.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef YIELD_H 3 | #define YIELD_H 4 | 5 | //#include 6 | //#include 7 | //#include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | 16 | typedef enum {NORMAL, NARROW, SUPERNARROW} zone; 17 | 18 | 19 | struct point { 20 | double x_; 21 | double y_; 22 | double theta_; 23 | }; 24 | 25 | class Yield { 26 | 27 | public: 28 | 29 | Yield(); 30 | Yield(std::string* yamlFile, std::string* pointsFile); 31 | 32 | void getClosestPoint(double curr_x, double curr_y, double& goal_x, double& goal_y, double& goal_theta); 33 | 34 | zone getType(double x, double y, bool &correct); 35 | 36 | bool loadYieldPoints(std::string* pointsFile); 37 | 38 | bool getYieldAreaCenterPoint(double rx, double ry, double& cx, double& cy); 39 | 40 | 41 | private: 42 | 43 | cv::Mat map; 44 | 45 | double origin[3]; 46 | double res; 47 | int mapWidth; 48 | int mapHeight; 49 | 50 | bool has_map; 51 | 52 | std::vector yield_points; 53 | 54 | std::vector center_points; 55 | 56 | 57 | }; 58 | 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /upo_navigation_macro_actions/launch/teleop_keyboard.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /upo_navigation_macro_actions/src/Upo_navigation_macro_actions_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char** argv) 5 | { 6 | ros::init(argc, argv, "navigation_macro_actions"); 7 | 8 | ROS_INFO("Starting navigation_macro_actions..."); 9 | tf::TransformListener tf(ros::Duration(10)); 10 | 11 | upo_nav::UpoNavigation UpoNav(tf, true); 12 | 13 | Upo_navigation_macro_actions *macro = new Upo_navigation_macro_actions(tf, &UpoNav); 14 | 15 | ros::spin(); 16 | 17 | return 0; 18 | } 19 | -------------------------------------------------------------------------------- /upo_robot_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(upo_robot_navigation) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /upo_robot_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | upo_robot_navigation 3 | 1.0.0 4 | A metapackage which contains the packages of the robot navigation system employed by UPO. 5 | Noé Pérez 6 | BSD 7 | 8 | https://github.com/robotics-upo 9 | 10 | Noé Pérez 11 | 12 | catkin 13 | 14 | ros_base 15 | 16 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /upo_rrt_planners/cfg/RRTRosWrapper.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = 'upo_rrt_planners' 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | gen.add("solve_time", double_t, 0, "time that the RRT is allowed to plan a path", 0.5, 0.1, 9.9); 10 | gen.add("visualize_rrt_tree", bool_t, 0, "Whether or not to publish the rrt tree for visualization purposes", True); 11 | gen.add("visualize_costmap", bool_t, 0, "Whether or not to publish the costmap for visualization purposes", True); 12 | gen.add("show_statistics", bool_t, 0, "Whether or not to print on the screen the RRT statistics", True); 13 | 14 | gen.add("path_smoothing", bool_t, 0, "Whether or not to do a simple smoothing of the RRT path", True); 15 | gen.add("smoothing_samples", int_t, 0, "If 'path_smoothing is enabled', it indicates the number of samples to include in the window for smoothing", 5, 0, 100); 16 | 17 | gen.add("goal_bias", double_t, 0, "Percentage of goal biasing", 0.05, 0.0, 20.0); 18 | gen.add("max_rrt_insertion_dist", double_t, 0, "Max distance (meters) to insert a new node", 0.25, 0.0, 5.0); 19 | gen.add("rrtstar_first_path_biasing", bool_t, 0, "Whether or not to bias the sampling from the first solution found", False); 20 | gen.add("rrtstar_first_path_bias", double_t, 0, "Percentage of sampling from the first solution found", 0.8, 0.0, 1.0); 21 | gen.add("rrtstar_first_path_stddev", double_t, 0, "Standard deviation (m) around the first solution found to sample from", 0.4, 0.0, 4.0); 22 | gen.add("full_path_biasing", bool_t, 0, "Whether or not to bias the sampling from a plath passed", True); 23 | gen.add("full_path_bias", double_t, 0, "Percentage of sampling from the path passed", 0.8, 0.0, 1.0); 24 | gen.add("full_path_stddev", double_t, 0, "Standard deviation (m) around the path passed to sample from", 1.0, 0.0, 4.0); 25 | gen.add("gmm_biasing", bool_t, 0, "Whether or not to bias the sampling from a GMM", False); 26 | gen.add("gmm_bias", double_t, 0, "Percentage of sampling from the GMM", 0.95, 0.0, 1.0); 27 | 28 | exit(gen.generate(PACKAGE, "upo_rrt_planners", "RRTRosWrapper")) 29 | -------------------------------------------------------------------------------- /upo_rrt_planners/include/upo_rrt_planners/Action.h: -------------------------------------------------------------------------------- 1 | #ifndef UPO_RRT_ACTION_ 2 | #define UPO_RRT_ACTION_ 3 | 4 | //#include 5 | 6 | namespace upo_RRT { 7 | 8 | class Action 9 | { 10 | public: 11 | Action(); 12 | Action(float vx, float vy, float vth, unsigned int steps); 13 | ~Action(); 14 | 15 | void getAction(float &vx, float &vy, float &vth, unsigned int &steps); 16 | float getVx(); 17 | float getVy(); 18 | float getVth(); 19 | unsigned int getAction(); 20 | unsigned int getSteps(); 21 | 22 | private: 23 | 24 | float vx_; 25 | 26 | float vy_; 27 | 28 | float vth_; 29 | 30 | // The number of steps the control is applied for 31 | unsigned int steps_; 32 | 33 | }; 34 | } 35 | #endif 36 | -------------------------------------------------------------------------------- /upo_rrt_planners/include/upo_rrt_planners/Node.h: -------------------------------------------------------------------------------- 1 | #ifndef UPO_RRT_NODE_ 2 | #define UPO_RRT_NODE_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include //NULL 9 | #include 10 | 11 | namespace upo_RRT { 12 | 13 | class Node 14 | { 15 | public: 16 | Node(); 17 | Node(State s); 18 | Node(State s, Action a); 19 | ~Node(); 20 | 21 | State* getState(); 22 | std::vector* getAction(); 23 | Node* getParent(); 24 | //std::vector getChildren(); 25 | float getCost(); 26 | float getIncCost(); 27 | float getAccCost(); 28 | 29 | 30 | void setParent(Node* p); 31 | //void setChildren(Node* s); 32 | void setState(State s); 33 | void addAction(Action a); 34 | void setAction(std::vector a); 35 | 36 | void setCost(float c); 37 | void setIncCost(float c); 38 | void setAccCost(float c); 39 | 40 | void addIntermediateState(State s); 41 | void setIntermediateStates(std::vector s); 42 | std::vector* getIntermediateStates(); 43 | bool hasIntermediateStates(); 44 | 45 | 46 | //bool deleteStateAndControl(); 47 | 48 | private: 49 | //The state contained by the node 50 | State state_; 51 | 52 | // The action contained by the node 53 | std::vector control_; 54 | 55 | // Intermediate points if they exists 56 | std::vector intermediate_states_; 57 | 58 | bool exist_intermediate_states_; 59 | 60 | // The parent node in the exploration tree 61 | Node* parent_; 62 | 63 | // The set of nodes descending from the current node 64 | //std::vector children_; 65 | 66 | // The cost up to this node 67 | float accCost_; 68 | 69 | // The incremental cost of this node's parent to this node 70 | float incCost_; 71 | 72 | // The cost associated to this single node 73 | float cost_; 74 | }; 75 | } 76 | #endif 77 | -------------------------------------------------------------------------------- /upo_rrt_planners/include/upo_rrt_planners/State.h: -------------------------------------------------------------------------------- 1 | #ifndef UPO_RRT_STATE_ 2 | #define UPO_RRT_STATE_ 3 | 4 | //#include 5 | 6 | namespace upo_RRT { 7 | 8 | class State 9 | { 10 | public: 11 | State(); 12 | State(float x, float y, float yaw = 0.0, float lv = 0.0, float av = 0.0); 13 | ~State(); 14 | 15 | void getState(float &x, float &y, float &yaw, float &lv, float &av); 16 | float getX(); 17 | float getY(); 18 | float getYaw(); 19 | float getLinVel(); 20 | float getAngVel(); 21 | 22 | void setX(float x); 23 | void setY(float y); 24 | void setYaw(float yaw); 25 | void setLv(float lv); 26 | void setAv(float av); 27 | 28 | private: 29 | 30 | float x_; 31 | 32 | float y_; 33 | 34 | float yaw_; 35 | 36 | float lin_vel_; 37 | 38 | float ang_vel_; 39 | 40 | }; 41 | } 42 | #endif 43 | -------------------------------------------------------------------------------- /upo_rrt_planners/include/upo_rrt_planners/StateChecker.h: -------------------------------------------------------------------------------- 1 | #ifndef UPO_RRT_STATE_CHECKER_ 2 | #define UPO_RRT_STATE_CHECKER_ 3 | 4 | #include 5 | #include 6 | 7 | namespace upo_RRT 8 | { 9 | class StateChecker 10 | { 11 | 12 | public: 13 | StateChecker(){}; 14 | virtual ~StateChecker(){} 15 | 16 | virtual bool isValid(State* s) const = 0; 17 | 18 | virtual float distance(State* s1, State* s2) const { 19 | float dx = s1->getX() - s2->getX(); 20 | float dy = s1->getY() - s2->getY(); 21 | return sqrt(dx*dx + dy*dy); 22 | } 23 | 24 | virtual float getCost(State* s) = 0; 25 | 26 | virtual void preplanning_computations() = 0; 27 | 28 | }; 29 | } 30 | #endif 31 | -------------------------------------------------------------------------------- /upo_rrt_planners/include/upo_rrt_planners/planners/control/RRT.h: -------------------------------------------------------------------------------- 1 | #ifndef UPO_RRT_RRT_ 2 | #define UPO_RRT_RRT_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | 20 | namespace upo_RRT 21 | { 22 | class RRT : public upo_RRT::Planner 23 | { 24 | public: 25 | RRT(); 26 | ~RRT(); 27 | 28 | bool steer(Node* fromNode, Node* toNode, Node* newNode); 29 | 30 | std::vector solve(float secs); 31 | 32 | 33 | /*void setMaxRange(float range) { 34 | maxRange_ = range; 35 | }*/ 36 | 37 | void setTimeStep(float step) { 38 | steering_->setTimeStep(step); 39 | } 40 | 41 | void setControlSteps(int min_steps, int max_steps) { 42 | steering_->setMinMaxSteps(min_steps, max_steps); 43 | } 44 | 45 | void setRobotAcc(float linear_acc, float angular_acc) { 46 | steering_->setAccelerations(linear_acc, angular_acc); 47 | } 48 | 49 | void setAccompanySteer(bool s){ 50 | accompany_steer_ = s; 51 | } 52 | 53 | 54 | private: 55 | 56 | bool accompany_steer_; 57 | 58 | //float maxRange_; //max distance to insert a new node 59 | 60 | //float timeStep_; //should be 1/freq with freq = freq of the controller(15~20Hz) 61 | //int minControlSteps_; //minTime = timeStep*minControlDuration 62 | //int maxControlSteps_; 63 | 64 | }; 65 | } 66 | #endif 67 | -------------------------------------------------------------------------------- /upo_rrt_planners/include/upo_rrt_planners/planners/simple/SimpleRRT.h: -------------------------------------------------------------------------------- 1 | #ifndef UPO_RRT_SIMPLE_RRT_ 2 | #define UPO_RRT_SIMPLE_RRT_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | namespace upo_RRT 20 | { 21 | class SimpleRRT : public upo_RRT::Planner 22 | { 23 | public: 24 | SimpleRRT(); 25 | ~SimpleRRT(); 26 | 27 | State* steer(State* fromState, State* toState, std::vector& istates); 28 | 29 | std::vector solve(float secs); 30 | 31 | //void setRobotPosition(); 32 | 33 | 34 | void setMaxRange(float range) { 35 | steering_->setMaxRange(range); 36 | } 37 | 38 | 39 | //private: 40 | 41 | //float maxRange_; //max distance to insert a new node 42 | 43 | }; 44 | } 45 | #endif 46 | -------------------------------------------------------------------------------- /upo_rrt_planners/include/upo_rrt_planners/planners/simple/SimpleRRTstar.h: -------------------------------------------------------------------------------- 1 | #ifndef UPO_RRT_SIMPLE_RRT_STAR_ 2 | #define UPO_RRT_SIMPLE_RRT_STAR_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace upo_RRT 19 | { 20 | class SimpleRRTstar : public upo_RRT::Planner 21 | { 22 | public: 23 | 24 | SimpleRRTstar(); 25 | ~SimpleRRTstar(); 26 | 27 | State* steer(State* fromState, State* toState, std::vector& istates); 28 | 29 | std::vector solve(float secs); 30 | 31 | 32 | void setMaxRange(float range) { 33 | maxRange_ = range; 34 | steering_->setMaxRange(range); 35 | } 36 | 37 | 38 | void set_useKnearest(bool b) { 39 | useKnearest_ = b; 40 | } 41 | 42 | void setRewireFactor(float f) { 43 | rewire_factor_ = f; 44 | } 45 | 46 | void set_useFirstPathBiasing(bool b) { 47 | useFirstPathBiasing_ = b; 48 | } 49 | 50 | 51 | 52 | void getNearestNeighbors(Node* node, std::vector &nbrs); 53 | 54 | void calculateParamsNearest(); 55 | 56 | //float motionCost(Node* n1, Node* n2); 57 | 58 | bool collisionFree(State* fromState, State* toState, std::vector& istates); 59 | 60 | 61 | 62 | 63 | private: 64 | 65 | //float timeStep_; //should be 1/freq with freq = freq of the controller(15~20Hz) 66 | //float minControlSteps_; //minTime = timeStep*minControlDuration 67 | //flaot maxControlSteps_; 68 | 69 | float maxRange_; //max distance to insert a new node 70 | 71 | bool useKnearest_; 72 | double k_rrt_; 73 | double r_rrt_; 74 | float rewire_factor_; 75 | 76 | bool useFirstPathBiasing_; 77 | 78 | 79 | 80 | }; 81 | } 82 | #endif 83 | -------------------------------------------------------------------------------- /upo_rrt_planners/package.xml: -------------------------------------------------------------------------------- 1 | 2 | upo_rrt_planners 3 | 1.0.0 4 | 5 | 6 | A library with some implementations of RRT planners. 7 | 8 | 9 | Noé Pérez-Higueras 10 | Noé Pérez 11 | BSD 12 | https://github.com/robotics-upo 13 | 14 | catkin 15 | 16 | angles 17 | cmake_modules 18 | 19 | geometry_msgs 20 | nav_msgs 21 | rosconsole 22 | roscpp 23 | std_msgs 24 | tf 25 | visualization_msgs 26 | upo_msgs 27 | navigation_features 28 | gmm_sampling 29 | dynamic_reconfigure 30 | message_generation 31 | 32 | angles 33 | cmake_modules 34 | 35 | geometry_msgs 36 | nav_msgs 37 | rosconsole 38 | roscpp 39 | std_msgs 40 | tf 41 | visualization_msgs 42 | upo_msgs 43 | navigation_features 44 | gmm_sampling 45 | dynamic_reconfigure 46 | message_runtime 47 | 48 | 49 | -------------------------------------------------------------------------------- /upo_rrt_planners/saved_input/input_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_rrt_planners/saved_input/input_1.jpg -------------------------------------------------------------------------------- /upo_rrt_planners/saved_input/input_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_rrt_planners/saved_input/input_2.jpg -------------------------------------------------------------------------------- /upo_rrt_planners/saved_input/input_3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_rrt_planners/saved_input/input_3.jpg -------------------------------------------------------------------------------- /upo_rrt_planners/saved_input/input_4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_rrt_planners/saved_input/input_4.jpg -------------------------------------------------------------------------------- /upo_rrt_planners/saved_input/input_5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_rrt_planners/saved_input/input_5.jpg -------------------------------------------------------------------------------- /upo_rrt_planners/saved_input/input_6.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/upo_robot_navigation/6e0b7668683b68037051b2dda0419bda38a51e01/upo_rrt_planners/saved_input/input_6.jpg -------------------------------------------------------------------------------- /upo_rrt_planners/src/Action.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | //Constructor 4 | upo_RRT::Action::Action() 5 | { 6 | vx_ = 0.0; 7 | vy_ = 0.0; 8 | vth_ = 0.0; 9 | steps_ = 1; 10 | } 11 | 12 | //Constructor 13 | upo_RRT::Action::Action(float vx, float vy, float vth, unsigned int steps) 14 | { 15 | vx_ = vx; 16 | vy_ = vy; 17 | vth_ = vth; 18 | steps_ = steps; 19 | } 20 | 21 | // Destructor 22 | upo_RRT::Action::~Action() 23 | { 24 | } 25 | 26 | 27 | void upo_RRT::Action::getAction(float &vx, float &vy, float &vth, unsigned int &steps) 28 | { 29 | vx = vx_; 30 | vy = vy_; 31 | vth = vth_; 32 | steps = steps_; 33 | } 34 | 35 | 36 | float upo_RRT::Action::getVx() {return vx_;} 37 | float upo_RRT::Action::getVy() {return vy_;} 38 | float upo_RRT::Action::getVth() {return vth_;} 39 | unsigned int upo_RRT::Action::getSteps() {return steps_;} 40 | -------------------------------------------------------------------------------- /upo_rrt_planners/src/State.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | //Constructor 4 | upo_RRT::State::State() 5 | { 6 | x_ = 0.0; 7 | y_ = 0.0; 8 | yaw_ = 0.0; 9 | lin_vel_ = 0.0; 10 | ang_vel_ = 0.0; 11 | } 12 | 13 | //Constructor 14 | upo_RRT::State::State(float x, float y, float yaw, float lv, float av) 15 | { 16 | x_ = x; 17 | y_ = y; 18 | yaw_ = yaw; 19 | lin_vel_ = lv; 20 | ang_vel_ = av; 21 | } 22 | 23 | // Destructor 24 | upo_RRT::State::~State() 25 | { 26 | } 27 | 28 | 29 | void upo_RRT::State::getState(float &x, float &y, float &yaw, float &lv, float &av) 30 | { 31 | x = x_; 32 | y = y_; 33 | yaw = yaw_; 34 | lv = lin_vel_; 35 | av = ang_vel_; 36 | } 37 | 38 | float upo_RRT::State::getX() {return x_;} 39 | float upo_RRT::State::getY() {return y_;} 40 | float upo_RRT::State::getYaw() {return yaw_;} 41 | float upo_RRT::State::getLinVel() {return lin_vel_;} 42 | float upo_RRT::State::getAngVel() {return ang_vel_;} 43 | 44 | void upo_RRT::State::setX(float x){x_ = x;} 45 | void upo_RRT::State::setY(float y){y_ = y;} 46 | void upo_RRT::State::setYaw(float yaw){yaw_ = yaw;} 47 | void upo_RRT::State::setLv(float lv){lin_vel_ = lv;} 48 | void upo_RRT::State::setAv(float av){ang_vel_ = av;} 49 | -------------------------------------------------------------------------------- /upo_rrt_planners/src/ros/ros_wrapper2_node.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | int main(int argc, char** argv){ 5 | ros::init(argc, argv, "ros_wrapper2_node"); 6 | tf::TransformListener tf(ros::Duration(10)); 7 | 8 | upo_RRT_ros::RRT_ros_wrapper2 rrt_wrapper(&tf); 9 | 10 | //ros::MultiThreadedSpinner s; 11 | ros::spin(); 12 | 13 | return(0); 14 | } 15 | -------------------------------------------------------------------------------- /upo_rrt_planners/src/ros/ros_wrapper3_node.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | int main(int argc, char** argv){ 5 | ros::init(argc, argv, "ros_wrapper3_node"); 6 | tf::TransformListener tf(ros::Duration(10)); 7 | 8 | upo_RRT_ros::RRT_ros_wrapper3 rrt_wrapper(&tf); 9 | 10 | //ros::MultiThreadedSpinner s; 11 | ros::spin(); 12 | 13 | return(0); 14 | } 15 | -------------------------------------------------------------------------------- /upo_rrt_planners/src/ros/ros_wrapper_node.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | int main(int argc, char** argv){ 5 | ros::init(argc, argv, "ros_wrapper_node"); 6 | tf::TransformListener tf(ros::Duration(10)); 7 | 8 | upo_RRT_ros::RRT_ros_wrapper rrt_wrapper(&tf); 9 | 10 | //ros::MultiThreadedSpinner s; 11 | ros::spin(); 12 | 13 | return(0); 14 | } 15 | -------------------------------------------------------------------------------- /upo_rrt_planners/srv/MakePlan.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped goal 2 | --- 3 | bool ok 4 | geometry_msgs/PoseStamped[] path 5 | -------------------------------------------------------------------------------- /upo_rrt_planners/srv/MakePlanCostmap.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped goal 2 | nav_msgs/OccupancyGrid costmap 3 | --- 4 | bool ok 5 | geometry_msgs/PoseStamped[] path 6 | -------------------------------------------------------------------------------- /upo_social_layer/.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | 31 | # Others 32 | *~ 33 | -------------------------------------------------------------------------------- /upo_social_layer/README.md: -------------------------------------------------------------------------------- 1 | # upo_social_layer 2 | This package contains a layer that can be used in the costmap_2d of ROS as a plugin. The layer is built by using the social functions of the navigation features package related to people. This way, a social cost related to the distance and orientation to people can be included in a costmap for robot navigation. 3 | 4 | The layer can be included as a plugin in the local and/or global costmap of ROS. 5 | 6 | ## Dependences 7 | 8 | * Package **navigation_features**. 9 | 10 | ## Parameters 11 | 12 | * **size_x**. Size of the layer area. Distance in meters from the center of the robot in the X axis. Size [-size_x, size_x]. 13 | * **size_y**. Size of the layer area. Distance in meters from the center of the robot in the Y axis. Size [-size_y, size_y]. 14 | * **all_features**. Boolean parameter to indicate the features functions to apply. 15 | - _True_. Obtain the cost of each cell of the layer as a combination of all the feature functions calculated in the navigation features library (people around the robot, distance to the goal and distance to the closest obstacle). 16 | - _False_. Obtain the cost of each cell of the layer by using the feature function related to the people around the robot. 17 | 18 | The directory _launch_ contains some examples of use of the layer. 19 | 20 | The package is a **work in progress** used in research prototyping. Pull requests and/or issues are highly encouraged. 21 | 22 | 23 | -------------------------------------------------------------------------------- /upo_social_layer/cfg/SocialPlugin.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t 4 | 5 | gen = ParameterGenerator() 6 | 7 | gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) 8 | gen.add("only_proxemics", bool_t, 0, "calculate all the features or only the proxemics feature.", False) 9 | gen.add("w1", double_t, 0, "weight for the goal distance feature", 0.1, 0.0, 1.0); 10 | gen.add("w2", double_t, 0, "weight for the obstacle distance feature", 0.5, 0.0, 1.0); 11 | gen.add("w3", double_t, 0, "weight for the proxemics feature", 0.4, 0.0, 1.0); 12 | gen.add("w4", double_t, 0, "weight for the proxemics back feature", 0.0, 0.0, 1.0); 13 | gen.add("w5", double_t, 0, "weight for the proxemics right side feature", 0.0, 0.0, 1.0); 14 | 15 | exit(gen.generate("upo_social_layer", "upo_social_layer", "SocialPlugin")) 16 | -------------------------------------------------------------------------------- /upo_social_layer/launch/in_global_costmap/params_sim/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | # 2D Costmap parameters 2 | # http://www.ros.org/wiki/costmap_2d 3 | 4 | ### Robot description parameters ### 5 | 6 | # Robot foot print in m (TERESA) 7 | footprint: [ [0.32, 0], [0.2263, -0.2263], [0, -0.32], [-0.2263, -0.2263], [-0.32, 0], [-0.2263, 0.2263], [0, 0.32], [0.2263, 0.2263]] 8 | 9 | # Inscribed and circunscribed radius in m 10 | inscribed_radius: 0.32 11 | circumscribed_radius: 0.32 12 | 13 | # Obstacle inflation in m (circumscribed robot radius) 14 | #inflation_radius: 0.32 15 | 16 | transform_tolerance: 0.5 17 | map_type: costmap 18 | 19 | -------------------------------------------------------------------------------- /upo_social_layer/launch/in_global_costmap/params_sim/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 2.0 #default=5.0 5 | publish_frequency: 2.0 6 | static_map: true 7 | rolling_window: false 8 | track_unknown_space: true 9 | always_send_full_costmap: false 10 | transform_tolerance: 0.2 11 | resolution: 0.05 #default=0.05 meters/cells 12 | 13 | 14 | plugins: 15 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 16 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 17 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 18 | - {name: social_layer, type: "social_layer::SocialLayer"} 19 | 20 | 21 | static_layer: 22 | map_topic: /map 23 | unknown_cost_value: -1 #default=-1 24 | trinary_costmap: true 25 | lethal_cost_threshold: 100 #default=100 26 | track_unknown_space: false 27 | use_maximum: false 28 | first_map_only: true #default=false 29 | subscribe_to_updates: false #if true, it subscribes to topic /map_topic_updates 30 | 31 | 32 | obstacle_layer: 33 | obstacle_range: 5.0 34 | max_obstacle_range: 6.0 35 | raytrace_range: 6.3 36 | track_unknown_space: true 37 | transform_tolerance: 0.2 #default=0.2 38 | observation_sources: laserfront_scan_sensor laserback_scan_sensor 39 | laserfront_scan_sensor: {sensor_frame: base_laser_link_0, data_type: LaserScan, topic: base_scan_0, marking: true, clearing: true, expected_update_rate: 1.0, obstacle_range: 5.0, raytrace_range: 6.3 } 40 | laserback_scan_sensor: {sensor_frame: base_laser_link_1, data_type: LaserScan, topic: base_scan_1, marking: true, clearing: true, expected_update_rate: 1.0, obstacle_range: 5.0, raytrace_range: 6.3} 41 | 42 | #observation_sources: laser 43 | #laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8, obstacle_range: 5.0, raytrace_range: 6.3} 44 | 45 | 46 | inflation_layer: 47 | inflation_radius: 2.0 48 | cost_scaling_factor: 3.0 49 | 50 | 51 | social_layer: 52 | size_x: 5.0 53 | size_y: 5.0 54 | all_features: false 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /upo_social_layer/launch/in_global_costmap/params_sim/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 4.0 5 | publish_frequency: 4.0 6 | static_map: false 7 | rolling_window: true 8 | width: 10.0 9 | height: 10.0 10 | resolution: 0.05 #0.025 11 | 12 | 13 | plugins: 14 | #- {name: static_layer, type: "costmap_2d::StaticLayer"} 15 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 16 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 17 | #- {name: social_layer, type: "social_layer::SocialLayer"} 18 | 19 | 20 | static_layer: 21 | map_topic: /map 22 | unknown_cost_value: -1 #default=-1 23 | trinary_costmap: true 24 | lethal_cost_threshold: 100 #default=100 25 | track_unknown_space: false 26 | use_maximum: false 27 | first_map_only: true #default=false 28 | subscribe_to_updates: true #if true, it subscribes to topic /_updates 29 | 30 | 31 | obstacle_layer: 32 | obstacle_range: 4.9 33 | max_obstacle_range: 4.9 34 | raytrace_range: 5.8 35 | track_unknown_space: true 36 | transform_tolerance: 0.2 #default=0.2 37 | observation_sources: laserfront_scan_sensor laserback_scan_sensor 38 | laserfront_scan_sensor: {sensor_frame: base_laser_link_0, data_type: LaserScan, topic: base_scan_0, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 1.0, obstacle_range: 4.95, raytrace_range: 5.8} 39 | laserback_scan_sensor: {sensor_frame: base_laser_link_1, data_type: LaserScan, topic: base_scan_1, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 1.0, obstacle_range: 4.95, raytrace_range: 5.8} 40 | #laser: {sensor_frame: base_link, data_type: LaserScan, topic: /scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 41 | 42 | 43 | inflation_layer: 44 | inflation_radius: 0.70 #0.34*2 = 0.68 45 | cost_scaling_factor: 10.0 #10.0=default 46 | 47 | 48 | #social_layer: 49 | # size_x: 5.0 50 | # size_y: 5.0 51 | # all_features: true 52 | 53 | -------------------------------------------------------------------------------- /upo_social_layer/launch/in_global_costmap/params_sim/navigation_params.yaml: -------------------------------------------------------------------------------- 1 | #-----GENERAL PARAMS FOR UPO_NAVIGATION ------------------------- 2 | base_global_planner: navfn/NavfnROS 3 | base_local_planner: simple_local_planner/PurePlannerROS 4 | recovery_behaviors: [{ 5 | name: conservative_reset, 6 | type: clear_costmap_recovery/ClearCostmapRecovery 7 | }, { 8 | name: aggressive_reset, 9 | type: clear_costmap_recovery/ClearCostmapRecovery 10 | # }, { 11 | # name: rotate_recovery1, 12 | # type: rotate_recovery/RotateRecovery 13 | # }, { 14 | # name: rotate_recovery2, 15 | # type: rotate_recovery/RotateRecovery 16 | }] 17 | planner_frequency: 5 #def:0 18 | controller_frequency: 15.0 19 | planner_patience: 15.0 20 | controller_patience: 10.0 21 | conservative_reset_dist: 4.0 22 | recovery_behavior_enabled: true 23 | clearing_rotation_allowed: false 24 | shutdown_costmaps: false 25 | oscillation_timeout: 0.0 26 | oscillation_distance: 0.5 27 | 28 | 29 | #------ GLOBAL PLANNER PARAMS ------------------------------------- 30 | # http://www.ros.org/wiki/navfn 31 | NavfnROS: 32 | 33 | allow_unknown: false 34 | planner_window_x: 0.0 35 | planner_window_y: 0.0 36 | default_tolerance: 0.05 37 | visualize_potential: true 38 | planner_costmap_publish_frequency: 1.0 39 | use_dijkstra: false 40 | old_navfn_behavior: false 41 | 42 | 43 | 44 | 45 | #------------------------------------------------------------ 46 | # Parameters for the PATH FOLLOWING CONTROLLER 47 | PurePlanner: 48 | 49 | # Robot Configuration Parameters 50 | max_trans_acc: 1.0 51 | max_rot_acc: 1.0 52 | max_trans_vel: 0.4 53 | min_trans_vel: 0.1 54 | max_rot_vel: 0.5 55 | min_rot_vel: 0.1 56 | min_in_place_rot_vel: 0.4 57 | 58 | # Goal Tolerance Parameters 59 | yaw_goal_tolerance: 0.1 60 | xy_goal_tolerance: 0.10 61 | wp_tolerance: 0.5 62 | 63 | # Forward Simulation Parameters 64 | sim_time: 0.5 65 | sim_granularity: 0.025 66 | angular_sim_granularity: 0.025 67 | 68 | controller_freq: 15.0 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /upo_social_layer/launch/in_global_costmap_all/params_sim/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | # 2D Costmap parameters 2 | # http://www.ros.org/wiki/costmap_2d 3 | 4 | ### Robot description parameters ### 5 | 6 | # Robot foot print in m (TERESA) 7 | footprint: [ [0.32, 0], [0.2263, -0.2263], [0, -0.32], [-0.2263, -0.2263], [-0.32, 0], [-0.2263, 0.2263], [0, 0.32], [0.2263, 0.2263]] 8 | 9 | # Inscribed and circunscribed radius in m 10 | inscribed_radius: 0.32 11 | circumscribed_radius: 0.32 12 | 13 | # Obstacle inflation in m (circumscribed robot radius) 14 | #inflation_radius: 0.32 15 | 16 | transform_tolerance: 0.5 17 | map_type: costmap 18 | 19 | -------------------------------------------------------------------------------- /upo_social_layer/launch/in_global_costmap_all/params_sim/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 2.0 #default=5.0 5 | publish_frequency: 2.0 6 | static_map: true 7 | rolling_window: false 8 | track_unknown_space: true 9 | always_send_full_costmap: false 10 | transform_tolerance: 0.2 11 | resolution: 0.10 #0.05 #default=0.05 meters/cells 12 | 13 | 14 | plugins: 15 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 16 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 17 | #- {name: inflation_layer, type: "costmap_2d::InflationLayer"} 18 | - {name: social_layer, type: "social_layer::SocialLayer"} 19 | 20 | 21 | static_layer: 22 | map_topic: /map 23 | unknown_cost_value: -1 #default=-1 24 | trinary_costmap: true 25 | lethal_cost_threshold: 100 #default=100 26 | track_unknown_space: false 27 | use_maximum: false 28 | first_map_only: true #default=false 29 | subscribe_to_updates: false #if true, it subscribes to topic /map_topic_updates 30 | 31 | 32 | obstacle_layer: 33 | obstacle_range: 5.0 34 | max_obstacle_range: 6.0 35 | raytrace_range: 6.3 36 | track_unknown_space: true 37 | transform_tolerance: 0.2 #default=0.2 38 | observation_sources: laserfront_scan_sensor laserback_scan_sensor 39 | laserfront_scan_sensor: {sensor_frame: base_laser_link_0, data_type: LaserScan, topic: base_scan_0, marking: true, clearing: true, expected_update_rate: 1.0, obstacle_range: 5.0, raytrace_range: 6.3 } 40 | laserback_scan_sensor: {sensor_frame: base_laser_link_1, data_type: LaserScan, topic: base_scan_1, marking: true, clearing: true, expected_update_rate: 1.0, obstacle_range: 5.0, raytrace_range: 6.3} 41 | 42 | #observation_sources: laser 43 | #laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8, obstacle_range: 5.0, raytrace_range: 6.3} 44 | 45 | 46 | #inflation_layer: 47 | # inflation_radius: 0.30 48 | # cost_scaling_factor: 10.0 49 | 50 | 51 | social_layer: 52 | size_x: 5.0 53 | size_y: 5.0 54 | only_proxemics: false 55 | w1: 0.2 # goal dist 56 | w2: 0.2 # obstacles 57 | w3: 0.2 # front gaussian 58 | w4: 0.2 # back gaussian 59 | w5: 0.2 # right side gaussian 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /upo_social_layer/launch/in_global_costmap_all/params_sim/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 4.0 5 | publish_frequency: 4.0 6 | static_map: false 7 | rolling_window: true 8 | width: 10.0 9 | height: 10.0 10 | resolution: 0.05 #0.025 11 | 12 | 13 | plugins: 14 | #- {name: static_layer, type: "costmap_2d::StaticLayer"} 15 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 16 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 17 | #- {name: social_layer, type: "social_layer::SocialLayer"} 18 | 19 | 20 | static_layer: 21 | map_topic: /map 22 | unknown_cost_value: -1 #default=-1 23 | trinary_costmap: true 24 | lethal_cost_threshold: 100 #default=100 25 | track_unknown_space: false 26 | use_maximum: false 27 | first_map_only: true #default=false 28 | subscribe_to_updates: true #if true, it subscribes to topic /_updates 29 | 30 | 31 | obstacle_layer: 32 | obstacle_range: 4.9 33 | max_obstacle_range: 4.9 34 | raytrace_range: 5.8 35 | track_unknown_space: true 36 | transform_tolerance: 0.2 #default=0.2 37 | observation_sources: laserfront_scan_sensor laserback_scan_sensor 38 | laserfront_scan_sensor: {sensor_frame: base_laser_link_0, data_type: LaserScan, topic: base_scan_0, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 1.0, obstacle_range: 4.95, raytrace_range: 5.8} 39 | laserback_scan_sensor: {sensor_frame: base_laser_link_1, data_type: LaserScan, topic: base_scan_1, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 1.0, obstacle_range: 4.95, raytrace_range: 5.8} 40 | #laser: {sensor_frame: base_link, data_type: LaserScan, topic: /scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 41 | 42 | 43 | inflation_layer: 44 | inflation_radius: 0.70 #0.34*2 = 0.68 45 | cost_scaling_factor: 10.0 #10.0=default 46 | 47 | 48 | #social_layer: 49 | # size_x: 5.0 50 | # size_y: 5.0 51 | # all_features: true 52 | 53 | -------------------------------------------------------------------------------- /upo_social_layer/launch/in_local_costmap/params_sim/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | # 2D Costmap parameters 2 | # http://www.ros.org/wiki/costmap_2d 3 | 4 | ### Robot description parameters ### 5 | 6 | # Robot foot print in m (TERESA) 7 | footprint: [ [0.32, 0], [0.2263, -0.2263], [0, -0.32], [-0.2263, -0.2263], [-0.32, 0], [-0.2263, 0.2263], [0, 0.32], [0.2263, 0.2263]] 8 | 9 | # Inscribed and circunscribed radius in m 10 | inscribed_radius: 0.32 11 | circumscribed_radius: 0.32 12 | 13 | # Obstacle inflation in m (circumscribed robot radius) 14 | #inflation_radius: 0.32 15 | 16 | transform_tolerance: 0.5 17 | map_type: costmap 18 | 19 | -------------------------------------------------------------------------------- /upo_social_layer/launch/in_local_costmap/params_sim/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 2.0 #default=5.0 5 | publish_frequency: 2.0 6 | static_map: true 7 | rolling_window: false 8 | track_unknown_space: true 9 | always_send_full_costmap: false 10 | transform_tolerance: 0.2 11 | resolution: 0.05 #default=0.05 meters/cells 12 | 13 | 14 | plugins: 15 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 16 | #- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 17 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 18 | #- {name: social_layer, type: "social_layer::SocialLayer"} 19 | 20 | 21 | static_layer: 22 | map_topic: /map 23 | unknown_cost_value: -1 #default=-1 24 | trinary_costmap: true 25 | lethal_cost_threshold: 100 #default=100 26 | track_unknown_space: false 27 | use_maximum: false 28 | first_map_only: true #default=false 29 | subscribe_to_updates: false #if true, it subscribes to topic /map_topic_updates 30 | 31 | 32 | obstacle_layer: 33 | obstacle_range: 5.0 34 | max_obstacle_range: 6.0 35 | raytrace_range: 6.3 36 | track_unknown_space: true 37 | transform_tolerance: 0.2 #default=0.2 38 | observation_sources: laserfront_scan_sensor laserback_scan_sensor 39 | #laserfront_scan_sensor: {sensor_frame: base_laser_link_0, data_type: LaserScan, topic: base_scan_0, marking: true, clearing: true, expected_update_rate: 1.0, obstacle_range: 5.0, raytrace_range: 6.3 } 40 | #laserback_scan_sensor: {sensor_frame: base_laser_link_1, data_type: LaserScan, topic: base_scan_1, marking: true, clearing: true, expected_update_rate: 1.0, obstacle_range: 5.0, raytrace_range: 6.3} 41 | 42 | #observation_sources: laser 43 | #laser: {sensor_frame: base_link, data_type: LaserScan, topic: scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.8, obstacle_range: 5.0, raytrace_range: 6.3} 44 | 45 | 46 | inflation_layer: 47 | inflation_radius: 2.0 48 | cost_scaling_factor: 3.0 49 | 50 | 51 | #social_layer: 52 | # size_x: 5.0 53 | # size_y: 5.0 54 | # all_features: false 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /upo_social_layer/launch/in_local_costmap/params_sim/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 4.0 5 | publish_frequency: 4.0 6 | static_map: false 7 | rolling_window: true 8 | width: 10.0 9 | height: 10.0 10 | resolution: 0.05 #0.025 11 | 12 | 13 | plugins: 14 | #- {name: static_layer, type: "costmap_2d::StaticLayer"} 15 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 16 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 17 | - {name: social_layer, type: "social_layer::SocialLayer"} 18 | 19 | 20 | static_layer: 21 | map_topic: /map 22 | unknown_cost_value: -1 #default=-1 23 | trinary_costmap: true 24 | lethal_cost_threshold: 100 #default=100 25 | track_unknown_space: false 26 | use_maximum: false 27 | first_map_only: true #default=false 28 | subscribe_to_updates: true #if true, it subscribes to topic /_updates 29 | 30 | 31 | obstacle_layer: 32 | obstacle_range: 4.9 33 | max_obstacle_range: 4.9 34 | raytrace_range: 5.8 35 | track_unknown_space: true 36 | transform_tolerance: 0.2 #default=0.2 37 | observation_sources: laserfront_scan_sensor laserback_scan_sensor 38 | laserfront_scan_sensor: {sensor_frame: base_laser_link_0, data_type: LaserScan, topic: base_scan_0, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 1.0, obstacle_range: 4.95, raytrace_range: 5.8} 39 | laserback_scan_sensor: {sensor_frame: base_laser_link_1, data_type: LaserScan, topic: base_scan_1, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 1.0, obstacle_range: 4.95, raytrace_range: 5.8} 40 | #laser: {sensor_frame: base_link, data_type: LaserScan, topic: /scan360, marking: true, clearing: true, inf_is_valid: false, expected_update_rate: 0.6, obstacle_range: 4.95, raytrace_range: 5.8} 41 | 42 | 43 | inflation_layer: 44 | inflation_radius: 0.70 #0.34*2 = 0.68 45 | cost_scaling_factor: 10.0 #10.0=default 46 | 47 | 48 | social_layer: 49 | size_x: 5.0 50 | size_y: 5.0 51 | all_features: true 52 | 53 | -------------------------------------------------------------------------------- /upo_social_layer/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | upo_social_layer 4 | 0.0.0 5 | The upo_social_layer package 6 | 7 | 8 | 9 | 10 | Noé Pérez 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 | costmap_2d 44 | dynamic_reconfigure 45 | roscpp 46 | message_generation 47 | navigation_features 48 | 49 | costmap_2d 50 | dynamic_reconfigure 51 | roscpp 52 | message_runtime 53 | navigation_features 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /upo_social_layer/social_layer_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Layer that add a social cost based on the people around the robot 4 | 5 | 6 | -------------------------------------------------------------------------------- /upo_social_layer/srv/Cost.srv: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | --- 4 | float32 cost 5 | -------------------------------------------------------------------------------- /upo_social_layer/srv/Features.srv: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | --- 4 | float32[] features 5 | -------------------------------------------------------------------------------- /upo_social_layer/srv/SetDemoPath.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped[] demo 2 | bool use_loss_func 3 | --- 4 | 5 | -------------------------------------------------------------------------------- /upo_social_layer/srv/SetGoal.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped goal 2 | --- 3 | 4 | --------------------------------------------------------------------------------