コード例 #1
0
    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)
コード例 #2
0
ファイル: cob_actions.py プロジェクト: ipa320/research-camp-5
	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
コード例 #3
0
ファイル: cob_actions.py プロジェクト: ipa320/research-camp-5
	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
コード例 #4
0
ファイル: cob_actions.py プロジェクト: ipa320/research-camp-5
	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
コード例 #5
0
    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
コード例 #6
0
ファイル: cob_actions.py プロジェクト: ipa320/research-camp-5
	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
コード例 #7
0
ファイル: cob_actions.py プロジェクト: ipa320/research-camp-5
	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
コード例 #8
0
ファイル: cob_actions.py プロジェクト: ipa320/research-camp-5
	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
コード例 #9
0
    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
コード例 #10
0
    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
コード例 #11
0
    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