pose_manager.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # ROS node to provide a few joint angle trajectories as default poses
00004 #
00005 # Copyright 2011 Armin Hornung & Daniel Maier University of Freiburg
00006 # http://www.ros.org/wiki/nao
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions are met:
00010 #
00011 #    # Redistributions of source code must retain the above copyright
00012 #       notice, this list of conditions and the following disclaimer.
00013 #    # Redistributions in binary form must reproduce the above copyright
00014 #       notice, this list of conditions and the following disclaimer in the
00015 #       documentation and/or other materials provided with the distribution.
00016 #    # Neither the name of the University of Freiburg nor the names of its
00017 #       contributors may be used to endorse or promote products derived from
00018 #       this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00021 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00022 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00023 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00024 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00025 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00026 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00027 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00028 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00029 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00030 # POSSIBILITY OF SUCH DAMAGE.
00031 #
00032 
00033 import rospy
00034 import sys
00035 
00036 import rospy.rostime
00037 from rospy.rostime import Duration
00038 
00039 import actionlib
00040 from actionlib_msgs.msg import GoalStatus
00041 import naoqi_bridge_msgs.msg
00042 from naoqi_bridge_msgs.msg import JointTrajectoryGoal, JointTrajectoryAction, BodyPoseAction, BodyPoseGoal
00043 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00044 
00045 from sensor_msgs.msg import JointState
00046 from std_msgs.msg import String
00047 from std_msgs.msg import Bool
00048 from std_srvs.srv import Empty
00049 
00050 import xapparser
00051 
00052 class PoseManager():
00053     def __init__(self):
00054         # ROS initialization:
00055         rospy.init_node('pose_manager')
00056 
00057         self.poseLibrary = dict()
00058         self.readInPoses()
00059         self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
00060                                                        execute_cb=self.executeBodyPose,
00061                                                        auto_start=False)
00062         self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
00063         if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
00064             try:
00065                 rospy.wait_for_service("stop_walk_srv", timeout=2.0)
00066                 self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
00067             except:
00068                 rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
00069                           +"This is normal if there is no nao_walker running.")
00070             self.stopWalkSrv = None
00071             self.poseServer.start()
00072 
00073             rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());
00074 
00075         else:
00076             rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
00077             rospy.signal_shutdown("Required component missing");
00078 
00079 
00080     def parseXapPoses(self, xaplibrary):
00081         try:
00082             poses = xapparser.getpostures(xaplibrary)
00083         except RuntimeError as re:
00084             rospy.logwarn("Error while parsing the XAP file: %s" % str(re))
00085             return
00086 
00087         for name, pose in poses.items():
00088 
00089             trajectory = JointTrajectory()
00090 
00091             trajectory.joint_names = pose.keys()
00092             joint_values = pose.values()
00093 
00094             point = JointTrajectoryPoint()
00095             point.time_from_start = Duration(2.0) # hardcoded duration!
00096             point.positions = pose.values()
00097             trajectory.points = [point]
00098 
00099             self.poseLibrary[name] = trajectory
00100 
00101 
00102     def readInPoses(self):
00103 
00104         xaplibrary = rospy.get_param('~xap', None)
00105         if xaplibrary:
00106             self.parseXapPoses(xaplibrary)
00107 
00108         poses = rospy.get_param('~poses', None)
00109         if poses:
00110             for key,value in poses.iteritems():
00111                 try:
00112                 # TODO: handle multiple points in trajectory
00113                     trajectory = JointTrajectory()
00114                     trajectory.joint_names = value["joint_names"]
00115                     point = JointTrajectoryPoint()
00116                     point.time_from_start = Duration(value["time_from_start"])
00117                     point.positions = value["positions"]
00118                     trajectory.points = [point]
00119                     self.poseLibrary[key] = trajectory
00120                 except:
00121                     rospy.logwarn("Could not parse pose \"%s\" from the param server:", key);
00122                     rospy.logwarn(sys.exc_info())
00123 
00124         # add a default crouching pose:
00125         if not "crouch" in self.poseLibrary:
00126             trajectory = JointTrajectory()
00127             trajectory.joint_names = ["Body"]
00128             point = JointTrajectoryPoint()
00129             point.time_from_start = Duration(1.5)
00130             point.positions = [0.0,0.0,                     # head
00131                 1.545, 0.33, -1.57, -0.486, 0.0, 0.0,        # left arm
00132                 -0.3, 0.057, -0.744, 2.192, -1.122, -0.035,     # left leg
00133                 -0.3, 0.057, -0.744, 2.192, -1.122, -0.035,    # right leg
00134                 1.545, -0.33, 1.57, 0.486, 0.0, 0.0]        # right arm
00135             trajectory.points = [point]
00136             self.poseLibrary["crouch"] = trajectory;
00137 
00138 
00139         rospy.loginfo("Loaded %d poses: %s", len(self.poseLibrary), self.poseLibrary.keys())
00140 
00141 
00142     def executeBodyPose(self, goal):
00143         if not goal.pose_name in self.poseLibrary:
00144             rospy.loginfo("Pose %s not in library, reload library from parameters..." % goal.pose_name)
00145             self.readInPoses()
00146 
00147         if goal.pose_name in self.poseLibrary:
00148             rospy.loginfo("Executing body pose %s...", goal.pose_name);
00149             if not self.stopWalkSrv is None:
00150                 self.stopWalkSrv()
00151 
00152             trajGoal = JointTrajectoryGoal()
00153             # time out one sec. after trajectory ended:
00154             trajGoal.trajectory = self.poseLibrary[goal.pose_name]
00155             timeout = trajGoal.trajectory.points[-1].time_from_start + Duration(1.0)
00156             trajGoal.trajectory.header.stamp = rospy.Time.now()
00157             # TODO: cancel goal after timeout is not working yet in nao_controller
00158             self.trajectoryClient.send_goal_and_wait(trajGoal, timeout)
00159             if self.trajectoryClient.get_state() != GoalStatus.SUCCEEDED:
00160                 self.poseServer.set_aborted(text="JointTrajectory action did not succeed (timeout?)");
00161 
00162             self.poseServer.set_succeeded()
00163             rospy.loginfo("Done.");
00164 
00165         else:
00166             msg = "pose \""+goal.pose_name+"\" not in pose_manager's library";
00167             rospy.logwarn("%s", msg)
00168             self.poseServer.set_aborted(text=str(msg));
00169 
00170 
00171 
00172 if __name__ == '__main__':
00173 
00174     manager = PoseManager()
00175     rospy.spin()
00176     exit(0)
00177 


naoqi_pose
Author(s): Armin Hornung, Séverin Lemaignan
autogenerated on Thu Aug 27 2015 14:05:40