Map is not being built_3D, Trajectories: two points are required for a LINE_STRIP marker. Constraints: inter constraints, different trajectories/4 -- Points should not be empty

[bmblbe1234]

Hello all,

I'm new to ROS and Cartographer, and I'm trying to build a 3D system that is mainly formed of a VLP16-LiDAR and a PhidgetSpatial IMU.

I was able to build all the required files: backpack_3D.launch, backpack_3D.lua, and VLP16-imu.urdf I was able to get a display and visualize the rellationship between the frames of references (base_link, velodyne, imu_link).

However, the map is not being built. The whole display moves when the system moves instead of building up the map as it should be. I attached a photo below to show my display.

Results

Also I get a bunch of errors from the trajectories, and constraints in Rviz:

Trajectory 0/0 -- At least two points are required for a LINE_STRIP marker. Inter constraints, different trajectories/4 -- Points should not be empty for specified marker type. Marker is fully transparent (color.a is 0.0). Inter residuals, different trajectories/5 -- Points should not be empty for specified marker type. Marker is fully transparent (color.a is 0.0).

Also some warnings I get in the terminal. I think it's more related to Velodyne as follows:

[ WARN] [1719343404.763616210]: W0625 15:23:24.000000  6742 range_data_collator.cc:82] Dropped 2 earlier points.
[ WARN] [1719343404.860702600]: Packet containing angle overflow, first angle: 35963 second angle: 2
[ WARN] [1719343407.587768861]: W0625 15:23:27.000000  6742 range_data_collator.cc:82] Dropped 4 earlier points.
[ WARN] [1719343408.093899590]: W0625 15:23:28.000000  6742 range_data_collator.cc:82] Dropped 2 earlier points.

Would you please recommend what i should do? I'd appreciate any help/guidance to fix this.

I attached below my configuration and launch files I used:

Launch file

<launch>

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

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

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

  <include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
    <arg name="device_ip" value="192.168.1.201"/> 
    <arg name="frame_id" value="velodyne"/>
  </include>

  <node pkg="nodelet" type="nodelet" name="imu_manager" args="manager" output="screen" />

  <node pkg="nodelet" type="nodelet" name="PhidgetsSpatialNodelet" args="load phidgets_spatial/PhidgetsSpatialNodelet imu_manager" output="screen">
    <param name="frame_id" value="kvh_link"/> 
    <param name="data_interval_ms" value="4"/>
  </node>

  <node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_map_to_base" args="0 0 0 0 0 0 map base_link" />

  <!-- Cartographer Node -->
  <node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" 
        args="-configuration_directory $(find cartographer_ros)/configuration_files -configuration_basename my_adapt_backpack_3d_2.lua" output="screen">
    <remap from="points2" to="velodyne_points" /> 
    <remap from="imu" to="/imu/data_raw" />
  </node>

  <!-- Cartographer Occupancy Grid Node -->
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros" type="cartographer_occupancy_grid_node" args="-resolution 0.05" output="screen" />

  <!-- RViz -->
  <node name="rviz" pkg="rviz" type="rviz" required="true" args="-d $(find cartographer_ros)/configuration_files/demo_3d_5.rviz" />

</launch>

Lua file

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "kvh_link", 
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = false, 
  publish_frame_projected_to_2d = false,
  --use_pose_extrapolator = true 
  use_odometry = false,
  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.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.0,
  odometry_sampling_ratio = 1.0,   
  fixed_frame_pose_sampling_ratio = 1.0,
  imu_sampling_ratio = 1.0,   
  landmarks_sampling_ratio = 1.0,
}

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1  
TRAJECTORY_BUILDER_3D.min_range = 0.4
TRAJECTORY_BUILDER_3D.max_range = 100.0
TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = true  

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 4

POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 40 
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 50 
POSE_GRAPH.constraint_builder.min_score = 0.55
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60

return options

URDF file

<robot name="VLP16_head">
  <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="kvh_link">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <box size="0.06 0.04 0.02" />
      </geometry>
      <material name="orange" />
    </visual>
  </link>

  <link name="velodyne">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <cylinder length="0.07" radius="0.05" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

  <link name="base_link" />

  <joint name="imu_to_base" type="fixed">
    <parent link="base_link" />
    <child link="kvh_link" />
    <origin xyz="-0.01 -0.005 -0.05" rpy="0 0 0" />
  </joint>

  <joint name="velodyne_to_base" type="fixed">
    <parent link="base_link" />
    <child link="velodyne" />
    <origin xyz="0 0.1334 -0.3175" rpy="0 0 0" />
  </joint>
</robot>

[bufeng-12]

您好！我已收到你的邮件！谢谢！          ——曾君

[bmblbe1234]

I still haven't been able to fix this. I'd be grateful if anyone can help!