Custom LiDAR data from vehicle #45
Description
I have recorded custom LiDAR data (VLP-16) mounted on vehicle and created PCD and waypoints. However, when I try to run that in simulation, it does not work. I am using "empty world" for simulation. It seems there is something that I am missing. It would be great if you can have a look and provide the pointers.
Below are the steps used in simulation.
==> Simulation with empty world
['roslaunch', 'sd_robot', 'sd_twizy_worlds.launch', 'world:=empty', 'enable_rviz:=True']
xacro: in-order processing became default in ROS Melodic. You can drop the option.
[Err] [REST.cc:205] Error in REST request
libcurl: (6) Could not resolve host: api.ignitionfuel.org
[INFO] [1647928810.637523, 0.000000]: Loading model XML from ros parameter robot_description
[INFO] [1647928810.653083, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
[INFO] [1647928811.557555, 0.068000]: Calling service /gazebo/spawn_urdf_model
[INFO] [1647928818.307326, 0.256000]: Spawn status: SpawnModel: Successfully spawned entity
==> On changing position of vehicle in Gazebo
[ INFO] [1647928266.899053536]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1647928266.900247978]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
Node::Advertise(): Error advertising a topic. Did you forget to start the discovery service?
Node::Advertise(): Error advertising a topic. Did you forget to start the discovery service?
==>TF Launch
['roslaunch', 'aslan_gui', 'baselink_to_localiser.launch', 'x:=0.95', 'y:=0.0', 'z:=1.11', 'yaw:=0.0', 'pitch:=0.0', 'roll:=0.0', 'frame_id:=/base_link']
==>PCD Launch
['roslaunch', 'pcd_loader', 'points_map_loader.launch', 'pcd_file:=/home/Aslan/aslan-220321.pcd']
==>TF (World to Map)
['roslaunch', 'map_tf_generator', 'map_tf_generate.launch']
==>Voxel grid filter
['roslaunch', 'voxel_grid_filter', 'voxel_grid_filter.launch', 'points_topic:=/points_raw']
==> Ray ground filter
['roslaunch', 'ray_ground_filter', 'ray_ground_filter.launch', 'input_point_topic:=/points_raw', 'no_ground_point_topic:=/points_no_ground', 'sensor_height:=1.8', 'clipping_height:=0.2', 'min_point_distance:=1.85', 'radial_divider_angle:=0.08', 'concentric_divider_distance:=0.01', 'local_max_slope:=8', 'general_max_slope:=5', 'min_height_threshold:=0.05', 'reclass_distance_threshold:=0.2']
==> NDT Matching
['roslaunch', 'lidar_localizer', 'ndt_matching.launch', 'use_odom:=False', 'use_imu:=False', 'imu_topic:=/imu_raw']
Log file:
method_type: 0
use_gnss: 0
queue_size: 1
offset: linear
get_height: 0
use_local_transform: 0
use_odom: 0
use_imu: 0
imu_upside_down: 0
imu_topic: /imu_raw
localizer: velodyne
(tf_x,tf_y,tf_z,tf_roll,tf_pitch,tf_yaw): (0.95, 0, 1.11, 0, 0, 0)
Updates the points
[ERROR] [1647929813.192455708, 559.657000000]: Could not find a connection between 'map' and 'velodyne_base_link' because they are not part of the same tree.Tf has two or more unconnected trees.
==> NDT monitor Green after sometime
['roslaunch', 'lidar_localizer', 'ndt_matching_monitor.launch', 'iteration_threshold_warn:=10', 'iteration_threshold_stop:=32', 'score_threshold_delta:=14', 'min_stable_samples:=30', 'fatal_time_threshold:=2']
==> Waypoint loader
['roslaunch', 'waypoint_maker', 'waypoint_loader.launch', 'multi_lane_csv:=/home/Aslan/saved_waypoints.csv', 'replanning_mode:=False', 'resample_mode:=True', 'resample_interval:=1', 'velocity_max:=20', 'radius_thresh:=20', 'radius_min:=6', 'velocity_min:=4', 'accel_limit:=0.98', 'decel_limit:=0.98', 'velocity_offset:=4', 'end_point_offset:=5', 'disable_decision_maker:=True']
==> Launch Obstacle search
['roslaunch', 'astar_planner', 'obstacle_avoid.launch']
DEBUGGING2: diff_time = 0.101: diff = 0.0210065
DEBUGGING2: diff_time = 0.106: diff = 0.00453382
DEBUGGING2: diff_time = 0.093: diff = 0.0112843
DEBUGGING2: diff_time = 0.104: diff = 0.0225604
DEBUGGING2: diff_time = 0.101: diff = 0.00766487
DEBUGGING2: diff_time = 0.094: diff = 0.0162595
DEBUGGING2: diff_time = 0.102: diff = 0.0103894
Warning: TF_OLD_DATA ignoring data from the past for frame velodyne at time 0.213 according to authority unknown_publisher
Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
at line 277 in /tmp/binarydeb/ros-melodic-tf2-0.6.5/src/buffer_core.cpp
Warning: TF_OLD_DATA ignoring data from the past for frame velodyne at time 0.213 according to authority unknown_publisher
Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
at line 277 in /tmp/binarydeb/ros-melodic-tf2-0.6.5/src/buffer_core.cpp
==> Velocity Request
['roslaunch', 'astar_planner', 'velocity_set.launch', 'lidar_points_topic:=points_no_ground', 'radar_points_topic:=radar/target_list_cartesian']
==> Lane Selection
['roslaunch', 'lane_planner', 'lane_select.launch', 'enablePlannerDynamicSwitch:=False']
Current vel from vehicle: 0.0945138
DEBUGGING2: diff_time = 0.096: diff = 0.0255666
Current vel from vehicle: 0.266319
DEBUGGING2: diff_time = 0.107: diff = 0.00591521
Current vel from vehicle: 0.0552824
DEBUGGING2: diff_time = 0.092: diff = 0.0113614
Current vel from vehicle: 0.123494
DEBUGGING2: diff_time = 0.1: diff = 0.00596655
(aslan_wizard.py:5763): Gtk-CRITICAL **: 11:55:27.855: gtk_box_gadget_distribute: assertion 'size >= 0' failed in GtkCheckButton
Current vel from vehicle: 0.0596655
DEBUGGING2: diff_time = 0.103: diff = 0.00122089
Current vel from vehicle: 0.0118533
DEBUGGING2: diff_time = 0.097: diff = 0.00125734
[ WARN] [1647930328.166020340, 833.752000000]: Necessary topics are not subscribed yet. Waiting...
[ WARN] [1647930328.166934191, 833.753000000]: Necessary topics are not subscribed yet. Waiting...
[ WARN] [1647930328.295092934, 833.805000000]: Necessary topics are not subscribed yet. Waiting...
[ WARN] [1647930328.295207656, 833.805000000]: current lane doesn't have change flag
==> Pursuit
['roslaunch', 'waypoint_follower', 'pure_pursuit.launch', 'is_linear_interpolation:=True', 'publishes_for_steering_robot:=False']
==> Low pass filtering
['roslaunch', 'waypoint_follower', 'twist_filter.launch']
==> On simulation
The vehicle does not move.
[ INFO] [1647931009.014193921, 1198.696000000]: kappa : 0.027029
[ WARN] [1647931009.321041523, 1198.829000000]: Necessary topics are not subscribed yet ...
twizy TwistAngular 2.09733 Steer 4
[ERROR] [1647931009.341758811, 1198.849000000]: ROSNDTMatchingMonitor FATAL CANNOT RECOVER - STOPPING.
[ WARN] [1647931009.341946545, 1198.849000000]: Necessary topics are not subscribed yet. Waiting...
[ WARN] [1647931009.342094869, 1198.850000000]: current lane doesn't have change flag
DEBUGGING2: diff_time = 0.101: diff = 0.489737
[ WARN] [1647931009.355048729, 1198.863000000]: Necessary topics are not subscribed yet ...
[ WARN] [1647931009.387945293, 1198.895000000]: Necessary topics are not subscribed yet ...
Current vel from vehicle: 4.84888
twizy TwistAngular 2.09733 Steer 4
DEBUGGING2: diff_time = 0.102: diff = 0.477731
[ WARN] [1647931009.531965770, 1198.949000000]: Necessary topics are not subscribed yet. Waiting...
[ERROR] [1647931009.531820091, 1198.949000000]: ROSNDTMatchingMonitor FATAL CANNOT RECOVER - STOPPING.
[ WARN] [1647931009.532087013, 1198.949000000]: current lane doesn't have change flag
[ WARN] [1647931009.544716230, 1198.962000000]: Necessary topics are not subscribed yet ...
Best regards,
Hitesh