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