Code for keeping a list of the "Pose" that Turtlebot has visited.

edit

Hello experts,

i am trying to write a node in cpp that keeps track of the positions that my Turtlebot already traveled to. the reason is that I want my node to compare any given nav goals against the said (visited) pose list and accepts the given nav goal if it is not already in the list. - what im trying to achieve here is a 2D full coverage method. (got to be a challenge to me!!!)

im using a kobuki base, hokuyo lidar, gmapping to build a map in ros indigo.

According to tutorials i came up with the code to retrieve the robot's pose in the /map frame, now i wanna save it and as im new to cpp programming, can anyone can help me with the piece of code that i need to add to my node in order to make it happen?

a piece of the Code where pose is looked up:

tf::StampedTransform transform;
        try{
            ros::Time now=ros::Time::now();
            listener.waitForTransform("/map","/base_link",now,ros::Duration(3.0));
            listener.lookupTransform("/map","/base_link",ros::Time(0), transform);
            ROS_INFO("Got a transform! x = %f, y = %f",transform.getOrigin().x(),transform.getOrigin().y()); //to check results
        }
        catch (tf::TransformException ex){

            ROS_ERROR("%s" ,ex.what());
       }

UPDATE:

Today I made a fresh installed of hector_slam on my Indigo to work with trajectory_server, everything works perfect with hector_mapping as i get a consistent map, good pose updates, etc. But the hector_trajectory_server doesnt seem to work. i use the same instructions as Stefan gave below in his comment along with hector_mapping.

My tf tree: map > base_link > laser

output of rosservice call trajectory is a series of

  header: 
    seq: 0
    stamp: 
      secs: 1454454715
      nsecs: 262124169
    frame_id: /map
  pose: 
    position: 
      x: 0.0
      y: 0.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

UPDATE2 Launch file:

    <launch>
      <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
      <arg name="base_frame" default="/base_link"/>
      <arg name="odom_frame" default="/base_link"/>
      <arg name="pub_map_odom_transform" default="true"/>
      <arg name="scan_subscriber_queue_size" default="5"/>
      <arg name="scan_topic" default="scan"/>
      <arg name="map_size" default="2048"/>
      <arg name="trajectory_source_frame_name" default="/base_link"/>
      <arg name="trajectory_update_rate" default="4"/>
      <arg name="trajectory_publish_rate" default="0.25"/>
      <arg name="map_file_base_name" default="hector_slam_map"/>
      <arg name="map_file_path" default="$(find test_nav)/maps"/>  

      <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">

        <!-- Frame names -->
        <param name="map_frame" value="/map" />
        <param name="base_frame" value="$(arg base_frame)" />
        <param name="odom_frame" value="$(arg odom_frame)" />

        <!-- Tf use -->
        <param name="use_tf_scan_transformation" value="true"/>
        <param name="use_tf_pose_start_estimate" value="false"/>
        <param name="pub_map_scanmatch_transform" value="false"/>
        <param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>

        <!-- Map size / start point -->
        <param name="map_resolution" value="0.05"/>
        <param name="map_size" value="$(arg map_size)"/>
        <param name="map_start_x" value="0.5"/>
        <param name="map_start_y" value="0.5" />
        <param name="map_multi_res_levels" value="2" />

        <!-- Map update parameters -->
        <param name="update_factor_free" value="0.4"/>
        <param name="update_factor_occupied" value="0.9" />    
        <param name="map_update_distance_thresh" value="0.4"/>
        <param name="map_update_angle_thresh" value="0.06" />
        <param name="laser_z_min_value" value = "-1.0" />
        <param name="laser_z_max_value" value = "1.0 ...