当前位置：网站首页>Learning notes - data set generation

# message_name [message contents] ipc_timestamp ipc_hostname logger_timestamp
# message formats defined: PARAM SYNC ODOM FLASER RLASER TRUEPOS 
# PARAM param_name param_value
# SYNC tagname
# ODOM x y theta tv rv accel
# FLASER num_readings [range_readings] x y theta odom_x odom_y odom_theta
# RLASER num_readings [range_readings] x y theta odom_x odom_y odom_theta
# TRUEPOS true_x true_y true_theta odom_x odom_y odom_theta
# NMEA-GGA utc latitude lat_orient longitude long_orient gps_quality num_sattelites hdop sea_level alitude geo_sea_level geo_sep data_age

The modified code is as follows

#!/usr/bin/env python
# -*- coding: utf-8 -*-

# Software License Agreement (BSD License)
#
# Copyright (c) 2016, Martin Guenther
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

'''This is a converter for the Intel Research Lab SLAM dataset
   ( http://kaspar.informatik.uni-freiburg.de/~slamEvaluation/datasets/intel.clf )
   to rosbag'''

import rospy
import rosbag
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from math import pi
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import TransformStamped
import tf

def make_tf_msg(x, y, z,theta, t,parentid,childid):
    trans = TransformStamped()
    trans.header.stamp = t
    trans.header.frame_id = parentid
    trans.child_frame_id = childid
    trans.transform.translation.x = x
    trans.transform.translation.y = y
    trans.transform.translation.z = z
    q = tf.transformations.quaternion_from_euler(0, 0, theta)
    trans.transform.rotation.x = q[0]
    trans.transform.rotation.y = q[1]
    trans.transform.rotation.z = q[2]
    trans.transform.rotation.w = q[3]

    msg = TFMessage()
    msg.transforms.append(trans)
    return msg

with open('aces.clf') as dataset:
    with rosbag.Bag('MIT_Infinite_Corridor_Dataset.bag', 'w') as bag:
        for line in dataset.readlines():
            line = line.strip()
            tokens = line.split(' ')
            if len(tokens) <= 2:
                continue
            if tokens[0] == 'FLASER':  #FLASER This is the original laser data marked in the original file , Generally, there is no need to change 
                msg = LaserScan()
                num_scans = int(tokens[1])

                if num_scans != 180 or len(tokens) < num_scans + 9:
                    rospy.logwarn("unsupported scan format")
                    continue

                msg.header.frame_id = 'base_laser'  # This is the laser coordinate system id, Change... As needed , stay ROS When reading data in, you will use , Generally, there is no need to change .
                t = rospy.Time(float(tokens[(num_scans + 8)])) # Acquisition time 
                msg.header.stamp = t
                msg.angle_min = -90.0 / 180.0 * pi
                msg.angle_max = 90.0 / 180.0 * pi
                msg.angle_increment = pi / num_scans
                msg.time_increment = 0.2 / 360.0
                msg.scan_time = 0.2
                msg.range_min = 0.001
                msg.range_max = 50.0
                msg.ranges = [float(r) for r in tokens[2:(num_scans + 2)]]

                bag.write('scan', msg, t)  # This is the topic of laser data release , Change according to the specific topic subscribed by the node program .

                odom_x, odom_y, odom_theta = [float(r) for r in tokens[(num_scans + 2):(num_scans + 5)]]
                tf_msg = make_tf_msg(0.1, 0, 0.2, 0, t,'base_link','base_laser')
                bag.write('tf', tf_msg, t)

            elif tokens[0] == 'ODOM':  # Mark odometer data 
                odom_x, odom_y, odom_theta = [float(t) for t in tokens[1:4]]
                t = rospy.Time(float(tokens[7]))
                msg=Odometry()

                msg.header.stamp=t
                msg.header.frame_id='odom'
                msg.child_frame_id='base_link'
                msg.pose.pose.position.x=odom_x
                msg.pose.pose.position.y=odom_y
                msg.pose.pose.position.z=0
                q = tf.transformations.quaternion_from_euler(0, 0, odom_theta)
                msg.pose.pose.orientation.x = q[0]
                msg.pose.pose.orientation.y = q[1]
                msg.pose.pose.orientation.z = q[2]
                msg.pose.pose.orientation.w = q[3]
                bag.write('odom', msg, t) 

                # tf_msg = make_tf_msg(odom_x, odom_y,0 , odom_theta, t,'odom','base_link')
                # bag.write('tf', tf_msg, t)

GitHub - Robots90/clf2bag: Transfer clf file to bag file for ROS

.log .clf File to .bag file github_Robots. The blog of -CSDN Blog

Run the laser with the data set slam,gmapping/karto_ Fan Kun 3371 The blog of -CSDN Blog

Refer to the following picture pairs corrected-log For further processing

obtain odom data

obtain lidar The data can be referred to ：

carmen_to_rosbag/old_laser_scan.py at master · art32fil/carmen_to_rosbag · GitHub

The final revision is as follows ：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
 
# Software License Agreement (BSD License)
#
# Copyright (c) 2016, Martin Guenther
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
 
'''This is a converter for the Intel Research Lab SLAM dataset
   ( http://kaspar.informatik.uni-freiburg.de/~slamEvaluation/datasets/intel.clf )
   to rosbag'''
 
import rospy
import rosbag
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from math import pi
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import TransformStamped
import tf
 
def make_tf_msg(x, y, z,theta, t,parentid,childid):
    trans = TransformStamped()
    trans.header.stamp = t
    trans.header.frame_id = parentid
    trans.child_frame_id = childid
    trans.transform.translation.x = x
    trans.transform.translation.y = y
    trans.transform.translation.z = z
    q = tf.transformations.quaternion_from_euler(0, 0, theta)
    trans.transform.rotation.x = q[0]
    trans.transform.rotation.y = q[1]
    trans.transform.rotation.z = q[2]
    trans.transform.rotation.w = q[3]
 
    msg = TFMessage()
    msg.transforms.append(trans)
    return msg
 
 #    0   |      1       | 2 - 1+num_readings | 2+num_readings | 3+num_readings |
# xLASER | num_readings |  [range_readings]  |       x        |        y       |
#-------------------------------------------------------------------------------
# 4+num_readings | 5+num_readings | 6+num_readings | 7+num_readings |
#      theta     |     odom_x     |     odom_y     |   odom_theta   |
#-------------------------------------------------------------------------------
# 8+num_readings | 9+num_readings | 10+num_readings
# ipc_timestamp | ipc_hostname  |logger_timestamp

with open('mit-killian.clf') as dataset:
    with rosbag.Bag('MIT.bag', 'w') as bag:
        for line in dataset.readlines():
            line = line.strip()
            tokens = line.split(' ')
            if len(tokens) <= 2:
                continue
            if tokens[0] == 'FLASER':  #FLASER This is the original laser data marked in the original file , Generally, there is no need to change 
                msg = LaserScan()
                num_scans = int(tokens[1])
 
                if num_scans != 180 or len(tokens) < num_scans + 9:
                    rospy.logwarn("unsupported scan format")
                    continue
 
                msg.header.frame_id = 'base_laser'  # This is the laser coordinate system id, Change... As needed , stay ROS When reading data in, you will use , Generally, there is no need to change .
                t = rospy.Time(float(tokens[(num_scans + 8)])) # Acquisition time 
                msg.header.stamp = t
                msg.angle_min = -90.0 / 180.0 * pi
                msg.angle_max = 90.0 / 180.0 * pi
                msg.angle_increment = pi / num_scans
                msg.time_increment = 0.2 / 360.0
                msg.scan_time = 0.2
                msg.range_min = 0.001
                msg.range_max = 50.0
                msg.ranges = [float(r) for r in tokens[2:(num_scans + 2)]]
 
                bag.write('scan', msg, t)  # This is the topic of laser data release , Change according to the specific topic subscribed by the node program .
 
                tf_msg = make_tf_msg(0.1, 0, 0.2, 0, t,'base_link','base_laser')
                bag.write('tf', tf_msg, t)

                # odom_x, odom_y, odom_theta = [float(r) for r in tokens[(num_scans + 2):(num_scans + 5)]] 
                # odom_x, odom_y, odom_theta = [float(r) for r in tokens[(num_scans + 5):(num_scans + 8)]]
                # msg1=Odometry()
                # msg1.header.stamp=t
                # msg1.header.frame_id='odom'
                # msg1.child_frame_id='base_link'
                # msg1.pose.pose.position.x=odom_x
                # msg1.pose.pose.position.y=odom_y
                # msg1.pose.pose.position.z=0
                # q = tf.transformations.quaternion_from_euler(0, 0, odom_theta)
                # msg1.pose.pose.orientation.x = q[0]
                # msg1.pose.pose.orientation.y = q[1]
                # msg1.pose.pose.orientation.z = q[2]
                # msg1.pose.pose.orientation.w = q[3]
                # bag.write('odom', msg1, t) 


#    0 | 1 | 2 |  3  | 4 | 5 |  6  |      7      |      8     |        9
#  ODOM| x | y |theta| tv| rv|accel|ipc_timestamp|ipc_hostname|logger_timestamp 
            elif tokens[0] == 'ODOM':  # Mark odometer data 
                odom_x, odom_y, odom_theta = [float(t) for t in tokens[1:4]]
                t = rospy.Time(float(tokens[7]))
                msg=Odometry()
                msg.header.stamp=t
                msg.header.frame_id='odom'
                msg.child_frame_id='base_link'
                msg.pose.pose.position.x=odom_x
                msg.pose.pose.position.y=odom_y
                msg.pose.pose.position.z=0
                q = tf.transformations.quaternion_from_euler(0, 0, odom_theta)
                msg.pose.pose.orientation.x = q[0]
                msg.pose.pose.orientation.y = q[1]
                msg.pose.pose.orientation.z = q[2]
                msg.pose.pose.orientation.w = q[3]
                bag.write('odom', msg, t) 
 
                # tf_msg = make_tf_msg(odom_x, odom_y,0 , odom_theta, t,'odom','base_link')
                # bag.write('tf', tf_msg, t)

run MIT Infinite Corridor Dataset The effect is as follows ：

The parameter documents used are as follows

-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
--      http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,                -- map_builder.lua Configuration information 
  trajectory_builder = TRAJECTORY_BUILDER,  -- trajectory_builder.lua Configuration information 
  
  map_frame = "map",                        --  The name of the map coordinate system 
  tracking_frame = "base_laser",              --  Convert all sensor data into this coordinate system 
  published_frame = "base_link",            -- tf: map -> published_frame
  odom_frame = "odom",                      --  The name of the odometer's coordinate system 
  provide_odom_frame = true,               --  Whether to provide odom Of tf,  If true, be tf The tree is map->odom->published_frame
                                            --  If false tf The tree is map->published_frame

  publish_frame_projected_to_2d = false,    --  Whether to project the coordinate system onto the plane 
  use_pose_extrapolator = true,            --  Release tf When using pose_extrapolator The pose of is still the pose calculated by the front end 

  use_odometry = true,                     --  Use odometer , If it is required to use, there must be odom Of tf
  use_nav_sat = false,                      --  Whether to use gps
  use_landmarks = false,                    --  Whether to use landmark
  num_laser_scans = 1,                      --  Whether to use single line laser data 
  num_multi_echo_laser_scans = 0,           --  Whether to use multi_echo_laser_scans data 
  num_subdivisions_per_laser_scan = 1,      -- 1 The frame data is divided into several processes , It's usually 1
  num_point_clouds = 0,                     --  Whether to use point cloud data 
  
  lookup_transform_timeout_sec = 0.2,       --  lookup tf Time out for 
  submap_publish_period_sec = 0.3,          --  Time interval for publishing data 
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,

  rangefinder_sampling_ratio = 1.,          --  Sampling frequency of sensor data 
  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.min_range = 0.001
TRAJECTORY_BUILDER_2D.max_range = 100.
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35

TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false  -- Do not use imu data 
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1

POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65

-- -- ####################################################################################################
-- MAP_BUILDER.use_trajectory_builder_2d = true
-- MAP_BUILDER.num_background_threads =6  -- It is recommended to optimize to each machine CPU Number of threads 
 
-- TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1 -- Accumulate several frames of laser data as a standard unit scan
-- TRAJECTORY_BUILDER_2D.min_range = 0.01  -- The nearest effective distance of the laser 
-- TRAJECTORY_BUILDER_2D.max_range = 26.   -- The farthest effective distance of the laser （ The acceptable range of adjustment parameters is 30 - 15 Between , recommend 20-25 Less error .）
-- -- TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5. -- Invalid laser data set the distance to this value 
-- TRAJECTORY_BUILDER_2D.use_imu_data = false  -- Whether to use imu data 
-- TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1 
-- -- Line distance search box , Within the size of this box , Search for the best scan matching    Reducing this parameter can enhance the effect of real-time drawing , Reduce the effect of closed-loop optimization , When forming a closed loop , More ghosting 
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher. angular_search_window = math.rad(10.) -- The size of the angle search box 
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 20.
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 2e-1  -- What affects is the effect of the process , It will indirectly affect the final optimization time 
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 30.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 30.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 7  -- The error of this parameter fluctuates greatly , When the average error is large, the maximum error is small , The average error is small and the maximum error is large , stay 7 The left and right are in relative equilibrium , recommend 7-10 To modify the parameter range 
-- TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80 --num_range_data The value set is the same as CPU There is such a relationship , Small value (10),CPU The utilization rate is relatively stable , Overall high , When it's high ,CPU Use for a short burst ( When inserting subgraphs ), Low usage rate at ordinary times , Present a state of great fluctuation .  The acceptable range of adjustment parameters is 90 - 60 Between , stay 80 The left and right effects are excellent .
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.55
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.49
-- TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.05   // Keep it as small as possible   //  If the moving distance is too small ,  Or the time is too short ,  Do not update the map 
-- TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.3)
-- TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.5

-- --  Add the following 
-- TRAJECTORY_BUILDER_2D.voxel_filter_size=0.1 -- The average error is 0.1  and 0.2 Ups and downs occur , although 0.1 The error ratio of 0.2 It's bigger , But because of 0.2 The drawing deviation is serious , It is recommended to choose less than 0.1 Value .
-- TRAJECTORY_BUILDER_2D.submaps.resolution=0.05 -- The average error is 0.1 It's the peak of time , Less than is recommended 0.1 Relatively good .
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range=30 -- The acceptable range of adjustment parameters is 50 - 20 Between , stay 30 The left and right effects are excellent .
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length=4 -- The acceptable range of adjustment parameters is 0.5 - 4 Between , stay 2 - 4 The left and right effects are excellent .
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points=200 -- The acceptable range of adjustment parameters is 100 - 200 Between ,100 The above effect is excellent .
 

-- POSE_GRAPH.optimization_problem.huber_scale = 1e2  -- Robust kernel function , Denoise 
-- POSE_GRAPH.optimize_every_n_nodes = 35   -- Back end optimization node  （ The recommended parameter modification range is 30-70, It is suggested in the official document to reduce this value to improve the speed , recommend 30 The left and right configurations are relatively good ）
-- POSE_GRAPH.global_constraint_search_after_n_seconds = 30   -- The variance and maximum error of this parameter decrease gradually with the increase of interval time , But the maximum error gradually increases , stay 30-40 At the balance of the two . recommend 30-40 To modify the parameter range 
-- POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 15  -- Optimize the number of iteration steps 
-- POSE_GRAPH.optimization_problem.ceres_solver_options.num_threads = 1
-- POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
-- POSE_GRAPH.constraint_builder.sampling_ratio = 0.25   -- The parameter is in 0.2-0.25 The left-right average error is relatively low . The position of the minimum variance is 0.25 about , The recommended parameter modification range is 0.2-0.3
-- POSE_GRAPH.constraint_builder.min_score = 0.75  -- The parameter is in 0.75-0.95 The variance and maximum error of the error are at a relatively low level , recommend 0.75-0.95 To modify the parameter range 
-- POSE_GRAPH.constraint_builder.global_localization_min_score = 0.6
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.linear_search_window = 3.
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth = 5. -- Search method , Defined branch method , Solve the problem to form a search tree ,depth Is the depth of the construction tree 
-- POSE_GRAPH.global_sampling_ratio = 0.001  -- stay 0.001,0.002 The average error is in a small position , The values that can be considered during optimization are 0.001 and 0.002
-- --  Add the following 


return options