Python Quaternion.z方法代码示例

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
def quaternion_product(q1, q2):

    q_prod = Quaternion()

    a1 = q1.w
    b1 = q1.x
    c1 = q1.y
    d1 = q1.z
    a2 = q2.w
    b2 = q2.x
    c2 = q2.y
    d2 = q2.z

    q_prod.w = a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2
    q_prod.x = a1 * b2 + b1 * a2 + c1 * d2 - d1 * c2
    q_prod.y = a1 * c2 - b1 * d2 + c1 * a2 + d1 * b2
    q_prod.z = a1 * d2 + b1 * c2 - c1 * b2 + d1 * a2

    # This is exactly the same quaternion as before, but with positive
    # real part (seems to be the standard for ROS TF)
    if q_prod.w < 0.0:
        q_prod.x = -q_prod.x
        q_prod.y = -q_prod.y
        q_prod.z = -q_prod.z
        q_prod.w = -q_prod.w

    return q_prod

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
 def goal_orientation(self, theta):
  orientation = Quaternion() 
  if -numpy.pi < theta < -numpy.pi*2.0/3.0:
   print 'reverse orientation'
   orientation.z = -numpy.sin(theta/2.0)
   orientation.w = -numpy.cos(theta/2.0)
  else:
   orientation.z = numpy.sin(theta/2.0)
   orientation.w = numpy.cos(theta/2.0)
  return orientation

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
 def GoalOrientation(self, theta):
  orientation = Quaternion() 
  
  if -numpy.pi < theta < -numpy.pi*2.0/3.0:
   orientation.z = -numpy.sin(theta/2.0)
   orientation.w = -numpy.cos(theta/2.0)
   
  else:
   orientation.z = numpy.sin(theta/2.0)
   orientation.w = numpy.cos(theta/2.0)
   
  return orientation

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
    def sense(self):
        """ collects sensor data from the Neato and publishes
            it to the neatoSensors topic
        """
        # receive and publish sensor data
        self.fields = self.neato.state.keys() # update sensor fields
        sensor_data  = NeatoSensors() # see NOTE above
        for field in self.fields:
            try:
                sensor_data.__setattr__(field, self.neato.state[field])
            except:
                pass
        self.sensorPub.publish(sensor_data)

        # receive and publish laser range data
        range_data = LaserRangeData()
        range_data.__setattr__("range_data", self.neato.range_data)
        self.rangePub.publish(range_data)

        # odomemtry in testing
        self.odomUpdate()
        
        # transform position into tf frame
        quaternionOdom = Quaternion()
        quaternionOdom.x = 0.0
        quaternionOdom.y = 0.0
        quaternionOdom.z = sin(self.theta/2)
        quaternionOdom.w = -cos(self.theta/2)

        quaternionLL = Quaternion()
        quaternionLL.x = 0.0
        quaternionLL.y = 0.0
        quaternionLL.z = 0.0
        quaternionLL.w = 1.0



        
        # base_link -> base_laser transformation
        self.odomBroadcaster.sendTransform(
            (0.0, -0.1, 0.0),
            (quaternionLL.x, quaternionLL.y, quaternionLL.z, quaternionLL.w),
            rospy.Time.now(),
            "/base_laser",
            "/base_link")
            
        # odom -> base_link transformation
        self.odomBroadcaster.sendTransform(
            (self.x/1000, self.y/1000, 0),
            (quaternionOdom.x, quaternionOdom.y, quaternionOdom.z, quaternionOdom.w),
            rospy.Time.now(),
            "/base_link",
            "/odom")

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
    def Publish(self, broadcaster, orientation, x_dist, y_dist, linear_speed, angular_speed, use_pose_ekf=False):
        ros_now = rospy.Time.now()

        quat = Quaternion()

        # Orientation can be one of two things:
        #    Euler Angles or Quaternion
        # Orientation is a dictionary keyed by 'euler' and 'quaternion'

        if orientation.has_key('euler'):
            euler = orientation['euler']

            # Note: Euler values are in degrees
            roll = euler['roll']
            pitch = euler['pitch']
            yaw = euler['yaw']

            q = transformations.quaternion_from_euler(roll, pitch, yaw)

            quat.x = q[0]
            quat.y = q[1]
            quat.z = q[2]
            quat.w = q[3]

        elif orientation.has_key('quaternion'):
            quat.x = orientation['quaternion']['x']
            quat.y = orientation['quaternion']['y']
            quat.z = orientation['quaternion']['z']
            quat.w = orientation['quaternion']['w']


        # Publish the transform from frame odom to frame base_link over tf

        #    Note: pose ekf is how turtlebot does its thing.  If there is an imu and/or gyro, it might be best to take
        #    this approach
        if use_pose_ekf:
            # This transform conflicts with transforms built into the Turtle stack
            # http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29
            # This is done in/with the robot_pose_ekf because it can integrate IMU/gyro data
            # using an "extended Kalman filter"
            # REMOVE this "line" if you use robot_pose_ekf
            pass
        else:
            broadcaster.sendTransform(
                (x_dist, y_dist, 0),
                (quat.x, quat.y, quat.z, quat.w),
                ros_now,
                "base_footprint",
                "odom"
            )

        self._publisher.publish(self._msg(quat, x_dist, y_dist, linear_speed, angular_speed, ros_now, use_pose_ekf))

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
    def interpOdom(self, o1, o2, frac):
        """
        Interpolates two odometry objects, and returns a third one based
        on frac, the distance between the two.
        """
        pos1 = o1.pose.pose.position
        pos2 = o2.pose.pose.position
        posOut = Point(x = pos1.x + (pos2.x - pos1.x) * frac,
                       y = pos1.y + (pos2.y - pos1.y) * frac,
                       z = pos1.z + (pos2.z - pos1.z) * frac)
        or1 = o1.pose.pose.orientation
        or2 = o2.pose.pose.orientation
        orOut = Quaternion()

        # Interpolating quaternions is complicated.  First,
        # compute the dot product of the quaternions.  "Theta" in what
        # follows is the angle between the two quaternions.
        cosHalfTheta = or1.z * or2.z + or1.w * or2.w

        if math.fabs(cosHalfTheta) >= 1.0:
            orOut.z = or1.z
            orOut.w = or1.w
        else:
            halfTheta = math.acos(cosHalfTheta)
            sinHalfTheta = (1.0 - cosHalfTheta**2)**0.5
            if math.fabs(sinHalfTheta) < 0.001:
                orOut.z = (or1.z + or2.z)/2.0
                orOut.w = (or1.w + or2.w)/2.0
            else:
                rA  = math.sin((1 - frac) * halfTheta) / sinHalfTheta
                rB  = math.sin(frac * halfTheta) / sinHalfTheta

                orOut.w = (or1.w * rA + or2.w * rB);
                orOut.z = (or1.z * rA + or2.z * rB);

        poseOut = Pose(position = posOut, orientation = orOut)
        poseCVOut = PoseWithCovariance(pose = poseOut)

        lin1 = o1.twist.twist.linear
        lin2 = o2.twist.twist.linear
        linOut = Vector3(x = lin1.x + (lin2.x - lin1.x) * frac,
                         y = lin1.y + (lin2.y - lin1.y) * frac,
                         z = lin1.z + (lin2.z - lin1.z) * frac)
        ang1 = o1.twist.twist.angular
        ang2 = o2.twist.twist.angular
        angOut = Vector3(x = ang1.x + (ang2.x - ang1.x) * frac,
                         y = ang1.y + (ang2.y - ang1.y) * frac,
                         z = ang1.z + (ang2.z - ang1.z) * frac)
        twistOut = Twist(linear = linOut, angular = angOut)
        twistCVOut = TwistWithCovariance(twist = twistOut)

        return Odometry(pose = poseCVOut, twist = twistCVOut)

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
    def poll(self):
        (x, y, theta) = self.arduino.arbot_read_odometry()

        quaternion = Quaternion()
        quaternion.x = 0.0 
        quaternion.y = 0.0
        quaternion.z = sin(theta / 2.0)
        quaternion.w = cos(theta / 2.0)
    
        # Create the odometry transform frame broadcaster.
        now = rospy.Time.now()
        self.odomBroadcaster.sendTransform(
            (x, y, 0), 
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            now,
            "base_link",
            "odom"
            )
    
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_link"
        odom.header.stamp = now
        odom.pose.pose.position.x = x
        odom.pose.pose.position.y = y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = quaternion
        
        odom.twist.twist.linear.x = self.forwardSpeed
        odom.twist.twist.linear.y = 0
        odom.twist.twist.angular.z = self.angularSpeed

        self.arduino.arbot_set_velocity(self.forwardSpeed, self.angularSpeed)

        self.odomPub.publish(odom)

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
    def publish(self, data):
        q = data.getOrientation()
        roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])

        # print "Before ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"

        array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))

        res = Quaternion()
        res.w = array[0]
        res.x = array[1]
        res.y = array[2]
        res.z = array[3]

        roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])

        # print "after ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"

        msg = Imu()
        msg.header.frame_id = self._frameId
        msg.header.stamp = rospy.get_rostime()
        msg.orientation = res
        msg.linear_acceleration = data.getAcceleration()
        msg.angular_velocity = data.getVelocity()

        magMsg = MagneticField()
        magMsg.header.frame_id = self._frameId
        magMsg.header.stamp = rospy.get_rostime()
        magMsg.magnetic_field = data.getMagnetometer()

        self._pub.publish(msg)
        self._pubMag.publish(magMsg)

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
def compare_all(data):
    global k 
    
    last_row = csv_data[k-frequency_offset]

    diff = Quaternion()
    diff.x = round(float(last_row[1])-convert_ppm(data.channels[3]),4)
    diff.y = round(float(last_row[2])-convert_ppm(data.channels[1]),4)
    diff.z = round(float(last_row[4])-convert_ppm(data.channels[0]),4)
    diff.w = round(float(last_row[5])+convert_ppm(data.channels[2]),4)
    
    testing_pub.publish(diff)

    if (k < len(csv_data)):
        row = csv_data[k]
        #log for post processing
        #joystick log
        c = csv.writer(open("/home/jinahadam/catkin_ws/src/testing/gps_joy.csv", "ab"))
        c.writerow([data.header.stamp.secs,row[1],row[2],row[4],row[5]])
        #rc Inn log
        c = csv.writer(open("/home/jinahadam/catkin_ws/src/testing/rc_in.csv", "ab"))
        c.writerow([data.header.stamp.secs,convert_ppm(data.channels[3]),convert_ppm(data.channels[1]),convert_ppm(data.channels[0]),convert_ppm(data.channels[2])])


        msg = Joy()
	msg.axes = (float(row[1]),float(row[2]),float(row[3]),float(row[4]),float(row[5]),float(row[6]),float(row[7]))
        msg.buttons = (0,0,0,0,0,0,0,0,0,0,0)
	k = k + 1
        pub.publish(msg)       
    else:
        #k = 0
        print("Joystick & RC In data logged with GPS Time")
        exit()

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
def quaternion_to_msg(q):
    msg = Quaternion()
    msg.x = q[0]
    msg.y = q[1]
    msg.z = q[2]
    msg.w = q[3]
    return msg

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
def array_to_quaternion(nparr):
    quat = Quaternion()
    quat.x = nparr[0]
    quat.y = nparr[1]
    quat.z = nparr[2]
    quat.w = nparr[3]
    return quat

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
def convert_planar_phi_to_quaternion(phi):
    quaternion = Quaternion()
    quaternion.x = 0
    quaternion.y = 0
    quaternion.z = math.sin(phi / 2)
    quaternion.w = math.cos(phi / 2)
    return quaternion

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
 def __init__(self):
  position=Point()
  rotation=Quaternion()
  rospy.init_node('tf_listener')
  self.pub = rospy.Publisher('robot_odom', robot_odom, queue_size=10)
  self.now=rospy.Time()
  self.tf_listener=tf.TransformListener()
  rospy.sleep(0.5)
  frame='/odom'
  wantedframe = self.frame_checker(frame)

  while not rospy.is_shutdown():
   (po,ro)=self.get_odom(frame,wantedframe)
   position.x=po[0]
   position.y=po[1]
   position.z=po[2]
   rotation.x=ro[0]
   rotation.y=ro[1]
   rotation.z=ro[2]
   rotation.w=ro[3]
   #rospy.sleep()
   self.publish(position,rotation,wantedframe)

   #print 'position: \n',position,'\n','rotation: \n',self.quat_to_angle(rotation),'time: ', self.robot_odom.header.stamp
   print self.robot_odom,'\nangular: ',self.quat_to_angle(self.robot_odom.rotation)#self.normalize_angle(self.quat_to_angle(self.robot_odom.rotation))

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
def list_to_dict(lst):

    """
    Convert a list to a pose dictionary, assuming it is in the order
    x, y, z, orientation x, orientation y, orientation z[, orientation w].
    """

    if len(lst) == 6:
        qtn = tf.transformations.quaternion_from_euler(lst[3], lst[4], lst[5])
    elif len(lst) == 7:
        qtn = Quaternion()
        qtn.x = lst[3]
        qtn.y = lst[4]
        qtn.z = lst[5]
        qtn.w = lst[6]
    else:
        raise MoveItCommanderException("""Expected either 6 or 7 elements
                in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)""")

    pnt = Point()
    pnt.x = lst[0]
    pnt.y = lst[1]
    pnt.z = lst[2]

    pose_dict = {
        'position': pnt,
        'orientation': qtn
    }
    return pose_dict

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import z [as 别名]
 def getOrientation(self):
     ort = Quaternion()
     ort.x = self._orientationX
     ort.y = self._orientationY
     ort.z = self._orientationZ
     ort.w = self._orientationW
     return ort