0
$\begingroup$

Rosanswers logo

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" />
        <param name="laser_max_dist" value = "5.3" />
        <param name="laser_min_dist" value = "0.1" />
        
        <!-- Advertising config --> 
        <param name="advertise_map_service" value="true"/>
        
        <param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
        <param name="scan_topic" value="$(arg scan_topic)"/>
        
        <!-- Debug parameters -->
        <!--
          <param name="output_timing" value="false"/>
          <param name="pub_drawings" value="true"/>
          <param name="pub_debug_output" value="true"/>
        -->
        <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
      </node>

      <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="hector_trajectory_server" output="screen">
        <param name="target_frame_name" type="string" value="/map" />
        <param name="source_frame_name" type="string" value="$(arg trajectory_source_frame_name)" />
        <param name="trajectory_update_rate" type="double" value="$(arg trajectory_update_rate)" />
        <param name="trajectory_publish_rate" type="double" value="$(arg trajectory_publish_rate)" />
      </node>

      <node pkg="hector_geotiff" type="geotiff_node" name="hector_geotiff_node" output="screen" launch-prefix="nice -n 15">
        <remap from="map" to="/dynamic_map" />
        <param name="map_file_path" type="string" value="$(arg map_file_path)" />
        <param name="map_file_base_name" type="string" value="$(arg map_file_base_name)" />
        <param name="geotiff_save_period" type="double" value="45" />
        <param name="draw_background_checkerboard" type="bool" value="true" />
        <param name="draw_free_space_grid" type="bool" value="true" />
        <param name="plugins" type="string" value="hector_geotiff_plugins/TrajectoryMapWriter" />
      </node>

      <node pkg="tf" type="static_transform_publisher" name="odom_base_broadcaster" args="0 0 0 0 0 0 /map /base_link 50"/> 
    
      <node pkg="tf" type="static_transform_publisher" name="laser_base_broadcaster" args="0 0 0 0 0 0 /base_link /laser 50"/>

</launch>

UPDATE3: here is an image created of the map created in Rviz by hector mapping. i start at the Blue marker and walk around the room to create the map, but as you can see the Path information is not there and rosservice call trajectory is not returning anything useful either. otherwise movements relative to frames are perfect despite im using a 10HZ lidar. Image:: hector_img.png

tf tree pub_map_odom_transform set to true. tf_tree

Is there anything that i could be missing here or something has changed in the hector slam?

Thank you!

Originally posted by sobot on ROS Answers with karma: 116 on 2015-03-15

Post score: 0

Original comments

Comment by Andromeda on 2015-03-15:
Do you publish your position over /odom ? If yes it is really easy to achieve your goal.

Anyway there are many ways to get that information. If you have odom I ll tell you what I would do in that case.

Comment by sobot on 2015-03-15:
@Andromeda well, as u can see in the code I want to save the pose with regards to the /map frame that comes from gmapping. and yes my robot does publish the /odom info. - i'm a bit lost in the cpp coding aspect of it. save and retrieve (for comparing).

Comment by Stefan Kohlbrecher on 2016-02-03:
So in rviz the pose of the robot is tracked correctly with regards to the /map frame?

Comment by sobot on 2016-02-03:
yes. works even better and faster than Gmapping for me.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

hector_trajectory_server allows keeping track of a robot's trajectory. It keeps track of a trajectory based on tf and publishes it as a nav_msgs/Path that can directly be visualized in rviz.

To install:

sudo apt-get install ros-indigo-hector-slam

(if your are using indigo, otherwise substiture "indigo" with the ROS distro you use).

You can then use the package in one of your launch files like so:

  <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="hector_trajectory_server" output="screen">
    <param name="target_frame_name" type="string" value="/map" />
    <param name="source_frame_name" type="string" value="/base_link" />
    <param name="trajectory_update_rate" type="double" value="4" />
    <param name="trajectory_publish_rate" type="double" value="0.25" />
  </node>

/edit: Your tf tree is wrong, there is a static transform publisher publishing the map->base_link transform (confusingly named odom_base_broadcaster). This job is supposed to be performed by hector_mapping. You should definitely comment that broadcaster out. Afterwards, hector_mapping should be publishing the necessary tf data in the output of view_frames.

Originally posted by Stefan Kohlbrecher with karma: 24361 on 2015-03-15

This answer was ACCEPTED on the original site

Post score: 3

Original comments

Comment by sobot on 2016-02-02:
Hello dear Stefan, I tried this at the time you answered me and i could at least visualize the trajectory, today i tried it again with a fresh install of hector_slam and it seems that trajectory_server doesn't work. updated this question already, or should i post a new question? cheers.

Comment by Stefan Kohlbrecher on 2016-02-03:
Please edit your original post with your launch file for the trajectory server. Do you get any debug output on your terminal (like tf failure warnings)?

Comment by sobot on 2016-02-03:
updated, launch file is basically a working mix of the ones provided by the hector_slam samples. and no i dont get any errors or anything. I see the /trajectory topic and can add path to RViz but it doesnt visualize lines.

Comment by Stefan Kohlbrecher on 2016-02-03:
I think you probably want to set pub_map_odom_transform to true.

Comment by sobot on 2016-02-03:
just did that, no luck with trajectory visualisation. do i need to remove the packages and do a clean install maybe?

Comment by Stefan Kohlbrecher on 2016-02-03:
I don't think so, this really hasn't changed for a long time. Do you see the mapping process in rviz (i.e. do you see the base_link frame moving relative to /map)?

Comment by sobot on 2016-02-03:
I just tried a clean install on my machine, also a new install on a VM Hydro with the same config. everything works perfectly movements relative to /map are precise. its just that the Trajectory info is not there, strange. i posted an image of the resulted map in the post.

Comment by Stefan Kohlbrecher on 2016-02-03:
The map actually can look plausible (and the poseupdate get written out) with pub_map_odom_transform set to false. Can you add the pdf output of "rosrun tf view_frames" to your post?

Comment by Stefan Kohlbrecher on 2016-02-03:
See edited answer.

Comment by sobot on 2016-02-03:
Lovely! it worked. Thanks So MUCH. may i get a little clarification though? can Hector SLAM replace Gmapping as a slam solution? can it recover from failures like that in PF based slam? slow Gmapping updates and occasional jumps are causing us some problems & am looking for a faster solution.

Comment by Stefan Kohlbrecher on 2016-02-03:
Yes, hector_mapping can be used as a drop-in replacement for gmapping. It does not track multiple hypotheses, but in many applications is reliable enough so this is not a disadvantage.

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.