ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

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

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

ktk1501 gravatar image

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

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


@gvdhoorn Hi, I have the same problem as you, can you please share the solution?

miku54 gravatar image miku54  ( 2022-12-13 07:01:01 -0500 )edit

I only edited the question, I did not post it.

gvdhoorn gravatar image gvdhoorn  ( 2022-12-13 10:30:17 -0500 )edit

Oh my God! Do you know the reason for this problem?😮

miku54 gravatar image miku54  ( 2022-12-13 19:01:23 -0500 )edit

No, I don't.

I'd recommend checking the answer below.

gvdhoorn gravatar image gvdhoorn  ( 2022-12-14 02:45:43 -0500 )edit

1 Answer

Sort by » oldest newest most voted

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

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

Question Tools

1 follower


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

Seen: 813 times

Last updated: Jul 15 '19