Python geometry_msgs.msg.TwistStamped() Examples

def __init__(self):
        self.evadeSet = False
        self.controller = XBox360()
        self.bridge = CvBridge()
        self.throttle = 0
        self.grid_img = None
        ##self.throttleLock = Lock()
        print "evasion"
        rospy.Subscriber("/lidargrid", Image, self.gridCB, queue_size=1)
        rospy.Subscriber("/cmd_vel", TwistStamped , self.twistCB , queue_size = 1)
        rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=5)
        self.pub_img = rospy.Publisher("/steering_img", Image)
        self.pub_twist = rospy.Publisher("/lidar_twist", TwistStamped, queue_size=1)
        self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1)        
        rospy.init_node ('lidar_cmd',anonymous=True)
        rospy.spin() 

def __init__(self):
        print "dataRecorder"
        self.record = False
        self.twist = None
        self.twistLock = Lock()
        self.bridge = CvBridge()
        self.globaltime = None
        self.controller = XBox360()
        ##rospy.Subscriber("/camera/depth/points" , PointCloud2, self.ptcloudCB)
        rospy.Subscriber("/camera/depth/image_rect_raw_drop", Image, self.depthCB)
        rospy.Subscriber("/camera/rgb/image_rect_color_drop", Image, self.streamCB)
        #rospy.Subscriber("/camera/rgb/image_rect_mono_drop", Image, self.streamCB)  ##for black and white images see run.launch and look at the drop fps nodes near the bottom
        rospy.Subscriber("/lidargrid", Image, self.lidargridCB)
        rospy.Subscriber("/cmd_vel", TwistStamped, self.cmd_velCB)
        rospy.Subscriber("/joy", Joy ,self.joyCB)
        self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1)
        rospy.init_node('dataRecorder',anonymous=True)
        rospy.spin() 

def __init__(self):
        rospy.loginfo("runner.__init__")
        # ----------------------------
        self.sess = tf.InteractiveSession()
        self.model = cnn_cccccfffff()
        saver = tf.train.Saver()
        saver.restore(self.sess, "/home/ubuntu/catkin_ws/src/car/scripts/model.ckpt")
        rospy.loginfo("runner.__init__: model restored")
        # ----------------------------
        self.bridge = CvBridge()
        self.netEnable = False
        self.controller = XBox360()
        rospy.Subscriber("/camera/rgb/image_rect_color", Image, self.runCB)
        rospy.Subscriber("/joy", Joy ,self.joyCB)
        self.pub_twist = rospy.Publisher("/neural_twist", TwistStamped, queue_size=1)
        self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1)
        rospy.init_node('neural_cmd',anonymous=True)
        rospy.spin() 

def __init__(self):
        super().__init__('controller')
        self.pub = self.create_publisher(
            TwistStamped, 'cmd_vel', qos_profile_system_default
        )
        self.tmr = self.create_timer(1.0, self.callback) 

def callback(self):
        msg = TwistStamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.twist.linear.x = 1.0
        self.pub.publish(msg) 

def twistCB(self, cmd_vel):
        if self.evadeSet == True:
            try:
                ##rospy.loginfo("cmd_vel Recieved")
                self.throttle = cmd_vel.twist.linear.x
                normed_throttle = (self.throttle*2.0)-1.0
                front_max = 0.3 + 4.5*(normed_throttle**2.5)   ##front region scales with throttle value
                rospy.loginfo('normed_throttle: '+str(normed_throttle) + ' front_max: '+str(front_max))
                front = self.Occupancy(self.grid_img, 0.1, front_max, -0.2,  0.2)    ##(2,0.2)  to (0.5,-0.2)
                right = self.Occupancy(self.grid_img, 0.0, 1, -0.7, -0.2)            ##(2,-0.2) to (0,-0.7)
                left  = self.Occupancy(self.grid_img, 0.0, 1,  0.2,  0.7)            ##(2,0.7)  to (0,0.2)
                everywhere = self.Occupancy(self.grid_img, -4.0, 4.0, -4.0, 4.0)
                cmd = TwistStamped()
                #rospy.loginfo(self.throttle)
                cmd.twist.angular.z = 0.5
                cmd.twist.linear.x = -1.0
                if front:
                    cmd.twist.linear.x = 0.5   ##stop
                    self.pub_twist.publish(cmd)                   
                    self.sound.publish("Forward collision detected")       
                elif left:
                    cmd.twist.angular.z = 0.7  ##turn right
                    self.pub_twist.publish(cmd)           
                    self.sound.publish("Left collision detected")                       
                elif right:
                    cmd.twist.angular.z = 0.3  ##turn left
                    self.pub_twist.publish(cmd)
                    self.sound.publish("Right collision detected")
                else:
                    #self.pub_twist.publish(cmd)
                    pass 
            except Exception as f:
                print(f)
        else:
            pass
            ##rospy.loginfo("Not using Evasion") 

def __init__(self):
        self.throttleInitialized = False
        self.joy_timeout = 2.0
        self.lid_timeout = 0.30
        self.cnn_timeout = 0.1
        self.joy_time = time.time()
        self.lid_time = time.time()
        self.cnn_time = time.time()
        self.controller = XBox360()
        self.joy_cmd = TwistStamped()
        self.lid_cmd = TwistStamped()
        self.cnn_cmd = TwistStamped()
        self.cruiseControl = False
        self.cruiseThrottle = 0.5
        self.steeringAngle = 0.5
        self.throttle = 0.5
        self.trim = 0.0
        ##self.throttleLock = Lock()
        print "cmd_control"
       
        rospy.Subscriber("/imu", Imu, self.imuCB, queue_size=5)
        rospy.Subscriber("/lidar_twist", TwistStamped, self.lidarTwistCB, queue_size=5)
        rospy.Subscriber("/neural_twist", TwistStamped, self.neuralTwistCB, queue_size=5)
        rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=5)
        self.vel_pub = rospy.Publisher("/cmd_vel", TwistStamped, queue_size = 1) 
        #self.state_pub = rospy.Publisher("/joint_states", JointState, queue_size=1)
        self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1)        
        rospy.init_node ('cmd_control',anonymous=True)
        rate = rospy.Rate(66)
        while not rospy.is_shutdown():
            self.cmdRouter()
            rate.sleep() 

def save_gps_vel_data(bag, kitti, gps_frame_id, topic):
    for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
        twist_msg = TwistStamped()
        twist_msg.header.frame_id = gps_frame_id
        twist_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
        twist_msg.twist.linear.x = oxts.packet.vf
        twist_msg.twist.linear.y = oxts.packet.vl
        twist_msg.twist.linear.z = oxts.packet.vu
        twist_msg.twist.angular.x = oxts.packet.wf
        twist_msg.twist.angular.y = oxts.packet.wl
        twist_msg.twist.angular.z = oxts.packet.wu
        bag.write(topic, twist_msg, t=twist_msg.header.stamp) 

def save_gps_vel_data(bag, kitti, gps_frame_id, topic):
    for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
        twist_msg = TwistStamped()
        twist_msg.header.frame_id = gps_frame_id
        twist_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
        twist_msg.twist.linear.x = oxts.packet.vf
        twist_msg.twist.linear.y = oxts.packet.vl
        twist_msg.twist.linear.z = oxts.packet.vu
        twist_msg.twist.angular.x = oxts.packet.wf
        twist_msg.twist.angular.y = oxts.packet.wl
        twist_msg.twist.angular.z = oxts.packet.wu
        bag.write(topic, twist_msg, t=twist_msg.header.stamp) 

def toNED(msg):
    # fliter measured velocity
    global vx_p, vy_p

    vx = 1*msg.twist.linear.x + 0*vx_p
    vy = 1*msg.twist.linear.y + 0*vy_p
    v_body = array([vx, -vy, 0])
    
    # transform body velocity to ENU 
    global q
    [qx, qy, qz, qw] = [q[0], q[1], q[2], q[3]]
    Tenu = array([[1-2*qy*qy-2*qz*qz,   2*qx*qy-2*qz*qw,   2*qx*qz+2*qy*qw],
                  [2*qx*qy + 2*qz*qw, 1-2*qx*qx-2*qz*qz,   2*qy*qz-2*qx*qw],
                  [2*qx*qz-2*qy*qw  , 2*qy*qz + 2*qx*qw, 1-2*qx*qx-2*qy*qy]])

    v = dot(Tenu, v_body)

    # ENU to NED: (x,y,z) -> (x,-y,-z)
    twist = TwistStamped()
    twist.header = Header()
    twist.header.frame_id = "ned"
    twist.header.stamp = rospy.Time.now()
    twist.twist.linear.x = v[0]
    twist.twist.linear.y = -v[1]
    twist.twist.linear.z = 0
    pub.publish(twist)

    vx_p = vx
    vy_p = vy

    # record data in vicon frame, compare with vicon
    q_ned_vicon = tf.transformations.quaternion_from_euler(math.pi, 0, -yaw)
    [qx, qy, qz, qw] = [q_ned_vicon[0], q_ned_vicon[1], q_ned_vicon[2], q_ned_vicon[3]]
    Tv = array([[1-2*qy*qy-2*qz*qz,   2*qx*qy-2*qz*qw,   2*qx*qz+2*qy*qw],
                  [2*qx*qy + 2*qz*qw, 1-2*qx*qx-2*qz*qz,   2*qy*qz-2*qx*qw],
                  [2*qx*qz-2*qy*qw  , 2*qy*qz + 2*qx*qw, 1-2*qx*qx-2*qy*qy]])
    vr = dot(Tv, array([v[0],-v[1],0]))

    outtxt.write(str.format("{0:.9f} ", msg.header.stamp.to_sec()))
    outtxt.write(str.format("{0:.9f} ", vx))
    outtxt.write(str.format("{0:.9f} ", vy))
    outtxt.write('0 ')
    outtxt.write('0 ')
    outtxt.write('0 ')
    outtxt.write('0 ')
    outtxt.write('0\n') 

def __init__(self, ns, train_mode):
        # Class variables
        self.__mode = train_mode                      # Mode of RL-agent (Training or Executrion ?)
        self.__ns = ns                          # namespace of simulation environment
        self.__is__new = False                  # True, if waypoint reached
        self.__static_scan = LaserScan()        # Laserscan only contains static objects
        self.__ped_scan = LaserScan()           # Laserscan only contains pedestrians
        self.__f_scan = LaserScan()
        self.__f_scan.header.frame_id = "base_footprint"
        self.__b_scan = LaserScan()
        self.__b_scan.header.frame_id = "base_footprint"

        self.__img = OccupancyGrid()            # input image
        self.__wps= Waypoint()                  # most recent Waypoints
        self.__twist = TwistStamped()           # most recent velocity
        self.__goal = Pose()                    # most recent goal position in robot frame
        self.__state_mode = 2                   # 0, if image as input state representation
                                                # 1, if stacked image representation in same frame
                                                # 2, if scan as input state representation



        # Subscriber
        self.wp_sub_ = rospy.Subscriber("%s/wp" % (self.__ns), Waypoint, self.__wp_callback, queue_size=1)

        if ["train", "eval"].__contains__(self.__mode):
            # Info only avaible during evaluation and training
            self.wp_sub_reached_ = rospy.Subscriber("%s/wp_reached" % (self.__ns), Waypoint, self.__wp_reached_callback, queue_size=1)

            self.static_scan_sub_ = rospy.Subscriber("%s/static_laser" % (self.__ns), LaserScan, self.__static_scan_callback,
                                                     queue_size=1)

            self.ped_scan_sub_ = rospy.Subscriber("%s/ped_laser" % (self.__ns), LaserScan, self.__ped_scan_callback,
                                                  queue_size=1)
            self.twist_sub_ = rospy.Subscriber("%s/twist" % (self.__ns), TwistStamped, self.__twist_callback, queue_size=1)
            self.goal_sub_ = rospy.Subscriber("%s/rl_agent/robot_to_goal" % (self.__ns), PoseStamped, self.__goal_callback, queue_size=1)
        else:
            self.static_scan_sub_ = rospy.Subscriber("%s/b_scan" % (self.__ns), LaserScan,
                                                     self.__b_scan_callback,
                                                     queue_size=1)
            self.static_scan_sub_ = rospy.Subscriber("%s/f_scan" % (self.__ns), LaserScan,
                                                     self.__f_scan_callback,
                                                     queue_size=1)

        # Service
        self.__img_srv = rospy.ServiceProxy('%s/image_generator/get_image' % (self.__ns), StateImageGenerationSrv)
        self.__sim_in_step = rospy.ServiceProxy('%s/is_in_step' % (self.__ns), SetBool)
        self.__merge_scans = rospy.ServiceProxy('%s/merge_scans' % (self.__ns), MergeScans)