Robotics StackExchange | Archived questions

ROS Navigation stack setup (Costmap2DROS transform timeout. Current time: 560.5860, global_pose stamp: 0.0000, tolerance: 0.3000)

I am currently working in four wheeled mecanum robot. So, to start with the navigation I created the simulation robot that has been described with simple geometric shapes.

The URDF file of the robot is the following,

<robot name="ROSBOT">

<gazebo>
    <plugin name="mecanum_drive" filename="libgazebo_ros_planar_move.so">
      <commandTopic>cmd_vel</commandTopic>
      <odometryTopic>odom</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <odometryRate>50.0</odometryRate>
      <robotBaseFrame>base_footprint</robotBaseFrame>
      <publishTF>false</publishTF>
    </plugin>
 </gazebo>

<gazebo reference="laser">
    <sensor type="ray" name="lds_lfcd_sensor">
      <pose>
        <x>0.0</x>
        <y>0.0</y>
        <z>0.0</z>
        <r>0.0</r>
        <p>0.0</p>
        <y>0.0</y>
      </pose>
      <visualize>true</visualize>
      <update_rate>50</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>0.0</min_angle>
            <max_angle>6.28319</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.120</min>
          <max>3.5</max>
          <resolution>0.015</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_lds_lfcd_controller" filename="libgazebo_ros_laser.so">
        <topicName>scan</topicName>
        <frameName>laser</frameName>
      </plugin>
    </sensor>
  </gazebo>
<gazebo reference="base_link">
    <material>Gazebo/Yellow</material>
</gazebo>

<gazebo reference="front_right_wheel_link">
    <material>Gazebo/Green</material>
    <mu1 value="0.1"/>
    <mu2 value="0."/>
    <kp value="50000.0" />
    <kd value="10.0" />
    <fdir1 value="1 0 0"/>
    <minDepth>0.001</minDepth>
    <maxVel>0.1</maxVel>  
</gazebo>

<gazebo reference="front_left_wheel_link">
    <material>Gazebo/Green</material>
    <mu1 value="0.1"/>
    <mu2 value="0."/>
    <kp value="50000.0" />
    <kd value="10.0" />
    <fdir1 value="1 0 0"/>
    <minDepth>0.001</minDepth>
    <maxVel>0.1</maxVel>  
</gazebo>

<gazebo reference="back_right_wheel_link">
    <material>Gazebo/Green</material>
    <mu1 value="0.1"/>
    <mu2 value="0."/>
    <kp value="50000.0" />
    <kd value="10.0" />
    <fdir1 value="1 0 0"/>
    <minDepth>0.001</minDepth>
    <maxVel>0.1</maxVel>  
</gazebo>

<gazebo reference="back_left_wheel_link">
    <material>Gazebo/Green</material>
    <mu1 value="0.1"/>
    <mu2 value="0."/>
    <kp value="50000.0" />
    <kd value="10.0" />
    <fdir1 value="1 0 0"/>
    <minDepth>0.001</minDepth>
    <maxVel>0.1</maxVel>  
</gazebo>

    <link name="map"/>
    <link name="odom"/>

    <joint name="map_odom" type="fixed">
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <parent link="map"/>
        <child link="odom"/>
    </joint>

    <link name="base_footprint"/>

<joint name="odom_base_footprint" type="fixed">
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <parent link="odom"/>
        <child link="base_footprint"/>
    </joint>

    <material name="yellow">
        <color rgba="0 1 1 1"/>
    </material>

    <material name="green">
        <color rgba="0 1 0 1"/>
    </material>

    <link name="base_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <box size="0.45 0.30 0.05"/>
            </geometry>
            <material name="yellow"/>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <box size="0.45 0.30 0.05"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <mass value="2.6"/>
            <inertia ixx="0.020041666666666666" ixy="0.0" ixz="0.0" iyy="0.04441666666666667" iyz="0.0" izz="0.063375" />
        </inertial>
    </link>

    <joint name="base_footprint_base_link_joint" type="fixed">
        <origin xyz="0 0 0.05" rpy="0 0 0"/>
        <parent link="base_footprint"/>
        <child link="base_link"/>
    </joint>

    <link name="front_left_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707963267948966 0 0"/>
            <geometry>
                <cylinder radius="0.05" length="0.05"/>
            </geometry>
            <material name="green"/>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="1.5707963267948966 0 0"/>
            <geometry>
                <cylinder radius="0.05" length="0.05"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0 0 0" rpy="1.5707963267948966 0 0"/>
            <mass value="0.4"/>
            <inertia ixx="0.00033333333333333343" ixy="0.0" ixz="0.0" iyy="0.00033333333333333343" iyz="0.0" izz="0.0005000000000000001"/>
        </inertial>
    </link>

    <joint name="front_left_wheel_base_link_joint" type="continuous">
        <origin xyz="0.185 0.190 0.0" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="front_left_wheel_link"/>
        <axis xyz="0 1 0"/>
        <limit effort="30.0" velocity="10.0"/>
    </joint>



    <link name="front_right_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707963267948966 0 0"/>
            <geometry>
                <cylinder radius="0.05" length="0.05"/>
            </geometry>
            <material name="green"/>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="1.5707963267948966 0 0"/>
            <geometry>
                <cylinder radius="0.05" length="0.05"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0 0 0" rpy="1.5707963267948966 0 0"/>
            <mass value="0.4"/>
            <inertia ixx="0.00033333333333333343" ixy="0.0" ixz="0.0" iyy="0.00033333333333333343" iyz="0.0" izz="0.0005000000000000001"/>
        </inertial>
    </link>

    <joint name="front_right_wheel_base_link_joint" type="continuous">
        <origin xyz="0.185 -0.190 0.0" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="front_right_wheel_link"/>
        <axis xyz="0 1 0"/>
        <limit effort="30.0" velocity="10.0"/>
    </joint>



    <link name="back_right_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707963267948966 0 0"/>
            <geometry>
                <cylinder radius="0.05" length="0.05"/>
            </geometry>
            <material name="green"/>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="1.5707963267948966 0 0"/>
            <geometry>
                <cylinder radius="0.05" length="0.05"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0 0 0" rpy="1.5707963267948966 0 0"/>
            <mass value="0.4"/>
            <inertia ixx="0.00033333333333333343" ixy="0.0" ixz="0.0" iyy="0.00033333333333333343" iyz="0.0" izz="0.0005000000000000001"/>
        </inertial>
    </link>

    <joint name="back_right_wheel_base_link_joint" type="continuous">
        <origin xyz="-0.185 -0.190 0.0" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="back_right_wheel_link"/>
        <axis xyz="0 1 0"/>
        <limit effort="30.0" velocity="10.0"/>
    </joint>



    <link name="back_left_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707963267948966 0 0"/>
            <geometry>
                <cylinder radius="0.05" length="0.05"/>
            </geometry>
            <material name="green"/>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="1.5707963267948966 0 0"/>
            <geometry>
                <cylinder radius="0.05" length="0.05"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0 0 0" rpy="1.5707963267948966 0 0"/>
            <mass value="0.4"/>
            <inertia ixx="0.00033333333333333343" ixy="0.0" ixz="0.0" iyy="0.00033333333333333343" iyz="0.0" izz="0.0005000000000000001"/>
        </inertial>
    </link>

    <joint name="back_left_wheel_base_link_joint" type="continuous">
        <origin xyz="-0.185 0.190 0.0" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="back_left_wheel_link"/>
        <axis xyz="0 1 0"/>
        <limit effort="30.0" velocity="10.0"/>
    </joint>



    <link name="laser">
        <visual>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <geometry>
                <mesh filename="package://my_urdf_tutorial/meshes/lidar.dae"/>
            </geometry>
            <material name="white">
                <color rgba="1 1 1 1"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh filename="package://my_urdf_tutorial/meshes/lidar.dae"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <mass value="0.25"/>
            <inertia ixx="0.00017916666666666667" ixy="0.0" ixz="0.0" iyy="0.00017916666666666667" iyz="0.0" izz="0.000225"/>
        </inertial>
    </link>

    <joint name="laser_sensor_base_link_joint" type="fixed">
        <origin xyz="0.175 0.0 0.045" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="laser"/>
    </joint>

</robot>

I am able to map the environment in simulation environment in gazebo. I, used hector_slam for mapping.

I am using amcl node for localisation.

The problem is after launching the navigationstack, with the map initially loaded in the mapserver, costmap is not getting loaded and ultimately move_base is not able to plan the path.

I am providing the parameters file that I used in the navigation stack.

Commoncostmapparameters:

footprint : [[0.0, 0.15], [0.225,0.0], [-0.225, 0.0], [0.0, -0.15]]
map_type: costmap
obstacle_range: 2.0
raytrace_range: 3.0
inflation_radius: 0.35
observation_sources: laser_scan_sensor 
laser_scan_sensor:
  data_type: LaserScan
  topic: scan
  marking: true

Global costmap parameters: globalcostmap: globalframe: map robotbaseframe: basefootprint updatefrequency: 10.0 staticmap: true clearing: true sensorframe: laser

Local costmap parameters:

local_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  update_frequency: 10.0
  publish_frequency: 10.0 
  static_map: false  
  rolling_window: true
  width: 3
  height: 3
  resolution: 0.05

Parameters that I used for the move_base is the following,

Local planner parameters:

TrajectoryPlannerROS:
    holonomic_robot: true
    meter_scoring: true
    max_vel_x: 0.5
    min_vel_x: -0.5
    max_vel_y: 0.5
    min_vel_y: -0.5
    max_trans_vel: 0.35
    min_trans_vel: -0.15
    max_vel_theta: 3.5
    min_vel_theta: -1.5
    acc_lim_x: 1.0
    acc_lim_y: 1.0
    acc_lim_theta: 2.0
    yaw_goal_tolerance: 0.1
    xy_goal_tolerance: 0.15
    sim_time: 1.0
    vx_samples: 20
    vy_samples: 15
    vtheta_samples: 20
    controller_frequency: 40
    meter_scoring: true
    path_distance_bias: 64
    goal_distance_bias: 24
    occdist_scale: 0.5
    forward_point_distance: 0.325
    stop_time_buffer: 0.2
    scaling_speed: 0.25
    max_scaling_factor: 0.2
    oscillaton_reset_dist: 0.05
    publish_traj_pc: true
    publish_cost_grid_pc: true
    global_frame_id: map
    prune_plan: false

Global Planner parameters: GlobalPlanner: # Also see: http://wiki.ros.org/global_planner

old_navfn_behavior: false                     # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false
  use_quadratic: true                           # Use the quadratic approximation of the potential. Otherwise, use a simpler calculation, default true
  use_dijkstra: false                           # Use dijkstra's algorithm. Otherwise, A*, default true
  use_grid_path: false                          # Create a path that follows the grid boundaries. Otherwise, use a gradient descent method, default false
  allow_unknown: true                           # Allow planner to plan through unknown space, default true
  planner_window_x: 0.0                         # default 0.0
  planner_window_y: 0.0                         # default 0.0
  default_tolerance: 0.0                        # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0

  publish_scale: 100                            # Scale by which the published potential gets multiplied, default 100
  planner_costmap_publish_frequency: 0.0        # default 0.0

  lethal_cost: 253                              # default 253
  neutral_cost: 50                              # default 50
  cost_factor: 3.0                              # Factor to multiply each cost from costmap by, default 3.0
  publish_potential: true                       # Publish Potential Costmap (this is not like the navfn pointcloud2 potential), default  
  visualize_potential: false

Common parameters that I use for move_base node:

base_global_planner: global_planner/GlobalPlanner
base_local_planner: base_local_planner/TrajectoryPlannerROS
shutdown_costmaps: false
controller_frequency: 20.0
planner_patience: 5.0
controller_patience: 15.0
recovery_behavior_enabled : true
conservative_reset_dist: 3.0
planner_frequency: 0.0
oscillation_timeout: 10.0
oscillation_distance: 0.2 

The following are the launch files that I used, Loading the urdf and starting the jointstatepublisher, robotstatepublisher

<launch>
    <param name="robot_description" command="cat $(find my_urdf_tutorial)/urdf/rosbot_simple_geometry.urdf"/>

    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
        <param name="use_gui" value="false"/>
    </node>

    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

    <!--node name="rviz" pkg="rviz" type="rviz" args="-d $(find my_urdf_tutorial)/rviz_config/sample_rosbot.rviz"/-->
</launch>

Loading the gazebo world:

<launch>
  <arg name="model" default="ROSBOT"/>
  <arg name="x_pos" default="-2.0"/>
  <arg name="y_pos" default="-0.5"/>
  <arg name="z_pos" default="0.0"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_world.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <param name="robot_description" command="cat '$(find my_urdf_tutorial)/urdf/rosbot_simple_geometry.urdf'"/>

  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model $(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />
</launch>

Launching the amcl node:

<launch>
  <!-- Arguments -->
  <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"/>

  <!-- AMCL -->
  <node pkg="amcl" type="amcl" name="amcl">

    <param name="min_particles"             value="500"/>
    <param name="max_particles"             value="3000"/>
    <param name="kld_err"                   value="0.02"/>
    <param name="update_min_d"              value="0.20"/>
    <param name="update_min_a"              value="0.20"/>
    <param name="resample_interval"         value="1"/>
    <param name="transform_tolerance"       value="0.5"/>
    <param name="recovery_alpha_slow"       value="0.00"/>
    <param name="recovery_alpha_fast"       value="0.00"/>
    <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)"/>
    <param name="gui_publish_rate"          value="50.0"/>

    <remap from="scan"                      to="$(arg scan_topic)"/>
    <param name="laser_max_range"           value="3.5"/>
    <param name="laser_max_beams"           value="180"/>
    <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_likelihood_max_dist" value="2.0"/>
    <param name="laser_model_type"          value="likelihood_field"/>
    <param name="first_map_only"            value="false"/>

    <param name="odom_model_type"           value="omni"/>
    <param name="odom_alpha1"               value="0.1"/>
    <param name="odom_alpha2"               value="0.1"/>
    <param name="odom_alpha3"               value="0.1"/>
    <param name="odom_alpha4"               value="0.1"/>
    <param name="odom_frame_id"             value="odom"/>
    <param name="base_frame_id"             value="base_footprint"/>
    <param name="tf_broadcast"              value="true"/>

  </node>
</launch>

Launching the move_base node:

<launch>
  <!-- Arguments -->
  <arg name="model" default="ROSBOT"/>
  <arg name="cmd_vel_topic" default="/cmd_vel" />
  <arg name="odom_topic" default="odom" />
  <arg name="move_forward_only" default="false"/>

  <!-- move_base -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <!--param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" /-->
    <rosparam file="$(find my_urdf_tutorial)/param/costmap/common_costmap_params_general_meca.yaml" command="load" ns="global_costmap"/>
    <rosparam file="$(find my_urdf_tutorial)/param/costmap/common_costmap_params_general_meca.yaml" command="load" ns="local_costmap"/>
    <rosparam file="$(find my_urdf_tutorial)/param/costmap/local_costmap_params_meca.yaml" command="load"/>
    <rosparam file="$(find my_urdf_tutorial)/param/costmap/global_costmap_params_meca.yaml" command="load"/>
    <rosparam file="$(find my_urdf_tutorial)/param/planner/move_base_params_meca.yaml" command="load"/>
    <rosparam file="$(find my_urdf_tutorial)/param/planner/local_planner_params_meca.yaml" command="load"/>
      <rosparam file="$(find my_urdf_tutorial)/param/planner/global_planner_params_meca.yaml" command="load"/>
    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
    <remap from="odom" to="$(arg odom_topic)"/>
    <!--param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" /-->
  </node>
</launch>

Loading the map in the mapserver and a launch file for starting the amcl and movebase(Only this file is launched, i have provided the move_base and amcl launch files for reference)

<launch>

    <!--Starting the map in the map_server-->
    <node name="map_server" type="map_server" pkg="map_server" args="$(find my_urdf_tutorial)/maps/map.yaml"/>

    <!--AMCL for localisation-->
    <include file="$(find amcl)/examples/amcl_omni.launch"/>

    <!--MOVE_BASE for path planning and necessary velocity commands-->
    <include file="$(find my_urdf_tutorial)/launch/move_base_launch.launch"/>

</launch>

The following links provide the image files for reference,

Output seen in the rviz: https://ibb.co/j5ny46Q

It is seen in RVIZ that global and local costmap is not received.

Information in the terminal after launching the whole ros navigation stack: https://ibb.co/SrY0hXb

I am not able to understand the warning message.

Results that i got on running roswtf command: https://ibb.co/8MVt8Vj

It is showing that local and global costmap nodes are not connected, but I don't know how to solve this error

TF tree: https://ibb.co/sJ7cjRB

Transforms for all the wheel frames are available. But for the laser frame is not available. Also map --> odom --> basefootprint --> baselink is not getting the transform.

Guidance needed to solve this....

Asked by Irudhaya on 2019-11-12 03:06:38 UTC

Comments

Hi! Have you solved the problem? I meet the same problem" Costmap2DROS transform timeout. Current time: 162.4450, global_pose stamp: 0.0000, tolerance: 1.0000" I don't know how to solve it.

Asked by michaellb on 2021-02-23 09:16:56 UTC

Answers