ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

# Odom Publisher by Subscribing Encoder Ticks for Differential Drive

I am trying to calculate odometry position and visualize in rviz. The movement can not be visualize properly although there is no syntax error

import math
from math import sin, cos, pi

import rospy
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
from odompackage.msg import EncoderTick

Left_Ticks=0
Right_Ticks=0

def tickcallback(msg):
global Left_Ticks
global Right_Ticks
Left_Ticks = msg.left
Right_Ticks = msg.right

ex_Left_Ticks = 0
ex_Right_Ticks = 0

rospy.init_node('odometry_publisher')

odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
tick_sub =rospy.Subscriber("TicksPublisher",EncoderTick,tickcallback)

current_time = rospy.Time.now()
last_time = rospy.Time.now()

r = rospy.Rate(10.0)

R = 0.0325        #//Wheel Radius
N = 40            #// Number of Ticks Per revolution
L = 0.125         #//Distance between left and right wheels

x = 0.0
y = 0.0
th = 0.0

vx =  0.0
vy =  0.0
vth =  0.0

dc = 0.0
dr = 0.0
dl = 0.0
dt = 0.0
dx = 0.0
dy = 0.0
dtheta = 0.0

while not rospy.is_shutdown():
current_time = rospy.Time.now()

L_delta_Tick = Left_Ticks - ex_Left_Ticks
R_delta_Tick = Right_Ticks - ex_Right_Ticks
dt = (current_time - last_time).to_sec()

dl = 2 * pi * R * L_delta_Tick / N
dr = 2 * pi * R * R_delta_Tick / N
dc = (dl + dr) / 2
dtheta = (dr - dl) / L
if dr==dl:
dx=dr*cos(th)
dy=dr*sin(th)
else:
dx=cos(dtheta)*(x-iccX)-sin(dtheta)*(y-iccY)+iccX-x
dy=sin(dtheta)*(x-iccX)-cos(dtheta)*(y-iccY)+iccY-y
x += dx  # delta_x
y += dy #delta_y
th =(th+dtheta)% (2 * pi) #delta_th
odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)

# first, we'll publish the transform over tf
(x, y, 0.),
odom_quat,
current_time,
"odom"
)

# next, we'll publish the odometry message over ROS
odom = Odometry()

odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))
vx=dx/dt
vy=dy/dt
vth=dtheta/dt
odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))

odom_pub.publish(odom)

ex_Left_Ticks=Left_Ticks
ex_Right_Ticks = Right_Ticks
last_time = current_time
r.sleep()


This is my encoder data publisher in arduino

#include <ros.h>
#include <odompackage/EncoderTick.h>

#define encoder_LF 7
#define encoder_RF 8

int Left_Ticks=0;
int Right_Ticks=0;

ros::NodeHandle  nh;

odompackage::EncoderTick TicksMsg;
ros::Publisher TicksPub("TicksPublisher", &TicksMsg);

void pciSetup(byte pin)
{
*digitalPinToPCMSK(pin) |= bit (digitalPinToPCMSKbit(pin));  // enable pin
PCIFR  |= bit (digitalPinToPCICRbit(pin)); // clear any outstanding interrupt
PCICR  |= bit (digitalPinToPCICRbit(pin)); // enable interrupt for the group
}

// Use one Routine to handle each group
ISR (PCINT2_vect) // handle pin change interrupt for D0 to D7 here
{
if (digitalRead(encoder_LF)){   // read the input pin
Left_Ticks +=1;}
}
ISR (PCINT0_vect) // handle pin change interrupt for D8 to D13 here
{
Right_Ticks += 1 ;}
}

void setup()
{
pciSetup(8);
pciSetup(7);
nh.initNode();
}

void loop()
{
TicksMsg.left=Left_Ticks;
TicksMsg.right=Right_Ticks;
TicksPub.publish(&TicksMsg);
nh.spinOnce();
delay(100);
}

edit retag close merge delete

Hi, did you find a solution for your issue ?

( 2019-06-20 02:56:34 -0600 )edit

Sort by ยป oldest newest most voted

You can use gazebo for getting odom by supplying real world data to gazebo through controllers in ros. https://youtu.be/ihuSkFIn7YU

more