def move_circ(pose_center,
frame_id,
start_angle,
end_angle,
radius,
profile,
rotate_only=False):
client = actionlib.SimpleActionClient('cartesian_trajectory_action',
CartesianControllerAction)
rospy.logwarn("Waiting for ActionServer...")
success = client.wait_for_server(rospy.Duration(2.0))
if (not success):
return (success, "ActionServer not available within timeout")
goal = CartesianControllerGoal()
goal.move_type = CartesianControllerGoal.CIRC
goal.move_circ.pose_center = pose_center
goal.move_circ.frame_id = frame_id
goal.move_circ.rotate_only = rotate_only
goal.move_circ.start_angle = start_angle
goal.move_circ.end_angle = end_angle
goal.move_circ.radius = radius
goal.move_circ.profile = profile
print goal
client.send_goal(goal)
print "goal sent"
state = client.get_state()
print state
client.wait_for_result()
print "result received"
result = client.get_result()
return (result.success, result.message)
def move_circ(pose_center, frame_id, start_angle, end_angle, radius, profile):
action_name = rospy.get_namespace()+'cartesian_trajectory_action'
client = actionlib.SimpleActionClient(action_name, CartesianControllerAction)
rospy.logwarn("Waiting for ActionServer: %s", action_name)
success = client.wait_for_server(rospy.Duration(2.0))
if(not success):
return (success, "ActionServer not available within timeout")
goal = CartesianControllerGoal()
goal.move_type = CartesianControllerGoal.CIRC
goal.move_circ.pose_center = pose_center
goal.move_circ.frame_id = frame_id
goal.move_circ.start_angle = start_angle
goal.move_circ.end_angle = end_angle
goal.move_circ.radius = radius
goal.profile = profile
# print goal
client.send_goal(goal)
print "goal sent"
state = client.get_state()
# print state
client.wait_for_result()
print "result received"
result = client.get_result()
return (result.success, result.message)
def move_lin(pose_goal, frame_id, profile):
action_name = rospy.get_namespace()+'cartesian_trajectory_action'
client = actionlib.SimpleActionClient(action_name, CartesianControllerAction)
rospy.logwarn("Waiting for ActionServer: %s", action_name)
success = client.wait_for_server(rospy.Duration(2.0))
if(not success):
return (success, "ActionServer not available within timeout")
goal = CartesianControllerGoal()
goal.move_type = CartesianControllerGoal.LIN
goal.move_lin.pose_goal = pose_goal
goal.move_lin.frame_id = frame_id
goal.profile = profile
# print goal
client.send_goal(goal)
print("goal sent")
state = client.get_state()
# print state
client.wait_for_result()
print("result received")
result = client.get_result()
return (result.success, result.message)
import actionlib
from cob_cartesian_controller.msg import CartesianControllerAction, CartesianControllerGoal
from cob_cartesian_controller.msg import Profile
if __name__ == '__main__':
rospy.init_node('test_move_circ')
action_name = rospy.get_namespace() + 'cartesian_trajectory_action'
client = actionlib.SimpleActionClient(action_name,
CartesianControllerAction)
rospy.logwarn("Waiting for ActionServer: %s", action_name)
client.wait_for_server()
rospy.logwarn("...done")
# Fill in the goal here
goal = CartesianControllerGoal()
goal.move_type = CartesianControllerGoal.CIRC
goal.move_circ.pose_center.position.x = 0.0
goal.move_circ.pose_center.position.y = 0.7
goal.move_circ.pose_center.position.z = 1.0
goal.move_circ.pose_center.orientation.x = 0.0
goal.move_circ.pose_center.orientation.y = 0.0
goal.move_circ.pose_center.orientation.z = 0.0
goal.move_circ.pose_center.orientation.w = 1.0
goal.move_circ.frame_id = 'world'
goal.move_circ.start_angle = 0 * math.pi / 180.0
goal.move_circ.end_angle = 90 * math.pi / 180.0
goal.move_circ.radius = 0.3
import rospy
import actionlib
from cob_cartesian_controller.msg import CartesianControllerAction, CartesianControllerGoal
from cob_cartesian_controller.msg import Profile
if __name__ == '__main__':
rospy.init_node('test_move_lin')
action_name = rospy.get_namespace()+'cartesian_trajectory_action'
client = actionlib.SimpleActionClient(action_name, CartesianControllerAction)
rospy.logwarn("Waiting for ActionServer: %s", action_name)
client.wait_for_server()
rospy.logwarn("...done")
# Fill in the goal here
goal = CartesianControllerGoal()
goal.move_type = CartesianControllerGoal.LIN
goal.move_lin.pose_goal.position.x = -0.9
goal.move_lin.pose_goal.position.y = 0.0
goal.move_lin.pose_goal.position.z = 0.0
goal.move_lin.pose_goal.orientation.x = 0.0
goal.move_lin.pose_goal.orientation.y = 0.0
goal.move_lin.pose_goal.orientation.z = 0.0
goal.move_lin.pose_goal.orientation.w = 1.0
goal.move_lin.frame_id = 'world'
goal.profile.vel = 0.2
goal.profile.accl = 0.1
goal.profile.profile_type = Profile.SINOID