00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060 import time
00061 import os
00062 import sys
00063 import types
00064 import thread
00065 import commands
00066
00067
00068 import roslib
00069 roslib.load_manifest('cob_script_server')
00070 import rospy
00071 import actionlib
00072
00073
00074 from trajectory_msgs.msg import *
00075 from geometry_msgs.msg import *
00076 from pr2_controllers_msgs.msg import *
00077 from move_base_msgs.msg import *
00078 from move_arm_msgs.msg import *
00079 from motion_planning_msgs.msg import *
00080 from tf.transformations import *
00081 from std_msgs.msg import String
00082
00083
00084 from cob_light.msg import *
00085 from cob_sound.msg import *
00086 from cob_script_server.msg import *
00087 from cob_srvs.srv import *
00088
00089
00090 import pygraphviz as pgv
00091
00092 graph=""
00093 graph_wait_list=[]
00094 function_counter = 0
00095 ah_counter = 0
00096 graph = pgv.AGraph()
00097 graph.node_attr['shape']='box'
00098 last_node = "Start"
00099
00100
00101
00102
00103 class script():
00104 def __init__(self):
00105
00106 filename = os.path.basename(sys.argv[0])
00107 self.basename, extension = os.path.splitext(filename)
00108 rospy.init_node(self.basename)
00109 self.graph_pub = rospy.Publisher("/script_server/graph", String)
00110
00111
00112 def Initialize(self):
00113 pass
00114
00115
00116 def Run(self):
00117 pass
00118
00119
00120
00121
00122 def Start(self):
00123 self.Parse()
00124 global ah_counter
00125 ah_counter = 0
00126 self.sss = simple_script_server()
00127 rospy.loginfo("Starting <<%s>> script...",self.basename)
00128 self.Initialize()
00129 self.Run()
00130
00131 rospy.loginfo("Wait for script to finish...")
00132 while ah_counter != 0:
00133 rospy.sleep(1)
00134 rospy.loginfo("...script finished.")
00135
00136
00137
00138
00139 def Parse(self):
00140 rospy.loginfo("Start parsing...")
00141 global graph
00142 global function_counter
00143 function_counter = 0
00144
00145 self.sss = simple_script_server(parse=True)
00146 self.Initialize()
00147 self.Run()
00148
00149
00150
00151 rospy.set_param("/script_server/graph", graph.string())
00152 self.graph_pub.publish(graph.string())
00153 rospy.loginfo("...parsing finished")
00154 function_counter = 0
00155 return graph.string()
00156
00157
00158
00159
00160 class simple_script_server:
00161
00162
00163
00164 def __init__(self, parse=False):
00165 global graph
00166 self.ns_global_prefix = "/script_server"
00167 self.wav_path = ""
00168 self.parse = parse
00169
00170
00171 self.pub_light = rospy.Publisher('light_controller/command', Light)
00172
00173 rospy.sleep(1)
00174
00175
00176
00177
00178
00179
00180
00181 def init(self,component_name,blocking=True):
00182 return self.trigger(component_name,"init",blocking)
00183
00184
00185
00186
00187
00188
00189 def stop(self,component_name):
00190 return self.trigger(component_name,"stop")
00191
00192
00193
00194
00195
00196
00197 def recover(self,component_name):
00198 return self.trigger(component_name,"recover")
00199
00200
00201
00202
00203
00204
00205
00206
00207 def trigger(self,component_name,service_name,blocking=True, planning=False):
00208 ah = action_handle(service_name, component_name, "", blocking, self.parse)
00209 if(self.parse):
00210 return ah
00211 else:
00212 ah.set_active()
00213
00214 rospy.loginfo("<<%s>> <<%s>>", service_name, component_name)
00215 rospy.loginfo("Wait for <<%s>> to <<%s>>...", component_name, service_name)
00216 service_full_name = "/" + component_name + "_controller/" + service_name
00217
00218
00219 try:
00220 rospy.wait_for_service(service_full_name,rospy.get_param('server_timeout',3))
00221 except rospy.ROSException, e:
00222 error_message = "%s"%e
00223 rospy.logerr("...<<%s>> service of <<%s>> not available, error: %s",service_name, component_name, error_message)
00224 ah.set_failed(4)
00225 return ah
00226
00227
00228 try:
00229 init = rospy.ServiceProxy(service_full_name,Trigger)
00230
00231 resp = init()
00232 except rospy.ServiceException, e:
00233 error_message = "%s"%e
00234 rospy.logerr("...calling <<%s>> service of <<%s>> not successfull, error: %s",service_name, component_name, error_message)
00235 ah.set_failed(10)
00236 return ah
00237
00238
00239 if not resp.success.data:
00240 rospy.logerr("...<<%s>> <<%s>> not successfull, error: %s",service_name, component_name, resp.error_message.data)
00241 ah.set_failed(10)
00242 return ah
00243
00244
00245 rospy.loginfo("...<<%s>> is <<%s>>", component_name, service_name)
00246 ah.set_succeeded()
00247 return ah
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257 def move(self,component_name,parameter_name,blocking=True, mode=None):
00258 if component_name == "base":
00259 return self.move_base(component_name,parameter_name,blocking, mode)
00260 elif component_name == "arm" and mode=="planned":
00261 return self.move_planned(component_name,parameter_name,blocking)
00262 else:
00263 return self.move_traj(component_name,parameter_name,blocking)
00264
00265
00266
00267
00268
00269
00270
00271
00272 def move_base(self,component_name,parameter_name,blocking, mode):
00273 ah = action_handle("move", component_name, parameter_name, blocking, self.parse)
00274 if(self.parse):
00275 return ah
00276 else:
00277 ah.set_active()
00278
00279 rospy.loginfo("Move <<%s>> to <<%s>>",component_name,parameter_name)
00280
00281
00282 if type(parameter_name) is str:
00283 if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name):
00284 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + parameter_name)
00285 ah.set_failed(2)
00286 return ah
00287 param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name)
00288 else:
00289 param = parameter_name
00290
00291
00292 if not type(param) is list:
00293 rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
00294 print "parameter is:",param
00295 ah.set_failed(3)
00296 return ah
00297 else:
00298
00299 DOF = 3
00300 if not len(param) == DOF:
00301 rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,DOF,len(param))
00302 print "parameter is:",param
00303 ah.set_failed(3)
00304 return ah
00305 else:
00306 for i in param:
00307
00308 if not ((type(i) is float) or (type(i) is int)):
00309
00310 rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name)
00311 print "parameter is:",param
00312 ah.set_failed(3)
00313 return ah
00314 else:
00315 rospy.logdebug("accepted parameter %f for %s",i,component_name)
00316
00317
00318 pose = PoseStamped()
00319 pose.header.stamp = rospy.Time.now()
00320 pose.header.frame_id = "/map"
00321 pose.pose.position.x = param[0]
00322 pose.pose.position.y = param[1]
00323 pose.pose.position.z = 0.0
00324 q = quaternion_from_euler(0, 0, param[2])
00325 pose.pose.orientation.x = q[0]
00326 pose.pose.orientation.y = q[1]
00327 pose.pose.orientation.z = q[2]
00328 pose.pose.orientation.w = q[3]
00329
00330
00331 if(mode == None):
00332 action_server_name = "/move_base"
00333 elif(mode == "omni"):
00334 action_server_name = "/move_base"
00335 elif(mode == "diff"):
00336 action_server_name = "/move_base_diff"
00337 elif(mode == "linear"):
00338 action_server_name = "/move_base_linear"
00339 else:
00340 rospy.logerr("no valid navigation mode given for %s, aborting...",component_name)
00341 print "navigation mode is:",mode
00342 ah.set_failed(33)
00343 return ah
00344
00345 rospy.logdebug("calling %s action server",action_server_name)
00346 client = actionlib.SimpleActionClient(action_server_name, MoveBaseAction)
00347
00348 rospy.logdebug("waiting for %s action server to start",action_server_name)
00349 if not client.wait_for_server(rospy.Duration(5)):
00350
00351 rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
00352 ah.set_failed(4)
00353 return ah
00354 else:
00355 rospy.logdebug("%s action server ready",action_server_name)
00356
00357
00358 client_goal = MoveBaseGoal()
00359 client_goal.target_pose = pose
00360
00361 client.send_goal(client_goal)
00362 ah.set_client(client)
00363
00364 ah.wait_inside()
00365
00366 return ah
00367
00368
00369
00370
00371
00372
00373
00374
00375 def move_traj(self,component_name,parameter_name,blocking):
00376 ah = action_handle("move", component_name, parameter_name, blocking, self.parse)
00377 if(self.parse):
00378 return ah
00379 else:
00380 ah.set_active()
00381
00382 rospy.loginfo("Move <<%s>> to <<%s>>",component_name,parameter_name)
00383
00384
00385 param_string = self.ns_global_prefix + "/" + component_name + "/joint_names"
00386 if not rospy.has_param(param_string):
00387 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",param_string)
00388 ah.set_failed(2)
00389 return ah
00390 joint_names = rospy.get_param(param_string)
00391
00392
00393 if not type(joint_names) is list:
00394 rospy.logerr("no valid joint_names for %s: not a list, aborting...",component_name)
00395 print "joint_names are:",joint_names
00396 ah.set_failed(3)
00397 return ah
00398 else:
00399 for i in joint_names:
00400
00401 if not type(i) is str:
00402 rospy.logerr("no valid joint_names for %s: not a list of strings, aborting...",component_name)
00403 print "joint_names are:",param
00404 ah.set_failed(3)
00405 return ah
00406 else:
00407 rospy.logdebug("accepted joint_names for component %s",component_name)
00408
00409
00410 if type(parameter_name) is str:
00411 if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name):
00412 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + parameter_name)
00413 ah.set_failed(2)
00414 return ah
00415 param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name)
00416 else:
00417 param = parameter_name
00418
00419
00420 if not type(param) is list:
00421 rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
00422 print "parameter is:",param
00423 ah.set_failed(3)
00424 return ah
00425
00426 traj = []
00427
00428 for point in param:
00429
00430 if type(point) is str:
00431 if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + point):
00432 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + point)
00433 ah.set_failed(2)
00434 return ah
00435 point = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + point)
00436 point = point[0]
00437
00438 elif type(point) is list:
00439 rospy.logdebug("point is a list")
00440 else:
00441 rospy.logerr("no valid parameter for %s: not a list of lists or strings, aborting...",component_name)
00442 print "parameter is:",param
00443 ah.set_failed(3)
00444 return ah
00445
00446
00447
00448 if not len(point) == len(joint_names):
00449 rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,len(joint_names),len(point))
00450 print "parameter is:",param
00451 ah.set_failed(3)
00452 return ah
00453
00454 for value in point:
00455
00456 if not ((type(value) is float) or (type(value) is int)):
00457
00458 rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name)
00459 print "parameter is:",param
00460 ah.set_failed(3)
00461 return ah
00462
00463 rospy.logdebug("accepted value %f for %s",value,component_name)
00464 traj.append(point)
00465
00466 rospy.logdebug("accepted trajectory for %s",component_name)
00467
00468
00469 traj_msg = JointTrajectory()
00470 traj_msg.header.stamp = rospy.Time.now()+rospy.Duration(0.5)
00471 traj_msg.joint_names = joint_names
00472 point_nr = 0
00473 for point in traj:
00474 point_nr = point_nr + 1
00475 point_msg = JointTrajectoryPoint()
00476 point_msg.positions = point
00477 point_msg.time_from_start=rospy.Duration(3*point_nr)
00478 traj_msg.points.append(point_msg)
00479
00480
00481 action_server_name = "/" + component_name + '_controller/joint_trajectory_action'
00482 rospy.logdebug("calling %s action server",action_server_name)
00483 client = actionlib.SimpleActionClient(action_server_name, JointTrajectoryAction)
00484
00485 rospy.logdebug("waiting for %s action server to start",action_server_name)
00486 if not client.wait_for_server(rospy.Duration(5)):
00487
00488 rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
00489 ah.set_failed(4)
00490 return ah
00491 else:
00492 rospy.logdebug("%s action server ready",action_server_name)
00493
00494
00495
00496
00497 self.set_operation_mode(component_name,"position")
00498
00499
00500 client_goal = JointTrajectoryGoal()
00501 client_goal.trajectory = traj_msg
00502
00503 client.send_goal(client_goal)
00504 ah.set_client(client)
00505
00506 ah.wait_inside()
00507 return ah
00508
00509 def move_planned(self, component_name, parameter_name, blocking=True):
00510 ah = action_handle("move_planned", component_name, parameter_name, blocking, self.parse)
00511 if(self.parse):
00512 return ah
00513 else:
00514 ah.set_active()
00515
00516 rospy.loginfo("Move planned <<%s>> to <<%s>>",component_name,parameter_name)
00517
00518
00519 param_string = self.ns_global_prefix + "/" + component_name + "/joint_names"
00520 if not rospy.has_param(param_string):
00521 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",param_string)
00522 ah.set_failed(2)
00523 return ah
00524 joint_names = rospy.get_param(param_string)
00525
00526
00527 if not type(joint_names) is list:
00528 rospy.logerr("no valid joint_names for %s: not a list, aborting...",component_name)
00529 print "joint_names are:",joint_names
00530 ah.set_failed(3)
00531 return ah
00532 else:
00533 for i in joint_names:
00534
00535 if not type(i) is str:
00536 rospy.logerr("no valid joint_names for %s: not a list of strings, aborting...",component_name)
00537 print "joint_names are:",param
00538 ah.set_failed(3)
00539 return ah
00540 else:
00541 rospy.logdebug("accepted joint_names for component %s",component_name)
00542
00543
00544 if type(parameter_name) is str:
00545 if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name):
00546 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + parameter_name)
00547 ah.set_failed(2)
00548 return ah
00549 param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name)
00550 else:
00551 param = parameter_name
00552
00553
00554 if not type(param) is list:
00555 rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
00556 print "parameter is:",param
00557 ah.set_failed(3)
00558 return ah
00559
00560 traj = []
00561
00562 for point in param:
00563
00564 if type(point) is str:
00565 if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + point):
00566 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + point)
00567 ah.set_failed(2)
00568 return ah
00569 point = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + point)
00570 point = point[0]
00571
00572 elif type(point) is list:
00573 rospy.logdebug("point is a list")
00574 else:
00575 rospy.logerr("no valid parameter for %s: not a list of lists or strings, aborting...",component_name)
00576 print "parameter is:",param
00577 ah.set_failed(3)
00578 return ah
00579
00580
00581
00582 if not len(point) == len(joint_names):
00583 rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,len(joint_names),len(point))
00584 print "parameter is:",param
00585 ah.set_failed(3)
00586 return ah
00587
00588 for value in point:
00589
00590 if not ((type(value) is float) or (type(value) is int)):
00591
00592 rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name)
00593 print "parameter is:",param
00594 ah.set_failed(3)
00595 return ah
00596
00597 rospy.logdebug("accepted value %f for %s",value,component_name)
00598 traj.append(point)
00599
00600 rospy.logdebug("accepted trajectory for %s",component_name)
00601
00602
00603 motion_plan = MotionPlanRequest()
00604 motion_plan.group_name = "arm"
00605 motion_plan.num_planning_attempts = 1
00606 motion_plan.allowed_planning_time = rospy.Duration(5.0)
00607
00608 motion_plan.planner_id= ""
00609 motion_plan.goal_constraints.joint_constraints=[]
00610
00611 for i in range(len(joint_names)):
00612 new_constraint = JointConstraint()
00613 new_constraint.joint_name = joint_names[i]
00614 new_constraint.position = 0.0
00615 new_constraint.tolerance_below = 0.4
00616 new_constraint.tolerance_above = 0.4
00617 motion_plan.goal_constraints.joint_constraints.append(new_constraint)
00618
00619 traj_endpoint = traj[len(traj)-1]
00620 for k in range(len(traj_endpoint)):
00621
00622 motion_plan.goal_constraints.joint_constraints[k].position = traj_endpoint[k]
00623
00624
00625
00626 action_server_name = "/move_arm"
00627 rospy.logdebug("calling %s action server",action_server_name)
00628 client = actionlib.SimpleActionClient(action_server_name, MoveArmAction)
00629
00630 rospy.logdebug("waiting for %s action server to start",action_server_name)
00631 if not client.wait_for_server(rospy.Duration(5)):
00632
00633 rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
00634 ah.set_failed(4)
00635 return ah
00636 else:
00637 rospy.logdebug("%s action server ready",action_server_name)
00638
00639
00640
00641
00642
00643 client_goal = MoveArmGoal()
00644 client_goal.planner_service_name = "ompl_planning/plan_kinematic_path"
00645
00646 client_goal.motion_plan_request = motion_plan
00647
00648 client.send_goal(client_goal)
00649 ah.set_client(client)
00650
00651 ah.wait_inside()
00652 return ah
00653
00654 def move_cart(self, component_name, parameter_name=["/base_link",[0.0, 0.0, 0.0],[0.0, 0.0, 0.0]], blocking=True):
00655 ah = action_handle("move_cart", component_name, parameter_name, blocking, self.parse)
00656 if(self.parse):
00657 return ah
00658 else:
00659 ah.set_active()
00660
00661 rospy.loginfo("Move <<%s>> to <<%s>>",component_name,parameter_name)
00662
00663 pose = PoseStamped()
00664 pose.header.stamp = rospy.Time.now()
00665 pose.header.frame_id = parameter_name[0]
00666 param = parameter_name[1:]
00667 pose.pose.position.x = param[0][0]
00668 pose.pose.position.y = param[0][1]
00669 pose.pose.position.z = param[0][2]
00670 q = quaternion_from_euler(param[1][0], param[1][1], param[1][2])
00671 pose.pose.orientation.x = q[0]
00672 pose.pose.orientation.y = q[1]
00673 pose.pose.orientation.z = q[2]
00674 pose.pose.orientation.w = q[3]
00675
00676
00677 action_server_name = "/" + component_name + '_controller/move_cart'
00678 rospy.logdebug("calling %s action server",action_server_name)
00679 client = actionlib.SimpleActionClient(action_server_name, MoveCartAction)
00680
00681 rospy.logdebug("waiting for %s action server to start",action_server_name)
00682 if not client.wait_for_server(rospy.Duration(5)):
00683
00684 rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
00685 ah.set_failed(4)
00686 return ah
00687 else:
00688 rospy.logdebug("%s action server ready",action_server_name)
00689
00690
00691 client_goal = MoveCartGoal()
00692 client_goal.goal_pose = pose
00693
00694 client.send_goal(client_goal)
00695 ah.set_client(client)
00696
00697 ah.wait_inside()
00698 return ah
00699
00700 def move_cart_rel(self, component_name, parameter_name=[[0.0, 0.0, 0.0],[0.0, 0.0, 0.0]], blocking=True):
00701 return move_cart(self, component_name, ["/arm_7_joint"]+parameter_name, blocking)
00702
00703
00704
00705
00706
00707
00708
00709
00710
00711
00712
00713 def move_cart_planned(self, component_name, parameter_name=[[0.0, 0.0, 0.0],[0.0, 0.0, 0.0]], blocking=True):
00714
00715
00716
00717
00718 ah = action_handle("move_cart_planned", component_name, "cartesian", blocking, self.parse)
00719 if(self.parse):
00720 return ah
00721 else:
00722 ah.set_active()
00723
00724
00725 motion_plan = MotionPlanRequest()
00726 motion_plan.group_name = "arm"
00727 motion_plan.num_planning_attempts = 1
00728 motion_plan.planner_id= ""
00729 motion_plan.allowed_planning_time = rospy.Duration(5.0)
00730
00731 pose_constraint = SimplePoseConstraint()
00732 pose_constraint.header.stamp = rospy.Time.now()
00733 pose_constraint.header.frame_id = "base_footprint"
00734 pose_constraint.link_name = "arm_7_link"
00735
00736 pose_constraint.absolute_position_tolerance.x = 0.1;
00737 pose_constraint.absolute_position_tolerance.y = 0.1;
00738 pose_constraint.absolute_position_tolerance.z = 0.1;
00739
00740 pose_constraint.absolute_roll_tolerance = 0.1;
00741 pose_constraint.absolute_pitch_tolerance = 0.1;
00742 pose_constraint.absolute_yaw_tolerance = 0.1;
00743
00744
00745 pose = PoseStamped()
00746 pose.header.stamp = rospy.Time.now()
00747 pose.header.frame_id = "base_footprint"
00748 pose.pose.position.x = parameter_name[0][0]
00749 pose.pose.position.y = parameter_name[0][1]
00750 pose.pose.position.z = parameter_name[0][2]
00751 q = quaternion_from_euler(parameter_name[1][0], parameter_name[1][1], parameter_name[1][2])
00752 pose.pose.orientation.x = q[0]
00753 pose.pose.orientation.y = q[1]
00754 pose.pose.orientation.z = q[2]
00755 pose.pose.orientation.w = q[3]
00756 pose_constraint.pose = pose.pose
00757
00758
00759
00760
00761
00762 position_constraint = PositionConstraint()
00763 position_constraint.header = pose_constraint.header
00764 position_constraint.link_name = pose_constraint.link_name
00765 position_constraint.position = pose_constraint.pose.position
00766
00767 position_constraint.constraint_region_shape.type = 1
00768 position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.x)
00769 position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.y)
00770 position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.z)
00771
00772 position_constraint.constraint_region_orientation.x = 0.0
00773 position_constraint.constraint_region_orientation.y = 0.0
00774 position_constraint.constraint_region_orientation.z = 0.0
00775 position_constraint.constraint_region_orientation.w = 1.0
00776
00777 position_constraint.weight = 1.0
00778
00779
00780
00781 orientation_constraint = OrientationConstraint()
00782 orientation_constraint.header = pose_constraint.header
00783 orientation_constraint.link_name = pose_constraint.link_name
00784 orientation_constraint.orientation = pose_constraint.pose.orientation
00785
00786
00787 orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance
00788 orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance
00789 orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance
00790
00791 orientation_constraint.weight = 1.0
00792
00793
00794 motion_plan.goal_constraints.joint_constraints = []
00795 motion_plan.goal_constraints.position_constraints = []
00796 motion_plan.goal_constraints.position_constraints.append(position_constraint)
00797 motion_plan.goal_constraints.orientation_constraints = []
00798 motion_plan.goal_constraints.orientation_constraints.append(orientation_constraint)
00799 motion_plan.goal_constraints.visibility_constraints = []
00800
00801
00802
00803 action_server_name = "/move_arm"
00804 rospy.logdebug("calling %s action server",action_server_name)
00805 client = actionlib.SimpleActionClient(action_server_name, MoveArmAction)
00806
00807 rospy.logdebug("waiting for %s action server to start",action_server_name)
00808 if not client.wait_for_server(rospy.Duration(5)):
00809
00810 rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
00811 ah.set_failed(4)
00812 return ah
00813 else:
00814 rospy.logdebug("%s action server ready",action_server_name)
00815
00816
00817 client_goal = MoveArmGoal()
00818 client_goal.planner_service_name = "ompl_planning/plan_kinematic_path"
00819
00820 client_goal.motion_plan_request = motion_plan
00821 client_goal.disable_ik = False
00822
00823 client.send_goal(client_goal)
00824 ah.set_client(client)
00825
00826 ah.wait_inside()
00827 return ah
00828
00829
00830
00831
00832
00833
00834
00835
00836
00837 def set_operation_mode(self,component_name,mode,blocking=True, planning=False):
00838
00839 rospy.set_param("/" + component_name + "_controller/OperationMode",mode)
00840
00841 try:
00842 set_operation_mode = rospy.ServiceProxy("/" + component_name + "_controller/set_operation_mode", SetOperationMode)
00843 req = SetOperationModeRequest()
00844 req.operation_mode.data = mode
00845
00846 resp = set_operation_mode(req)
00847
00848 except rospy.ServiceException, e:
00849 print "Service call failed: %s"%e
00850
00851
00852
00853
00854
00855
00856
00857 def set_light(self,parameter_name,blocking=False):
00858 ah = action_handle("set", "light", parameter_name, blocking, self.parse)
00859 if(self.parse):
00860 return ah
00861 else:
00862 ah.set_active()
00863
00864 rospy.loginfo("Set light to <<%s>>",parameter_name)
00865
00866
00867 if type(parameter_name) is str:
00868 if not rospy.has_param(self.ns_global_prefix + "/light/" + parameter_name):
00869 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/light/" + parameter_name)
00870 return 2
00871 param = rospy.get_param(self.ns_global_prefix + "/light/" + parameter_name)
00872 else:
00873 param = parameter_name
00874
00875
00876 if not type(param) is list:
00877 rospy.logerr("no valid parameter for light: not a list, aborting...")
00878 print "parameter is:",param
00879 ah.error_code = 3
00880 return ah
00881 else:
00882 if not len(param) == 3:
00883 rospy.logerr("no valid parameter for light: dimension should be 3 (r,g,b) and is %d, aborting...",len(param))
00884 print "parameter is:",param
00885 ah.error_code = 3
00886 return ah
00887 else:
00888 for i in param:
00889
00890 if not ((type(i) is float) or (type(i) is int)):
00891
00892 rospy.logerr("no valid parameter for light: not a list of float or int, aborting...")
00893 print "parameter is:",param
00894 ah.error_code = 3
00895 return ah
00896 else:
00897 rospy.logdebug("accepted parameter %f for light",i)
00898
00899
00900 color = Light()
00901 color.header.stamp = rospy.Time.now()
00902 if type(parameter_name) is str:
00903 color.name.data = parameter_name
00904 else:
00905 color.name.data = "unspecified"
00906 color.r = param[0]
00907 color.g = param[1]
00908 color.b = param[2]
00909
00910
00911 self.pub_light.publish(color)
00912
00913 ah.set_succeeded()
00914 ah.error_code = 0
00915 return ah
00916
00917
00918
00919
00920
00921
00922
00923
00924 def say(self,parameter_name,blocking=True):
00925 component_name = "sound"
00926 ah = action_handle("say", component_name, parameter_name, blocking, self.parse)
00927 if(self.parse):
00928 return ah
00929 else:
00930 ah.set_active()
00931
00932 text = ""
00933
00934
00935 language = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/language","en")
00936 if type(parameter_name) is str:
00937 if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + language + "/" + parameter_name):
00938 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + language + "/" + parameter_name)
00939 ah.set_failed(2)
00940 return ah
00941 param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + language + "/" + parameter_name)
00942 else:
00943 param = parameter_name
00944
00945
00946 if not type(param) is list:
00947 rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
00948 print "parameter is:",param
00949 ah.set_failed(3)
00950 return ah
00951 else:
00952 for i in param:
00953
00954 if not type(i) is str:
00955 rospy.logerr("no valid parameter for %s: not a list of strings, aborting...",component_name)
00956 print "parameter is:",param
00957 ah.set_failed(3)
00958 return ah
00959 else:
00960 text = text + i + " "
00961 rospy.logdebug("accepted parameter <<%s>> for <<%s>>",i,component_name)
00962
00963 rospy.loginfo("Saying <<%s>>",text)
00964
00965
00966 action_server_name = "/sound_controller/say"
00967 rospy.logdebug("calling %s action server",action_server_name)
00968 client = actionlib.SimpleActionClient(action_server_name, SayAction)
00969
00970 rospy.logdebug("waiting for %s action server to start",action_server_name)
00971 if not client.wait_for_server(rospy.Duration(5)):
00972
00973 rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
00974 ah.set_failed(4)
00975 return ah
00976 else:
00977 rospy.logdebug("%s action server ready",action_server_name)
00978
00979
00980 client_goal = SayGoal()
00981 client_goal.text.data = text
00982
00983 client.send_goal(client_goal)
00984 ah.set_client(client)
00985
00986 ah.wait_inside()
00987 return ah
00988
00989
00990
00991
00992
00993 def play(self,parameter_name,blocking=True):
00994 component_name = "sound"
00995 ah = action_handle("play", component_name, parameter_name, False, self.parse)
00996 if(self.parse):
00997 return ah
00998 else:
00999 ah.set_active()
01000
01001 language = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/language","en")
01002 if self.wav_path == "":
01003 wav_path = commands.getoutput("rospack find cob_script_server")
01004 else:
01005 wav_path = self.wav_path
01006 filename = wav_path + "/common/files/" + language + "/" + parameter_name + ".wav"
01007
01008 rospy.loginfo("Playing <<%s>>",filename)
01009
01010
01011
01012
01013
01014
01015
01016
01017
01018 if blocking:
01019 os.system("aplay -q " + filename)
01020 else:
01021 os.system("aplay -q " + filename + "&")
01022 ah.set_succeeded()
01023 return ah
01024
01025 def set_wav_path(self,parameter_name,blocking=True):
01026 if type(parameter_name) is str:
01027 self.wav_path = parameter_name
01028 else:
01029 rospy.logerr("invalid wav_path parameter specified, aborting...")
01030 print "parameter is:", parameter_name
01031 ah.set_failed(2)
01032 return ah
01033
01034
01035
01036
01037
01038
01039 def add_object(self,object_name,blocking=True):
01040 component_name = "object_handler"
01041 ah = action_handle("add", component_name, "add_" + object_name, False, self.parse)
01042 if(self.parse):
01043 return ah
01044 else:
01045 ah.set_active()
01046
01047 try:
01048 adder = rospy.ServiceProxy("/object_handler/add_object", HandleObject)
01049 req = HandleObjectRequest()
01050 req.object.data = object_name
01051
01052 res = adder(req)
01053
01054 except rospy.ServiceException, e:
01055 print "Service call failed: %s"%e
01056 ah.set_failed(1)
01057 return ah
01058
01059 ah.set_succeeded()
01060 return ah
01061
01062
01063
01064
01065
01066 def remove_object(self,object_name,blocking=True):
01067 component_name = "object_handler"
01068 ah = action_handle("remove", component_name, "remove_" + object_name, False, self.parse)
01069 if(self.parse):
01070 return ah
01071 else:
01072 ah.set_active()
01073
01074 try:
01075 remover = rospy.ServiceProxy("/object_handler/remove_object", HandleObject)
01076 req = HandleObjectRequest()
01077 req.object.data = object_name
01078
01079 res = remover(req)
01080
01081 except rospy.ServiceException, e:
01082 print "Service call failed: %s"%e
01083 ah.set_failed(1)
01084 return ah
01085
01086 ah.set_succeeded()
01087 return ah
01088
01089
01090
01091
01092
01093 def attach_object(self,object_name,blocking=True):
01094 component_name = "object_handler"
01095 ah = action_handle("attach", component_name, "attach_" + object_name, False, self.parse)
01096 if(self.parse):
01097 return ah
01098 else:
01099 ah.set_active()
01100
01101 try:
01102 attacher = rospy.ServiceProxy("/object_handler/attach_object", HandleObject)
01103 req = HandleObjectRequest()
01104 req.object.data = object_name
01105
01106 res = attacher(req)
01107
01108 except rospy.ServiceException, e:
01109 print "Service call failed: %s"%e
01110 ah.set_failed(1)
01111 return ah
01112
01113 ah.set_succeeded()
01114 return ah
01115
01116
01117
01118
01119
01120 def detach_object(self,object_name,blocking=True):
01121 component_name = "object_handler"
01122 ah = action_handle("attach", component_name, "detach_" + object_name, False, self.parse)
01123 if(self.parse):
01124 return ah
01125 else:
01126 ah.set_active()
01127
01128 try:
01129 detacher = rospy.ServiceProxy("/object_handler/detach_object", HandleObject)
01130 req = HandleObjectRequest()
01131 req.object.data = object_name
01132
01133 res = detacher(req)
01134
01135 except rospy.ServiceException, e:
01136 print "Service call failed: %s"%e
01137 ah.set_failed(1)
01138 return ah
01139
01140 ah.set_succeeded()
01141 return ah
01142
01143
01144
01145
01146
01147
01148
01149 def sleep(self,duration):
01150 ah = action_handle("sleep", "", str(duration), True, self.parse)
01151 if(self.parse):
01152 return ah
01153 else:
01154 ah.set_active()
01155 rospy.loginfo("Wait for %f sec",duration)
01156 rospy.sleep(duration)
01157
01158 ah.set_succeeded()
01159
01160
01161
01162
01163
01164
01165
01166
01167 def wait_for_input(self,duration=0):
01168 ah = action_handle("wait", "input", str(duration), True, self.parse)
01169 if(self.parse):
01170 return ah
01171 else:
01172 ah.set_active()
01173
01174 if (duration != 0):
01175 rospy.logerr("Wait with duration not implemented yet")
01176
01177 rospy.loginfo("Wait for user input...")
01178 retVal = raw_input()
01179 rospy.loginfo("...got string <<%s>>",retVal)
01180 ah.set_succeeded()
01181 return retVal
01182
01183
01184
01185
01186
01187 class action_handle:
01188
01189 def __init__(self, function_name, component_name, parameter_name, blocking, parse):
01190 global graph
01191 global function_counter
01192 self.parent_node = ""
01193 self.error_code = -1
01194 self.wait_log = False
01195 self.function_counter = function_counter
01196 self.function_name = function_name
01197 self.component_name = component_name
01198 self.parameter_name = parameter_name
01199 self.state = ScriptState.UNKNOWN
01200 self.blocking = blocking
01201 self.parse = parse
01202 self.level = int(rospy.get_param("/script_server/level",100))
01203 self.state_pub = rospy.Publisher("/script_server/state", ScriptState)
01204 self.AppendNode(blocking)
01205
01206
01207 def set_client(self,client):
01208 self.client = client
01209
01210
01211 def set_active(self):
01212 self.check_pause()
01213 self.state = ScriptState.ACTIVE
01214 self.error_code = -1
01215 self.PublishState()
01216
01217 global ah_counter
01218 ah_counter += 1
01219
01220
01221 def check_pause(self):
01222 param_string = "/script_server/pause"
01223 while bool(rospy.get_param(param_string,False)):
01224 rospy.logwarn("Script is paused...")
01225 self.state = ScriptState.PAUSED
01226 self.PublishState()
01227 rospy.sleep(1)
01228 if self.state == ScriptState.PAUSED:
01229 rospy.loginfo("...continuing script")
01230
01231
01232 def set_succeeded(self):
01233 self.state = ScriptState.SUCCEEDED
01234 self.error_code = 0
01235 self.PublishState()
01236
01237 global ah_counter
01238 ah_counter -= 1
01239
01240
01241 def set_failed(self,error_code):
01242 self.state = ScriptState.FAILED
01243 self.error_code = error_code
01244 self.PublishState()
01245
01246 global ah_counter
01247 ah_counter -= 1
01248
01249
01250 def get_state(self):
01251 return self.client.get_state()
01252
01253
01254 def get_error_code(self):
01255 return self.error_code
01256
01257
01258 def GetGraphstring(self):
01259 if type(self.parameter_name) is types.StringType:
01260 graphstring = str(self.function_counter)+"_"+self.function_name+"_"+self.component_name+"_"+self.parameter_name
01261 else:
01262 graphstring = str(self.function_counter)+"_"+self.function_name+"_"+self.component_name
01263 return graphstring
01264
01265
01266 def GetLevel(self,function_name):
01267 if (function_name == "move"):
01268 level = 0
01269 elif (function_name == "init"):
01270 level = 1
01271 elif (function_name == "stop"):
01272 level = 1
01273 elif (function_name == "sleep"):
01274 level = 2
01275 else:
01276 level = 100
01277 return level
01278
01279
01280 def AppendNode(self, blocking=True):
01281 global graph
01282 global graph_wait_list
01283 global function_counter
01284 global last_node
01285 graphstring = self.GetGraphstring()
01286 if self.parse:
01287 if ( self.level >= self.GetLevel(self.function_name)):
01288
01289 graph.add_edge(last_node, graphstring)
01290 for waiter in graph_wait_list:
01291 graph.add_edge(waiter, graphstring)
01292 graph_wait_list=[]
01293 if blocking:
01294 last_node = graphstring
01295 else:
01296 self.parent_node = graphstring
01297
01298
01299
01300
01301 function_counter += 1
01302
01303
01304 def PublishState(self):
01305 script_state = ScriptState()
01306 script_state.header.stamp = rospy.Time.now()
01307 script_state.number = self.function_counter
01308 script_state.function_name = self.function_name
01309 script_state.component_name = self.component_name
01310 script_state.full_graph_name = self.GetGraphstring()
01311 if ( type(self.parameter_name) is str ):
01312 script_state.parameter_name = self.parameter_name
01313 else:
01314 script_state.parameter_name = ""
01315 script_state.state = self.state
01316 script_state.error_code = self.error_code
01317 self.state_pub.publish(script_state)
01318
01319
01320
01321
01322
01323
01324 def wait(self, duration=None):
01325 global ah_counter
01326 ah_counter += 1
01327 self.blocking = True
01328 self.wait_for_finished(duration,True)
01329
01330
01331
01332
01333
01334
01335 def wait_inside(self, duration=None):
01336 if self.blocking:
01337 self.wait_for_finished(duration,True)
01338 else:
01339 thread.start_new_thread(self.wait_for_finished,(duration,False,))
01340 return self.error_code
01341
01342
01343
01344
01345
01346
01347
01348 def wait_for_finished(self, duration, logging):
01349 global graph_wait_list
01350 if(self.parse):
01351 if(self.parent_node != ""):
01352 graph_wait_list.append(self.parent_node)
01353 return
01354
01355 if self.error_code <= 0:
01356 if duration is None:
01357 if logging:
01358 rospy.loginfo("Wait for <<%s>> reaching <<%s>>...",self.component_name, self.parameter_name)
01359 self.client.wait_for_result()
01360 else:
01361 if logging:
01362 rospy.loginfo("Wait for <<%s>> reached <<%s>> (max %f secs)...",self.component_name, self.parameter_name,duration)
01363 if not self.client.wait_for_result(rospy.Duration(duration)):
01364 if logging:
01365 rospy.logerr("Timeout while waiting for <<%s>> to reach <<%s>>. Continuing...",self.component_name, self.parameter_name)
01366 self.set_failed(10)
01367 return
01368
01369
01370 if self.client.get_state() != 3:
01371 if logging:
01372 rospy.logerr("...<<%s>> could not reach <<%s>>, aborting...",self.component_name, self.parameter_name)
01373 self.set_failed(11)
01374 return
01375
01376 if logging:
01377 rospy.loginfo("...<<%s>> reached <<%s>>",self.component_name, self.parameter_name)
01378 else:
01379 rospy.logwarn("Execution of <<%s>> to <<%s>> was aborted, wait not possible. Continuing...",self.component_name, self.parameter_name)
01380 self.set_failed(self.error_code)
01381 return
01382
01383 self.set_succeeded()