update
This commit is contained in:
@@ -1,48 +0,0 @@
|
||||
#For full documentation of the parameters in this file, and a list of all the
|
||||
#parameters available for TrajectoryPlannerROS, please see
|
||||
#http://www.ros.org/wiki/base_local_planner
|
||||
TrajectoryPlannerROS:
|
||||
#Set the acceleration limits of the robot
|
||||
acc_lim_th: 3.2
|
||||
acc_lim_x: 0.5
|
||||
acc_lim_y: 0.5
|
||||
|
||||
#Set the velocity limits of the robot
|
||||
max_vel_x: 0.4
|
||||
min_vel_x: 0.1
|
||||
max_rotational_vel: 1.0
|
||||
min_in_place_rotational_vel: 0.4
|
||||
|
||||
#The velocity the robot will command when trying to escape from a stuck situation
|
||||
escape_vel: -0.2
|
||||
|
||||
#For this example, we'll use a holonomic robot
|
||||
holonomic_robot: false
|
||||
|
||||
#Since we're using a holonomic robot, we'll set the set of y velocities it will sample
|
||||
y_vels: [-0.3, -0.1, 0.1, 0.3]
|
||||
|
||||
#Set the tolerance on achieving a goal
|
||||
xy_goal_tolerance: 0.1
|
||||
yaw_goal_tolerance: 0.05
|
||||
|
||||
#We'll configure how long and with what granularity we'll forward simulate trajectories
|
||||
sim_time: 3.0
|
||||
sim_granularity: 0.025
|
||||
vx_samples: 5
|
||||
vtheta_samples: 20
|
||||
|
||||
#Parameters for scoring trajectories
|
||||
goal_distance_bias: 0.4
|
||||
path_distance_bias: 0.7
|
||||
occdist_scale: 0.3
|
||||
heading_lookahead: 0.325
|
||||
|
||||
#We'll use the Dynamic Window Approach to control instead of Trajectory Rollout for this example
|
||||
dwa: false
|
||||
|
||||
#How far the robot must travel before oscillation flags are reset
|
||||
oscillation_reset_dist: 0.01
|
||||
|
||||
#Eat up the plan as the robot moves along it
|
||||
prune_plan: false
|
||||
@@ -1,54 +0,0 @@
|
||||
robot_base_frame: base_footprint
|
||||
transform_tolerance: 1.0
|
||||
obstacle_range: 3.0
|
||||
#mark_threshold: 1
|
||||
publish_voxel_map: true
|
||||
footprint_padding: 0.0
|
||||
|
||||
navigation_map:
|
||||
map_topic: /map
|
||||
map_pkg: managerments
|
||||
map_file: maze
|
||||
|
||||
virtual_walls_map:
|
||||
map_topic: /virtual_walls/map
|
||||
namespace: /virtual_walls
|
||||
map_pkg: managerments
|
||||
map_file: maze
|
||||
use_maximum: true
|
||||
lethal_cost_threshold: 100
|
||||
|
||||
obstacles:
|
||||
observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
|
||||
f_scan_marking:
|
||||
topic: /f_scan
|
||||
data_type: LaserScan
|
||||
clearing: false
|
||||
marking: true
|
||||
inf_is_valid: false
|
||||
min_obstacle_height: 0.0
|
||||
max_obstacle_height: 0.25
|
||||
f_scan_clearing:
|
||||
topic: /f_scan
|
||||
data_type: LaserScan
|
||||
clearing: true
|
||||
marking: false
|
||||
inf_is_valid: false
|
||||
min_obstacle_height: 0.0
|
||||
max_obstacle_height: 0.25
|
||||
b_scan_marking:
|
||||
topic: /b_scan
|
||||
data_type: LaserScan
|
||||
clearing: false
|
||||
marking: true
|
||||
inf_is_valid: false
|
||||
min_obstacle_height: 0.0
|
||||
max_obstacle_height: 0.25
|
||||
b_scan_clearing:
|
||||
topic: /b_scan
|
||||
data_type: LaserScan
|
||||
clearing: true
|
||||
marking: false
|
||||
inf_is_valid: false
|
||||
min_obstacle_height: 0.0
|
||||
max_obstacle_height: 0.25
|
||||
@@ -1,13 +0,0 @@
|
||||
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: 10.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.6 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
|
||||
|
||||
@@ -1,10 +0,0 @@
|
||||
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: inflation, type: "costmap_2d::InflationLayer" }
|
||||
obstacles:
|
||||
enabled: false
|
||||
footprint_clearing_enabled: false
|
||||
@@ -1,16 +0,0 @@
|
||||
local_costmap:
|
||||
global_frame: 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: 10.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.3 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
|
||||
width: 8.0
|
||||
height: 8.0
|
||||
origin_x: 0.0
|
||||
origin_y: 0.0
|
||||
@@ -1,8 +0,0 @@
|
||||
local_costmap:
|
||||
frame_id: odom
|
||||
plugins:
|
||||
- {name: obstacles, type: "costmap_2d::VoxelLayer" }
|
||||
- {name: inflation, type: "costmap_2d::InflationLayer" }
|
||||
obstacles:
|
||||
enabled: true
|
||||
footprint_clearing_enabled: true
|
||||
@@ -1,17 +0,0 @@
|
||||
base_global_planner: CustomPlanner
|
||||
CustomPlanner:
|
||||
environment_type: XYThetaLattice
|
||||
planner_type: ARAPlanner
|
||||
allocated_time: 10.0
|
||||
initial_epsilon: 1.0
|
||||
force_scratch_limit: 10000
|
||||
forward_search: false
|
||||
nominalvel_mpersecs: 0.8
|
||||
timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
|
||||
allow_unknown: true
|
||||
directory_to_save_paths: "/init/paths"
|
||||
pathway_filename: "pathway.txt"
|
||||
current_pose_topic_name: "/amcl_pose"
|
||||
map_frame_id: "map"
|
||||
base_frame_id: "base_link"
|
||||
|
||||
@@ -1,55 +0,0 @@
|
||||
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
|
||||
@@ -1,217 +0,0 @@
|
||||
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
|
||||
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
|
||||
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
|
||||
frequency: 50
|
||||
|
||||
# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
|
||||
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
|
||||
# filter will generate new output. Defaults to 1 / frequency if not specified.
|
||||
sensor_timeout: 0.1
|
||||
|
||||
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
|
||||
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
|
||||
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
|
||||
# by, for example, an IMU. Defaults to false if unspecified.
|
||||
two_d_mode: true
|
||||
|
||||
# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
|
||||
# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
|
||||
# unspecified.
|
||||
transform_time_offset: 0.0
|
||||
|
||||
# Use this parameter to specify how long the tf listener should wait for a transform to become available.
|
||||
# Defaults to 0.0 if unspecified.
|
||||
transform_timeout: 0.0
|
||||
|
||||
# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
|
||||
# unhappy with any settings or data.
|
||||
print_diagnostics: true
|
||||
|
||||
# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
|
||||
# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
|
||||
# effects on the performance of the node. Defaults to false if unspecified.
|
||||
debug: false
|
||||
|
||||
# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
|
||||
debug_out_file: /path/to/debug/file.txt
|
||||
|
||||
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
|
||||
publish_tf: true
|
||||
|
||||
# Whether to publish the acceleration state. Defaults to false if unspecified.
|
||||
publish_acceleration: false
|
||||
|
||||
# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
|
||||
# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
|
||||
# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
|
||||
# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
|
||||
# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
|
||||
# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
|
||||
# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
|
||||
# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
|
||||
# Here is how to use the following settings:
|
||||
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
|
||||
# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
|
||||
# odom_frame.
|
||||
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
|
||||
# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
|
||||
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
|
||||
# from landmark observations) then:
|
||||
# 3a. Set your "world_frame" to your map_frame value
|
||||
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
|
||||
# estimation node from robot_localization! However, that instance should *not* fuse the global data.
|
||||
map_frame: map # Defaults to "map" if unspecified
|
||||
odom_frame: $(arg tf_prefix)odom # Defaults to "odom" if unspecified
|
||||
base_link_frame: $(arg tf_prefix)base_footprint # Defaults to "base_link" if unspecified
|
||||
world_frame: $(arg tf_prefix)odom # Defaults to the value of odom_frame if unspecified
|
||||
|
||||
# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
|
||||
# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
|
||||
# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
|
||||
# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
|
||||
# default values, and must be specified.
|
||||
odom0: odom
|
||||
|
||||
# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
|
||||
# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
|
||||
# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
|
||||
# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
|
||||
# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
|
||||
# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
|
||||
# if unspecified, effectively making this parameter required for each sensor.
|
||||
# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html
|
||||
odom0_config: [false, false, false, # x y z
|
||||
false, false, false, # roll pitch yaw
|
||||
true, true, false, # vx vy vz
|
||||
false, false, true, # vroll vpitch vyaw
|
||||
false, false, false] # ax ay az
|
||||
|
||||
# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
|
||||
# the size of the subscription queue so that more measurements are fused.
|
||||
odom0_queue_size: 10
|
||||
|
||||
# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
|
||||
# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
|
||||
# algorithm.
|
||||
odom0_nodelay: false
|
||||
|
||||
# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
|
||||
# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
|
||||
# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
|
||||
# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
|
||||
# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
|
||||
# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
|
||||
# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
|
||||
# for twist measurements has no effect.
|
||||
odom0_differential: false
|
||||
|
||||
# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
|
||||
# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
|
||||
# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
|
||||
# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
|
||||
odom0_relative: false
|
||||
|
||||
# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
|
||||
# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
|
||||
# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
|
||||
# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
|
||||
# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
|
||||
# the thresholds.
|
||||
#odom0_pose_rejection_threshold: 5
|
||||
#odom0_twist_rejection_threshold: 1
|
||||
|
||||
# Further input parameter examples
|
||||
# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html
|
||||
imu0: imu_data
|
||||
imu0_config: [false, false, false, # x y z
|
||||
false, false, true, # roll pitch yaw
|
||||
false, false, false, # vx vy vz
|
||||
false, false, true, # vroll vpitch vyaw
|
||||
true, false, false] # ax ay az
|
||||
imu0_nodelay: false
|
||||
imu0_differential: false
|
||||
imu0_relative: true
|
||||
imu0_queue_size: 10
|
||||
#imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
|
||||
#imu0_twist_rejection_threshold: 0.8 #
|
||||
#imu0_linear_acceleration_rejection_threshold: 0.8 #
|
||||
|
||||
# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
|
||||
# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
|
||||
imu0_remove_gravitational_acceleration: false
|
||||
|
||||
# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
|
||||
# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
|
||||
# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
|
||||
# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
|
||||
# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
|
||||
# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
|
||||
# for the velocity variable in question, or decrease the variance of the variable in question in the measurement
|
||||
# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
|
||||
# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
|
||||
# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
|
||||
# inputs, the control term will be ignored.
|
||||
# Whether or not we use the control input during predicition. Defaults to false.
|
||||
use_control: false
|
||||
# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
|
||||
# false.
|
||||
stamped_control: false
|
||||
# The last issued control command will be used in prediction for this period. Defaults to 0.2.
|
||||
control_timeout: 0.2
|
||||
# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
|
||||
control_config: [true, false, false, false, false, true]
|
||||
# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
|
||||
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
|
||||
# Acceleration and deceleration limits are not always the same for robots.
|
||||
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
|
||||
# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
|
||||
# gains
|
||||
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
|
||||
# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
|
||||
# gains
|
||||
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
|
||||
|
||||
# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
|
||||
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
|
||||
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
|
||||
# However, if users find that a given variable is slow to converge, one approach is to increase the
|
||||
# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
|
||||
# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
|
||||
# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
|
||||
# unspecified.
|
||||
process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
|
||||
|
||||
# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
|
||||
# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
|
||||
# question. Users should take care not to use large values for variables that will not be measured directly. The values
|
||||
# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
|
||||
#if unspecified.
|
||||
initial_estimate_covariance: [100.0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 100.0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10.0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10.0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]
|
||||
@@ -1,36 +0,0 @@
|
||||
Amcl:
|
||||
use_map_topic: true
|
||||
odom_model_type: "diff-corrected"
|
||||
gui_publish_rate: 5.0
|
||||
save_pose_rate: 0.5
|
||||
laser_max_beams: 300
|
||||
laser_min_range: -1.0
|
||||
laser_max_range: -1.0
|
||||
min_particles: 1000
|
||||
max_particles: 3000
|
||||
kld_err: 0.05
|
||||
kld_z: 0.99
|
||||
odom_alpha1: 0.02
|
||||
odom_alpha2: 0.01
|
||||
odom_alpha3: 0.01
|
||||
odom_alpha4: 0.02
|
||||
laser_z_hit: 0.5
|
||||
laser_z_short: 0.05
|
||||
laser_z_max: 0.05
|
||||
laser_z_rand: 0.5
|
||||
laser_sigma_hit: 0.2
|
||||
laser_lambda_short: 0.1
|
||||
laser_model_type: "likelihood_field"
|
||||
laser_likelihood_max_dist: 1.0
|
||||
update_min_d: 0.05
|
||||
update_min_a: 0.05
|
||||
odom_frame_id: odom
|
||||
base_frame_id: base_footprint
|
||||
global_frame_id: map
|
||||
resample_interval: 1
|
||||
transform_tolerance: 0.2
|
||||
recovery_alpha_slow: 0.001
|
||||
recovery_alpha_fast: 0.001
|
||||
initial_pose_x: 0.0
|
||||
initial_pose_y: 0.0
|
||||
initial_pose_a: 0.0
|
||||
@@ -1,120 +0,0 @@
|
||||
maker_sources: trolley charger dock_station undock_station
|
||||
trolley:
|
||||
plugins:
|
||||
- {name: 4legs, docking_planner: "DockPlanner", docking_nav: ""}
|
||||
- {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" }
|
||||
|
||||
4legs:
|
||||
maker_goal_frame: trolley_goal
|
||||
footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
|
||||
delay: 2.0
|
||||
timeout: 60.0
|
||||
vel_x: 0.1
|
||||
vel_theta: 0.3
|
||||
yaw_goal_tolerance: 0.017
|
||||
xy_goal_tolerance: 0.01
|
||||
min_lookahead_dist: 0.4
|
||||
max_lookahead_dist: 1.0
|
||||
lookahead_time: 1.5
|
||||
angle_threshold: 0.36
|
||||
|
||||
qrcode:
|
||||
maker_goal_frame: qr_trolley
|
||||
delay: 2.0
|
||||
timeout: 60.0
|
||||
vel_x: 0.05
|
||||
vel_theta: 0.2
|
||||
allow_rotate: true
|
||||
yaw_goal_tolerance: 0.017
|
||||
xy_goal_tolerance: 0.01
|
||||
min_lookahead_dist: 0.4
|
||||
max_lookahead_dist: 1.0
|
||||
lookahead_time: 1.5
|
||||
angle_threshold: 0.36
|
||||
|
||||
charger:
|
||||
plugins:
|
||||
- {name: charger, docking_planner: "DockPlanner", docking_nav: ""}
|
||||
|
||||
charger:
|
||||
maker_goal_frame: charger_goal
|
||||
footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
|
||||
delay: 2
|
||||
timeout: 60
|
||||
vel_x: 0.1
|
||||
yaw_goal_tolerance: 0.017
|
||||
xy_goal_tolerance: 0.01
|
||||
min_lookahead_dist: 0.4
|
||||
max_lookahead_dist: 1.0
|
||||
lookahead_time: 1.5
|
||||
angle_threshold: 0.36
|
||||
|
||||
dock_station:
|
||||
plugins:
|
||||
- {name: station, docking_planner: "DockPlanner", docking_nav: ""}
|
||||
|
||||
station:
|
||||
maker_goal_frame: dock_station_goal
|
||||
footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]]
|
||||
delay: 2
|
||||
timeout: 60
|
||||
vel_x: 0.15
|
||||
vel_theta: 0.3
|
||||
yaw_goal_tolerance: 0.01
|
||||
xy_goal_tolerance: 0.01
|
||||
min_lookahead_dist: 0.4
|
||||
max_lookahead_dist: 1.0
|
||||
lookahead_time: 1.5
|
||||
angle_threshold: 0.36
|
||||
|
||||
|
||||
dock_station_2:
|
||||
plugins:
|
||||
- {name: station, docking_planner: "DockPlanner", docking_nav: ""}
|
||||
|
||||
station:
|
||||
maker_goal_frame: dock_station_goal_2
|
||||
footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]]
|
||||
delay: 2
|
||||
timeout: 60
|
||||
vel_x: 0.15
|
||||
vel_theta: 0.3
|
||||
yaw_goal_tolerance: 0.01
|
||||
xy_goal_tolerance: 0.01
|
||||
min_lookahead_dist: 0.4
|
||||
max_lookahead_dist: 1.0
|
||||
lookahead_time: 1.5
|
||||
angle_threshold: 0.36
|
||||
|
||||
undock_station:
|
||||
plugins:
|
||||
- {name: station, docking_planner: "DockPlanner", docking_nav: ""}
|
||||
- {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" }
|
||||
|
||||
station:
|
||||
maker_goal_frame: undock_station_goal
|
||||
footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
|
||||
delay: 2.0
|
||||
timeout: 60.0
|
||||
vel_x: 0.15
|
||||
vel_theta: 0.3
|
||||
yaw_goal_tolerance: 0.017
|
||||
xy_goal_tolerance: 0.01
|
||||
min_lookahead_dist: 0.4
|
||||
max_lookahead_dist: 1.0
|
||||
lookahead_time: 1.5
|
||||
angle_threshold: 0.36
|
||||
|
||||
qrcode:
|
||||
maker_goal_frame: qr_station
|
||||
delay: 2.0
|
||||
timeout: 60.0
|
||||
vel_x: 0.05
|
||||
vel_theta: 0.2
|
||||
allow_rotate: true
|
||||
yaw_goal_tolerance: 0.01
|
||||
xy_goal_tolerance: 0.01
|
||||
min_lookahead_dist: 0.4
|
||||
max_lookahead_dist: 1.0
|
||||
lookahead_time: 1.5
|
||||
angle_threshold: 0.36
|
||||
@@ -1,63 +0,0 @@
|
||||
SlamToolBox:
|
||||
# Plugin params
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ROS Parameters
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_link
|
||||
scan_topic: /scan
|
||||
mode: mapping #localization
|
||||
debug_logging: false
|
||||
throttle_scans: 1
|
||||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 10.0
|
||||
resolution: 0.05
|
||||
max_laser_range: 20.0 #for rastering images
|
||||
minimum_time_interval: 0.5
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 14400.
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
enable_interactive_mode: true
|
||||
|
||||
# General Parameters
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.5
|
||||
minimum_travel_heading: 0.5
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_maximum_scan_distance: 10
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
loop_search_maximum_distance: 3.0
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
|
||||
# Correlation Parameters - Correlation Parameters
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# Correlation Parameters - Loop Closure Parameters
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# Scan Matcher Parameters
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
@@ -1,24 +0,0 @@
|
||||
|
||||
### replanning
|
||||
controller_frequency: 20.0 # run controller at 15.0 Hz
|
||||
controller_patience: 0.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: -1.0 # abort controller and trigger recovery behaviors after 30.0 s
|
||||
oscillation_distance: 1.5
|
||||
### recovery behaviors
|
||||
recovery_behavior_enabled: false
|
||||
recovery_behaviors: [
|
||||
{name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
|
||||
{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
|
||||
]
|
||||
|
||||
conservative_reset:
|
||||
reset_distance: 3.0 # clear obstacles farther away than 3.0 m
|
||||
invert_area_to_clear: true
|
||||
|
||||
aggressive_reset:
|
||||
reset_distance: 3.0
|
||||
|
||||
|
||||
@@ -1,106 +0,0 @@
|
||||
base_local_planner: mpc_local_planner/MpcLocalPlannerROS
|
||||
MpcLocalPlannerROS:
|
||||
|
||||
odom_topic: odom
|
||||
|
||||
## Robot settings
|
||||
robot:
|
||||
type: "unicycle"
|
||||
unicycle:
|
||||
max_vel_x: 0.5
|
||||
max_vel_x_backwards: 0.3
|
||||
max_vel_theta: 0.3
|
||||
acc_lim_x: 0.4 # deactive bounds with zero
|
||||
dec_lim_x: 0.4 # deactive bounds with zero
|
||||
acc_lim_theta: 0.6 # deactivate bounds with zero
|
||||
|
||||
## Footprint model for collision avoidance
|
||||
footprint_model:
|
||||
type: "point"
|
||||
is_footprint_dynamic: False
|
||||
|
||||
## Collision avoidance
|
||||
collision_avoidance:
|
||||
min_obstacle_dist: 0.5 # Note, this parameter must be chosen w.r.t. the footprint_model
|
||||
enable_dynamic_obstacles: False
|
||||
force_inclusion_dist: 0.5
|
||||
cutoff_dist: 2.0
|
||||
include_costmap_obstacles: True
|
||||
costmap_obstacles_behind_robot_dist: 1.5
|
||||
collision_check_no_poses: 5
|
||||
|
||||
## Planning grid
|
||||
grid:
|
||||
type: "fd_grid"
|
||||
grid_size_ref: 20 # Set horizon length here (T = (grid_size_ref-1) * dt_ref); Note, also check max_global_plan_lookahead_dist
|
||||
dt_ref: 0.3 # and here the corresponding temporal resolution
|
||||
xf_fixed: [False, False, False] # Unfix final state -> we use terminal cost below
|
||||
warm_start: True
|
||||
collocation_method: "forward_differences"
|
||||
cost_integration_method: "left_sum"
|
||||
variable_grid:
|
||||
enable: False # We want a fixed grid
|
||||
min_dt: 0.0;
|
||||
max_dt: 10.0;
|
||||
grid_adaptation:
|
||||
enable: True
|
||||
dt_hyst_ratio: 0.1
|
||||
min_grid_size: 2
|
||||
max_grid_size: 50
|
||||
|
||||
## Planning options
|
||||
planning:
|
||||
objective:
|
||||
type: "quadratic_form" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly
|
||||
quadratic_form:
|
||||
state_weights: [2.0, 2.0, 0.25]
|
||||
control_weights: [0.1, 0.05]
|
||||
integral_form: False
|
||||
terminal_cost:
|
||||
type: "quadratic" # can be "none"
|
||||
quadratic:
|
||||
final_state_weights: [10.0, 10.0, 0.5]
|
||||
terminal_constraint:
|
||||
type: "none" # can be "none"
|
||||
l2_ball:
|
||||
weight_matrix: [1.0, 1.0, 1.0]
|
||||
radius: 5
|
||||
|
||||
## Controller options
|
||||
controller:
|
||||
outer_ocp_iterations: 1
|
||||
xy_goal_tolerance: 0.05
|
||||
yaw_goal_tolerance: 0.04
|
||||
global_plan_overwrite_orientation: False
|
||||
global_plan_prune_distance: 1.5
|
||||
allow_init_with_backward_motion: True
|
||||
max_global_plan_lookahead_dist: 1.0 # Check horizon length
|
||||
force_reinit_new_goal_dist: 1.0
|
||||
force_reinit_new_goal_angular: 1.57
|
||||
force_reinit_num_steps: 0
|
||||
prefer_x_feedback: False
|
||||
publish_ocp_results: True
|
||||
|
||||
## Solver settings
|
||||
solver:
|
||||
type: "ipopt"
|
||||
ipopt:
|
||||
iterations: 100
|
||||
max_cpu_time: -1.0
|
||||
ipopt_numeric_options:
|
||||
tol: 1e-3
|
||||
ipopt_string_options:
|
||||
linear_solver: "mumps"
|
||||
hessian_approximation: "exact" # exact or limited-memory
|
||||
lsq_lm:
|
||||
iterations: 10
|
||||
weight_init_eq: 2
|
||||
weight_init_ineq: 2
|
||||
weight_init_bounds: 2
|
||||
weight_adapt_factor_eq: 1.5
|
||||
weight_adapt_factor_ineq: 1.5
|
||||
weight_adapt_factor_bounds: 1.5
|
||||
weight_adapt_max_eq: 500
|
||||
weight_adapt_max_ineq: 500
|
||||
weight_adapt_max_bounds: 500
|
||||
|
||||
@@ -1,280 +0,0 @@
|
||||
% /*
|
||||
% * 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');
|
||||
@@ -1,416 +0,0 @@
|
||||
#!/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_highcost_5cm.mprim'
|
||||
genmprim_unicycle(outfilename, visualize=True)
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -1,8 +0,0 @@
|
||||
MQTT:
|
||||
Name: T801
|
||||
Host: 172.20.235.170
|
||||
Port: 1885
|
||||
Client_ID: T801
|
||||
Username: robotics
|
||||
Password: robotics
|
||||
Keep_Alive: 60
|
||||
@@ -1,188 +0,0 @@
|
||||
|
||||
position_planner_name: PNKXLocalPlanner
|
||||
docking_planner_name: PNKXDockingLocalPlanner
|
||||
go_straight_planner_name: PNKXGoStraightLocalPlanner
|
||||
rotate_planner_name: PNKXRotateLocalPlanner
|
||||
|
||||
base_local_planner: nav_core_adapter::LocalPlannerAdapter
|
||||
yaw_goal_tolerance: 0.017
|
||||
xy_goal_tolerance: 0.015
|
||||
stateful: true
|
||||
publish_topic: true
|
||||
|
||||
LocalPlannerAdapter:
|
||||
PNKXLocalPlanner:
|
||||
# Algorithm
|
||||
trajectory_generator_name: mkt_plugins::LimitedAccelGenerator
|
||||
algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory
|
||||
algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
|
||||
# Goal checking
|
||||
goal_checker_name: mkt_plugins::GoalChecker
|
||||
|
||||
PNKXDockingLocalPlanner:
|
||||
# Algorithm
|
||||
trajectory_generator_name: mkt_plugins::LimitedAccelGenerator
|
||||
algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory
|
||||
algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
|
||||
# Goal checking
|
||||
goal_checker_name: mkt_plugins::GoalChecker
|
||||
|
||||
PNKXGoStraightLocalPlanner:
|
||||
# Algorithm
|
||||
trajectory_generator_name: mkt_plugins::LimitedAccelGenerator
|
||||
algorithm_nav_name: mkt_algorithm::diff::GoStraight
|
||||
# Goal checking
|
||||
goal_checker_name: mkt_plugins::GoalChecker
|
||||
|
||||
PNKXRotateLocalPlanner:
|
||||
# Algorithm
|
||||
algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
|
||||
trajectory_generator_name: mkt_plugins::LimitedAccelGenerator
|
||||
# Goal checking
|
||||
goal_checker_name: mkt_plugins::SimpleGoalChecker
|
||||
stateful: true
|
||||
|
||||
LimitedAccelGenerator:
|
||||
max_vel_x: 1.2
|
||||
min_vel_x: -1.2
|
||||
|
||||
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: 0.3 # max_rot_vel: 1.0 # choose slightly less than the base's capability
|
||||
min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity
|
||||
|
||||
acc_lim_x: 1.0
|
||||
acc_lim_y: 0.0 # diff drive robot
|
||||
acc_lim_theta: 0.5
|
||||
decel_lim_x: -2.0
|
||||
decel_lim_y: -0.0
|
||||
decel_lim_theta: -1.0
|
||||
|
||||
# Whether to split the path into segments or not
|
||||
split_path: true
|
||||
sim_time: 1.5
|
||||
vx_samples: 15
|
||||
vy_samples: 1
|
||||
vtheta_samples: 10
|
||||
discretize_by_time: true
|
||||
angular_granularity: 0.05
|
||||
linear_granularity: 0.1
|
||||
# sim_period
|
||||
include_last_point: true
|
||||
|
||||
PredictiveTrajectory:
|
||||
avoid_obstacles: false
|
||||
xy_local_goal_tolerance: 0.01
|
||||
angle_threshold: 0.5
|
||||
index_samples: 60
|
||||
follow_step_path: true
|
||||
|
||||
# Lookahead
|
||||
use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
|
||||
# only when false:
|
||||
lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
|
||||
# only when true:
|
||||
min_lookahead_dist: 0.5 # The minimum lookahead distance (m) threshold. (default: 0.3)
|
||||
max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9)
|
||||
lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
|
||||
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)
|
||||
max_journey_squared: 0.8 # Maximum squared journey to consider for goal (default: 0.2)
|
||||
|
||||
# Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
|
||||
use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
|
||||
# only when true:
|
||||
rotate_to_heading_min_angle: 0.03 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
|
||||
|
||||
# stoped
|
||||
rot_stopped_velocity: 0.03
|
||||
trans_stopped_velocity: 0.06
|
||||
min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
|
||||
|
||||
# Regulated linear velocity scaling
|
||||
use_regulated_linear_velocity_scaling: true # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
|
||||
# only when true:
|
||||
regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
|
||||
regulated_linear_scaling_min_speed: 0.05 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
|
||||
|
||||
# Inflation cost scaling (Limit velocity by proximity to obstacles)
|
||||
use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
|
||||
inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0
|
||||
cost_scaling_dist: 0.2 # (default: 0.6)
|
||||
cost_scaling_gain: 2.0 # (default: 1.0)
|
||||
|
||||
GoStraight:
|
||||
avoid_obstacles: false
|
||||
xy_local_goal_tolerance: 0.01
|
||||
angle_threshold: 0.6
|
||||
index_samples: 60
|
||||
follow_step_path: true
|
||||
|
||||
# Lookahead
|
||||
use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
|
||||
# only when false:
|
||||
lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
|
||||
# only when true:
|
||||
min_lookahead_dist: 0.5 # The minimum lookahead distance (m) threshold. (default: 0.3)
|
||||
max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9)
|
||||
lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
|
||||
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)
|
||||
max_journey_squared: 0.6 # Minimum squared journey to consider for goal (default: 0.2)
|
||||
|
||||
# Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
|
||||
use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
|
||||
# only when true:
|
||||
rotate_to_heading_min_angle: 0.35 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
|
||||
# Speed
|
||||
min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
|
||||
|
||||
# stoped
|
||||
rot_stopped_velocity: 0.03
|
||||
trans_stopped_velocity: 0.06
|
||||
min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
|
||||
|
||||
use_regulated_linear_velocity_scaling: false
|
||||
use_cost_regulated_linear_velocity_scaling: false
|
||||
|
||||
RotateToGoal:
|
||||
avoid_obstacles: false
|
||||
xy_local_goal_tolerance: 0.01
|
||||
angle_threshold: 0.6
|
||||
index_samples: 60
|
||||
follow_step_path: true
|
||||
|
||||
# Lookahead
|
||||
use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
|
||||
# only when false:
|
||||
lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
|
||||
# only when true:
|
||||
min_lookahead_dist: 0.5 # The minimum lookahead distance (m) threshold. (default: 0.3)
|
||||
max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9)
|
||||
lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
|
||||
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)
|
||||
max_journey_squared: 0.6 # Maximum squared journey to consider for goal (default: 0.2)
|
||||
|
||||
# Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
|
||||
use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
|
||||
# only when true:
|
||||
rotate_to_heading_min_angle: 0.03 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
|
||||
|
||||
# stoped
|
||||
rot_stopped_velocity: 0.03
|
||||
trans_stopped_velocity: 0.06
|
||||
min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
|
||||
|
||||
# Regulated linear velocity scaling
|
||||
use_regulated_linear_velocity_scaling: true # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
|
||||
# only when true:
|
||||
regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
|
||||
regulated_linear_scaling_min_speed: 0.05 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
|
||||
|
||||
# Inflation cost scaling (Limit velocity by proximity to obstacles)
|
||||
use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
|
||||
inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0
|
||||
cost_scaling_dist: 0.2 # (default: 0.6)
|
||||
cost_scaling_gain: 2.0 # (default: 1.0)
|
||||
@@ -1,71 +0,0 @@
|
||||
# -----------------------------------
|
||||
left_wheel : 'left_wheel_joint'
|
||||
right_wheel : 'right_wheel_joint'
|
||||
publish_rate: 50 # this is what the real MiR platform publishes (default: 50)
|
||||
# These covariances are exactly what the real MiR platform publishes
|
||||
pose_covariance_diagonal : [0.00001, 0.00001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
|
||||
twist_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 1000.0]
|
||||
enable_odom_tf: true
|
||||
enable_wheel_tf: true
|
||||
allow_multiple_cmd_vel_publishers: true
|
||||
# open_loop: false
|
||||
# Wheel separation and diameter. These are both optional.
|
||||
# diff_drive_controller will attempt to read either one or both from the
|
||||
# URDF if not specified as a parameter.
|
||||
# We don't set the value here because it's different for each MiR type (100, 250, ...), and
|
||||
# the plugin figures out the correct values.
|
||||
wheel_separation : 0.5106
|
||||
wheel_radius : 0.1
|
||||
|
||||
# Wheel separation and radius multipliers
|
||||
wheel_separation_multiplier: 1.0 # default: 1.0
|
||||
wheel_radius_multiplier : 1.0 # default: 1.0
|
||||
|
||||
# Velocity commands timeout [s], default 0.5
|
||||
cmd_vel_timeout: 1.0
|
||||
|
||||
# frame_ids (same as real MiR platform)
|
||||
base_frame_id: base_footprint # default: base_link base_footprint
|
||||
odom_frame_id: odom # default: odom
|
||||
|
||||
# Velocity and acceleration limits
|
||||
# Whenever a min_* is unspecified, default to -max_*
|
||||
linear:
|
||||
x:
|
||||
has_velocity_limits : true
|
||||
max_velocity : 1.5 # m/s; move_base max_vel_x: 0.8
|
||||
min_velocity : -1.5 # m/s
|
||||
has_acceleration_limits: true
|
||||
max_acceleration : 3.0 # m/s^2; move_base acc_lim_x: 1.5
|
||||
min_acceleration : -3.0 # m/s^2
|
||||
has_jerk_limits : true
|
||||
max_jerk : 5.0 # m/s^3
|
||||
angular:
|
||||
z:
|
||||
has_velocity_limits : true
|
||||
max_velocity : 0.6 # rad/s; move_base max_rot_vel: 1.0
|
||||
min_velocity : -0.6
|
||||
has_acceleration_limits: true
|
||||
max_acceleration : 3.0 # rad/s^2; move_base acc_lim_th: 2.0
|
||||
has_jerk_limits : true
|
||||
max_jerk : 2.5 # rad/s^3
|
||||
|
||||
left_wheel_joint:
|
||||
lookup_name: WheelPlugin
|
||||
max_publish_rate: 50
|
||||
mesurement_topic: left_encoder
|
||||
frame_id: left_wheel_link
|
||||
wheel_topic: /left_wheel
|
||||
subscribe_queue_size: 1
|
||||
command_timeout: 5.0
|
||||
origin : [0.0, 0.255, 0.075, 0.0, 0.0, 0.0] # origin from base_frame_id
|
||||
|
||||
right_wheel_joint:
|
||||
lookup_name: WheelPlugin
|
||||
max_publish_rate: 50
|
||||
mesurement_topic: right_encoder
|
||||
frame_id: right_wheel_link
|
||||
wheel_topic: /right_wheel
|
||||
subscribe_queue_size: 1
|
||||
command_timeout: 5.0
|
||||
origin : [0.0, -0.255, 0.075, 0.0, 0.0, 0.0] # origin from base_frame_id
|
||||
@@ -1,10 +0,0 @@
|
||||
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.2
|
||||
timetoturn45degsinplace_secs: 0.6 # = 0.6 rad/s
|
||||
@@ -1,61 +0,0 @@
|
||||
bus:
|
||||
device: can0
|
||||
driver_plugin: can::SocketCANInterface
|
||||
master_allocator: canopen::SimpleMaster::Allocator
|
||||
sync:
|
||||
interval_ms: 10
|
||||
overflow: 0
|
||||
#
|
||||
# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats)
|
||||
# rate: 100 # heartbeat rate (1/rate in seconds)
|
||||
# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started
|
||||
nodes:
|
||||
f_mlse:
|
||||
id: 0x01 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
|
||||
eds_pkg: sick_line_guidance # package name for relative path
|
||||
eds_file: mls/SICK-MLS.eds # path to EDS/DCF file
|
||||
publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"]
|
||||
# MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022
|
||||
|
||||
# sick_line_guidance configuration of this node:
|
||||
sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported
|
||||
sick_topic: "f_mlse" # MLS_Measurement messages are published in topic "/mls"
|
||||
subscribe_queue_size: 1
|
||||
sick_frame_id: "f_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame"
|
||||
|
||||
# device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
|
||||
# example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
|
||||
# dcf_overlay:
|
||||
# "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
|
||||
# "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
|
||||
# "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
|
||||
# "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600
|
||||
# "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0
|
||||
# "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
|
||||
# "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0
|
||||
#
|
||||
|
||||
b_mlse:
|
||||
id: 0x02 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
|
||||
eds_pkg: sick_line_guidance # package name for relative path
|
||||
eds_file: mls/SICK-MLS.eds # path to EDS/DCF file
|
||||
publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"]
|
||||
# MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022
|
||||
|
||||
# sick_line_guidance configuration of this node:
|
||||
sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported
|
||||
sick_topic: "b_mlse" # MLS_Measurement messages are published in topic "/mls"
|
||||
subscribe_queue_size: 1
|
||||
sick_frame_id: "b_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame"
|
||||
|
||||
# device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
|
||||
# example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
|
||||
# dcf_overlay:
|
||||
# "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
|
||||
# "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
|
||||
# "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
|
||||
# "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600
|
||||
# "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0
|
||||
# "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
|
||||
# "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0
|
||||
#
|
||||
@@ -1,3 +0,0 @@
|
||||
base_global_planner: TwoPointsPlanner
|
||||
TwoPointsPlanner:
|
||||
lethal_obstacle: 20
|
||||
Reference in New Issue
Block a user