git commit -m "first commit"
This commit is contained in:
105
mir_robot/mir_navigation/launch/amcl.launch
Executable file
105
mir_robot/mir_navigation/launch/amcl.launch
Executable file
@@ -0,0 +1,105 @@
|
||||
<?xml version="1.0" ?>
|
||||
|
||||
<launch>
|
||||
<arg name="tf_prefix" default="" />
|
||||
<arg name="namespace" default="$(arg tf_prefix)" doc="Namespace to push all topics into."/>
|
||||
|
||||
<arg name="use_map_topic" default="true"/>
|
||||
<arg name="scan_topic" default="scan"/>
|
||||
<arg name="map_topic" default="/map"/> <!-- if use_map_topic = true -->
|
||||
<arg name="map_service" default="/static_map"/> <!-- if use_map_topic = false -->
|
||||
<arg name="initial_pose_x" default="0.0"/>
|
||||
<arg name="initial_pose_y" default="0.0"/>
|
||||
<arg name="initial_pose_a" default="0.0"/>
|
||||
<arg name="odom_frame_id" default="$(arg tf_prefix)/odom"/>
|
||||
<arg name="base_frame_id" default="$(arg tf_prefix)/base_footprint"/>
|
||||
<arg name="global_frame_id" default="/map"/>
|
||||
|
||||
<group if="$(eval namespace != '')" ns="$(arg namespace)">
|
||||
<node pkg="amcl" type="amcl" name="amcl" output="screen" >
|
||||
<param name="use_map_topic" value="$(arg use_map_topic)"/>
|
||||
<param name="odom_model_type" value="diff-corrected"/>
|
||||
<param name="gui_publish_rate" value="10.0"/>
|
||||
<param name="save_pose_rate" value="0.5"/>
|
||||
<param name="laser_max_beams" value="50"/>
|
||||
<param name="laser_min_range" value="-1.0"/>
|
||||
<param name="laser_max_range" value="-1.0"/>
|
||||
<param name="min_particles" value="500"/>
|
||||
<param name="max_particles" value="5000"/>
|
||||
<param name="kld_err" value="0.05"/>
|
||||
<param name="kld_z" value="0.99"/>
|
||||
<param name="odom_alpha1" value="0.02"/>
|
||||
<param name="odom_alpha2" value="0.01"/>
|
||||
<param name="odom_alpha3" value="0.01"/>
|
||||
<param name="odom_alpha4" value="0.02"/>
|
||||
<param name="laser_z_hit" value="0.5"/>
|
||||
<param name="laser_z_short" value="0.05"/>
|
||||
<param name="laser_z_max" value="0.05"/>
|
||||
<param name="laser_z_rand" value="0.5"/>
|
||||
<param name="laser_sigma_hit" value="0.2"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_model_type" value="likelihood_field"/>
|
||||
<param name="laser_likelihood_max_dist" value="2.0"/>
|
||||
<param name="update_min_d" value="0.2"/>
|
||||
<param name="update_min_a" value="0.2"/>
|
||||
<param name="odom_frame_id" value="$(arg odom_frame_id)"/>
|
||||
<param name="base_frame_id" value="$(arg base_frame_id)"/>
|
||||
<param name="global_frame_id" value="$(arg global_frame_id)"/>
|
||||
<param name="resample_interval" value="1"/>
|
||||
<param name="transform_tolerance" value="0.2"/>
|
||||
<param name="recovery_alpha_slow" value="0.0"/>
|
||||
<param name="recovery_alpha_fast" value="0.0"/>
|
||||
<param name="initial_pose_x" value="$(arg initial_pose_x)"/>
|
||||
<param name="initial_pose_y" value="$(arg initial_pose_y)"/>
|
||||
<param name="initial_pose_a" value="$(arg initial_pose_a)"/>
|
||||
<remap from="scan" to="$(arg scan_topic)"/>
|
||||
<remap from="map" to="$(arg map_topic)"/>
|
||||
<remap from="static_map" to="$(arg map_service)"/>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
<!-- Duplicate of the above in case namespace is empty. This is necessary to
|
||||
avoid the "<group> tag has an empty 'ns' attribute" parsing error. -->
|
||||
<group unless="$(eval namespace != '')">
|
||||
<node pkg="amcl" type="amcl" name="amcl" output="screen" >
|
||||
<param name="use_map_topic" value="$(arg use_map_topic)"/>
|
||||
<param name="odom_model_type" value="diff-corrected"/>
|
||||
<param name="gui_publish_rate" value="10.0"/>
|
||||
<param name="save_pose_rate" value="0.5"/>
|
||||
<param name="laser_max_beams" value="50"/>
|
||||
<param name="laser_min_range" value="-1.0"/>
|
||||
<param name="laser_max_range" value="-1.0"/>
|
||||
<param name="min_particles" value="500"/>
|
||||
<param name="max_particles" value="5000"/>
|
||||
<param name="kld_err" value="0.05"/>
|
||||
<param name="kld_z" value="0.99"/>
|
||||
<param name="odom_alpha1" value="0.02"/>
|
||||
<param name="odom_alpha2" value="0.01"/>
|
||||
<param name="odom_alpha3" value="0.01"/>
|
||||
<param name="odom_alpha4" value="0.02"/>
|
||||
<param name="laser_z_hit" value="0.5"/>
|
||||
<param name="laser_z_short" value="0.05"/>
|
||||
<param name="laser_z_max" value="0.05"/>
|
||||
<param name="laser_z_rand" value="0.5"/>
|
||||
<param name="laser_sigma_hit" value="0.2"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_model_type" value="likelihood_field"/>
|
||||
<param name="laser_likelihood_max_dist" value="2.0"/>
|
||||
<param name="update_min_d" value="0.2"/>
|
||||
<param name="update_min_a" value="0.2"/>
|
||||
<param name="odom_frame_id" value="$(arg odom_frame_id)"/>
|
||||
<param name="base_frame_id" value="$(arg base_frame_id)"/>
|
||||
<param name="global_frame_id" value="$(arg global_frame_id)"/>
|
||||
<param name="resample_interval" value="1"/>
|
||||
<param name="transform_tolerance" value="0.2"/>
|
||||
<param name="recovery_alpha_slow" value="0.0"/>
|
||||
<param name="recovery_alpha_fast" value="0.0"/>
|
||||
<param name="initial_pose_x" value="$(arg initial_pose_x)"/>
|
||||
<param name="initial_pose_y" value="$(arg initial_pose_y)"/>
|
||||
<param name="initial_pose_a" value="$(arg initial_pose_a)"/>
|
||||
<remap from="scan" to="$(arg scan_topic)"/>
|
||||
<remap from="map" to="$(arg map_topic)"/>
|
||||
<remap from="static_map" to="$(arg map_service)"/>
|
||||
</node>
|
||||
</group>
|
||||
</launch>
|
||||
68
mir_robot/mir_navigation/launch/hector_mapping.launch
Executable file
68
mir_robot/mir_navigation/launch/hector_mapping.launch
Executable file
@@ -0,0 +1,68 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
|
||||
<arg name="base_frame" default="base_footprint"/>
|
||||
<arg name="odom_frame" default="odom"/>
|
||||
<arg name="pub_map_odom_transform" default="true"/>
|
||||
<arg name="scan_subscriber_queue_size" default="5"/>
|
||||
<arg name="scan_topic" default="scan"/>
|
||||
<arg name="map_size" default="2048"/>
|
||||
<!-- set use_tf_pose_start_estimate and map_with_known_poses to `true` when
|
||||
the map-base_footprint transform is provided by a different node, such
|
||||
as fake_localization -->
|
||||
<arg name="disable_localization" default="false"/>
|
||||
|
||||
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
|
||||
|
||||
<!-- Frame names -->
|
||||
<param name="map_frame" value="map" />
|
||||
<param name="base_frame" value="$(arg base_frame)" />
|
||||
<param name="odom_frame" value="$(arg odom_frame)" />
|
||||
|
||||
<!-- Tf use -->
|
||||
<param name="use_tf_scan_transformation" value="true"/>
|
||||
<param name="use_tf_pose_start_estimate" value="$(arg disable_localization)"/>
|
||||
<param name="map_with_known_poses" value="$(arg disable_localization)" />
|
||||
<param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>
|
||||
<param name="pub_map_scanmatch_transform" value="true" />
|
||||
|
||||
<!-- Map size / start point -->
|
||||
<param name="map_resolution" value="0.050"/>
|
||||
<param name="map_size" value="$(arg map_size)"/>
|
||||
<param name="map_start_x" value="0.5"/>
|
||||
<param name="map_start_y" value="0.5" />
|
||||
<param name="map_multi_res_levels" value="3" />
|
||||
<param name="map_pub_period" value ="2.0" />
|
||||
|
||||
<!-- Map update parameters -->
|
||||
<param name="update_factor_free" value="0.4"/>
|
||||
<param name="update_factor_occupied" value="0.9" />
|
||||
<param name="map_update_distance_thresh" value="0.4"/>
|
||||
<param name="map_update_angle_thresh" value="0.06" />
|
||||
<param name="laser_min_dist" value="0.4" />
|
||||
<param name="laser_max_dist" value="30.0" />
|
||||
<param name="laser_z_min_value" value="-1.0" />
|
||||
<param name="laser_z_max_value" value="1.0" />
|
||||
|
||||
<!-- Advertising config -->
|
||||
<param name="advertise_map_service" value="true"/>
|
||||
|
||||
<param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
|
||||
<param name="scan_topic" value="$(arg scan_topic)"/>
|
||||
<param name="pose_update_topic" value="poseupdate" />
|
||||
<param name="sys_msg_topic" value="syscommand" />
|
||||
<param name="pub_odometry" value="false" />
|
||||
|
||||
<!-- Debug parameters -->
|
||||
<!--
|
||||
<param name="output_timing" value="false"/>
|
||||
<param name="pub_drawings" value="true"/>
|
||||
<param name="pub_debug_output" value="true"/>
|
||||
-->
|
||||
<param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
|
||||
|
||||
<remap from="map" to="/map" />
|
||||
</node>
|
||||
|
||||
<!--<node pkg="tf" type="static_transform_publisher" name="map_nav_broadcaster" args="0 0 0 0 0 0 map nav 100"/>-->
|
||||
</launch>
|
||||
29
mir_robot/mir_navigation/launch/move_base.xml
Executable file
29
mir_robot/mir_navigation/launch/move_base.xml
Executable file
@@ -0,0 +1,29 @@
|
||||
<launch>
|
||||
<!--node ns="local_costmap" name="voxel_grid_throttle" pkg="topic_tools" type="throttle" args="messages voxel_grid 3.0 voxel_grid_throttled" /-->
|
||||
<arg name="local_planner" default="dwb" doc="Local planner can be either dwa, base, teb or pose"/>
|
||||
<arg name="with_virtual_walls" default="true" doc="Enables usage of virtual walls when set. Set to false when running SLAM." />
|
||||
<arg name="with_preferred_zones" default="true" doc="Enables usage of preferred zones when set. Set to false when running SLAM." />
|
||||
<arg name="with_unpreferred_zones" default="true" doc="Enables usage of unpreferred zones when set. Set to false when running SLAM." />
|
||||
<arg name="with_critical_zones" default="true" doc="Enables usage of critical zones when set. Set to false when running SLAM." />
|
||||
<arg name="with_direction_zones" default="false" doc="Enables usage of direction zones when set. Set to false when running SLAM." />
|
||||
<arg name="prefix" default="" doc="Prefix used for robot tf frames" /> <!-- used in the config files -->
|
||||
|
||||
<node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen" clear_params="true">
|
||||
<param name="SBPLLatticePlanner/primitive_filename" value="$(find mir_navigation)/mprim/unicycle_highcost_5cm.mprim" />
|
||||
<rosparam file="$(find mir_navigation)/config/move_base_common_params.yaml" command="load" />
|
||||
<rosparam file="$(find mir_navigation)/config/sbpl_global_params.yaml" command="load" />
|
||||
<rosparam file="$(find mir_navigation)/config/$(arg local_planner)_local_planner_params.yaml" command="load" />
|
||||
<!-- global costmap params -->
|
||||
<rosparam file="$(find mir_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" subst_value="true" />
|
||||
<rosparam file="$(find mir_navigation)/config/costmap_global_params.yaml" command="load" />
|
||||
<rosparam file="$(find mir_navigation)/config/costmap_global_params_plugins.yaml" command="load" />
|
||||
|
||||
<!-- local costmap params -->
|
||||
<rosparam file="$(find mir_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" subst_value="true" />
|
||||
<rosparam file="$(find mir_navigation)/config/costmap_local_params.yaml" command="load" subst_value="true" />
|
||||
<rosparam file="$(find mir_navigation)/config/costmap_local_params_plugins.yaml" command="load" />
|
||||
|
||||
<remap from="map" to="/map" />
|
||||
<remap from="marker" to="move_base_node/DWBLocalPlanner/markers" />
|
||||
</node>
|
||||
</launch>
|
||||
44
mir_robot/mir_navigation/launch/start_maps.launch
Executable file
44
mir_robot/mir_navigation/launch/start_maps.launch
Executable file
@@ -0,0 +1,44 @@
|
||||
<launch>
|
||||
<arg name="map_file" default="" doc="Path to a map .yaml file (required)." />
|
||||
<arg name="empty_file" default="" doc="Path to a map .yaml file (required)." />
|
||||
<arg name="virtual_walls_map_file" default="$(arg map_file)" doc="Path to a virtual walls map .yaml file (optional)." />
|
||||
<arg name="with_virtual_walls" default="true" />
|
||||
<arg name="preferred_zones_map_file" default="$(arg empty_file)" doc="Path to a preferred zones .yaml file (optional)." />
|
||||
<arg name="with_preferred_zones" default="true" />
|
||||
<arg name="unpreferred_zones_map_file" default="$(arg empty_file)" doc="Path to a unpreferred zones .yaml file (optional)." />
|
||||
<arg name="with_unpreferred_zones" default="true" />
|
||||
<arg name="critical_zones_map_file" default="$(arg empty_file)" doc="Path to a critical zones .yaml file (optional)." />
|
||||
<arg name="with_critical_zones" default="true" />
|
||||
<arg name="direction_zones_map_file" default="$(arg empty_file)" doc="Path to a direction zones .yaml file (optional)." />
|
||||
<arg name="with_direction_zones" default="true" />
|
||||
|
||||
<node name="static_map_server" pkg="map_server" type="map_server" args="$(arg map_file)" ns="/" output="screen">
|
||||
<param name="frame_id" type="string" value="map"/>
|
||||
<param name="type" type="int" value="8"/>
|
||||
</node>
|
||||
|
||||
<node if="$(arg with_virtual_walls)" name="virtual_walls_map_server" pkg="map_server" type="map_server" args="$(arg virtual_walls_map_file)" ns="/virtual_walls" output="screen">
|
||||
<param name="frame_id" type="string" value="map"/>
|
||||
<param name="type" type="int" value="8"/>
|
||||
</node>
|
||||
|
||||
<node if="$(arg with_preferred_zones)" name="preferred_zones_map_server" pkg="map_server" type="map_server" args="$(arg preferred_zones_map_file)" ns="/preferred_zones" output="screen">
|
||||
<param name="frame_id" type="string" value="map"/>
|
||||
<param name="type" type="int" value="8"/>
|
||||
</node>
|
||||
|
||||
<node if="$(arg with_unpreferred_zones)" name="unpreferred_zones_map_server" pkg="map_server" type="map_server" args="$(arg unpreferred_zones_map_file)" ns="/unpreferred_zones" output="screen">
|
||||
<param name="frame_id" type="string" value="map"/>
|
||||
<param name="type" type="int" value="8"/>
|
||||
</node>
|
||||
|
||||
<node if="$(arg with_critical_zones)" name="critical_zones_map_server" pkg="map_server" type="map_server" args="$(arg critical_zones_map_file)" ns="/critical_zones" output="screen">
|
||||
<param name="frame_id" type="string" value="map"/>
|
||||
<param name="type" type="int" value="8"/>
|
||||
</node>
|
||||
|
||||
<node if="$(arg with_direction_zones)" name="direction_zones_map_server" pkg="map_server" type="map_server" args="$(arg direction_zones_map_file)" ns="/direction_zones" output="screen">
|
||||
<param name="frame_id" type="string" value="map"/>
|
||||
<param name="type" type="int" value="32"/>
|
||||
</node>
|
||||
</launch>
|
||||
89
mir_robot/mir_navigation/launch/start_planner.launch
Executable file
89
mir_robot/mir_navigation/launch/start_planner.launch
Executable file
@@ -0,0 +1,89 @@
|
||||
<launch>
|
||||
<arg name="local_planner" default="dwb" doc="Local planner can be either dwa, dwb, eband, base, teb or pose" />
|
||||
<arg name="map_file" default="" doc="Path to a map .yaml file (required)." />
|
||||
<arg name="empty_file" default="" doc="Path to a map .yaml file (required)." />
|
||||
|
||||
<arg name="virtual_walls_map_file" default="$(arg map_file)" doc="Path to a virtual walls map .yaml file (optional)." />
|
||||
<arg name="with_virtual_walls" default="true" />
|
||||
|
||||
<arg name="preferred_zones_map_file" default="$(arg empty_file)" doc="Path to a preferred zones .yaml file (optional)." />
|
||||
<arg name="with_preferred_zones" default="true" />
|
||||
|
||||
<arg name="unpreferred_zones_map_file" default="$(arg empty_file)" doc="Path to a unpreferred zones .yaml file (optional)." />
|
||||
<arg name="with_unpreferred_zones" default="true" />
|
||||
|
||||
<arg name="critical_zones_map_file" default="$(arg empty_file)" doc="Path to a critical zones .yaml file (optional)." />
|
||||
<arg name="with_critical_zones" default="true" />
|
||||
|
||||
<arg name="direction_zones_map_file" default="$(arg empty_file)" doc="Path to a direction zones .yaml file (optional)." />
|
||||
<arg name="with_direction_zones" default="true" />
|
||||
|
||||
<arg name="prefix" default="" />
|
||||
<arg name="namespace" default="$(arg prefix)" doc="Namespace to push all topics into."/>
|
||||
|
||||
<group if="$(eval namespace != '')" ns="$(arg namespace)">
|
||||
<include file="$(find mir_navigation)/launch/start_maps.launch">
|
||||
<arg name="map_file" value="$(arg map_file)" />
|
||||
<arg name="empty_file" value="$(arg empty_file)" />
|
||||
<arg name="virtual_walls_map_file" value="$(arg virtual_walls_map_file)" />
|
||||
<arg name="with_virtual_walls" value="$(arg with_virtual_walls)" />
|
||||
<!-- Đang thử nghiệm -->
|
||||
<arg name="preferred_zones_map_file" default="$(arg preferred_zones_map_file)" doc="Path to a preferred zones .yaml file (optional)." />
|
||||
<arg name="with_preferred_zones" default="$(arg with_preferred_zones)" />
|
||||
|
||||
<arg name="unpreferred_zones_map_file" default="$(arg unpreferred_zones_map_file)" doc="Path to a unpreferred zones .yaml file (optional)." />
|
||||
<arg name="with_unpreferred_zones" default="$(arg with_unpreferred_zones)" />
|
||||
|
||||
<arg name="critical_zones_map_file" default="$(arg critical_zones_map_file)" doc="Path to a critical zones .yaml file (optional)." />
|
||||
<arg name="with_critical_zones" default="$(arg with_critical_zones)" />
|
||||
|
||||
<arg name="direction_zones_map_file" default="$(arg direction_zones_map_file)" doc="Path to a direction zones .yaml file (optional)." />
|
||||
<arg name="with_direction_zones" default="$(arg with_direction_zones)" />
|
||||
</include>
|
||||
|
||||
<include file="$(find mir_navigation)/launch/move_base.xml">
|
||||
<arg name="local_planner" value="$(arg local_planner)"/>
|
||||
<arg name="with_virtual_walls" value="$(arg with_virtual_walls)" />
|
||||
<!-- Đang thử nghiệm -->
|
||||
<arg name="with_preferred_zones" default="$(arg with_preferred_zones)" />
|
||||
<arg name="with_unpreferred_zones" default="$(arg with_unpreferred_zones)" />
|
||||
<arg name="with_critical_zones" default="$(arg with_critical_zones)" />
|
||||
<arg name="with_direction_zones" default="$(arg with_direction_zones)" />
|
||||
<arg name="prefix" value="$(arg prefix)" />
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- Duplicate of the above in case namespace is empty. This is necessary to
|
||||
avoid the "<group> tag has an empty 'ns' attribute" parsing error. -->
|
||||
<group unless="$(eval namespace != '')">
|
||||
<include file="$(find mir_navigation)/launch/start_maps.launch">
|
||||
<arg name="map_file" value="$(arg map_file)" />
|
||||
<arg name="empty_file" value="$(arg empty_file)" />
|
||||
<arg name="virtual_walls_map_file" value="$(arg virtual_walls_map_file)" />
|
||||
<arg name="with_virtual_walls" value="$(arg with_virtual_walls)" />
|
||||
<!-- Đang thử nghiệm -->
|
||||
<arg name="preferred_zones_map_file" default="$(arg preferred_zones_map_file)" doc="Path to a preferred zones .yaml file (optional)." />
|
||||
<arg name="with_preferred_zones" default="$(arg with_preferred_zones)" />
|
||||
|
||||
<arg name="unpreferred_zones_map_file" default="$(arg unpreferred_zones_map_file)" doc="Path to a unpreferred zones .yaml file (optional)." />
|
||||
<arg name="with_unpreferred_zones" default="$(arg with_unpreferred_zones)" />
|
||||
|
||||
<arg name="critical_zones_map_file" default="$(arg critical_zones_map_file)" doc="Path to a critical zones .yaml file (optional)." />
|
||||
<arg name="with_critical_zones" default="$(arg with_critical_zones)" />
|
||||
|
||||
<arg name="direction_zones_map_file" default="$(arg direction_zones_map_file)" doc="Path to a direction zones .yaml file (optional)." />
|
||||
<arg name="with_direction_zones" default="$(arg with_direction_zones)" />
|
||||
</include>
|
||||
|
||||
<include file="$(find mir_navigation)/launch/move_base.xml">
|
||||
<arg name="local_planner" value="$(arg local_planner)"/>
|
||||
<arg name="with_virtual_walls" value="$(arg with_virtual_walls)" />
|
||||
<!-- Đang thử nghiệm -->
|
||||
<arg name="with_preferred_zones" default="$(arg with_preferred_zones)" />
|
||||
<arg name="with_unpreferred_zones" default="$(arg with_unpreferred_zones)" />
|
||||
<arg name="with_critical_zones" default="$(arg with_critical_zones)" />
|
||||
<arg name="with_direction_zones" default="$(arg with_direction_zones)" />
|
||||
<arg name="prefix" value="$(arg prefix)" />
|
||||
</include>
|
||||
</group>
|
||||
</launch>
|
||||
Reference in New Issue
Block a user