21 from cob_cartesian_controller.msg
import CartesianControllerAction, CartesianControllerGoal
22 from cob_cartesian_controller.msg
import Profile
24 if __name__ ==
'__main__':
25 rospy.init_node(
'test_move_lin')
26 action_name = rospy.get_namespace()+
'cartesian_trajectory_action' 28 rospy.logwarn(
"Waiting for ActionServer: %s", action_name)
29 client.wait_for_server()
30 rospy.logwarn(
"...done")
33 goal = CartesianControllerGoal()
35 goal.move_type = CartesianControllerGoal.LIN
36 goal.move_lin.pose_goal.position.x = -0.9
37 goal.move_lin.pose_goal.position.y = 0.0
38 goal.move_lin.pose_goal.position.z = 0.0
39 goal.move_lin.pose_goal.orientation.x = 0.0
40 goal.move_lin.pose_goal.orientation.y = 0.0
41 goal.move_lin.pose_goal.orientation.z = 0.0
42 goal.move_lin.pose_goal.orientation.w = 1.0
43 goal.move_lin.frame_id =
'world' 45 goal.profile.vel = 0.2
46 goal.profile.accl = 0.1
47 goal.profile.profile_type = Profile.SINOID
52 client.send_goal(goal)
53 client.wait_for_result()