Speed when turning

Course Support Basic Kinematics of Mobile Robots
1

Hi, I write 2 code: publish twist and convert velocities for each wheels

  • Publish Twist
    #! /usr/bin/env python

import rospy
from geometry_msgs.msg import Twist

class PublishTwist():

def __init__(self):
    self.publisher = rospy.Publisher("/twist_vels", Twist, queue_size=1)    
    self.twistVels = Twist()
    
    self.rate10Hz = rospy.Rate(10)
    self.rate1Hz  = rospy.Rate(1)

    self.ctrlC = False
    rospy.on_shutdown(self.shutdownhook)


def shutdownhook(self):
    self.ctrlC = True

    self.twistVels.linear.x = 0
    self.twistVels.angular.z = 0
    self.publisher.publish(self.twistVels)
    
    rospy.loginfo("Stop")

def publish(self, linearSpeed=0.2, angularSpeed=0.2):
    self.twistVels.linear.x = linearSpeed
    self.twistVels.angular.z = angularSpeed
    i = 0

    while self.ctrlC is False:
        self.publisher.publish(self.twistVels)
        rospy.loginfo("twistVels published - {0}".format(i))
        i = i + 1
        rospy.sleep(3)
        rospy.loginfo("Wake up")

if name == “main”:
# rospy.init_node(“publish_twist_node”, anonymous=True)
rospy.init_node(“publish_twist_node”)
publishTwist = PublishTwist()

try:
    publishTwist.publish(linearSpeed=0.2, angularSpeed=0.2)
except rospy.ROSInterruptException as e:
    print(e)
  • Convert velocities
    #! /usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Float64

class SubscribeTwist():

def __init__(self):
    self.subscriber = rospy.Subscriber("/twist_vels", Twist, self.callback)    
    self.twistVels  = Twist()

    self.vLeftPublisher  = rospy.Publisher("/left_wheel_controller/command", Float64, queue_size=1)
    self.vLeft      = Float64()

    self.vRightPublisher  = rospy.Publisher("/right_wheel_controller/command", Float64, queue_size=1)
    self.vRight     = Float64()

    self.rate10Hz = rospy.Rate(10)
    self.rate1Hz  = rospy.Rate(1)

    self.ctrlC = False
    rospy.on_shutdown(self.shutdownhook)

    self.L = 0.3
    self.R = 0.1


def shutdownhook(self):
    self.ctrlC = True

    self.vLeft.data      = 0
    self.vRight.data     = 0

    self.vLeftPublisher.publish(self.vLeft)
    self.vRightPublisher.publish(self.vRight)
   
    rospy.loginfo("Stop")

def callback(self, msg):
    linearXVel  = msg.linear.x
    angularZVel = msg.angular.z

    self.vLeft.data     = (2*linearXVel + angularZVel*self.L)/(2*self.R)
    self.vRight.data    = (2*linearXVel - angularZVel*self.L)/(2*self.R) 

    rospy.loginfo("callback is called")

def publish(self):
    i = 0

    while self.ctrlC is False:
        self.vLeftPublisher.publish(self.vLeft)
        self.vRightPublisher.publish(self.vRight)

        rospy.loginfo("leftWheelCommand published - {0}".format(i))
        rospy.loginfo("rightWheelCommand published - {0}".format(i))
       
        i = i + 1
        rospy.sleep(3)
        rospy.loginfo("Wake up")

if name == “main”:
# rospy.init_node(“publish_twist_node”, anonymous=True)
rospy.init_node(“subscribe_twist_node”)
subscribeTwist = SubscribeTwist()

try:
    # rospy.spin()
    subscribeTwist.publish()
except rospy.ROSInterruptException as e:
    print(e)

So, when linearSpeedX=0.2, angularSpeedX=0.2. Robot will turn left. The speed of left side has to be smaller than the right side.

But When I check my program.
→ linearSpeedX=0.2, angularSpeedX=0.2. Robot will turn left
→ But the speed of left side is higher than the right side. ???
Could you help me to understand this case?

user:~$ rostopic echo /left_wheel_controller/command -n1
WARNING: no messages received and simulated time is active.
Is /clock being published?
data: 2.3

user:~$ rostopic echo /right_wheel_controller/command -n1
WARNING: no messages received and simulated time is active.
Is /clock being published?
data: 1.7

2

Angular Speed is rotation around Z-Axis, To reverse the rotation , you can reverse the Sign “-” in angularSpeed

Then your Robot will turn in Opposite direction.

1 Like
3

Hi @NguyenDuyDuc,
It looks like there might be an issue in your calculation for the left and right wheel velocities in the callback method of the SubscribeTwist class. Let’s go through the relevant part of the code:

def callback(self, msg):
    linearXVel  = msg.linear.x
    angularZVel = msg.angular.z

    self.vLeft.data     = (2*linearXVel + angularZVel*self.L)/(2*self.R)
    self.vRight.data    = (2*linearXVel - angularZVel*self.L)/(2*self.R) 

    rospy.loginfo("callback is called")

if you expect the left wheel to have a smaller speed when turning left, you might need to adjust the signs in your formulas.

Here’s an updated version of your callback method:

def callback(self, msg):
    linearXVel  = msg.linear.x
    angularZVel = msg.angular.z

    # Adjust signs to make left wheel slower when turning left
    self.vLeft.data     = (2*linearXVel - angularZVel*self.L)/(2*self.R)
    self.vRight.data    = (2*linearXVel + angularZVel*self.L)/(2*self.R) 

    rospy.loginfo("callback is called")

I changed the signs in the calculation for the left wheel velocity (self.vLeft.data ). This adjustment should make the left wheel slower when the robot is turning left. Please try this modification and see if it resolves the issue.

good luck
Hamdi

4

Hi,

def callback(self, msg):
linearXVel = msg.linear.x
angularZVel = msg.angular.z

# Adjust signs to make left wheel slower when turning left
self.vLeft.data     = (2*linearXVel - angularZVel*self.L)/(2*self.R)
self.vRight.data    = (2*linearXVel + angularZVel*self.L)/(2*self.R) 

rospy.loginfo("callback is called")

I use the code. And the result is that The robot will turn right if I set:
try:
publishTwist.publish(linearSpeed=0.2, angularSpeed=0.2)
except rospy.ROSInterruptException as e:
print(e)

If linearSpeed=0.2, angularSpeed=0.2, the robot has to turn left and left speed has to be smaller than right speed. But it is not true, that’s my problem. Please help

5

@NguyenDuyDuc could you please share the output of terminal here when you run the codes.

6

I send output of the terminal in the question on the top. Please check

7

This is just to check but you re-built the package? I know in python and ros1 it isn’t necessary but it could be the case because if we take a mathematics approach to the formula you are right, the robot is supposed to turn left, right velocity is set with those parameters to 2.3 and left velocity to 1.7, I have three ideas of what could be the problem:

  1. The code isn’t taking the new changes of the code
  2. At some part while publishing it is inverting the velocity values
  3. There is a problem with the robot topics (I don’t think this is the one)
8

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.