Login

u/Complete_Astronaut_2

▲ 4 r/ROS

Laser scan moves and rotates with the robot in real life

Hello everyone,

I know this is the most asked question in any navigation robots, but I have been stuck for a long on this and I cant still figure out the problem. I have robot based on the Josh Newans diff drive navigation robot, I am using rasperry pi 4 with ros2 humble on both the pi and the dev machine. I am building the hardware and I am using rplidar c1. Everything in simulation works perfect, the mapping and localizing is okay, the ros2 controllers are loaded fine and the laser scan stays in it place when the robot moves away from it. However with the hardware, I cant do SLAM because the laser scan reading moves along with the robot whenever I moves or rotate it.

I read about all this (even 4 days ago someone asked the same question) and everyone said its an odometry and TF issue, but I tested the encoders and adjusted the robot's URDF to match with the real one, I did the following regarding the encoders and odometry:

  1. I rotated the wheels one full revolution and adjusted the encoder counts per revolution tag in the ros2 controllers xacro, now one full spin in real life is same in rviz2 with correct direction (clockwise and counter clockwise)
  2. I moved the robot one full meter in real life and adjusted the wheel radius until the motion is the same in the rviz2
  3. I rotated the robot in place and adjusted the separation distance until it synced.

The robot starts and the controllers are loaded successfully and the odom, tf and scan and static_tf are also published correctly, however in rviz2 when I switch the fixed frame to odom frame, the scan moves with the robot.

I dont know how to figure out this, Josh Newans actually mentioned this issue here as TF sync issue at 7:30 but he dismissed it as he doesn't know the solution and hoped that it worked for everyone else...

I feel like this is a problem that is hiding in the plain sight and I cant see, and its extremely annoying because I am stuck with this issue that I have to have it fix in order to proceed with my actual project, and I cant just solve for like three weeks now. I theorize that this issue is closely related to ros2_controller or the joint_state_broadcaster but I cant still fix it no matter I do or try, so please any help will be extremely beneficial.

This is the TF tree

https://preview.redd.it/eqxkzorbiq1h1.png?width=1574&format=png&auto=webp&s=28f1e73c06ebae4a08685ab7ece7ced18ec9ffb3

This is ros2_controller yaml file:

    update_rate: 30
    # use_sim_time: true

    diff_cont:
      type: diff_drive_controller/DiffDriveController

    joint_broad:
      type: joint_state_broadcaster/JointStateBroadcaster

diff_cont:
  ros__parameters:

    publish_rate: 30.0

    base_frame_id: base_link

    left_wheel_names: ['left_wheel_joint']
    right_wheel_names: ['right_wheel_joint']
    wheel_separation: 0.255
    wheel_radius: 0.033

    use_stamped_vel: false
    enable_odom_tf: true
    odom_frame_id: odom
    # open_loop: false    

    # wheels_per_side: x
    # wheel_separation_multiplier: x
    # left_wheel_radius_multiplier: x
    # right_wheel_radius_multiplier: x

    # odom_frame_id: x
    # pose_covariance_diagonal: x
    # twist_covariance_diagonal: x
    # open_loop: x
    # enable_odom_tf: x

    # cmd_vel_timeout: x
    # publish_limited_velocity: x
    # velocity_rolling_window_size: x
    

    # linear.x.has_velocity_limits: false
    # linear.x.has_acceleration_limits: false
    # linear.x.has_jerk_limits: false
    # linear.x.max_velocity: NAN
    # linear.x.min_velocity: NAN
    # linear.x.max_acceleration: NAN
    # linear.x.min_acceleration: NAN
    # linear.x.max_jerk: NAN
    # linear.x.min_jerk: NAN

    # angular.z.has_velocity_limits: false
    # angular.z.has_acceleration_limits: false
    # angular.z.has_jerk_limits: false
    # angular.z.max_velocity: NAN
    # angular.z.min_velocity: NAN
    # angular.z.max_acceleration: NAN
    # angular.z.min_acceleration: NAN
    # angular.z.max_jerk: NAN
    # angular.z.min_jerk: NAN




# joint_broad:
#   ros__parameters:
#     publish_rate: 30.0

This is the launch_robot.launch.py

import os

from ament_index_python.packages import get_package_share_directory


from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command
from launch.substitutions import LaunchConfiguration
from launch.actions import RegisterEventHandler
from launch.event_handlers import OnProcessStart

from launch_ros.actions import Node



def generate_launch_description():

    package_name='articubot_one' #<--- CHANGE ME
    lidar_package='rplidar_ros'

    lidar_serial_port = LaunchConfiguration('lidar_serial_port')
    lidar_serial_baudrate = LaunchConfiguration('lidar_serial_baudrate')
    lidar_frame_id = LaunchConfiguration('lidar_frame_id')

    declare_lidar_serial_port = DeclareLaunchArgument(
        'lidar_serial_port',
        default_value='/dev/ttyUSB1',
        description='Serial port used by RPLidar'
    )

    declare_lidar_serial_baudrate = DeclareLaunchArgument(
        'lidar_serial_baudrate',
        default_value='460800',
        description='Serial baudrate used by RPLidar'
    )

    declare_lidar_frame_id = DeclareLaunchArgument(
        'lidar_frame_id',
        default_value='laser_frame',
        description='TF frame_id used by /scan messages'
    )

    

    rsp = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    get_package_share_directory(package_name),'launch','rsp.launch.py'
                )]), launch_arguments={'use_sim_time': 'false', 'use_ros2_control': 'true'}.items()
    )

    camera = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory(package_name),'launch','camera.launch.py'
        )])
    )

    joystick = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    get_package_share_directory(package_name),'launch','joystick.launch.py'
                )])
    )

    lidar = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory(lidar_package), 'launch', 'rplidar_c1_launch.py'
        )]),
        launch_arguments={
            'serial_port': lidar_serial_port,
            'serial_baudrate': lidar_serial_baudrate,
            'frame_id': lidar_frame_id,
        }.items()
    )


    twist_mux_params = os.path.join(get_package_share_directory(package_name),'config','twist_mux.yaml')
    twist_mux = Node(
            package="twist_mux",
            executable="twist_mux",
            parameters=[twist_mux_params],
            remappings=[('/cmd_vel_out','/diff_cont/cmd_vel_unstamped')]
        )

    


    robot_description = Command(['ros2 param get --hide-type /robot_state_publisher robot_description'])

    controller_params_file = os.path.join(get_package_share_directory(package_name),'config','my_controllers.yaml')

    controller_manager = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[{'robot_description': robot_description},
                    controller_params_file]
    )

    delayed_controller_manager = TimerAction(period=3.0, actions=[controller_manager])

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

    delayed_diff_drive_spawner = RegisterEventHandler(
        event_handler=OnProcessStart(
            target_action=controller_manager,
            on_start=[diff_drive_spawner],
        )
    )

    joint_broad_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_broad"]
    )

    delayed_joint_broad_spawner = RegisterEventHandler(
        event_handler=OnProcessStart(
            target_action=controller_manager,
            on_start=[joint_broad_spawner],
        )
    )




    return LaunchDescription([
        declare_lidar_serial_port,
        declare_lidar_serial_baudrate,
        declare_lidar_frame_id,
        joystick,
        rsp,
        delayed_controller_manager,
        delayed_diff_drive_spawner,
        delayed_joint_broad_spawner,
        camera,

    ])

this is the rsp.launch.py

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration, Command
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node

import xacro


def generate_launch_description():

    # Check if we're told to use sim time
    use_sim_time = LaunchConfiguration('use_sim_time')
    use_ros2_control = LaunchConfiguration('use_ros2_control')

    # Process the URDF file
    pkg_path = os.path.join(get_package_share_directory('articubot_one'))
    xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
    # robot_description_config = xacro.process_file(xacro_file).toxml()
    robot_description_config = Command(['xacro ', xacro_file, ' use_ros2_control:=', use_ros2_control, ' sim_mode:=', use_sim_time])
    
    # Create a robot_state_publisher node
    params = {'robot_description': robot_description_config, 'use_sim_time': use_sim_time}
    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[params]
    )


    # Launch!
    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use sim time if true'),
        DeclareLaunchArgument(
            'use_ros2_control',
            default_value='true',
            description='Use ros2_control if true'),

        node_robot_state_publisher
    ])

This is the rplidar c1 launch file:

#!/usr/bin/env python3

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import LogInfo
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    channel_type =  LaunchConfiguration('channel_type', default='serial')
    serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB1')
    serial_baudrate = LaunchConfiguration('serial_baudrate', default='460800')
    frame_id = LaunchConfiguration('frame_id', default='laser_frame')
    inverted = LaunchConfiguration('inverted', default='false')
    angle_compensate = LaunchConfiguration('angle_compensate', default='true')
    scan_mode = LaunchConfiguration('scan_mode', default='Standard')

    return LaunchDescription([
        DeclareLaunchArgument(
            'channel_type',
            default_value=channel_type,
            description='Specifying channel type of lidar'),

        DeclareLaunchArgument(
            'serial_port',
            default_value=serial_port,
            description='Specifying usb port to connected lidar'),

        DeclareLaunchArgument(
            'serial_baudrate',
            default_value=serial_baudrate,
            description='Specifying usb port baudrate to connected lidar'),
        
        DeclareLaunchArgument(
            'frame_id',
            default_value=frame_id,
            description='Specifying frame_id of lidar'),

        DeclareLaunchArgument(
            'inverted',
            default_value=inverted,
            description='Specifying whether or not to invert scan data'),

        DeclareLaunchArgument(
            'angle_compensate',
            default_value=angle_compensate,
            description='Specifying whether or not to enable angle_compensate of scan data'),

        DeclareLaunchArgument(
            'scan_mode',
            default_value=scan_mode,
            description='Specifying scan mode of lidar'),

        Node(
            package='rplidar_ros',
            executable='rplidar_node',
            name='rplidar_node',
            parameters=[{'channel_type':channel_type,
                         'serial_port': serial_port,
                         'serial_baudrate': serial_baudrate,
                         'frame_id': frame_id,
                         'inverted': inverted,
                         'angle_compensate': angle_compensate,
                         'scan_mode': scan_mode}],
            output='screen'),
    ])

this is the ros2 controller xacro files

this is the ros_controller file 
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:unless value="$(arg sim_mode)">
        <ros2_control name="RealRobot" type="system">
            <hardware>
                <plugin>diffdrive_arduino/DiffDriveArduinoHardware</plugin>
                <param name="left_wheel_name">left_wheel_joint</param>
                <param name="right_wheel_name">right_wheel_joint</param>
                <param name="loop_rate">30</param>
                <param name="device">/dev/ttyUSB0</param>
                <param name="baud_rate">57600</param>
                <param name="timeout_ms">1000</param>
                <param name="enc_counts_per_rev">1496</param>
            </hardware>
            <joint name="left_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-10</param>
                    <param name="max">10</param>
                </command_interface>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
            </joint>
            <joint name="right_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-10</param>
                    <param name="max">10</param>
                </command_interface>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
            </joint>
        </ros2_control>
    </xacro:unless>

    <xacro:if value="$(arg sim_mode)">
        <ros2_control name="GazeboSystem" type="system">
            <hardware>
                <plugin>gazebo_ros2_control/GazeboSystem</plugin>
            </hardware>
            <joint name="left_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-10</param>
                    <param name="max">10</param>
                </command_interface>
                <state_interface name="velocity"/>
                <state_interface name="position"/>
            </joint>
            <joint name="right_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-10</param>
                    <param name="max">10</param>
                </command_interface>
                <state_interface name="velocity"/>
                <state_interface name="position"/>
            </joint>
        </ros2_control>
    </xacro:if>

    <gazebo>
        <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
            <parameters>$(find articubot_one)/config/my_controllers.yaml</parameters>
            <parameters>$(find articubot_one)/config/gaz_ros2_ctl_use_sim.yaml</parameters>
        </plugin>
    </gazebo>

</robot>

The robot_core.xacro

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

    <xacro:include filename="inertial_macros.xacro"/>


    <xacro:property name="chassis_length" value="0.335"/>
    <xacro:property name="chassis_width" value="0.265"/>
    <xacro:property name="chassis_height" value="0.138"/>
    <xacro:property name="chassis_mass" value="1.0"/>
    <xacro:property name="wheel_radius" value="0.03125"/>
    <xacro:property name="wheel_thickness" value="0.026"/>
    <xacro:property name="wheel_mass" value="0.05"/>
    <xacro:property name="wheel_offset_x" value="0.026"/>
    <xacro:property name="wheel_offset_y" value="0.1485"/>
    <xacro:property name="wheel_offset_z" value="0.01"/>
    <xacro:property name="caster_wheel_radius" value="0.01"/>
    <xacro:property name="caster_wheel_mass" value="0.01"/>
    <xacro:property name="caster_wheel_offset_x" value="0.075"/>
    <xacro:property name="caster_wheel_offset_z" value="${wheel_offset_z - wheel_radius + caster_wheel_radius}"/>

    <material name="white">
        <color rgba="1 1 1 1" />
    </material>

    <material name="orange">
        <color rgba="1 0.3 0.1 1"/>
    </material>

    <material name="blue">
        <color rgba="0.2 0.2 1 1"/>
    </material>

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

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

    <!-- BASE LINK -->

    <link name="base_link">

    </link>

    <!-- BASE_FOOTPRINT LINK -->

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

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


    <!-- CHASSIS LINK -->

    <joint name="chassis_joint" type="fixed">
        <parent link="base_link"/>
        <child link="chassis"/>
        <origin xyz="${-wheel_offset_x} 0 ${-wheel_offset_z}"/>
    </joint>

    <link name="chassis">
        <visual>
            <origin xyz="${chassis_length/2} 0 ${chassis_height/2}"/>
            <geometry>
                <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
            </geometry>
            <material name="orange"/>
        </visual>
        <collision>
            <origin xyz="${chassis_length/2} 0 ${chassis_height/2}"/>
            <geometry>
                <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
            </geometry>
        </collision>
        <xacro:inertial_box mass="0.5" x="${chassis_length}" y="${chassis_width}" z="${chassis_height}">
            <origin xyz="${chassis_length/2} 0 ${chassis_height/2}" rpy="0 0 0"/>
        </xacro:inertial_box>
    </link>

    <gazebo reference="chassis">
        <material>Gazebo/Orange</material>
    </gazebo>

    <!-- LEFT WHEEL LINK -->

    <joint name="left_wheel_joint" type="continuous">
        <parent link="base_link"/>
        <child link="left_wheel"/>
        <origin xyz="0 ${wheel_offset_y} 0" rpy="-${pi/2} 0 0" />
        <axis xyz="0 0 1"/>
    </joint>

    <link name="left_wheel">
        <visual>
            <geometry>
                <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <sphere radius="${wheel_radius}"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="${wheel_mass}" length="${wheel_thickness}" radius="${wheel_radius}">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_cylinder>
    </link>

    <gazebo reference="left_wheel">
        <material>Gazebo/Blue</material>
    </gazebo>




    <!-- RIGHT WHEEL LINK -->

    <joint name="right_wheel_joint" type="continuous">
        <parent link="base_link"/>
        <child link="right_wheel"/>
        <origin xyz="0 ${-wheel_offset_y} 0" rpy="${pi/2} 0 0" />
        <axis xyz="0 0 -1"/>
    </joint>

    <link name="right_wheel">
        <visual>
            <geometry>
                <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <sphere radius="${wheel_radius}"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="${wheel_mass}" length="${wheel_thickness}" radius="${wheel_radius}">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_cylinder>
    </link>

    <gazebo reference="right_wheel">
        <material>Gazebo/Blue</material>
    </gazebo>


    <!-- CASTER WHEEL LINK -->

    <joint name="caster_wheel_joint" type="fixed">
        <parent link="chassis"/>
        <child link="caster_wheel"/>
        <origin xyz="${caster_wheel_offset_x} 0 ${caster_wheel_offset_z}"/>
    </joint>


    <link name="caster_wheel">
        <visual>
            <geometry>
                <sphere radius="${caster_wheel_radius}"/>
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <geometry>
                <sphere radius="${caster_wheel_radius}"/>
            </geometry>
        </collision>
        <xacro:inertial_sphere mass="${caster_wheel_mass}" radius="${caster_wheel_radius}">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_sphere>
    </link>

    <gazebo reference="caster_wheel">
        <material>Gazebo/White</material>
        <mu1 value="0.001"/>
        <mu2 value="0.001"/>
    </gazebo>

</robot>

This is the lidar.xacro

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >

    <joint name="laser_joint" type="fixed">
        <parent link="chassis"/>
        <child link="laser_frame"/>
        <origin xyz="0.162 0 0.212" rpy="0 0 0"/>
    </joint>

    <link name="laser_frame">
        <visual>
            <geometry>
                <cylinder radius="0.05" length="0.04"/>
            </geometry>
            <material name="black"/>
        </visual>
        <visual>
            <origin xyz="0 0 -0.05"/>
            <geometry>
                <cylinder radius="0.01" length="0.1"/>
            </geometry>
            <material name="black"/>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="0.05" length="0.04"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.05">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_cylinder>
    </link>



    <gazebo reference="laser_frame">
        <material>Gazebo/Black</material>

        <sensor name="laser" type="ray">
            <pose> 0 0 0 0 0 0 </pose>
            <visualize>false</visualize>
            <update_rate>10</update_rate>
            <ray>
                <scan>
                    <horizontal>
                        <samples>360</samples>
                        <min_angle>-3.14</min_angle>
                        <max_angle>3.14</max_angle>
                    </horizontal>
                </scan>
                <range>
                    <min>0.3</min>
                    <max>12</max>
                </range>
            </ray>
            <plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">
                <ros>
                    <argument>~/out:=scan</argument>
                </ros>
                <output_type>sensor_msgs/LaserScan</output_type>
                <frame_name>laser_frame</frame_name>
            </plugin>
        </sensor>
    </gazebo>

</robot><robot xmlns:xacro="http://www.ros.org/wiki/xacro" >

    <joint name="laser_joint" type="fixed">
        <parent link="chassis"/>
        <child link="laser_frame"/>
        <origin xyz="0.162 0 0.212" rpy="0 0 0"/>
    </joint>

    <link name="laser_frame">
        <visual>
            <geometry>
                <cylinder radius="0.05" length="0.04"/>
            </geometry>
            <material name="black"/>
        </visual>
        <visual>
            <origin xyz="0 0 -0.05"/>
            <geometry>
                <cylinder radius="0.01" length="0.1"/>
            </geometry>
            <material name="black"/>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="0.05" length="0.04"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.05">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_cylinder>
    </link>



    <gazebo reference="laser_frame">
        <material>Gazebo/Black</material>

        <sensor name="laser" type="ray">
            <pose> 0 0 0 0 0 0 </pose>
            <visualize>false</visualize>
            <update_rate>10</update_rate>
            <ray>
                <scan>
                    <horizontal>
                        <samples>360</samples>
                        <min_angle>-3.14</min_angle>
                        <max_angle>3.14</max_angle>
                    </horizontal>
                </scan>
                <range>
                    <min>0.3</min>
                    <max>12</max>
                </range>
            </ray>
            <plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">
                <ros>
                    <argument>~/out:=scan</argument>
                </ros>
                <output_type>sensor_msgs/LaserScan</output_type>
                <frame_name>laser_frame</frame_name>
            </plugin>
        </sensor>
    </gazebo>

</robot>

moving the real robot moves the laser scan as well in rviz2

reddit.com
u/Complete_Astronaut_2 — 5 days ago
▲ 4 r/ROS

Hello everyone,

So in my final year project, I have to make a mobile manipulator service robot using AI system and depth camera. The hardware robot of choice is kuka youbot because it's the only mobile manipulator available in the university at the moment.

Kuka youbot uses ros1 indigo and ubuntu 14.04 and my laptop has ros2 humble and Ubuntu 22, the initial plan was using ros bridge and make the whole system run from my laptop. However, ros1 bridge is not working for indigo. I tried using docker image with ros indigo but Ubuntu 14 was so old that I couldnt run visual studio code on it and was hit with multiple failures in building and launching the code(specifically the youbot drivers package)

I need suggestions on the best way to connect and operate the robot

reddit.com
u/Complete_Astronaut_2 — 26 days ago