Exemplo n.º 1
0
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()
Exemplo n.º 2
0
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()
Exemplo n.º 4
0
 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()
Exemplo n.º 5
0
    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))
Exemplo n.º 6
0
    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))
Exemplo n.º 7
0
    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()
Exemplo n.º 8
0
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...")
Exemplo n.º 9
0
    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
Exemplo n.º 10
0
    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
Exemplo n.º 11
0
    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