HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

182
mir_robot/mir_navigation/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,182 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package mir_navigation
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.1.7 (2023-01-20)
------------------
* Don't set cmake_policy CMP0048
* Add license headers
* Fix flake8 warnings
* Contributors: Martin Günther
1.1.6 (2022-06-02)
------------------
* navigation: Reduce footprint to actual size
This reduces the footprint:
* 18 mm in front
* 42 mm in the back
* 27 mm at the sides
Now the footprint exactly matches the bounding box of the mesh, with no
padding. This should make navigation in tight spaces easier; let's hope
it doesn't lead to collisions.
* navigation: Move footprint_padding to proper namespace
The footprint_padding parameter in the upper namespace was ignored,
needed to be moved into local_costmap/global_costmap to take effect.
* genmprim: Remove obsolete plt.hold()
This fixes the following error:
AttributeError: module 'matplotlib.pyplot' has no attribute 'hold'
* mir_navigation: Remove static_map parameter
This fixes the following warning:
[ WARN] local_costmap: Pre-Hydro parameter "static_map" unused since "plugins" is provided
* Contributors: Martin Günther
1.1.5 (2022-02-11)
------------------
1.1.4 (2021-12-10)
------------------
1.1.3 (2021-06-11)
------------------
* Merge branch 'melodic-2.8' into noetic
* Rename tf frame and topic 'odom_comb' -> 'odom'
This is how they are called on the real MiR since MiR software 2.0.
* Reformat python code using black
* Contributors: Martin Günther
1.1.2 (2021-05-12)
------------------
* Uncomment available dependencies in noetic (`#79 <https://github.com/DFKI-NI/mir_robot/issues/79>`_)
* Contributors: Oscar Lima
1.1.1 (2021-02-11)
------------------
* Add optional namespace to launch files
* Add prefix to start_planner.launch (`#67 <https://github.com/DFKI-NI/mir_robot/issues/67>`_)
* Update scripts to Python3 (Noetic)
* Contributors: Martin Günther
1.1.0 (2020-06-30)
------------------
* Initial release into noetic
* Remove hector_mapping dependency (not released in noetic)
* Update scripts to Python3 (Noetic)
* Contributors: Martin Günther
1.0.6 (2020-06-30)
------------------
* Add missing matplotlib dependency
* plot_mprim: Fix color display
* Fix bug in genmprim_unicycle_highcost_5cm
In Python3, np.arange doesn't accept floats.
* Fix some catkin_lint warnings
* Set cmake_policy CMP0048 to fix warning
* Contributors: Martin Günther
1.0.5 (2020-05-01)
------------------
* Rename hector_mapping.launch, add dependency
* genmprim.py: Improve plotting
* genmprim.py: Make executable
* SBPL: Reduce allocated_time + initial_epsilon params
This leads to shorter planning times, but will perhaps fail on larger
maps.
* Update mprim file to mir-software 2.0.17
This was updated in 2.0.17 and hasn't changed through 2.6 at least.
* Add genmprim_unicycle matlab + python script, fix mprim file
* Adjust dwb params: split_path, finer trajectories (`#43 <https://github.com/DFKI-NI/mir_robot/issues/43>`_)
- use split_path option to enforce following complex paths
- more trajectory samples over a smaller simulated time. This fixes a
problem where the robot would stop too far away from the goal, as all
possible trajectories either overshot the goal, or were too short to
reach into the next gridcell of the critics.
- remove Oscillation critic (never helped)
* added PathDistPrunedCritic for dwb (`#42 <https://github.com/DFKI-NI/mir_robot/issues/42>`_)
which works exactly like the original PathDistCritic, except that it
searches for a local minimum in the distance from the global path to the robots
current position. It then prunes the global_path from the start up to
this point, therefore approximately cutting of a segment of the path
that the robot already followed.
* Add default local_planner to move_base launch file
This makes the hector_mapping Gazebo demo work with the instructions
from the README (see `#32 <https://github.com/DFKI-NI/mir_robot/issues/32>`_).
* Contributors: Martin Günther, Nils Niemann
1.0.4 (2019-05-06)
------------------
* Rviz config: Add planned paths + costmap from real MiR
* Contributors: Martin Günther
1.0.3 (2019-03-04)
------------------
* fix frame_id for melodic (`#18 <https://github.com/DFKI-NI/mir_robot/issues/18>`_)
* Tune dwb parameters
* PathProgressCritic: Add heading score
* Use dwb_local_planner in move_base config
* Move footprint param to move_base root namespace
This allows other move_base plugins, such as dwb_local_planner, to
access this parameter.
* Add hector_mapping
* amcl.launch: Change default, remap service
This is required if amcl.launch is started within a namespace.
* teb_local_planner: Fix odom topic name
* Merge pull request `#16 <https://github.com/DFKI-NI/mir_robot/issues/16>`_ from niniemann/add-prefix-argument-to-configs
Add prefix argument to configs
* adds $(arg prefix) to a lot of configs
This is an important step to be able to re-parameterize move base,
the diffdrive controller, ekf, amcl and the costmaps for adding a
tf prefix to the robots links
* mir_navigation: Adjust helper node topics
* Add amcl launchfile (`#11 <https://github.com/DFKI-NI/mir_robot/issues/11>`_)
* added amcl.launch
* changed amcl params to default mir amcl parameters
* Merge pull request `#13 <https://github.com/DFKI-NI/mir_robot/issues/13>`_ from niniemann/fix-virtual-walls
The previous configuration of the local costmap didn't work for me -- obstacles seen in the laser scans were not added, or were overridden by the virtual\_walls\_map layer. Reordering the layers and loading the virtual walls before the obstacles fixes this for me.
Also, I added a `with_virtual_walls` parameter to `start_maps.launch` and `start_planner.launch`.
* added with_virtual_walls parameter to start_maps and start_planner
* reorder local costmap plugins
* Revert "mir_navigation: Disable virtual walls if no map file set"
This reverts commit 0cfda301b2bb1e8b3458e698efd24a7901e5d132.
The reason is that the `eval` keyword was introduced in kinetic, so it
doesn't work in indigo.
* mir_navigation: Update rviz config
* mir_navigation: Disable virtual walls if no map file set
* mir_navigation: Rename virtual_walls args + files
* mir_navigation: Remove parameter first_map_only
This parameter must be set to false (the default) when running SLAM
(otherwise the map updates won't be received), and when running a static
map_server it doesn't matter; even then, it should be false to allow
restarting the map_server with a different map. Therefore this commit
removes it altogether and leaves it at the default of "false".
* split parameter files between mapping/planning (`#10 <https://github.com/DFKI-NI/mir_robot/issues/10>`_)
The differences are simple: When mapping, first_map_only must be
set to false, and the virtual walls plugin must not be loaded
(else move_base will wait for a topic that is not going to be
published).
* Document move_base params, add max_planning_retries
Setting max_planning_retries to 10 makes the planner fail faster if the
planning problem is infeasible. By default, there's an infinite number
of retries, so we had to wait until the planner_patience ran out (5 s).
* Update rviz config
Make topics relative, so that ROS_NAMESPACE=... works.
* Switch to binary sbpl_lattice_planner dependency
... instead of compiling from source.
* Split scan_rep117 topic into two separate topics
This fixes the problem that the back laser scanner was ignored in the
navigation costmap in Gazebo (probably because in Gazebo, both laser
scanners have the exact same timestamp).
* mir_navigation: Add clear_params to move_base launch
* mir_navigation: marking + clearing were switched
Other than misleading names, this had no effect.
* Contributors: Martin Günther, Nils Niemann, Noël Martignoni
1.0.2 (2018-07-30)
------------------
1.0.1 (2018-07-17)
------------------
1.0.0 (2018-07-12)
------------------
* Initial release
* Contributors: Martin Günther

76
mir_robot/mir_navigation/CMakeLists.txt Executable file
View File

@@ -0,0 +1,76 @@
cmake_minimum_required(VERSION 3.5.1)
project(mir_navigation)
find_package(catkin REQUIRED COMPONENTS
roslaunch
move_base
amcl
nav_core_adapter
base_local_planner
dwb_local_planner
dwb_plugins
dwb_critics
sbpl_lattice_planner
teb_local_planner
map_server
)
###################################
## catkin specific configuration ##
###################################
catkin_package()
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
# Mark executable scripts (Python etc.) for installation
# in contrast to setup.py, you can choose the destination
install(PROGRAMS
mprim/genmprim_unicycle_highcost_5cm.py
nodes/acc_finder.py
nodes/min_max_finder.py
scripts/plot_mprim.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark executables and/or libraries for installation
# install(TARGETS mir_navigation mir_navigation_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
# Mark other files for installation (e.g. launch and bag files, etc.)
install(DIRECTORY
config
launch
mprim
rviz
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_mir_navigation.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
roslaunch_add_file_check(launch)

44
mir_robot/mir_navigation/config/base_local_planner_params.yaml Executable file
View File

@@ -0,0 +1,44 @@
base_local_planner: base_local_planner/TrajectoryPlannerROS
TrajectoryPlannerROS:
# Robot configuration
acc_lim_x: 1.5
acc_lim_y: 0
acc_lim_theta: 2.0
max_vel_x: 0.8
min_vel_x: 0.1
max_vel_theta: 1.0
min_vel_theta: -1.0
min_in_place_vel_theta: 0.2
escape_vel: -0.1
holonomic_robot: false
# Goal tolerance
yaw_goal_tolerance: 0.03
xy_goal_tolerance: 0.08
latch_xy_goal_tolerance: true
# Forward simulaton parameters
sim_time: 1.2
sim_granularity: 0.025
angular_sim_granularity: 0.04
vx_samples: 15
vtheta_samples: 20
controller_frequency: 10
# Trajectory scoring parameters
meter_scoring: true
pdist_scale: 2.2 #0.6
gdist_scale: 0.8 #0.8
occdist_scale: 0.1
heading_lookahead: 0.325
heading_scoring: true
heading_scoring_timestep: 0.8
dwa: true
publish_cost_grid: false
global_frame_id: map
# Oscillation prevention parameter
oscillation_reset_dist: 0.05
# Global plan parameters
prune_plan: true

65
mir_robot/mir_navigation/config/costmap_common_params.yaml Executable file
View File

@@ -0,0 +1,65 @@
robot_base_frame: $(arg prefix)base_footprint
transform_tolerance: 0.2
obstacle_range: 3.0
#mark_threshold: 1
publish_voxel_map: true
footprint_padding: 0.0
navigation_map:
map_topic: /map
virtual_walls_map:
map_topic: /virtual_walls/map
use_maximum: true
lethal_cost_threshold: 100
preferred_zones_map:
map_topic: /preferred_zones/map
lethal_cost_threshold: 100
use_maximum: true
unpreferred_zones_map:
map_topic: /unpreferred_zones/map
lethal_cost_threshold: 100
use_maximum: true
critical_zones_map:
map_topic: /critical_zones/map
lethal_cost_threshold: 100
use_maximum: true
# subscribe_to_updates: true
direction_zones_map:
map_topic: /direction_zones/map
base_frame_id: $(arg prefix)base_footprint
lethal_cost_threshold: 100
use_maximum: true
# subscribe_to_updates: true
obstacles:
observation_sources: b_scan_marking b_scan_clearing f_scan_marking f_scan_clearing
b_scan_marking:
topic: b_scan_rep117
data_type: LaserScan
clearing: false
marking: true
inf_is_valid: false
min_obstacle_height: 0.13
max_obstacle_height: 0.25
b_scan_clearing:
topic: b_scan_rep117
data_type: LaserScan
clearing: true
marking: false
inf_is_valid: false
min_obstacle_height: 0.13
max_obstacle_height: 0.25
f_scan_marking:
topic: f_scan_rep117
data_type: LaserScan
clearing: false
marking: true
inf_is_valid: false
min_obstacle_height: 0.13
max_obstacle_height: 0.25
f_scan_clearing:
topic: f_scan_rep117
data_type: LaserScan
clearing: true
marking: false
inf_is_valid: false
min_obstacle_height: 0.13
max_obstacle_height: 0.25

14
mir_robot/mir_navigation/config/costmap_global_params.yaml Executable file
View File

@@ -0,0 +1,14 @@
global_costmap:
global_frame: map
update_frequency: 1.0
publish_frequency: 1.0
raytrace_range: 2.0
resolution: 0.05
z_resolution: 0.2
rolling_window: false
z_voxels: 10
inflation:
cost_scaling_factor: 4.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
inflation_radius: 0.8 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
# plugins are loaded via costmap_global_params_plugins_[mapping|planning].yaml

13
mir_robot/mir_navigation/config/costmap_global_params_plugins.yaml Executable file
View File

@@ -0,0 +1,13 @@
global_costmap:
frame_id: map
plugins:
- {name: navigation_map, type: "costmap_2d::StaticLayer" }
- {name: virtual_walls_map, type: "costmap_2d::StaticLayer" }
- {name: obstacles, type: "costmap_2d::VoxelLayer" }
- {name: preferred_zones_map, type: "costmap_2d::PreferredLayer" }
- {name: unpreferred_zones_map, type: "costmap_2d::UnPreferredLayer" }
- {name: direction_zones_map, type: "costmap_2d::DirectionalLayer" }
- {name: inflation, type: "costmap_2d::InflationLayer" }
- {name: critical_zones_map, type: "costmap_2d::CriticalLayer" }
# obstacles:
# track_unknown_space: true

18
mir_robot/mir_navigation/config/costmap_local_params.yaml Executable file
View File

@@ -0,0 +1,18 @@
local_costmap:
global_frame: $(arg prefix)odom
update_frequency: 5.0
publish_frequency: 5.0
rolling_window: true
raytrace_range: 2.0
resolution: 0.05
z_resolution: 0.15
z_voxels: 8
inflation:
cost_scaling_factor: 5.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
inflation_radius: 0.8 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
width: 10.0
height: 10.0
origin_x: 0.0
origin_y: 0.0
# plugins are loaded via costmap_local_params_plugins_[mapping|planning].yaml

7
mir_robot/mir_navigation/config/costmap_local_params_plugins.yaml Executable file
View File

@@ -0,0 +1,7 @@
local_costmap:
frame_id: odom
plugins:
- {name: virtual_walls_map, type: "costmap_2d::StaticLayer" } # các ô chi phí xung quanh "tường ảo" sẽ được tăng lên giá trị cao hơn, tạo ra một vùng an toàn xung quanh các vật cản.
- {name: obstacles, type: "costmap_2d::VoxelLayer" } # các ô chi phí xung quanh các "vật cản" sẽ được tăng lên giá trị cao hơn, tạo ra một vùng an toàn xung quanh các vật cản.
- {name: inflation, type: "costmap_2d::InflationLayer" } # các ô chi phí xung quanh các "vật cản" sẽ được tăng lên giá trị cao hơn, tạo ra một vùng an toàn xung quanh các vật cản.
# - {name: critical_zones_map, type: "costmap_2d::CriticalLayer" }

55
mir_robot/mir_navigation/config/dwa_local_planner_params.yaml Executable file
View File

@@ -0,0 +1,55 @@
base_local_planner: dwa_local_planner/DWAPlannerROS
DWAPlannerROS:
# Robot configuration
max_vel_x: 0.8
min_vel_x: -0.2
max_vel_y: 0.0 # diff drive robot
min_vel_y: 0.0 # diff drive robot
max_trans_vel: 0.8 # choose slightly less than the base's capability
min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity
trans_stopped_vel: 0.03
# Warning!
# do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
# are non-negligible and small in place rotational velocities will be created.
max_rot_vel: 1.0 # choose slightly less than the base's capability
min_rot_vel: 0.1 # this is the min angular velocity when there is negligible translational velocity
rot_stopped_vel: 0.1
acc_lim_x: 1.5
acc_lim_y: 0.0 # diff drive robot
acc_limit_trans: 1.5
acc_lim_theta: 2.0
# Goal tolerance
yaw_goal_tolerance: 0.03 # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide)
xy_goal_tolerance: 0.08 # xy_goal_tolerance > (sim_time * min_vel_x) / 2
latch_xy_goal_tolerance: true
# Forward simulation
sim_time: 1.2
vx_samples: 15
vy_samples: 1 # diff drive robot, there is only one sample
vtheta_samples: 20
# Trajectory scoring
path_distance_bias: 64.0 # default: 32.0 mir: 32.0 - weighting for how much it should stick to the global path plan
goal_distance_bias: 12.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal
occdist_scale: 0.5 # default: 0.01 mir: 0.01 - weighting for how much the controller should avoid obstacles
forward_point_distance: 0.325 # default: 0.325 mir: 0.325 - how far along to place an additional scoring point
stop_time_buffer: 0.2 # default: 0.2 mir: 0.2 - amount of time a robot must stop before colliding for a valid traj.
scaling_speed: 0.25 # default: 0.25 mir: 0.25 - absolute velocity at which to start scaling the robot's footprint
max_scaling_factor: 0.2 # default: 0.2 mir: 0.2 - how much to scale the robot's footprint when at speed.
prune_plan: true
# Oscillation prevention
oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags, in m
oscillation_reset_angle: 0.2 # 0.2 - the angle the robot must turn before resetting Oscillation flags, in rad
# Debugging
publish_traj_pc : true
publish_cost_grid_pc: true
global_frame_id: /odom # or <robot namespace>/odom

102
mir_robot/mir_navigation/config/dwb_local_planner_params.yaml Executable file
View File

@@ -0,0 +1,102 @@
base_local_planner: nav_core_adapter::LocalPlannerAdapter
LocalPlannerAdapter:
planner_name: dwb_local_planner::DWBLocalPlanner
DWBLocalPlanner:
# Robot configuration
max_vel_x: 1.5
min_vel_x: -0.3
max_vel_y: 0.0 # diff drive robot
min_vel_y: 0.0 # diff drive robot
max_speed_xy: 2.0 # max_trans_vel: 0.8 # choose slightly less than the base's capability
min_speed_xy: 0.1 # min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity
max_vel_theta: 1.0 # max_rot_vel: 1.0 # choose slightly less than the base's capability
min_speed_theta: 0.1 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity
acc_lim_x: 0.4
acc_lim_y: 0.0 # diff drive robot
acc_lim_theta: 1.0
decel_lim_x: -1.0
decel_lim_y: -0.0
decel_lim_theta: -1.0
# Goal tolerance
yaw_goal_tolerance: 0.03 # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide)
xy_goal_tolerance: 0.05 # xy_goal_tolerance > (sim_time * min_vel_x) / 2
# latch_xy_goal_tolerance: true
# Whether to split the path into segments or not
# Requires https://github.com/locusrobotics/robot_navigation/pull/50
split_path: true
# Forward simulation (trajectory generation)
trajectory_generator_name: dwb_plugins::StandardTrajectoryGenerator # or dwb_plugins::LimitedAccelGenerator
sim_time: 1.2
vx_samples: 12
vy_samples: 1 # diff drive robot, there is only one sample
vtheta_samples: 12
discretize_by_time: true
angular_granularity: 0.05
linear_granularity: 0.1
# sim_period
include_last_point: true
# Goal checking
goal_checker_name: dwb_plugins::SimpleGoalChecker
# stateful: true
# Critics (trajectory scoring)
# default_critic_namespaces: [dwb_critics, mir_dwb_critics]
critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathProgress]
# critics: [MetratronikTechnik]
# MetratronikTechnik:
# scale: 0.01
# sq_window: 1.0
# xy_goal_tolerance: 0.02
# trans_stopped_velocity: 0.3
# slowing_factor: 1.0
# angle_threshold: 0.436332313
# Lm: 0.25
RotateToGoal:
scale: 100.0
trans_stopped_velocity: 0.25
lookahead_time: -1.0
slowing_factor: 10.0
ObstacleFootprint:
scale: 0.01 # default: 0.01 mir: 0.01 - weighting for how much the controller should avoid obstacles
max_scaling_factor: 0.5 # default: 0.2 mir: 0.2 - how much to scale the robot's footprint when at speed.
scaling_speed: 0.25 # default: 0.25 mir: 0.25 - absolute velocity at which to start scaling the robot's footprint
sum_scores: false # if true, return sum of scores of all trajectory points instead of only last one
PathAlign:
scale: 16.0
forward_point_distance: 0.325 # default: 0.325 mir: 0.325 - how far along to place an additional scoring point
PathDistPruned:
scale: 0.01 # default: 32.0 mir: 32.0 - weighting for how much it should stick to the global path plan
class: 'mir_dwb_critics::PathDistPruned'
PathProgress:
scale: 24.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal
heading_scale: 0.01
class: 'mir_dwb_critics::PathProgress'
xy_local_goal_tolerance: 0.5
angle_threshold: 0.523598776 # 30 degrees
# angle_threshold: 0.785398163 # 45 degrees
# Prune already passed poses from plan
prune_plan: true
prune_distance: 1.0 # Old poses farther away than prune_distance (in m) will be pruned.
# If the robot ever gets away further than this distance from the plan,
# the error "Resulting plan has 0 poses in it" will be thrown and
# replanning will be triggered.
# Debugging
publish_cost_grid_pc: true
debug_trajectory_details: false
publish_evaluation: true
publish_global_plan: true
publish_input_params: true
publish_local_plan: true
publish_trajectories: true
publish_transformed_plan: true
marker_lifetime: 0.5

44
mir_robot/mir_navigation/config/eband_local_planner_params.yaml Executable file
View File

@@ -0,0 +1,44 @@
base_local_planner: eband_local_planner/EBandPlannerROS
EBandPlannerROS:
# Robot configuration
max_vel_lin: 0.8 # choose slightly less than the base's capability
min_vel_lin: 0.1 # this is the min trans velocity when there is negligible rotational velocity
trans_stopped_vel: 0.03
max_vel_th: 1.0 # choose slightly less than the base's capability
min_vel_th: 0.1 # this is the min angular velocity when there is negligible translational velocity
rot_stopped_vel: 0.1
min_in_place_vel_th: 0.1 # Minimum in-place angular velocity
in_place_trans_vel: 0.0 # Minimum in place linear velocity
max_acceleration: 1.5 # Maximum allowable acceleration
max_translational_acceleration: 1.5 # Maximum linear acceleration
max_rotational_acceleration: 2.0 # Maximum angular acceleration
# Goal tolerance
yaw_goal_tolerance: 0.03 # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide)
xy_goal_tolerance: 0.08 # xy_goal_tolerance > (sim_time * min_vel_x) / 2
# Elastic Band Parameters
eband_min_relative_overlap: 0.7 # Min distance that denotes connectivity between consecutive bubbles
eband_tiny_bubble_distance: 0.01 # Bubble geometric bound regarding tiny bubble distance
eband_tiny_bubble_expansion: 0.01 # Bubble geometric bound regarding tiny bubble expansion
eband_internal_force_gain: 1.0 # Force gain of forces between consecutive bubbles that tend to stretch the elastic band
eband_external_force_gain: 2.0 # Force gain of forces that tend to move the bubbles away from obstacles
num_iterations_eband_optimization: 3 # Number of iterations for eband optimization
eband_equilibrium_approx_max_recursion_depth: 4 # Number of iterations for reaching the equilibrium between internal and external forces
eband_equilibrium_relative_overshoot: 0.75 # Maximum relative equlibrium overshoot
eband_significant_force_lower_bound: 0.15 # Minimum magnitude of force that is considered significant and used in the calculations
costmap_weight: 10.0 # Costmap weight factor used in the calculation of distance to obstacles
# Trajectory Controller Parameters
k_prop: 4.0 # Proportional gain of the PID controller
k_damp: 3.5 # Damping gain of the PID controller
Ctrl_Rate: 10.0 # Control rate
virtual_mass: 0.75 # Virtual mass
rotation_correction_threshold: 0.5 # Rotation correction threshold
differential_drive: True # Denotes whether to use the differential drive mode
bubble_velocity_multiplier: 2.0 # Multiplier of bubble radius
rotation_threshold_multiplier: 1.0 # Multiplier of rotation threshold
disallow_hysteresis: False # Determines whether to try getting closer to the goal, in case of going past the tolerance

81
mir_robot/mir_navigation/config/mir_local_planner_params.yaml Executable file
View File

@@ -0,0 +1,81 @@
base_local_planner: mirLocalPlanner/MIRPlannerROS
MIRPlannerROS:
# Robot configuration
max_vel_x: 0.8
min_vel_x: -0.2
min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity
# Warning!
# do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
# are non-negligible and small in place rotational velocities will be created.
max_rot_vel: 1.0 # choose slightly less than the base's capability
min_rot_vel: 0.1 # this is the min angular velocity when there is negligible translational velocity
acc_lim_x: 1.5
acc_lim_y: 0.0 # diff drive robot
acc_lim_th: 2.0
min_inplace_rot: 0.15
max_inplace_rot: 0.6
min_in_place_rotational_vel: 0.2
escape_vel: -0.1
holonomic_robot: false
# Goal tolerance
yaw_goal_tolerance: 0.03 # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide)
xy_goal_tolerance: 0.08 # xy_goal_tolerance > (sim_time * min_vel_x) / 2
# Forward simulaton
sim_time: 1.2
sim_granularity: 0.025
vx_samples: 15
vth_samples: 20
vtheta_samples: 20
# Trajectory scoring
path_distance_bias: 0.4 # weighting for how much it should stick to the global path plan
goal_distance_bias: 0.6 # weighting for how much it should attempt to reach its goal
occdist_scale: 0.01 # weighting for how much the controller should avoid obstacles
forward_point_distance: 0.325 # how far along to place an additional scoring point
stop_time_buffer: 0.2 # amount of time a robot must stop before colliding for a valid traj.
scaling_speed: 0.25 # absolute velocity at which to start scaling the robot's footprint
max_scaling_factor: 0.2 # how much to scale the robot's footprint when at speed.
heading_lookahead: 0.325
dwa: true
# Oscillation prevention
oscillation_reset_dist: 0.05 # how far to travel before resetting oscillation flags, in m
# Debugging
publish_visualization: false
publish_cost_grid_pc: false
global_frame_id: odom
# MiR specific parameters
tau_p: 5.0 # The proportional value in the CTE PID tracking
tau_i: 0.1 # The integral value in the CTE PID tracking
tau_d: 0.5 # The differential value in the CTE PID tracking
tau_w: 9.0 # The proportional angle value in the CTE tracking
k_rho: 1.0 # The proportional value to goal in Goal tracking
k_alfa: 8.0 # The diff angle between the robot and the goal in the Goal tracking
k_beta: -2.5 # The angle to the goal from the robot in the Goal tracking
blocked_path_dist: 3.0 # At what distance should the planner react when the path is blocked
blocked_path_dev: 60.0 # How far can we move from the planned path when it is blocked
blocked_path_action: new_plan # Which action to take, when path is blocked
occ_path_dist: 3.0 # At what distance should the planner react when the path is near obstacle
occ_path_dev: 15.0 # How far can we move from the planned path when the path is near a obstacle
occ_path_level: 120.0 # Threshold level for a obstacle
cte_look_ahead: 0.2 # The max/min distance to add for the CTE tracking
penalize_negative_x: true # Whether to penalize trajectories that have negative x velocities.
# non-dynamic parameters
dist_towards_obstacles: 1.5
dist_towards_obstacles_trolley: 1.75
goal_seek_tolerance: 2.0
goal_seek_tolerance_trolley: 0.25

46
mir_robot/mir_navigation/config/move_base_common_params.yaml Executable file
View File

@@ -0,0 +1,46 @@
### footprint
# footprint: [[0.488,-0.293],[0.488,0.293],[-0.412,0.293],[-0.412,-0.293]] # mir 100
footprint: [[0.405,-0.293],[0.405,0.293],[-0.412,0.293],[-0.412,-0.293]] # mir 250
# footprint: [[0.394,-0.385],[0.394,0.385],[-0.363,0.385],[-0.363,-0.385]]
### replanning
controller_frequency: 15.0 # run controller at 5.0 Hz
controller_patience: 2.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
planner_frequency: 0.0 # don't continually replan (only when controller failed)
planner_patience: 5.0 # if the first planning attempt failed, abort planning retries after 5.0 s...
max_planning_retries: 5 # ... or after 10 attempts (whichever happens first)
oscillation_timeout: 30.0 # abort controller and trigger recovery behaviors after 30.0 s
oscillation_distance: 0.3
### recovery behaviors
recovery_behavior_enabled: true
recovery_behaviors: [
{name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
# {name: particular, type: twist_recovery/TwistRecovery},
# {name: by_pass, type: direction_zones_recovery/DirectionZonesRecovery},
]
conservative_reset:
reset_distance: 3.0 # clear obstacles farther away than 3.0 m
force_updating: true
# invert_area_to_clear: true
aggressive_reset:
reset_distance: 3.0
force_updating: true
particular:
linear_x: -0.3
linear_y: 0.0
angular_z: 0.0
duration: 1.5
# advoicedance:
# use_local_frame: false
# plan_topic: /move_base_node/SBPLLatticePlanner/plan
# planning_distance: 4.0
by_pass:
reset_distance: 8.0
force_updating: true
affected_maps: global

46
mir_robot/mir_navigation/config/pose_local_planner_params.yaml Executable file
View File

@@ -0,0 +1,46 @@
base_local_planner: pose_follower/PoseFollower
PoseFollower:
k_trans: 2.0
k_rot: 1.0
# within this distance to the goal, finally rotate to the goal heading (also, we've reached our goal only if we're within this dist)
tolerance_trans: 0.3
# we've reached our goal only if we're within this angular distance
tolerance_rot: 0.3
# we've reached our goal only if we're within range for this long and stopped
tolerance_timeout: 0.5
# set this to true if you're using a holonomic robot
holonomic: false
# number of samples (scaling factors of our current desired twist) to check the validity of
samples: 10
# go no faster than this
max_vel_lin: 0.3
max_vel_th: 0.5
# minimum velocities to keep from getting stuck
min_vel_lin: 0.03
min_vel_th: 0.1
# if we're rotating in place, go at least this fast to avoid getting stuck
min_in_place_vel_th: 0.1
# when we're near the end and would be trying to go no faster than this translationally, just rotate in place instead
in_place_trans_vel: 0.1
# we're "stopped" if we're going slower than these velocities
trans_stopped_velocity: 0.03
rot_stopped_velocity: 0.1
# if this is true, we don't care whether we go backwards or forwards
allow_backwards: true
# if this is true, turn in place to face the new goal instead of arcing toward it
turn_in_place_first: true
# if turn_in_place_first is true, turn in place if our heading is more than this far from facing the goal location
max_heading_diff_before_moving: 0.2

10
mir_robot/mir_navigation/config/sbpl_global_params.yaml Executable file
View File

@@ -0,0 +1,10 @@
base_global_planner: SBPLLatticePlanner
SBPLLatticePlanner:
environment_type: XYThetaLattice
planner_type: ARAPlanner
allocated_time: 10.0
initial_epsilon: 1.0
force_scratch_limit: 10000
forward_search: true
nominalvel_mpersecs: 0.3
timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s

105
mir_robot/mir_navigation/config/teb_local_planner_params.yaml Executable file
View File

@@ -0,0 +1,105 @@
# NOTE: When using the teb_local_planner, make sure to set the local costmap
# resolution high (for example 0.1 m), otherwise the optimization will take
# forever (around 10 minutes for each iteration).
base_local_planner: teb_local_planner/TebLocalPlannerROS
TebLocalPlannerROS:
# Trajectory
teb_autosize: true # Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)
dt_ref: 0.3 # Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)
dt_hysteresis: 0.1 # Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref
global_plan_overwrite_orientation: false # Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically
allow_init_with_backwards_motion: false # If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors)
max_global_plan_lookahead_dist: 3.0 # Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]
force_reinit_new_goal_dist: 1.0 # Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)
feasibility_check_no_poses: 5 # Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval
global_plan_viapoint_sep: -0.1 # Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]
via_points_ordered: false # If true, the planner adheres to the order of via-points in the storage container
exact_arc_length: false # If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used.
publish_feedback: false # Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)
# Robot
max_vel_x: 0.8 # Maximum translational velocity of the robot
max_vel_x_backwards: 0.2 # Maximum translational velocity of the robot for driving backwards
max_vel_y: 0.0 # Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)
max_vel_theta: 1.0 # Maximum angular velocity of the robot
acc_lim_x: 1.5 # Maximum translational acceleration of the robot
acc_lim_y: 0.0 # Maximum strafing acceleration of the robot
acc_lim_theta: 2.0 # Maximum angular acceleration of the robot
min_turning_radius: 0.0 # Minimum turning radius of a carlike robot (diff-drive robot: zero)
wheelbase: 1.0 # The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!
cmd_angle_instead_rotvel: false # Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')
is_footprint_dynamic: false # If true, update the footprint before checking trajectory feasibility
footprint_model:
type: "polygon"
vertices: [[0.506,-0.32],[0.506,0.32],[-0.454,0.32],[-0.454,-0.32]]
# Goal tolerance
xy_goal_tolerance: 0.03 # Allowed final euclidean distance to the goal position
yaw_goal_tolerance: 0.08 # Allowed final orientation error to the goal orientation
free_goal_vel: false # Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)
# Obstacles
min_obstacle_dist: 0.4 # Minimum desired separation from obstacles
inflation_dist: 0.1 # Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)
dynamic_obstacle_inflation_dist: 0.6 # Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)
include_dynamic_obstacles: false # Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.
include_costmap_obstacles: true # Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)
legacy_obstacle_association: false # If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only 'relevant' obstacles).
obstacle_association_force_inclusion_factor: 1.5 # The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.
obstacle_association_cutoff_factor: 5.0 # See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.
costmap_obstacles_behind_robot_dist: 1.5 # Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)
obstacle_poses_affected: 10 # The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well
#costmap_converter_plugin: "" #
#costmap_converter_spin_thread: true #
#costmap_converter_rate: 5 #
# Optimization
no_inner_iterations: 5 # Number of solver iterations called in each outerloop iteration
no_outer_iterations: 4 # Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations
optimization_activate: true # Activate the optimization
optimization_verbose: false # Print verbose information
penalty_epsilon: 0.1 # Add a small safty margin to penalty functions for hard-constraint approximations
weight_max_vel_x: 2.0 # Optimization weight for satisfying the maximum allowed translational velocity
weight_max_vel_y: 2.0 # Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)
weight_max_vel_theta: 1.0 # Optimization weight for satisfying the maximum allowed angular velocity
weight_acc_lim_x: 1.0 # Optimization weight for satisfying the maximum allowed translational acceleration
weight_acc_lim_y: 1.0 # Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)
weight_acc_lim_theta: 1.0 # Optimization weight for satisfying the maximum allowed angular acceleration
weight_kinematics_nh: 1000.0 # Optimization weight for satisfying the non-holonomic kinematics
weight_kinematics_forward_drive: 1.0 # Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)
weight_kinematics_turning_radius: 1.0 # Optimization weight for enforcing a minimum turning radius (carlike robots)
weight_optimaltime: 1.0 # Optimization weight for contracting the trajectory w.r.t transition time
weight_obstacle: 50.0 # Optimization weight for satisfying a minimum seperation from obstacles
weight_inflation: 0.1 # Optimization weight for the inflation penalty (should be small)
weight_dynamic_obstacle: 50.0 # Optimization weight for satisfying a minimum seperation from dynamic obstacles
weight_dynamic_obstacle_inflation: 0.1 # Optimization weight for the inflation penalty of dynamic obstacles (should be small)
weight_viapoint: 1.0 # Optimization weight for minimizing the distance to via-points
weight_adapt_factor: 2.0 # Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new: weight_old * factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.
# Homotopy Class Planner
# enable_homotopy_class_planning: true #
enable_multithreading: true # Activate multiple threading for planning multiple trajectories in parallel
# simple_exploration: false #
max_number_classes: 2 # Specify the maximum number of allowed alternative homotopy classes (limits computational effort)
selection_cost_hysteresis: 1.0 # Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor)
selection_prefer_initial_plan: 0.95 # Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)
selection_obst_cost_scale: 100.0 # Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)
selection_viapoint_cost_scale: 1.0 # Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)
selection_alternative_time_cost: false # If true, time cost is replaced by the total transition time.
roadmap_graph_no_samples: 15 # Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off
roadmap_graph_area_width: 5.0 # Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)
roadmap_graph_area_length_scale: 1.0 # The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal!)
h_signature_prescaler: 1.0 # Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2<H<=1)
h_signature_threshold: 0.1 # Two h-signuteres are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold
# obstacle_keypoint_offset: 0.1 #
obstacle_heading_threshold: 0.45 # Specify the value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration)
viapoints_all_candidates: true # If true, all trajectories of different topologies are attached to the set of via-points, otherwise only the trajectory sharing the same one as the initial/global plan is attached (no effect in test_optim_node).
visualize_hc_graph: false # Visualize the graph that is created for exploring new homotopy classes
visualize_with_time_as_z_axis_scale: 0.0 # If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.
# Recovery
shrink_horizon_backup: true # Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues.
oscillation_recovery: true # Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards).
odom_topic: odom
map_frame: odom

105
mir_robot/mir_navigation/launch/amcl.launch Executable file
View File

@@ -0,0 +1,105 @@
<?xml version="1.0" ?>
<launch>
<arg name="tf_prefix" default="" />
<arg name="namespace" default="$(arg tf_prefix)" doc="Namespace to push all topics into."/>
<arg name="use_map_topic" default="true"/>
<arg name="scan_topic" default="scan"/>
<arg name="map_topic" default="/map"/> <!-- if use_map_topic = true -->
<arg name="map_service" default="/static_map"/> <!-- if use_map_topic = false -->
<arg name="initial_pose_x" default="0.0"/>
<arg name="initial_pose_y" default="0.0"/>
<arg name="initial_pose_a" default="0.0"/>
<arg name="odom_frame_id" default="$(arg tf_prefix)/odom"/>
<arg name="base_frame_id" default="$(arg tf_prefix)/base_footprint"/>
<arg name="global_frame_id" default="/map"/>
<group if="$(eval namespace != '')" ns="$(arg namespace)">
<node pkg="amcl" type="amcl" name="amcl" output="screen" >
<param name="use_map_topic" value="$(arg use_map_topic)"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="50"/>
<param name="laser_min_range" value="-1.0"/>
<param name="laser_max_range" value="-1.0"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.02"/>
<param name="odom_alpha2" value="0.01"/>
<param name="odom_alpha3" value="0.01"/>
<param name="odom_alpha4" value="0.02"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.2"/>
<param name="odom_frame_id" value="$(arg odom_frame_id)"/>
<param name="base_frame_id" value="$(arg base_frame_id)"/>
<param name="global_frame_id" value="$(arg global_frame_id)"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.2"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="$(arg initial_pose_x)"/>
<param name="initial_pose_y" value="$(arg initial_pose_y)"/>
<param name="initial_pose_a" value="$(arg initial_pose_a)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="map" to="$(arg map_topic)"/>
<remap from="static_map" to="$(arg map_service)"/>
</node>
</group>
<!-- Duplicate of the above in case namespace is empty. This is necessary to
avoid the "<group> tag has an empty 'ns' attribute" parsing error. -->
<group unless="$(eval namespace != '')">
<node pkg="amcl" type="amcl" name="amcl" output="screen" >
<param name="use_map_topic" value="$(arg use_map_topic)"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="50"/>
<param name="laser_min_range" value="-1.0"/>
<param name="laser_max_range" value="-1.0"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.02"/>
<param name="odom_alpha2" value="0.01"/>
<param name="odom_alpha3" value="0.01"/>
<param name="odom_alpha4" value="0.02"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.2"/>
<param name="odom_frame_id" value="$(arg odom_frame_id)"/>
<param name="base_frame_id" value="$(arg base_frame_id)"/>
<param name="global_frame_id" value="$(arg global_frame_id)"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.2"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="$(arg initial_pose_x)"/>
<param name="initial_pose_y" value="$(arg initial_pose_y)"/>
<param name="initial_pose_a" value="$(arg initial_pose_a)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="map" to="$(arg map_topic)"/>
<remap from="static_map" to="$(arg map_service)"/>
</node>
</group>
</launch>

68
mir_robot/mir_navigation/launch/hector_mapping.launch Executable file
View File

@@ -0,0 +1,68 @@
<?xml version="1.0"?>
<launch>
<arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
<arg name="base_frame" default="base_footprint"/>
<arg name="odom_frame" default="odom"/>
<arg name="pub_map_odom_transform" default="true"/>
<arg name="scan_subscriber_queue_size" default="5"/>
<arg name="scan_topic" default="scan"/>
<arg name="map_size" default="2048"/>
<!-- set use_tf_pose_start_estimate and map_with_known_poses to `true` when
the map-base_footprint transform is provided by a different node, such
as fake_localization -->
<arg name="disable_localization" default="false"/>
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
<!-- Frame names -->
<param name="map_frame" value="map" />
<param name="base_frame" value="$(arg base_frame)" />
<param name="odom_frame" value="$(arg odom_frame)" />
<!-- Tf use -->
<param name="use_tf_scan_transformation" value="true"/>
<param name="use_tf_pose_start_estimate" value="$(arg disable_localization)"/>
<param name="map_with_known_poses" value="$(arg disable_localization)" />
<param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>
<param name="pub_map_scanmatch_transform" value="true" />
<!-- Map size / start point -->
<param name="map_resolution" value="0.050"/>
<param name="map_size" value="$(arg map_size)"/>
<param name="map_start_x" value="0.5"/>
<param name="map_start_y" value="0.5" />
<param name="map_multi_res_levels" value="3" />
<param name="map_pub_period" value ="2.0" />
<!-- Map update parameters -->
<param name="update_factor_free" value="0.4"/>
<param name="update_factor_occupied" value="0.9" />
<param name="map_update_distance_thresh" value="0.4"/>
<param name="map_update_angle_thresh" value="0.06" />
<param name="laser_min_dist" value="0.4" />
<param name="laser_max_dist" value="30.0" />
<param name="laser_z_min_value" value="-1.0" />
<param name="laser_z_max_value" value="1.0" />
<!-- Advertising config -->
<param name="advertise_map_service" value="true"/>
<param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
<param name="scan_topic" value="$(arg scan_topic)"/>
<param name="pose_update_topic" value="poseupdate" />
<param name="sys_msg_topic" value="syscommand" />
<param name="pub_odometry" value="false" />
<!-- Debug parameters -->
<!--
<param name="output_timing" value="false"/>
<param name="pub_drawings" value="true"/>
<param name="pub_debug_output" value="true"/>
-->
<param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
<remap from="map" to="/map" />
</node>
<!--<node pkg="tf" type="static_transform_publisher" name="map_nav_broadcaster" args="0 0 0 0 0 0 map nav 100"/>-->
</launch>

29
mir_robot/mir_navigation/launch/move_base.xml Executable file
View File

@@ -0,0 +1,29 @@
<launch>
<!--node ns="local_costmap" name="voxel_grid_throttle" pkg="topic_tools" type="throttle" args="messages voxel_grid 3.0 voxel_grid_throttled" /-->
<arg name="local_planner" default="dwb" doc="Local planner can be either dwa, base, teb or pose"/>
<arg name="with_virtual_walls" default="true" doc="Enables usage of virtual walls when set. Set to false when running SLAM." />
<arg name="with_preferred_zones" default="true" doc="Enables usage of preferred zones when set. Set to false when running SLAM." />
<arg name="with_unpreferred_zones" default="true" doc="Enables usage of unpreferred zones when set. Set to false when running SLAM." />
<arg name="with_critical_zones" default="true" doc="Enables usage of critical zones when set. Set to false when running SLAM." />
<arg name="with_direction_zones" default="false" doc="Enables usage of direction zones when set. Set to false when running SLAM." />
<arg name="prefix" default="" doc="Prefix used for robot tf frames" /> <!-- used in the config files -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen" clear_params="true">
<param name="SBPLLatticePlanner/primitive_filename" value="$(find mir_navigation)/mprim/unicycle_highcost_5cm.mprim" />
<rosparam file="$(find mir_navigation)/config/move_base_common_params.yaml" command="load" />
<rosparam file="$(find mir_navigation)/config/sbpl_global_params.yaml" command="load" />
<rosparam file="$(find mir_navigation)/config/$(arg local_planner)_local_planner_params.yaml" command="load" />
<!-- global costmap params -->
<rosparam file="$(find mir_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" subst_value="true" />
<rosparam file="$(find mir_navigation)/config/costmap_global_params.yaml" command="load" />
<rosparam file="$(find mir_navigation)/config/costmap_global_params_plugins.yaml" command="load" />
<!-- local costmap params -->
<rosparam file="$(find mir_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" subst_value="true" />
<rosparam file="$(find mir_navigation)/config/costmap_local_params.yaml" command="load" subst_value="true" />
<rosparam file="$(find mir_navigation)/config/costmap_local_params_plugins.yaml" command="load" />
<remap from="map" to="/map" />
<remap from="marker" to="move_base_node/DWBLocalPlanner/markers" />
</node>
</launch>

44
mir_robot/mir_navigation/launch/start_maps.launch Executable file
View File

@@ -0,0 +1,44 @@
<launch>
<arg name="map_file" default="" doc="Path to a map .yaml file (required)." />
<arg name="empty_file" default="" doc="Path to a map .yaml file (required)." />
<arg name="virtual_walls_map_file" default="$(arg map_file)" doc="Path to a virtual walls map .yaml file (optional)." />
<arg name="with_virtual_walls" default="true" />
<arg name="preferred_zones_map_file" default="$(arg empty_file)" doc="Path to a preferred zones .yaml file (optional)." />
<arg name="with_preferred_zones" default="true" />
<arg name="unpreferred_zones_map_file" default="$(arg empty_file)" doc="Path to a unpreferred zones .yaml file (optional)." />
<arg name="with_unpreferred_zones" default="true" />
<arg name="critical_zones_map_file" default="$(arg empty_file)" doc="Path to a critical zones .yaml file (optional)." />
<arg name="with_critical_zones" default="true" />
<arg name="direction_zones_map_file" default="$(arg empty_file)" doc="Path to a direction zones .yaml file (optional)." />
<arg name="with_direction_zones" default="true" />
<node name="static_map_server" pkg="map_server" type="map_server" args="$(arg map_file)" ns="/" output="screen">
<param name="frame_id" type="string" value="map"/>
<param name="type" type="int" value="8"/>
</node>
<node if="$(arg with_virtual_walls)" name="virtual_walls_map_server" pkg="map_server" type="map_server" args="$(arg virtual_walls_map_file)" ns="/virtual_walls" output="screen">
<param name="frame_id" type="string" value="map"/>
<param name="type" type="int" value="8"/>
</node>
<node if="$(arg with_preferred_zones)" name="preferred_zones_map_server" pkg="map_server" type="map_server" args="$(arg preferred_zones_map_file)" ns="/preferred_zones" output="screen">
<param name="frame_id" type="string" value="map"/>
<param name="type" type="int" value="8"/>
</node>
<node if="$(arg with_unpreferred_zones)" name="unpreferred_zones_map_server" pkg="map_server" type="map_server" args="$(arg unpreferred_zones_map_file)" ns="/unpreferred_zones" output="screen">
<param name="frame_id" type="string" value="map"/>
<param name="type" type="int" value="8"/>
</node>
<node if="$(arg with_critical_zones)" name="critical_zones_map_server" pkg="map_server" type="map_server" args="$(arg critical_zones_map_file)" ns="/critical_zones" output="screen">
<param name="frame_id" type="string" value="map"/>
<param name="type" type="int" value="8"/>
</node>
<node if="$(arg with_direction_zones)" name="direction_zones_map_server" pkg="map_server" type="map_server" args="$(arg direction_zones_map_file)" ns="/direction_zones" output="screen">
<param name="frame_id" type="string" value="map"/>
<param name="type" type="int" value="32"/>
</node>
</launch>

89
mir_robot/mir_navigation/launch/start_planner.launch Executable file
View File

@@ -0,0 +1,89 @@
<launch>
<arg name="local_planner" default="dwb" doc="Local planner can be either dwa, dwb, eband, base, teb or pose" />
<arg name="map_file" default="" doc="Path to a map .yaml file (required)." />
<arg name="empty_file" default="" doc="Path to a map .yaml file (required)." />
<arg name="virtual_walls_map_file" default="$(arg map_file)" doc="Path to a virtual walls map .yaml file (optional)." />
<arg name="with_virtual_walls" default="true" />
<arg name="preferred_zones_map_file" default="$(arg empty_file)" doc="Path to a preferred zones .yaml file (optional)." />
<arg name="with_preferred_zones" default="true" />
<arg name="unpreferred_zones_map_file" default="$(arg empty_file)" doc="Path to a unpreferred zones .yaml file (optional)." />
<arg name="with_unpreferred_zones" default="true" />
<arg name="critical_zones_map_file" default="$(arg empty_file)" doc="Path to a critical zones .yaml file (optional)." />
<arg name="with_critical_zones" default="true" />
<arg name="direction_zones_map_file" default="$(arg empty_file)" doc="Path to a direction zones .yaml file (optional)." />
<arg name="with_direction_zones" default="true" />
<arg name="prefix" default="" />
<arg name="namespace" default="$(arg prefix)" doc="Namespace to push all topics into."/>
<group if="$(eval namespace != '')" ns="$(arg namespace)">
<include file="$(find mir_navigation)/launch/start_maps.launch">
<arg name="map_file" value="$(arg map_file)" />
<arg name="empty_file" value="$(arg empty_file)" />
<arg name="virtual_walls_map_file" value="$(arg virtual_walls_map_file)" />
<arg name="with_virtual_walls" value="$(arg with_virtual_walls)" />
<!-- Đang thử nghiệm -->
<arg name="preferred_zones_map_file" default="$(arg preferred_zones_map_file)" doc="Path to a preferred zones .yaml file (optional)." />
<arg name="with_preferred_zones" default="$(arg with_preferred_zones)" />
<arg name="unpreferred_zones_map_file" default="$(arg unpreferred_zones_map_file)" doc="Path to a unpreferred zones .yaml file (optional)." />
<arg name="with_unpreferred_zones" default="$(arg with_unpreferred_zones)" />
<arg name="critical_zones_map_file" default="$(arg critical_zones_map_file)" doc="Path to a critical zones .yaml file (optional)." />
<arg name="with_critical_zones" default="$(arg with_critical_zones)" />
<arg name="direction_zones_map_file" default="$(arg direction_zones_map_file)" doc="Path to a direction zones .yaml file (optional)." />
<arg name="with_direction_zones" default="$(arg with_direction_zones)" />
</include>
<include file="$(find mir_navigation)/launch/move_base.xml">
<arg name="local_planner" value="$(arg local_planner)"/>
<arg name="with_virtual_walls" value="$(arg with_virtual_walls)" />
<!-- Đang thử nghiệm -->
<arg name="with_preferred_zones" default="$(arg with_preferred_zones)" />
<arg name="with_unpreferred_zones" default="$(arg with_unpreferred_zones)" />
<arg name="with_critical_zones" default="$(arg with_critical_zones)" />
<arg name="with_direction_zones" default="$(arg with_direction_zones)" />
<arg name="prefix" value="$(arg prefix)" />
</include>
</group>
<!-- Duplicate of the above in case namespace is empty. This is necessary to
avoid the "<group> tag has an empty 'ns' attribute" parsing error. -->
<group unless="$(eval namespace != '')">
<include file="$(find mir_navigation)/launch/start_maps.launch">
<arg name="map_file" value="$(arg map_file)" />
<arg name="empty_file" value="$(arg empty_file)" />
<arg name="virtual_walls_map_file" value="$(arg virtual_walls_map_file)" />
<arg name="with_virtual_walls" value="$(arg with_virtual_walls)" />
<!-- Đang thử nghiệm -->
<arg name="preferred_zones_map_file" default="$(arg preferred_zones_map_file)" doc="Path to a preferred zones .yaml file (optional)." />
<arg name="with_preferred_zones" default="$(arg with_preferred_zones)" />
<arg name="unpreferred_zones_map_file" default="$(arg unpreferred_zones_map_file)" doc="Path to a unpreferred zones .yaml file (optional)." />
<arg name="with_unpreferred_zones" default="$(arg with_unpreferred_zones)" />
<arg name="critical_zones_map_file" default="$(arg critical_zones_map_file)" doc="Path to a critical zones .yaml file (optional)." />
<arg name="with_critical_zones" default="$(arg with_critical_zones)" />
<arg name="direction_zones_map_file" default="$(arg direction_zones_map_file)" doc="Path to a direction zones .yaml file (optional)." />
<arg name="with_direction_zones" default="$(arg with_direction_zones)" />
</include>
<include file="$(find mir_navigation)/launch/move_base.xml">
<arg name="local_planner" value="$(arg local_planner)"/>
<arg name="with_virtual_walls" value="$(arg with_virtual_walls)" />
<!-- Đang thử nghiệm -->
<arg name="with_preferred_zones" default="$(arg with_preferred_zones)" />
<arg name="with_unpreferred_zones" default="$(arg with_unpreferred_zones)" />
<arg name="with_critical_zones" default="$(arg with_critical_zones)" />
<arg name="with_direction_zones" default="$(arg with_direction_zones)" />
<arg name="prefix" value="$(arg prefix)" />
</include>
</group>
</launch>

280
mir_robot/mir_navigation/mprim/genmprim_unicycle_highcost_5cm.m Executable file
View File

@@ -0,0 +1,280 @@
% /*
% * Copyright (c) 2008, Maxim Likhachev
% * All rights reserved.
% *
% * Redistribution and use in source and binary forms, with or without
% * modification, are permitted provided that the following conditions are met:
% *
% * * Redistributions of source code must retain the above copyright
% * notice, this list of conditions and the following disclaimer.
% * * Redistributions in binary form must reproduce the above copyright
% * notice, this list of conditions and the following disclaimer in the
% * documentation and/or other materials provided with the distribution.
% * * Neither the name of the Carnegie Mellon University nor the names of its
% * contributors may be used to endorse or promote products derived from
% * this software without specific prior written permission.
% *
% * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% * POSSIBILITY OF SUCH DAMAGE.
% */
function[] = genmprim_unicycle_highcost_5cm(outfilename)
%
%generates motion primitives and saves them into file
%
%written by Maxim Likhachev
%---------------------------------------------------
%
%defines
UNICYCLE_MPRIM_16DEGS = 1;
if UNICYCLE_MPRIM_16DEGS == 1
resolution = 0.05;
numberofangles = 16; %preferably a power of 2, definitely multiple of 8
numberofprimsperangle = 7;
%multipliers (multiplier is used as costmult*cost)
forwardcostmult = 1;
backwardcostmult = 40;
forwardandturncostmult = 2;
sidestepcostmult = 10;
turninplacecostmult = 20;
%note, what is shown x,y,theta changes (not absolute numbers)
%0 degreees
basemprimendpts0_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult
%x aligned with the heading of the robot, angles are positive
%counterclockwise
%0 theta change
basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult];
basemprimendpts0_c(2,:) = [8 0 0 forwardcostmult];
basemprimendpts0_c(3,:) = [-1 0 0 backwardcostmult];
%1/16 theta change
basemprimendpts0_c(4,:) = [8 1 1 forwardandturncostmult];
basemprimendpts0_c(5,:) = [8 -1 -1 forwardandturncostmult];
%turn in place
basemprimendpts0_c(6,:) = [0 0 1 turninplacecostmult];
basemprimendpts0_c(7,:) = [0 0 -1 turninplacecostmult];
%45 degrees
basemprimendpts45_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
%x aligned with the heading of the robot, angles are positive
%counterclockwise
%0 theta change
basemprimendpts45_c(1,:) = [1 1 0 forwardcostmult];
basemprimendpts45_c(2,:) = [6 6 0 forwardcostmult];
basemprimendpts45_c(3,:) = [-1 -1 0 backwardcostmult];
%1/16 theta change
basemprimendpts45_c(4,:) = [5 7 1 forwardandturncostmult];
basemprimendpts45_c(5,:) = [7 5 -1 forwardandturncostmult];
%turn in place
basemprimendpts45_c(6,:) = [0 0 1 turninplacecostmult];
basemprimendpts45_c(7,:) = [0 0 -1 turninplacecostmult];
%22.5 degrees
basemprimendpts22p5_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
%x aligned with the heading of the robot, angles are positive
%counterclockwise
%0 theta change
basemprimendpts22p5_c(1,:) = [2 1 0 forwardcostmult];
basemprimendpts22p5_c(2,:) = [6 3 0 forwardcostmult];
basemprimendpts22p5_c(3,:) = [-2 -1 0 backwardcostmult];
%1/16 theta change
basemprimendpts22p5_c(4,:) = [5 4 1 forwardandturncostmult];
basemprimendpts22p5_c(5,:) = [7 2 -1 forwardandturncostmult];
%turn in place
basemprimendpts22p5_c(6,:) = [0 0 1 turninplacecostmult];
basemprimendpts22p5_c(7,:) = [0 0 -1 turninplacecostmult];
else
fprintf(1, 'ERROR: undefined mprims type\n');
return;
end;
fout = fopen(outfilename, 'w');
%write the header
fprintf(fout, 'resolution_m: %f\n', resolution);
fprintf(fout, 'numberofangles: %d\n', numberofangles);
fprintf(fout, 'totalnumberofprimitives: %d\n', numberofprimsperangle*numberofangles);
%iterate over angles
for angleind = 1:numberofangles
figure(1);
hold off;
text(0, 0, int2str(angleind));
%iterate over primitives
for primind = 1:numberofprimsperangle
fprintf(fout, 'primID: %d\n', primind-1);
fprintf(fout, 'startangle_c: %d\n', angleind-1);
%current angle
currentangle = (angleind-1)*2*pi/numberofangles;
currentangle_36000int = round((angleind-1)*36000/numberofangles);
%compute which template to use
if (rem(currentangle_36000int, 9000) == 0)
basemprimendpts_c = basemprimendpts0_c(primind,:);
angle = currentangle;
elseif (rem(currentangle_36000int, 4500) == 0)
basemprimendpts_c = basemprimendpts45_c(primind,:);
angle = currentangle - 45*pi/180;
elseif (rem(currentangle_36000int-7875, 9000) == 0)
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
basemprimendpts_c(1) = basemprimendpts33p75_c(primind, 2); %reverse x and y
basemprimendpts_c(2) = basemprimendpts33p75_c(primind, 1);
basemprimendpts_c(3) = -basemprimendpts33p75_c(primind, 3); %reverse the angle as well
angle = currentangle - 78.75*pi/180;
fprintf(1, '78p75\n');
elseif (rem(currentangle_36000int-6750, 9000) == 0)
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
basemprimendpts_c(1) = basemprimendpts22p5_c(primind, 2); %reverse x and y
basemprimendpts_c(2) = basemprimendpts22p5_c(primind, 1);
basemprimendpts_c(3) = -basemprimendpts22p5_c(primind, 3); %reverse the angle as well
%fprintf(1, '%d %d %d onto %d %d %d\n', basemprimendpts22p5_c(1), basemprimendpts22p5_c(2), basemprimendpts22p5_c(3), ...
% basemprimendpts_c(1), basemprimendpts_c(2), basemprimendpts_c(3));
angle = currentangle - 67.5*pi/180;
fprintf(1, '67p5\n');
elseif (rem(currentangle_36000int-5625, 9000) == 0)
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
basemprimendpts_c(1) = basemprimendpts11p25_c(primind, 2); %reverse x and y
basemprimendpts_c(2) = basemprimendpts11p25_c(primind, 1);
basemprimendpts_c(3) = -basemprimendpts11p25_c(primind, 3); %reverse the angle as well
angle = currentangle - 56.25*pi/180;
fprintf(1, '56p25\n');
elseif (rem(currentangle_36000int-3375, 9000) == 0)
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
angle = currentangle - 33.75*pi/180;
fprintf(1, '33p75\n');
elseif (rem(currentangle_36000int-2250, 9000) == 0)
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
angle = currentangle - 22.5*pi/180;
fprintf(1, '22p5\n');
elseif (rem(currentangle_36000int-1125, 9000) == 0)
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
angle = currentangle - 11.25*pi/180;
fprintf(1, '11p25\n');
else
fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int);
return;
end;
%now figure out what action will be
baseendpose_c = basemprimendpts_c(1:3);
additionalactioncostmult = basemprimendpts_c(4);
endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle));
endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle));
endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles);
endpose_c = [endx_c endy_c endtheta_c];
fprintf(1, 'rotation angle=%f\n', angle*180/pi);
if baseendpose_c(2) == 0 & baseendpose_c(3) == 0
%fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
end;
%generate intermediate poses (remember they are w.r.t 0,0 (and not
%centers of the cells)
numofsamples = 10;
intermcells_m = zeros(numofsamples,3);
if UNICYCLE_MPRIM_16DEGS == 1
startpt = [0 0 currentangle];
endpt = [endpose_c(1)*resolution endpose_c(2)*resolution ...
rem(angleind - 1 + baseendpose_c(3), numberofangles)*2*pi/numberofangles];
intermcells_m = zeros(numofsamples,3);
if ((endx_c == 0 & endy_c == 0) | baseendpose_c(3) == 0) %turn in place or move forward
for iind = 1:numofsamples
intermcells_m(iind,:) = [startpt(1) + (endpt(1) - startpt(1))*(iind-1)/(numofsamples-1) ...
startpt(2) + (endpt(2) - startpt(2))*(iind-1)/(numofsamples-1) ...
0];
rotation_angle = (baseendpose_c(3) ) * (2*pi/numberofangles);
intermcells_m(iind,3) = rem(startpt(3) + (rotation_angle)*(iind-1)/(numofsamples-1), 2*pi);
end;
else %unicycle-based move forward or backward
R = [cos(startpt(3)) sin(endpt(3)) - sin(startpt(3));
sin(startpt(3)) -(cos(endpt(3)) - cos(startpt(3)))];
S = pinv(R)*[endpt(1) - startpt(1); endpt(2) - startpt(2)];
l = S(1);
tvoverrv = S(2);
rv = (baseendpose_c(3)*2*pi/numberofangles + l/tvoverrv);
tv = tvoverrv*rv;
if l < 0
fprintf(1, 'WARNING: l = %d < 0 -> bad action start/end points\n', l);
l = 0;
end;
%compute rv
%rv = baseendpose_c(3)*2*pi/numberofangles;
%compute tv
%tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
%tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
%tv = (tvx + tvy)/2.0;
%generate samples
for iind = 1:numofsamples
dt = (iind-1)/(numofsamples-1);
%dtheta = rv*dt + startpt(3);
%intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
% startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
% dtheta];
if(dt*tv < l)
intermcells_m(iind,:) = [startpt(1) + dt*tv*cos(startpt(3)) ...
startpt(2) + dt*tv*sin(startpt(3)) ...
startpt(3)];
else
dtheta = rv*(dt - l/tv) + startpt(3);
intermcells_m(iind,:) = [startpt(1) + l*cos(startpt(3)) + tvoverrv*(sin(dtheta) - sin(startpt(3))) ...
startpt(2) + l*sin(startpt(3)) - tvoverrv*(cos(dtheta) - cos(startpt(3))) ...
dtheta];
end;
end;
%correct
errorxy = [endpt(1) - intermcells_m(numofsamples,1) ...
endpt(2) - intermcells_m(numofsamples,2)];
fprintf(1, 'l=%f errx=%f erry=%f\n', l, errorxy(1), errorxy(2));
interpfactor = [0:1/(numofsamples-1):1];
intermcells_m(:,1) = intermcells_m(:,1) + errorxy(1)*interpfactor';
intermcells_m(:,2) = intermcells_m(:,2) + errorxy(2)*interpfactor';
end;
end;
%write out
fprintf(fout, 'endpose_c: %d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
fprintf(fout, 'additionalactioncostmult: %d\n', additionalactioncostmult);
fprintf(fout, 'intermediateposes: %d\n', size(intermcells_m,1));
for interind = 1:size(intermcells_m, 1)
fprintf(fout, '%.4f %.4f %.4f\n', intermcells_m(interind,1), intermcells_m(interind,2), intermcells_m(interind,3));
end;
plot(intermcells_m(:,1), intermcells_m(:,2));
axis([-0.3 0.3 -0.3 0.3]);
text(intermcells_m(numofsamples,1), intermcells_m(numofsamples,2), int2str(endpose_c(3)));
hold on;
end;
grid;
pause;
end;
fclose('all');

416
mir_robot/mir_navigation/mprim/genmprim_unicycle_highcost_5cm.py Executable file
View File

@@ -0,0 +1,416 @@
#!/usr/bin/env python3
#
# Copyright (c) 2016, David Conner (Christopher Newport University)
# Based on genmprim_unicycle.m
# Copyright (c) 2008, Maxim Likhachev
# All rights reserved.
# converted by libermate utility (https://github.com/awesomebytes/libermate)
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of the Carnegie Mellon University nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
import numpy as np
import rospkg
# if available import pylab (from matlibplot)
matplotlib_found = False
try:
import matplotlib.pylab as plt
matplotlib_found = True
except ImportError:
pass
def matrix_size(mat, elem=None):
if not elem:
return mat.shape
else:
return mat.shape[int(elem) - 1]
def genmprim_unicycle(outfilename, visualize=False, separate_plots=False):
visualize = matplotlib_found and visualize # Plot the primitives
# Local Variables: basemprimendpts22p5_c, endtheta_c, endx_c,
# baseendpose_c, additionalactioncostmult, fout, numofsamples,
# basemprimendpts45_c, primind, basemprimendpts0_c, rv, angle, outfilename,
# numberofangles, startpt, UNICYCLE_MPRIM_16DEGS, sidestepcostmult,
# rotation_angle, basemprimendpts_c, forwardandturncostmult,
# forwardcostmult, turninplacecostmult, endpose_c, backwardcostmult,
# interpfactor, S, R, tvoverrv, dtheta, intermcells_m, tv, dt,
# currentangle, numberofprimsperangle, resolution, currentangle_36000int,
# l, iind, errorxy, interind, endy_c, angleind, endpt
# Function calls: plot, cos, pi, grid, figure, genmprim_unicycle, text,
# int2str, pause, axis, sin, pinv, fprintf, fclose, rem, zeros, fopen,
# round, size
# %
# %generates motion primitives and saves them into file
# %
# %written by Maxim Likhachev
# %---------------------------------------------------
# %
# %defines
UNICYCLE_MPRIM_16DEGS = 1.0
if UNICYCLE_MPRIM_16DEGS == 1.0:
resolution = 0.05
numberofangles = 16
# %preferably a power of 2, definitely multiple of 8
numberofprimsperangle = 7
# %multipliers (multiplier is used as costmult*cost)
forwardcostmult = 1.0
backwardcostmult = 40.0
forwardandturncostmult = 2.0
# sidestepcostmult = 10.0
turninplacecostmult = 20.0
# %note, what is shown x,y,theta changes (not absolute numbers)
# %0 degreees
basemprimendpts0_c = np.zeros((numberofprimsperangle, 4))
# %x,y,theta,costmult
# %x aligned with the heading of the robot, angles are positive
# %counterclockwise
# %0 theta change
basemprimendpts0_c[0, :] = np.array(np.hstack((1.0, 0.0, 0.0, forwardcostmult)))
basemprimendpts0_c[1, :] = np.array(np.hstack((8.0, 0.0, 0.0, forwardcostmult)))
basemprimendpts0_c[2, :] = np.array(np.hstack((-1.0, 0.0, 0.0, backwardcostmult)))
# %1/16 theta change
basemprimendpts0_c[3, :] = np.array(np.hstack((8.0, 1.0, 1.0, forwardandturncostmult)))
basemprimendpts0_c[4, :] = np.array(np.hstack((8.0, -1.0, -1.0, forwardandturncostmult)))
# %turn in place
basemprimendpts0_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
basemprimendpts0_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
# %45 degrees
basemprimendpts45_c = np.zeros((numberofprimsperangle, 4))
# %x,y,theta,costmult (multiplier is used as costmult*cost)
# %x aligned with the heading of the robot, angles are positive
# %counterclockwise
# %0 theta change
basemprimendpts45_c[0, :] = np.array(np.hstack((1.0, 1.0, 0.0, forwardcostmult)))
basemprimendpts45_c[1, :] = np.array(np.hstack((6.0, 6.0, 0.0, forwardcostmult)))
basemprimendpts45_c[2, :] = np.array(np.hstack((-1.0, -1.0, 0.0, backwardcostmult)))
# %1/16 theta change
basemprimendpts45_c[3, :] = np.array(np.hstack((5.0, 7.0, 1.0, forwardandturncostmult)))
basemprimendpts45_c[4, :] = np.array(np.hstack((7.0, 5.0, -1.0, forwardandturncostmult)))
# %turn in place
basemprimendpts45_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
basemprimendpts45_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
# %22.5 degrees
basemprimendpts22p5_c = np.zeros((numberofprimsperangle, 4))
# %x,y,theta,costmult (multiplier is used as costmult*cost)
# %x aligned with the heading of the robot, angles are positive
# %counterclockwise
# %0 theta change
basemprimendpts22p5_c[0, :] = np.array(np.hstack((2.0, 1.0, 0.0, forwardcostmult)))
basemprimendpts22p5_c[1, :] = np.array(np.hstack((6.0, 3.0, 0.0, forwardcostmult)))
basemprimendpts22p5_c[2, :] = np.array(np.hstack((-2.0, -1.0, 0.0, backwardcostmult)))
# %1/16 theta change
basemprimendpts22p5_c[3, :] = np.array(np.hstack((5.0, 4.0, 1.0, forwardandturncostmult)))
basemprimendpts22p5_c[4, :] = np.array(np.hstack((7.0, 2.0, -1.0, forwardandturncostmult)))
# %turn in place
basemprimendpts22p5_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
basemprimendpts22p5_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
else:
print('ERROR: undefined mprims type\n')
return []
fout = open(outfilename, 'w')
# %write the header
fout.write('resolution_m: %f\n' % (resolution))
fout.write('numberofangles: %d\n' % (numberofangles))
fout.write('totalnumberofprimitives: %d\n' % (numberofprimsperangle * numberofangles))
# %iterate over angles
for angleind in np.arange(1.0, (numberofangles) + 1):
currentangle = ((angleind - 1) * 2.0 * np.pi) / numberofangles
currentangle_36000int = np.round((angleind - 1) * 36000.0 / numberofangles)
if visualize:
if separate_plots:
fig = plt.figure(angleind)
plt.title('angle {:2.0f} (= {:3.1f} degrees)'.format(angleind - 1, currentangle_36000int / 100.0))
else:
fig = plt.figure(1)
plt.axis('equal')
plt.axis([-10 * resolution, 10 * resolution, -10 * resolution, 10 * resolution])
ax = fig.add_subplot(1, 1, 1)
major_ticks = np.arange(-8 * resolution, 9 * resolution, 4 * resolution)
minor_ticks = np.arange(-8 * resolution, 9 * resolution, resolution)
ax.set_xticks(major_ticks)
ax.set_xticks(minor_ticks, minor=True)
ax.set_yticks(major_ticks)
ax.set_yticks(minor_ticks, minor=True)
ax.grid(which='minor', alpha=0.5)
ax.grid(which='major', alpha=0.9)
# %iterate over primitives
for primind in np.arange(1.0, (numberofprimsperangle) + 1):
fout.write('primID: %d\n' % (primind - 1))
fout.write('startangle_c: %d\n' % (angleind - 1))
# %current angle
# %compute which template to use
if (currentangle_36000int % 9000) == 0:
basemprimendpts_c = basemprimendpts0_c[int(primind) - 1, :]
angle = currentangle
elif (currentangle_36000int % 4500) == 0:
basemprimendpts_c = basemprimendpts45_c[int(primind) - 1, :]
angle = currentangle - 45.0 * np.pi / 180.0
# commented out because basemprimendpts33p75_c is undefined
# elif ((currentangle_36000int - 7875) % 9000) == 0:
# basemprimendpts_c = (
# 1 * basemprimendpts33p75_c[primind, :]
# ) # 1* to force deep copy to avoid reference update below
# basemprimendpts_c[0] = basemprimendpts33p75_c[primind, 1]
# # %reverse x and y
# basemprimendpts_c[1] = basemprimendpts33p75_c[primind, 0]
# basemprimendpts_c[2] = -basemprimendpts33p75_c[primind, 2]
# # %reverse the angle as well
# angle = currentangle - (78.75 * np.pi) / 180.0
# print('78p75\n')
elif ((currentangle_36000int - 6750) % 9000) == 0:
basemprimendpts_c = (
1 * basemprimendpts22p5_c[int(primind) - 1, :]
) # 1* to force deep copy to avoid reference update below
basemprimendpts_c[0] = basemprimendpts22p5_c[int(primind) - 1, 1]
# %reverse x and y
basemprimendpts_c[1] = basemprimendpts22p5_c[int(primind) - 1, 0]
basemprimendpts_c[2] = -basemprimendpts22p5_c[int(primind) - 1, 2]
# %reverse the angle as well
# print(
# '%d : %d %d %d onto %d %d %d\n'
# % (
# primind - 1,
# basemprimendpts22p5_c[int(primind) - 1, 0],
# basemprimendpts22p5_c[int(primind) - 1, 1],
# basemprimendpts22p5_c[int(primind) - 1, 2],
# basemprimendpts_c[0],
# basemprimendpts_c[1],
# basemprimendpts_c[2],
# )
# )
angle = currentangle - (67.5 * np.pi) / 180.0
print('67p5\n')
# commented out because basemprimendpts11p25_c is undefined
# elif ((currentangle_36000int - 5625) % 9000) == 0:
# basemprimendpts_c = (
# 1 * basemprimendpts11p25_c[primind, :]
# ) # 1* to force deep copy to avoid reference update below
# basemprimendpts_c[0] = basemprimendpts11p25_c[primind, 1]
# # %reverse x and y
# basemprimendpts_c[1] = basemprimendpts11p25_c[primind, 0]
# basemprimendpts_c[2] = -basemprimendpts11p25_c[primind, 2]
# # %reverse the angle as well
# angle = currentangle - (56.25 * np.pi) / 180.0
# print('56p25\n')
# commented out because basemprimendpts33p75_c is undefined
# elif ((currentangle_36000int - 3375) % 9000) == 0:
# basemprimendpts_c = basemprimendpts33p75_c[int(primind), :]
# angle = currentangle - (33.75 * np.pi) / 180.0
# print('33p75\n')
elif ((currentangle_36000int - 2250) % 9000) == 0:
basemprimendpts_c = basemprimendpts22p5_c[int(primind) - 1, :]
angle = currentangle - (22.5 * np.pi) / 180.0
print('22p5\n')
# commented out because basemprimendpts11p25_c is undefined
# elif ((currentangle_36000int - 1125) % 9000) == 0:
# basemprimendpts_c = basemprimendpts11p25_c[int(primind), :]
# angle = currentangle - (11.25 * np.pi) / 180.0
# print('11p25\n')
else:
print('ERROR: invalid angular resolution. angle = %d\n' % currentangle_36000int)
return []
# %now figure out what action will be
baseendpose_c = basemprimendpts_c[0:3]
additionalactioncostmult = basemprimendpts_c[3]
endx_c = np.round((baseendpose_c[0] * np.cos(angle)) - (baseendpose_c[1] * np.sin(angle)))
endy_c = np.round((baseendpose_c[0] * np.sin(angle)) + (baseendpose_c[1] * np.cos(angle)))
endtheta_c = np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)
endpose_c = np.array(np.hstack((endx_c, endy_c, endtheta_c)))
print("endpose_c=", endpose_c)
print(('rotation angle=%f\n' % (angle * 180.0 / np.pi)))
# if np.logical_and(baseendpose_c[1] == 0., baseendpose_c[2] == 0.):
# %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
# %generate intermediate poses (remember they are w.r.t 0,0 (and not
# %centers of the cells)
numofsamples = 10
intermcells_m = np.zeros((numofsamples, 3))
if UNICYCLE_MPRIM_16DEGS == 1.0:
startpt = np.array(np.hstack((0.0, 0.0, currentangle)))
endpt = np.array(
np.hstack(
(
(endpose_c[0] * resolution),
(endpose_c[1] * resolution),
(
((np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)) * 2.0 * np.pi)
/ numberofangles
),
)
)
)
print("startpt =", startpt)
print("endpt =", endpt)
intermcells_m = np.zeros((numofsamples, 3))
if np.logical_or(np.logical_and(endx_c == 0.0, endy_c == 0.0), baseendpose_c[2] == 0.0):
# %turn in place or move forward
for iind in np.arange(1.0, (numofsamples) + 1):
fraction = float(iind - 1) / (numofsamples - 1)
intermcells_m[int(iind) - 1, :] = np.array(
(
startpt[0] + (endpt[0] - startpt[0]) * fraction,
startpt[1] + (endpt[1] - startpt[1]) * fraction,
0,
)
)
rotation_angle = baseendpose_c[2] * (2.0 * np.pi / numberofangles)
intermcells_m[int(iind) - 1, 2] = np.fmod(startpt[2] + rotation_angle * fraction, (2.0 * np.pi))
# print " ",iind," of ",numofsamples," fraction=",fraction," rotation=",rotation_angle
else:
# %unicycle-based move forward or backward (http://sbpl.net/node/53)
R = np.array(
np.vstack(
(
np.hstack((np.cos(startpt[2]), np.sin(endpt[2]) - np.sin(startpt[2]))),
np.hstack((np.sin(startpt[2]), -np.cos(endpt[2]) + np.cos(startpt[2]))),
)
)
)
S = np.dot(np.linalg.pinv(R), np.array(np.vstack((endpt[0] - startpt[0], endpt[1] - startpt[1]))))
l = S[0]
tvoverrv = S[1]
rv = (baseendpose_c[2] * 2.0 * np.pi / numberofangles) + l / tvoverrv
tv = tvoverrv * rv
# print "R=\n",R
# print "Rpi=\n",np.linalg.pinv(R)
# print "S=\n",S
# print "l=",l
# print "tvoverrv=",tvoverrv
# print "rv=",rv
# print "tv=",tv
if l < 0.0:
print(('WARNING: l = %f < 0 -> bad action start/end points\n' % (l)))
l = 0.0
# %compute rv
# %rv = baseendpose_c(3)*2*pi/numberofangles;
# %compute tv
# %tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
# %tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
# %tv = (tvx + tvy)/2.0;
# %generate samples
for iind in np.arange(1, numofsamples + 1):
dt = (iind - 1) / (numofsamples - 1)
# %dtheta = rv*dt + startpt(3);
# %intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
# % startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
# % dtheta];
if (dt * tv) < l:
intermcells_m[int(iind) - 1, :] = np.array(
np.hstack(
(
startpt[0] + dt * tv * np.cos(startpt[2]),
startpt[1] + dt * tv * np.sin(startpt[2]),
startpt[2],
)
)
)
else:
dtheta = rv * (dt - l / tv) + startpt[2]
intermcells_m[int(iind) - 1, :] = np.array(
np.hstack(
(
startpt[0]
+ l * np.cos(startpt[2])
+ tvoverrv * (np.sin(dtheta) - np.sin(startpt[2])),
startpt[1]
+ l * np.sin(startpt[2])
- tvoverrv * (np.cos(dtheta) - np.cos(startpt[2])),
dtheta,
)
)
)
# %correct
errorxy = np.array(
np.hstack(
(
endpt[0] - intermcells_m[int(numofsamples) - 1, 0],
endpt[1] - intermcells_m[int(numofsamples) - 1, 1],
)
)
)
# print('l=%f errx=%f erry=%f\n'%(l, errorxy[0], errorxy[1]))
interpfactor = np.array(
np.hstack((np.arange(0.0, 1.0 + (1.0 / (numofsamples)), 1.0 / (numofsamples - 1))))
)
# print "intermcells_m=",intermcells_m
# print "interp'=",interpfactor.conj().T
intermcells_m[:, 0] = intermcells_m[:, 0] + errorxy[0] * interpfactor.conj().T
intermcells_m[:, 1] = intermcells_m[:, 1] + errorxy[1] * interpfactor.conj().T
# %write out
fout.write('endpose_c: %d %d %d\n' % (endpose_c[0], endpose_c[1], endpose_c[2]))
fout.write('additionalactioncostmult: %d\n' % (additionalactioncostmult))
fout.write('intermediateposes: %d\n' % (matrix_size(intermcells_m, 1.0)))
for interind in np.arange(1.0, (matrix_size(intermcells_m, 1.0)) + 1):
fout.write(
'%.4f %.4f %.4f\n'
% (
intermcells_m[int(interind) - 1, 0],
intermcells_m[int(interind) - 1, 1],
intermcells_m[int(interind) - 1, 2],
)
)
if visualize:
plt.plot(intermcells_m[:, 0], intermcells_m[:, 1], linestyle="-", marker="o")
plt.text(endpt[0], endpt[1], '{:2.0f}'.format(endpose_c[2]))
# if (visualize):
# plt.waitforbuttonpress() # uncomment to plot each primitive set one at a time
fout.close()
if visualize:
# plt.waitforbuttonpress() # hold until buttom pressed
plt.show() # Keep windows open until the program is terminated
return []
if __name__ == "__main__":
rospack = rospkg.RosPack()
outfilename = rospack.get_path('mir_navigation') + '/mprim/unicycle_5cm.mprim'
genmprim_unicycle(outfilename, visualize=True, separate_plots= False)

1203
mir_robot/mir_navigation/mprim/unicycle_5cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
mir_robot/mir_navigation/mprim/unicycle_5cm_expensive_turn_in_place.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
mir_robot/mir_navigation/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

2403
mir_robot/mir_navigation/mprim/unicycle_5cm_noreverse_trolley.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
mir_robot/mir_navigation/mprim/unicycle_highcost_10cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
mir_robot/mir_navigation/mprim/unicycle_highcost_1cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
mir_robot/mir_navigation/mprim/unicycle_highcost_2_5cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
mir_robot/mir_navigation/mprim/unicycle_highcost_2cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
mir_robot/mir_navigation/mprim/unicycle_highcost_5cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

104
mir_robot/mir_navigation/nodes/acc_finder.py Executable file
View File

@@ -0,0 +1,104 @@
#!/usr/bin/env python3
# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
#
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
#
# * Neither the name of the the copyright holder nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Martin Günther
import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
LIN_MAX = 1.0
ANG_MAX = 1.5 # adjust this value to the rough maximum angular velocity
state = 'stopped'
start = rospy.Time(0)
def odom_cb(msg):
global state
twist = msg.twist.twist
t = (rospy.Time.now() - start).to_sec()
if state == 'wait_for_stop':
if -0.05 < twist.linear.x < 0.05 and -0.1 < twist.angular.z < 0.1:
state = 'stopped'
rospy.loginfo('state transition --> %s', state)
return
if state == 'backward' and twist.linear.x < -0.9 * LIN_MAX:
rospy.loginfo('backward from 0 to %f m/s in %f sec', twist.linear.x, t)
elif state == 'forward' and twist.linear.x > 0.9 * LIN_MAX:
rospy.loginfo('forward from 0 to %f m/s in %f sec', twist.linear.x, t)
elif state == 'turning_clockwise' and twist.angular.z < -0.9 * ANG_MAX:
rospy.loginfo('turning_clockwise from 0 to %f rad/s in %f sec', twist.angular.z, t)
elif state == 'turning_counter_clockwise' and twist.angular.z > 0.9 * ANG_MAX:
rospy.loginfo('turning_counter_clockwise from 0 to %f rad/s in %f sec', twist.angular.z, t)
else:
return
state = 'wait_for_stop'
rospy.loginfo('state transition --> %s', state)
def cmd_vel_cb(msg):
global state, start
if state != 'stopped':
return
if msg.linear.x <= -LIN_MAX:
start = rospy.Time.now()
state = 'backward'
elif msg.linear.x >= LIN_MAX:
start = rospy.Time.now()
state = 'forward'
elif msg.angular.z <= -ANG_MAX:
start = rospy.Time.now()
state = 'turning_clockwise'
elif msg.angular.z >= ANG_MAX:
start = rospy.Time.now()
state = 'turning_counter_clockwise'
else:
return
rospy.loginfo('state transition --> %s', state)
def main():
rospy.init_node('acc_finder', anonymous=True)
rospy.Subscriber('odom', Odometry, odom_cb)
rospy.Subscriber('cmd_vel', Twist, cmd_vel_cb)
rospy.loginfo('acc_finder node ready and listening. now use teleop to move your robot to the limits!')
rospy.spin()
if __name__ == '__main__':
main()

64
mir_robot/mir_navigation/nodes/min_max_finder.py Executable file
View File

@@ -0,0 +1,64 @@
#!/usr/bin/env python3
# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
#
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
#
# * Neither the name of the the copyright holder nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Martin Günther
import rospy
from nav_msgs.msg import Odometry
lin_min = 0.0
lin_max = 0.0
ang_min = 0.0
ang_max = 0.0
def odom_cb(msg):
global lin_min, lin_max, ang_min, ang_max
if lin_min > msg.twist.twist.linear.x:
lin_min = msg.twist.twist.linear.x
if lin_max < msg.twist.twist.linear.x:
lin_max = msg.twist.twist.linear.x
if ang_min > msg.twist.twist.angular.z:
ang_min = msg.twist.twist.angular.z
if ang_max < msg.twist.twist.angular.z:
ang_max = msg.twist.twist.angular.z
rospy.loginfo('linear: [%f, %f] angular: [%f, %f]', lin_min, lin_max, ang_min, ang_max)
def main():
rospy.init_node('min_max_finder', anonymous=True)
rospy.Subscriber('odom', Odometry, odom_cb)
rospy.loginfo('min_max_finde node ready and listening. now use teleop to move your robot to the limits!')
rospy.spin()
if __name__ == '__main__':
main()

61
mir_robot/mir_navigation/package.xml Executable file
View File

@@ -0,0 +1,61 @@
<?xml version="1.0"?>
<package format="2">
<name>mir_navigation</name>
<version>1.1.7</version>
<description>Launch and configuration files for move_base, localization etc. on the MiR robot.</description>
<maintainer email="martin.guenther@dfki.de">Martin Günther</maintainer>
<author email="martin.guenther@dfki.de">Martin Günther</author>
<license>BSD</license>
<url type="website">https://github.com/DFKI-NI/mir_robot</url>
<url type="repository">https://github.com/DFKI-NI/mir_robot</url>
<url type="bugtracker">https://github.com/DFKI-NI/mir_robot/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roslaunch</build_depend>
<build_depend>amcl</build_depend>
<exec_depend>amcl</exec_depend>
<build_depend>base_local_planner</build_depend>
<exec_depend>base_local_planner</exec_depend>
<exec_depend>dwa_local_planner</exec_depend>
<build_depend>dwb_critics</build_depend>
<exec_depend>dwb_critics</exec_depend> <!-- for dwb_local_planner -->
<build_depend>dwb_local_planner</build_depend> <!-- for dwb_local_planner -->
<exec_depend>dwb_local_planner</exec_depend>
<build_depend>dwb_plugins</build_depend>
<exec_depend>dwb_plugins</exec_depend> <!-- for dwb_local_planner -->
<!-- <exec_depend>eband_local_planner</exec_depend> -->
<exec_depend>hector_mapping</exec_depend>
<build_depend>map_server</build_depend>
<exec_depend>map_server</exec_depend>
<build_depend>mir_driver</build_depend>
<exec_depend>mir_driver</exec_depend>
<exec_depend>mir_dwb_critics</exec_depend> <!-- for dwb_local_planner -->
<exec_depend>mir_gazebo</exec_depend>
<build_depend>move_base</build_depend>
<exec_depend>move_base</exec_depend>
<build_depend>nav_core_adapter</build_depend>
<exec_depend>nav_core_adapter</exec_depend> <!-- for dwb_local_planner -->
<exec_depend>python3-matplotlib</exec_depend>
<build_depend>sbpl_lattice_planner</build_depend>
<exec_depend>sbpl_lattice_planner</exec_depend>
<build_depend>teb_local_planner</build_depend>
<exec_depend>teb_local_planner</exec_depend>
</package>

784
mir_robot/mir_navigation/rviz/navigation.rviz Executable file
View File

@@ -0,0 +1,784 @@
Panels:
- Class: rviz/Displays
Help Height: 81
Name: Displays
Property Tree Widget:
Expanded:
- /Grid1/Offset1
- /TF1/Frames1
- /TF1/Tree1
- /Global Map1
- /Global Map1/Costmap1
- /Global Map1/Full Plan1
- /Local Map1
- /Local Map1/DWB Markers1/Namespaces1
Splitter Ratio: 0.42881646752357483
Tree Height: 641
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Name: Time
SyncMode: 0
SyncSource: LaserScan
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 50
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
back_laser_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_footprint:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
bl_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
bl_caster_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
br_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
br_caster_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front_laser_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
imu_frame:
Alpha: 1
Show Axes: false
Show Trail: false
imu_link:
Alpha: 1
Show Axes: false
Show Trail: false
left_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
surface:
Alpha: 1
Show Axes: false
Show Trail: false
us_1_frame:
Alpha: 1
Show Axes: false
Show Trail: false
us_2_frame:
Alpha: 1
Show Axes: false
Show Trail: false
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: true
Filter (blacklist): ""
Filter (whitelist): ""
Frame Timeout: 15
Frames:
All Enabled: false
back_laser_link:
Value: true
base_footprint:
Value: true
base_link:
Value: true
bl_caster_rotation_link:
Value: true
bl_caster_wheel_link:
Value: true
br_caster_rotation_link:
Value: true
br_caster_wheel_link:
Value: true
fl_caster_rotation_link:
Value: true
fl_caster_wheel_link:
Value: true
fr_caster_rotation_link:
Value: true
fr_caster_wheel_link:
Value: true
front_laser_link:
Value: true
imu_frame:
Value: true
imu_link:
Value: true
left_wheel_link:
Value: true
map:
Value: true
odom:
Value: true
right_wheel_link:
Value: true
surface:
Value: true
us_1_frame:
Value: true
us_2_frame:
Value: true
Marker Alpha: 1
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
map:
odom:
base_footprint:
base_link:
back_laser_link:
{}
bl_caster_rotation_link:
bl_caster_wheel_link:
{}
br_caster_rotation_link:
br_caster_wheel_link:
{}
fl_caster_rotation_link:
fl_caster_wheel_link:
{}
fr_caster_rotation_link:
fr_caster_wheel_link:
{}
front_laser_link:
{}
imu_link:
imu_frame:
{}
left_wheel_link:
{}
right_wheel_link:
{}
surface:
{}
us_1_frame:
{}
us_2_frame:
{}
Update Interval: 0
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 5
Size (m): 0.20000000298023224
Style: Points
Topic: scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 0; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: Bumper Hit
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.07999999821186066
Style: Spheres
Topic: mobile_base/sensors/bumper_pointcloud
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz/Group
Displays:
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: costmap
Draw Behind: true
Enabled: true
Name: Costmap
Topic: /move_base_node/global_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 252; 175; 62
Enabled: false
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.029999999329447746
Name: Global Plan (DWB)
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /move_base_node/DWBLocalPlanner/global_plan
Unreliable: false
Value: false
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 239; 41; 41
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.029999999329447746
Name: Full Plan
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: Axes
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /move_base_node/SBPLLatticePlanner/plan
Unreliable: true
Value: true
- Alpha: 1
Class: rviz/Polygon
Color: 0; 12; 255
Enabled: false
Name: Footprint
Queue Size: 10
Topic: move_base_node/global_costmap/footprint
Unreliable: true
Value: false
- Class: rviz/Marker
Enabled: false
Marker Topic: /move_base_node/SBPLLatticePlanner/footprint_markers
Name: Marker
Namespaces:
{}
Queue Size: 100
Value: false
Enabled: true
Name: Global Map
- Class: rviz/Group
Displays:
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Name: Costmap
Topic: move_base_node/local_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Class: rviz/GridCells
Color: 25; 255; 0
Enabled: false
Name: Costmap (MiR)
Queue Size: 10
Topic: move_base_node/local_costmap/obstacles
Unreliable: false
Value: false
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 0; 12; 255
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.20000000298023224
Line Style: Lines
Line Width: 0.05000000074505806
Name: Local Plan (DWB)
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: Axes
Queue Size: 10
Radius: 0.019999999552965164
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: move_base_node/DWBLocalPlanner/local_plan
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 25; 0
Enabled: false
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.029999999329447746
Name: TransformedToLocalPath
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /move_base_node/DWBLocalPlanner/transformed_global_plan
Unreliable: false
Value: false
- Alpha: 1
Class: rviz/Polygon
Color: 204; 0; 0
Enabled: true
Name: Footprint
Queue Size: 10
Topic: move_base_node/local_costmap/footprint
Unreliable: false
Value: true
- Alpha: 0.30000001192092896
Autocompute Intensity Bounds: false
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: total_cost
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 40
Min Color: 0; 0; 0
Min Intensity: 0
Name: Cost Cloud
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.03999999910593033
Style: Flat Squares
Topic: move_base_node/DWBLocalPlanner/cost_cloud
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: move_base_node/DWBLocalPlanner/markers
Name: DWB Markers
Namespaces:
{}
Queue Size: 100
Value: true
Enabled: true
Name: Local Map
- Alpha: 1
Arrow Length: 0.20000000298023224
Axes Length: 0.30000001192092896
Axes Radius: 0.009999999776482582
Class: rviz/PoseArray
Color: 0; 192; 0
Enabled: false
Head Length: 0.07000000029802322
Head Radius: 0.029999999329447746
Name: Amcl Particle Swarm
Queue Size: 10
Shaft Length: 0.23000000417232513
Shaft Radius: 0.009999999776482582
Shape: Arrow (Flat)
Topic: particlecloud
Unreliable: false
Value: false
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /move_base_node/WaypointsSBPLPlanner/waypoints
Name: Waypoints
Namespaces:
{}
Queue Size: 100
Value: false
- Alpha: 1
Arrow Length: 2
Axes Length: 0.20000000298023224
Axes Radius: 0.019999999552965164
Class: rviz/PoseArray
Color: 255; 25; 0
Enabled: false
Head Length: 0.10000000149011612
Head Radius: 0.029999999329447746
Name: waypoints
Queue Size: 6
Shaft Length: 0.33000001311302185
Shaft Radius: 0.019999999552965164
Shape: Axes
Topic: /move_base_node/DWBLocalPlanner/PathProgress/reached_intermediate_goals
Unreliable: false
Value: false
- Alpha: 1
Axes Length: 0.30000001192092896
Axes Radius: 0.019999999552965164
Class: rviz/Pose
Color: 114; 159; 207
Enabled: true
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Name: Pose_closet
Queue Size: 10
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Shape: Axes
Topic: /move_base_node/DWBLocalPlanner/PathProgress/closet_robot_goal
Unreliable: false
Value: true
- Alpha: 1
Axes Length: 0.30000001192092896
Axes Radius: 0.019999999552965164
Class: rviz/Pose
Color: 46; 52; 54
Enabled: true
Head Length: 0.10000000149011612
Head Radius: 0.10000000149011612
Name: Pose
Queue Size: 10
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Shape: Axes
Topic: /move_base_simple/goal
Unreliable: true
Value: true
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /map
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: false
Name: Directional zones
Topic: /direction_zones/map
Unreliable: false
Use Timestamp: false
Value: false
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: false
Name: Forbidden zones
Topic: /virtual_walls/map
Unreliable: false
Use Timestamp: false
Value: false
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: false
Name: Critical zones
Topic: /critical_zones/map
Unreliable: false
Use Timestamp: false
Value: false
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: false
Name: Preferred zones
Topic: /preferred_zones/map
Unreliable: false
Use Timestamp: false
Value: false
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: false
Name: Unpreferred zones
Topic: /unpreferred_zones/map
Unreliable: false
Use Timestamp: false
Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/MoveCamera
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/Select
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: move_base_simple/goal
- Class: rviz/Measure
- Class: rviz/PublishPoint
Single click: false
Topic: clicked_point
Value: true
Views:
Current:
Angle: -3.145003080368042
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Scale: -52.603572845458984
Target Frame: <Fixed Frame>
X: 8.760619163513184
Y: 10.58740234375
Saved:
- Angle: -0.004999996162950993
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: TopDownOrtho
Near Clip Distance: 0.009999999776482582
Scale: 23.103717803955078
Target Frame: <Fixed Frame>
X: 17.860042572021484
Y: 15.485589981079102
- Angle: -0.004999996162950993
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: TopDownOrtho
Near Clip Distance: 0.009999999776482582
Scale: 23.103717803955078
Target Frame: <Fixed Frame>
X: 17.860042572021484
Y: 15.485589981079102
- Angle: -0.004999996162950993
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: TopDownOrtho
Near Clip Distance: 0.009999999776482582
Scale: 23.103717803955078
Target Frame: <Fixed Frame>
X: 17.860042572021484
Y: 15.485589981079102
- Angle: -0.004999996162950993
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: TopDownOrtho
Near Clip Distance: 0.009999999776482582
Scale: 23.103717803955078
Target Frame: <Fixed Frame>
X: 17.860042572021484
Y: 15.485589981079102
- Angle: -0.004999995231628418
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: TopDownOrtho
Near Clip Distance: 0.009999999776482582
Scale: 52.42099380493164
Target Frame: <Fixed Frame>
X: 0.5443607568740845
Y: -0.26071029901504517
- Angle: -1.5799980163574219
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: TopDownOrtho
Near Clip Distance: 0.009999999776482582
Scale: 131.970703125
Target Frame: <Fixed Frame>
X: -1.8128122091293335
Y: -5.972503662109375
Window Geometry:
Displays:
collapsed: false
Height: 1016
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000004fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d0000039e0000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000009d00fffffffb000000100044006900730070006c0061007900730300000431000000830000043800000311000000010000010f00000374fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000374000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000004350000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1077
X: 0
Y: 27

92
mir_robot/mir_navigation/scripts/plot_mprim.py Executable file
View File

@@ -0,0 +1,92 @@
#!/usr/bin/env python3
# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
#
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
#
# * Neither the name of the the copyright holder nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Martin Günther
import matplotlib.pyplot as plt
import numpy as np
import sys
def get_value(strline, name):
if strline.find(name) < 0:
raise Exception("File format not matching the parser expectation", name)
return strline.replace(name, "", 1)
def get_pose(line):
ss = line.split()
return np.array([float(ss[0]), float(ss[1]), float(ss[2])])
class MPrim:
def __init__(self, f):
self.primID = int(get_value(f.readline(), "primID:"))
self.startAngle = int(get_value(f.readline(), "startangle_c:"))
self.endPose = get_pose(get_value(f.readline(), "endpose_c:"))
self.cost = float(get_value(f.readline(), "additionalactioncostmult:"))
self.nrPoses = int(get_value(f.readline(), "intermediateposes:"))
poses = []
for _ in range(self.nrPoses):
poses.append(f.readline())
self.poses = np.loadtxt(poses, delimiter=" ")
self.cmap = plt.get_cmap("nipy_spectral")
def plot(self, nr_angles):
plt.plot(self.poses[:, 0], self.poses[:, 1], c=self.cmap(float(self.startAngle) / nr_angles))
class MPrims:
def __init__(self, filename):
f = open(filename, "r")
self.resolution = float(get_value(f.readline(), "resolution_m:"))
self.nrAngles = int(get_value(f.readline(), "numberofangles:"))
self.nrPrims = int(get_value(f.readline(), "totalnumberofprimitives:"))
self.prims = []
for _ in range(self.nrPrims):
self.prims.append(MPrim(f))
f.close()
def plot(self):
fig = plt.figure()
ax = fig.add_subplot(111)
ax.set_xticks(np.arange(-1, 1, self.resolution))
ax.set_yticks(np.arange(-1, 1, self.resolution))
for prim in self.prims:
prim.plot(self.nrAngles)
plt.grid()
plt.show()
prims = MPrims(sys.argv[1])
prims.plot()