22 from cob_cartesian_controller.msg
import CartesianControllerAction, CartesianControllerGoal
23 from cob_cartesian_controller.msg
import Profile
25 if __name__ ==
'__main__':
26 rospy.init_node(
'test_move_circ')
27 action_name = rospy.get_namespace()+
'cartesian_trajectory_action' 29 rospy.logwarn(
"Waiting for ActionServer: %s", action_name)
30 client.wait_for_server()
31 rospy.logwarn(
"...done")
34 goal = CartesianControllerGoal()
36 goal.move_type = CartesianControllerGoal.CIRC
37 goal.move_circ.pose_center.position.x = 0.0
38 goal.move_circ.pose_center.position.y = 0.7
39 goal.move_circ.pose_center.position.z = 1.0
40 goal.move_circ.pose_center.orientation.x = 0.0
41 goal.move_circ.pose_center.orientation.y = 0.0
42 goal.move_circ.pose_center.orientation.z = 0.0
43 goal.move_circ.pose_center.orientation.w = 1.0
44 goal.move_circ.frame_id =
'world' 46 goal.move_circ.start_angle = 0 * math.pi / 180.0
47 goal.move_circ.end_angle = 90 * math.pi / 180.0
48 goal.move_circ.radius = 0.3
50 goal.profile.vel = 0.1
51 goal.profile.accl = 0.2
52 goal.profile.profile_type = Profile.SINOID
57 client.send_goal(goal)
58 client.wait_for_result()