RVIZ HECTOR SLAM

edit

Hello everybody,

I am also trying to use the full Hector_slam package (simulation on Gazebo). Nevertheless, as soon as I set the ground_truth_node to false when spawning the quadrotor, I get highly unstable result. Here is my launch file:

    <?xml version="1.0"?>

<launch>

  <!-- Start Gazebo with wg world running in (max) realtime -->
  <include file="$(find hector_gazebo_worlds)/launch/willow_garage.launch"/>

  <!-- Spawn simulated quadrotor uav -->


              <arg name="name" default="quadrotor"/>

              <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
              <arg name="controller_definition" default="$(find hector_quadrotor_controller)/launch/controller.launch"/>
              <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro"/> 

              <arg name="x" default="0.0"/>
              <arg name="y" default="0.0"/>
              <arg name="z" default="0.3"/>

              <arg name="use_ground_truth_for_tf" default="false" />
              <arg name="use_ground_truth_for_control" default="false" />
              <arg name="use_pose_estimation" default="true"/>

              <arg name="world_frame" default="/map"/> <!-- This should actually be "/world". See https://github.com/ros-simulation/gazebo_ros_pkgs/pull/324 -->
              <arg name="base_link_frame" default="$(arg tf_prefix)/base_link"/>

              <!-- send the robot XML to param server -->
              <param name="robot_description" command="$(find xacro)/xacro '$(arg model)' base_link_frame:=$(arg base_link_frame) world_frame:=$(arg world_frame)" />
              <param name="tf_prefix" type="string" value="$(arg tf_prefix)" />
              <param name="base_link_frame" type="string" value="$(arg base_link_frame)"/>
              <param name="world_frame" type="string" value="$(arg world_frame)"/>

              <!-- push robot_description to factory and spawn robot in gazebo -->
              <node name="spawn_robot" pkg="gazebo_ros" type="spawn_model"
                args="-param robot_description
                   -urdf
                   -x $(arg x)
                   -y $(arg y)
                   -z $(arg z)
                   -model $(arg name)"
                respawn="false" output="screen"/>

              <!-- start robot state publisher -->
              <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" >
                <param name="publish_frequency" type="double" value="50.0" />
              </node>

              <!-- publish state and tf
              <node name="ground_truth_to_tf" pkg="message_to_tf" type="message_to_tf" output="screen">
                <param name="odometry_topic" value="ground_truth/state" />
                <param name="frame_id" value="$(arg world_frame)" />
                <param name="tf_prefix" value="$(arg tf_prefix)" if="$(arg use_ground_truth_for_tf)" />
                <param name="tf_prefix" value="$(arg tf_prefix)/ground_truth" unless="$(arg use_ground_truth_for_tf)" />
              </node>  -->

              <group if="$(arg use_pose_estimation)">
                <node name="pose_estimation" pkg="hector_quadrotor_pose_estimation" type="hector_quadrotor_pose_estimation" output="screen">
                  <rosparam file="$(find hector_quadrotor_pose_estimation)/params/simulation.yaml" />
                  <param name="nav_frame" value="nav" />
                  <param name="publish_world_nav_transform" value="false" />
                  <param name="tf_prefix" value="$(arg tf_prefix)" unless="$(arg use_ground_truth_for_tf)" />
                  <param name="tf_prefix" value="$(arg tf_prefix)/pose_estimation" if="$(arg use_ground_truth_for_tf)" />
                </node>
              </group>

              <!-- spawn controller -->
              <group if="$(arg use_ground_truth_for_control)">
                <param name="controller/state_topic" value="" />
                <param name="controller/imu_topic" value="" />
              </group>
              <group unless="$(arg use_ground_truth_for_control)">
                <param name="controller/state_topic" value="state" />
                <param name="controller/imu_topic" value="imu" />
              </group>
              <include file="$(arg controller_definition)" />

              <arg name="motors" default="robbe_2827-34_epp1045" />
              <rosparam command="load" file="$(find hector_quadrotor_model)/param/quadrotor_aerodynamics.yaml" />
              <rosparam command="load" file="$(find hector_quadrotor_model)/param/$(arg motors).yaml" />

<!-- Start SLAM system -->
              <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatch_frame"/>
              <arg name="base_frame" default="base_footprint"/>
              <arg name="odom_frame" default="nav"/>

              <arg name="pub_map_odom_transform" default="true"/>
              <arg name="pub_map_scanmatch_transform" value="true" />

              <arg name="scan_subscriber_queue_size" default="5"/>
              <arg name="scan_topic" default="scan"/>
              <arg name="map_size" default="2048"/>

              <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="false"/>
                <param name="pub_map_odom_transform ...