rospibot6 robot6.launch

rospibot6 robot6.launch

<launch>

<arg name="use_map_topic" default="false"/>
<arg name="scan_topic" default="scan"/>
<arg name="initial_pose_x" default="0.0"/>
<arg name="initial_pose_y" default="0.0"/>
<arg name="initial_pose_a" default="0.0"/>

<rosparam param="base_width">0.1928</rosparam>
<rosparam param="ticks_meter">21140</rosparam>

<node name="diff_tf" pkg="rospibot6" type="diff_tf.py" output="screen">

<rosparam param="ticks_meter">21140</rosparam>
<rosparam param="base_width">0.1928</rosparam>
<rosparam param="base_frame_id">base_link</rosparam>
<rosparam param="odom_frame_id">odom</rosparam>
<rosparam param="rate">50</rosparam>
</node>

<node pkg="tf" type="static_transform_publisher" name="neato_laser_tf" args="0.0 0 0.10 0 0 0 /base_link /neato_laser 50" />

<node name="xv11" pkg="xv_11_laser_driver" type="neato_laser_publisher" respawn="true" output="screen">
<param name="port" value="/dev/ttyAMA0"/>
<!--<param name="firmware_version" value="2"/>-->
</node>

<node name="gmap" pkg="gmapping" type="slam_gmapping" respawn="true" respawn_delay="30" output="screen">
<param name="linearUpdate" type="double" value="0.0" />
<param name="angularUpdate" type="double" value="0.0" />
<param name="map_update_interval" type="double" value="5.0" />
<param name="throttle_scans" type="int" value="1" />

<!-- map size & resolution-->
<param name="xmin" type="double" value="-20.0" />
<param name="ymin" type="double" value="-20.0" />
<param name="xmax" type="double" value="20.0" />
<param name="ymax" type="double" value="20.0" />
<param name="delta" type="double" value="0.05" />

<!-- odometry error -->
<param name="srr" type="double" value="0.01" />
<param name="srt" type="double" value="0.02" />
<param name="str" type="double" value="0.01" />
<param name="stt" type="double" value="0.02" />
</node>

<node name="move_base" pkg="move_base" type="move_base" respawn="false" output="screen">
<rosparam file="$(find rospibot6)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find rospibot6)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rospibot6)/param/local_costmap_params.yaml" command="load" />
<rosparam file="$(find rospibot6)/param/global_costmap_params.yaml" command="load" />
<rosparam file="$(find rospibot6)/param/base_local_planner_params.yaml" command="load" />
<rosparam file="$(find rospibot6)/param/dwa_local_planner_params.yaml" command="load" />
<rosparam file="$(find rospibot6)/param/move_base_params.yaml" command="load" />
</node>

<!--
<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
<param name="output_frame" value="odom"/>
<param name="freq" value="30.0"/>
<param name="sensor_timeout" value="1.0"/>
<param name="odom_used" value="true"/>
<param name="imu_used" value="true"/>
<param name="vo_used" value="false"/>
<param name="debug" value="true"/>
<param name="self_diagnose" value="true"/>
</node>
-->

<node pkg="amcl" type="amcl" name="amcl">
<param name="use_map_topic" value="$(arg use_map_topic)"/>
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="60"/>
<param name="laser_max_range" value="12.0"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="2000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.2"/>
<param name="odom_alpha4" value="0.2"/>
<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_model_type" value="beam"/> -->
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.25"/>
<param name="update_min_a" value="0.2"/>
<param name="odom_frame_id" value="odom"/>
<param name="base_frame_id" value="base_footprint"/>
<param name="resample_interval" value="1"/>
<!-- Increase tolerance because the computer can get quite busy -->
<param name="transform_tolerance" value="1.0"/>
<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)"/>
</node>

<node name="base_node" pkg="rospibot6" type="base5" respawn="true" respawn_delay="30" output="screen">
<!--<rosparam file="$(find mypackage)/config/example.yaml" command="load" />-->
<remap from="wheel_left" to="lwheel"/>
<remap from="wheel_right" to="rwheel"/>
<param name="sample_rate" type="int" value="100" />
<param name="pid_enabled" type="bool" value="0" />
<param name="odom_enabled" type="bool" value="0" />
<param name="cmdvel_enabled" type="bool" value="1" />
<param name="cmdvel_k" type="double" value="0.3" />
<!-- BASE -->
<param name="wheel_rad" type="double" value="0.03385" />
<param name="wheel_sep" type="double" value="0.1928" />
<param name="wheel_encoder_pulses" type="double" value="4496.0" />
<!-- PID -->
<param name="pid_out_trh" type="double" value="0.0" />
<param name="pid_out_min" type="double" value="0.0" />
<param name="pid_out_max" type="double" value="64.0" />
<param name="pid_out_k" type="double" value="500.0" />
<param name="pid_sample_time" type="double" value="0.050" />
<param name="pid_setpoint_max" type="double" value="150000.0" />
<param name="pid_left_kp" type="double" value="0.7" />
<param name="pid_left_ki" type="double" value="0.01" />
<param name="pid_left_kd" type="double" value="0.0" />
<param name="pid_right_kp" type="double" value="0.7" />
<param name="pid_right_ki" type="double" value="0.01" />
<param name="pid_right_kd" type="double" value="0.0" />
<!-- IMU -->
<param name="frame_id" type="str" value="base_imu" />
<param name="ax" type="int" value="0" />
<param name="ay" type="int" value="0" />
<param name="az" type="int" value="0" />
<param name="gx" type="int" value="0" />
<param name="gy" type="int" value="0" />
<param name="gz" type="int" value="0" />
<param name="ado" type="bool" value="false" />
<param name="debug" type="bool" value="false" />
</node>

</launch>