def move(self, left_shoulder, left_elbow, left_hand,
		     right_shoulder, right_elbow, right_hand):
		"""
		Creates and sends threads for Baxter's arm movement
		"""
		# Collects angles from each controller
		angles = {'left': [], 'right': []}
		self.human_to_baxter(left_shoulder, left_elbow, left_hand,
		                     right_shoulder, right_elbow, right_hand, angles)
		
		# Creates threads for each of Baxter's arms
		left_queue = Queue.Queue()
		right_queue = Queue.Queue()
		left_thread = threading.Thread(target=self.move_thread,
			                           args=('left', angles['left'], left_queue))
		right_thread = threading.Thread(target=self.move_thread,
			                            args=('right', angles['right'], right_queue))
		left_thread.daemon = True
		right_thread.daemon = True
		left_thread.start()
		right_thread.start()
		dataflow.wait_for(lambda: \
			              not (left_thread.is_alive() or right_thread.is_alive()),
			              timeout=20.0,
			              rate=10)
		left_thread.join()
		right_thread.join()
Ejemplo n.º 2
0
    def dance(self):
        self.set_neutral()

        done = False
        while not done and not rospy.is_shutdown():
            for move in self.moves:
                angles = {
                    'left': self.all_moves(move + '_l'),
                    'right': self.all_moves(move + '_r')
                }
                print angles['left']
                print angles['right']

                left_queue = Queue.Queue()
                right_queue = Queue.Queue()
                left_thread = threading.Thread(target=self.move_thread,
                                               args=('left', angles['left'],
                                                     left_queue))
                right_thread = threading.Thread(target=self.move_thread,
                                                args=('right', angles['right'],
                                                      right_queue))
                left_thread.daemon = True
                right_thread.daemon = True
                left_thread.start()
                right_thread.start()
                dataflow.wait_for(lambda: \
                             not (left_thread.is_alive() or right_thread.is_alive()),
                             timeout=20.0,
                             rate=10)
                left_thread.join()
                right_thread.join()
                rospy.sleep(2.0)
Ejemplo n.º 3
0
    def __init__(self, component_id):
        """
        @param component_id - unique id of analog component
        """
        self._id = component_id
        self._component_type = 'analog_io'
        self._is_output = False

        self._state = {}

        type_ns = '/sdk/robot/' + self._component_type
        topic_base = type_ns + '/' + self._id

        self._sub_state = rospy.Subscriber(
            topic_base + '/state',
            AnalogIOState,
            self._on_io_state)

        dataflow.wait_for(
            lambda: len(self._state.keys()) != 0,
            timeout=2.0,
            timeout_msg="Failed to get current analog_io state from %s" \
            % (topic_base,),
            )

        # check if output-capable before creating publisher
        if self._is_output:
            self._pub_output = rospy.Publisher(
                type_ns + '/command',
                AnalogOutputCommand)
Ejemplo n.º 4
0
	def dance(self):
		self.set_neutral()

		done = False
		while not done and not rospy.is_shutdown():
			for move in self.moves:
				angles = {'left': self.all_moves(move + '_l'),
				          'right': self.all_moves(move + '_r')
				          }
				print angles['left']
				print angles['right']

				left_queue = Queue.Queue()
				right_queue = Queue.Queue()
				left_thread = threading.Thread(target=self.move_thread,
					                           args=('left', angles['left'], left_queue))
				right_thread = threading.Thread(target=self.move_thread,
					                            args=('right', angles['right'], right_queue))
				left_thread.daemon = True
				right_thread.daemon = True
				left_thread.start()
				right_thread.start()
				dataflow.wait_for(lambda: \
			              not (left_thread.is_alive() or right_thread.is_alive()),
			              timeout=20.0,
			              rate=10)
				left_thread.join()
				right_thread.join()
				rospy.sleep(2.0)
Ejemplo n.º 5
0
    def command_nod(self, timeout=5.0):
        """
        Command the head to nod once.

        @param timeout (float)  - Seconds to wait for the head to nod.
                                  If 0, just command once and return.  [0]
        """
        self._pub_nod.publish(True)

        if not timeout == 0:
            # Wait for nod to initiate
            dataflow.wait_for(
                test=lambda: self.nodding(),
                timeout=timeout,
                rate=100,
                timeout_msg="Failed to initiate head nod command",
                body=lambda: self._pub_nod.publish(True)
                )

            # Wait for nod to complete
            dataflow.wait_for(
                test=lambda: not self.nodding(),
                timeout=timeout,
                rate=100,
                timeout_msg="Failed to complete head nod command",
                body=lambda: self._pub_nod.publish(True)
                )
Ejemplo n.º 6
0
    def __init__(self):
        """
        Interface class for the head on the Baxter Robot.

        Used to control the head pan angle and to enable/disable the head nod
        action.
        """
        self._state = {}

        self._pub_pan = rospy.Publisher(
            '/sdk/robot/head/command_head_pan',
            baxter_msgs.msg.HeadPanCommand)

        self._pub_nod = rospy.Publisher(
            '/robot/head/command_head_nod',
            std_msgs.msg.Bool)

        self._sub_state = rospy.Subscriber(
            '/sdk/robot/head/head_state',
            baxter_msgs.msg.HeadState,
            self._on_head_state)

        dataflow.wait_for(
            lambda: len(self._state) != 0,
            timeout=5.0,
            timeout_msg="Failed to get current head state from /sdk/robot/head/head_state",
        )
Ejemplo n.º 7
0
    def __init__(self):
        self.status_changed = dataflow.Signal()

        self._status = UpdateStatus()
        self._avail_updates = UpdateSources()

        self._update_sources = rospy.Subscriber(
            '/usb/update_sources',
            UpdateSources,
            self._on_update_sources)

        self._updater_status_sub = rospy.Subscriber(
            '/updater/status',
            UpdateStatus,
            self._on_update_status)

        self._updater_start = rospy.Publisher(
            '/updater/start',
            std_msgs.msg.String)

        self._updater_stop = rospy.Publisher(
            '/updater/stop',
            std_msgs.msg.Empty)

        dataflow.wait_for(
            lambda: self._avail_updates.uuid != '',
            timeout = 1.0,
            timeout_msg = "Failed to get list of available updates")
Ejemplo n.º 8
0
    def __init__(self, component_id):
        """
        @param component_id - unique id of the digital component
        """
        self._id = component_id
        self._component_type = 'digital_io'
        self._is_output = False

        self._state = {}

        type_ns = '/sdk/robot/' + self._component_type
        topic_base = type_ns + '/' + self._id

        self._sub_state = rospy.Subscriber(
            topic_base + '/state',
            DigitalIOState,
            self._on_io_state)

        dataflow.wait_for(
            lambda: len(self._state.keys()) != 0,
            timeout=2.0,
            timeout_msg="Failed to get current digital_io state from %s" \
            % (topic_base,),
            )

        # check if output-capable before creating publisher
        if self._is_output:
            self._pub_output = rospy.Publisher(
                type_ns + '/command',
                DigitalOutputCommand)
Ejemplo n.º 9
0
    def __init__(self, gripper):
        """
        Interface class for a gripper on the Baxter robot.

        @param gripper - gripper to interface
        """
        self.name = gripper

        ns = '/robot/limb/' + self.name + '/accessory/gripper/'
        sdkns = '/sdk' + ns

        self._pub_enable = rospy.Publisher(
            ns + 'set_enabled',
            stdmsg.Bool)

        self._pub_reset = rospy.Publisher(
            ns + 'command_reset',
            stdmsg.Bool)

        self._pub_calibrate = rospy.Publisher(
            ns + 'command_calibrate',
            stdmsg.Empty)

        self._pub_command = rospy.Publisher(
            sdkns + 'command_set',
            baxmsg.GripperCommand)

        self._sub_identity = rospy.Subscriber(
            sdkns + 'identity',
            baxmsg.GripperIdentity,
            self._on_gripper_identity)

        self._sub_properties = rospy.Subscriber(
            sdkns + 'properties',
            baxmsg.GripperProperties,
            self._on_gripper_properties)

        self._sub_state = rospy.Subscriber(
            sdkns + 'state',
            baxmsg.GripperState,
            self._on_gripper_state)

        self._command = baxmsg.GripperCommand(
            position=0.0,
            force=30.0,
            velocity=100.0,
            holding=0.0,
            deadZone=3.0)

        self._identity = baxmsg.GripperIdentity()
        self._properties = baxmsg.GripperProperties()
        self._state = None

        dataflow.wait_for(
            lambda: not self._state is None,
            timeout=5.0,
            timeout_msg="Failed to get current gripper state from %s" % (sdkns + 'state'),
        )
Ejemplo n.º 10
0
    def _toggle_enabled(self, status):

        pub = rospy.Publisher('/robot/set_super_enable', std_msgs.msg.Bool)

        dataflow.wait_for(
            test=lambda: self._state.enabled == status,
            timeout=2.0 if status else 5.0,
            timeout_msg="Failed to %sable robot" % ('en' if status else 'dis',),
            body=lambda: pub.publish(status),
        )
Ejemplo n.º 11
0
 def reboot(self, timeout=2.0):
     """
     Reboot the gripper
     """
     self._pub_reset.publish(True)
     dataflow.wait_for(
         test=lambda: not self.calibrated(),
         timeout=timeout,
         body=lambda: self._pub_reset.publish(True)
         )
Ejemplo n.º 12
0
 def disable(self, timeout=2.0):
     """
     Disable the gripper
     """
     self._pub_enable.publish(False)
     dataflow.wait_for(
         test=lambda: not self.enabled(),
         timeout=timeout,
         body=lambda: self._pub_enable.publish(False)
         )
Ejemplo n.º 13
0
 def enable(self, timeout=2.0):
     """
     Enable the gripper
     """
     self._pub_enable.publish(True)
     dataflow.wait_for(
         test=lambda: self.enabled(),
         timeout=timeout,
         body=lambda: self._pub_enable.publish(True)
         )
Ejemplo n.º 14
0
 def reset(self, timeout=2.0):
     """
     Reset the gripper
     """
     self._pub_reset.publish(False)
     dataflow.wait_for(
         test=lambda: not self.error(),
         timeout=timeout,
         body=lambda: self._pub_reset.publish(False)
         )
Ejemplo n.º 15
0
    def _toggle_enabled(self, status):

        pub = rospy.Publisher('/robot/set_super_enable', std_msgs.msg.Bool)

        dataflow.wait_for(
            test=lambda: self._state.enabled == status,
            timeout=2.0 if status else 5.0,
            timeout_msg="Failed to %sable robot" %
            ('en' if status else 'dis', ),
            body=lambda: pub.publish(status),
        )
Ejemplo n.º 16
0
 def calibrate(self, timeout=5.0):
     """
     Calibrate the gripper
     """
     self._pub_calibrate.publish(stdmsg.Empty())
     self.enable()
     dataflow.wait_for(
         test=lambda: self.calibrated(),
         timeout=timeout,
         body=lambda: self._pub_calibrate.publish(stdmsg.Empty())
         )
Ejemplo n.º 17
0
 def stop(self):
     """
     Simulate an e-stop button being pressed.  Robot must be reset to clear
     the stopped state.
     """
     pub = rospy.Publisher('/robot/set_super_stop', std_msgs.msg.Empty)
     dataflow.wait_for(
         test=lambda: self._state.stopped == True,
         timeout=3.0,
         timeout_msg="Failed to stop the robot",
         body=lambda: pub.publish(),
     )
Ejemplo n.º 18
0
 def stop(self):
     """
     Simulate an e-stop button being pressed.  Robot must be reset to clear
     the stopped state.
     """
     pub = rospy.Publisher('/robot/set_super_stop', std_msgs.msg.Empty)
     dataflow.wait_for(
         test=lambda: self._state.stopped == True,
         timeout=3.0,
         timeout_msg="Failed to stop the robot",
         body=lambda: pub.publish(),
     )
Ejemplo n.º 19
0
    def __init__(self):
        self._state = None
        topic = '/sdk/robot/state'
        self._state_sub = rospy.Subscriber(topic,
                                           baxter_msgs.msg.AssemblyState,
                                           self._state_callback)

        dataflow.wait_for(
            lambda: not self._state is None,
            timeout=2.0,
            timeout_msg="Failed to get current robot state from %s" %
            (topic, ),
        )
Ejemplo n.º 20
0
    def __init__(self):
        self._state = None
        topic = '/sdk/robot/state'
        self._state_sub = rospy.Subscriber(
            topic,
            baxter_msgs.msg.AssemblyState,
            self._state_callback)

        dataflow.wait_for(
            lambda: not self._state is None,
            timeout=2.0,
            timeout_msg="Failed to get current robot state from %s" % (topic,),
        )
Ejemplo n.º 21
0
    def __init__(self, limb):
        """
        Interface class for a limb on the Baxter robot.

        @param limb - limb to interface
        """
        self.name = limb
        self._joint_angle = {}
        self._joint_velocity = {}
        self._joint_effort = {}
        self._cartesian_pose = {}
        self._cartesian_velocity = {}
        self._cartesian_effort = {}

        self._joint_names = {
            'left': ['left_s0', 'left_s1', 'left_e0', 'left_e1', \
                'left_w0', 'left_w1', 'left_w2'],
            'right': ['right_s0', 'right_s1', 'right_e0', 'right_e1', \
                'right_w0', 'right_w1', 'right_w2']
            }

        ns = '/robot/limb/' + limb + '/'
        sdkns = '/sdk' + ns

        self._pub_joint_mode = rospy.Publisher(
            ns + 'joint_command_mode',
            baxter_msgs.msg.JointCommandMode)

        self._pub_joint_position = rospy.Publisher(
            ns + 'command_joint_angles',
            baxter_msgs.msg.JointPositions)

        self._pub_joint_velocity = rospy.Publisher(
            ns + 'command_joint_velocities',
            baxter_msgs.msg.JointVelocities)

        self._last_state_time = None
        self._state_rate = 0

        joint_state_sub = rospy.Subscriber(
            '/robot/joint_states',
            sensor_msgs.msg.JointState,
            self._on_joint_states)

        cartesian_state_sub = rospy.Subscriber(
            sdkns + 'endpoint/state',
            baxter_msgs.msg.EndpointState,
            self._on_endpoint_states)

        dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0)
Ejemplo n.º 22
0
    def __init__(self, limb):
        """
        Interface class for a limb on the Baxter robot.

        @param limb - limb to interface
        """
        self.name = limb
        self._joint_angle = {}
        self._joint_velocity = {}
        self._joint_effort = {}
        self._cartesian_pose = {}
        self._cartesian_velocity = {}
        self._cartesian_effort = {}

        self._joint_names = {
            'left': ['left_s0', 'left_s1', 'left_e0', 'left_e1', \
                'left_w0', 'left_w1', 'left_w2'],
            'right': ['right_s0', 'right_s1', 'right_e0', 'right_e1', \
                'right_w0', 'right_w1', 'right_w2']
            }

        ns = '/robot/limb/' + limb + '/'
        sdkns = '/sdk' + ns

        self._pub_joint_mode = rospy.Publisher(
            ns + 'joint_command_mode', baxter_msgs.msg.JointCommandMode)

        self._pub_joint_position = rospy.Publisher(
            ns + 'command_joint_angles', baxter_msgs.msg.JointPositions)

        self._pub_joint_velocity = rospy.Publisher(
            ns + 'command_joint_velocities', baxter_msgs.msg.JointVelocities)

        self._last_state_time = None
        self._state_rate = 0

        joint_state_sub = rospy.Subscriber('/robot/joint_states',
                                           sensor_msgs.msg.JointState,
                                           self._on_joint_states)

        cartesian_state_sub = rospy.Subscriber(sdkns + 'endpoint/state',
                                               baxter_msgs.msg.EndpointState,
                                               self._on_endpoint_states)

        dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0)
Ejemplo n.º 23
0
	def openCamera(self):

		if self._id == 'head_camera':
            		self._set_control_value(CameraControl.CAMERA_CONTROL_FLIP, True)
            		self._set_control_value(CameraControl.CAMERA_CONTROL_MIRROR, True)

		ret = self._open_svc(self._id, self._settings)
		if ret.err != 0:
				raise OSError(ret.err, "Failed to open camera")
        	self._open = True

		self._image_sub = rospy.Subscriber('/cameras/'+self._id+'/image', sensor_msgs.msg.Image, self._on_image_update)

		dataflow.wait_for(
            		lambda: not (self._frameMsg) is None,
            		timeout=5.0,
            		timeout_msg="Failed to get current image",
        	)
Ejemplo n.º 24
0
    def reset(self):
        """
        Reset all joints.  Trigger JRCP hardware to reset all faults.  Disable
        the robot.
        """
        is_reset = lambda: self._state.enabled == False\
            and self._state.stopped == False \
            and self._state.error == False \
            and self._state.estop_button == 0 \
            and self._state.estop_source == 0

        pub = rospy.Publisher('/robot/set_super_reset', std_msgs.msg.Empty)

        dataflow.wait_for(
            test=is_reset,
            timeout=3.0,
            timeout_msg="Failed to reset robot",
            body=lambda: pub.publish(),
        )
Ejemplo n.º 25
0
    def reset(self):
        """
        Reset all joints.  Trigger JRCP hardware to reset all faults.  Disable
        the robot.
        """
        is_reset = lambda: self._state.enabled == False\
            and self._state.stopped == False \
            and self._state.error == False \
            and self._state.estop_button == 0 \
            and self._state.estop_source == 0

        pub = rospy.Publisher('/robot/set_super_reset', std_msgs.msg.Empty)

        dataflow.wait_for(
            test=is_reset,
            timeout=3.0,
            timeout_msg="Failed to reset robot",
            body=lambda: pub.publish(),
        )
Ejemplo n.º 26
0
    def __init__(self, location):
        if not location in self.__LOCATIONS:
            raise AttributeError("Invalid Navigator name '%s'" % (location,))
        self._id = location
        self._state = None
        self.button0_changed = dataflow.Signal()
        self.button1_changed = dataflow.Signal()
        self.button2_changed = dataflow.Signal()
        self.wheel_changed = dataflow.Signal()

        self._state_sub = rospy.Subscriber(
            "/sdk/robot/itb/%s_itb/state" % (self._id,), baxter_msgs.msg.ITB, self._on_state
        )

        self._inner_led = digital_io.DigitalIO("%s_itb_light_inner" % (self._id,))

        self._outer_led = digital_io.DigitalIO("%s_itb_light_outer" % (self._id,))

        dataflow.wait_for(lambda: self._state != None)
Ejemplo n.º 27
0
    def set_pan(self, angle, speed=100, timeout=10.0):
        """
        Pan at the given speed to the desired angle.

        @param angle (float)    - Desired pan angle in radians.
        @param speed (int)      - Desired speed to pan at, range is 0-100 [100]
        @param timeout (float)  - Seconds to wait for the head to pan to the specified
                                  angle.  If 0, just command once and return.  [0]
        """
        msg = baxter_msgs.msg.HeadPanCommand(angle, speed)
        self._pub_pan.publish(msg)

        if not timeout == 0:
            dataflow.wait_for(
                lambda: abs(self.pan()-angle) <= settings.HEAD_PAN_ANGLE_TOLERANCE,
                timeout=timeout,
                rate=100,
                timeout_msg="Failed to move head to pan command %f" % angle,
                body=lambda: self._pub_pan.publish(msg)
                )
Ejemplo n.º 28
0
    def move_to_joint_positions(self, positions, timeout=15.0):
        """
        @param positions dict({str:float})   - dictionary of joint_name:angle
        @param timeout    - seconds to wait for move to finish [15]

        Commands the limb to the provided positions.  Waits until the reported
        joint state matches that specified.
        """
        def genf(joint, angle):
            def joint_diff():
                return abs(angle - self._joint_angle[joint])
            return joint_diff

        diffs = [genf(j,a) for j,a in positions.items() if j in self._joint_angle]

        dataflow.wait_for(
            lambda: not any(diff() >= settings.JOINT_ANGLE_TOLERANCE for diff in diffs),
            timeout=timeout,
            rate=100,
            body=lambda: self.set_joint_positions(positions)
            )
Ejemplo n.º 29
0
	def open_arms(self):
		angles_l = [ 0.4, 0.2, -1.0, 0.4, 0.0, 0.4, 0.0]
		angles_r = [-0.4, 0.2,  1.0, 0.4, 0.0, 0.4, 0.0]
	
		left_queue = Queue.Queue()
		right_queue = Queue.Queue()

		left_thread = threading.Thread(
	    	target=self.move_thread,
            args=(self.left_arm,
                  dict(zip(self.left_arm.joint_names(), angles_l)),
                  left_queue)
	    	)
		right_thread = threading.Thread(
	    	target=self.move_thread,
	    	args=(self.right_arm,
                  dict(zip(self.right_arm.joint_names(), angles_r)),
                  right_queue)
            )

		left_thread.daemon = True
		right_thread.daemon = True
		left_thread.start()
		right_thread.start()
		dataflow.wait_for(
	    	lambda: \
	    	not (left_thread.is_alive() or right_thread.is_alive()),
	    	timeout=20.0,
	    	rate=10,
	    	)
		left_thread.join()
		right_thread.join()
		result = left_queue.get()
		if not result == None:
			raise left_queue.get()
		result = right_queue.get()
		if not result == None:
			raise right_queue.get()
		rospy.sleep(1.0)
Ejemplo n.º 30
0
    def set_output(self, value, timeout=2.0):
        """
        Control the state of the Digital Output.

        @param value bool    - new state {True, False} of the Output.
        @param timeout (float)  - Seconds to wait for the io to reflect command.
                                  If 0, just command once and return.  [0]
        """
        if not self._is_output:
            raise IOError(errno.EACCES, "Component is not an output [%s: %s]" % (self._component_type, self._id))
        cmd = DigitalOutputCommand()
        cmd.name = self._id
        cmd.value = value
        self._pub_output.publish(cmd)

        if not timeout == 0:
            dataflow.wait_for(
                test=lambda: self.state() == value,
                timeout=timeout,
                rate=100,
                timeout_msg=("Failed to command digital io to: %r" % (value,)),
                body=lambda: self._pub_output.publish(cmd),
            )
Ejemplo n.º 31
0
    def close_arms(self):
        angles_l = [-0.6, 0.2, -1.8, 0.4, 0.0, 0.4, 0.0]
        angles_r = [0.6, 0.3, 1.8, 0.4, 0.0, 0.4, -0.3]

        left_queue = Queue.Queue()
        right_queue = Queue.Queue()

        left_thread = threading.Thread(
            target=self.move_thread,
            args=(self.left_arm,
                  dict(zip(self.left_arm.joint_names(),
                           angles_l)), left_queue))
        right_thread = threading.Thread(
            target=self.move_thread,
            args=(self.right_arm,
                  dict(zip(self.right_arm.joint_names(),
                           angles_r)), right_queue))

        left_thread.daemon = True
        right_thread.daemon = True
        left_thread.start()
        right_thread.start()
        dataflow.wait_for(
         lambda: \
         not (left_thread.is_alive() or right_thread.is_alive()),
         timeout=20.0,
         rate=10,
         )
        left_thread.join()
        right_thread.join()
        result = left_queue.get()
        if not result == None:
            raise left_queue.get()
        result = right_queue.get()
        if not result == None:
            raise right_queue.get()
        rospy.sleep(1.0)
Ejemplo n.º 32
0
    def move_to_joint_positions(self, positions, timeout=15.0):
        """
        @param positions dict({str:float})   - dictionary of joint_name:angle
        @param timeout    - seconds to wait for move to finish [15]

        Commands the limb to the provided positions.  Waits until the reported
        joint state matches that specified.
        """
        def genf(joint, angle):
            def joint_diff():
                return abs(angle - self._joint_angle[joint])

            return joint_diff

        diffs = [
            genf(j, a) for j, a in positions.items() if j in self._joint_angle
        ]

        dataflow.wait_for(
            lambda: not any(diff() >= settings.JOINT_ANGLE_TOLERANCE
                            for diff in diffs),
            timeout=timeout,
            rate=100,
            body=lambda: self.set_joint_positions(positions))
Ejemplo n.º 33
0
    def __init__(self, component_id):
        """
        @param component_id - unique id of the digital component
        """
        self._id = component_id
        self._component_type = "digital_io"
        self._is_output = False

        self._state = {}

        type_ns = "/sdk/robot/" + self._component_type
        topic_base = type_ns + "/" + self._id

        self._sub_state = rospy.Subscriber(topic_base + "/state", DigitalIOState, self._on_io_state)

        dataflow.wait_for(
            lambda: len(self._state.keys()) != 0,
            timeout=2.0,
            timeout_msg="Failed to get current digital_io state from %s" % (topic_base,),
        )

        # check if output-capable before creating publisher
        if self._is_output:
            self._pub_output = rospy.Publisher(type_ns + "/command", DigitalOutputCommand)
Ejemplo n.º 34
0
    def set_output(self, value, timeout=2.0):
        """
        Control the state of the Digital Output.

        @param value bool    - new state {True, False} of the Output.
        @param timeout (float)  - Seconds to wait for the io to reflect command.
                                  If 0, just command once and return.  [0]
        """
        if not self._is_output:
            raise IOError(errno.EACCES, "Component is not an output [%s: %s]" %
                (self._component_type, self._id))
        cmd = DigitalOutputCommand()
        cmd.name = self._id
        cmd.value = value
        self._pub_output.publish(cmd)

        if not timeout == 0:
            dataflow.wait_for(
                test=lambda: self.state() == value,
                timeout=timeout,
                rate=100,
                timeout_msg=("Failed to command digital io to: %r" % (value,)),
                body=lambda: self._pub_output.publish(cmd)
                )
    def _on_trajectory_action(self, goal):
        joint_names = goal.trajectory.joint_names
        trajectory_points = goal.trajectory.points
        # Load parameters for trajectory
        if not self._get_trajectory_parameters(joint_names):
            self._server.set_aborted()
            return

        # Create a new discretized joint trajectory
        num_points = len(trajectory_points)

        if num_points == 0:
            rospy.logerr("%s: Empty Trajectory" % (self._action_name, ))
            self._server.set_aborted()
            return

        # If all time_from_start are zero,
        # interject these based on default velocities
        if all(pt.time_from_start.to_sec() == 0.0 for pt in trajectory_points):
            last_point = self._get_current_position(joint_names)
            move_time = 0.0
            for point in trajectory_points:
                diffs = map(operator.sub, point.positions, last_point)
                diffs = map(operator.abs, diffs)
                dflt_vel = [self._dflt_vel[jnt] for jnt in joint_names]
                move_time = move_time + max(map(operator.div, diffs, dflt_vel))
                point.time_from_start = rospy.Duration(move_time)
                last_point = point.positions

        def interp(a, b, pct):
            return a + (b - a) * pct

        def interp_positions(p1, p2, pct):
            return map(interp, p1.positions, p2.positions,
                       [pct] * len(p1.positions))

        end_time = trajectory_points[-1].time_from_start.to_sec()
        control_rate = rospy.Rate(self._control_rate)

        pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]

        # Wait for the specified execution time, if not provided use now
        start_time = goal.trajectory.header.stamp.to_sec()
        if start_time == 0.0:
            start_time = rospy.get_time()
        dataflow.wait_for(lambda: rospy.get_time() >= start_time,
                          timeout=float('inf'))

        # Loop until end of trajectory time.  Provide a single time step
        # of the control rate past the end to ensure we get to the end.
        now_from_start = rospy.get_time() - start_time
        while now_from_start < end_time + (1.0 / self._control_rate):
            idx = bisect.bisect(pnt_times, now_from_start)

            if idx == 0:
                # If our current time is before the first specified point
                # in the trajectory, then we should interpolate between
                # our current position and that point.
                p1 = JointTrajectoryPoint()
                p1.positions = self._get_current_position(joint_names)
            else:
                p1 = trajectory_points[idx - 1]

            if idx != num_points:
                p2 = trajectory_points[idx]
                pct = ((now_from_start - p1.time_from_start.to_sec()) /
                       (p2.time_from_start - p1.time_from_start).to_sec())
                point = interp_positions(p1, p2, pct)
            else:
                # If the current time is after the last trajectory point,
                # just hold that position.
                point = p1.positions

            if not self._command_velocities(joint_names, point):
                return

            control_rate.sleep()
            now_from_start = rospy.get_time() - start_time

        # Keep trying to meet goal until goal_time constraint expired
        last_point = trajectory_points[-1].positions
        last_time = trajectory_points[-1].time_from_start.to_sec()

        def check_goal_state():
            for error in self._get_current_error(joint_names, last_point):
                if (self._goal_error[error[0]] > 0
                        and self._goal_error[error[0]] < math.fabs(error[1])):
                    return error[0]
            else:
                return None

        while ((rospy.get_time() < start_time + last_time + self._goal_time)
               and check_goal_state()):
            if not self._command_velocities(joint_names, last_point):
                return
            control_rate.sleep()

        # Verify goal constraint
        result = check_goal_state()
        if result != None:
            self._command_stop(goal.trajectory.joint_names)
            rospy.logerr("%s: Exceeded Goal Threshold Error %s" % (
                self._action_name,
                result,
            ))
            self._server.set_aborted()
        else:
            self._command_stop(goal.trajectory.joint_names)
            self._server.set_succeeded()
Ejemplo n.º 36
0
class MoveArms(SmokeTest):
    """Move both arms to numerous joint positions.
    """
    def __init__(self, name='MoveArms'):
        super(MoveArms, self).__init__(name)

    def start_test(self):
        """Runs MoveArms Smoke Test
        """

        def move_thread(limb, angle, queue, timeout=15.0):
            """Threaded joint movement allowing for simultaneous joint moves
            """
            try:
                limb.move_to_joint_positions(angle, timeout)
                queue.put(None)
            except Exception, exception:
                queue.put(traceback.format_exc())
                queue.put(exception)

        try:
            print("Enabling robot...")
            self._rs.enable()
            print("Test: Create Limb Instances")
            right = baxter_interface.Limb('right')
            left = baxter_interface.Limb('left')
            left_queue = Queue.Queue()
            right_queue = Queue.Queue()
            # Max Joint Range (s0, s1, e0, e1, w0, w1, w2)
            #     ( 1.701,  1.047,  3.054,  2.618,  3.059,  2.094,  3.059)
            # Min Joint Range (e0, e1, s0, s1, w0, w1, w2) 
            #     (-1.701, -2.147, -3.054, -0.050, -3.059, -1.571, -3.059)
            joint_moves = (
                [ 0.0, -0.55, 0.0, 0.75, 0.0, 1.26,  0.0],
                [ 0.5,  -0.8, 2.8, 0.15, 0.0,  1.9,  2.8],
                [-0.1,  -0.8,-1.0, 2.5,  0.0, -1.4, -2.8],
                [ 0.0, -0.55, 0.0, 0.75, 0.0, 1.26,  0.0],
                )
            for move in joint_moves:
                print(
                    "Test: Moving to Joint Positions: " + \
                    ", ".join("%.2f" % x for x in move)
                    )
                left_thread = threading.Thread(
                    target=move_thread, 
                    args=(left, dict(zip(left.joint_names(), move)), left_queue)
                    )
                right_thread = threading.Thread(
                    target=move_thread, 
                    args=(right, dict(zip(right.joint_names(), move)), right_queue)
                    )
                left_thread.daemon = True
                right_thread.daemon = True
                left_thread.start()
                right_thread.start()
                dataflow.wait_for(
                    lambda: \
                    not (left_thread.is_alive() or right_thread.is_alive()),
                    timeout=20.0,
                    rate=10,
                    )
                left_thread.join()
                right_thread.join()
                result = left_queue.get()
                if not result == None:
                    raise left_queue.get()
                result = right_queue.get()
                if not result == None:
                    raise right_queue.get()
                rospy.sleep(1.0)
            print("Disabling robot...")
            self._rs.disable()
            self.result[0] = True
            rospy.sleep(5.0)
        except:
            self.return_failure(traceback.format_exc())
    def _on_trajectory_action(self, goal):
        joint_names = goal.trajectory.joint_names
        trajectory_points = goal.trajectory.points
        # Load parameters for trajectory
        if not self._get_trajectory_parameters(joint_names):
            self._server.set_aborted()
            return

        # Create a new discretized joint trajectory
        num_points = len(trajectory_points)

        if num_points == 0:
            rospy.logerr("%s: Empty Trajectory" % (self._action_name,))
            self._server.set_aborted()
            return

        # If all time_from_start are zero,
        # interject these based on default velocities
        if all(pt.time_from_start.to_sec() == 0.0 for pt in trajectory_points):
            last_point = self._get_current_position(joint_names)
            move_time = 0.0
            for point in trajectory_points:
                diffs = map(operator.sub, point.positions,
                            last_point)
                diffs = map(operator.abs, diffs)
                dflt_vel= [self._dflt_vel[jnt] for jnt in joint_names]
                move_time = move_time + max(map(operator.div, diffs, dflt_vel))
                point.time_from_start = rospy.Duration(move_time)
                last_point = point.positions

        def interp(a, b, pct):
            return a + (b - a) * pct

        def interp_positions(p1, p2, pct):
            return map(interp, p1.positions, p2.positions, [pct] *
                       len(p1.positions))

        end_time = trajectory_points[-1].time_from_start.to_sec()
        control_rate = rospy.Rate(self._control_rate)

        pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]

         # Wait for the specified execution time, if not provided use now
        start_time = goal.trajectory.header.stamp.to_sec()
        if start_time == 0.0:
            start_time = rospy.get_time()
        dataflow.wait_for(
            lambda: rospy.get_time() >= start_time,
            timeout=float('inf')
        )

        # Loop until end of trajectory time.  Provide a single time step
        # of the control rate past the end to ensure we get to the end.
        now_from_start = rospy.get_time() - start_time
        while now_from_start < end_time + (1.0 / self._control_rate):
            idx = bisect.bisect(pnt_times, now_from_start)

            if idx == 0:
                # If our current time is before the first specified point
                # in the trajectory, then we should interpolate between
                # our current position and that point.
                p1 = JointTrajectoryPoint()
                p1.positions = self._get_current_position(joint_names)
            else:
                p1 = trajectory_points[idx - 1]

            if idx != num_points:
                p2 = trajectory_points[idx]
                pct = ((now_from_start - p1.time_from_start.to_sec()) /
                       (p2.time_from_start - p1.time_from_start).to_sec())
                point = interp_positions(p1, p2, pct)
            else:
                # If the current time is after the last trajectory point,
                # just hold that position.
                point = p1.positions


            if not self._command_velocities(joint_names, point):
                return

            control_rate.sleep()
            now_from_start = rospy.get_time() - start_time

        # Keep trying to meet goal until goal_time constraint expired
        last_point = trajectory_points[-1].positions
        last_time = trajectory_points[-1].time_from_start.to_sec()
        def check_goal_state():
            for error in self._get_current_error(joint_names, last_point):
                if (self._goal_error[error[0]] > 0
                    and self._goal_error[error[0]] < math.fabs(error[1])):
                    return error[0]
            else:
                return None

        while ((rospy.get_time() < start_time + last_time + self._goal_time)
               and check_goal_state()):
            if not self._command_velocities(joint_names, last_point):
                return
            control_rate.sleep()

        # Verify goal constraint
        result = check_goal_state()
        if result != None:
            self._command_stop(goal.trajectory.joint_names)
            rospy.logerr("%s: Exceeded Goal Threshold Error %s" %
                         (self._action_name, result,))
            self._server.set_aborted()
        else:
            self._command_stop(goal.trajectory.joint_names)
            self._server.set_succeeded()
Ejemplo n.º 38
0
        updater.stop_update()
    rospy.on_shutdown(on_shutdown)

    updater.status_changed.connect(on_update_status)

    try:
        updater.command_update(uuid)
    except OSError, e:
        if e.errno == errno.EINVAL:
            print(str(e))
            return 1
        raise

    try:
        dataflow.wait_for(
            lambda: nl.done == True,
            timeout = 5 * 60,
            timeout_msg = "Timeout waiting for update to succeed")
    except Exception, e:
        if not (hasattr(e, 'errno') and e.errno == errno.ESHUTDOWN):
            print (str(e))
        nl.rc = 1

    return nl.rc

def usage():
    print """
%s [ARGUMENTS]

    -h, --help          This screen
    -l, --list          List available updates
    -u, --update [UUID] Launch the given update