Python Twist.angular方法代码示例

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
def reverse_robot():
    print "Reversing Robot\n"
    import time

    global vel_lin_buffer, vel_ang_buffer, reversing
    global cmd_vel_pub, restored_bump_pub
    reversing = True
    for l, a in zip(reversed(vel_lin_buffer), reversed(vel_ang_buffer)):
        lin_vec = Vector3(-l[0], -l[1], -l[2])
        ang_vec = Vector3(-a[0], -a[1], -a[2])
        time.sleep(0.1)
        twist_msg = Twist()
        twist_msg.linear = lin_vec
        twist_msg.angular = ang_vec
        cmd_vel_pub.publish(twist_msg)

    for _ in range(10):
        time.sleep(0.05)
        twist_msg = Twist()
        twist_msg.linear = Vector3(0, 0, 0)
        twist_msg.angular = Vector3(0, 0, 0)

        cmd_vel_pub.publish(twist_msg)
    reversing = False
    restored_bump_pub.publish(True)

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
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()

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
    def _parse_joy(self, joy=None):
        if self._msg_type == 'twist':
            cmd = Twist()
        else:
            cmd = Accel()
        if joy is not None:
            # Linear velocities:
            l = Vector3(0, 0, 0)

            if self._axes['x'] > -1 and abs(joy.axes[self._axes['x']]) > self._deadzone:
                l.x += self._axes_gain['x'] * joy.axes[self._axes['x']]

            if self._axes['y'] > -1 and abs(joy.axes[self._axes['y']]) > self._deadzone:
                l.y += self._axes_gain['y'] * joy.axes[self._axes['y']]

            if self._axes['z'] > -1 and abs(joy.axes[self._axes['z']]) > self._deadzone:
                l.z += self._axes_gain['z'] * joy.axes[self._axes['z']]

            if self._axes['xfast'] > -1 and abs(joy.axes[self._axes['xfast']]) > self._deadzone:
                l.x += self._axes_gain['xfast'] * joy.axes[self._axes['xfast']]

            if self._axes['yfast'] > -1 and abs(joy.axes[self._axes['yfast']]) > self._deadzone:
                l.y += self._axes_gain['yfast'] * joy.axes[self._axes['yfast']]

            if self._axes['zfast'] > -1 and abs(joy.axes[self._axes['zfast']]) > self._deadzone:
                l.z += self._axes_gain['zfast'] * joy.axes[self._axes['zfast']]

            # Angular velocities:
            a = Vector3(0, 0, 0)

            if self._axes['roll'] > -1 and abs(joy.axes[self._axes['roll']]) > self._deadzone:
                a.x += self._axes_gain['roll'] * joy.axes[self._axes['roll']]

            if self._axes['rollfast'] > -1 and abs(joy.axes[self._axes['rollfast']]) > self._deadzone:
                a.x += self._axes_gain['rollfast'] * joy.axes[self._axes['rollfast']]

            if self._axes['pitch'] > -1 and abs(joy.axes[self._axes['pitch']]) > self._deadzone:
                a.y += self._axes_gain['pitch'] * joy.axes[self._axes['pitch']]

            if self._axes['pitchfast'] > -1 and abs(joy.axes[self._axes['pitchfast']]) > self._deadzone:
                a.y += self._axes_gain['pitchfast'] * joy.axes[self._axes['pitchfast']]

            if self._axes['yaw'] > -1 and abs(joy.axes[self._axes['yaw']]) > self._deadzone:
                a.z += self._axes_gain['yaw'] * joy.axes[self._axes['yaw']]

            if self._axes['yawfast'] > -1 and abs(joy.axes[self._axes['yawfast']]) > self._deadzone:
                a.z += self._axes_gain['yawfast'] * joy.axes[self._axes['yawfast']]

            cmd.linear = l
            cmd.angular = a
        else:
            cmd.linear = Vector3(0, 0, 0)
            cmd.angular = Vector3(0, 0, 0)
        return cmd

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
  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)

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
	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)

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
  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)

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
    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)

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
 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)

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
	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)

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
def reverse_robot():
    global logger
    logger.info("Reversing Robot ...\n")
    import time
    global vel_lin_buffer,vel_ang_buffer,reversing
    global cmd_vel_pub,restored_bump_pub
    global curr_cam_data,save_pose_data,image_dir,save_img_seq
    reversing = True
    
    logger.debug("Posting cmd_vel messages backwards with a %.2f delay",utils.REVERSE_PUBLISH_DELAY)
    for l,a in zip(reversed(vel_lin_buffer),reversed(vel_ang_buffer)):
        lin_vec = Vector3(-l[0],-l[1],-l[2])
        ang_vec = Vector3(-a[0],-a[1],-a[2])
        twist_msg = Twist()
        twist_msg.linear = lin_vec
        twist_msg.angular = ang_vec
	rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
        cmd_vel_pub.publish(twist_msg)
    # publish last twist message so the robot reverse a bit more
    for _ in range(5):
	rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
	cmd_vel_pub.publish(twist_msg)

    logger.debug("Finished posting cmd_vel messages backwards ...\n")
    rospy.sleep(0.5)
    logger.debug("Posting zero cmd_vel messages with %.2f delay",utils.ZERO_VEL_PUBLISH_DELAY)
    for _ in range(100):
        rospy.sleep(utils.ZERO_VEL_PUBLISH_DELAY)
        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 zero cmd_vel messages ...\n")
    reversing = False
    restored_bump_pub.publish(True)
    
    if image_dir is not None:
        if save_img_seq:
            pickle.dump(curr_cam_data,open(image_dir+os.sep+'images_'+str(episode)+'.pkl','wb'))
            curr_cam_data=[]
        pickle.dump(save_pose_data,open(image_dir+os.sep+'pose_'+str(episode)+'.pkl','wb'))
        save_pose_data=[]

    logger.info("Reverse finished ...\n")

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
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)

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
    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)

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
    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

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
def reverse_robot():
    global logger
    logger.info("Reversing Robot ...\n")
    import time
    global vel_lin_buffer,vel_ang_buffer,reversing
    global cmd_vel_pub,restored_bump_pub
    global curr_cam_data,save_pose_data,image_dir,save_img_seq
    reversing = True
    
    logger.debug("Posting cmd_vel messages backwards with a %.2f delay",utils.REVERSE_PUBLISH_DELAY)
    for l,a in zip(reversed(vel_lin_buffer),reversed(vel_ang_buffer)):
        lin_vec = Vector3(-l[0],-l[1],-l[2])
        ang_vec = Vector3(-a[0],-a[1],-a[2])
        twist_msg = Twist()
        twist_msg.linear = lin_vec
        twist_msg.angular = ang_vec
	rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
        cmd_vel_pub.publish(twist_msg)
    # publish last twist message so the robot reverse a bit more
    for _ in range(5):
	rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
	cmd_vel_pub.publish(twist_msg)

    logger.debug("Finished posting cmd_vel messages backwards ...\n")
    rospy.sleep(0.5)
    logger.debug("Posting zero cmd_vel messages with %.2f delay",utils.ZERO_VEL_PUBLISH_DELAY)
    for _ in range(100):
        rospy.sleep(utils.ZERO_VEL_PUBLISH_DELAY)
        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 zero cmd_vel messages ...\n")
    reversing = False
    restored_bump_pub.publish(True)

    logger.info("Reverse finished ...\n")
    
    #save_img_seq_thread = threading.Thread(target=save_img_sequence_pose_threading())
    #save_img_seq_thread.start()
    save_img_sequence_pose()
    logger.info('Saving images finished...')

# 需要导入模块: from geometry_msgs.msg import Twist [as 别名]
# 或者: from geometry_msgs.msg.Twist import angular [as 别名]
def gui_bridge():	
	#setup ROS node
	rospy.init_node('gui_bridge')
	pub = rospy.Publisher('/golfcart_pilot/abs_cmd', Twist)
	
	#setup socket
	HOST = ''
	PORT = 50012
	s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
	s.bind((HOST, PORT))
	print 'Waiting for connection on port ', PORT
	s.listen(1)
	conn, addr = s.accept()
	print 'Connected by', addr
	
	rospy.sleep(0.5)	
	
	while not rospy.is_shutdown():
		try:
			data = conn.recv(1024)
			if data.count("{") > 1:
				data = data.split("}{")
				data = "{"+data[len(data)-1]
			guiCmd = eval(data)
			print guiCmd
			wheel_angle = float(guiCmd['wheel']) * 0.0174532925 # d2r
			speed = float(guiCmd['speed'])
		
			command = Twist()
			command.linear = Vector3 (speed,0,0)
			command.angular = Vector3 (0,wheel_angle,0)
			pub.publish(command)
			rospy.sleep(0.2) # 5Hz
		except socket.error:
			command = Twist()
			command.linear = Vector3 (0,0,0)
			command.angular = Vector3 (0,0,0)
			pub.publish(command)
			print 'gui_bridge: SOCKET ERROR OCCURED, STOPPING DRIVE OPERATIONS'
			break
			
	s.close()