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

Revision history [back]

Hello! Yes, you need set up an odometry and tf to do mapping with gmapping. Your tf tree have to look like odom->base_link->laser. You have to make launch files, for the rplidar node specifying the frame and the topic, for the gmapping node also you have to specify the topic and the frames of the laser and the odom. In this link you have a guide to obtain the best results from a rplidar on gmapping http://geduino.blogspot.com/2015/04/gmapping-and-rplidar.html. For setting up the tf you have to make a tf broadcaster that make the tf link from base_link to laser, an example code in c++:

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv){
ros::init(argc, argv, "tina_tf_publisher");
ros::NodeHandle n;

ros::Rate r(100);

tf::TransformBroadcaster broadcaster;

while(n.ok()){
broadcaster.sendTransform(
  tf::StampedTransform(
    tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.25, 0.0, 0.2)),
    ros::Time::now(),"base_link", "laser"));
r.sleep();
}
}

Also you have a program to compute odom, in our robot we have a topic from an arduino that publish the velocity and from that we have the odom, the code in python is:

#!/usr/bin/env python

import math
from math import sin, cos, pi
import rospy
import tf
from nav_msgs.msg  import Odometry
from std_msgs.msg  import Float64
from geometry_msgs.msg import Twist, Point, Quaternion, Vector3, Pose
v_x = 0
v_y = 0
vth = 0


def callback_x(msg):
   global v_x
   v_x = msg.data
def callback_y(msg):
   global v_y
   v_y = msg.data


rospy.init_node('odometry')
rospy.Subscriber('velo_x', Float64, callback_x)
rospy.Subscriber('velo_y', Float64, callback_y)
odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()  
x = 0.0
y = 0.0
th = 0.0
vx = (v_x+v_y)/2
vy = 0.0
vth = (v_x-v_y)/0.5
current_time = rospy.Time.now()
last_time = rospy.Time.now()
r = rospy.Rate(10.0)
while not rospy.is_shutdown():
   vx = (v_x+v_y)/2
   vy = 0.0
   vth = (v_x-v_y)/0.5

   current_time = rospy.Time.now()
   dt = (current_time - last_time).to_sec()
   delta_x = (vx * cos(th))*dt
   delta_y = (vx * sin(th))*dt
   delta_th = vth * dt

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

   odom_quat = tf.transformations.quaternion_from_euler(0,0,th)

   odom_broadcaster.sendTransform((x,y,0.),odom_quat,current_time,"base_link","odom")

   odom = Odometry()
   odom.header.stamp = current_time
   odom.header.frame_id = "odom"

   odom.pose.pose = Pose(Point(x,y,0.),Quaternion(*odom_quat))

   odom.child_frame_id = "base_link"
   odom.twist.twist = Twist(Vector3(vx,vy,0), Vector3(0,0,vth))

   odom_pub.publish(odom)

   last_time = current_time
   r.sleep()

I hope that it helps!