gistfile1.txt

#!/usr/bin/env python
# controllers: trot_ros, static_walk_ros, freeze, free_gait_impedence_ros
# trot_ros: WalkingTrot, Stand
# static_walk: walk, stand
import sys
import time
import rospy
from rocoma_msgs.srv import SwitchController as rocomaSwitchController
from anymal_msgs.srv import SwitchController as anymalSwitchController
from joy_manager_msgs.msg import AnyJoy

def switch_controller(controlType):
    
    rospy.wait_for_service('/anymal_highlevel_controller/switch_controller')
    try:
        switch_control_srv = rospy.ServiceProxy('/anymal_highlevel_controller/switch_controller', rocomaSwitchController)
        resp1 = switch_control_srv(controlType)
        return resp1.status
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

def switch_trot_mode(controlMode):
    rospy.wait_for_service('/trot_ros/go_to_mode')
    try:
        switch_mode_srv = rospy.ServiceProxy('/trot_ros/go_to_mode', anymalSwitchController)
        resp1 = switch_mode_srv(controlMode,True)
        return resp1.status
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

def switch_static_walk_mode(controlMode):
    
    rospy.wait_for_service('/static_walk_ros/go_to_mode')
    try:
        switch_mode_srv = rospy.ServiceProxy('/static_walk_ros/go_to_mode', anymalSwitchController)
        resp1 = switch_mode_srv(controlMode,True)
        return resp1.status
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e


def sendVelocityMessage(linearVelocity, angularVelocity):
    msg = AnyJoy()
    t = rospy.get_rostime() #get time as rospy.Time instance
    #print t
    # 0 : lateral in range -1:1
    # 1 : for/bk in range -1:1
    # 2 : unused part of right joiy
    # 3 : turning -1:1
    msg.header.stamp = t
    #msg.joy.axes = [0, m.linear_velocity.x ,0,m.angular_velocity.z]
    msg.joy.axes = [0, linearVelocity, 0, angularVelocity]
    msg.joy.header.stamp = t

    #rospy.loginfo(hello_str)
    velocityPub.publish(msg)


if __name__ == "__main__":
    velocityPub = rospy.Publisher('/anyjoy/onboard', AnyJoy, queue_size=10)
    rospy.init_node('talker', anonymous=True)

    #rospy.init_node('talker', anonymous=True)
    
    result = switch_controller("trot_ros")
    print "Switch Controller to Trot", result
    sendVelocityMessage(0.0, 0.0)
    time.sleep( 10 )
    
    result = switch_trot_mode("WalkingTrot")
    print "Switch Mode to Trot:WalkingTrot", result
    sendVelocityMessage(0.3, 0.0)
    time.sleep( 5 )

    sendVelocityMessage(0.0, -0.2)
    time.sleep( 5 )

    result = switch_trot_mode("Stand")
    print "Switch Mode to Trot:Stand", result
    time.sleep( 10 )

    #####################################
    result = switch_controller("static_walk_ros")
    print "Switch Controller to Walk", result
    sendVelocityMessage(0.0, 0.0)
    time.sleep( 10 )

    result = switch_static_walk_mode("walk")
    print "Switch Mode to StaticWalk:Walk", result
    sendVelocityMessage(-0.3, 0.0)
    time.sleep( 15 )
    sendVelocityMessage(0.0, 0.2)
    time.sleep( 5 )

    result = switch_static_walk_mode("stand")
    print "Switch Mode to StaticWalk:Stand", result
    sendVelocityMessage(0., 0.)