I have a mobile robot that is able with 2D Lidar , Realsence Depth Camera with IMU and Roboteq Controller for brushless DC Motor. Im using slam toolbox for SLAM. So first, run the lidar with
ros2 launch lslidar_driver lslidar_launch.py
Then run the the roboteq driver with
ros2 launch roboteq_ros2_driver roboteq_ros2_driver.launch.py
This is publishing odom. And is the only thing that publish odom. So need something to tell the robot how far it's moved, current velocity, etc. SoI need another node that publishes odometry from lidar data. So this the package doing that Simple-2D-LiDAR-Odometry. Running with
ros2 run lidar_odometry lidar_odometry_node
It publich odometry under /scan_odom topic as can see in the ekf config of the slam_toolbox
### ekf config file ###
ekf_filter_node:
ros__parameters:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 30.0
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
two_d_mode: false
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: true
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: true
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame"
# to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark
# observations) then:
# 3a. Set your "world_frame" to your map_frame value
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node
# from robot_localization! However, that instance should *not* fuse the global data.
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" if unspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified
odom0: /scan_odom
odom0_config: [true, true, true,
false, false, false,
true, true, true,
true, true, true,
false, false, false]
imu0: /camera/camera/accel/sample
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, false,
true, true, true]
Then my launch file is this
import launch
from launch.substitutions import Command, LaunchConfiguration
import launch_ros
import os
def generate_launch_description():
pkg_share = launch_ros.substitutions.FindPackageShare(package='amr_bot').find('amr_bot')
default_model_path = os.path.join(pkg_share, 'src/description/amr_bot_description.urdf')
default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf_config.rviz')
#world_path=os.path.join(pkg_share, 'world/my_world.sdf')
robot_state_publisher_node = launch_ros.actions.Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
)
joint_state_publisher_node = launch_ros.actions.Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
)
rviz_node = launch_ros.actions.Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', LaunchConfiguration('rvizconfig')],
)
spawn_entity = launch_ros.actions.Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', 'amr_bot', '-topic', 'robot_description'],
output='screen'
)
robot_localization_node = launch_ros.actions.Node(
package='robot_localization',
executable='ekf_node',
name='ekf_filter_node',
output='screen',
parameters=[os.path.join(pkg_share, 'config/ekf.yaml'), {'use_sim_time': LaunchConfiguration('use_sim_time')}]
)
return launch.LaunchDescription([
launch.actions.DeclareLaunchArgument(name='gui', default_value='True',
description='Flag to enable joint_state_publisher_gui'),
launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
description='Absolute path to robot urdf file'),
launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
# description='Absolute path to rviz config file'),
launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='True',
description='Flag to enable use_sim_time'),
#launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', world_path], output='screen'),
joint_state_publisher_node,
robot_state_publisher_node,
#spawn_entity,
robot_localization_node,
rviz_node
])
the map parameter file is
slam_toolbox: ros__parameters:
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_footprint
scan_topic: /scan
mode: mapping
# if you'd like to immediately start continuing a map at a given pose
# or at the dock, but they are mutually exclusive, if pose is given
# will use pose
map_file_name: /home/user/serial_map_2_22
# map_start_pose: [0.0, 0.0, 0.0]
map_start_at_dock: true
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 1.0
resolution: 0.05
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
enable_interactive_mode: true
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.1
minimum_travel_heading: 0.1
scan_buffer_size: 50
scan_buffer_maximum_scan_distance: 10.0
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Scan Matcher Parameters
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true
So in RVIZ the map is twisted as can see.
The Lidar frame id is laser_link. Here is the URDF file:
<?xml version="1.0"?>
<robot name="sam_bot" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Define robot constants -->
<xacro:property name="base_width" value="0.31"/>
<xacro:property name="base_length" value="0.42"/>
<xacro:property name="base_height" value="0.18"/>
<xacro:property name="wheel_radius" value="0.10"/>
<xacro:property name="wheel_width" value="0.04"/>
<xacro:property name="wheel_ygap" value="0.025"/>
<xacro:property name="wheel_zoff" value="0.05"/>
<xacro:property name="wheel_xoff" value="0.12"/>
<xacro:property name="caster_xoff" value="0.14"/>
<!-- Define some commonly used intertial properties -->
<xacro:macro name="box_inertia" params="m w h d">
<inertial>
<origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>
<mass value="${m}"/>
<inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertia" params="m r h">
<inertial>
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
<mass value="${m}"/>
<inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy = "0" ixz = "0" iyy="${(m/12) * (3*r*r + h*h)}" iyz = "0" izz="${(m/2) * (r*r)}"/>
</inertial>
</xacro:macro>
<xacro:macro name="sphere_inertia" params="m r">
<inertial>
<mass value="${m}"/>
<inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}"/>
</inertial>
</xacro:macro>
<!-- Robot Base -->
<link name="base_link">
<visual>
<geometry>
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
<material name="Cyan">
<color rgba="0 1.0 1.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
</collision>
<xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}"/>
</link>
<!-- Robot Footprint -->
<link name="base_footprint">
<xacro:box_inertia m="0" w="0" d="0" h="0"/>
</link>
<joint name="base_joint" type="fixed">
<parent link="base_link"/>
<child link="base_footprint"/>
<origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
</joint>
<!-- Wheels -->
<xacro:macro name="wheel" params="prefix x_reflect y_reflect">
<link name="${prefix}_link">
<visual>
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
</geometry>
<material name="Gray">
<color rgba="0.5 0.5 0.5 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
</geometry>
</collision>
<xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}"/>
</link>
<joint name="${prefix}_joint" type="continuous">
<parent link="base_link"/>
<child link="${prefix}_link"/>
<origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
</xacro:macro>
<xacro:wheel prefix="drivewhl_l" x_reflect="-1" y_reflect="1" />
<xacro:wheel prefix="drivewhl_r" x_reflect="-1" y_reflect="-1" />
<link name="front_caster">
<visual>
<geometry>
<sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
</geometry>
<material name="Cyan">
<color rgba="0 1.0 1.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
</geometry>
</collision>
<xacro:sphere_inertia m="0.5" r="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
</link>
<joint name="caster_joint" type="fixed">
<parent link="base_link"/>
<child link="front_caster"/>
<origin xyz="${caster_xoff} 0.0 ${-(base_height/2)}" rpy="0 0 0"/>
</joint>
<link name="imu_link">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1"/>
</link>
<joint name="imu_joint" type="fixed">
<parent link="base_link"/>
<child link="imu_link"/>
<origin xyz="0 0 0.01"/>
</joint>
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<ros>
<namespace>/demo</namespace>
<remapping>~/out:=imu</remapping>
</ros>
<initial_orientation_as_reference>false</initial_orientation_as_reference>
</plugin>
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>
<gazebo>
<plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
<ros>
<namespace>/demo</namespace>
</ros>
<!-- wheels -->
<left_joint>drivewhl_l_joint</left_joint>
<right_joint>drivewhl_r_joint</right_joint>
<!-- kinematics -->
<wheel_separation>0.4</wheel_separation>
<wheel_diameter>0.2</wheel_diameter>
<!-- limits -->
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>
<!-- output -->
<publish_odom>true</publish_odom>
<publish_odom_tf>false</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_link</robot_base_frame>
</plugin>
</gazebo>
<link name="lidar_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.125"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.0508" length="0.055"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.0508" length="0.055"/>
</geometry>
</visual>
</link>
<joint name="lidar_joint" type="fixed">
<parent link="base_link"/>
<child link="lidar_link"/>
<origin xyz="0 0 0.12" rpy="0 0 0"/>
</joint>
<gazebo reference="lidar_link">
<sensor name="lidar" type="ray">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>5</update_rate>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1.000000</resolution>
<min_angle>0.000000</min_angle>
<max_angle>6.280000</max_angle>
</horizontal>
</scan>
<range>
<min>0.120000</min>
<max>3.5</max>
<resolution>0.015000</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="scan" filename="libgazebo_ros_ray_sensor.so">
<ros>
<remapping>~/out:=scan</remapping>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>laser_link</frame_name>
</plugin>
</sensor>
</gazebo>
<link name="camera_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.015 0.130 0.022"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.015 0.130 0.022"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.035"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
</inertial>
</link>
<joint name="camera_joint" type="fixed">
<parent link="base_link"/>
<child link="camera_link"/>
<origin xyz="0.215 0 0.05" rpy="0 0 0"/>
</joint>
<link name="camera_depth_frame"/>
<joint name="camera_depth_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
<parent link="camera_link"/>
<child link="camera_depth_frame"/>
</joint>
<gazebo reference="camera_link">
<sensor name="depth_camera" type="depth">
<visualize>true</visualize>
<update_rate>30.0</update_rate>
<camera name="camera">
<horizontal_fov>1.047198</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.05</near>
<far>3</far>
</clip>
</camera>
<plugin name="depth_camera_controller" filename="libgazebo_ros_camera.so">
<baseline>0.2</baseline>
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<frame_name>camera_depth_frame</frame_name>
<pointCloudCutoff>0.5</pointCloudCutoff>
<pointCloudCutoffMax>3.0</pointCloudCutoffMax>
<distortionK1>0</distortionK1>
<distortionK2>0</distortionK2>
<distortionK3>0</distortionK3>
<distortionT1>0</distortionT1>
<distortionT2>0</distortionT2>
<CxPrime>0</CxPrime>
<Cx>0</Cx>
<Cy>0</Cy>
<focalLength>0</focalLength>
<hackBaseline>0</hackBaseline>
</plugin>
</sensor>
</gazebo>
</robot>
Any help?