Ask Your Question

Have trouble in ‘ndt matching’ with my custom pcd file

asked 2019-07-15 00:23:05 -0600

ktk1501 gravatar image

updated 2019-07-15 00:56:21 -0600

gvdhoorn gravatar image

I am trying to run ‘ndt matching’ using my custom pcd file, which was made using ‘ndt mapping’. I made a launch file refer to ‘moriyama quick start’ demo. I uploaded my launch file :

  <arg name="period_in_ms" default="10"/>   
  <node pkg="tf" type="static_transform_publisher" name="world_to_map" args="788935 -2362 -82 0 0 0 /world /map $(arg period_in_ms)"/>  
  <!-- map_to_mobility_tf -->  
  <node pkg="tf"  type="static_transform_publisher" name="map_to_mobility" args="0 0 0 0 0 0 /map /mobility 10" />  
  <!-- pointcloud -->  
  <node pkg="map_file" type="points_map_loader" name="points_map_loader" args="noupdate $(env HOME)/Desktop/KimTaekyung/mapping2matching/190614_DATA_FIRST_NDTwIMU.pcd"/>  
  <!-- VLP-16 -->  
  <arg name="velodyne_calib" default="$(find velodyne_pointcloud)/params/VLP16db.yaml"/>  
  <include file="$(find velodyne_pointcloud)/launch/velodyne_vlp16.launch">  
    <arg name="calibration" value="$(arg velodyne_calib)"/>  
  <!-- setting path parameter -->  
  <arg name="get_height" value="true" />  
  <!-- Setup -->  
  <include file="$(find runtime_manager)/scripts/setup_tf.launch">  
    <arg name="x" value="1.2" />  
    <arg name="y" value="0.0" />  
    <arg name="z" value="2.0" />   
    <arg name="yaw" value="0.0" />  
    <arg name="pitch" value="0.0" />  
    <arg name="roll" value="0.0" />  
    <arg name="frame_id" value="/base_link" />  
    <arg name="child_frame_id" value="/velodyne" />  
    <arg name="period_in_ms" value="10"/>  
  <include file="$(find model_publisher)/launch/vehicle_model.launch" />  
  <!-- voxel_grid_filter -->  
  <arg name="sync" default="false" />  
  <arg name="node_name" default="voxel_grid_filter" />  
  <arg name="points_topic" default="velodyne_points" />  
  <arg name="output_log" default="false" />  
  <node pkg="points_downsampler" name="$(arg node_name)" type="$(arg node_name)">  
    <param name="points_topic" value="$(arg points_topic)" />  
    <remap from="/points_raw" to="/sync_drivers/points_raw" if="$(arg sync)" />  
    <param name="output_log" value="$(arg output_log)" />  
  <!-- nmea2tfpose -->  
  <include file="$(find gnss_localizer)/launch/nmea2tfpose.launch"/>  

‘ 788935 -2362 -82’ is the first GPS coordinate in my rosbag file when I ran ‘ndt mapping’. I think I had followed all I need to do, but ‘ndt matching’ is not working. The ‘base_link’ is just following the ‘gps’ frame, and ‘gps’ frame is very far from pointcloud map. So it does not perform matching properly.

I had checked the ‘frame’ coordinates of moriyama quick start demo, and my custom environment. Every coordinates are same. The only difference is, the pointcloud map is just beyond the ‘map’ frame. Moriyama demo’s pointcloud map is displayed far from ‘map’ frame, actually on the GPS coordinates.

So I had tried to shift the pointcloud map to the GPS coordinates, just as moriyama demo does. Firstly, I had tried using ‘TF’ to shift it, but pointcloud map in pcd file seems to be registered on ‘map’ frame directly. So I couldn’t modify its position using ‘TF’. Second, I had checked source code and launch file of ‘points_map_loader’ and ‘ndt_mapping’. But I failed to find out how to do.

Is there any way to solve this problem??

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted

answered 2019-07-15 14:37:38 -0600

Josh Whitley gravatar image

There are two things I recommend:

  1. Use vel_pose_connect to relay the topic /ndt_pose as /current_pose and your vehicle's current velocity from /vehicle/twist to /current_velocity. See the launch file below for this.
  2. Provide an initial pose using RViz. To do this, open RViz and center on the map frame. Find your approximate location in the PCD map and click the "2D Pose Estimate" button at the top, and then click and drag from your current location on the map in the direction that the vehicle is facing.

Example vel_pose_connect launch:

<include file="$(find autoware_connector)/launch/vel_pose_connect.launch">
  <arg name="topic_pose_stamped" value="/ndt_pose" />
  <arg name="topic_twist_stamped" value="/vehicle/twist" />

The /current_pose is used in several nodes and Autoware doesn't make an assumption about which localizer you're using to output the current pose so it requires you to either remap or republish the /ndt_pose output from the NDT matching to the /current_pose topic. The vel_pose_connect launch script does this.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2019-07-15 00:23:05 -0600

Seen: 391 times

Last updated: Jul 15 '19