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

building a map using slam_gmapping

asked 2015-03-25 10:16:54 -0500

nouf gravatar image

updated 2015-03-28 06:07:43 -0500

Hello,

I am using ubuntu 14.04 and indigo version.

Also, I used kinect and custom made robot with encoders.

I published laser scan, odom, and tf information and now I'm starting building the map but I faced some problems

This the launch file for publishing laser scan information:

<launch>    
  <arg name="camera" default="camera"/>     
  <include file="$(find openni_launch)/launch/openni.launch">   
  <arg name="camera" default="$(arg camera)"/>  
  </include>    
  <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">   
  <remap from="cloud_in" to="/camera/depth/points"/>    
  </node>   
</launch>

And I can view the laser scans successfully in rviz.

This is the node to publish odom information (the nodes assumes the robot is moving in circle):

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "odometry_publisher");

  ros::NodeHandle n;
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  tf::TransformBroadcaster odom_broadcaster;

  double x = 0.0;
  double y = 0.0;
  double th = 0.0;

  double vx = 0.1;
  double vy = -0.1;
  double vth = 0.1;

  ros::Time current_time, last_time;
  current_time = ros::Time::now();
  last_time = ros::Time::now();

  ros::Rate r(1.0);
  while(n.ok()){

    ros::spinOnce();               // check for incoming messages
    current_time = ros::Time::now();

    //compute odometry in a typical way given the velocities of the robot
    double dt = (current_time - last_time).toSec();
    double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
    double delta_th = vth * dt;

    x += delta_x;
    y += delta_y;
    th += delta_th;

    //since all odometry is 6DOF we'll need a quaternion created from yaw
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

    //first, we'll publish the transform over tf
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;

    //send the transform
    odom_broadcaster.sendTransform(odom_trans);

    //next, we'll publish the odometry message over ROS
    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;

    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = vth;

    //publish the message
    odom_pub.publish(odom);

    last_time = current_time;
    r.sleep();
  }
}

I followed the steps in this tutorial

When I reached this command : rosbag play --clock mylaserdata.bag the other terminal gives this error:

Warning: TF_OLD_DATA ignoring data from the past for frame base_laser at time 1.4273e+09 according to authority unknown_publisher
Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
         at line 260 in /tmp/buildd/ros-indigo-tf2-0.5.7-0trusty-20141230-0040/src/buffer_core.cpp

Also, after typing this command: rosrun map_server map_saver it says Waiting for the map and didn't do any thing.

UPDATE:

I used robot_state_publisher package to publish the TF

and this is the TF of the robot: TF

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2015-03-27 11:29:51 -0500

updated 2015-03-29 05:57:54 -0500

Dear nouf,

did you remember to set use_sim_time param to true?

rosparam set use_sim_time true

Please note that your odometry publisher node has a problem in main loop:

ros::spinOnce();       

[...]

odom_broadcaster.sendTransform(odom_trans);

[...]

odom_pub.publish(odom);

r.sleep();

Following execution flow you:

  • broadcast tf;
  • publish odom topic;
  • wait for rate;
  • invoke ros::spinOnce().

I remember you that tf and topics are not really published until you invoke ros::spinOnce(): so, basically, you are publishing this information with one second delay. I suggest you to move ros::spinOnce() just after odom topic publisher line.

Hope this help...

UPDATE

First: I suggest you to modify your code in order to have tf published in right way. Further more you are broadcasting odom -> base_link transformation with at 1Hz rate and base_link -> base_laser transformation at 100Hz and this is not a good practice. I cannot tell you that this will fix your problem but you should fix them before go on.

Second: run

rosrun tf view_frames

and

roswtf

The first will create a PDF contains the tf tree pf your robot: you can us it in order to be sure tf chain is not broken. The second will start some automatic diagnostic tool in order to check your transformation health status.

UPDATE

Sorry for late reply.... there a lot of inconsistence in your tf tree:

  • map -> odom transformation is missing. This should be provided by gmapping so if it was not running when you get view frames this is ok;
  • posted log refer to a base_laser frame but in your tf tree you named it laser_link (I assume both them refer to laser frame of course :-) );
  • base_link -> camera_link transformation is missing. If should be provided by robot_state_publisher too.

Try to fix those issues. Can you also post output from roswtf?

edit flag offensive delete link more

Comments

Yes I did set use_sim_time to true. and thank you for your suggestion. Although I think there is problem somewhere in TF. I think I missed something because I really got confused reading about it. I read all the tutorials and explanations in wiki.ros but I didn't get full understanding!

nouf gravatar image nouf  ( 2015-03-27 12:19:59 -0500 )edit

May you please explain in brief what should I do in steps regarding TF? Thank you

nouf gravatar image nouf  ( 2015-03-27 12:20:51 -0500 )edit

May you please see the update?

nouf gravatar image nouf  ( 2015-03-28 06:08:15 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2015-03-25 10:16:54 -0500

Seen: 1,644 times

Last updated: Mar 29 '15