Python Quaternion.x方法代码示例

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import x [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 x [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 x [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 x [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 x [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 x [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 x [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 x [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 x [as 别名]
def quaternion_vector2quaternion_ros(quaternion_vector):
  """
  Quaternion: [x, y, z, w]
  """
  quaternion_ros = Quaternion()
  quaternion_ros.x, quaternion_ros.y, quaternion_ros.z, quaternion_ros.w = quaternion_vector
  return quaternion_ros

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

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import x [as 别名]
def update_kalman(ar_tags):
    print "started update"
    rospy.wait_for_service('innovation')
    update = rospy.ServiceProxy('innovation', NuSrv)
    listener = tf.TransformListener()
    while True:
        try:
            try:
               (trans, rot) = listener.lookupTransform(ar_tags['arZ'], ar_tags['ar1'], rospy.Time(0))
            except:
	       print "Couldn't look up transform"
               continue
            lin = Vector3()
            quat = Quaternion()
            lin.x = trans[0]
            lin.y = trans[1]
            lin.z = trans[2]
            quat.x = rot[0]
            quat.y = rot[1]
            quat.z = rot[2]
            quat.w = rot[3]
            transform = Transform()
            transform.translation = lin
            transform.rotation = quat
            test = update(transform, ar_tags['ar1'])
            print "Service call succeeded"
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import x [as 别名]
	def Publish_Odom_Tf(self):
		
		
	#quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
		quaternion = Quaternion()
		quaternion.x = 0 
		quaternion.y = 0
		quaternion.z = math.sin(self.Heading / 2.0)
		quaternion.w = math.cos(self.Heading / 2.0)
			
		rosNow = rospy.Time.now()
		self._tf_broad_caster.sendTransform(
				(self.X_Pos, self.Y_Pos, 0), 
					(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
					rosNow,
					"base_footprint",
					"odom"
					)

		# next, we'll publish the odometry message over ROS
		odometry = Odometry()
		odometry.header.frame_id = "odom"
		odometry.header.stamp = rosNow
		odometry.pose.pose.position.x = self.X_Pos
		odometry.pose.pose.position.y = self.Y_Pos
		odometry.pose.pose.position.z = 0
		odometry.pose.pose.orientation = quaternion


		odometry.child_frame_id = "base_link"
		odometry.twist.twist.linear.x = self.Velocity
		odometry.twist.twist.linear.y = 0
		odometry.twist.twist.angular.z = self.Omega
		self._odom_publisher.publish(odometry)

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import x [as 别名]
def axis_angle_to_quaternion(axis, angle):
    q = Quaternion()
    q.x = axis.x * math.sin(angle / 2.0)
    q.y = axis.y * math.sin(angle / 2.0)
    q.z = axis.z * math.sin(angle / 2.0)
    q.w = math.cos(angle / 2.0)
    return q

# 需要导入模块: from geometry_msgs.msg import Quaternion [as 别名]
# 或者: from geometry_msgs.msg.Quaternion import x [as 别名]
def rotateQuaternion(q_orig, yaw):
    """
    Converts a basic rotation about the z-axis (in radians) into the
    Quaternion notation required by ROS transform and pose messages.
    
    :Args:
       | q_orig (geometry_msgs.msg.Quaternion): to be rotated
       | yaw (double): rotate by this amount in radians
    :Return:
       | (geometry_msgs.msg.Quaternion) q_orig rotated yaw about the z axis
     """
    # Create a temporary Quaternion to represent the change in heading
    q_headingChange = Quaternion()

    p = 0
    y = yaw / 2.0
    r = 0
 
    sinp = math.sin(p)
    siny = math.sin(y)
    sinr = math.sin(r)
    cosp = math.cos(p)
    cosy = math.cos(y)
    cosr = math.cos(r)
 
    q_headingChange.x = sinr * cosp * cosy - cosr * sinp * siny
    q_headingChange.y = cosr * sinp * cosy + sinr * cosp * siny
    q_headingChange.z = cosr * cosp * siny - sinr * sinp * cosy
    q_headingChange.w = cosr * cosp * cosy + sinr * sinp * siny

    # Multiply new (heading-only) quaternion by the existing (pitch and bank) 
    # quaternion. Order is important! Original orientation is the second 
    # argument rotation which will be applied to the quaternion is the first 
    # argument. 
    return multiply_quaternions(q_headingChange, q_orig)

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