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