cartographer_ros intel.bag Intel Research Lab Radish Datasets

clf_to_rosbag.py

#!/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 nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
from math import pi
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import Point, Quaternion, TransformStamped
import tf

def make_tf_msg(x, y, theta, t):
    trans = TransformStamped()
    trans.header.stamp = t #+ rospy.Duration(0.05)
    trans.header.frame_id = 'odom'
    trans.child_frame_id = 'base_link'
    trans.transform.translation.x = x
    trans.transform.translation.y = y
    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

def make_odom_msg(x, y, theta, t):
    trans = Odometry()
    trans.header.stamp = t #+ rospy.Duration(0.05)
    trans.header.frame_id = 'odom'
    trans.child_frame_id = 'base_link'
    trans.pose.pose.position = Point(x,y,0)

    quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)

    trans.pose.pose.orientation.x = quaternion[0]
    trans.pose.pose.orientation.y = quaternion[1]
    trans.pose.pose.orientation.z = quaternion[2]
    trans.pose.pose.orientation.w = quaternion[3]

    return trans

with open('intel.clf') as dataset:
    with rosbag.Bag('intel_odom.bag', 'w') as bag:
        for line in dataset.readlines():
            line = line.strip()
            tokens = line.split(' ')
            if len(tokens) <= 2:
                continue
            if tokens[0] == 'FLASER':
                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 = 'laser_link'
                t = rospy.Time(float(tokens[(num_scans + 8)]))
                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#0.2 / 360.0
                msg.scan_time = 0#0.2
                msg.range_min = 0.001
                msg.range_max = 50.0
                msg.ranges = [float(r) for r in tokens[2:(num_scans + 2)]]
                msg.ranges.append(float(0))

                bag.write('scan', msg, t)

                odom_x, odom_y, odom_theta = [float(r) for r in tokens[(num_scans + 2):(num_scans + 5)]]
                tf_msg = make_tf_msg(odom_x, odom_y, odom_theta, t)
                odom_msg = make_odom_msg(odom_x, odom_y, odom_theta, t)
                bag.write('tf', tf_msg, t)
                bag.write('odom', odom_msg, t)

            elif tokens[0] == 'ODOM':
                odom_x, odom_y, odom_theta = [float(t) for t in tokens[1:4]]
                t = rospy.Time(float(tokens[7]))
                tf_msg = make_tf_msg(odom_x, odom_y, odom_theta, t)
                odom_msg = make_odom_msg(odom_x, odom_y, odom_theta, t)
                bag.write('tf', tf_msg, t)
                bag.write('odom', odom_msg, t)

intel_backpack_2d.urdf

<!--
  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.
-->

<robot name="cartographer_backpack_2d">
  <material name="orange">
    <color rgba="1.0 0.5 0.2 1" />
  </material>
  <material name="gray">
    <color rgba="0.2 0.2 0.2 1" />
  </material>

  <link name="laser_link">
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <cylinder length="0.05" radius="0.03" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>


  <link name="base_link" />

  <joint name="laser_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="laser_link" />
    <origin xyz="0.0 0 0.0" />
  </joint>

  


</robot>

intel_cartographer.launch

<launch>
  <param name="/use_sim_time" value="true" />

  <!--<node pkg="tf" type="static_transform_publisher" name="map_publisher" args="0.0 0 0 0 0 0 map odom 100" />-->
  <!--<node pkg="tf" type="static_transform_publisher" name="map1_publisher" args="0.0 0 0 0 0 0 odom base_link 100" />-->

  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/intel_backpack_2d.urdf" />

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename intelbag.lua"
      output="screen">
    <!--<remap from="scan" to="FLASER" />
    <remap from="odom" to="ODOM" />-->
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.025" />

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
  <node name="playbag" pkg="rosbag" type="play"
      args="--clock -r 1 $(arg bag_filename)" />
</launch>

intelbag.lua

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

TRAJECTORY_BUILDER_2D = {
  use_imu_data = false,                         --true
  min_range = 0.,
  max_range = 50.,                              --30.
  min_z = -0.8,
  max_z = 2.,
  missing_data_ray_length = 5.,
  num_accumulated_range_data = 1,
  voxel_filter_size = 0.025,

  adaptive_voxel_filter = {
    max_length = 0.5,
    min_num_points = 200,
    max_range = 50.,
  },

  loop_closure_adaptive_voxel_filter = {
    max_length = 0.9,
    min_num_points = 100,
    max_range = 50.,
  },

  use_online_correlative_scan_matching = true,  --false
  real_time_correlative_scan_matcher = {
    linear_search_window = 0.1,
    angular_search_window = math.rad(180.),     --20
    translation_delta_cost_weight = 10.,        --1e-1
    rotation_delta_cost_weight = 1e-1,          --1e-1
  },

  ceres_scan_matcher = {
    occupied_space_weight = 1.,
    translation_weight = 10.,
    rotation_weight = 1.,                       --40.
    ceres_solver_options = {
      use_nonmonotonic_steps = false,
      max_num_iterations = 20,
      num_threads = 4,                          --1
    },
  },

  motion_filter = {
    max_time_seconds = 5.,
    max_distance_meters = 0.2,
    max_angle_radians = math.rad(1.),
  },

  imu_gravity_time_constant = 10.,

  submaps = {
    num_range_data = 20,                       -- 90
    grid_options_2d = {
      grid_type = "PROBABILITY_GRID",
      resolution = 0.05,
    },
    range_data_inserter = {
      range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
      probability_grid_range_data_inserter = {
        insert_free_space = true,
        hit_probability = 0.55,
        miss_probability = 0.49,
      },
    },
  },
}

TRAJECTORY_BUILDER = {
  trajectory_builder_2d = TRAJECTORY_BUILDER_2D,
  trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
  pure_localization = false,
}


options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map", --map
  tracking_frame = "base_link", --odom
  published_frame = "odom", --odom
  odom_frame = "odom_c", --odom
  provide_odom_frame = true, -- false
  publish_frame_projected_to_2d = false,
  use_odometry = true, -- 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.1, -- 0.2 0.61
  submap_publish_period_sec = 0.3, --0.3
  pose_publish_period_sec = 5e-3, --0.2 5e-3
  trajectory_publish_period_sec = 30e-3, --30e-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

-- POSE_GRAPH.optimize_every_n_nodes = 0
POSE_GRAPH.optimize_every_n_nodes = 0 --90
-- POSE_GRAPH.constraint_builder.min_score = 0.95

-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth = 7
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.branch_and_bound_depth = 8
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.full_resolution_depth = 3

POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight = 1e5
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight = 1e5
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e4
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e4

-- POSE_GRAPH.optimization_problem.huber_scale = 1e1
POSE_GRAPH.constraint_builder.log_matches = true


-- MAP_BUILDER.use_trajectory_builder_2d = true
-- TRAJECTORY_BUILDER_2D.use_imu_data = false

-- POSE_GRAPH.optimize_every_n_nodes = 0

-- TRAJECTORY_BUILDER_2D.min_range = 0.1
-- TRAJECTORY_BUILDER_2D.max_range = 40.
-- TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
-- TRAJECTORY_BUILDER_2D.submaps.num_range_data = 90
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.insert_free_space = true
-- 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.missing_data_ray_length = 1.

-- TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 5.
-- TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.2
-- TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(1.)

-- TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.025

-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 50.

-- TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05


-- 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.angular_search_window = math.rad(180.)
-- 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-2 -- 1e-1


-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1e-2
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.use_nonmonotonic_steps = false
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 30
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.num_threads = 4

-- POSE_GRAPH.optimization_problem.huber_scale = 1e2
-- POSE_GRAPH.constraint_builder.min_score = 0.65

return options

intelbag_bak.lua

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map", --map
  tracking_frame = "base_link", --odom
  published_frame = "odom", --odom
  odom_frame = "odom_c", --odom
  provide_odom_frame = true, -- false
  publish_frame_projected_to_2d = false,
  use_odometry = true, -- 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.1, -- 0.2 0.61
  submap_publish_period_sec = 0.3, --0.3
  pose_publish_period_sec = 5e-3, --0.2 5e-3
  trajectory_publish_period_sec = 30e-3, --30e-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.use_imu_data = false

POSE_GRAPH.optimize_every_n_nodes = 0

TRAJECTORY_BUILDER_2D.min_range = 0.1
TRAJECTORY_BUILDER_2D.max_range = 40.
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
-- TRAJECTORY_BUILDER_2D.submaps.num_range_data = 100
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 15
-- TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.

-- TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.5
-- TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.04
-- TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(2.)

TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.025

-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 20
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 1

TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05


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.angular_search_window = math.rad(180.)
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 -- 1e-1


TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.use_nonmonotonic_steps = false
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 20
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.num_threads = 4

-- POSE_GRAPH.optimization_problem.huber_scale = 1e2
-- POSE_GRAPH.constraint_builder.min_score = 0.65

return options