python类Twist()的实例源码

def drive(gest):

        if gest.data == 1: #FIST
        turtlesimPub.publish("go back")
        tsPub.publish(Twist(Vector3(-1.0, 0, 0), Vector3(0, 0, 0)))
        elif gest.data == 2 and armState == 1: #WAVE_IN, RIGHT arm
        turtlesimPub.publish("go left")
        tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, 1.0)))
        elif gest.data == 2 and armState == 2: #WAVE_IN, LEFT arm
        turtlesimPub.publish("go right")
        tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, -1.0)))
        elif gest.data == 3 and armState == 1: #WAVE_OUT, RIGHT arm
        turtlesimPub.publish("go right")
        tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, -1.0)))
        elif gest.data == 3 and armState == 2: #WAVE_OUT, LEFT arm
        turtlesimPub.publish("go left")
        tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, 1.0)))
        elif gest.data == 4: #FINGERS_SPREAD
        turtlesimPub.publish("go forward")
        tsPub.publish(Twist(Vector3(1.0, 0, 0), Vector3(0, 0, 0)))

def init(self):
        pygame.init()
        clock = pygame.time.Clock()
        screen = pygame.display.set_mode((250, 250))

        rospy.init_node('highway_teleop')
        self.rate = rospy.Rate(rospy.get_param('~hz', 10)) 
        self.acc = rospy.get_param('~acc', 5)
        self.yaw = rospy.get_param('~yaw', 0)

        self.robot_pub = rospy.Publisher('robot_0/cmd_vel', Twist, queue_size=1)

        print "Usage: \n \
                up arrow: accelerate \n \
                down arrow: decelerate \n \
                left arrow: turn left \n \
                right arrow: turn right"

def compute_control_actions():
    global i
    controller.compute_control_actions(current_pose, current_twist, i)
    data_container['t'].append(i * DELTA_T)
    data_container['x'].append(current_pose.position.x)
    data_container['x_ref'].append(controller.x_ref_n)
    data_container['y'].append(current_pose.position.y)
    data_container['y_ref'].append(controller.y_ref_n)
    data_container['theta'].append(controller.theta_n)
    data_container['theta_ref'].append(controller.theta_ref_n)
    data_container['v_c'].append(controller.v_c_n)
    data_container['w_c'].append(controller.w_c_n)
    data_container['zeros'].append(0)

    twist = Twist()
    twist.linear.x = controller.v_c_n
    twist.angular.z = controller.w_c_n
    twist_publisher.publish(twist)

    i += 1

def start():
    global left_motor_pub,right_motor_pub
    # publishing to "turtle1/cmd_vel" to control turtle1
    global pub
    pub = rospy.Publisher('turtle1/cmd_vel', Twist)
    left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
    right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
    # subscribed to joystick inputs on topic "joy"
    rospy.Subscriber("joy", Joy, callback)
    # starts the node
    rospy.init_node('Joy2Turtle')
    rospy.spin()

def start():
    global left_motor_pub,right_motor_pub
    # publishing to "turtle1/cmd_vel" to control turtle1
    global pub
    pub = rospy.Publisher('turtle1/cmd_vel', Twist)
    left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
    right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
    # subscribed to joystick inputs on topic "joy"
    rospy.Subscriber("joy", Joy, callback)
    # starts the node
    rospy.init_node('Joy2Turtle')
    rospy.spin()

def contr(keynumber):
    # turtlesim??topic
    pub = rospy.Publisher('~cmd_vel', Twist, queue_size=5)
    countnum = 0
    if keynumber == 3:
        while(1):
            twist = Twist()
            twist.linear.x = 0.2
            twist.linear.y = 0
            twist.linear.z = 0
            twist.angular.x = 0
            twist.angular.y = 0
            twist.angular.z = 0.14
            pub.publish(twist)
            countnum += 1

            if countnum > 100000:
                countnum = 0
                exit(0)

def __init__(self):
        """Constructor for the class
        initialize topic subscription and 
        instance variables
        """
        self.r = rospy.Rate(5)
        self.publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        rospy.Subscriber('/scan', LaserScan, self.process_scan)
        # user chooses which parking mode to be performed
        self.is_parallel = rospy.get_param('~parallel', False)

        # Instance Variables
        self.timestamp1 = None
        self.timestamp2 = None
        self.dist2Neato = None
        self.dist2Wall = None
        self.widthOfSpot = None
        self.twist = None
        self.radius = None
        # Adjusment to be made before moving along the arc
        self.adjustment = 0 
        self.isAligned = False

def __init__(self):
        """ Initialize the parking spot recognizer """

        #Subscribe to topics of interest
        rospy.Subscriber("/camera/image_raw", Image, self.process_image)
        rospy.Subscriber('/cmd_vel', Twist, self.process_twist)

        # Initialize video images
        cv2.namedWindow('video_window')        
        self.cv_image = None # the latest image from the camera
        self.dst =  np.zeros(IMG_SHAPE, np.uint8)
        self.arc_image = np.zeros(IMG_SHAPE, np.uint8)
        self.transformed = np.zeros(IMG_SHAPE, np.uint8)

        # Declare instance variables
        self.bridge = CvBridge() # used to convert ROS messages to OpenCV
        self.hsv_lb = np.array([0, 70, 60]) # hsv lower bound to filter for parking lines
        self.hsv_ub = np.array([30, 255, 140]) # hsv upper bound
        self.vel = None
        self.omega = None
        self.color = (0,0,255)

def __init__(self):
        rospy.loginfo('[robot_move_pkg]->linear_move is initial')
        #???????????????
        self.robot_state = robot_state.robot_position_state()
        self.cmd_move_pub = rospy.Publisher('/cmd_move', g_msgs.Twist, queue_size = 100)
        self.rate = rospy.Rate(150)
        #???????????
        self.stop_tolerance = config.high_speed_stop_tolerance
        self.angular_tolerance = config.high_turn_angular_stop_tolerance
        #?????????????
        self.accurate_turn_an_angular = turn_an_angular.turn_an_angular()
        self.x_speed = 0.0
        self.y_speed = 0.0
        self.w_speed = config.high_turn_angular_speed
        #???????
        self.linear_sp = spline_func.growth_curve()
        self.amend_speed = 0.12

def __init__(self):
        #?????????? m/s
        self.move_cmd_pub = rospy.Publisher('cmd_move_robot',g_msgs.Twist,queue_size=100)

        self.move_speed = config.go_along_circle_speed
        self.get_position = robot_state.robot_position_state()
        #????????? rad
        self.stop_tolerance = config.go_along_circle_angular_tolerance
        #????????
        rospy.on_shutdown(self.brake)
        # ??sleep ??? ???????????
        self.rate = 100.0
        self.R = rospy.Rate(int(self.rate))
        self.MODE = { 1:(-1, 1),
                      2:( 1,-1),
                      3:( 1, 1),
                      4:(-1,-1)}

def go_to(self, radius, angular, mode):
        # ??????????
        symbol_y,symbol_w = self.MODE[mode]
        ang_has_moved = 0.0
        self.reach_goal = False
        move_vel = g_msgs.Twist()
        start_yaw = self.get_position.get_robot_current_w()
        while not rospy.is_shutdown() and self.reach_goal != True:
            current_yaw = self.get_position.get_robot_current_w()
            ang_has_moved += abs(abs(current_yaw) - abs(start_yaw))
            start_yaw = current_yaw
            if abs(ang_has_moved - abs(angular)) <= self.stop_tolerance:
                self.reach_goal = True
                break
            move_vel.linear.y = self.move_speed*symbol_y
            # ???????, ???? dw*dt = dt*atan2(dv*dt/2.0, radius)
            move_vel.angular.z = self.rate*atan2(self.move_speed/self.rate,radius)*symbol_w
            print ang_has_moved
            self.move_cmd_pub.publish(move_vel)
            self.R.sleep()
        self.brake()

        print ang_has_moved

def correct_angle(self):
        # rospy.loginfo("sadasdafasf")
        (x,theta,if_close_line) = self.close_line_cmd.find_line()
        r = rospy.Rate(50)
        move_velocity = g_msgs.Twist()
        if theta > 0:
            move_velocity.angular.z = -0.10
        else:
            move_velocity.angular.z = 0.10
        while not rospy.is_shutdown():
            (x,theta,if_close_line) = self.close_line_cmd.find_line()
            self.cmd_move_pub.publish(move_velocity)
            #????????????????

            if theta < 0.02 and theta > -0.02:
                rospy.loginfo("will Stop!!!!!!!!!!")
                self.brake()
                break
            r.sleep()


    #??3??????

def __init__(self):
        rospy.loginfo('[robot_move_pkg]->linear_move is initial')
        #???????????????
        self.robot_state = robot_state.robot_position_state()
        self.cmd_move_pub = rospy.Publisher('/cmd_move', g_msgs.Twist, queue_size = 100)
        self.rate = rospy.Rate(150)
        #???????????
        self.stop_tolerance = config.high_speed_stop_tolerance
        self.angular_tolerance = config.high_turn_angular_stop_tolerance
        #?????????????
        self.accurate_turn_an_angular = turn_an_angular.turn_an_angular()
        self.x_speed = 0.0
        self.y_speed = 0.0
        self.w_speed = config.high_turn_angular_speed
        #???????
        self.linear_sp = spline_func.growth_curve()
        self.amend_speed = 0.18

def turn(self, goal_angular):
        # ???????????????,??????????????
        # ????,????
        rospy.loginfo('[robot_move_pkg]->move_in_robot.turn will turn %s'%goal_angular)
        current_angular = start_angular = self.robot_state.get_robot_current_w()#??????????

        r = rospy.Rate(100)
        delta_angular = current_angular - start_angular
        move_velocity = g_msgs.Twist()
        while not rospy.is_shutdown() :
            if abs(delta_angular - abs(goal_angular)) < self.turn_move_stop_tolerance:
                break
            current_angular = self.robot_state.get_robot_current_w()
            move_velocity.angular.z = math.copysign(self.w_speed, goal_angular)
            delta_angular += abs(abs(current_angular) - abs(start_angular) )
            start_angular = current_angular
            self.cmd_move_pub.publish(move_velocity) #???????????
            r.sleep()
        self.brake()

def turn_to(self,goal_angular):
        rospy.on_shutdown(self.brake) #???????????
        current_angular = start_angular = self.robot_state.get_robot_current_w()#??????????
        is_arrive_goal = False
        r = rospy.Rate(100)
        delta_angular = current_angular - start_angular
        delta_upper_limit = abs(goal_angular) + 0.02 #????
        delta_lower_limit = abs(goal_angular) - 0.02 #????
        move_velocity = g_msgs.Twist()
        while not rospy.is_shutdown() and not is_arrive_goal:
            if abs(delta_angular)<=delta_upper_limit and abs(delta_angular) >= delta_lower_limit: #????
                self.brake()
                is_arrive_goal = True
                break
            current_angular = self.robot_state.get_robot_current_w()
            if goal_angular > 0:
                move_velocity.angular.z = 0.1
            else:
                move_velocity.angular.z = -0.1
            delta_angular += abs(abs(current_angular) - abs(start_angular) )
            start_angular = current_angular
            self.cmd_move_pub.publish(move_velocity) #???????????
            r.sleep()
        self.brake()

def __init__(self):
        self.find_ball_client = rospy.ServiceProxy('volleyball_data',volleyballdata)
        self.cmd_vel_pub = rospy.Publisher('cmd_move_robot' , g_msgs.Twist , queue_size=100)
        self.cmd_position = get_robot_position.robot_position_state()
        self.move_speed = 0.21
        #????????
        rospy.on_shutdown(self.brake)
        #????????????
        self.MODE = { 1:(-1, 1),
                      2:( 1,-1),
                      3:( 1, 1),
                      4:(-1,-1)}
        rospy.loginfo('waiting for the find ball..')
        self.find_ball_client.wait_for_service()
        rospy.loginfo('connect to the find ball!!!')

    #??????????????????

def __init__(self):
        self.base_pub = rospy.Publisher("/base_controller/command", Twist,
                                        queue_size=1)
        token = rospy.get_param('/telegram/token', None)

        # Create the Updater and pass it your bot's token.
        updater = Updater(token)

        # Add command and error handlers
        updater.dispatcher.add_handler(CommandHandler('start', self.start))
        updater.dispatcher.add_handler(CommandHandler('help', self.help))
        updater.dispatcher.add_handler(MessageHandler(Filters.text, self.echo))
        updater.dispatcher.add_error_handler(self.error)

        # Start the Bot
        updater.start_polling()

def drive(gest):
        global movingState
        global zero
        global speed
        if gest.data == 1: #FIST
            movingState -= 1
        elif gest.data == 4 or gest.data == 2: #FINGERS_SPREAD
            movingState += 1
        elif gest.data == 3 :
            zero = y

        if movingState > 0 :
            movingState = 1
            turtlesimPub.publish("go forward")
            speed = 1
#           tsPub.publish(Twist(Vector3(1.0, 0, 0), Vector3(0, 0, 0)))
        elif movingState < 0 :
            movingState = -1
            turtlesimPub.publish("go back")
            speed = -1
#           tsPub.publish(Twist(Vector3(-1.0, 0, 0), Vector3(0, 0, 0)))
        else :
            speed = 0
        print (speed)

def strength(emgArr1):
        emgArr=emgArr1.data
        # Define proportional control constant:
        K = 0.005
        # Get the average muscle activation of the left, right, and all sides
        aveRight=(emgArr[0]+emgArr[1]+emgArr[2]+emgArr[3])/4
        aveLeft=(emgArr[4]+emgArr[5]+emgArr[6]+emgArr[7])/4
        ave=(aveLeft+aveRight)/2
        # If all muscles activated, drive forward exponentially
        if ave > 500:
            tsPub.publish(Twist(Vector3(0.1*math.exp(K*ave),0,0),Vector3(0,0,0)))
        # If only left muscles activated, rotate proportionally
        elif aveLeft > (aveRight + 200):
            tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,K*ave)))
        # If only right muscles activated, rotate proportionally
        elif aveRight > (aveLeft + 200):
            tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,-K*ave)))
        # If not very activated, don't move (high-pass filter)
        else:
            tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,0)))

def strength(emgArr1):
    emgArr=emgArr1.data
    # Define proportional control constant:
    K = 0.005
    # Get the average muscle activation of the left, right, and all sides
    aveRight=(emgArr[0]+emgArr[1]+emgArr[2]+emgArr[3])/4
    aveLeft=(emgArr[4]+emgArr[5]+emgArr[6]+emgArr[7])/4
    ave=(aveLeft+aveRight)/2
    # If all muscles activated, drive forward exponentially
    if ave > 500:
        tsPub.publish(Twist(Vector3(0.1*math.exp(K*ave),0,0),Vector3(0,0,0)))
    # If only left muscles activated, rotate proportionally
    elif aveLeft > (aveRight + 200):
        tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,K*ave)))
    # If only right muscles activated, rotate proportionally
    elif aveRight > (aveLeft + 200):
        tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,-K*ave)))    
    # If not very activated, don't move (high-pass filter)    
    else: 
        tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,0)))

def __init__(self):
        # setup
        CozmoTeleop.settings = termios.tcgetattr(sys.stdin)
        atexit.register(self.reset_terminal)

        # vars
        self.head_angle = STD_HEAD_ANGLE
        self.lift_height = STD_LIFT_HEIGHT

        # params
        self.lin_vel = rospy.get_param('~lin_vel', 0.2)
        self.ang_vel = rospy.get_param('~ang_vel', 1.5757)

        # pubs
        self._head_pub = rospy.Publisher('head_angle', Float64, queue_size=1)
        self._lift_pub = rospy.Publisher('lift_height', Float64, queue_size=1)
        self._cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)

def run(self):
        if self.finished:
            return TaskStatus.SUCCESS
        else:
            rospy.loginfo('Vacuuming the floor in the ' + str(self.room))

            while self.counter > 0:
                self.cmd_vel_pub.publish(self.cmd_vel_msg)
                self.cmd_vel_msg.linear.x *= -1
                rospy.loginfo(self.counter)
                self.counter -= 1
                rospy.sleep(1)
                return TaskStatus.RUNNING

            self.finished = True
            self.cmd_vel_pub.publish(Twist())
            message = "Finished vacuuming the " + str(self.room) + "!"
            rospy.loginfo(message)

def __init__(self):

      """ROS Subscriptions """
      self.image_sub = rospy.Subscriber("/raspicam_node/image/image_raw",Image,self.cvt_image) 
      self.image_pub = rospy.Publisher("/image_converter/output_video",Image, queue_size=10)
      self.cmdVel_pub = rospy.Publisher("/platform_control/cmd_vel", Twist, queue_size=10)
      self.cmdVelStamped_pub = rospy.Publisher('/platform_control/cmd_vel_stamped', TwistStamped, queue_size=10)

      """ Variables """
      self.model_path = 'home/wil/ros/catkin_ws/src/diy_driverless_car_ROS/rover_ml/behavior_cloning/src/behavior_cloning/model.h5'
      self.cmdvel = Twist()
      self.baseVelocity = TwistStamped()
      self.bridge = CvBridge()
      self.latestImage = None
      self.outputImage = None
      self.resized_image = None
      self.imgRcvd = False

def cmdVel_publish(self, cmdVelocity):

         # Publish Twist
         self.cmdVel_pub.publish(cmdVelocity)

         # Publish TwistStamped 
         self.baseVelocity.twist = cmdVelocity

         baseVelocity = TwistStamped()

         baseVelocity.twist = cmdVelocity

         now = rospy.get_rostime()
         baseVelocity.header.stamp.secs = now.secs
         baseVelocity.header.stamp.nsecs = now.nsecs
         self.cmdVelStamped_pub.publish(baseVelocity)

def __init__(self):
        rospy.init_node('localizer')
        rospy.Subscriber('/odom', Odometry, self.process_odom)

        self.sound_speed = 340.29*100 # cm/s
        self.mic_dist = 30 # cm
        self.buffer = 200
        self.angles = []

        # angle odometry
        self.angle_curr = 0.0
        self.angle_k = 1
        self.angle_error = None
        self.at_speaker = False
        self.angle_pred = 0.0

        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

def __init__(self):
        self.bridge = cv_bridge.CvBridge()
        cv2.namedWindow("input", 1)
        cv2.createTrackbar('param1', 'input', 10, 300, nothing)
        cv2.createTrackbar('param2', 'input', 15, 300, nothing)
        cv2.namedWindow("processed", 1)
        self.image_sb = rospy.Subscriber('/usb_cam/image_raw', Image, self.image_callback)

        self.motion = Twist()

        rate = rospy.Rate(20)

        # publish to cmd_vel of the jackal
        pub = rospy.Publisher("/jackal_velocity_controller/cmd_vel", Twist, queue_size=10)

        while not rospy.is_shutdown():
            # publish Twist
            pub.publish(self.motion)
            rate.sleep()

def __init__(self):
        self.petrone = Petrone()

        # subscriber
        self.sub_flight = rospy.Subscriber('cmd_fly', Int8, self.cb_fly, queue_size=1)
        self.sub_cmd = rospy.Subscriber('cmd_vel_raw', Twist, self.cb_cmd, queue_size=1)
        self.sub_color = rospy.Subscriber('led_color', String, self.cb_color, queue_size=1)

        # publisher
        self.pub_battery = rospy.Publisher('battery', Float32, queue_size=1)
        self.pub_status_flight = rospy.Publisher('status/flight', Int8, queue_size=1)
        self.pub_status_system = rospy.Publisher('status/system', Int8, queue_size=1)
        self.pub_imu = rospy.Publisher('imu', Imu, queue_size=1)

        # cache
        self.is_disconnected = True
        self.last_values = defaultdict(lambda: 0)

        # flight parameters
        self.twist = Twist()
        self.twist_at = 0

def __init__(self):
        self.pub_vel_wheel = rospy.Publisher('/bycycle_interaction/vel_wheel', Twist, queue_size=1)

        self.twist = Twist()
        self.vel_wheel = 0
        self.angle_wheel = 0

        self.rate = rospy.get_param('~rate', 3.0)
        self.wheel_radius = rospy.get_param('~wheel_radius', 0.30)

        self.srv = Server(bicycle_interactionConfig, self.reconfig_callback) # define dynamic_reconfigure callback

        rate = rospy.Rate(self.rate)

        while not rospy.is_shutdown():
            self.publish_vel_wheel()
            rate.sleep()

def tj_callback(data):

    # start publisher of cmd_vel to control Turtlesim
    pub = rospy.Publisher("turtle1/cmd_vel", Twist, queue_size=1)

    # Create Twist message & add linear x and angular z from left joystick
    twist = Twist()
    twist.linear.x = data.axes[1]
    twist.angular.z = data.axes[0]

    # record values to log file and screen
    rospy.loginfo("twist.linear: %f ; angular %f", twist.linear.x, twist.angular.z)

    # process joystick buttons
    if data.buttons[0] == 1:        # green button on xbox controller
        move_circle()

    # publish cmd_vel move command to Turtlesim
    pub.publish(twist)

def move_circle():

    # Create a publisher which can "talk" to Turtlesim and tell it to move
    pub = rospy.Publisher('turtle1/cmd_vel', Twist, queue_size=1)

    # Create a Twist message and add linear x and angular z values
    move_cmd = Twist()
    move_cmd.linear.x = 1.0
    move_cmd.angular.z = 1.0

    # Save current time and set publish rate at 10 Hz
    now = rospy.Time.now()
    rate = rospy.Rate(10)

    # For the next 6 seconds publish cmd_vel move commands to Turtlesim
    while rospy.Time.now() < now + rospy.Duration.from_sec(6):
        pub.publish(move_cmd)
        rate.sleep()