In order to simulate the google cartographer:
After the installation is complete, first use the official rosbag for experimentation, and then try to use your own rosbag file!
Important references:
https://google-cartographer-ros.readthedocs.io/en/latest/ros_api.html
https://google-cartographer-ros.readthedocs.io/en/latest/configuration.html
https://www.guyuehome.com/19781
Need to configure your own .lua file
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false,
publish_frame_projected_to_2d = true,
use_pose_extrapolator = true,
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 50e-3,
trajectory_publish_period_sec = 50e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.min_range=0.1
TRAJECTORY_BUILDER_2D.max_range=16.0
TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.05
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
return optionsExplanation of important parameters:
The address of the map reference system created by map_frame is the default map
Choose tracking_frame according to your own situation. If you have imu, it’s better to choose imu_link, otherwise you can choose base_link
published_frame publishes the subject's reference system, because the zero point of the odom system in my bag file is different from the map, I choose relative odom here
odom_frame: here is set to odom, if provide_odom_frame is set to true, a warning will appear, Ignoring transform from authority "unknown_publisher" with frame_id and child_frame_id "odom" because they are the same, so I set it to false here
publish_frame_projected_to_2d = true means that only two-dimensional poses are published, and there is no pitch angle, etc.
I use laserscan and odom here, so I set it to true, use_nav_sat means GPS positioning, I don’t have use_landmarks here
num_laserscans=1 means there is only one radar, echoes echo radar does not
samplling_ratio represents the weight of the credibility of each data, which can be adjusted according to your own situation!
Some parameters in TRAJECTORY_BUILDER_2D are adjusted according to their own sensors and needs. Use_online_correlative_scan_matching is set to true to indicate real-time closed-loop detection!
ROSLAUNCH file
<launch>
<arg name = "rosbag_file" value = "/home/x/xxx.bag"/>
<!--machine name="local_alt" address="localhost" default="true" /-->
<param name="/use_sim_time" value="true"/>
<!--node name="stage_ros" pkg="stage_ros" type="stageros" args="$(find stage_ros)/xxxx.world" /-->
<node name="player" pkg="rosbag" type="play" args=" -r 1.0 $(arg rosbag_file)" cwd="node" required="true"/>
<!--param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" /-->
<!--node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /-->
<node name="rviz" pkg="rviz" type="rviz" required="true" args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
<node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" args=" -configuration_directory $(find cartographer_ros)/configuration_files -configuration_basename cartographer.lua" output="screen">
<!--remap from="scan" to="/stage/xx/ranger_0/laserscan" /-->
<!--remap from="odom" to="/stage/xx/odometry"/-->
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros" type="cartographer_occupancy_grid_node" args="-resolution 0.1" />
</launch>If urdf file is used, this needs to enable robot state publisher
Since the coordinate system name in my bag file does not meet actual needs, I need to modify it because another python node is enabled for forwarding
#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped, Twist
from sensor_msgs.msg import LaserScan
from tf2_msgs.msg import TFMessage
import tf2_ros
def tf_frame(x):
return {
'/stage/xx/odom': 'odom',
'/stage/xx/base_footprint': 'base_footprint',
'/stage/xx/base_link':'base_link',
'/stage/xx/base_laser_link':'base_laser_link'
}.get(x,x)
def tf_callback(msg):
tfs = []
for tf in msg.transforms:
t = TransformStamped()
t.header = tf.header
#print 'tf',tf.header.frame_id
#print 'tf2',tf.child_frame_id
t.header.frame_id = tf_frame(tf.header.frame_id)
t.child_frame_id = tf_frame(tf.child_frame_id)
t.transform=tf.transform
tfs.append(t)
tfm = TFMessage(tfs)
pub_tf.publish(tfm)
def odom_callback(msg):
odo = Odometry()
odo.header = msg.header
#print 'odom', msg.header.frame_id
odo.header.frame_id=tf_frame(msg.header.frame_id)
odo.child_frame_id = 'base_link'
odo.pose = msg.pose
odo.twist = msg.twist
pub_odom.publish(odo)
def ls_callback(msg):
ls = LaserScan()
ls.header = msg.header
#print 'laserscan',msg.header.frame_id
ls.header.frame_id = tf_frame(msg.header.frame_id)
ls.angle_min = msg.angle_min
ls.angle_max = msg.angle_max
ls.angle_increment = msg.angle_increment
ls.range_min = msg.range_min
ls.range_max = msg.range_max
ls.ranges = msg.ranges
ls.intensities = msg.intensities
pub_ls.publish(ls)
if __name__ == '__main__':
rospy.init_node('broadcaster_for_cartographer')
#br = tf2_ros.TransformBroadcaster()
pub_tf = rospy.Publisher('/tf', TFMessage, queue_size=1)
rospy.Subscriber('/tf', TFMessage, tf_callback,queue_size=1)
pub_odom = rospy.Publisher('odom', Odometry, queue_size=1)
rospy.Subscriber('/stage/xx/odometry', Odometry, odom_callback,queue_size=100)
pub_ls = rospy.Publisher('scan', LaserScan, queue_size=1)
rospy.Subscriber('/stage/xx/ranger_0/laserscan', LaserScan, ls_callback,queue_size=100)
rospy.spin()The final effect is as follows:
