move_atlas.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('hrpsys_gazebo')
00004 import rospy, math
00005 import sys
00006 from math import *
00007 
00008 from std_msgs.msg import String
00009 from geometry_msgs.msg import Pose
00010 from atlas_msgs.msg import AtlasSimInterfaceCommand
00011 
00012 def move(x, y, z, w=0):
00013     # Initialize the node
00014     rospy.init_node('move_atlas')
00015 
00016     # Setup the publishers
00017     mode = rospy.Publisher('/atlas/mode', String, None, False, True, None)
00018     sim_interface = rospy.Publisher('/atlas/atlas_sim_interface_command', AtlasSimInterfaceCommand, None, False, True, None)
00019     set_pose= rospy.Publisher('/atlas/set_pose', Pose, None, False, True, None)
00020 
00021     while set_pose.get_num_connections() == 0:
00022           rospy.sleep(0.1)
00023     while rospy.get_time() < 5.0:
00024           rospy.sleep(0.1)
00025 
00026     p = Pose()
00027     p.position.x = x
00028     p.position.y = y
00029     p.position.z = z
00030     p.orientation.x = 0
00031     p.orientation.y = 0
00032     p.orientation.z = sin(w/2.0)
00033     p.orientation.w = cos(w/2.0)
00034 
00035     com = AtlasSimInterfaceCommand()
00036 
00037     rospy.sleep(0.1)
00038     mode.publish("harnessed")
00039     com.behavior = com.FREEZE
00040     sim_interface.publish(com)
00041     com.behavior = com.STAND_PREP
00042     sim_interface.publish(com)
00043     rospy.sleep(2.0)
00044     mode.publish("nominal")
00045     rospy.sleep(0.05)
00046     set_pose.publish(p)
00047     rospy.sleep(0.2)
00048     com.behavior = com.STAND
00049     sim_interface.publish(com)
00050 
00051 if __name__ == '__main__':
00052     argvs = sys.argv
00053     argc = len(argvs)
00054 
00055     try:
00056         move(float(argvs[1]), float(argvs[2]), float(argvs[3]), float(argvs[4])*pi/180)
00057     except rospy.ROSInterruptException: pass
00058 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


hrpsys_gazebo
Author(s): Yohei Kakiuchi
autogenerated on Thu Jun 27 2013 14:58:14