python类Twist()的实例源码

def keyboard():
    pub = rospy.Publisher('/mobile_base/commands/velocity',Twist, queue_size=10)
    rospy.init_node('teleop_py',anonymous=True)
    rate = rospy.Rate(10)
    k = 1
    while not rospy.is_shutdown() and k < 250:
      twist.linear.x = 1.0
      twist.angular.z = 0.01
      twist.linear.y = 0.0
      pub.publish(twist)
      rate.sleep()
      k +=1
    k = 1
    # while not rospy.is_shutdown() and k < 400:
    #   twist.linear.x = 0.0
    #   twist.angular.z = 0.1
    #   twist.linear.y = 0.0
    #   pub.publish(twist)
    #   rate.sleep()
    #   k +=1

def callback(data,pub):
    t = Twist()

    if data.data == 1:
        t.linear.x = .25
        print t
    if data.data == 2:
        t.angular.z = 2
    if data.data == 3:
        t.linear.x = .25
        t.angular.z = 1

    time = rospy.get_time()
    while rospy.get_time()-time < 6:
        pub.publish (t)
        rospy.sleep(.005)

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()

def __init__(self):
      self.imgprocess = ImageProcessor.ImageProcessor()
      self.bridge = CvBridge()
      self.latestimage = None
      self.process = False

      """ROS Subscriptions """
      self.image_pub = rospy.Publisher("/image_converter/output_video",Image, queue_size=10)
      self.image_sub = rospy.Subscriber("/cam/camera_/image_raw",Image,self.cvt_image)
      self.cmdVelocityPub = rospy.Publisher('/platform_control/cmd_vel', Twist, queue_size=10)

      self.targetlane = 0
      self.cmdvel = Twist()
      self.last_time = rospy.Time()
      self.sim_time = rospy.Time()
      self.dt = 0
      self.position_er = 0
      self.position_er_last = 0;
      self.cp = 0
      self.vel_er = 0
      self.cd = 0
      self.Kp = 3
      self.Kd = 3.5

def speed_converter():
  rospy.init_node('wheel_speed', anonymous=True)
  pub1 = rospy.Publisher('lwheel_vtarget', Float32, queue_size=10)
  pub2 = rospy.Publisher('rwheel_vtarget', Float32, queue_size=10)
  rate = rospy.Rate(10)
  while not rospy.is_shutdown():
    rospy.Subscriber('cmd_vel', Twist, callback)
    s1 = "The left wheel's target speed is %f m/s." % lv
    s2 = "The right wheel's target speed is %f m/s." % rv
    rospy.loginfo(s1)
    rospy.loginfo(s2)
    pub1.publish(lv)
    pub2.publish(rv) 
    rate.sleep()

def initPubs(self):
        self.robot_pubs = {}
        count = 1
        for i in range(self.num_lanes):
            num_car_lane = self.num_cars_per_lane[i]
            for j in range(num_car_lane):
                self.robot_pubs[count] = {}
                self.robot_pubs[count]['laneid'] = i
                self.robot_pubs[count]['pub'] = rospy.Publisher('robot_' + str(count) + '/cmd_vel', Twist, queue_size=1)
                count += 1

def send_control(self, robot_pub, vel, yaw):
        msg = Twist()
        msg.linear.x = vel
        msg.angular.z = 0
        robot_pub.publish(msg)

def send_control(self, vel, yaw):
        msg = Twist()
        msg.linear.x = vel
        msg.angular.z = yaw
        self.robot_pub.publish(msg)

def init(self):
        self.hz = 500
        self.rate = rospy.Rate(self.hz)

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

        self.start_game = False
        # self.start_game = True
        # self.initTime = time() 
        # self.index = 0
        self.initUpdateVel = True
        self.TIME_PER_STEP = 0.1

        self.LoadControls()
        self.lenControls = len(self.controls)

def init(self):

        self.hz = rospy.get_param('~hz', 10)
        self.max_speed = rospy.get_param('~max_speed', 5)
        self.min_speed = rospy.get_param('~min_speed', -5)
        self.rate = rospy.Rate(self.hz) 

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

        self.acc = 0
        self.yaw = 0
        self.vel = 0

def send_control(self, vel, yaw):
        msg = Twist()
        msg.linear.x = vel
        msg.angular.z = yaw
        self.robot_pub.publish(msg)

def __init__(self, name):
        self.name = name

        # Publisher
        self.cmd_vel = rospy.Publisher("/%s/cmd_vel" % self.name,
                                       Twist, queue_size=1)

        # Subscriber
        self.odom = rospy.Subscriber("/%s/odom" % self.name,
                                     Odometry, self.odom_callback,
                                     queue_size=1)

        self.laser = rospy.Subscriber("/%s/front_laser/scan" % self.name,
                                      LaserScan, self.laser_callback,
                                      queue_size=1)

        self.camera = rospy.Subscriber("/%s/front_camera/image_raw" % self.name,
                                       Image, self.camera_callback,
                                       queue_size=1)

        self.pose_data = None
        self.laser_data = None
        self.camera_data = None

        self.cv_bridge = cv_bridge.CvBridge()

        self.rate = rospy.Rate(10)
        self.rate.sleep()

def set_speed(self, linear, angular):
        movecmd = Twist()
        movecmd.linear.x = linear
        movecmd.angular.z = angular
        self.cmd_vel.publish(movecmd)

def __init__(self):
        rospy.init_node('nav_asr', anonymous = True)
        rospy.Subscriber("/recognizer/output", String, self.SpeechUpdateGoal)
        rospy.Subscriber("/WPsOK", String, self.GetWayPoints)

        self.waypoint_list     = dict()
        self.marker_list       = list()
        self.makerArray        = MarkerArray()

        self.makerArray_pub    = rospy.Publisher("Waypoint_Array",MarkerArray,queue_size=5)

        rospy.on_shutdown(self.shutdown) # @@@@
        # How long in seconds should the robot pause at each location?
        self.rest_time = rospy.get_param("~rest_time", 2)
        # Are we running in the fake simulator?
        self.fake_test = rospy.get_param("~fake_test", True)
        # Publisher to manually control the robot (e.g. to stop it, queue_size=5)
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        # Subscribe to the move_base action server
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        rospy.loginfo("Waiting for move_base action server...")
        # Wait 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))
        rospy.loginfo("Connected to move base server")

    # Create the waypoints list from txt

def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        self.move_base.cancel_goal()
        rospy.sleep(2)
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1)

def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        self.move_base.cancel_goal()
        rospy.sleep(2)
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1)

def __init__(self):

        self.force_reset = True
        self.vel_pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=5)
        self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
        self.pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
        self.reset_proxy = rospy.ServiceProxy('/gazebo/reset_simulation', Empty)

        self.img_rows = 32
        self.img_cols = 32
        self.img_channels = 1

def myround(x):
    return int(round(x) - .5) + (x > 0)

# Author: Andrew Dai
# This ROS Node converts Joystick inputs from the joy node
# into commands for turtlesim

# Receives joystick messages (subscribed to Joy topic)
# then converts the joysick inputs into Twist commands
# axis 1 aka left stick vertical controls linear speed
# axis 0 aka left stick horizonal controls angular speed

def myround(x):
    return int(round(x) - .5) + (x > 0)

# Author: Andrew Dai
# This ROS Node converts Joystick inputs from the joy node
# into commands for turtlesim

# Receives joystick messages (subscribed to Joy topic)
# then converts the joysick inputs into Twist commands
# axis 1 aka left stick vertical controls linear speed
# axis 0 aka left stick horizonal controls angular speed