def execute(self, component_name, target="", blocking=True): ah = ActionHandle("move_arm_joint_direct", component_name, target, blocking) rospy.loginfo("Move <<%s>> PARAMETER to <<%s>>", component_name, target) # get pose from parameter server if type(target) is str: if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + target): rospy.logerr( "parameter %s does not exist on ROS Parameter Server, aborting...", self.ns_global_prefix + "/" + component_name + "/" + target) ah.set_failed(2) return ah param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + target) else: ah.set_failed(3) return ah return self.actions.move_arm_joint_direct(component_name, param, blocking)
def execute(self,parameter_name,blocking=False): ah = ActionHandle("set", "light", parameter_name, blocking, self.parse) if(self.parse): return ah else: ah.set_active(mode="topic") rospy.loginfo("Set light to <<%s>>",parameter_name) # get joint values from parameter server if type(parameter_name) is str: if not rospy.has_param(self.ns_global_prefix + "/light/" + parameter_name): rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/light/" + parameter_name) return 2 param = rospy.get_param(self.ns_global_prefix + "/light/" + parameter_name) else: param = parameter_name # check color parameters if not type(param) is list: # check outer list rospy.logerr("no valid parameter for light: not a list, aborting...") print "parameter is:",param ah.error_code = 3 return ah else: if not len(param) == 3: # check dimension rospy.logerr("no valid parameter for light: dimension should be 3 (r,g,b) and is %d, aborting...",len(param)) print "parameter is:",param ah.error_code = 3 return ah else: for i in param: #print i,"type1 = ", type(i) if not ((type(i) is float) or (type(i) is int)): # check type #print type(i) rospy.logerr("no valid parameter for light: not a list of float or int, aborting...") print "parameter is:",param ah.error_code = 3 return ah else: rospy.logdebug("accepted parameter %f for light",i) # convert to ColorRGBA message color = ColorRGBA() color.r = param[0] color.g = param[1] color.b = param[2] color.a = 1 # Transparency # publish color self.pub_light.publish(color) ah.set_succeeded() ah.error_code = 0 return ah
def execute(self, component_name, parameter_name=[0,0,0], blocking=True): ah = ActionHandle("move_base_rel", component_name, parameter_name, blocking, self.parse) if(self.parse): return ah else: ah.set_active(mode="topic") rospy.loginfo("Move base relatively by <<%s>>", parameter_name) # step 0: check validity of parameters: 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)): rospy.logerr("Non-numeric parameter_name list, aborting move_base_rel") print("parameter_name must be numeric list of length 3; (relative x and y transl [m], relative rotation [rad])") ah.set_failed(4) return ah if math.sqrt(parameter_name[0]**2 + parameter_name[1]**2) >= 0.15: rospy.logerr("Maximal relative translation step exceeded, aborting move_base_rel") print("Maximal relative translation step is 0.1 m") ah.set_failed(4) return(ah) if abs(parameter_name[2]) >= math.pi/2: rospy.logerr("Maximal relative rotation step exceeded, aborting move_base_rel") print("Maximal relative rotation step is pi/2") ah.set_failed(4) return(ah) # step 1: determine duration of motion so that upper thresholds for both translational as well as rotational velocity are not exceeded max_trans_vel = 0.05 # [m/s] max_rot_vel = 0.2 # [rad/s] duration_trans_sec = math.sqrt(parameter_name[0]**2 + parameter_name[1]**2) / max_trans_vel duration_rot_sec = abs(parameter_name[2] / max_rot_vel) duration_sec = max(duration_trans_sec, duration_rot_sec) duration_ros = rospy.Duration.from_sec(duration_sec) # duration of motion in ROS time # step 2: determine actual velocities based on calculated duration x_vel = parameter_name[0] / duration_sec y_vel = parameter_name[1] / duration_sec rot_vel = parameter_name[2] / duration_sec # step 3: send constant velocity command to base_controller for the calculated duration of motion pub = rospy.Publisher('/base_controller/command_safe', Twist) # todo: use Matthias G.'s safe_command twist = Twist() twist.linear.x = x_vel twist.linear.y = y_vel twist.angular.z = rot_vel r = rospy.Rate(10) # send velocity commands at 10 Hz end_time = rospy.Time.now() + duration_ros while not rospy.is_shutdown() and rospy.Time.now() < end_time: pub.publish(twist) r.sleep() ah.set_succeeded() return ah
def execute(self, component_name, target=PoseStamped(), blocking=True): ah = ActionHandle("move_arm_cartesian_planned", component_name, target, blocking) rospy.loginfo("Move <<%s>> CARTESIAN PLANNED", component_name) mp = MotionPlan() mp += MoveArm("arm",[target,['sdh_grasp_link']]) planning_res = mp.plan(2) print planning_res if planning_res.success: for e in mp.execute(): exec_res = e.wait() print exec_res if not exec_res.success: rospy.logerr("Execution of MotionExecutable %s failed", e.name) ah.set_failed(4) break else: rospy.logerr("Planning failed") ah.set_failed(4) return ah
def execute(self, component_name, as_name, target="", blocking=True): ah = ActionHandle("move_joint_trajectory", component_name, target, blocking) rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target) # get joint_names from parameter server param_string = self.ns_global_prefix + "/" + component_name + "/joint_names" if not rospy.has_param(param_string): rospy.logerr( "parameter %s does not exist on ROS Parameter Server, aborting...", param_string) ah.set_failed(2) return ah joint_names = rospy.get_param(param_string) # check joint_names parameter if not type(joint_names) is list: # check list rospy.logerr( "no valid joint_names for %s: not a list, aborting...", component_name) print "joint_names are:", joint_names ah.set_failed(3) return ah else: for i in joint_names: #print i,"type1 = ", type(i) if not type(i) is str: # check string rospy.logerr( "no valid joint_names for %s: not a list of strings, aborting...", component_name) print "joint_names are:", param ah.set_failed(3) return ah else: rospy.logdebug("accepted joint_names for component %s", component_name) # get joint values from parameter server if type(target) is str: if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + target): rospy.logerr( "parameter %s does not exist on ROS Parameter Server, aborting...", self.ns_global_prefix + "/" + component_name + "/" + target) ah.set_failed(2) return ah param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + target) else: param = target # check trajectory parameters if not type(param) is list: # check outer list rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name) print "parameter is:", param ah.set_failed(3) return ah traj = [] for point in param: #print point,"type1 = ", type(point) if type(point) is str: if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + point): rospy.logerr( "parameter %s does not exist on ROS Parameter Server, aborting...", self.ns_global_prefix + "/" + component_name + "/" + point) ah.set_failed(2) return ah point = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + point) point = point[ 0] # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories #print point elif type(point) is list: rospy.logdebug("point is a list") else: rospy.logerr( "no valid parameter for %s: not a list of lists or strings, aborting...", component_name) print "parameter is:", param ah.set_failed(3) return ah # here: point should be list of floats/ints #print point if not len(point) == len(joint_names): # check dimension rospy.logerr( "no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, len(joint_names), len(point)) print "parameter is:", param ah.set_failed(3) return ah for value in point: #print value,"type2 = ", type(value) if not ((type(value) is float) or (type(value) is int)): # check type #print type(value) rospy.logerr( "no valid parameter for %s: not a list of float or int, aborting...", component_name) print "parameter is:", param ah.set_failed(3) return ah rospy.logdebug("accepted value %f for %s", value, component_name) traj.append(point) rospy.logdebug("accepted trajectory for %s", component_name) # convert to ROS trajectory message traj_msg = JointTrajectory() traj_msg.header.stamp = rospy.Time.now() + rospy.Duration(0.5) traj_msg.joint_names = joint_names point_nr = 0 for point in traj: point_nr = point_nr + 1 point_msg = JointTrajectoryPoint() point_msg.positions = point point_msg.velocities = [0] * len(joint_names) point_msg.time_from_start = rospy.Duration( 3 * point_nr ) # this value is set to 3 sec per point. \todo TODO: read from parameter traj_msg.points.append(point_msg) # call action server action_server_name = "/" + component_name + '_controller/follow_joint_trajectory' rospy.logdebug("calling %s action server", action_server_name) client = actionlib.SimpleActionClient(action_server_name, FollowJointTrajectoryAction) # trying to connect to server rospy.logdebug("waiting for %s action server to start", action_server_name) if not client.wait_for_server(rospy.Duration(5)): # error: server did not respond rospy.logerr( "%s action server not ready within timeout, aborting...", action_server_name) ah.set_failed(4) return ah else: rospy.logdebug("%s action server ready", action_server_name) # sending goal client_goal = FollowJointTrajectoryGoal() client_goal.trajectory = traj_msg #print client_goal client.send_goal(client_goal) ah.set_client(client) ah.wait_inside() return ah
def execute(self, target=PoseStamped(), blocking=True): ah = ActionHandle("pick_up", "dummy", "", blocking) rospy.loginfo("Picking up object...") #ah = self.actions.grasp_object(target, blocking) wi = WorldInterface() wi.reset_attached_objects() wi.reset_collision_objects() # add table table_extent = (2.0, 2.0, 1.0) table_pose = conversions.create_pose_stamped([ -0.5 - table_extent[0]/2.0, 0 ,table_extent[2]/2.0 ,0,0,0,1], 'base_link') wi.add_collision_box(table_pose, table_extent, "table") mp = MotionPlan() mp += CallFunction(sss.move, 'torso','front') #mp += MoveArm('arm',['pregrasp']) mp += CallFunction(sss.move, 'sdh','cylopen', False) # OpenIssues: # - where to place the sdh_grasp_link to grasp the object located at target? # - there is no orientation in the target? -> hardcoded taken from pregrasp grasp_pose = PoseStamped() grasp_pose = deepcopy(target) grasp_pose.header.stamp = rospy.Time.now() grasp_pose.pose.orientation.x = 0.220 grasp_pose.pose.orientation.y = 0.670 grasp_pose.pose.orientation.z = -0.663 grasp_pose.pose.orientation.w = -0.253 mp += MoveArm('arm',[grasp_pose,['sdh_grasp_link']], seed = 'pregrasp') mp += CallFunction(sss.move, 'sdh','cylclosed', True) #ah = self.actions.lift_object(target, blocking) lift_pose = PoseStamped() lift_pose = deepcopy(target) lift_pose.header.stamp = rospy.Time.now() lift_pose.pose.position.z += 0.08 lift_pose.pose.orientation.x = 0.220 lift_pose.pose.orientation.y = 0.670 lift_pose.pose.orientation.z = -0.663 lift_pose.pose.orientation.w = -0.253 mp += MoveArm('arm',[lift_pose,['sdh_grasp_link']]) #ah = self.actions.retrieve_object(target, blocking) #mp += MoveArm('arm',['pregrasp']) mp += MoveArm('arm',['wavein']) planning_res = mp.plan(2) print planning_res if planning_res.success: for e in mp.execute(): exec_res = e.wait() print exec_res if not exec_res.success: #rospy.logerr("Execution of MotionExecutable %s failed", e.name) ah.set_failed(4) break ah.set_succeeded() else: rospy.logerr("Planning failed") ah.set_failed(4) return ah
def execute(self, component_name, target="", blocking=True): ah = ActionHandle("move_arm_joint_planned", component_name, target, blocking) rospy.loginfo("Move planned <<%s>> to <<%s>>",component_name,target) # get joint values from parameter server if type(target) is str: if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + target): rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + target) ah.set_failed(2) return ah param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + target) else: param = target # check trajectory parameters if not type(param) is list: # check outer list rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name) print "parameter is:",param ah.set_failed(3) return ah last_point = param[-1] if type(last_point) is str: if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + last_point): rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + last_point) ah.set_failed(2) return ah last_point = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + last_point) last_point = last_point[0] # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories #print last_point elif type(last_point) is list: rospy.logdebug("last_point is a list") else: rospy.logerr("no valid parameter for %s: not a list of lists or strings, aborting...",component_name) print "parameter is:",param ah.set_failed(3) return ah # here: last_point should be list of floats/ints #print last_point if not len(last_point) == self.DOF: # check dimension rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,self.DOF,len(last_point)) print "parameter is:",param ah.set_failed(3) return ah for value in last_point: #print value,"type2 = ", type(value) if not ((type(value) is float) or (type(value) is int)): # check type #print type(value) rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name) print "parameter is:",param ah.set_failed(3) return ah rospy.logdebug("accepted value %f for %s",value,component_name) mp = MotionPlan() mp += MoveArm("arm", [last_point]) planning_res = mp.plan(2) print planning_res if planning_res.success: for e in mp.execute(): exec_res = e.wait() print exec_res if not exec_res.success: rospy.logerr("Execution of MotionExecutable %s failed", e.name) ah.set_failed(3) break else: rospy.logerr("Planning failed") ah.set_failed(3) return ah
def execute(self, parameter_name, blocking=True, mode=''): component_name = 'base' ah = ActionHandle("move", component_name, parameter_name, blocking) if(mode == None or mode == ""): rospy.loginfo("Move <<%s>> to <<%s>>",component_name,parameter_name) else: rospy.loginfo("Move <<%s>> to <<%s>> using <<%s>> mode",component_name,parameter_name,mode) # get joint values from parameter server if type(parameter_name) is str: if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name): rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + parameter_name) ah.set_failed(2) return ah param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name) else: param = parameter_name # check trajectory parameters if not type(param) is list: # check outer list rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name) print "parameter is:",param ah.set_failed(3) return ah else: #print i,"type1 = ", type(i) DOF = 3 if not len(param) == DOF: # check dimension rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,DOF,len(param)) print "parameter is:",param ah.set_failed(3) return ah else: for i in param: #print i,"type2 = ", type(i) if not ((type(i) is float) or (type(i) is int)): # check type #print type(i) rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name) print "parameter is:",param ah.set_failed(3) return ah else: rospy.logdebug("accepted parameter %f for %s",i,component_name) # convert to pose message pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "/map" pose.pose.position.x = param[0] pose.pose.position.y = param[1] pose.pose.position.z = 0.0 q = tf.transformations.quaternion_from_euler(0, 0, param[2]) pose.pose.orientation.x = q[0] pose.pose.orientation.y = q[1] pose.pose.orientation.z = q[2] pose.pose.orientation.w = q[3] # call action server if(mode == None or mode == ""): action_server_name = "/move_base" elif(mode == "omni"): action_server_name = "/move_base" elif(mode == "diff"): action_server_name = "/move_base_diff" elif(mode == "linear"): action_server_name = "/move_base_linear" else: rospy.logerr("no valid navigation mode given for %s, aborting...",component_name) print "navigation mode is:",mode ah.set_failed(33) return ah rospy.logdebug("calling %s action server",action_server_name) client = actionlib.SimpleActionClient(action_server_name, MoveBaseAction) # trying to connect to server rospy.logdebug("waiting for %s action server to start",action_server_name) if not client.wait_for_server(rospy.Duration(5)): # error: server did not respond rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name) ah.set_failed(4) return ah else: rospy.logdebug("%s action server ready",action_server_name) # sending goal client_goal = MoveBaseGoal() client_goal.target_pose = pose #print client_goal client.send_goal(client_goal) ah.set_client(client) ah.wait_inside() return ah
def execute(self, target="", blocking=True): component_name = "gripper" ah = ActionHandle("move_gripper", component_name, target, blocking) rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target) # get pose from parameter server if type(target) is str: if not rospy.has_param(self.action_server_name_prefix + "/" + component_name + "/" + target): rospy.logerr( "parameter %s does not exist on ROS Parameter Server, aborting...", self.action_server_name_prefix + "/" + component_name + "/" + target) ah.set_failed(2) return ah param = rospy.get_param(self.action_server_name_prefix + "/" + component_name + "/" + target) else: param = target # check pose """ if not type(param) is list: # check outer list rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name) print "parameter is:", param ah.set_failed(3) return ah else: print i,"type1 = ", type(i) DOF = 2 if not len(param) == DOF: # check dimension rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(param)) print "parameter is:", param ah.set_failed(3) return ah else: for i in param: print i,"type2 = ", type(i) if not ((type(i) is float) or (type(i) is int)): # check type print type(i) rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name) print "parameter is:", param ah.set_failed(3) return ah else: rospy.logdebug("accepted parameter %f for %s", i, component_name) """ pose_goal = MoveToJointConfigurationGoal() DOF = 2 for i in range(DOF): jv = JointValue() jv.joint_uri = self.gripper1_joint_names[i] jv.value = param[i] jv.unit = "m" pose_goal.goal.positions.append(jv) action_server_name = "/arm_1/gripper_controller/MoveToJointConfigurationDirect" rospy.logdebug("calling %s action server", action_server_name) client = actionlib.SimpleActionClient(action_server_name, MoveToJointConfigurationAction) # trying to connect to server rospy.logdebug("waiting for %s action server to start", action_server_name) if not client.wait_for_server(rospy.Duration(5)): # error: server did not respond rospy.logerr( "%s action server not ready within timeout, aborting...", action_server_name) ah.set_failed(4) return ah else: rospy.logdebug("%s action server ready", action_server_name) print pose_goal client.send_goal(pose_goal) ah.set_client(client) ah.wait_inside() # it should block but it does not, so we wait! rospy.sleep(3.0) return ah
def execute(self, component_name, target=[0, 0, 0, "/base_link"], blocking=True): ah = ActionHandle("move_arm_cart_sample_rpy_direct", component_name, target, blocking) rospy.loginfo("Move <<%s>> DIRECT to <<%s>>", component_name, target) # check pose if not type(target) is list: # check outer list rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name) print "parameter is:", param ah.set_failed(3) return ah else: if not len(target) == 4 or not type( target[0]) == float or not type( target[1]) == float or not type( target[2]) == float or not type( target[3]) == str: # checking parameters rospy.logerr( "no valid target parameters for %s: %s; aborting...", component_name, target) ah.set_failed(3) return ah # convert to pose message pose = MoveToCartesianPoseGoal() pose.goal.header.stamp = rospy.Time.now() pose.goal.header.frame_id = target[3] pose.goal.pose.position.x = target[0] pose.goal.pose.position.y = target[1] pose.goal.pose.position.z = target[2] (qx, qy, qz, qw) = tf.transformations.quaternion_from_euler(0, 0, 0) pose.goal.pose.orientation.x = qx pose.goal.pose.orientation.y = qy pose.goal.pose.orientation.z = qz pose.goal.pose.orientation.w = qw rospy.logdebug("calling %s action server", self.action_server_name) # trying to connect to server rospy.logdebug("waiting for %s action server to start", self.action_server_name) if not self.client.wait_for_server(rospy.Duration(5)): # error: server did not respond rospy.logerr( "%s action server not ready within timeout, aborting...", self.action_server_name) ah.set_failed(4) return ah else: rospy.logdebug("%s action server ready", self.action_server_name) self.client.send_goal(pose) ah.set_client(self.client) ah.wait_inside() return ah
def execute(self, component_name, target="", blocking=True): ah = ActionHandle("move_arm_joint_direct", component_name, target, blocking) rospy.loginfo("Move <<%s>> DIRECT to <<%s>>", component_name, target) # check pose if not type(target) is list: # check outer list rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name) print "parameter is:", target ah.set_failed(3) return ah else: #print i,"type1 = ", type(i) DOF = 5 if not len(target) == DOF: # check dimension rospy.logerr( "no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(target)) print "parameter is:", target ah.set_failed(3) return ah else: for i in target: #print i,"type2 = ", type(i) if not ((type(i) is float) or (type(i) is int)): # check type #print type(i) rospy.logerr( "no valid parameter for %s: not a list of float or int, aborting...", component_name) print "parameter is:", target ah.set_failed(3) return ah else: rospy.logdebug("accepted parameter %f for %s", i, component_name) pose_goal = MoveToJointConfigurationGoal() for i in range(DOF): jv = JointValue() jv.joint_uri = self.arm1_joint_names[i] jv.value = target[i] jv.unit = "rad" pose_goal.goal.positions.append(jv) rospy.logdebug("calling %s action server", self.action_server_name) # trying to connect to server rospy.logdebug("waiting for %s action server to start", self.action_server_name) if not self.client.wait_for_server(rospy.Duration(5)): # error: server did not respond rospy.logerr( "%s action server not ready within timeout, aborting...", self.action_server_name) ah.set_failed(4) return ah else: rospy.logdebug("%s action server ready", self.action_server_name) #print client_goal self.client.send_goal(pose_goal) ah.set_client(self.client) ah.wait_inside() return ah