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

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

asked 2015-03-15 00:15:04 -0600

sobot gravatar image

updated 2016-02-03 14:23:11 -0600

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;
            ros::Time now=ros::Time::now();
            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());


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

    seq: 0
      secs: 1454454715
      nsecs: 262124169
    frame_id: /map
      x: 0.0
      y: 0.0
      z: 0.0
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

UPDATE2 Launch file:

      <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 ...
edit retag flag offensive close merge delete


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.

Andromeda gravatar image Andromeda  ( 2015-03-15 01:41:52 -0600 )edit

@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).

sobot gravatar image sobot  ( 2015-03-15 02:21:38 -0600 )edit

So in rviz the pose of the robot is tracked correctly with regards to the /map frame?

Stefan Kohlbrecher gravatar image Stefan Kohlbrecher  ( 2016-02-03 04:13:25 -0600 )edit

yes. works even better and faster than Gmapping for me.

sobot gravatar image sobot  ( 2016-02-03 13:18:53 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2015-03-15 04:46:25 -0600

updated 2016-02-03 15:08:39 -0600

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" />

/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.

edit flag offensive delete link more


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.

sobot gravatar image sobot  ( 2016-02-02 17:23:33 -0600 )edit

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)?

Stefan Kohlbrecher gravatar image Stefan Kohlbrecher  ( 2016-02-03 03:21:38 -0600 )edit

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.

sobot gravatar image sobot  ( 2016-02-03 04:01:55 -0600 )edit

I think you probably want to set pub_map_odom_transform to true.

Stefan Kohlbrecher gravatar image Stefan Kohlbrecher  ( 2016-02-03 04:21:15 -0600 )edit

just did that, no luck with trajectory visualisation. do i need to remove the packages and do a clean install maybe?

sobot gravatar image sobot  ( 2016-02-03 04:38:39 -0600 )edit

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)?

Stefan Kohlbrecher gravatar image Stefan Kohlbrecher  ( 2016-02-03 05:30:58 -0600 )edit

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.

sobot gravatar image sobot  ( 2016-02-03 13:13:01 -0600 )edit

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?

Stefan Kohlbrecher gravatar image Stefan Kohlbrecher  ( 2016-02-03 13:24:51 -0600 )edit

Question Tools

1 follower


Asked: 2015-03-15 00:15:04 -0600

Seen: 576 times

Last updated: Feb 03 '16