def publish_position():
    '''joint position publisher'''    
    msg = JointState()
    msg.header.stamp = rospy.Time.now()
    msg.name = JOINT_NAME_ARRAY
    msg.position = joints_pos
    pub_js.publish(msg)

def talker():
    joint1 = radians(90)
    pub = rospy.Publisher('/kukaAllegroHand/robot_cmd', JointState)
    rospy.init_node('talker', anonymous=True)
    r = rospy.Rate(10000) # 10hz
    msg = JointState()
    msg.position = [0,joint1,0,0,0,0,0]
    pub.publish(msg)
    r.sleep()
    time.sleep(.4)
    
    while True:
        for acc in range(600,601):
            print '************************* acceleratioin: %d *************************' % acc
            # acc = 200
            trjFile = '../trj/acc_new_' + str(acc) + '.dat'
            trj = np.genfromtxt(trjFile, delimiter="\t")
            done = True
            i = 1
            while not rospy.is_shutdown() and i < len(trj):
                print i
                msg = JointState()
                msg.position = [0,joint1,0,0,0,0,0]
                msg.position[0] = radians(trj[i][0])
                msg.position[2] = radians(trj[i][0])
                msg.position[3] = radians(trj[i][0])
                msg.position[4] = radians(trj[i][0])
                msg.position[5] = radians(trj[i][0])
                msg.position[6] = radians(trj[i][0])
                pub.publish(msg)
                r.sleep()
                time.sleep((trj[i][2] - trj[i-1][2])/10)
                i += 1

    def command_joint_position(self, desired_pose):
        """
        Command a specific desired hand pose.

        The desired pose must be the correct dimensionality (self._num_joints).
        Only the pose is commanded, and **no bound-checking happens here**:
        any commanded pose must be valid or Bad Things May Happen. (Generally,
        values between 0.0 and 1.5 are fine, but use this at your own risk.)

        :param desired_pose: The desired joint configurations.
        :return: True if pose is published, False otherwise.
        """

        # Check that the desired pose can have len() applied to it, and that
        # the number of dimensions is the same as the number of hand joints.
        if (not hasattr(desired_pose, '__len__') or
                len(desired_pose) != self._num_joints):
            rospy.logwarn('Desired pose must be a {}-d array: got {}.'
                          .format(self._num_joints, desired_pose))
            return False

        msg = JointState()  # Create and publish
        try:
            msg.position = desired_pose
            self.pub_joint.publish(msg)
            rospy.logdebug('Published desired pose.')
            return True
        except rospy.exceptions.ROSSerializationException:
            rospy.logwarn('Incorrect type for desired pose: {}.'.format(
                desired_pose))
            return False

 def gripper_cb(self, data):
     js = JointState()
     js.header = data.header
     js.name = ["Gripper"]
     js.position = [data.current_pos]
     js.velocity = [data.velocity]
     self.gripper_pub.publish(js)

def handstate_callback(handstate_msg):
    assert len(handstate_msg.positions) == 4
    assert len(handstate_msg.inner_links) == 3
    assert len(handstate_msg.outer_links) == 3

    jointstate_msg = JointState()
    jointstate_msg.header.stamp = handstate_msg.header.stamp
    jointstate_msg.name = [
        joint_prefix + '00',
        # Joint j01 is a mimic joint that is published by robot_state_publisher.
        joint_prefix + '01',
        joint_prefix + '11',
        joint_prefix + '21',
        # TODO: These are not currently being published. See below.
        #joint_prefix + '02',
        #joint_prefix + '12',
        #joint_prefix + '22',
    ]
    jointstate_msg.position += [ handstate_msg.positions[3] ]
    jointstate_msg.position += handstate_msg.inner_links

    # TODO: These positions look very wrong, so we'll let robot_state_publisher
    # use the mimic ratios in HERB's URDF to compute the distal joint angles. 
    #jointstate_msg.position += handstate_msg.outer_links

    jointstate_pub.publish(jointstate_msg)

    def command_joint_torques(self, desired_torques):
        """
        Command a desired torque for each joint.

        The desired torque must be the correct dimensionality
        (self._num_joints). Similarly to poses, we do not sanity-check
        the inputs. As a rule of thumb, values between +- 0.5 are fine.

        :param desired_torques: The desired joint torques.
        :return: True if message is published, False otherwise.
        """

        # Check that the desired torque vector can have len() applied to it,
        # and that the number of dimensions is the same as the number of
        # joints. This prevents passing singletons or incorrectly-shaped lists
        # to the message creation (which does no checking).
        if (not hasattr(desired_torques, '__len__') or
                len(desired_torques) != self._num_joints):
            rospy.logwarn('Desired torques must be a {}-d array: got {}.'
                          .format(self._num_joints, desired_torques))
            return False

        msg = JointState()  # Create and publish
        try:
            msg.effort = desired_torques
            self.pub_joint.publish(msg)
            rospy.logdebug('Published desired torques.')
            return True
        except rospy.exceptions.ROSSerializationException:
            rospy.logwarn('Incorrect type for desired torques: {}.'.format(
                desired_torques))
            return False

def sorted_joint_state_msg(msg, joint_names):
  """
  Returns a sorted C{sensor_msgs/JointState} for the given joint names
  @type  msg: sensor_msgs/JointState
  @param msg: The input message
  @type  joint_names: list
  @param joint_names: The sorted joint names
  @rtype: sensor_msgs/JointState
  @return: The C{JointState} message with the fields in the order given by joint names
  """
  valid_names = set(joint_names).intersection(set(msg.name))
  valid_position = len(msg.name) == len(msg.position)
  valid_velocity = len(msg.name) == len(msg.velocity)
  valid_effort = len(msg.name) == len(msg.effort)
  num_joints = len(valid_names)
  retmsg = JointState()
  retmsg.header = copy.deepcopy(msg.header)
  for name in joint_names:
    if name not in valid_names:
      continue
    idx = msg.name.index(name)
    retmsg.name.append(name)
    if valid_position:
      retmsg.position.append(msg.position[idx])
    if valid_velocity:
      retmsg.velocity.append(msg.velocity[idx])
    if valid_effort:
      retmsg.effort.append(msg.effort[idx])
  return retmsg

    def execute(self, goal):
        #pickle.dump(goal, open("trajecoryGoalMsg3.data", "wb"))
        joint_names = goal.trajectory.joint_names
        trajectory_points = goal.trajectory.points
        num_points = len(trajectory_points)
        rospy.loginfo("Received %i trajectory points" % num_points)
        pprint(goal.trajectory)

        end_time = trajectory_points[-1].time_from_start.to_sec()
        control_rate = rospy.Rate(self._control_rate)
        i=num_points
        rospy.loginfo("the num of point is %i" % i )
        self.jointTrajectoryPublisher.publish(goal.trajectory)

        for trajectory_point in trajectory_points:
            js = JointState()
            #rospy.loginfo("the length of js.name is %i" %len(js.name))
            js.name=copy.deepcopy(joint_names)
            for j in range(len(joint_names)):
                #rospy.loginfo("j is %i" % j)
                js.name[j] = js.name[j]+'_'+str(i)
            i=i-1
            #js.position = trajectory_point.positions
            #js.velocity=trajectory_point.velocities
            #self.jointStatePublisher.publish(js)
            control_rate.sleep()
            
        start_time = rospy.get_time()
        now_from_start = rospy.get_time() - start_time
        
        last_idx = -1
        self._result.error_code = self._result.SUCCESSFUL
        self.server.set_succeeded(self._result)

    def default(self, ci="unused"):
        """ Publish the data of the posture sensor as a ROS JointState message """
        js = JointState()
        js.header = self.get_ros_header()

        js.name = [
            "kuka_arm_0_joint",
            "kuka_arm_1_joint",
            "kuka_arm_2_joint",
            "kuka_arm_3_joint",
            "kuka_arm_4_joint",
            "kuka_arm_5_joint",
            "kuka_arm_6_joint",
            "head_pan_joint",
            "head_tilt_joint",
        ]
        js.position = [
            self.data["seg0"],
            self.data["seg1"],
            self.data["seg2"],
            self.data["seg3"],
            self.data["seg4"],
            self.data["seg5"],
            self.data["seg6"],
            self.data["pan"],
            self.data["tilt"],
        ]
        # js.velocity = [1, 1, 1, 1, 1, 1, 1]
        # js.effort = [50, 50, 50, 50, 50, 50, 50]

        self.publish(js)

 def joint_state_publisher(self):
 
 
     try:
 
         joint_state_msg = JointState()
         joint_fb_msg = FollowJointTrajectoryFeedback()
         time = rospy.Time.now()
         
         with self.lock:
             
             #Joint states
                 
             joint_state_msg.header.stamp = time
             joint_state_msg.name = self.joint_names
             joint_state_msg.position = self.motion_ctrl.get_joint_positions()
             
             #self.joint_state_pub.publish(joint_state_msg)
             
             #Joint feedback
             joint_fb_msg.header.stamp = time
             joint_fb_msg.joint_names = self.joint_names
             joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions()
             
             #self.joint_feedback_pub.publish(joint_fb_msg)
                      
     except Exception as e:
         rospy.logerr('Unexpected exception in joint state publisher: %s', e)

    def computePositionsForNextJoint(self, currPositions, jointsToSweep, maxs, mins, joint_list):
        if len(jointsToSweep) != 0:    
            for kk in range(0, int(1.0/self.resolutionOfSweeps)): # The actual sweeping
                currPositions[joint_list.index(jointsToSweep[0])] 
                currPositions[joint_list.index(jointsToSweep[0])] = mins[joint_list.index(jointsToSweep[0])]+(self.resolutionOfSweeps*kk)*(maxs[joint_list.index(jointsToSweep[0])]-mins[joint_list.index(jointsToSweep[0])])
                if (len(jointsToSweep) > 1):
                    self.computePositionsForNextJoint(currPositions, jointsToSweep[1:len(jointsToSweep)], maxs, mins, joint_list)

                # Combine message and publish
                msg = JointState()
                msg.header.stamp = rospy.Time.now()
                msg.name = self.joint_list
                # print self.free_joints
                msg.position = currPositions
                msg.velocity = [0.0]*len(maxs)
                msg.effort = [0.0]*len(maxs)
                time.sleep(0.0001)
                self.pub.publish(msg)
                # Get End Effector Positions
                rate = rospy.Rate(20.0)
                try:
                    (trans,rot) = self.listener.lookupTransform(self.baseLink, self.finalLink, rospy.Time(0))
                    if (self.currX != trans[0]) or (self.currY != trans[1] or self.currZ != trans[2]):
                        self.csvWriter.writerow(trans)
                    self.currX = trans[0]
                    self.currY = trans[1]
                    self.currZ = trans[2]
                except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                    print "ERROR: looking up tf"
                    time.sleep(0.5) # Sleep for a bit to let system catch up
                    continue

 def generate_views(self):        
     rospy.loginfo('Generate views (%s views at pose)' % self.views_at_pose)
     try:
         resp = self.nav_goals(10, self.inflation_radius, self.roi)
         
         if not resp.goals.poses:
             self.views = []
             return
         for j in range(len(resp.goals.poses)):
             for i in range(0,self.views_at_pose):
                 pose = resp.goals.poses[j]
                 pan = random.uniform(self.min_pan, self.max_pan)
                 tilt = random.uniform(self.min_tilt, self.max_tilt)
                 jointstate = JointState()
                 jointstate.name = ['pan', 'tilt']
                 jointstate.position = [pan, tilt]
                 jointstate.velocity = [self.velocity, self.velocity]
                 jointstate.effort = [float(1.0), float(1.0)]
                 resp_ptu_pose = self.ptu_fk(pan,tilt,pose)
                 p = Pose()
                 view = ScitosView(self.next_id(), pose,jointstate, resp_ptu_pose.pose)
                 self.views.append(view)
             
     except rospy.ServiceException, e:
         rospy.logerr("Service call failed: %s" % e)

    def arm_joint_state(self, robot_state=None, fail_if_joint_not_found=True):
        '''
        Returns the joint state of the arm in the current planning scene state or the passed in state.

        **Args:**

            *robot_state (arm_navigation_msgs.msg.RobotState):* robot state from which to find the joints.  
            If None, will use the current robot state in the planning scene interface
            
            *fail_if_joint_not_found (boolean):* Raise a value error if an arm joint is not found in the robot state

        **Returns:**
            A sensor_msgs.msg.JointState containing just the arm joints as they are in robot_state

        **Raises:**

            **ValueError:** if fail_if_joint_not_found is True and an arm joint is not found in the robot state
        '''
        if not robot_state:
            robot_state = self._psi.get_robot_state()
        joint_state = JointState()
        joint_state.header = robot_state.joint_state.header
        for n in self.joint_names:
            found = False
            for i in range(len(robot_state.joint_state.name)):
                if robot_state.joint_state.name[i] == n:
                    joint_state.name.append(n)
                    joint_state.position.append(robot_state.joint_state.position[i])
                    found = True
                    break
            if not found and fail_if_joint_not_found:
                raise ValueError('Joint '+n+' is missing from robot state')
        return joint_state

def rc_talker():
    '''init pyxhook'''
    #Create hookmanager
    hookman = pyxhook.HookManager()
    #Define our callback to fire when a key is pressed down
    hookman.KeyDown = key_down_event
    hookman.KeyUp   = key_up_event
    #Hook the keyboard
    hookman.HookKeyboard()
    #Start our listener
    hookman.start()

    pub = rospy.Publisher('rc_sender', String, queue_size=10)
    # 2016-10-26 test urdf joint control
    # controls a file in ~/toy_code/toy_ros_space/src/rc_sim_oub/test.launch
    #roslaunch test.launch model:='$(find urdf_tutorial)/urdf/07-physics.urdf' gui:=True
    pub_2 = rospy.Publisher('test_joint', JointState, queue_size=10)
    rospy.init_node('rc_talker')
    rate = rospy.Rate(50)
    while not rospy.is_shutdown():
        #hello_str = "hello world %s" % rospy.get_time()
        pub.publish(''.join(rc_keys))

        # 2016-10-26 test urdf joint control
        msg = JointState()
        msg.header.stamp = rospy.Time.now()
        msg.name = ['base_to_left_arm', 'base_to_right_arm']
        msg.position = [-0.3*int(rc_keys[0]),0.3*int(rc_keys[1])]
        pub_2.publish(msg)
        #print rc_keys
        rate.sleep()
        if running == False:
            #Close the listener when we are done
            hookman.cancel()
            break

    def _move_to(self, point, dur):
            
        rospy.sleep(dur)
        msg = JointState()
        
        with self.lock:
            if not self.sig_stop:
                self.joint_positions = point.positions[:]
		self.joint_velocities = point.velocities[:]

                msg.name = ['joint_1',
                            'joint_2',
                            'joint_3',
                            'joint_4',
                            'joint_5',
                            'joint_6',
                            'joint_7']
		#msg.name = self.joint_names
		msg.position = self.joint_positions
		msg.velocity = self.joint_velocities #[0.2]*7
                msg.header.stamp = rospy.Time.now()

                self.joint_command_pub.publish(msg)

                #rospy.loginfo('Moved to position: %s in %s', str(self.joint_positions), str(dur))
            else:
                rospy.loginfo('Stoping motion immediately, clearing stop signal')
                self.sig_stop = False

 def publishJointState(self):
     msg = JointState()
     msg.header.stamp = rospy.Time.now()
     msg.name = self.jointNames
     msg.position = self.getJointAngles()
     msg.effort = self.getLoads()
     self.jointStatePub.publish(msg)

        def publish(eventStop):

            # for arbotix
            pub1 = rospy.Publisher("q1/command", Float64, queue_size=5)
            pub2 = rospy.Publisher("q2/command", Float64, queue_size=5)
            pub3 = rospy.Publisher("q3/command", Float64, queue_size=5)
            pub4 = rospy.Publisher("q4/command", Float64, queue_size=5)
            pub5 = rospy.Publisher("q5/command", Float64, queue_size=5)

            # for visualization
            jointPub = rospy.Publisher(
                "/joint_states", JointState, queue_size=5)
            jmsg = JointState()
            jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5']

            while not rospy.is_shutdown() and not eventStop.is_set():

                jmsg.header.stamp = rospy.Time.now()
                jmsg.position = self.q
                if self.sim:
                    jointPub.publish(jmsg)

                pub1.publish(self.q[0])
                pub2.publish(self.q[1])
                pub3.publish(self.q[2])
                pub4.publish(self.q[3])
                pub5.publish(self.q[4])

                eventStop.wait(self.dt)

 def test_fk_easy4(self):
     chain_state = JointState()
     chain_state.position = [0, 0, 0]
     eef = self.chain.fk(chain_state, -1)
     print eef
     eef_expected = numpy.matrix([[1, 0, 0, 3], [0, 1, 0, 0], [0, 0, 1, 2], [0, 0, 0, 1]])
     self.assertAlmostEqual(numpy.linalg.norm(eef - eef_expected), 0.0, 6)

 def js_cb(self, a):
     rospy.loginfo('Received array')
     js = JointState()
     js.name = ['head_pan', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
     jList = a.data
     jMatrix = np.reshape(jList,(jList.shape[0]/15,15))
     i = 0
     for pos in jMatrix:
         rospy.loginfo(pos)
         js.position = pos
         gsvr = GetStateValidityRequest()
         rs = RobotState()
         rs.joint_state = js
         gsvr.robot_state = rs
         gsvr.group_name = "both_arms"
         resp = self.coll_client(gsvr)
         if (resp.valid == False):
             rospy.loginfo('false')
             rospy.loginfo(i)
             self.js_pub.publish(0)
             return
         i = i + 1
     self.js_pub.publish(1)
     rospy.loginfo('true')
     return   

def resethead(): # rotate the head(not PTU)
    pub = rospy.Publisher('/head/commanded_state', JointState)       
    head_command = JointState() 
    head_command.name=["HeadPan"]
    head_command.position=[0.0] #forwards  
    pub.publish(head_command)
    eyeplay(0)# reset eyes