search

cartographer-project / cartographer_ros

Provides ROS integration for Cartographer.
Apache License 2.0
1.66k stars 1.21k forks source link

Could someone help me with this issue? This at times builds map sometimes doesn't, robot is unable to localize itself if there is map. How can I resolve this? Any help would be much appreciated. #1814

Open AswinKumar1 opened 8 months ago

AswinKumar1 commented 8 months ago

spot_one.launch

<launch>

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

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
    -configuration_directory $(find spot_slam)/lua
    -configuration_basename spot_one.lua"
      output="screen">

   <remap from="points2" to="/spot/lidar/points" />
   <remap from="odom" to="/spot/odometry_corrected" />
  </node>

  <node pkg="tf" type="static_transform_publisher" name="map_to_odom_publisher" args="0 0 0 0 0 0 map odom 100" />
  <node pkg="tf" type="static_transform_publisher" name="odom_to_body_publisher" args="0 0 0 0 0 0 odom body 100" />
  <node pkg="tf" type="static_transform_publisher" name="body_to_base_link_publisher" args="0 0 0 0 0 0 body base_link 100" />

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

spot.urdf

<?xml version="1.0" ?>
<robot name="spot" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <link name="body">
        <visual>
            <geometry>
                <mesh filename="package://spot_description/meshes/body.dae" />
            </geometry>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://spot_description/meshes/body_collision.stl" />
            </geometry>
        </collision>
    </link>

    <link name="base_link"/>
    <joint name="base_link_joint" type="fixed">
        <origin xyz="0 0 0" rpy="0 0 0" />
        <parent link="body" />
        <child link="base_link" />
    </joint>

    <link name="front_rail"/>
    <joint name="front_rail_joint" type="fixed">
        <origin xyz="0.223 0 0.0805" rpy="0 0 0" />
        <parent link="body" />
        <child link="front_rail" />
    </joint>

    <link name="real_front_rail"/>
    <joint name="real_front_rail_joint" type="fixed">
        <origin xyz="0.2075 0 0.0805" rpy="0 0 0" />
        <parent link="body" />
        <child link="real_front_rail" />
    </joint>

    <link name="rear_rail"/>
    <joint name="rear_rail_joint" type="fixed">
        <origin xyz="-0.223 0 0.0805" rpy="0 0 0" />
        <parent link="body" />
        <child link="rear_rail" />
    </joint>

    <link name="real_rear_rail"/>
    <joint name="real_rear_rail_joint" type="fixed">
        <origin xyz="-0.415 0 0" rpy="0 0 0" />
        <parent link="real_front_rail" />
        <child link="real_rear_rail" />
    </joint>

    <link name="front_left_hip">
        <visual>
            <geometry>
                <mesh filename="package://spot_description/meshes/front_left_hip.dae" />
            </geometry>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://spot_description/meshes/front_left_hip_collision.stl" />
            </geometry>
        </collision>
    </link>
    <joint name="front_left_hip_x" type="revolute">
        <origin xyz="0.29785 0.05500 0.00000" rpy="0 0 0" />
        <axis xyz="1 0 0" />
        <parent link="body" />
        <child link="front_left_hip" />
        <limit effort="1000" velocity="1000.00" lower="-0.78539816339744827899" upper="0.78539816339744827899" />
    </joint>
    <link name="front_left_upper_leg">
        <visual>
            <geometry>
                <mesh filename="package://spot_description/meshes/front_left_upper_leg.dae" />
            </geometry>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://spot_description/meshes/front_left_upper_leg_collision.stl" />
            </geometry>
        </collision>
    </link>
    <joint name="front_left_hip_y" type="revolute">
        <origin xyz="0.0 0.110945 0.0" rpy="0 0 0" />
        <axis xyz="0 1 0" />
        <parent link="front_left_hip" />
        <child link="front_left_upper_leg" />
        <limit effort="1000" velocity="1000.00" lower="-0.89884456477707963539" upper="2.2951079663725435509" />
    </joint>
    <link name="front_left_lower_leg">
        <visual>
            <geometry>
                <mesh filename="package://spot_description/meshes/front_left_lower_leg.dae" />
            </geometry>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://spot_description/meshes/front_left_lower_leg_collision.stl" />
            </geometry>
        </collision>
    </link>
    <joint name="front_left_knee" type="revolute">
        <origin xyz="0.025 0.000 -0.3205" rpy="0 0 0" />
        <axis xyz="0 1 0" />
        <parent link="front_left_upper_leg" />
        <child link="front_left_lower_leg" />
        <limit effort="1000" velocity="1000.00" lower="-2.7929" upper="-0.254801" />
    </joint>
    <link name="front_right_hip">
        <visual>
            <geometry>
                <mesh filename="package://spot_description/meshes/front_right_hip.dae" />
            </geometry>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://spot_description/meshes/front_right_hip_collision.stl" />
            </geometry>
        </collision>
    </link>
    <joint name="front_right_hip_x" type="revolute">
        <origin xyz="0.29785 -0.05500 0.00000" rpy="0 0 0" />
        <axis xyz="1 0 0" />
        <parent link="body" />
        <child link="front_right_hip" />
        <limit effort="1000" velocity="1000.00" lower="-0.78539816339744827899" upper="0.78539816339744827899" />
    </joint>
    <link name="front_right_upper_leg">
        <visual>
            <geometry>
                <mesh filename="package://spot_description/meshes/front_right_upper_leg.dae" />
            </geometry>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://spot_description/meshes/front_right_upper_leg_collision.stl" />
            </geometry>
        </collision>
    </link>
    <joint name="front_right_hip_y" type="revolute">
        <origin xyz="0.0 -0.110945 0.0" rpy="0 0 0" />
        <axis xyz="0 1 0" />
        <parent link="front_right_hip" />
        <child link="front_right_upper_leg" />
        <limit effort="1000" velocity="1000.00" lower="-0.89884456477707963539" upper="2.2951079663725435509" />
    </joint>
    <link name="front_right_lower_leg">
        <visual>
            <geometry>
                <mesh filename="package://spot_description/meshes/front_right_lower_leg.dae" />
            </geometry>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://spot_description/meshes/front_right_lower_leg_collision.stl" />
            </geometry>
        </collision>
    </link>
    <joint name="front_right_knee" type="revolute">
        <origin xyz="0.025 0.000 -0.3205" rpy="0 0 0" />
        <axis xyz="0 1 0" />
        <parent link="front_right_upper_leg" />
        <child link="front_right_lower_leg" />
        <limit effort="1000" velocity="1000.00" lower="-2.7929" upper="-0.247563" />
    </joint>
    <link name="rear_left_hip">
        <visual>
            <geometry>
                <mesh filename="package://spot_description/meshes/rear_left_hip.dae" />
            </geometry>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://spot_description/meshes/rear_left_hip_collision.stl" />
            </geometry>
        </collision>
    </link>
    <joint name="rear_left_hip_x" type="revolute">
        <origin xyz="-0.29785 0.05500 0.00000" rpy="0 0 0" />
        <axis xyz="1 0 0" />
        <parent link="body" />
        <child link="rear_left_hip" />
        <limit effort="1000" velocity="1000.00" lower="-0.78539816339744827899" upper="0.78539816339744827899" />
    </joint>
    <link name="rear_left_upper_leg">
        <visual>
            <geometry>
                <mesh filename="package://spot_description/meshes/rear_left_upper_leg.dae" />
            </geometry>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://spot_description/meshes/rear_left_upper_leg_collision.stl" />
            </geometry>
        </collision>
    </link>
    <joint name="rear_left_hip_y" type="revolute">
        <origin xyz="0.0 0.110945 0.0" rpy="0 0 0" />
        <axis xyz="0 1 0" />
        <parent link="rear_left_hip" />
        <child link="rear_left_upper_leg" />
        <limit effort="1000" velocity="1000.00" lower="-0.89884456477707963539" upper="2.2951079663725435509" />
    </joint>
    <link name="rear_left_lower_leg">
        <visual>
            <geometry>
                <mesh filename="package://spot_description/meshes/rear_left_lower_leg.dae" />
            </geometry>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://spot_description/meshes/rear_left_lower_leg_collision.stl" />
            </geometry>
        </collision>
    </link>
    <joint name="rear_left_knee" type="revolute">
        <origin xyz="0.025 0.000 -0.3205" rpy="0 0 0" />
        <axis xyz="0 1 0" />
        <parent link="rear_left_upper_leg" />
        <child link="rear_left_lower_leg" />
        <limit effort="1000" velocity="1000.00" lower="-2.7929" upper="-0.267153" />
    </joint>
    <link name="rear_right_hip">
        <visual>
            <geometry>
                <mesh filename="package://spot_description/meshes/rear_right_hip.dae" />
            </geometry>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://spot_description/meshes/rear_right_hip_collision.stl" />
            </geometry>
        </collision>
    </link>
    <joint name="rear_right_hip_x" type="revolute">
        <origin xyz="-0.29785 -0.05500 0.00000" rpy="0 0 0" />
        <axis xyz="1 0 0" />
        <parent link="body" />
        <child link="rear_right_hip" />
        <limit effort="1000" velocity="1000.00" lower="-0.78539816339744827899" upper="0.78539816339744827899" />
    </joint>
    <link name="rear_right_upper_leg">
        <visual>
            <geometry>
                <mesh filename="package://spot_description/meshes/rear_right_upper_leg.dae" />
            </geometry>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://spot_description/meshes/rear_right_upper_leg_collision.stl" />
            </geometry>
        </collision>
    </link>
    <joint name="rear_right_hip_y" type="revolute">
        <origin xyz="0.0 -0.110945 0.0" rpy="0 0 0" />
        <axis xyz="0 1 0" />
        <parent link="rear_right_hip" />
        <child link="rear_right_upper_leg" />
        <limit effort="1000" velocity="1000.00" lower="-0.89884456477707963539" upper="2.2951079663725435509" />
    </joint>
    <link name="rear_right_lower_leg">
        <visual>
            <geometry>
                <mesh filename="package://spot_description/meshes/rear_right_lower_leg.dae" />
            </geometry>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://spot_description/meshes/rear_right_lower_leg_collision.stl" />
            </geometry>
        </collision>
    </link>
    <joint name="rear_right_knee" type="revolute">
        <origin xyz="0.025 0.000 -0.3205" rpy="0 0 0" />
        <axis xyz="0 1 0" />
        <parent link="rear_right_upper_leg" />
        <child link="rear_right_lower_leg" />
        <limit effort="1000" velocity="1000.00" lower="-2.7929" upper="-0.257725" />
    </joint>

    <!-- Standard accessories. -->
    <xacro:include filename="$(find spot_description)/urdf/accessories.urdf.xacro" />

    <!-- Optional custom includes. -->
    <xacro:include filename="$(optenv SPOT_URDF_EXTRAS empty.urdf)" />
</robot>

spot_one.lua


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 = false,
  use_odometry = true,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 1,
  lookup_transform_timeout_sec = 0.5,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 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.num_accumulated_range_data = 1
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35

TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false

TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)

POSE_GRAPH.constraint_builder.min_score = 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7

POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35 -- Decrease
MAP_BUILDER.num_background_threads = 7 -- Increase up to number of cores
-------------
--TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10
--TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 15
--POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.angular_search_window = math.rad(15)
--POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.linear_search_window = 3.

--POSE_GRAPH.global_sampling_ratio = 0.003
--POSE_GRAPH.constraint_builder.sampling_ratio = 0.4 -- Decrease
--POSE_GRAPH.global_constraint_search_after_n_seconds = 30
--TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 5

--TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 100
--TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 10
--TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 1.0
--TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.min_num_points = 50
--TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_range = 10
--TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_length = 1.0
--TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.05
--TRAJECTORY_BUILDER_2D.max_range = 10
----------------

POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.65 -- Increase
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66

POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight = 0.5
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight = 3
POSE_GRAPH.optimization_problem.odometry_translation_weight = 3
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 0.5
return options