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))
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))
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)
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)
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)
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))
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))
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))