Python TransformListener.lookupTransform方法代码示例

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]
def subscriber_callback(data):
	occupancyMap = transformation(data.data, data.info.width, data.info.height)
	way_points = find_goals(occupancyMap, data.info.width, data.info.height)
	result = random.choice(way_points)

	try:
		planMaker = rospy.ServiceProxy('move_base/make_plan', nav_srv.GetPlan)
		listener = TransformListener()
		listener.waitForTransform("base_link", "map", rospy.Time(), rospy.Duration(4.0))
		t = listener.getLatestCommonTime("base_link", "map")
		position, quaternion = listener.lookupTransform("base_link", "map", t)
		pos_x = position[0]
		pos_y = position[1]
		pos_z = position[2]
		goal_robot = create_goal((result[1]-2000)*data.info.resolution,(result[0]-2000)*data.info.resolution)
		#Make plan with 0.5 meter flexibility, from target pose and current pose (with same header)
		start_pose = create_message(pos_x,pos_y,pos_z,quaternion)

		plan = planMaker(
			start_pose,
			goal_robot.target_pose,
			0.5)
		action_server = actionlib.SimpleActionClient('move_base', mb_msg.MoveBaseAction);
		action_server.wait_for_server()
		send_goal(goal_robot, action_server)

	except rospy.ServiceException, e:
		print "plan service call failed: %s"%e

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]
class FaceCommander(Head):
    def __init__(self):
        Head.__init__(self)
        self._world = 'base'
        self._tf_listener = TransformListener()
        self.set_pan(0)

    def look_at(self, obj=None):
        if obj is None:
            self.set_pan(0)
            return True

        if isinstance(obj, str):
            pose = self._tf_listener.lookupTransform('head', obj, rospy.Time(0))
        else:
            rospy.logerr("FaceCommander.look_at() accepts only strings atm")
            return False

        hyp = transformations.norm(pose)
        adj = pose[0][1]
        angle = pi/2 - arccos(float(adj)/hyp)

        if isnan(angle):
            rospy.logerr('FaceCommander cannot look at {}'.format(obj))
            return False

        self.set_pan(angle)
        return True

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]
class ChallengeProblemLogger(object):
  _knownObstacles = {}
  _placedObstacle = False
  _lastgzlog = 0.0
  _tf_listener = None
  
  def __init__(self,name):
    self._name = name;

    self._experiment_started = False
    self._tf_listener = TransformListener()

    # Subscribe to robot pose ground truth from gazebo
    rospy.Subscriber("/gazebo/model_states", ModelStates, callback=self.gazebo_model_monitor,
                     queue_size=1)

  # Whenever we get a report from Gazebo, map the gazebo coordinate to map coordinates and
  # log this
  # Only do this every second - this should be accurate enough
  # TODO: model is assumed to be the third in the list. Need to make this based
  #       on the array to account for obstacles (maybe)
  def gazebo_model_monitor(self, data):
    if (len(data.pose))<=2:
      return
    data_time = rospy.get_rostime().to_sec()
    if ((self._lastgzlog == 0.0) | (data_time - self._lastgzlog >= 1)):
      # Only do this every second

      # Get the turtlebot model state information (assumed to be indexed at 2)    
      tb_pose = data.pose[2]
      tb_position = tb_pose.position
      self._lastgzlog = data_time

      # Do this only if the transform exists
      if self._tf_listener.frameExists("/base_link") and self._tf_listener.frameExists("/map"):
        self._tf_listener.waitForTransform("/map", "/base_link", rospy.Time(0), rospy.Duration(1))
        (trans,rot) = self._tf_listener.lookupTransform("/map", "/base_link",rospy.Time(0))
        rospy.loginfo("BRASS | Turtlebot | {},{}".format(trans[0], trans[1]))
      
      # Log any obstacle information, but do it only once. This currently assumes one obstacle
      # TODO: test this
      if len(data.name) > 3:
        addedObstacles = {}
        removedObstacles = self._knownObstacles.copy()
        for obs in range(3, len(data.name)-1):
          if (data.name[obs] not in self._knownObstacles):
            addedObstacles[data.name[obs]] = obs
          else:
             self._knownObstacles[data.name[obs]] = obs
 	     del removedObstacles[data.name[obs]]
        
        for key, value in removedObstacles.iteritems():
           rospy.logInfo("BRASS | Obstacle {} | removed".format(key))
           del self._knownObstacles[key]

        for key, value in addedObstacles.iteritems():
	   obs_pos = data.pose[value].position
           rospy.logInfo ("BRASS | Obstacle {} | {},{}".format(key, obs_pos.x, obs_pos.y))
	   self._knownObstacles[key] = value

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]
def calculate_distance_to_rows():
    tflisten = TransformListener()
    dist = []
    for i in range(0,n_rows):
        (veh_trans,veh_rot) = tflisten.lookupTransform("odom","base_footprint",rospy.Time(0))
        
        
    pass

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]
class TfFilter:
    def __init__(self, buffer_size):
        self.tf = TransformListener(True, rospy.Duration(5))
        self.target = ''
        self.buffer = np.zeros((buffer_size, 1))
        self.buffer_ptr = 0
        self.buffer_size = buffer_size
        self.tf_sub = rospy.Subscriber('tf', tfMessage, self.tf_cb)
        self.tf_pub = rospy.Publisher('tf', tfMessage)
        self.srv = rospy.Service(SERVICE, PublishGoal, self.publish_goal_cb)

    def tf_cb(self, msg):
        for t in msg.transforms:
            if t.child_frame_id == self.target:
                time = self.tf.getLatestCommonTime(self.source, self.target)
                p, q = self.tf.lookupTransform(self.source, self.target, time)
                rm = tfs.quaternion_matrix(q)
                # assemble a new coordinate frame that has z-axis aligned with
                # the vertical direction and x-axis facing the z-axis of the
                # original frame
                z = rm[:3, 2]
                z[2] = 0
                axis_x = tfs.unit_vector(z)
                axis_z = tfs.unit_vector([0, 0, 1])
                axis_y = np.cross(axis_x, axis_z)
                rm = np.vstack((axis_x, axis_y, axis_z)).transpose()
                # shift the pose along the x-axis
                self.position = p + np.dot(rm, self.d)[:3]
                self.buffer[self.buffer_ptr] = tfs.euler_from_matrix(rm)[2]
                self.buffer_ptr = (self.buffer_ptr + 1) % self.buffer_size
                self.publish_filtered_tf(t.header)

    def publish_filtered_tf(self, header):
        yaw = np.median(self.buffer)
        q = tfs.quaternion_from_euler(0, 0, yaw - math.pi)
        ts = TransformStamped()
        ts.header = header
        ts.header.frame_id = self.source
        ts.child_frame_id = self.goal
        ts.transform.translation.x = self.position[0]
        ts.transform.translation.y = self.position[1]
        ts.transform.translation.z = 0
        ts.transform.rotation.x = q[0]
        ts.transform.rotation.y = q[1]
        ts.transform.rotation.z = q[2]
        ts.transform.rotation.w = q[3]
        msg = tfMessage()
        msg.transforms.append(ts)
        self.tf_pub.publish(msg)

    def publish_goal_cb(self, r):
        rospy.loginfo('Received [%s] request. Will start publishing %s frame' %
                      (SERVICE, r.goal_frame_id))
        self.source = r.source_frame_id
        self.target = r.target_frame_id
        self.goal = r.goal_frame_id
        self.d = [r.displacement.x, r.displacement.y, r.displacement.z]
        return []

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]
class KinectNiteHelp:
    def __init__(self, name="kinect_listener", user=1):
        # rospy.init_node(name, anonymous=True)
        self.tf = TransformListener()
        self.user = user

    def getLeftHandPos(self):
        if self.tf.frameExists(BASE_FRAME) and self.tf.frameExists("left_hand_1"):
            print " Inside TF IF Get LEft HAnd POS "
            t = self.tf.getLatestCommonTime(BASE_FRAME, "left_hand_1")
            (leftHandPos, quaternion) = self.tf.lookupTransform(BASE_FRAME, "left_hand_1", t)
            return leftHandPos

    def getRightHandPos(self):
        if self.tf.frameExists(BASE_FRAME) and self.tf.frameExists("right_hand_1"):
            t = self.tf.getLatestCommonTime(BASE_FRAME, "right_hand_1")
            (rightHandPos, quaternion) = self.tf.lookupTransform(BASE_FRAME, "right_hand_1", t)
            return rightHandPos

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]
class TransformNode:
    def __init__(self, *args):
        self.tf = TransformListener()
        rospy.Subscriber("/tf", TFMessage, transform, queue_size=1)

    def transform(self, msg):
        if self.tf.frameExists("/base_link") and self.tf.frameExists("/map"):
            t = self.tf.getLatestCommonTime("/base_link", "/map")
            position, quaternion = self.tf.lookupTransform("/base_link", "/map", t)
            print position, quaternion

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]
class Demo:
    def __init__(self, goals):
        rospy.init_node("demo", anonymous=True)
        self.frame = rospy.get_param("~frame")
        self.pubGoal = rospy.Publisher("goal", PoseStamped, queue_size=1)
        self.listener = TransformListener()
        self.goals = goals
        self.goalIndex = 0

    def run(self):
        self.listener.waitForTransform("/world", self.frame, rospy.Time(), rospy.Duration(5.0))
        goal = PoseStamped()
        goal.header.seq = 0
        goal.header.frame_id = "world"
        while not rospy.is_shutdown():
            goal.header.seq += 1
            goal.header.stamp = rospy.Time.now()
            goal.pose.position.x = self.goals[self.goalIndex][0]
            goal.pose.position.y = self.goals[self.goalIndex][1]
            goal.pose.position.z = self.goals[self.goalIndex][2]
            quaternion = tf.transformations.quaternion_from_euler(0, 0, self.goals[self.goalIndex][3])
            goal.pose.orientation.x = quaternion[0]
            goal.pose.orientation.y = quaternion[1]
            goal.pose.orientation.z = quaternion[2]
            goal.pose.orientation.w = quaternion[3]

            self.pubGoal.publish(goal)

            t = self.listener.getLatestCommonTime("/world", self.frame)
            if self.listener.canTransform("/world", self.frame, t):
                position, quaternion = self.listener.lookupTransform("/world", self.frame, t)
                rpy = tf.transformations.euler_from_quaternion(quaternion)
                if (
                    math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.3
                    and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.3
                    and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.3
                    and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10)
                    and self.goalIndex < len(self.goals) - 1
                ):
                    rospy.sleep(self.goals[self.goalIndex][4])
                    self.goalIndex += 1

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]
class TwistToPoseConverter(object):
    def __init__(self):
        self.ee_frame = rospy.get_param('~ee_frame', "/l_gripper_tool_frame")
        self.twist_sub = rospy.Subscriber('/twist_in', TwistStamped, self.twist_cb)
        self.pose_pub = rospy.Publisher('/pose_out', PoseStamped)
        self.tf_listener = TransformListener()

    def get_ee_pose(self, frame='/torso_lift_link', time=None):
        """Get current end effector pose from tf."""
        try:
            now = rospy.Time.now() if time is None else time
            self.tf_listener.waitForTransform(frame, self.ee_frame, now, rospy.Duration(4.0))
            pos, quat = self.tf_listener.lookupTransform(frame, self.ee_frame, now)
        except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
            rospy.logwarn("[%s] TF Failure getting current end-effector pose:\r\n %s"
                            %(rospy.get_name(), e))
            return None, None
        return pos, quat

    def twist_cb(self, ts):
        """Get current end effector pose and augment with twist command."""
        cur_pos, cur_quat = self.get_ee_pose(ts.header.frame_id)
        ps = PoseStamped()
        ps.header.frame_id = ts.header.frame_id
        ps.header.stamp = ts.header.stamp
        ps.pose.position.x = cur_pos[0] + ts.twist.linear.x
        ps.pose.position.y = cur_pos[1] + ts.twist.linear.y
        ps.pose.position.z = cur_pos[2] + ts.twist.linear.z
        twist_quat = trans.quaternion_from_euler(ts.twist.angular.x,
                                                 ts.twist.angular.y,
                                                 ts.twist.angular.z)
        final_quat = trans.quaternion_multiply(twist_quat, cur_quat)
        ps.pose.orientation = Quaternion(*final_quat)
        try:
            self.tf_listener.waitForTransform('/torso_lift_link', ps.header.frame_id,
                                              ps.header.stamp, rospy.Duration(3.0))
            pose_out = self.tf_listener.transformPose('/torso_lift_link', ps)
            self.pose_pub.publish(pose_out)
        except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
            rospy.logwarn("[%s] TF Failure transforming goal pose to torso_lift_link:\r\n %s"
                           %(rospy.get_name(), e))

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]
class Transformation():
	def __init__(self):
		pub = 0
		self.tf = TransformListener()
		self.tf1 = TransformerROS()
		self.fdata = geometry_msgs.msg.TransformStamped()
		self.fdata_base = geometry_msgs.msg.TransformStamped()
		self.transform = tf.TransformBroadcaster()
		self.wrench = WrenchStamped()
		self.wrench_bl = WrenchStamped()
		
	def wrench_cb(self,msg):
		self.wrench = msg.wrench
		self.transform.sendTransform((0,0,-0.025),quaternion_from_euler(3.14, 0, 3.665195102, 'rxyz'),rospy.Time.now(),'/ft_debug_link','/arm_7_link')
		self.fdata.transform.translation.x = self.wrench.force.x
		self.fdata.transform.translation.y = self.wrench.force.y
		self.fdata.transform.translation.z = self.wrench.force.z
		
		try:
			if self.tf.frameExists("/dummy_link") and self.tf.frameExists("ft_debug_link"):
				t = self.tf.getLatestCommonTime("/dummy_link", "/ft_debug_link")
				(transform_ee_base_position,transform_ee_base_quaternion) = self.tf.lookupTransform("/dummy_link", '/ft_debug_link', t)
	   				#print transform_ee_base_position
	   				#print transform_ee_base_quaternion
	   				#print self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)
		except(tf.LookupException,tf.ConnectivityException):
			print("TRANSFORMATION ERROR")
			sss.say(["error"])	
			#return 'failed'
		
		self.fdata_base.transform.translation.x =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,3]))
		
		self.fdata_base.transform.translation.y =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,3]))

		self.fdata_base.transform.translation.z =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,3]))	
		
		self.wrench_bl.wrench.force.y = self.fdata_base.transform.translation.y
		self.wrench_bl.wrench.force.x = self.fdata_base.transform.translation.x
		self.wrench_bl.wrench.force.z = self.fdata_base.transform.translation.z
		self.wrench_bl.header.stamp = rospy.Time.now();
		self.pub.publish(self.wrench_bl)

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]
class getStepTowardsPoint(smach.State):
    """
    Calculates the next step towards a point defined in userdata.
    """
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded'], input_keys=['next_x','next_y'],output_keys=['step_next_x','step_next_y'])
        self.__listen = TransformListener()
        self.__cur_pos = None
        self.__step_next_pos = None
        self.__odom_frame = rospy.get_param("~odom_frame","/odom")
        self.__base_frame = rospy.get_param("~base_frame","/base_footprint")
        self.step = 1
    
    def __get_current_position(self):
        ret = False
        try:
            (self.__cur_pos,rot) = self.__listen.lookupTransform( self.__odom_frame,self.__base_frame,rospy.Time(0))
            ret = True
        except (tf.LookupException, tf.ConnectivityException),err:
            rospy.loginfo("could not locate vehicle")
        return ret

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]
class Controller():
    Manual = 0
    Automatic = 1
    TakeOff = 2

    def __init__(self):
        self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        self.listener = TransformListener()
        rospy.Subscriber("joy", Joy, self._joyChanged)
        rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged)
        self.cmd_vel_telop = Twist()
        #self.pidX = PID(20, 12, 0.0, -30, 30, "x")
        #self.pidY = PID(-20, -12, 0.0, -30, 30, "y")
        #self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z")
        #self.pidYaw = PID(50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
        self.pidX = PID(20, 12.0, 0.2, -30, 30, "x")
        self.pidY = PID(-20, -12.0, -0.2, -30, 30, "y")
        #50000  800
        self.pidZ = PID(15000, 3000.0,  1500.0, 10000, 57000, "z")
        self.pidYaw = PID(50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
        self.state = Controller.Manual
        self.targetZ = 1
        self.targetX = 0.0
        self.targetY = -1.0
        self.des_angle = 90.0
        self.lastZ = 0.0
        self.power = 50000.0
	self.pubVz = rospy.Publisher('vel_z', Float32, queue_size=1)
        self.lastJoy = Joy()

    def _cmdVelTelopChanged(self, data):
        self.cmd_vel_telop = data
        if self.state == Controller.Manual:
            self.pubNav.publish(data)

    def pidReset(self):
        self.pidX.reset()
        self.pidZ.reset()
        self.pidZ.reset()
        self.pidYaw.reset()

    def _joyChanged(self, data):
        if len(data.buttons) == len(self.lastJoy.buttons):
            delta = np.array(data.buttons) - np.array(self.lastJoy.buttons)
            print ("Buton ok")
            #Button 1
            if delta[0] == 1 and self.state != Controller.Automatic:
                print("Automatic!")
                #thrust = self.cmd_vel_telop.linear.z
                #print(thrust)
                self.pidReset()
                self.pidZ.integral = self.power/self.pidZ.ki
                self.lastZ = 0.0
                #self.targetZ = 1
                self.state = Controller.Automatic
            #Button 2
            if delta[1] == 1 and self.state != Controller.Manual:
                print("Manual!")
                self.pubNav.publish(self.cmd_vel_telop)
                self.state = Controller.Manual
            #Button 3
            if delta[2] == 1:
                self.state = Controller.TakeOff
                print("TakeOff!")
            #Button 5
            if delta[4] == 1:
                self.targetY = 0.0
                self.des_angle = -90.0
                #print(self.targetZ)
                #self.power += 100.0
                print(self.power)
                #self.state = Controller.Automatic
            #Button 6
            if delta[5] == 1:
                self.targetY = -1.0
                self.des_angle = 90.0
                #print(self.targetZ)
                #self.power -= 100.0
                print(self.power)
                #self.state = Controller.Automatic
        self.lastJoy = data

    def run(self):
        thrust = 0
        print("jello")
        while not rospy.is_shutdown():
            if self.state == Controller.TakeOff:
                t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark3")
                if self.listener.canTransform("/mocap", "/Nano_Mark3", t):
                    position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark3", t)
                    print(position[0],position[1],position[2])			
                    if position[2] > 0.2 or thrust > 54000:
                        self.pidReset()
                        #self.pidZ.integral = thrust / self.pidZ.ki
                        #self.targetZ = 0.5
                        self.state = Controller.Automatic
                        thrust = 0
                    else:
                        thrust += 100
                        self.power = thrust
#.........这里部分代码省略.........

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]
class SprayAction:
    """
        Drives x meters either forward or backwards depending on the given distance.
        the velocity should always be positive. 
    """

    def __init__(self, name, odom_frame, base_frame):
        """
        
        @param name: the name of the action
        @param odom_frame: the frame the robot is moving in (odom_combined)
        @param base_frame: the vehicles own frame (usually base_link)
        """

        self._action_name = name
        self.__odom_frame = odom_frame
        self.__base_frame = base_frame
        self.__server = actionlib.SimpleActionServer(self._action_name, sprayAction, auto_start=False)
        self.__server.register_preempt_callback(self.preempt_cb)
        self.__server.register_goal_callback(self.goal_cb)

        self.__cur_pos = 0
        self.__start_pos = 0
        self.__start_yaw = 0
        self.__cur_yaw = 0

        self.__feedback = sprayFeedback()

        self.__listen = TransformListener()
        # self.vel_pub = rospy.Publisher("/fmControllers/cmd_vel_auto",Twist)
        self.spray_pub = rospy.Publisher("/fmControllers/spray", UInt32)

        self.__start_time = rospy.Time.now()
        self.new_goal = False
        self.n_sprays = 0
        self.spray_msg = UInt32()

        self.spray_l = False
        self.spray_cnt = 0

        self.__server.start()

    def preempt_cb(self):
        rospy.loginfo("Preempt requested")
        self.spray_msg.data = 0
        self.spray_pub.publish(self.spray_msg)
        self.__server.set_preempted()

    def goal_cb(self):
        """
            called when we receive a new goal
            the goal contains a desired radius and a success radius in which we check if the turn succeeded or not
            the message also contains if we should turn left or right
        """
        g = self.__server.accept_new_goal()
        self.spray_dist = g.distance
        self.spraytime = g.spray_time

        self.new_goal = True

    def on_timer(self, e):
        """
            called regularly by a ros timer
            in here we simply orders the vehicle to start moving either forward 
            or backwards depending on the distance sign, while monitoring the 
            travelled distance, if the distance is equal to or greater then this
            action succeeds.
        """
        if self.__server.is_active():
            if self.new_goal:
                self.new_goal = False
                self.n_sprays = 0
                self.__get_start_position()
            else:
                if self.__get_current_position():
                    if self.__get_distance() >= self.spray_dist:
                        self.do_spray()
                        self.__get_start_position()
                else:
                    self.__server.set_aborted(None, "could not locate")
                    rospy.logerr("Could not locate vehicle")
                    self.spray_msg.data = 0
                    self.spray_pub.publish(self.spray_msg)

    def __get_distance(self):
        return math.sqrt(
            math.pow(self.__cur_pos[0][0] - self.__start_pos[0][0], 2)
            + math.pow(self.__cur_pos[0][1] - self.__start_pos[0][1], 2)
        )

    def __get_start_position(self):
        ret = False
        try:
            self.__start_pos = self.__listen.lookupTransform(self.__odom_frame, self.__base_frame, rospy.Time(0))
            self.__start_yaw = tf.transformations.euler_from_quaternion(self.__start_pos[1])[2]
            ret = True
        except (tf.LookupException, tf.ConnectivityException), err:
            rospy.loginfo("could not locate vehicle")

        return ret

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]

#.........这里部分代码省略.........
        """
            Initialise the action server 
            @param name: the name of the action server to start
            @param rowtopic: the topic id of the row message to use for navigating
            @param odom_frame: the frame in which the robot is moving
            @param vehicle_frame: the frame id of the vehicles base
        """
        
        ## Setup some default values even though they are received via the goal
        self.desired_offset = 0.3
        self.max_out_of_headland_count = 10
        self.pgain = 0.2
        self.distance_scale = 0.1
        self.forward_velocity= 0.5
        
        
        # store the odometry frame id for use in distance calculation
        self.odom_frame = odom_frame
        self.vehicle_frame = vehicle_frame
        
        # reset counters used for detecting loss of rows and headland
        self.outofheadlandcount = 0
        self.left_valid_count = 0
        self.right_valid_count = 0
    
        self.__listen = TransformListener()
 
        self.cur_row = None
        
        self.vel_pub = rospy.Publisher("/fmControllers/cmd_vel_auto",Twist)
        
        self._action_name = name
        self._server = actionlib.SimpleActionServer(self._action_name,navigate_in_row_simpleAction,auto_start=False)
        self._server.register_goal_callback(self.goal_cb)
        self._server.register_preempt_callback(self.preempt_cb);
        
        self._subscriber = rospy.Subscriber(rowtopic, row, callback=self.row_msg_callback)
        self._last_row_msg = None
        
        self._server.start()
        
        
        
        self._timer = rospy.Timer(rospy.Duration(0.1),self.on_timer)
        
    def preempt_cb(self):
        """
            Called when the action client wishes to preempt us.
            Currently we just publish a velocity of zero in order to stop the vehicle.
        """
        rospy.loginfo("preempt received")
        self.__send_safe_velocity()
        self._server.set_preempted()
        
    
    def goal_cb(self):
        """
            Called when we receive a new goal.
            We could inspect the goal before accepting it,
            however for start we only accept it.
        """
        rospy.loginfo("goal received")
        
        g = self._server.accept_new_goal()
        
        self.desired_offset = g.desired_offset_from_row
        self.max_out_of_headland_count = g.headland_timeout
        self.pgain = g.P
        self.distance_scale = g.distance_scale
        self.forward_velocity= g.forward_velcoity
        self.outofheadlandcount = 0
        
        # reset start and end pose
        self.__start_pose = self.get_pose()
        self.__end_pose = None
        
        self.left_valid_count = self.right_valid_count = 0
        
        self.start_time = rospy.Time.now()
        
    def row_msg_callback(self,row):
        """
            Stores the row received
        """
        self.cur_row = row

        
    def get_pose(self):
        """
            returns the pose of the vehicle in the odometry frame
            returns none if the vehicle could not be located
        """
        cur_pos = None
        
        try:
            cur_pos = self.__listen.lookupTransform(self.odom_frame,self.vehicle_frame,rospy.Time(0))
        except (LookupException, ConnectivityException),err:
            rospy.loginfo("could not locate vehicle")
            
        return cur_pos

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import lookupTransform [as 别名]
class GripperMarkers:

    _offset = 0.09
    def __init__(self, side_prefix):
        self._ik_service = IK(side_prefix)

        r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
        self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
        rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
        self.r_traj_action_client.wait_for_server()

        l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
        self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
        rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
        self.l_traj_action_client.wait_for_server()

        self.r_joint_names = ['r_shoulder_pan_joint',
                              'r_shoulder_lift_joint',
                              'r_upper_arm_roll_joint',
                              'r_elbow_flex_joint',
                              'r_forearm_roll_joint',
                              'r_wrist_flex_joint',
                              'r_wrist_roll_joint']
        self.l_joint_names = ['l_shoulder_pan_joint',
                              'l_shoulder_lift_joint',
                              'l_upper_arm_roll_joint',
                              'l_elbow_flex_joint',
                              'l_forearm_roll_joint',
                              'l_wrist_flex_joint',
                              'l_wrist_roll_joint']

        rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb)
	
	self.side_prefix = side_prefix
        self._im_server = InteractiveMarkerServer("ik_request_markers_{}".format(side_prefix))
	self._tf_listener = TransformListener()
        self._menu_handler = MenuHandler()

        self._menu_handler.insert("Move {} arm here".format(side_prefix), callback=self.move_to_pose_cb)

        self.is_control_visible = False
        self.ee_pose = self.get_ee_pose()
        self.update_viz()
        self._menu_handler.apply(self._im_server, 'ik_target_marker')
        self._im_server.applyChanges()

    def get_ee_pose(self):
	from_frame = '/base_link'
	to_frame = '/' + self.side_prefix + '_wrist_roll_link'
	try:
	    t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
	    (pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
	except Exception as e:
	    rospy.logerr('Could not get end effector pose through TF.')
	    pos = [1.0, 0.0, 1.0]
	    rot = [0.0, 0.0, 0.0, 1.0]
	    import traceback
 	    traceback.print_exc()


	return Pose(Point(pos[0], pos[1], pos[2]),
			Quaternion(rot[0], rot[1], rot[2], rot[3]))

    def move_to_pose_cb(self, dummy):
        target_pose = GripperMarkers._offset_pose(self.ee_pose)
        ik_sol = self._ik_service.get_ik_for_ee(target_pose)
        self.move_to_joints(self.side_prefix, [ik_sol], 1.0)

    def marker_clicked_cb(self, feedback):
        if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
            self.ee_pose = feedback.pose
            self.update_viz()
            self._menu_handler.reApply(self._im_server)
            self._im_server.applyChanges()

        elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
            rospy.loginfo('Changing visibility of the pose controls.')
            if (self.is_control_visible):
                self.is_control_visible = False
            else:
                self.is_control_visible = True
        else:
            rospy.loginfo('Unhandled event: ' + str(feedback.event_type))

    def marker_cb(self, pose_markers):
        rospy.loginfo('AR Marker Pose updating')
        transform = GripperMarkers.get_matrix_from_pose(pose_markers.markers[0].pose.pose)
        offset_array = [0, 0, .03]
        offset_transform = tf.transformations.translation_matrix(offset_array)
        hand_transform = tf.transformations.concatenate_matrices(transform,
                                                             offset_transform)
        self.ee_pose = GripperMarkers.get_pose_from_transform(hand_transform)
        self.update_viz() 

    def update_viz(self):
        
        menu_control = InteractiveMarkerControl()
        menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
        menu_control.always_visible = True
        frame_id = 'base_link'
#.........这里部分代码省略.........