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


nao_remote
Author(s): Armin Hornung
autogenerated on Tue Oct 15 2013 10:07:07