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