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

update

Browse Source
This commit is contained in:
HiepLM
2025-12-10 09:10:17 +07:00
parent b41d614d1b
commit 33afdfcc5c
24 changed files with 1847 additions and 347 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
config/mprim/base_local_planner_params.yaml Normal file
View File

@@ -0,0 +1,48 @@
#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

17
config/mprim/custom_global_params.yaml Normal file
View File

@@ -0,0 +1,17 @@
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"

55
config/mprim/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

217
config/mprim/ekf.yaml Executable file
View File

@@ -0,0 +1,217 @@
# 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]

36
config/mprim/localization.yaml Normal file
View File

@@ -0,0 +1,36 @@
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

152
config/mprim/maker_sources.yaml Normal file
View File

@@ -0,0 +1,152 @@
maker_sources: trolley charger dock_station undock_station dock_station_2 undock_station_2
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: 1.5 # Cấm sửa không là không chạy được
timeout: 60.0
vel_x: 0.25
vel_theta: 0.3
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.015
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.16
qrcode:
maker_goal_frame: qr_trolley
delay: 1.5 # Cấm sửa không là không chạy được
timeout: 60.0
vel_x: 0.05
vel_theta: 0.5
allow_rotate: true
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.02
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.16
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: 1.5 # Cấm sửa không là không chạy được
timeout: 60
vel_x: 0.1
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.015
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.16
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: 1.5 # Cấm sửa không là không chạy được
timeout: 60
vel_x: 0.15
vel_theta: 0.3
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.015
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.4
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: 1.5 # Cấm sửa không là không chạy được
timeout: 60
vel_x: 0.15
vel_theta: 0.3
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.015
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.4
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: 1.5 # Cấm sửa không là không chạy được
timeout: 60.0
vel_x: 0.25
vel_theta: 0.3
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.015
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.16
qrcode:
maker_goal_frame: qr_trolley
delay: 1.5 # Cấm sửa không là không chạy được
timeout: 60.0
vel_x: 0.05
vel_theta: 0.5
allow_rotate: true
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.03
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.16
undock_station_2:
plugins:
- {name: station, docking_planner: "DockPlanner", docking_nav: ""}
- {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" }
station:
maker_goal_frame: undock_station_goal_2
footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
delay: 1.5 # Cấm sửa không là không chạy được
timeout: 60.0
vel_x: 0.25
vel_theta: 0.5
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.03
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.16
qrcode:
maker_goal_frame: qr_trolley
delay: 1.5 # Cấm sửa không là không chạy được
timeout: 60.0
vel_x: 0.05
vel_theta: 0.5
allow_rotate: true
yaw_goal_tolerance: 0.02
xy_goal_tolerance: 0.02
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.16

63
config/mprim/mapping.yaml Normal file
View File

@@ -0,0 +1,63 @@
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

106
config/mprim/mpc_local_planner_params.yaml Executable file
View File

@@ -0,0 +1,106 @@
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

8
config/mprim/mqtt_general.yaml Normal file
View File

@@ -0,0 +1,8 @@
MQTT:
Name: T800
Host: 172.20.235.170
Port: 1885
Client_ID: T800
Username: robotics
Password: robotics
Keep_Alive: 60

71
config/mprim/ros_diff_drive_controller.yaml Executable file
View File

@@ -0,0 +1,71 @@
# -----------------------------------
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 : 0.5 # m/s^2; move_base acc_lim_x: 1.5
min_acceleration : -0.5 # m/s^2
has_jerk_limits : true
max_jerk : 3.0 # m/s^3
angular:
z:
has_velocity_limits : true
max_velocity : 1.0 # rad/s; move_base max_rot_vel: 1.0
min_velocity : -1.0
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

10
config/mprim/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.2
timetoturn45degsinplace_secs: 0.6 # = 0.6 rad/s

61
config/mprim/sick_line_guidance_mls.yaml Normal file
View File

@@ -0,0 +1,61 @@
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
#