0
$\begingroup$

I try to make a differential drives robot with 2 coder at my wheels and a lidar in front of the mobile base.

Currently i can teleop it, i have a good odometry and the laser scan data come correctly to rviz2.

But when i want to add the localization, there is problem. When i '2D pose estimate' it then teleop it, it look like it teleport into the map.

There is a lot of screen of error message, rqt_graph, etc.. below

I think the problem could be with amcl because it don't subscribe to /tf topic.

Thank you per advance for any kind of help !

There is my launch file :

import os

from ament_index_python.packages import get_package_share_directory


from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.events.matchers import matches_node_name
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
import launch_ros.actions


import xacro


def generate_launch_description():
    ld = LaunchDescription()

    robot_description_path = os.path.join(
        get_package_share_directory('ros2_control_demo_example_2'),
        'urdf',
        'diffbot.urdf.xacro')
    robot_description_config = xacro.process_file(robot_description_path)
    robot_description = {'robot_description': robot_description_config.toxml()}

    test_controller = os.path.join(
        get_package_share_directory('ros2_control_demo_example_2'),
        'controllers',
        'base_mobile_controller.yaml')

    remote_modbus_rtu = Node(
        package="remote_modbus_rtu",
        executable="remote_modbus_rtu_standalone",
        parameters=[
            {"modbus_is_remote": False},
            {"serial_is_remote": False},
            {"serial_dev_name": '/dev/ttyUSB0'},
            {"serial_baud_rate": 115200},
            {"serial_parity": True},
            {"serial_data": 8},
            {"serial_stop": 1},
            {"serial_flow_control": True},
            {"modbus_prefix": '/modbus/example_bus'}
        ]
    )

    controller_manager = Node(
        package='controller_manager',
        executable='ros2_control_node',
        parameters=[robot_description, test_controller],
        output={
            'stdout': 'screen',
            'stderr': 'screen',
        },
    )

    sick_safetyscanners2 = Node(
        package="sick_safetyscanners2",
        executable="sick_safetyscanners2_node",
        name="sick_safetyscanners2_node",
        output="screen",
        emulate_tty=True,
        parameters=[
            {"frame_id": "scan",
            "sensor_ip": "10.10.10.1",
            "host_ip": "10.10.10.10",
            "interface_ip": "0.0.0.0",
            "host_udp_port": 6060,
            "channel": 0,
            "channel_enabled": True,
            "skip": 0,
            "angle_start": 0.0,
            "angle_end": 0.0,
            "time_offset": 0.0,
            "general_system_state": True,
            "derived_settings": True,
            "measurement_data": True,
            "intrusion_data": True,
            "application_io_data": True,
            "use_persistent_config": False,
            "min_intensities": 0.0
            }
        ]
    )

    robot_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            PathJoinSubstitution(
                [FindPackageShare("ros2_control_demo_example_2"), "urdf", "diffbot.urdf.xacro"]
            ),
        ]
    )
    robot_description = {"robot_description": robot_description_content}

    robot_state_pub_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        parameters=[robot_description],
        remappings=[
            ("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"),
        ],
    )

    diff_drive_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["diff_controller"],
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster"],
        output='screen',
    )

    static_transform_publisher = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        arguments=['0','0','0','0','0','0','world','base_link'],
        output='screen',
    )

There is my urdf file :

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:macro name="diffbot" params="prefix">

  <!-- Constants for robot dimensions -->
  <xacro:property name="PI" value="3.1415926535897931"/>
  <xacro:property name="base_mass" value="150" /> <!-- arbitrary value for base mass -->
  <xacro:property name="base_width" value="0.700" />
  <xacro:property name="base_length" value="0.480" />
  <xacro:property name="base_height" value="0.2" />
  <xacro:property name="wheel_mass" value="0.3" /> <!-- arbitrary value for wheel mass -->
  <xacro:property name="wheel_len" value="0.050" />
  <xacro:property name="wheel_radius" value="0.095" />
  <xacro:property name="caster_wheel_mass" value="0.1" /> <!-- arbitrary value for caster wheel mass -->
  <xacro:property name="caster_wheel_radius" value="0.04" />
  <xacro:property name="z_offset" value="-0.04" /> <!-- Space btw top of beam and the each joint -->

  <!-- Base Link -->
  <link name="${prefix}base_link">
<collision>
  <origin xyz="0 0 0" rpy="0 0 0"/>
  <geometry>
    <box size="${base_width} ${base_length} ${base_height}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${base_height/2}" rpy="0 0 0"/>
  <geometry>
    <box size="${base_width} ${base_length} ${base_height}"/>
      </geometry>
      <material name="orange"/>
    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${base_mass}"/>
  <inertia
    ixx="${base_mass / 12.0 * (base_length*base_length + base_height*base_height)}" ixy="0.0" ixz="0.0"
        iyy="${base_mass / 12.0 * (base_height*base_height + base_width*base_width)}" iyz="0.0"
    izz="${base_mass / 12.0 * (base_width*base_width + base_length*base_length)}"/>
      </inertial>
    </link>

    <joint name="${prefix}left_wheel_joint" type="continuous">
  <parent link="${prefix}base_link"/>
      <child link="${prefix}left_wheel"/>
      <origin xyz="0 0.215 0.01" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
      <dynamics damping="0.2"/>
    </joint>

    <!-- left wheel Link -->
    <link name="${prefix}left_wheel">
  <collision>
    <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
        <geometry>
          <cylinder length="${wheel_len}" radius="${wheel_radius}"/>
        </geometry>
      </collision>

      <visual>
        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
    <geometry>
      <cylinder length="${wheel_len}" radius="${wheel_radius}"/>
        </geometry>
        <material name="black"/>
      </visual>

      <inertial>
        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
    <mass value="${wheel_mass}"/>
        <inertia
          ixx="${wheel_mass / 12.0 * (3*wheel_radius + wheel_len*wheel_len)}" ixy="0.0" ixz="0.0"
      iyy="${wheel_mass / 12.0 * wheel_radius*wheel_radius}" iyz="0.0"
          izz="${wheel_mass / 12.0 * wheel_radius*wheel_radius}"/>
        </inertial>
      </link>

    <joint name="${prefix}right_wheel_joint" type="continuous">
  <parent link="${prefix}base_link"/>
      <child link="${prefix}right_wheel"/>
      <origin xyz="0 -0.215 0.01" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
      <dynamics damping="0.2"/>
    </joint>

    <!-- right wheel Link -->
    <link name="${prefix}right_wheel">
  <collision>
    <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
        <geometry>
          <cylinder length="${wheel_len}" radius="${wheel_radius}"/>
        </geometry>
      </collision>

      <visual>
        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
    <geometry>
      <cylinder length="${wheel_len}" radius="${wheel_radius}"/>
        </geometry>
        <material name="black"/>
      </visual>

      <inertial>
        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
    <mass value="${wheel_mass}"/>
        <inertia
          ixx="${wheel_mass / 12.0 * (3*wheel_radius + wheel_len*wheel_len)}" ixy="0.0" ixz="0.0"
      iyy="${wheel_mass / 12.0 * wheel_radius*wheel_radius}" iyz="0.0"
          izz="${wheel_mass / 12.0 * wheel_radius*wheel_radius}"/>
        </inertial>
      </link>

    <joint name="${prefix}caster_frontal_right_wheel_joint" type="fixed">
  <parent link="${prefix}base_link"/>
      <child link="${prefix}caster_frontal_right_wheel"/>
  <origin xyz="0.215 0.155 ${z_offset}" rpy="0 0 0"/>
    </joint>

    <!-- caster frontal right wheel Link -->
    <link name="${prefix}caster_frontal_right_wheel">
  <collision>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <sphere radius="${caster_wheel_radius}"/>
        </geometry>
      </collision>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="${caster_wheel_radius}"/>
        </geometry>
        <material name="white"/>
      </visual>

      <inertial>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <mass value="${caster_wheel_mass}"/>
    <inertia
      ixx="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}" ixy="0.0" ixz="0.0"
          iyy="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}" iyz="0.0"
      izz="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}"/>
        </inertial>
      </link>

    <joint name="${prefix}caster_frontal_left_wheel_joint" type="fixed">
  <parent link="${prefix}base_link"/>
      <child link="${prefix}caster_frontal_left_wheel"/>
  <origin xyz="0.215 -0.155 ${z_offset}" rpy="0 0 0"/>
    </joint>

    <!-- caster frontal left wheel Link -->
    <link name="${prefix}caster_frontal_left_wheel">
  <collision>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <sphere radius="${caster_wheel_radius}"/>
        </geometry>
      </collision>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="${caster_wheel_radius}"/>
        </geometry>
        <material name="white"/>
      </visual>

      <inertial>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <mass value="${caster_wheel_mass}"/>
    <inertia
      ixx="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}" ixy="0.0" ixz="0.0"
          iyy="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}" iyz="0.0"
      izz="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}"/>
      </inertial>
    </link>

    <joint name="${prefix}caster_back_right_wheel_joint" type="fixed">
  <parent link="${prefix}base_link"/>
      <child link="${prefix}caster_back_right_wheel"/>
  <origin xyz="-0.215 0.155 ${z_offset}" rpy="0 0 0"/>
    </joint>

    <!-- caster back right wheel Link -->
    <link name="${prefix}caster_back_right_wheel">
  <collision>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <sphere radius="${caster_wheel_radius}"/>
        </geometry>
      </collision>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="${caster_wheel_radius}"/>
        </geometry>
        <material name="white"/>
      </visual>

      <inertial>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <mass value="${caster_wheel_mass}"/>
    <inertia
      ixx="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}" ixy="0.0" ixz="0.0"
          iyy="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}" iyz="0.0"
      izz="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}"/>
        </inertial>
      </link>

    <joint name="${prefix}caster_back_left_wheel_joint" type="fixed">
  <parent link="${prefix}base_link"/>
      <child link="${prefix}caster_back_left_wheel"/>
  <origin xyz="-0.215 -0.155 ${z_offset}" rpy="0 0 0"/>
    </joint>

    <!-- caster back left wheel Link -->
    <link name="${prefix}caster_back_left_wheel">
  <collision>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <sphere radius="${caster_wheel_radius}"/>
        </geometry>
      </collision>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="${caster_wheel_radius}"/>
        </geometry>
        <material name="white"/>
      </visual>

      <inertial>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <mass value="${caster_wheel_mass}"/>
    <inertia
      ixx="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}" ixy="0.0" ixz="0.0"
          iyy="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}" iyz="0.0"
      izz="${caster_wheel_mass * 0.4 * (caster_wheel_radius * caster_wheel_radius)}"/>
        </inertial>
      </link>

    <!-- lidar -->
    <joint name="laser_joint" type="fixed">
        <parent link="${prefix}base_link"/>
        <child link="scan"/>
        <origin xyz="0.277 0 0.16" rpy="3.14 0 0"/>
    </joint>

    <link name="scan">
        <visual>
            <geometry>
                <cylinder radius="0.05" length="0.04"/>
            </geometry>
            <material name="red"/>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="0.05" length="0.04"/>
            </geometry>
        </collision>
    </link>

    <link name="base_footprint">
    </link>

    <joint name="base_joint" type="fixed">
      <parent link="diffbot_base_link"/>
      <child link="base_footprint"/>
      <origin xyz="0.0 0.0 0.05" rpy="0 0 0"/>
    </joint>

  </xacro:macro>

</robot>

There is my rqt_graph : enter image description here

There is the result of the tf_monitor :

RESULTS: for all Frames

Frames:
Frame: base_footprint, published by <no authority available>, Average Delay: 1531.62, Max Delay: 1531.62
Frame: base_link, published by <no authority available>, Average Delay: 1531.65, Max Delay: 1531.65
Frame: diffbot_base_link, published by <no authority available>, Average Delay: -0.00354796, Max Delay: 0.0200486
Frame: diffbot_caster_back_left_wheel, published by <no authority available>, Average Delay: 1531.62, Max Delay: 1531.62
Frame: diffbot_caster_back_right_wheel, published by <no authority available>, Average Delay: 1531.62, Max Delay: 1531.62
Frame: diffbot_caster_frontal_left_wheel, published by <no authority available>, Average Delay: 1531.62, Max Delay: 1531.62
Frame: diffbot_caster_frontal_right_wheel, published by <no authority available>, Average Delay: 1531.62, Max Delay: 1531.62
Frame: diffbot_left_wheel, published by <no authority available>, Average Delay: -0.000404738, Max Delay: 0.0375586
Frame: diffbot_right_wheel, published by <no authority available>, Average Delay: -0.000401797, Max Delay: 0.03756
Frame: odom, published by <no authority available>, Average Delay: -0.851394, Max Delay: 0
Frame: scan, published by <no authority available>, Average Delay: 1531.62, Max Delay: 1531.62

All Broadcasters:
Node: <no authority available> 31.8382 Hz, Average Delay: 8.77442 Max Delay: 1531.65

Tree frame : enter image description here

I have two error :

  • first one on console when launching amcl :
[amcl-2] [INFO] [1691749429.058724371] [amcl]: Message Filter dropping message: frame 'scan' at time 1691749428.765 for reason 'discarding message because the queue is full'
[amcl-2] terminate called after throwing an instance of 'tf2_ros::InvalidTimerHandleException'
[amcl-2]   what():  Invalid timer handle in remove()
[ERROR] [amcl-2]: process has died [pid 3149, exit code -6, cmd '/opt/ros/humble/lib/nav2_amcl/amcl --ros-args --log-level info --ros-args -r __node:=amcl --params-file /tmp/tmp1e0h19_4 -r /tf:=tf -r /tf_static:=tf_static'].
  • second one on the terminal where i launch rviz
[ERROR] [1691678663.940263931] [rviz2]: Lookup would require extrapolation into the future.  Requested time 1691678663.715215 but the latest data is at time 1691678663.712955, when looking up transform from frame [scan] to frame [map]

amcl config :

amcl:
  ros__parameters:
    use_sim_time: True
    alpha1: 0.05
    alpha2: 0.05
    alpha3: 0.05
    alpha4: 0.05
    alpha5: 0.2
    base_frame_id: "diffbot_base_link"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    laser_lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_sigma_hit: 0.2
    laser_max_range: 4000.0
    laser_min_range: 2.0
    laser_model_type: "likelihood_field"
    max_beams: 1600
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.1
    recovery_alpha_slow: 0.001
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.1
    update_min_d: 0.1
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan
    set_initial_pose: true
    map_topic: "map"
    first_map_only: false
    initial_pose:
      x: 0.0
      y: 0.0
      yaw: 0.0

Amcl node info :

/amcl
  Subscribers:
    /bond: bond/msg/Status
    /clock: rosgraph_msgs/msg/Clock
    /initialpose: geometry_msgs/msg/PoseWithCovarianceStamped
    /map: nav_msgs/msg/OccupancyGrid
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /scan: sensor_msgs/msg/LaserScan
  Publishers:
    /amcl/transition_event: lifecycle_msgs/msg/TransitionEvent
    /amcl_pose: geometry_msgs/msg/PoseWithCovarianceStamped
    /bond: bond/msg/Status
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /particle_cloud: nav2_msgs/msg/ParticleCloud
    /rosout: rcl_interfaces/msg/Log
    /tf: tf2_msgs/msg/TFMessage
  Service Servers:
    /amcl/change_state: lifecycle_msgs/srv/ChangeState
    /amcl/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /amcl/get_available_states: lifecycle_msgs/srv/GetAvailableStates
    /amcl/get_available_transitions: lifecycle_msgs/srv/GetAvailableTransitions
    /amcl/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /amcl/get_parameters: rcl_interfaces/srv/GetParameters
    /amcl/get_state: lifecycle_msgs/srv/GetState
    /amcl/get_transition_graph: lifecycle_msgs/srv/GetAvailableTransitions
    /amcl/list_parameters: rcl_interfaces/srv/ListParameters
    /amcl/set_parameters: rcl_interfaces/srv/SetParameters
    /amcl/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
    /reinitialize_global_localization: std_srvs/srv/Empty
    /request_nomotion_update: std_srvs/srv/Empty
  Service Clients:

  Action Servers:

  Action Clients:
Improve this question
$\endgroup$
4
  • $\begingroup$ Your delay of 1531 seconds suggests that you have a timing problem on your system do you have multiple computers? Are you sure it's not being published or just not synchronized? Why do you think amcl is the problem? tf2_tools view_frames would be helpful. $\endgroup$
    – Tully ♦
    Aug 9 at 21:16
  • $\begingroup$ Thank you for your response ! I add the view_frame. I was supposing that the problem was with amcl because i don't see the topic /tf in the rqt_graph. But il echo this topic and there is data published. Now i don't really understand why i have big delay. Currently i launch rviz in my computer and the system is running on a raspberry pi in the real robot. I try to set time on both computer with NTP server and i think it's good but i have already the problem. I add two errors that can be linked on the question above. $\endgroup$
    – Josselin
    Aug 10 at 14:37
  • $\begingroup$ Ok, with the error messages you're now showing it looks like you do have a problem with amcl. Please revise your question to include motivation, what you're doing. What you expect to be doing and what's different. We're getting deep into an X-Y problem here. Your question is presupposing problems and asking about how to solve them instead of focusing on determining what your problem is and then solving it. In particular the amcl scan message filter likely means that required inputs are missing. And the "scan" frame_id is unusual and I suspect may be a misconfiguration. $\endgroup$
    – Tully ♦
    Aug 10 at 16:21
  • $\begingroup$ thank, i will revise the question ! The last screen i add, show that amcl don't subscribe to /tf topic. This isn't normal, right ? $\endgroup$
    – Josselin
    Aug 11 at 7:29

2 Answers 2

Reset to default
1
$\begingroup$

That error shows that amcl has crashed and is no longer running. That is where I would start your search.

To that end you're not the first to experience this. There's an open bug issue at

https://github.com/ros-planning/navigation2/issues/3575

I would suggest following up there.

In the future please make sure to include things like error messages in your description the first time around.

Improve this answer
$\endgroup$
1
  • $\begingroup$ Hello, Amcl just crash sometimes, so i think i have already a problem with the amcl subscribers. I just actualized the rqt_graph on the question. I will follow for the amcl crash on the suggested question ! $\endgroup$
    – Josselin
    Aug 16 at 7:27
0
$\begingroup$

The fix for

[amcl-2] terminate called after throwing an instance of 'tf2_ros::InvalidTimerHandleException' [amcl-2] what(): Invalid timer handle in remove()

is here! https://github.com/ros2/geometry2/pull/474

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.