はじめに
今までは、ディファレンシャルドライブのオドメトリとImuセンサを用いてセンサ融合を行っていました。
今回は、imuなしでMapを作成するための構成を掲載しておきます。
前提条件
前提条件は、以下の通りです。
- worldファイルの作成が完了している
- Rviz2 上で /map を表示する方法が分かる (わからない方はこちらを参考)
URDF
first_robot.urdf
<?xml version="1.0" ?>
<robot name="rover">
<!-- main center object -->
<link name="base_link">
<visual>
<geometry>
<box size="0.4 0.35 0.075"/>
</geometry>
<material name="gray">
<color rgba="0.2 0.2 0.2 1"/>
</material>
</visual>
<inertial>
<mass value="2"/>
<inertia ixx="0.04" ixy="0.0" ixz="0.0" iyy="0.17" iyz="0.0" izz="0.25" />
</inertial>
<collision>
<geometry>
<box size="0.4 0.35 0.1"/>
</geometry>
</collision>
</link>
<!-- Robot Footprint -->
<link name="base_footprint"/>
<joint name="base_joint" type="fixed">
<parent link="base_link"/>
<child link="base_footprint"/>
<origin xyz="0.0 0.0 -0.075" rpy="0 0 0"/>
</joint>
<!-- front right wheel object -->
<link name="wheel_front_right">
<inertial>
<mass value="2"/>
<inertia ixx="1e-3" ixy="0.0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3"/>
</inertial>
<visual>
<geometry>
<cylinder radius="0.075" length="0.05"/>
</geometry>
<material name="whtie">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.075" length="0.05"/>
</geometry>
<contact_coefficients mu="1" kp="1e+13" kd="1.0"/>
</collision>
</link>
<!-- front right wheel joint -->
<joint name="wheel_front_right_joint" type="continuous">
<origin xyz="0.125 0.2 0.0" rpy="1.57 0.0 0.0"/>
<parent link="base_link"/>
<child link="wheel_front_right"/>
<axis xyz="0.0 0.0 1.0"/>
</joint>
<!-- front left wheel object -->
<link name="wheel_front_left">
<inertial>
<mass value="2"/>
<inertia ixx="1e-3" ixy="0.0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3"/>
</inertial>
<visual>
<geometry>
<cylinder radius="0.075" length="0.05"/>
</geometry>
<material name="whtie">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.075" length="0.05"/>
</geometry>
<contact_coefficients mu="1" kp="1e+13" kd="1.0"/>
</collision>
</link>
<!-- front left wheel joint -->
<joint name="wheel_front_left_joint" type="continuous">
<origin xyz="0.125 -0.2 0.0" rpy="1.57 0.0 0.0"/>
<parent link="base_link"/>
<child link="wheel_front_left"/>
<axis xyz="0.0 0.0 1.0"/>
</joint>
<!-- Caster Wheel -->
<link name="front_caster" type="fix">
<visual>
<geometry>
<sphere radius="0.075"/>
</geometry>
<material name="Cyan">
<color rgba="0 1.0 1.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<sphere radius="0.075"/>
</geometry>
</collision>
</link>
<joint name="caster_joint" type="fixed">
<parent link="base_link"/>
<child link="front_caster"/>
<origin xyz="-0.15 0.0 0" rpy="0 0 0"/>
</joint>
<!-- add camera object -->
<link name="camera_link">
<inertial>
<mass value="0.5"/>
<inertia ixx="0.01" ixy="0.0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
<visual>
<geometry>
<box size="0.05 0.25 0.1"/>
</geometry>
<material name="blue">
<color rgba="0 0 1 1"/>
</material>
</visual>
</link>
<!-- add camera joint -->
<joint name="camera_joint" type="fixed">
<origin xyz="-0.175 0 0.1" rpy="0 0.0 3.14"/>
<parent link="base_link"/>
<child link="camera_link"/>
<axis xyz="0.0 0.0 1.0"/>
</joint>
<!-- add lidar object -->
<link name="lidar_link">
<inertial>
<mass value="0.5"/>
<inertia ixx="0.01" ixy="0.0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
<visual>
<geometry>
<cylinder radius="0.05" length="0.05"/>
</geometry>
<material name="red">
<color rgba="1 0 0 0.7"/>
</material>
</visual>
</link>
<!-- add lidar joint -->
<joint name="lidar_joint" type="fixed">
<origin xyz="-0.215 0 0.0" rpy="0 0.0 1.57"/>
<parent link="base_link"/>
<child link="lidar_link"/>
<axis xyz="0.0 0.0 1.0"/>
</joint>
<!-- add imu object -->
<link name="imu_link">
<visual>
<inertial>
<mass value="0.3"/>
<inertia ixx="0.01" ixy="0.0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
<material name="green">
<color rgba="0 1 0 0.7"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</collision>
</link>
<!-- add imu joint -->
<joint name="imu_joint" type="fixed">
<origin xyz="0 0 0.075" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="imu_link"/>
<axis xyz="0 0 1.0"/>
</joint>
<!-- front diff drive status -->
<gazebo>
<plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">
<ros>
<remapping>cmd_vel:=cmd_vel</remapping>
<namespace>/first_robot</namespace>
<remapping>odom:=odom</remapping>
</ros>
<left_joint>wheel_front_left_joint</left_joint>
<right_joint>wheel_front_right_joint</right_joint>
<wheel_separation>0.4</wheel_separation>
<wheel_diameter>0.15</wheel_diameter>
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>
<publish_odom>true</publish_odom>
<publish_odom_tf>false</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
<update_rate>30</update_rate>
<odometry_topic>odom</odometry_topic>
<odometry_frame>odom</odometry_frame>
<base_frame_id>base_footprint</base_frame_id>
<robot_base_frame>base_link</robot_base_frame>
<command_topic>cmd_vel</command_topic>
</plugin>
</gazebo>
<!-- camera plugin -->
<gazebo reference="camera_link">
<sensor type="camera" name="camera1">
<alwaysOn>true</alwaysOn>
<visualize>true</visualize>
<update_rate>5.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<cameraName>/camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
</plugin>
</sensor>
<material>Gazebo/Blue</material>
</gazebo>
<!-- imu plugin -->
<gazebo reference="imu_link">
<sensor name="gazebo_imu" type="imu">
<gravity>true</gravity>
<visualize>true</visualize>
<update_rate>30</update_rate>
<always_on>true</always_on>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<ros>
<namespace>/first_robot</namespace>
<remapping>~/out:=imu</remapping>
</ros>
<initial_orientation_as_reference>false</initial_orientation_as_reference>
<output_type>sensor_msgs/Imu</output_type>
<frame_name>imu_link</frame_name>
</plugin>
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-04</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</x>
<y>
<nosie type="gaussian">
<mean>0.0</mean>
<stddev>2e-04</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</nosie>
</y>
<z>
<nosie type="gaussian">
<mean>0.0</mean>
<stddev>2e-04</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</nosie>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<nosie type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-02</stddev>
<bias_mean>0.001</bias_mean>
<bias_stddev>0.001</bias_stddev>
</nosie>
</x>
<y>
<nosie type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-02</stddev>
<bias_mean>0.001</bias_mean>
<bias_stddev>0.001</bias_stddev>
</nosie>
</y>
<z>
<nosie type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-02</stddev>
<bias_mean>0.001</bias_mean>
<bias_stddev>0.001</bias_stddev>
</nosie>
</z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>
<!-- lidar plugin -->
<gazebo reference="lidar_link">
<sensor name="gazebo_lidar" type="ray">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>60</update_rate>
<plugin filename="libgazebo_ros_ray_sensor.so" name="gazebo_lidar">
<ros>
<remapping>~/out:=scan</remapping>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>lidar_link</frame_name>
</plugin>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1.000000</resolution>
<min_angle>0.000000</min_angle>
<max_angle>3.140000</max_angle>
</horizontal>
</scan>
<range>
<min>0.150</min>
<max>6.0</max>
<resolution>0.015</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
</sensor>
<material>Gazebo/DarkGrey</material>
</gazebo>
</robot>World
first_world.sdf
<sdf version='1.7'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<audio>
<device>default</device>
</audio>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='first_robot'>
<link name='base_link'>
<inertial>
<pose>-0.065 0 0.016667 0 -0 0</pose>
<mass>3</mass>
<inertia>
<ixx>0.0641667</ixx>
<ixy>0</ixy>
<ixz>0.0055</ixz>
<iyy>0.219917</iyy>
<iyz>0</iyz>
<izz>0.29575</izz>
</inertia>
</inertial>
<collision name='base_link_collision'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.4 0.35 0.1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<collision name='base_link_fixed_joint_lump__front_caster_collision_1'>
<pose>-0.15 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.075</radius>
</sphere>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<collision name='base_link_fixed_joint_lump__imu_link_collision_2'>
<pose>0 0 0.075 0 -0 0</pose>
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<surface>
<contact>
<ode/>
</contact>
<friction>
<ode/>
<torsional>
<ode/>
</torsional>
</friction>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='base_link_visual'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.4 0.35 0.075</size>
</box>
</geometry>
</visual>
<visual name='base_link_fixed_joint_lump__camera_link_visual_1'>
<pose>-0.175 0 0.1 0 -0 3.14</pose>
<geometry>
<box>
<size>0.05 0.25 0.1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Blue</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<visual name='base_link_fixed_joint_lump__front_caster_visual_2'>
<pose>-0.15 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.075</radius>
</sphere>
</geometry>
</visual>
<visual name='base_link_fixed_joint_lump__imu_link_visual_3'>
<pose>0 0 0.075 0 -0 0</pose>
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</visual>
<visual name='base_link_fixed_joint_lump__lidar_link_visual_4'>
<pose>-0.215 0 0 0 -0 1.57</pose>
<geometry>
<cylinder>
<length>0.05</length>
<radius>0.05</radius>
</cylinder>
</geometry>
<material>
<script>
<name>Gazebo/DarkGrey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<sensor name='camera1' type='camera'>
<visualize>1</visualize>
<update_rate>5</update_rate>
<camera name='head'>
<horizontal_fov>1.39626</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name='camera_controller' filename='libgazebo_ros_camera.so'>
<cameraName>/camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
</plugin>
<pose>-0.175 0 0.1 0 -0 3.14</pose>
<alwaysOn>1</alwaysOn>
</sensor>
<sensor name='gazebo_imu' type='imu'>
<visualize>1</visualize>
<update_rate>30</update_rate>
<always_on>1</always_on>
<plugin name='imu_plugin' filename='libgazebo_ros_imu_sensor.so'>
<ros>
<namespace>/first_robot</namespace>
<remapping>~/out:=imu</remapping>
</ros>
<initial_orientation_as_reference>0</initial_orientation_as_reference>
<output_type>sensor_msgs/Imu</output_type>
<frame_name>imu_link</frame_name>
</plugin>
<imu>
<angular_velocity>
<x>
<noise type='gaussian'>
<mean>0</mean>
<stddev>0.0002</stddev>
<bias_mean>7.5e-06</bias_mean>
<bias_stddev>8e-07</bias_stddev>
</noise>
</x>
<y>
<noise type='none'/>
<nosie type='gaussian'>
<mean>0.0</mean>
<stddev>2e-04</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</nosie>
</y>
<z>
<noise type='none'/>
<nosie type='gaussian'>
<mean>0.0</mean>
<stddev>2e-04</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</nosie>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type='none'/>
<nosie type='gaussian'>
<mean>0.0</mean>
<stddev>1.7e-02</stddev>
<bias_mean>0.001</bias_mean>
<bias_stddev>0.001</bias_stddev>
</nosie>
</x>
<y>
<noise type='none'/>
<nosie type='gaussian'>
<mean>0.0</mean>
<stddev>1.7e-02</stddev>
<bias_mean>0.001</bias_mean>
<bias_stddev>0.001</bias_stddev>
</nosie>
</y>
<z>
<noise type='none'/>
<nosie type='gaussian'>
<mean>0.0</mean>
<stddev>1.7e-02</stddev>
<bias_mean>0.001</bias_mean>
<bias_stddev>0.001</bias_stddev>
</nosie>
</z>
</linear_acceleration>
</imu>
<pose>0 0 0.075 0 -0 0</pose>
<gravity>1</gravity>
</sensor>
<sensor name='gazebo_lidar' type='ray'>
<always_on>1</always_on>
<visualize>1</visualize>
<update_rate>60</update_rate>
<plugin name='gazebo_lidar' filename='libgazebo_ros_ray_sensor.so'>
<ros>
<remapping>~/out:=scan</remapping>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>lidar_link</frame_name>
</plugin>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1</resolution>
<min_angle>0</min_angle>
<max_angle>3.14</max_angle>
</horizontal>
<vertical>
<samples>1</samples>
<min_angle>0</min_angle>
<max_angle>0</max_angle>
</vertical>
</scan>
<range>
<min>0.15</min>
<max>6</max>
<resolution>0.015</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<pose>-0.215 0 0 0 -0 1.57</pose>
</sensor>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<joint name='wheel_front_left_joint' type='revolute'>
<pose relative_to='base_link'>0.125 -0.2 0 1.57 -0 0</pose>
<parent>base_link</parent>
<child>wheel_front_left</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='wheel_front_left'>
<pose relative_to='wheel_front_left_joint'>0 0 0 0 -0 0</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>2</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.001</iyy>
<iyz>0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name='wheel_front_left_collision'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<length>0.05</length>
<radius>0.075</radius>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='wheel_front_left_visual'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<length>0.05</length>
<radius>0.075</radius>
</cylinder>
</geometry>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<joint name='wheel_front_right_joint' type='revolute'>
<pose relative_to='base_link'>0.125 0.2 0 1.57 -0 0</pose>
<parent>base_link</parent>
<child>wheel_front_right</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='wheel_front_right'>
<pose relative_to='wheel_front_right_joint'>0 0 0 0 -0 0</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>2</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.001</iyy>
<iyz>0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name='wheel_front_right_collision'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<length>0.05</length>
<radius>0.075</radius>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='wheel_front_right_visual'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<length>0.05</length>
<radius>0.075</radius>
</cylinder>
</geometry>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<static>0</static>
<plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
<ros>
<remapping>cmd_vel:=cmd_vel</remapping>
<namespace>/first_robot</namespace>
<remapping>odom:=odom</remapping>
</ros>
<left_joint>wheel_front_left_joint</left_joint>
<right_joint>wheel_front_right_joint</right_joint>
<wheel_separation>0.4</wheel_separation>
<wheel_diameter>0.15</wheel_diameter>
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>
<publish_odom>1</publish_odom>
<publish_odom_tf>0</publish_odom_tf>
<publish_wheel_tf>1</publish_wheel_tf>
<update_rate>30</update_rate>
<odometry_topic>odom</odometry_topic>
<odometry_frame>odom</odometry_frame>
<base_frame_id>base_footprint</base_frame_id>
<robot_base_frame>base_link</robot_base_frame>
<command_topic>cmd_vel</command_topic>
</plugin>
<pose>0 0 0 0 -0 0</pose>
</model>
<state world_name='default'>
<sim_time>85 425000000</sim_time>
<real_time>86 534032298</real_time>
<wall_time>1705369601 344027836</wall_time>
<iterations>85425</iterations>
<model name='cafe'>
<pose>3.26542 -6.22712 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>3.26542 -6.22712 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='first_robot'>
<pose>3.57979 -6.67003 0.264809 0 -6e-05 -1.01821</pose>
<scale>1 1 1</scale>
<link name='base_link'>
<pose>3.57979 -6.67003 0.264809 0 -6e-05 -1.01821</pose>
<velocity>0.000423 -0.00075 -0.001719 0.009915 0.005781 -8.2e-05</velocity>
<acceleration>1.02157 -1.1232 -1.94689 -2.91743 0.52183 0.271105</acceleration>
<wrench>3.06472 -3.36959 -5.84068 0 -0 0</wrench>
</link>
<link name='wheel_front_left'>
<pose>3.47516 -6.88141 0.264816 1.5697 -0.777665 -1.01743</pose>
<velocity>0.000406 -0.00078 -0.003219 0.008333 0.008348 -0.001588</velocity>
<acceleration>0.68783 -0.831266 -6.43748 -2.82733 -0.143385 2.74644</acceleration>
<wrench>1.37566 -1.66253 -12.875 0 -0 0</wrench>
</link>
<link name='wheel_front_right'>
<pose>3.81563 -6.67145 0.264817 1.56992 0.466972 -1.0186</pose>
<velocity>0.000419 -0.000798 -0.003094 0.008328 0.008356 -0.001205</velocity>
<acceleration>0.790008 -0.992654 -6.18865 -1.05285 0.973515 -2.77191</acceleration>
<wrench>1.58002 -1.98531 -12.3773 0 -0 0</wrench>
</link>
</model>
<model name='ground_plane'>
<pose>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose>0 0 10 0 -0 0</pose>
</light>
</state>
<model name='cafe'>
<static>1</static>
<link name='link'>
<collision name='main_floor'>
<pose>-0.4 -0.75 0.0948 0 -0 0</pose>
<geometry>
<box>
<size>9.38 22.63 0.19</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<collision name='wall_1'>
<pose>-5.03 0.53 1.415 0 -0 0</pose>
<geometry>
<box>
<size>0.12 23.16 2.83</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<collision name='wall_2'>
<pose>4.24 -0.31 1.415 0 -0 0</pose>
<geometry>
<box>
<size>0.12 21.48 2.83</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<collision name='wall_3'>
<pose>-1.48 7.43 1.415 0 -0 0</pose>
<geometry>
<box>
<size>7.2 0.12 2.83</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<collision name='wall_4'>
<pose>2.09 8.9 1.435 0 -0 0</pose>
<geometry>
<box>
<size>0.12 3.05 2.87</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<collision name='wall_5'>
<pose>3.155 10.4 1.435 0 -0 0</pose>
<geometry>
<box>
<size>2.13 0.12 2.87</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<collision name='wall_6'>
<pose>0.615 -10.98 1.415 0 -0 0</pose>
<geometry>
<box>
<size>7.36 0.15 2.83</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<collision name='wall_7'>
<pose>-4.62 -10.98 1.415 0 -0 0</pose>
<geometry>
<box>
<size>0.93 0.15 2.83</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<collision name='wall_8'>
<pose>-3.61 -10.98 2.69 0 -0 0</pose>
<geometry>
<box>
<size>1.09 0.15 0.28</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<collision name='coke'>
<pose>-4.385 0.26 0.95 0 -0 0</pose>
<geometry>
<box>
<size>1.03 1.03 1.52</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>model://cafe/meshes/cafe.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>-0.223247 0.216961 0 0 -0 0</pose>
</model>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>2.6866 -8.93942 3.93553 0 0.895643 0.960195</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>
launch
first_localization.launch.py
import os
from ament_index_python.packages import get_package_share_directory
import launch
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
package_dir = get_package_share_directory("ros2_first_test")
urdf = os.path.join(package_dir, "urdf" , "first_robot.urdf")
rviz = os.path.join(package_dir, "rviz" , "first_robot.rviz")
ekf_path = os.path.join(package_dir, "config", 'first_ekf.yaml')
# use_sim_time = LaunchConfiguration('use_sim_time', default='true')
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
world_path = os.path.join(package_dir, 'world', 'first_world.sdf')
return LaunchDescription([
launch.actions.DeclareLaunchArgument(name='gui', default_value='True',
description='Flag to enable joint_state_publisher_gui'),
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
arguments=[urdf],
),
Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
output="screen"
# arguments=[urdf],
),
Node(
package='robot_localization',
executable='ekf_node',
name='ekf_filter_node',
output='screen',
parameters=[ekf_path, {'use_sim_time': use_sim_time}]),
# gazebo settings
#launch.actions.ExecuteProcess(
# cmd=["gazebo", "--verbose", "-s", "libgazebo_ros_factory.so"],),
launch.actions.ExecuteProcess(
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so', world_path], output='screen'),
Node(
package="gazebo_ros",
executable="spawn_entity.py",
name="urdf_spawner",
output="screen",
parameters=[{'use_sim_time': use_sim_time}],
arguments=["-topic", "/robot_description", "-entity", "first_robot"],
),
Node(
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz],
parameters=[{'use_sim_time': use_sim_time}],
output="screen",
),
])first_slam.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
package_dir = get_package_share_directory("ros2_first_test")
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
slam_params = os.path.join(package_dir, "config", 'first_slam_settings.yaml')
return LaunchDescription([
Node(
parameters=[
{"slam_params_file": slam_params},
{'use_sim_time': use_sim_time}
],
package='slam_toolbox',
executable='async_slam_toolbox_node',
name='slam_toolbox',
output='screen'),
])configs
first_ekf.yaml
### 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 theinputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 10.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: false
# Whether to broadcast the transformation over the /tf topic. Defaultsto 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" ifunspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified
odom0: first_robot/odom
odom0_config: [true, true, false, # position of xyz
false, false, true, # angle of rpy
true, true, false, # velocity of xyz
false, false, true, # angular velocity of rpy
true, true, false,] # acceleraion of xyz
# imu0: first_robot/imu
# imu0_config: [true, true, false, # position of xyz
# false, false, true, # angle of rpy
# true, true, false, # velocity of xyz
# false, false, true, # angular velocity of rpy
# true, true, false] # acceleraion of xyz
first_localization.yaml
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
use_map_saver: true
mode: mapping #localization
# 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: test_steve
# 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: 5.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.0
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.5
minimum_travel_heading: 0.5
scan_buffer_size: 200
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実行方法
cd ~/ros2_ws
colcon build
source install/setup.bash
ros2 launch ros2_first_test first_slam.launch.py別のターミナルを開きます。
source install/setup.bash
ros2 launch ros2_first_test first_localization.launch.pyこれで実行完了です。
少し待つと、Rviz2 上にマップが生成されていきます。
エラー対応
[async_slam_toolbox_node-1] [INFO] [1705533866.974027065] [slam_toolbox]: Message Filter dropping message: frame ‘lidar_link’ at time 97.682 for reason ‘discarding message because the queue is full’
と表示される場合は
first_localization.yaml の 44 行目
scan_buffer_size: 200の数値を増やしてください。
おわりに
今回は imu なしで map を生成するための備忘録です。
この後は Map 作成完結編へと続きます。
コメント