-
-
Save atotto/f2754f75bedb6ea56e3e0264ec405dcf to your computer and use it in GitHub Desktop.
| #!/usr/bin/env python | |
| 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 | |
| rospy.init_node('odometry_publisher') | |
| odom_pub = rospy.Publisher("odom", Odometry, queue_size=50) | |
| odom_broadcaster = tf.TransformBroadcaster() | |
| x = 0.0 | |
| y = 0.0 | |
| th = 0.0 | |
| vx = 0.1 | |
| vy = -0.1 | |
| vth = 0.1 | |
| current_time = rospy.Time.now() | |
| last_time = rospy.Time.now() | |
| r = rospy.Rate(1.0) | |
| while not rospy.is_shutdown(): | |
| current_time = rospy.Time.now() | |
| # compute odometry in a typical way given the velocities of the robot | |
| dt = (current_time - last_time).to_sec() | |
| delta_x = (vx * cos(th) - vy * sin(th)) * dt | |
| delta_y = (vx * sin(th) + vy * cos(th)) * dt | |
| 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 | |
| odom_quat = tf.transformations.quaternion_from_euler(0, 0, th) | |
| # first, we'll publish the transform over tf | |
| odom_broadcaster.sendTransform( | |
| (x, y, 0.), | |
| odom_quat, | |
| current_time, | |
| "base_link", | |
| "odom" | |
| ) | |
| # next, we'll publish the odometry message over ROS | |
| odom = Odometry() | |
| odom.header.stamp = current_time | |
| odom.header.frame_id = "odom" | |
| # set the position | |
| odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat)) | |
| # set the velocity | |
| odom.child_frame_id = "base_link" | |
| odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth)) | |
| # publish the message | |
| odom_pub.publish(odom) | |
| last_time = current_time | |
| r.sleep() |
Hi,
I write an Arduino code to calculate the position (x, y and theta) of the differential vehicle. How can I run the code I wrote below integrated with the ros odometry code above. My goal is to obtain the odometry of a real differential vehicle. Could you please help me?
#include<math.h>
uint8_t ticksPerRevolution = 800;
float wheel_radius=12.5; //wheel radius
float distanceWheels = 58.5; // between the distance of the wheels
float x = 0;
float y = 0;
float theta = 0;
float xend = 0;
float yend = 0;
float thetaend = 0;
//void Odometry();
#define encoder1A 18
#define encoder1B 19
#define encoder2A 20
#define encoder2B 21
long encoder_left_ticks = 0;
long encoder_left_ticks_old = 0;
long dleft = 0;
// Right Encoder
long encoder_right_ticks = 0;
long encoder_right_ticks_old = 0;
long dright = 0;
void count1A(){ // left encoder tick count
if(digitalRead(encoder1A)==LOW) {
encoder_right_ticks++;
}else{
encoder_right_ticks--;}
}
void count1B(){ // left encoder tick count
if(digitalRead(encoder1B)==LOW) {
encoder_right_ticks--;
}else{
encoder_right_ticks++;}
}
void count2A(){ // right encoder tick count
if(digitalRead(encoder2A)==LOW) {
encoder_left_ticks--;
}else{
encoder_left_ticks++;}
}
void count2B(){ // right encoder tick count
if(digitalRead(encoder2B)==LOW) {
encoder_left_ticks++;
}else{
encoder_left_ticks--;}
}
void setup()
{
delay(1000);
Serial.begin(9600); //or 115200
pinMode(encoder1A, INPUT);
pinMode(encoder1B, INPUT);
pinMode(encoder2A, INPUT);
pinMode(encoder2B, INPUT);
digitalWrite(encoder1A, HIGH);
digitalWrite(encoder1B, HIGH);
digitalWrite(encoder2A, HIGH);
digitalWrite(encoder2B, HIGH);
attachInterrupt(4,count1A,RISING ); // left encoder new function
attachInterrupt(5,count1B,RISING ); // left encoder new function
attachInterrupt(2,count2A,RISING ); // right encoder new function
attachInterrupt(3,count2B,RISING ); // right encoder new function
}
void loop()
{
//Odometry();
Serial.print(x);
Serial.print('\t');
Serial.print(y);
Serial.print('\t');
Serial.print(theta); //neden 2 ile çarpılıyor ve bu thetaP ve thetaPsent nedir?
Serial.print('\t');
Serial.print(encoder_left_ticks);
Serial.print('\t');
Serial.println(encoder_right_ticks);
delay(10);
}
void Odometry()
{
float dRight = (encoder_right_ticks - encoder_right_ticks_old) * 2 * PI * wheel_radius /(double) ticksPerRevolution; //d=2pir*(deltaticks)/N
float dLeft = (encoder_left_ticks - encoder_left_ticks_old) * 2 * PI * wheel_radius / (double) ticksPerRevolution;
encoder_left_ticks_old = encoder_left_ticks;
encoder_right_ticks_old = encoder_right_ticks;
float dCenter = (dRight + dLeft) / 2;
float phi = (dRight - dLeft) / distanceWheels;
thetaend = theta + phi;
if (thetaend >= 2.0 * 3.1416) thetaend = thetaend - 2.0 * PI;
if (thetaend < 0.0) thetaend = thetaend + 2.0 * PI;
xend = x + dCenter * cos(theta);
yend = y + dCenter * sin(theta);
theta = thetaend;
x = xend;
y = yend;
}
This is outstanding - a Python conversion of the odom tutorial is exactly what I needed. I wish I had found it two hours ago :-) Thanks!
#!/usr/bin/env python3
import rospy
import sys
import math
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Quaternion
from roboclaw_3 import Roboclaw
from time import sleep
from std_msgs.msg import *
address_right = 0x80
address_left = 0x80
loop_rate = 3
class BaseController(object):
def __init__(self):
self.wheel_odom_pub = rospy.Publisher('wheel_odom', Odometry, queue_size=10)
rospy.Subscriber("cmd_vel",Twist, self.cmdVelCallback)
self.odomBroadcaster = tf.TransformBroadcaster()
self.roboclaw_port = rospy.get_param('roboclaw_motors_port',"/dev/ttyACM0")
self._wheel_base = rospy.get_param('wheel_base', 0.2358)
self._wheel_radius = rospy.get_param('wheel_radius', 0.0625)
self.baud = rospy.get_param('serial_baudrate', 115200)
self._ppr = rospy.get_param('pulse_per_revolution',398*20)
self._p_1_1 = rospy.get_param('P_1_1',4.4714) #left rear
self._p_2_1 = rospy.get_param('P_2_1',4.4714) #right rear
self._i_1_1 = rospy.get_param('I_1_1',0.5403)
self._i_2_1 = rospy.get_param('I_2_1',0.5237)
self._d_1_1 = rospy.get_param('D_1_1',0)
self._d_2_1 = rospy.get_param('D_2_1',0)
self._qpps_1_1 = rospy.get_param('QPPS_1_1',4100)
self._qpps_2_1 = rospy.get_param('QPPS_2_1',4100)
self.roboclaw = Roboclaw(self.roboclaw_port, self.baud) #Create instance
self.roboclaw.Open() #Open Roboclaws
#self.roboclaw.SetM1VelocityPID(address_left,4.4714,0.5403,0,4100)
#self.roboclaw.SetM2VelocityPID(address_right,self._p_1_2,self._i_1_2,self._d_1_2,self._qpps_1_2)
#self.roboclaw.SetM2VelocityPID(address_left,self._p_2_1,self._i_2_1,self._d_2_1,self._qpps_2_1)
#self.roboclaw.SetM2VelocityPID(address_right,self._p_2_2,self._i_2_2,self._d_2_2,self._qpps_2_2)
self.x = 0
self.y = 0
self.th = 0
self._x = 0
self._y = 0
self._z = 0
self.prev_time = rospy.Time.now()
print ("test ok")
rate_1 = rospy.Rate(loop_rate)
def cmdVelCallback(self, msg):
linear_x = msg.linear.x
angular_z = msg.angular.z
if linear_x>0 or linear_x<0 :
print("received linear data")
if angular_z>0 or angular_z<0:
print("received angular data")
else :
print("no data " )
right_wheel_speed = (linear_x / self._wheel_radius) + (self._wheel_base/self._wheel_radius * angular_z) # rad/s
left_wheel_speed = (linear_x / self._wheel_radius) - (self._wheel_base/self._wheel_radius * angular_z)
self.roboclaw.SpeedM1(address_left,self.QPPS_function(left_wheel_speed))
self.roboclaw.SpeedM2(address_left,self.QPPS_function(right_wheel_speed))
def QPPS_function(self, rad_per_sec): # rad/sec to qpps conversion
QPPS = (rad_per_sec*self._ppr)/(2*math.pi)
QPPS = int(QPPS)
return QPPS
def rad_per_sec_function(self, QPPS):
rad_per_sec = QPPS*(2*math.pi)/self._ppr
return rad_per_sec
def publish_odom(self):
rospy.loginfo("Odom !")
curr_time = rospy.Time.now()
dt = (curr_time - self.prev_time).to_sec()
self.prev_time = curr_time
left_rear = self.roboclaw.ReadSpeedM1(address_left)
right_rear = self.roboclaw.ReadSpeedM1(address_left)
right_qpps = right_rear[1] #average of right motors
left_qpps = left_rear[1] #average of left motors
l_speed = -self.rad_per_sec_function(int(left_qpps))
r_speed = self.rad_per_sec_function(int(right_qpps))
print ('left-rear= ',l_speed)
print ('right-rear= ',r_speed)
vx = (r_speed - l_speed) / 2.0
vy = 0
vth = (r_speed + l_speed) / (2 * self._wheel_base)
delta_x = (vx * math.cos(self.th) - vy * math.sin(self.th)) * dt
delta_y = (vx * math.sin(self.th) + vy * math.cos(self.th)) * dt
delta_th = vth * dt
self.x += delta_x
self.y += delta_y
self.th += delta_th
odom_quat = Quaternion()
odom_quat = tf.transformations.quaternion_from_euler(0,0,self.th)
# Create the odometry transform frame broadcaster (publish tf)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
odom_quat,
rospy.Time.now(),
"base_link",
"odom"
)
# Publish Odom
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = rospy.Time.now()
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = self._z
odom.pose.pose.orientation = Quaternion(*odom_quat)
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.wheel_odom_pub.publish(odom)
def main():
rospy.init_node("base_controller_node", anonymous=True)
_object = BaseController()
rate = rospy.Rate(loop_rate)
while not rospy.is_shutdown():
rate.sleep()
_object.publish_odom()
rospy.loginfo("Ok !")
print ("Keyboard Interrupt !")
_object.roboclaw.SpeedM1(address_left,0)
_object.roboclaw.SpeedM2(address_left,0)
#_object.roboclaw.SpeedM2(address_right,0)
if name=="main":
main()
this is my code for two motors with roboclaw but i have facing issues that is odometry values not coming
and many errors is there pls help to troubleshooot
I tried and OK, thanks