Progress with rospy but cant get sidestep to work

On the plus side: raspberry pi and camera fitted sorted out enabling of servos, as instructions stab use tab completion! got the first part of ball following to work, i.e. marty walks

Modified the initial walking example to check out some of the other commands hello, walk and lean work

Cant get side step to work.

I have included a snippet from the code below:

def sidestep_command (side=0, steps = 1, move_time = 2000, step_length = 50):
    '''
    < little endian
    B 15
    B side
    B steps
    H move time
    B step length
    '''
    cmd=struct.pack('<BBBHB',15,side, steps, move_time,step_length)
    return struct.unpack('bbbbbb', cmd)

def lean_command (side=0, amount = 50, move_time = 2000): ''' < little endian B 6 B side B steps H move time B step length ''' cmd=struct.pack('<bbbh',2,side, ...="" <="" amount,="" cmd)="" move_time)="" pre="" return="" struct.unpack('bbbbb',=""></bbbh',2,side,>

My follower function is:


def follower():
    pub = rospy.Publisher('/marty/socket_cmd', ByteArray, queue_size = 10)
    rospy.init_node('follower', anonymous=True)
    rate = rospy.Rate(0.5) # parameter is hertz
    while not rospy.is_shutdown():
        cmd = sidestep_command(0,1,2000,50)
        rospy.loginfo(cmd)
        pub.publish(cmd)
        rate.sleep()
        cmd=hello_command()
        rospy.loginfo(cmd)
        pub.publish(cmd)
        rate.sleep()
        cmd=lean_command()
        rospy.loginfo(cmd)
        pub.publish(cmd)
        rate.sleep()

So I think I have got the message formatting correct, in the folower function I have tried

the side step on its own and changed the parameters but marty does not respond.

Can anyone spot what I have done wrong or got a working example I can look at.

Thanks

Steve

Discussion

Leave a comment and join the discussion here


Please Log In or Sign Up to write a comment response

0 Responses

No responses just yet... Be the first to write one!