def head_state():
pub = rospy.Publisher('/robot/head/command_head_pan', HeadPanCommand, queue_size=10)
rospy.init_node('head_state_node', anonymous=False)
HS = HeadPanCommand()
HS.speed = 10
HS.target = 0.0
rate = rospy.Rate(10) # 10hz
#when using the state machine use the subscriber below to intiate callback
#and pass rate
#rospy.Subscriber('/state_or_whatever',....,callback, HS,rate)
while not rospy.is_shutdown():
pub.publish(HS)
rate.sleep()
def head_state():
pub = rospy.Publisher('/robot/head/command_head_pan',
HeadPanCommand,
queue_size=10)
rospy.init_node('head_state_node', anonymous=False)
HS = HeadPanCommand()
HS.speed = 10
HS.target = 0.0
rate = rospy.Rate(10) # 10hz
#when using the state machine use the subscriber below to intiate callback
#and pass rate
#rospy.Subscriber('/state_or_whatever',....,callback, HS,rate)
while not rospy.is_shutdown():
pub.publish(HS)
rate.sleep()
def execute_path(data_path):
left_limb_commander = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
queue_size=10)
right_limb_commander = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
head_pan_commander = rospy.Publisher('/robot/head/command_head_pan',
HeadPanCommand,
queue_size=10)
head_nod_commander = rospy.Publisher('/robot/head/command_head_nod',
Bool,
queue_size=10)
rospy.init_node('joint_commander', anonymous=True)
traj = get_trajectory(data_path)
for joint_state in traj:
left_limb_joint_names = []
left_limb_joint_values = []
right_limb_joint_names = []
right_limb_joint_values = []
head_pan_target = None
head_nod = False
joint_state = literal_eval(joint_state)
for joint_name, state in joint_state.items():
if joint_name.startswith('left'):
left_limb_joint_names.append(joint_name)
left_limb_joint_values.append(state)
elif joint_name.startswith('right'):
right_limb_joint_names.append(joint_name)
right_limb_joint_values.append(state)
elif joint_name == "head_pan":
head_pan_target = state
elif joint_name == "head_nod":
if abs(state) > 0:
head_nod = True
else:
head_nod = False
left_command = JointCommand()
left_command.mode = 1
left_command.names = left_limb_joint_names
left_command.command = left_limb_joint_values
right_command = JointCommand()
right_command.mode = 1
right_command.names = right_limb_joint_names
right_command.command = right_limb_joint_values
head_pan_command = HeadPanCommand()
head_pan_command.target = head_pan_target
head_pan_command.speed = 0.1
head_nod_command = Bool()
head_nod_command.data = head_nod
left_limb_commander.publish(left_command)
right_limb_commander.publish(right_command)
head_pan_commander.publish(head_pan_command)
head_nod_commander.publish(head_nod_command)
rospy.loginfo("State reached...")
def process(self):
if self.currentAssemblyState == None:
#not enabled, return
print "Waiting for /robot/state..."
time.sleep(1.0)
return
if self.currentAssemblyState.stopped:
print "Robot is stopped"
time.sleep(1.0)
return
if self.currentJointStates == None:
print "Waiting for joint states to be published..."
time.sleep(1.0)
return
if self.lastUpdateTime == None:
self.lastUpdateTime = self.currentJointStates.header.stamp
return
self.currentTime = self.currentJointStates.header.stamp
#convert to Klamp't message
klamptInput = dict()
klamptInput['t'] = self.currentTime.to_sec()
klamptInput['dt'] = self.currentTime.to_sec(
) - self.lastUpdateTime.to_sec()
if klamptInput['dt'] == 0.0:
#no new clock messages read
return
klamptInput['q'] = [0.0] * self.robot_model.numLinks()
klamptInput['dq'] = [0.0] * self.robot_model.numLinks()
klamptInput['torque'] = [0.0] * self.robot_model.numDrivers()
for i, n in enumerate(self.currentJointStates.name):
if n not in self.nameToLinkIndex:
if n != 'torso_t0':
print "Ignoring state of link", n
continue
klamptIndex = self.nameToLinkIndex[n]
klamptInput['q'][klamptIndex] = self.currentJointStates.position[i]
if len(self.currentJointStates.velocity) > 0:
klamptInput['dq'][
klamptIndex] = self.currentJointStates.velocity[i]
if len(self.currentJointStates.effort) > 0:
driverIndex = self.nameToDriverIndex[n]
klamptInput['torque'][
driverIndex] = self.currentJointStates.effort[i]
#if self.currentHeadState != None:
# klamptInput['q'][self.head_pan_link_index] = self.currentHeadState.pan
#output is defined in SerialController
klamptReply = self.output(**klamptInput)
if klamptReply == None:
return False
#now conver the Klamp't reply to a Baxter command
lcmd = JointCommand()
rcmd = JointCommand()
lcmd.names = self.baxter_larm_names
rcmd.names = self.baxter_rarm_names
hpcmd = HeadPanCommand()
hncmd = Bool()
if 'qcmd' in klamptReply:
#use position mode
lcmd.mode = rcmd.mode = POSITION_MODE
q = klamptReply['qcmd']
lcmd.command = [q[self.nameToLinkIndex[n]] for n in lcmd.names]
rcmd.command = [q[self.nameToLinkIndex[n]] for n in rcmd.names]
hpcmd.target = q[self.head_pan_link_index]
hpcmd.speed = q[self.head_pan_link_index]
elif 'dqcmd' in klamptReply:
lcmd.mode = rcmd.mode = VELOCITY_MODE
dq = klamptReply['dqcmd']
lcmd.command = [dq[self.nameToLinkIndex[n]] for n in lcmd.names]
rcmd.command = [dq[self.nameToLinkIndex[n]] for n in rcmd.names]
hpcmd = None
elif 'torquecmd' in klamptReply:
lcmd.mode = rcmd.mode = TORQUE_MODE
torque = klamptReply['torquecmd']
lcmd.command = [
torque[self.nameToDriverIndex[n]] for n in lcmd.names
]
rcmd.command = [
torque[self.nameToDriverIndex[n]] for n in rcmd.names
]
hpcmd = None
else:
lcmd = rcmd = None
hpcmd = None
hncmd = None
if self.currentAssemblyState.estop_button != AssemblyState.ESTOP_BUTTON_UNPRESSED:
print "Estop is on..."
time.sleep(1.0)
elif not self.currentAssemblyState.enabled:
print "Waiting for robot to turn on..."
time.sleep(1.0)
else:
#publish the messages
if lcmd:
self.pub_larm.publish(lcmd)
if rcmd:
self.pub_rarm.publish(rcmd)
if hpcmd:
self.pub_hpan.publish(hpcmd)
if hncmd:
self.pub_hnod.publish(hncmd)
self.lastUpdateTime = self.currentTime
return True
def process(self):
if self.currentAssemblyState==None:
#not enabled, return
print "Waiting for /robot/state..."
time.sleep(1.0)
return
if self.currentAssemblyState.stopped:
print "Robot is stopped"
time.sleep(1.0)
return
if self.currentJointStates==None:
print "Waiting for joint states to be published..."
time.sleep(1.0)
return
if self.lastUpdateTime == None:
self.lastUpdateTime = self.currentJointStates.header.stamp
return
self.currentTime = self.currentJointStates.header.stamp
#convert to Klamp't message
klamptInput = dict()
klamptInput['t'] = self.currentTime.to_sec()
klamptInput['dt'] = self.currentTime.to_sec() - self.lastUpdateTime.to_sec()
if klamptInput['dt'] == 0.0:
#no new clock messages read
return
klamptInput['q'] = [0.0]*self.robot_model.numLinks()
klamptInput['dq'] = [0.0]*self.robot_model.numLinks()
klamptInput['torque'] = [0.0]*self.robot_model.numDrivers()
for i,n in enumerate(self.currentJointStates.name):
if n not in self.nameToLinkIndex:
if n != 'torso_t0':
print "Ignoring state of link",n
continue
klamptIndex = self.nameToLinkIndex[n]
klamptInput['q'][klamptIndex] = self.currentJointStates.position[i]
if len(self.currentJointStates.velocity) > 0:
klamptInput['dq'][klamptIndex] = self.currentJointStates.velocity[i]
if len(self.currentJointStates.effort) > 0:
driverIndex = self.nameToDriverIndex[n]
klamptInput['torque'][driverIndex] = self.currentJointStates.effort[i]
#if self.currentHeadState != None:
# klamptInput['q'][self.head_pan_link_index] = self.currentHeadState.pan
#output is defined in SerialController
klamptReply = self.output(**klamptInput)
if klamptReply == None:
return False
#now conver the Klamp't reply to a Baxter command
lcmd = JointCommand()
rcmd = JointCommand()
lcmd.names = self.baxter_larm_names
rcmd.names = self.baxter_rarm_names
hpcmd = HeadPanCommand()
hncmd = Bool()
if 'qcmd' in klamptReply:
#use position mode
lcmd.mode = rcmd.mode = POSITION_MODE
q = klamptReply['qcmd']
lcmd.command = [q[self.nameToLinkIndex[n]] for n in lcmd.names]
rcmd.command = [q[self.nameToLinkIndex[n]] for n in rcmd.names]
hpcmd.target = q[self.head_pan_link_index]
hpcmd.speed = q[self.head_pan_link_index]
elif 'dqcmd' in klamptReply:
lcmd.mode = rcmd.mode = VELOCITY_MODE
dq = klamptReply['dqcmd']
lcmd.command = [dq[self.nameToLinkIndex[n]] for n in lcmd.names]
rcmd.command = [dq[self.nameToLinkIndex[n]] for n in rcmd.names]
hpcmd = None
elif 'torquecmd' in klamptReply:
lcmd.mode = rcmd.mode = TORQUE_MODE
torque = klamptReply['torquecmd']
lcmd.command = [torque[self.nameToDriverIndex[n]] for n in lcmd.names]
rcmd.command = [torque[self.nameToDriverIndex[n]] for n in rcmd.names]
hpcmd = None
else:
lcmd = rcmd = None
hpcmd = None
hncmd = None
if self.currentAssemblyState.estop_button != AssemblyState.ESTOP_BUTTON_UNPRESSED:
print "Estop is on..."
time.sleep(1.0)
elif not self.currentAssemblyState.enabled:
print "Waiting for robot to turn on..."
time.sleep(1.0)
else:
#publish the messages
if lcmd:
self.pub_larm.publish(lcmd)
if rcmd:
self.pub_rarm.publish(rcmd)
if hpcmd:
self.pub_hpan.publish(hpcmd)
if hncmd:
self.pub_hnod.publish(hncmd)
self.lastUpdateTime = self.currentTime
return True
