Esempio n. 1
0
    def __on_packet(self, buf):
        state = RobotState.unpack(buf)
        self.last_state = state
        #import deserialize; deserialize.pstate(self.last_state)

        #log("Packet. Mode=%s" % state.robot_mode_data.robot_mode)

        if not state.robot_mode_data.real_robot_enabled:
            rospy.logfatal(
                "Real robot is no longer enabled. Driver is fuxored")
            time.sleep(2)
            sys.exit(1)

        if state.robot_mode_data.emergency_stopped:
            pub_emergency_stop.publish(True)
        else:
            pub_emergency_stop.publish(False)
        # If the urscript program is not executing, then the driver
        # needs to publish joint states using information from the
        # robot state packet.
        if self.robot_state != self.EXECUTING:
            msg = JointState()
            msg.header.stamp = rospy.get_rostime()
            msg.header.frame_id = "From binary state data"
            msg.name = joint_names
            msg.position = [0.0] * 6
            for i, jd in enumerate(state.joint_data):
                msg.position[i] = jd.q_actual + joint_offsets.get(
                    joint_names[i], 0.0)
            msg.velocity = [jd.qd_actual for jd in state.joint_data]
            msg.effort = [0] * 6
            pub_joint_states.publish(msg)
            self.last_joint_states = msg

        # Updates the state machine that determines whether we can program the robot.
        can_execute = (state.robot_mode_data.robot_mode
                       in [RobotMode.READY, RobotMode.RUNNING])
        if self.robot_state == self.CONNECTED:
            if can_execute:
                self.__trigger_ready_to_program()
                self.robot_state = self.READY_TO_PROGRAM
        elif self.robot_state == self.READY_TO_PROGRAM:
            if not can_execute:
                self.robot_state = self.CONNECTED
        elif self.robot_state == self.EXECUTING:
            if not can_execute:
                self.__trigger_halted()
                self.robot_state = self.CONNECTED

        # Report on any unknown packet types that were received
        if len(state.unknown_ptypes) > 0:
            state.unknown_ptypes.sort()
            s_unknown_ptypes = [str(ptype) for ptype in state.unknown_ptypes]
            self.throttle_warn_unknown(
                1.0, "Ignoring unknown pkt type(s): %s. "
                "Please report." % ", ".join(s_unknown_ptypes))
Esempio n. 2
0
    def __on_packet(self, buf):
        state = RobotState.unpack(buf)
        self.last_state = state
        #import deserialize; deserialize.pstate(self.last_state)

        #log("Packet.  Mode=%s" % state.robot_mode_data.robot_mode)

        if not state.robot_mode_data.real_robot_enabled:
            rospy.logfatal("Real robot is no longer enabled.  Driver is fuxored")
            time.sleep(2)
            sys.exit(1)

        # If the urscript program is not executing, then the driver
        # needs to publish joint states using information from the
        # robot state packet.
        if self.robot_state != self.EXECUTING:
            msg = JointState()
            msg.header.stamp = rospy.get_rostime()
            msg.header.frame_id = "From binary state data"
            msg.name = joint_names
            msg.position = [0.0] * 6
            for i, jd in enumerate(state.joint_data):
                msg.position[i] = jd.q_actual + joint_offsets.get(joint_names[i], 0.0)
            msg.velocity = [jd.qd_actual for jd in state.joint_data]
            msg.effort = [0]*6
            pub_joint_states.publish(msg)
            self.last_joint_states = msg

        # Updates the state machine that determines whether we can program the robot.
        can_execute = (state.robot_mode_data.robot_mode in [RobotMode.READY, RobotMode.RUNNING])
        if self.robot_state == self.CONNECTED:
            if can_execute:
                self.__trigger_ready_to_program()
                self.robot_state = self.READY_TO_PROGRAM
        elif self.robot_state == self.READY_TO_PROGRAM:
            if not can_execute:
                self.robot_state = self.CONNECTED
        elif self.robot_state == self.EXECUTING:
            if not can_execute:
                self.__trigger_halted()
                self.robot_state = self.CONNECTED

        # Report on any unknown packet types that were received
        if len(state.unknown_ptypes) > 0:
            state.unknown_ptypes.sort()
            s_unknown_ptypes = [str(ptype) for ptype in state.unknown_ptypes]
            self.throttle_warn_unknown(1.0, "Ignoring unknown pkt type(s): %s. "
                          "Please report." % ", ".join(s_unknown_ptypes))
Esempio n. 3
0
def __on_packet(buf):
    state = RobotState.unpack(buf)

    mb_msg = MasterboardDataMsg()
    mb_msg.digital_input_bits = state.masterboard_data.digital_input_bits
    mb_msg.digital_output_bits = state.masterboard_data.digital_output_bits
    mb_msg.analog_input_range0 = state.masterboard_data.analog_input_range0
    mb_msg.analog_input_range1 = state.masterboard_data.analog_input_range1
    mb_msg.analog_input0 = state.masterboard_data.analog_input0
    mb_msg.analog_input1 = state.masterboard_data.analog_input1
    mb_msg.analog_output_domain0 = state.masterboard_data.analog_output_domain0
    mb_msg.analog_output_domain1 = state.masterboard_data.analog_output_domain1
    mb_msg.analog_output0 = state.masterboard_data.analog_output0
    mb_msg.analog_output1 = state.masterboard_data.analog_output1
    mb_msg.masterboard_temperature = state.masterboard_data.masterboard_temperature
    mb_msg.robot_voltage_48V = state.masterboard_data.robot_voltage_48V
    mb_msg.robot_current = state.masterboard_data.robot_current
    mb_msg.master_io_current = state.masterboard_data.master_io_current
    mb_msg.master_safety_state = state.masterboard_data.master_safety_state
    mb_msg.master_onoff_state = state.masterboard_data.master_onoff_state
    pub_masterboard_state.publish(mb_msg)

    # Use information from the robot state packet to publish IOStates
    msg = IOStates()
    #gets digital in states
    for i in range(0, 10):
        msg.digital_in_states.append(
            DigitalIn(i, (state.masterboard_data.digital_input_bits &
                          (1 << i)) >> i))
    #gets digital out states
    for i in range(0, 10):
        msg.digital_out_states.append(
            DigitalOut(i, (state.masterboard_data.digital_output_bits &
                           (1 << i)) >> i))
    #gets analog_in[0] state
    inp = state.masterboard_data.analog_input0 / MULT_analog_robotstate
    msg.analog_in_states.append(Analog(0, inp))
    #gets analog_in[1] state
    inp = state.masterboard_data.analog_input1 / MULT_analog_robotstate
    msg.analog_in_states.append(Analog(1, inp))
    #gets analog_out[0] state
    inp = state.masterboard_data.analog_output0 / MULT_analog_robotstate
    msg.analog_out_states.append(Analog(0, inp))
    #gets analog_out[1] state
    inp = state.masterboard_data.analog_output1 / MULT_analog_robotstate
    msg.analog_out_states.append(Analog(1, inp))
    #print "Publish IO-Data from robot state data"
    pub_io_states.publish(msg)
Esempio n. 4
0
def __on_packet(buf):
    state = RobotState.unpack(buf)

    mb_msg = MasterboardDataMsg()
    mb_msg.digital_input_bits = state.masterboard_data.digital_input_bits
    mb_msg.digital_output_bits = state.masterboard_data.digital_output_bits
    mb_msg.analog_input_range0 = state.masterboard_data.analog_input_range0
    mb_msg.analog_input_range1 = state.masterboard_data.analog_input_range1
    mb_msg.analog_input0 = state.masterboard_data.analog_input0
    mb_msg.analog_input1 = state.masterboard_data.analog_input1
    mb_msg.analog_output_domain0 = state.masterboard_data.analog_output_domain0
    mb_msg.analog_output_domain1 = state.masterboard_data.analog_output_domain1
    mb_msg.analog_output0 = state.masterboard_data.analog_output0
    mb_msg.analog_output1 = state.masterboard_data.analog_output1
    mb_msg.masterboard_temperature = state.masterboard_data.masterboard_temperature
    mb_msg.robot_voltage_48V = state.masterboard_data.robot_voltage_48V
    mb_msg.robot_current = state.masterboard_data.robot_current
    mb_msg.master_io_current = state.masterboard_data.master_io_current
    mb_msg.master_safety_state = state.masterboard_data.master_safety_state
    mb_msg.master_onoff_state = state.masterboard_data.master_onoff_state
    pub_masterboard_state.publish(mb_msg)

    msg = IOStates()

    for i in range(0, 10):
        msg.digital_in_states.append(
            DigitalIn(i, (state.masterboard_data.digital_input_bits &
                          (1 << i)) >> i))

    for i in range(0, 10):
        msg.digital_out_states.append(
            DigitalOut(i, (state.masterboard_data.digital_output_bits &
                           (1 << i)) >> i))

    inp = state.masterboard_data.analog_input0 / MULT_analog_robotstate
    msg.analog_in_states.append(Analog(0, inp))

    inp = state.masterboard_data.analog_input1 / MULT_analog_robotstate
    msg.analog_in_states.append(Analog(1, inp))

    inp = state.masterboard_data.analog_output0 / MULT_analog_robotstate
    msg.analog_out_states.append(Analog(0, inp))

    inp = state.masterboard_data.analog_output1 / MULT_analog_robotstate
    msg.analog_out_states.append(Analog(1, inp))

    pub_io_states.publish(msg)
Esempio n. 5
0
def __on_packet(buf):
    state = RobotState.unpack(buf)
    
    mb_msg = MasterboardDataMsg()
    mb_msg.digital_input_bits = state.masterboard_data.digital_input_bits
    mb_msg.digital_output_bits = state.masterboard_data.digital_output_bits
    mb_msg.analog_input_range0 = state.masterboard_data.analog_input_range0
    mb_msg.analog_input_range1 = state.masterboard_data.analog_input_range1
    mb_msg.analog_input0 = state.masterboard_data.analog_input0
    mb_msg.analog_input1 = state.masterboard_data.analog_input1
    mb_msg.analog_output_domain0 = state.masterboard_data.analog_output_domain0
    mb_msg.analog_output_domain1 = state.masterboard_data.analog_output_domain1
    mb_msg.analog_output0 = state.masterboard_data.analog_output0
    mb_msg.analog_output1 = state.masterboard_data.analog_output1
    mb_msg.masterboard_temperature = state.masterboard_data.masterboard_temperature
    mb_msg.robot_voltage_48V = state.masterboard_data.robot_voltage_48V
    mb_msg.robot_current = state.masterboard_data.robot_current
    mb_msg.master_io_current = state.masterboard_data.master_io_current
    mb_msg.master_safety_state = state.masterboard_data.master_safety_state
    mb_msg.master_onoff_state = state.masterboard_data.master_onoff_state
    pub_masterboard_state.publish(mb_msg)
    
    
    # Use information from the robot state packet to publish IOStates        
    msg = IOStates()
    #gets digital in states
    for i in range(0, 10):
        msg.digital_in_states.append(DigitalIn(i, (state.masterboard_data.digital_input_bits & (1<<i))>>i))
    #gets digital out states
    for i in range(0, 10):
        msg.digital_out_states.append(DigitalOut(i, (state.masterboard_data.digital_output_bits & (1<<i))>>i))
    #gets analog_in[0] state
    inp = state.masterboard_data.analog_input0 / MULT_analog_robotstate
    msg.analog_in_states.append(Analog(0, inp))
    #gets analog_in[1] state
    inp = state.masterboard_data.analog_input1 / MULT_analog_robotstate
    msg.analog_in_states.append(Analog(1, inp))      
    #gets analog_out[0] state
    inp = state.masterboard_data.analog_output0 / MULT_analog_robotstate
    msg.analog_out_states.append(Analog(0, inp))     
    #gets analog_out[1] state
    inp = state.masterboard_data.analog_output1 / MULT_analog_robotstate
    msg.analog_out_states.append(Analog(1, inp))     
    #print "Publish IO-Data from robot state data"
    pub_io_states.publish(msg)
Esempio n. 6
0
    def __on_packet(self, buf):
        state = RobotState.unpack(buf)
        self.last_state = state
        

        if not state.robot_mode_data.real_robot_enabled:
            rospy.logfatal("Real robot is no longer enabled.  Driver is fuxored")
            time.sleep(2)
            sys.exit(1)
           
        msg = IOStates()
        for i in range(0, 10):
            msg.digital_in_states.append(DigitalIn(i, (state.masterboard_data.digital_input_bits & (1<<i))>>i))
        for i in range(0, 10):
            msg.digital_out_states.append(DigitalOut(i, (state.masterboard_data.digital_output_bits & (1<<i))>>i))
        inp = state.masterboard_data.analog_input0 / MULT_analog_robotstate
        msg.analog_in_states.append(Analog(0, inp))
        inp = state.masterboard_data.analog_input1 / MULT_analog_robotstate
        msg.analog_in_states.append(Analog(1, inp))      
        inp = state.masterboard_data.analog_output0 / MULT_analog_robotstate
        msg.analog_out_states.append(Analog(0, inp))     
        inp = state.masterboard_data.analog_output1 / MULT_analog_robotstate
        msg.analog_out_states.append(Analog(1, inp))     
        pub_io_states.publish(msg)
        can_execute = (state.robot_mode_data.robot_mode in [RobotMode.READY, RobotMode.RUNNING])
        if self.robot_state == self.CONNECTED:
            if can_execute:
                self.__trigger_ready_to_program()
                self.robot_state = self.READY_TO_PROGRAM
        elif self.robot_state == self.READY_TO_PROGRAM:
            if not can_execute:
                self.robot_state = self.CONNECTED
        elif self.robot_state == self.EXECUTING:
            if not can_execute:
                self.__trigger_halted()
                self.robot_state = self.CONNECTED

     
        if len(state.unknown_ptypes) > 0:
            state.unknown_ptypes.sort()
            s_unknown_ptypes = [str(ptype) for ptype in state.unknown_ptypes]
            self.throttle_warn_unknown(1.0, "Ignoring unknown pkt type(s): %s. "
                          "Please report." % ", ".join(s_unknown_ptypes))
Esempio n. 7
0
    def __on_packet(self, buf):
        state = RobotState.unpack(buf)
        self.last_state = state
        #import deserialize; deserialize.pstate(self.last_state)

        #log("Packet.  Mode=%s" % state.robot_mode_data.robot_mode)

        if not state.robot_mode_data.real_robot_enabled:
            rospy.logfatal(
                "Real robot is no longer enabled.  Driver is fuxored")
            time.sleep(2)
            sys.exit(1)

        ###
        # IO-Support is EXPERIMENTAL
        #
        # Notes:
        # - Where are the flags coming from? Do we need flags? No, as 'prog' does not use them and other scripts are not running!
        # - analog_input2 and analog_input3 are within ToolData
        # - What to do with the different analog_input/output_range/domain?
        # - Shall we have appropriate ur_msgs definitions in order to reflect MasterboardData, ToolData,...?
        ###

        # Use information from the robot state packet to publish IOStates
        msg = IOStates()
        #gets digital in states
        for i in range(0, 10):
            msg.digital_in_states.append(
                DigitalIn(i, (state.masterboard_data.digital_input_bits &
                              (1 << i)) >> i))
        #gets digital out states
        for i in range(0, 10):
            msg.digital_out_states.append(
                DigitalOut(i, (state.masterboard_data.digital_output_bits &
                               (1 << i)) >> i))
        #gets analog_in[0] state
        inp = state.masterboard_data.analog_input0 / MULT_analog_robotstate
        msg.analog_in_states.append(Analog(0, inp))
        #gets analog_in[1] state
        inp = state.masterboard_data.analog_input1 / MULT_analog_robotstate
        msg.analog_in_states.append(Analog(1, inp))
        #gets analog_out[0] state
        inp = state.masterboard_data.analog_output0 / MULT_analog_robotstate
        msg.analog_out_states.append(Analog(0, inp))
        #gets analog_out[1] state
        inp = state.masterboard_data.analog_output1 / MULT_analog_robotstate
        msg.analog_out_states.append(Analog(1, inp))
        #print "Publish IO-Data from robot state data"
        pub_io_states.publish(msg)

        # Updates the state machine that determines whether we can program the robot.
        can_execute = (state.robot_mode_data.robot_mode
                       in [RobotMode.READY, RobotMode.RUNNING])
        if self.robot_state == self.CONNECTED:
            if can_execute:
                self.__trigger_ready_to_program()
                self.robot_state = self.READY_TO_PROGRAM
        elif self.robot_state == self.READY_TO_PROGRAM:
            if not can_execute:
                self.robot_state = self.CONNECTED
        elif self.robot_state == self.EXECUTING:
            if not can_execute:
                self.__trigger_halted()
                self.robot_state = self.CONNECTED

        # Report on any unknown packet types that were received
        if len(state.unknown_ptypes) > 0:
            state.unknown_ptypes.sort()
            s_unknown_ptypes = [str(ptype) for ptype in state.unknown_ptypes]
            self.throttle_warn_unknown(
                1.0, "Ignoring unknown pkt type(s): %s. "
                "Please report." % ", ".join(s_unknown_ptypes))
Esempio n. 8
0
    def __on_packet(self, buf):
        state = RobotState.unpack(buf)
        self.last_state = state
        #import deserialize; deserialize.pstate(self.last_state)

        #log("Packet.  Mode=%s" % state.robot_mode_data.robot_mode)

        if not state.robot_mode_data.real_robot_enabled:
            rospy.logfatal("Real robot is no longer enabled.  Driver is fuxored")
            time.sleep(2)
            sys.exit(1)
        
        ###
        # IO-Support is EXPERIMENTAL
        # 
        # Notes: 
        # - Where are the flags coming from? Do we need flags? No, as 'prog' does not use them and other scripts are not running!
        # - analog_input2 and analog_input3 are within ToolData
        # - What to do with the different analog_input/output_range/domain?
        # - Shall we have appropriate ur_msgs definitions in order to reflect MasterboardData, ToolData,...?
        ###
        
        # Use information from the robot state packet to publish IOStates        
        msg = IOStates()
        #gets digital in states
        for i in range(0, 10):
            msg.digital_in_states.append(DigitalIn(i, (state.masterboard_data.digital_input_bits & (1<<i))>>i))
        #gets digital out states
        for i in range(0, 10):
            msg.digital_out_states.append(DigitalOut(i, (state.masterboard_data.digital_output_bits & (1<<i))>>i))
        #gets analog_in[0] state
        inp = state.masterboard_data.analog_input0 / MULT_analog_robotstate
        msg.analog_in_states.append(Analog(0, inp))
        #gets analog_in[1] state
        inp = state.masterboard_data.analog_input1 / MULT_analog_robotstate
        msg.analog_in_states.append(Analog(1, inp))      
        #gets analog_out[0] state
        inp = state.masterboard_data.analog_output0 / MULT_analog_robotstate
        msg.analog_out_states.append(Analog(0, inp))     
        #gets analog_out[1] state
        inp = state.masterboard_data.analog_output1 / MULT_analog_robotstate
        msg.analog_out_states.append(Analog(1, inp))     
        #print "Publish IO-Data from robot state data"
        pub_io_states.publish(msg)
        

        # Updates the state machine that determines whether we can program the robot.
        can_execute = (state.robot_mode_data.robot_mode in [RobotMode.READY, RobotMode.RUNNING])
        if self.robot_state == self.CONNECTED:
            if can_execute:
                self.__trigger_ready_to_program()
                self.robot_state = self.READY_TO_PROGRAM
        elif self.robot_state == self.READY_TO_PROGRAM:
            if not can_execute:
                self.robot_state = self.CONNECTED
        elif self.robot_state == self.EXECUTING:
            if not can_execute:
                self.__trigger_halted()
                self.robot_state = self.CONNECTED

        # Report on any unknown packet types that were received
        if len(state.unknown_ptypes) > 0:
            state.unknown_ptypes.sort()
            s_unknown_ptypes = [str(ptype) for ptype in state.unknown_ptypes]
            self.throttle_warn_unknown(1.0, "Ignoring unknown pkt type(s): %s. "
                          "Please report." % ", ".join(s_unknown_ptypes))