ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

answered 2015-12-07 21:14:12 -0500

quentin gravatar image

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>

<include file="$(find hector_gazebo_worlds)/launch/willow_garage.launch"/>

          <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" />

<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" value="$(arg pub_map_odom_transform)"/>

            <!-- 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="1" />

            <!-- 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_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)"/>

            <!-- Debug parameters -->
            <!--
            -->
              <param name="output_timing" value="false"/>
              <param name="pub_drawings" value="true"/>
              <param name="pub_debug_output" value="true"/>

            <param name="pub_map_scanmatch_transform" value="$(arg pub_map_scanmatch_transform)" />
            <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
          </node>

          <!-- <node pkg="tf" type="static_transform_publisher" name="map_nav_broadcaster" args="0 0 0 0 0 0 scanmatch_frame nav 100"/> -->

<!-- Hector trajectory --> 

          <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="hector_trajectory_server" output="screen">
            <param name="target_frame_name" type="string" value="/map" />
            <param name="source_frame_name" type="string" value="/base_link" />
            <param name="trajectory_update_rate" type="double" value="4" />
            <param name="trajectory_publish_rate" type="double" value="0.25" />
          </node>

<node pkg="rviz" type="rviz" name="rviz" args="-d $(find hector_quadrotor_demo)/rviz_cfg/Working_test_1.rviz"/>

</launch>

This is the error I get in the terminal: 

SearchDir angle change too large

My roswtf returns: 

Beginning tests of your ROS graph. These may take awhile...

analyzing graph... ... done analyzing graph running graph rules... ... done running graph rules running tf checks, this will take a second... ... tf checks complete

Online checks summary:

Found 2 warning(s). Warnings are things that may be just fine, but are sometimes at fault WARNING The following node subscriptions are unconnected: * /robot_state_publisher: * /joint_states * /pose_estimation: * /rollpitch * /twistupdate * /ahrs * /syscommand * /rviz: * /tf_static * /particlecloud * /map_updates * /hector_mapping: * /tf_static * /syscommand * /hector_trajectory_server: * /tf_static * /syscommand * /gazebo: * /gazebo/set_link_state * /motor_pwm * /command/twist * /cmd_vel * /wind * /gazebo/set_model_state

WARNING These nodes have died: * spawn_robot-4

Does anybody have an idea?

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>

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

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"/>

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" value="$(arg pub_map_odom_transform)"/>

             <!-- 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="1" />

             <!-- 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_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)"/>

             <!-- Debug parameters -->
             <!--
             -->
               <param name="output_timing" value="false"/>
               <param name="pub_drawings" value="true"/>
               <param name="pub_debug_output" value="true"/>

             <param name="pub_map_scanmatch_transform" value="$(arg pub_map_scanmatch_transform)" />
             <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
           </node>

           <!-- <node pkg="tf" type="static_transform_publisher" name="map_nav_broadcaster" args="0 0 0 0 0 0 scanmatch_frame nav 100"/> -->

 <!-- Hector trajectory --> 

           <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="hector_trajectory_server" output="screen">
             <param name="target_frame_name" type="string" value="/map" />
             <param name="source_frame_name" type="string" value="/base_link" />
             <param name="trajectory_update_rate" type="double" value="4" />
             <param name="trajectory_publish_rate" type="double" value="0.25" />
           </node>

<!-- Start GeoTIFF mapper <include file="$(find hector_geotiff)/launch/geotiff_mapper.launch"> <arg name="trajectory_publish_rate" value="4"/> </include> --> <!-- Start rviz visualization with preset config --> <node pkg="rviz" type="rviz" name="rviz" args="-d $(find hector_quadrotor_demo)/rviz_cfg/Working_test_1.rviz"/>

</launch>

hector_quadrotor_demo)/rviz_cfg/Working_test_1.rviz"/> </launch>

This is the error I get in the terminal: 

 SearchDir angle change too large

[ WARN] [1449543969.848957815, 5.550000000]: Update for yaw with error [ -0.451888 ], |error| = 10.7943 sigma exceeds max_error!

My roswtf returns: 

 Beginning tests of your ROS graph. These may take awhile...

analyzing graph... ... done analyzing graph running graph rules... ... done running graph rules running tf checks, this will take a second... ... tf checks complete

complete Online checks summary:

summary: Found 2 warning(s). Warnings are things that may be just fine, but are sometimes at fault WARNING The following node subscriptions are unconnected: * /robot_state_publisher: * /joint_states * /pose_estimation: * /rollpitch * /twistupdate * /ahrs * /syscommand * /rviz: * /tf_static * /particlecloud * /map_updates * /hector_mapping: * /tf_static * /syscommand * /hector_trajectory_server: * /tf_static * /syscommand * /gazebo: * /gazebo/set_link_state * /motor_pwm * /command/twist * /cmd_vel * /wind * /gazebo/set_model_state

/gazebo/set_model_state WARNING These nodes have died: * spawn_robot-4spawn_robot-4

Here is my tf tree: link text

And my node graph: link text

Does anybody have an idea?

ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
about | faq | help | privacy policy | terms of service
Powered by Askbot version 0.10.2