test_move_circ.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 #
3 # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
4 #
5 # Licensed under the Apache License, Version 2.0 (the "License");
6 # you may not use this file except in compliance with the License.
7 # You may obtain a copy of the License at
8 #
9 # http://www.apache.org/licenses/LICENSE-2.0
10 #
11 # Unless required by applicable law or agreed to in writing, software
12 # distributed under the License is distributed on an "AS IS" BASIS,
13 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 # See the License for the specific language governing permissions and
15 # limitations under the License.
16 
17 
18 import math
19 import rospy
20 import actionlib
21 
22 from cob_cartesian_controller.msg import CartesianControllerAction, CartesianControllerGoal
23 from cob_cartesian_controller.msg import Profile
24 
25 if __name__ == '__main__':
26  rospy.init_node('test_move_circ')
27  action_name = rospy.get_namespace()+'cartesian_trajectory_action'
28  client = actionlib.SimpleActionClient(action_name, CartesianControllerAction)
29  rospy.logwarn("Waiting for ActionServer: %s", action_name)
30  client.wait_for_server()
31  rospy.logwarn("...done")
32 
33  # Fill in the goal here
34  goal = CartesianControllerGoal()
35 
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'
45 
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
49 
50  goal.profile.vel = 0.1
51  goal.profile.accl = 0.2
52  goal.profile.profile_type = Profile.SINOID
53 
54  #print goal
55 
56  # Send the goal
57  client.send_goal(goal)
58  client.wait_for_result()


cob_cartesian_controller
Author(s): Christoph Mark
autogenerated on Thu Apr 8 2021 02:40:13