Exercise 4.2 - model not responding

[adamSB]

I’m guessing there is something wrong with my code but I’m not seeing it. I believe I defined the matrix and vector appropriately. Please help me see what I’m doing wrong. Here is my source:

########################################################
import rospy, math, numpy as np
from std_msgs.msg import Float32MultiArray
from utilities import reset_world

def twist2wheels(wz, vx, vy):
l = 986
w = 662

model = np.array([
[-l-w, 1, -1],
[l+w, 1, 1],
[l+w, 1, -1],
[-l-w, 1, 1],
]) * 1/254

twist = [
    [wz],
    [vx], 
    [vy],
]

u.append(np.dot(model, twist))
return u

rospy.init_node(‘holonomic_controller’, anonymous=True)
pub = rospy.Publisher(‘wheel_speed’, Float32MultiArray, queue_size=10)

u=
u = twist2wheels(wz=1.5, vx=1, vy=0)
print(u)
msg = Float32MultiArray(data=u)
pub.publish(msg)
rospy.sleep(1)
stop = [0,0,0,0]
msg = Float32MultiArray(data=stop)
pub.publish(msg)

[girishkumar.kannan]

Hi @adamSB ,

I faced the exact same issue when I was learning that course.

The solution is to add rospy.sleep(1.0) after initializing the publisher.

rospy.init_node(‘holonomic_controller’, anonymous=True)
pub = rospy.Publisher(‘wheel_speed’, Float32MultiArray, queue_size=10)
rospy.sleep(1.0)  # add the line here <<<----------

The above is a quick solution only. The proper solution (best-practice) is to wait for at least one connection to the publisher as explained in this post: How to publish once (only one message) into a topic and get it to work

I remember asking the exact same question. Here is the link to actual solution provided by @albertoezquerro : [SOLVED] Holonomic Robot Not Moving - #2 by albertoezquerro

This should definitely fix the problem you are having.

Regards,
Girish