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 display_publsisher(path, pan_target):
img = cv2.imread(path)
msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
pub = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1)
pub.publish(msg)
rospy.sleep(1)
for i in range(1, 10):
pub3 = rospy.Publisher('/robot/head/command_head_pan',
HeadPanCommand,
queue_size=1)
msg = HeadPanCommand()
msg.target = pan_target
msg.speed_ratio = 100
msg.enable_pan_request = True
pub3.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(0.1)
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 move_to_pan(self,
angle,
speed,
blocking=False,
margin=0.03,
timeout=5):
start = rospy.get_time()
msg = HeadPanCommand(angle, speed, True)
fix_publisher.publish(msg)
if blocking:
rate = rospy.Rate(10)
while abs(self.pan() - angle) > margin:
if (rospy.get_time() - start) > timeout:
rospy.logerr('Could not reach the target angle (+/- ' +
str(margin) + ' rad) within ' + str(timeout) +
'sec')
break
rate.sleep()
def set_pan(self, angle, speed=1.0, timeout=10.0, scale_speed=False):
"""
Pan at the given speed to the desired angle.
@type angle: float
@param angle: Desired pan angle in radians.
@type speed: int
@param speed: Desired speed to pan at, range is 0-1.0 [1.0]
@type timeout: float
@param timeout: Seconds to wait for the head to pan to the
specified angle. If 0, just command once and
return. [10]
@param scale_speed: Scale speed to pan at by a factor of 100,
to use legacy range between 0-100 [100]
"""
if scale_speed:
cmd_speed = speed / 100.0
else:
cmd_speed = speed
if (cmd_speed < HeadPanCommand.MIN_SPEED_RATIO
or cmd_speed > HeadPanCommand.MAX_SPEED_RATIO):
rospy.logerr(
("Commanded Speed, ({0}), outside of valid range"
" [{1}, {2}]").format(cmd_speed,
HeadPanCommand.MIN_SPEED_RATIO,
HeadPanCommand.MAX_SPEED_RATIO))
msg = HeadPanCommand(angle, cmd_speed, True)
self._pub_pan.publish(msg)
if not timeout == 0:
baxter_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))
def set_pan(self, angle, speed=100, timeout=10.0):
"""
Pan at the given speed to the desired angle.
@type angle: float
@param angle: Desired pan angle in radians.
@type speed: int
@param speed: Desired speed to pan at, range is 0-100 [100]
@type timeout: float
@param timeout: Seconds to wait for the head to pan to the
specified angle. If 0, just command once and
return. [10]
"""
msg = HeadPanCommand(angle, speed)
self._pub_pan.publish(msg)
if not timeout == 0:
baxter_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))
def spin(self):
while not rospy.is_shutdown():
msg = HeadPanCommand()
msg.enable_pan_request = msg.REQUEST_PAN_DISABLE
# move the head if we are executing an action
if not self._done:
# set the target
msg.target = self._target
# speed ratio will be a function of goal time
msg.speed_ratio = np.clip(
1 / (2 * (self._goal_duration.to_sec() + 1e-8)), 0.01, 1)
elif self._enable_noise:
# add noise
# TODO add perlin noise
noise = 0
msg.target = self._target + noise
self.pub.publish(msg)
self._noise_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 __init__(self, addr, robot_model, robot_name='robot'):
SerialController.__init__(self, addr)
#defined in SerialController
self.robot_model = robot_model
self.baxter_larm_names = [
'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1',
'left_w2'
]
self.baxter_rarm_names = [
'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0',
'right_w1', 'right_w2'
]
self.names_klampt_to_baxter = {
'left_upper_shoulder': 'left_s0',
'left_lower_shoulder': 'left_s1',
'left_upper_elbow': 'left_e0',
'left_lower_elbow': 'left_e1',
'left_upper_forearm': 'left_w0',
'left_lower_forearm': 'left_w1',
'left_wrist': 'left_w2',
'right_upper_shoulder': 'right_s0',
'right_lower_shoulder': 'right_s1',
'right_upper_elbow': 'right_e0',
'right_lower_elbow': 'right_e1',
'right_upper_forearm': 'right_w0',
'right_lower_forearm': 'right_w1',
'right_wrist': 'right_w2',
'head': 'head_pan',
'screen': 'head_nod'
}
for l in range(self.robot_model.numLinks()):
klamptName = self.robot_model.link(l).getName()
if klamptName not in self.names_klampt_to_baxter:
self.names_klampt_to_baxter[klamptName] = klamptName
self.baxterDriverNames = [
self.names_klampt_to_baxter[self.robot_model.getDriver(
d).getName()] for d in range(self.robot_model.numDrivers())
]
self.head_pan_link_index = self.robot_model.link("head").index
self.head_nod_link_index = self.robot_model.link("screen").index
self.larm_command = JointCommand()
self.rarm_command = JointCommand()
self.hpan_command = HeadPanCommand()
self.hnod_command = Bool()
# fast indexing structure for partial commands
self.nameToDriverIndex = dict(
zip(self.baxterDriverNames, range(len(self.baxterDriverNames))))
self.nameToLinkIndex = dict(
(self.names_klampt_to_baxter[self.robot_model.link(l).getName()],
l) for l in range(self.robot_model.numLinks()))
# Setup publisher of robot states
self.currentTime = None
self.lastUpdateTime = None
self.currentAssemblyState = None
self.currentJointStates = None
self.currentHeadState = None
rospy.Subscriber("/%s/state" % (robot_name, ), AssemblyState,
self.assemblyStateCallback)
rospy.Subscriber("/%s/joint_states" % (robot_name, ), JointState,
self.jointStatesCallback)
rospy.Subscriber("/%s/head/head_state" % (robot_name, ), HeadState,
self.headStateCallback)
self.pub_larm = rospy.Publisher(
'/%s/limb/left/joint_command' % (robot_name), JointCommand)
self.pub_rarm = rospy.Publisher(
'/%s/limb/right/joint_command' % (robot_name), JointCommand)
self.pub_hpan = rospy.Publisher(
'/%s/head/command_head_pan' % (robot_name), HeadPanCommand)
self.pub_hnod = rospy.Publisher(
'/%s/head/command_head_nod' % (robot_name), Bool)
return
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
