cob_cartesian_controller: simple_cartesian

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00004 #
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #
00009 #   http://www.apache.org/licenses/LICENSE-2.0
00010 #
00011 # Unless required by applicable law or agreed to in writing, software
00012 # distributed under the License is distributed on an "AS IS" BASIS,
00013 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014 # See the License for the specific language governing permissions and
00015 # limitations under the License.
00016 
00017 
00018 import rospy
00019 import threading
00020 import actionlib
00021 
00022 from tf.transformations import *
00023 from geometry_msgs.msg import Pose
00024 from cob_cartesian_controller.msg import CartesianControllerAction, CartesianControllerGoal
00025 from cob_cartesian_controller.msg import Profile
00026 
00027 
00028 def move_lin(pose_goal, frame_id, profile):
00029     action_name = rospy.get_namespace()+'cartesian_trajectory_action'
00030     client = actionlib.SimpleActionClient(action_name, CartesianControllerAction)
00031     rospy.logwarn("Waiting for ActionServer: %s", action_name)
00032     success = client.wait_for_server(rospy.Duration(2.0))
00033     if(not success):
00034         return (success, "ActionServer not available within timeout")
00035 
00036     goal = CartesianControllerGoal()
00037     goal.move_type = CartesianControllerGoal.LIN
00038     goal.move_lin.pose_goal = pose_goal
00039     goal.move_lin.frame_id = frame_id
00040     goal.profile = profile
00041     # print goal
00042 
00043     client.send_goal(goal)
00044     print "goal sent"
00045     state = client.get_state()
00046     # print state
00047     client.wait_for_result()
00048     print "result received"
00049     result = client.get_result()
00050     return (result.success, result.message)
00051 
00052 def move_circ(pose_center, frame_id, start_angle, end_angle, radius, profile):
00053     action_name = rospy.get_namespace()+'cartesian_trajectory_action'
00054     client = actionlib.SimpleActionClient(action_name, CartesianControllerAction)
00055     rospy.logwarn("Waiting for ActionServer: %s", action_name)
00056     success = client.wait_for_server(rospy.Duration(2.0))
00057     if(not success):
00058         return (success, "ActionServer not available within timeout")
00059 
00060     goal = CartesianControllerGoal()
00061     goal.move_type = CartesianControllerGoal.CIRC
00062     goal.move_circ.pose_center = pose_center
00063     goal.move_circ.frame_id = frame_id
00064     goal.move_circ.start_angle = start_angle
00065     goal.move_circ.end_angle = end_angle
00066     goal.move_circ.radius = radius
00067     goal.profile = profile
00068     # print goal
00069 
00070     client.send_goal(goal)
00071     print "goal sent"
00072     state = client.get_state()
00073     # print state
00074     client.wait_for_result()
00075     print "result received"
00076     result = client.get_result()
00077     return (result.success, result.message)
00078 
00079 
00080 
00081 ####################################
00082 #  helpers
00083 ####################################
00084 
00085 
00086 '''
00087 Generates a geometry_msgs.Pose from (x,y,z) and (r,p,y)
00088 '''
00089 def gen_pose(pos=[0,0,0], rpy=[0,0,0]):
00090     pose = Pose()
00091     pose.position.x, pose.position.y, pose.position.z = pos
00092     pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w = quaternion_from_euler(*rpy)
00093 
00094 
00095     return pose