Python msg.Twist类代码示例

  def coordinate_to_action(self, msg):
    x = msg.x
    y = msg.y
    r = msg.z

    depth_proportion = -0.025
    depth_intercept = 1.35
    depth = r*depth_proportion + depth_intercept
    # print depth
    y_transform = self.frame_height/2 - y
    x_transform = x-self.frame_width/2
    angle_diff = x_transform
    print angle_diff

    if (angle_diff-10) < 0 and (angle_diff + 10) > 0:
      angle_diff = 0 

    if depth-.05<0.5 and depth+.05>0.5:
      depth = 0.5

    # print "x: ", x_transform
    # print "y: ",y
    # print "d: ", depth
    # print "a: ", angle_diff

    twist = Twist()

    lin_proportion = -(0.5-depth)*0.07
    twist.linear = Vector3(lin_proportion, 0, 0)

    turn_proportion = -0.0005*(angle_diff)

    twist.angular = Vector3(0, 0, turn_proportion)

    self.move_pub.publish(twist.linear, twist.angular)

	def notifyClientUpdated(self, topic):
		#rank the votes and convert to a numpy array
		linearCandidates = self.getLinearCandidates()
		angularCandidates = self.getAngularCandidates()

		linearRankings, angularRankings = rankings = self.calculateRankings(rospy.get_rostime())

		#sum the rankings of each candidate
		linearBordaCount = np.sum(linearRankings, axis=0)
		angularBordaCount = np.sum(angularRankings, axis=0)

		#highest ranked candidate
		bestLinearCandidateIndex = np.argmax(linearBordaCount)
		bestAngularCandidateIndex = np.argmax(angularBordaCount)
		bestLinearCandidate = linearCandidates[bestLinearCandidateIndex]
		bestAngularCandidate = angularCandidates[bestAngularCandidateIndex]

		#publish the best ranked action
		rospy.loginfo("aggregator realtime_weighted_borda_full: %s - %s",(bestLinearCandidate,bestAngularCandidate),self.weights)

		action=Twist()
		if bestLinearCandidate:
			action.linear = bestLinearCandidate.linear
		if bestAngularCandidate:
			action.angular = bestAngularCandidate.angular
		self.publish(action)

def goalPub():
	global currentGoal
	goalCount = 0

	goalinworld = rospy.Publisher('robot_2/goal',Twist,queue_size=10)
	goal = Twist()
	goal.linear = Vector3(currentGoal.x,currentGoal.y,0.0)
	goal.angular = Vector3(0.0,0.0,0.0)

	rate = rospy.Rate(10.0)
	while not rospy.is_shutdown():
		result = goal2_client()
		if result.robot2_thereOrNot == 1:
			goalCount = goalCount + 1

		if goalCount>=1:
			currentGoal.x = -13.0
			currentGoal.y = -10.0
		else:
			currentGoal.x = -13.0
			currentGoal.y = -5.0

		goal.linear = Vector3(currentGoal.x,currentGoal.y,0.0)
		goal.angular = Vector3(0.0,0.0,0.0)

		rospy.loginfo(goalCount)
		rospy.loginfo(goal.linear)
		goalinworld.publish(goal)
		rate.sleep()

  def coordinate_to_action(self, msg):
    x = msg.x
    y = msg.y
    r = msg.z

    depth_proportion = -0.025
    depth_intercept = 1.35
    depth = r*depth_proportion + depth_intercept
    # print depth
    y_transform = self.frame_height/2 - y
    x_transform = x-self.frame_width/2
    angle_diff = math.tan(x_transform/depth)
    print "x: ", x_transform
    print "y: ",y
    print "d: ", depth
    print "a: ", angle_diff

    twist = Twist()

    lin_proportion = 0#-(0.5-depth)*0.1
    twist.linear = Vector3(lin_proportion, 0, 0)

    turn_proportion = 0*(angle_diff)

    twist.angular = Vector3(0, 0, turn_proportion)

    self.move_pub.publish(twist.linear, twist.angular)

    def move_base(self, x, y, z):
        print "MOVING"

        twist_msg = Twist()
        twist_msg.linear = Vector3(x, 0.0, 0.0)
        twist_msg.angular = Vector3(0.0, 0.0, z)
        self.cmd_vel_pub.publish(twist_msg)

 def center_flag(self, flag):
     if not self.is_centered(flag):
         print "gotta align"
         print "angle is currently " + str(flag.angle)
         msg = Twist()
         msg.linear = Vector3(0, 0, 0)
         msg.angular = Vector3(0, 0, flag.angle)
         self.velocity_publisher.publish(msg)

	def drive(self, lin_vel, ang_vel):
		"""Publishes specified linear and angular command velocities"""
		# Note: for the Neato platforms, only x-linear and z-rotational motion is possible
		twist = Twist()

		twist.linear = Vector3(lin_vel,0,0)
		twist.angular = Vector3(0,0,ang_vel)
		self.pub.publish(twist.linear, twist.angular)

def stop_uav(pub):
   """
   Tells the UAV to hold at its current position.
   """
   zero = Vector3(0,0,0)
   msg = Twist()
   msg.linear = zero
   msg.angular = zero
   pub.publish(msg)

    def base_action(self, x, y, z, theta_x, theta_y, theta_z):
        topic_name = '/base_controller/command'
        base_publisher = rospy.Publisher(topic_name, Twist)

        twist_msg = Twist()
        twist_msg.linear = Vector3(x, y, z)
        twist_msg.angular = Vector3(theta_x, theta_y, theta_z)
        
        base_publisher.publish(twist_msg)

    def do_GET(self):
        global publisher
        query_string = urlparse.urlparse(self.path).query
        parameters = urlparse.parse_qs(query_string)

        if 'type' not in parameters:
            try:
                if self.path == "/":
                    self.path = "/index.html"
                elif self.path == "favicon.ico":
                    return
                elif self.path == "map.gif":
                    # Draw robot position on map and send image back
                    draw_map()
                    return
                fname,ext = os.path.splitext(self.path)
                print "Loading file", self.path
                with open(os.path.join(os.getcwd(),self.path[1:])) as f:
                    self.send_response(200)
                    self.send_header('Content-type', types_map[ext])
                    self.end_headers()
                    self.wfile.write(f.read())
                return
            except IOError:
                self.send_error(404)
                return

        command_type = parameters['type'][0]
        
        if command_type == 'base':
            base_x = 0.0
            if 'x' in parameters:
                base_x = float(parameters['x'][0])
            base_y = 0.0
            if 'y' in parameters:
                base_y = float(parameters['y'][0])
            base_z = 0.0
            if 'z' in parameters:
                base_z = float(parameters['z'][0])
            twist_msg = Twist()
            twist_msg.linear = Vector3(base_x, base_y, 0.0)
            twist_msg.angular = Vector3(0.0, 0.0, base_z)
            mobile_base_velocity.publish(twist_msg)

        elif command_type == 'speak':
            text = parameters['say'][0]
            sr = SoundRequest()
            sr.sound = -3 #Say text
            sr.command = 1 #Play once
            sr.arg = text
            publisher.publish(sr)
            #os.system("espeak -s 155 -a 200 '" + text + "' ")


        # response
        self.send_response(204)
        return

    def default(self, ci='unused'):
        twist = Twist()
        twist.header = self.get_ros_header()

        # Fill twist-msg with the values from the sensor
        twist.linear.x = self.data['velocity'][0]
        twist.linear.y = self.data['velocity'][1]
        twist.linear.z = self.data['velocity'][2]

        self.publish(twist)

 def send_cmd_msg(self):
     msg = Twist()
     msg.linear = Vector3()
     msg.angular = Vector3()
     msg.linear.x = self.x
     msg.linear.y = 0
     msg.linear.z = 0
     msg.angular.x = 0
     msg.angular.y = 0
     msg.angular.z = self.omega
     self.publisher.publish(msg)        

 def send_cmd_msg(self, speed, omega):
     #print('receieved odometry')
     msg = Twist()
     msg.linear = Vector3()
     msg.angular = Vector3()
     msg.linear.x = speed
     msg.linear.y = 0
     msg.linear.z = 0
     msg.angular.x = 0
     msg.angular.y = 0
     msg.angular.z = omega
     self.publisher.publish(msg)

def stop_robot():
    import time
    global cmd_vel_pub
    global logger
    logger.debug("Posting 0 cmd_vel data ....")
    for _ in range(100):
	rospy.sleep(0.01)
	twist_msg = Twist()
	twist_msg.linear = Vector3(0,0,0)
	twist_msg.angular = Vector3(0,0,0)
	cmd_vel_pub.publish(twist_msg)
    logger.debug("Finished posting 0 cmd_vel data ...\n")

	def get_motion(self):
		t = Twist()
		buf = StringIO()
		self.motion.serialize(buf)
		t.deserialize(buf.getvalue())
		if DONT_BUMP:
			if any(self.impact):
				self.backingsince = time.time()
				self.motion.linear.x = 0
			if self.backingsince is not None:
				if time.time() < self.backingsince + BUMP_REV_TIME:
					t.linear.x = -BUMP_REV_SPEED
				else:
					self.backingsince = None
		return t