def transform_quaternion_stamped(self, new_frame, quat_stamped, robot_state):
        """
        Transforms a quaternion into new_frame_id according to robot state.
        
        **Args:**

            **new_frame_id (string):** new frame id

            **quat_stamped (geometry_msgs.msg.QuaternionStamped):** quaternion stamped (current frame is defined by 
            header)
            
            **robot_state (arm_navigation_msgs.msg.RobotState):** Robot state used for transformations
          
        **Returns:**
            A geometry_msgs.msg.QuaternionStamped defined in new_frame_id

        **Raises:**

            **exceptions.TransformStateError:** if there is an error getting the transform
        """
        qs = QuaternionStamped()
        qs.header.frame_id = new_frame
        qs.quaternion = self.transform_quaternion(
            new_frame, quat_stamped.header.frame_id, quat_stamped.quaternion, robot_state
        )
        return qs
Exemple #2
0
def do_transform_imu(imu_msg, transform):
    """
    Transforms the given Imu message into the given target frame
    :param imu_msg:
    :type imu_msg: Imu
    :param transform:
    :type transform: TransformStamped
    :return: Imu message transformed into the target frame
    :rtype: Imu
    """
    src_l = Vector3Stamped()
    src_l.header = imu_msg.header
    src_l.vector = imu_msg.linear_acceleration
    src_a = Vector3Stamped()
    src_a.header = imu_msg.header
    src_a.vector = imu_msg.angular_velocity
    src_o = QuaternionStamped()
    src_o.header = imu_msg.header
    src_o.quaternion = imu_msg.orientation

    tgt_l = do_transform_vector3(src_l, transform)
    tgt_a = do_transform_vector3(src_a, transform)
    tgt_o = do_transform_quaternion(src_o, transform)

    tgt_imu = copy.deepcopy(imu_msg)
    tgt_imu.header.frame_id = transform.child_frame_id
    tgt_imu.linear_acceleration = tgt_l
    tgt_imu.angular_velocity = tgt_a
    tgt_imu.orientation = tgt_o
    return tgt_imu
Exemple #3
0
def align_object(object_world, listener):
    '''
    Align the object frame coordinates to be consistent relative to the world frame.
    '''

    object_world_aligned = copy.deepcopy(object_world)
    #Transform object frame from camera frame to world frame
    #listener = tf.TransformListener()
    obj_qnt_stamped_world = QuaternionStamped()
    obj_qnt_stamped_world.header.frame_id = object_world.header.frame_id
    obj_qnt_stamped_world.quaternion = copy.deepcopy(
        object_world.pose.orientation)
    #object_qnt_world = listener.transformQuaternion('world', obj_qnt_stamped_cam)

    #Convert from quarternion to matrix
    quarternion = [
        obj_qnt_stamped_world.quaternion.x, obj_qnt_stamped_world.quaternion.y,
        obj_qnt_stamped_world.quaternion.z, obj_qnt_stamped_world.quaternion.w
    ]
    trans_mat = tf.transformations.quaternion_matrix(quarternion)
    rot_mat = trans_mat[:3, :3]

    #Convert from matrix to quarternion
    # Adapt to the new robot arm pose:
    # robot arm 1st joint zero angles faces the table.
    align_trans_matrix = np.identity(4)
    object_size = [object_world.width, object_world.height, object_world.depth]

    #Find and align x axes.
    x_axis = [1., 0., 0.]
    align_x_axis, min_ang_axis_idx = find_min_ang_vec(x_axis, rot_mat)
    rot_mat = np.delete(rot_mat, min_ang_axis_idx, axis=1)
    #align_trans_matrix[:3, 1] = align_x_axis
    align_trans_matrix[:3, 0] = align_x_axis
    object_world_aligned.height = object_size[min_ang_axis_idx]

    #y axes
    y_axis = [0., 1., 0.]
    align_y_axis, min_ang_axis_idx = find_min_ang_vec(y_axis, rot_mat)
    rot_mat = np.delete(rot_mat, min_ang_axis_idx, axis=1)
    #align_trans_matrix[:3, 0] = -align_y_axis
    align_trans_matrix[:3, 1] = align_y_axis
    object_world_aligned.width = object_size[min_ang_axis_idx]

    #z axes
    z_axis = [0., 0., 1.]
    align_z_axis, min_ang_axis_idx = find_min_ang_vec(z_axis, rot_mat)
    align_trans_matrix[:3, 2] = align_z_axis
    object_world_aligned.depth = object_size[min_ang_axis_idx]

    align_qtn_array = tf.transformations.quaternion_from_matrix(
        align_trans_matrix)

    align_qnt_stamped = QuaternionStamped()
    align_qnt_stamped.header.frame_id = object_world.header.frame_id
    align_qnt_stamped.quaternion.x, align_qnt_stamped.quaternion.y, \
            align_qnt_stamped.quaternion.z, align_qnt_stamped.quaternion.w = align_qtn_array

    object_world_aligned.pose.orientation = align_qnt_stamped.quaternion
    return object_world_aligned
    def transform_quaternion_stamped(self, new_frame, quat_stamped,
                                     robot_state):
        '''
        Transforms a quaternion into new_frame_id according to robot state.
        
        **Args:**

            **new_frame_id (string):** new frame id

            **quat_stamped (geometry_msgs.msg.QuaternionStamped):** quaternion stamped (current frame is defined by 
            header)
            
            **robot_state (arm_navigation_msgs.msg.RobotState):** Robot state used for transformations
          
        **Returns:**
            A geometry_msgs.msg.QuaternionStamped defined in new_frame_id

        **Raises:**

            **exceptions.TransformStateError:** if there is an error getting the transform
        '''
        qs = QuaternionStamped()
        qs.header.frame_id = new_frame
        qs.quaternion = self.transform_quaternion(new_frame,
                                                  quat_stamped.header.frame_id,
                                                  quat_stamped.quaternion,
                                                  robot_state)
        return qs
Exemple #5
0
 def publish(self, imu_msg, nav_msg):
     if self.tfBuffer.can_transform(nav_msg.header.frame_id, "odom",
                                    rospy.Time.now(),
                                    rospy.Duration.from_sec(0.5)):
         if self.tfBuffer.can_transform(imu_msg.header.frame_id,
                                        "base_link", rospy.Time.now(),
                                        rospy.Duration.from_sec(0.5)):
             unfiltered_msg = Odometry()
             unfiltered_msg.header.frame_id = "odom"
             unfiltered_msg.child_frame_id = "base_link"
             imu_q = QuaternionStamped()
             imu_q.quaternion = imu_msg.orientation
             imu_q.header = imu_msg.header
             unfiltered_msg.pose.pose.orientation = self.tfListener.transformQuaternion(
                 "base_link",
                 imu_q).quaternion  #TF2 FOR KINETIC JUST AIN'T WORKING
             nav_p = PointStamped()
             nav_p.point = nav_msg.pose.pose.position
             nav_p.header = nav_msg.header
             unfiltered_msg.pose.pose.position = self.tfListener.transformPoint(
                 "odom", nav_p).point
             self.pub.publish(unfiltered_msg)
         else:
             rospy.logwarn("{} to base_link tf unavailable!".format(
                 imu_msg.header.frame_id))
     else:
         rospy.logwarn("{} to odom tf unavailable!".format(
             nav_msg.header.frame_id))
    def imu_callback(self, msg):
        raw_imu_quat = QuaternionStamped()
        raw_imu_quat.header = msg.header
        raw_imu_quat.quaternion = msg.orientation

        raw_imu_av = Vector3Stamped()
        raw_imu_av.header = msg.header
        raw_imu_av.vector = msg.angular_velocity

        raw_imu_acc = Vector3Stamped()
        raw_imu_acc.header = msg.header
        raw_imu_acc.vector = msg.linear_acceleration
                
        try:
            self.listener.waitForTransform(self.base_link_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(1.0)) # gyrometer->body
            imu_rootlink_quat = self.listener.transformQuaternion(self.base_link_frame, raw_imu_quat)
            imu_rootlink_av = self.listener.transformVector3(self.base_link_frame, raw_imu_av)
            imu_rootlink_acc = self.listener.transformVector3(self.base_link_frame, raw_imu_acc)
            # todo: convert covariance
        except:
            rospy.logwarn("[%s] failed to solve imu_to_base tf: %s to %s", rospy.get_name(), msg.header.frame_id, self.base_link_frame)
            return

        msg.header = imu_rootlink_quat.header
        msg.orientation = imu_rootlink_quat.quaternion
        msg.angular_velocity = imu_rootlink_av.vector        
        msg.linear_acceleration = imu_rootlink_acc.vector
                
        # publish
        self.pub.publish(msg)
Exemple #7
0
def make_quaternion_msg(q, frame_id, time=0.0):
    """ Create a QuaternionStamped message. """
    msg = QuaternionStamped()
    msg.header.stamp = rospy.Time.from_sec(time)
    msg.header.frame_id = frame_id
    msg.quaternion = Quaternion(q[1], q[2], q[3], q[0])

    return msg
Exemple #8
0
 def on_enter(self, userdata):
     pose = userdata.target_pose  # type: PoseStamped
     position = PointStamped()
     position.header.frame_id = pose.header.frame_id
     position.point = pose.pose.position
     orientation = QuaternionStamped()
     orientation.header.frame_id = pose.header.frame_id
     orientation.quaternion = pose.pose.orientation
     userdata.target_position = position
     userdata.target_orientation = orientation
Exemple #9
0
def do_transform_quaternion(quaternion_msg, transform):
    # Use PoseStamped to transform the Quaternion
    src_ps = PoseStamped()
    src_ps.header = quaternion_msg.header
    src_ps.pose.orientation = quaternion_msg.quaternion
    tgt_ps = do_transform_pose(src_ps, transform)
    tgt_o = QuaternionStamped()
    tgt_o.header = tgt_ps.header
    tgt_o.quaternion = tgt_ps.pose.orientation
    return tgt_o
    def __init__(self):
        self.current_heading = QuaternionStamped()
        self.best_heading = QuaternionStamped()
        self.vel_sub = rospy.Subscriber('tcpvel', TwistStamped, self.vel_callback)

        self.head_pub = rospy.Publisher('heading', QuaternionStamped, queue_size=1)

        self.VelYaw = 0.0

        self.heading_last_good = QuaternionStamped()
Exemple #11
0
def align_obj_ort(object_world_pose, listener):
    '''
    Align the object frame coordinates to be consistent relative to the world frame.
    '''
    obj_qnt_stamped_world = QuaternionStamped()
    obj_qnt_stamped_world.header.frame_id = object_world_pose.header.frame_id
    obj_qnt_stamped_world.quaternion = copy.deepcopy(
        object_world_pose.pose.orientation)

    #Convert from quarternion to matrix
    quarternion = [
        obj_qnt_stamped_world.quaternion.x, obj_qnt_stamped_world.quaternion.y,
        obj_qnt_stamped_world.quaternion.z, obj_qnt_stamped_world.quaternion.w
    ]
    trans_mat = tf.transformations.quaternion_matrix(quarternion)
    rot_mat = trans_mat[:3, :3]

    #Convert from matrix to quarternion
    # Adapt to the new robot arm pose:
    # robot arm 1st joint zero angles faces the table.
    align_trans_matrix = np.identity(4)

    #Find and align x axes.
    x_axis = [1., 0., 0.]
    align_x_axis, min_ang_axis_idx = find_min_ang_vec(x_axis, rot_mat)
    rot_mat = np.delete(rot_mat, min_ang_axis_idx, axis=1)
    align_trans_matrix[:3, 1] = align_x_axis

    #y axes
    y_axis = [0., 1., 0.]
    align_y_axis, min_ang_axis_idx = find_min_ang_vec(y_axis, rot_mat)
    rot_mat = np.delete(rot_mat, min_ang_axis_idx, axis=1)
    align_trans_matrix[:3, 0] = -align_y_axis

    #z axes
    z_axis = [0., 0., 1.]
    align_z_axis, min_ang_axis_idx = find_min_ang_vec(z_axis, rot_mat)
    align_trans_matrix[:3, 2] = align_z_axis

    align_qtn_array = tf.transformations.quaternion_from_matrix(
        align_trans_matrix)

    align_qnt_stamped = QuaternionStamped()
    align_qnt_stamped.header.frame_id = object_world_pose.header.frame_id
    align_qnt_stamped.quaternion.x, align_qnt_stamped.quaternion.y, \
            align_qnt_stamped.quaternion.z, align_qnt_stamped.quaternion.w = align_qtn_array

    return align_qnt_stamped
    def __init__(self, number):
        # self.vehicle_number = rospy.get_param('vehicle_number')
        self.vehicle_number = number
        self.orientation = 0.0
        self.orientation_ref = 0.0
        self.steer_angle = QuaternionStamped()
        self.velocity = 0.0
        self.orientation_vel = 0.0
        self.linear_vel = 0.0
        KP = rospy.get_param('ackermann_control/pid_controller/ori_kp')
        KI = rospy.get_param('ackermann_control/pid_controller/ori_ki')
        KD = rospy.get_param('ackermann_control/pid_controller/ori_kd')
        TS = rospy.get_param('ackermann_control/pid_controller/ori_ts')
        ETOL = rospy.get_param('ackermann_control/pid_controller/ori_etol')
        self.controlador = PID(KP, KI, KD, TS, ETOL)

        rospy.init_node('orientation_PID_control_sim_' +
                        str(self.vehicle_number),
                        anonymous=True)
        rospy.Subscriber("/odom", Odometry, self.callback_velocity)
        rospy.Subscriber("/imu_data", Imu, self.callback_imu)
        rospy.Subscriber("/yaw_angle", QuaternionStamped,
                         self.callback_reference)
        #rospy.Subscriber("/cmd_vel", TwistStamped, self.callback_reference)
        #rospy.Subscriber("/cmd_vel", Twist, self.callback_reference)
        self.pub_angle = rospy.Publisher('/cmd_steer_' +
                                         str(self.vehicle_number),
                                         QuaternionStamped,
                                         queue_size=1)
Exemple #13
0
    def get_lines(self, fmt):
        t = Time()
        t.secs = 0

        # MESSAGE_TO_TUM_SHORT
        line = ROSMsg2CSVLine.to(fmt, Point(), t,
                                 ROSMessageTypes.GEOMETRY_MSGS_VECTOR3)
        line = ROSMsg2CSVLine.to(fmt, PointStamped(), t,
                                 ROSMessageTypes.GEOMETRY_MSGS_POINTSTAMPED)
        line = ROSMsg2CSVLine.to(fmt, Vector3(), t,
                                 ROSMessageTypes.GEOMETRY_MSGS_VECTOR3)
        line = ROSMsg2CSVLine.to(fmt, Vector3Stamped(), t,
                                 ROSMessageTypes.GEOMETRY_MSGS_VECTOR3STAMPED)
        line = ROSMsg2CSVLine.to(
            fmt, PoseWithCovarianceStamped(), t,
            ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED)
        line = ROSMsg2CSVLine.to(
            fmt, PoseWithCovariance(), t,
            ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCE)
        line = ROSMsg2CSVLine.to(fmt, PoseStamped(), t,
                                 ROSMessageTypes.GEOMETRY_MSGS_POSESTAMPED)
        line = ROSMsg2CSVLine.to(fmt, Pose(), t,
                                 ROSMessageTypes.GEOMETRY_MSGS_POSE)
        line = ROSMsg2CSVLine.to(fmt, Quaternion(), t,
                                 ROSMessageTypes.GEOMETRY_MSGS_QUATERNION)
        line = ROSMsg2CSVLine.to(
            fmt, QuaternionStamped(), t,
            ROSMessageTypes.GEOMETRY_MSGS_QUATERNIONSTAMPED)
        line = ROSMsg2CSVLine.to(fmt, Transform(), t,
                                 ROSMessageTypes.GEOMETRY_MSGS_TRANSFORM)
        line = ROSMsg2CSVLine.to(
            fmt, TransformStamped(), t,
            ROSMessageTypes.GEOMETRY_MSGS_TRANSFORMSTAMPED)
        return line
Exemple #14
0
def main():
    global ser, pub
    while 1:
        msg = ser.readline().split(",");
        
	print "Compass line:", msg

	degrees = float(msg[1])
        radians = degrees * math.pi / 180.0 + math.pi / 2.0 # east is 0

        radians = -(radians + math.pi) 

        while radians > math.pi:
            radians = radians - 2*math.pi
        
	while radians < -math.pi:
            radians = radians + 2*math.pi

        (qx, qy, qz, qw) = quaternion_from_euler(0,0,radians)
	msg = QuaternionStamped()
	msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = 'base_link'
        msg.quaternion.x = qx
        msg.quaternion.y = qy
        msg.quaternion.z = qz
        msg.quaternion.w = qw

        pub.publish(msg)
 def reset_vars(self):
     self.imu_msg = Imu()
     self.imu_msg.orientation_covariance = (-1., ) * 9
     self.imu_msg.angular_velocity_covariance = (-1., ) * 9
     self.imu_msg.linear_acceleration_covariance = (-1., ) * 9
     self.pub_imu = False
     self.raw_gps_msg = NavSatFix()
     self.pub_raw_gps = False
     self.pos_gps_msg = NavSatFix()
     self.pub_pos_gps = False
     self.imu_msg_dq = Imu()
     self.imu_msg_dq.orientation_covariance = (-1., ) * 9
     self.imu_msg_dq.angular_velocity_covariance = (-1., ) * 9
     self.imu_msg_dq.linear_acceleration_covariance = (-1., ) * 9
     self.pub_imu_dq = False
     self.delta_q_msg = QuaternionStamped()
     self.pub_delta_q = False
     self.vel_msg = TwistStamped()
     self.pub_vel = False
     self.mag_msg = MagneticField()
     self.mag_msg.magnetic_field_covariance = (0, ) * 9
     self.pub_mag = False
     self.temp_msg = Temperature()
     self.temp_msg.variance = 0.
     self.pub_temp = False
     self.press_msg = FluidPressure()
     self.press_msg.variance = 0.
     self.pub_press = False
     self.anin1_msg = UInt16()
     self.pub_anin1 = False
     self.anin2_msg = UInt16()
     self.pub_anin2 = False
     self.ecef_msg = PointStamped()
     self.pub_ecef = False
     self.pub_diag = False
Exemple #16
0
    def publish_vehicle_attitude(self):
        """Function for publishing the vehicle attitude
		"""

        #get pose from airsim relative (NED -> FRD)
        pose_NED_FRD = self.client.simGetVehiclePose(vehicle_name='drone')

        #extract quaternion components
        w = pose_NED_FRD.orientation.w_val
        x = pose_NED_FRD.orientation.x_val
        y = pose_NED_FRD.orientation.y_val
        z = pose_NED_FRD.orientation.z_val

        #extract rotation matrix
        R_NED_FRD = tf.transformations.quaternion_matrix([x, y, z, w])

        #convert rotation to (NWU -> FLU)
        R_NWU_FLU = self.T_rot.dot(R_NED_FRD).dot(self.T_rot)

        #convert to quaternion
        q_NWU_FLU = tf.transformations.quaternion_from_matrix(R_NWU_FLU)

        #publish attitude
        msg = QuaternionStamped()
        msg.quaternion.x = q_NWU_FLU[0]
        msg.quaternion.y = q_NWU_FLU[1]
        msg.quaternion.z = q_NWU_FLU[2]
        msg.quaternion.w = q_NWU_FLU[3]
        self.vehicle_attitude_pub.publish(msg)

        #get euler angles
        euler_angles = tf.transformations.euler_from_matrix(R_NWU_FLU, 'ryxz')

        return euler_angles[0], euler_angles[1], euler_angles[2]
Exemple #17
0
 def angle_head(self,req):
     roll = 0
     yaw = req.pan
     pitch = req.tilt
     (qx,qy,qz,qw) = tf.transformations.quaternion_from_euler(roll,pitch,yaw)
     final_quat = QuaternionStamped()
     final_quat.header.frame_id = "torso_lift_link"
     final_quat.header.stamp = rospy.Time.now()
     final_quat.quaternion.x = qx
     final_quat.quaternion.y = qy
     final_quat.quaternion.z = qz
     final_quat.quaternion.w = qw
     #Convert the angle to the current stereo frame
     self.listener.waitForTransform("double_stereo_link","torso_lift_link",rospy.Time.now(),rospy.Duration(10.0))
     rel_quat = self.listener.transformQuaternion("double_stereo_link",final_quat)
     rel_quat_tuple = (rel_quat.quaternion.x,rel_quat.quaternion.y,rel_quat.quaternion.z,rel_quat.quaternion.w)
     #Get the point which corresponds to one meter ahead of the stereo frame
     straight_target_q = (1.0,0,0,0)
     #Rotate by the quaternion
     (x,y,z,trash) = tf.transformations.quaternion_multiply(rel_quat_tuple,tf.transformations.quaternion_multiply(straight_target_q,tf.transformations.quaternion_inverse(rel_quat_tuple)))
     new_target = PointStamped()
     new_target.header.stamp = rospy.Time.now()
     new_target.header.frame_id = "double_stereo_link"
     new_target.point.x = x
     new_target.point.y = y
     new_target.point.z = z
     #Now put in torso_lift_link frame
     self.listener.waitForTransform("torso_lift_link","double_stereo_link",rospy.Time.now(),rospy.Duration(10.0))
     target = self.listener.transformPoint("torso_lift_link",new_target)
     #self.follow_target = target
     success = self.look_at(target)
     self.follow_target = False
     return AngleHeadResponse(success)
Exemple #18
0
 def __init__(self):
     self._quaternion = QuaternionStamped()
     self._quaternion.quaternion.x = 0.0
     self._quaternion.quaternion.y = 0.0
     self._quaternion.quaternion.z = 0.0
     self._quaternion.quaternion.w = 1.0
     bg_color_str = rospy.get_param('~bg_color', "255,255,255")
     self.bg_color_set = list()
     for item in bg_color_str.split(","):
         self.bg_color_set.append(int(item))
     self.hz = rospy.get_param('~hz', 30)
     rospy.Subscriber("src_topic", QuaternionStamped,
                      self.quaternion_callback)
     # pygame
     pygame.init()
     if rospy.get_param('~is_fullscreen', "false"):
         cmd1 = ['xrandr']
         cmd2 = ['grep', '*']
         resolution_string, _ = self._command2pipe(cmd1, cmd2)
         resolution = resolution_string.split()[0]
         width, height = resolution.split('x')
         self.screen_size = (int(width), int(height))
         self.screen = pygame.display.set_mode(self.screen_size, FULLSCREEN,
                                               32)
     else:
         self.screen_size = (rospy.get_param('~width', 800),
                             rospy.get_param('~height', 480))
         self.screen = pygame.display.set_mode(self.screen_size)
     pygame.display.set_caption("roseyes")
     # pygame
     self.update_loop()
Exemple #19
0
    def serialize(self, data):
        msg = QuaternionStamped()
        msg.header.stamp.sec = data.tm.sec
        msg.header.stamp.nanosec = data.tm.nsec
        msg.quaternion.x = float(data.data.x)
        msg.quaternion.y = float(data.data.y)
        msg.quaternion.z = float(data.data.z)
        msg.quaternion.w = float(data.data.w)

        return OpenRTM_aist.ByteDataStreamBase.SERIALIZE_OK, msg
    def serialize(self, data):
        msg = QuaternionStamped()
        msg.header.stamp.secs = data.tm.sec
        msg.header.stamp.nsecs = data.tm.nsec
        msg.quaternion.x = data.data.x
        msg.quaternion.y = data.data.y
        msg.quaternion.z = data.data.z
        msg.quaternion.w = data.data.w

        buf = ros_serialize(msg)

        return OpenRTM_aist.ByteDataStreamBase.SERIALIZE_OK, buf
Exemple #21
0
    def from_TUM(line, line_number, msg_type):
        msg = None
        s = CSVFormatPose.parse(line, CSVFormatPose.TUM)
        h = Header()
        h.stamp = h.stamp.from_sec(s.t)
        h.seq = int(line_number)

        if msg_type == ROSMessageTypes.GEOMETRY_MSGS_POINTSTAMPED:
            msg = PointStamped()
            msg.header = h
            msg.point = Point(x=s.tx, y=s.ty, z=s.tz)
        elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_VECTOR3:
            msg = Vector3(x=s.tx, y=s.ty, z=s.tz)
        elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_VECTOR3STAMPED:
            msg = Vector3Stamped()
            msg.header = h
            msg.vector = Vector3(x=s.tx, y=s.ty, z=s.tz)
        elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED:
            msg = PoseWithCovarianceStamped()
            msg.header = h
            msg.pose.pose.position = Point(x=s.tx, y=s.ty, z=s.tz)
            msg.pose.pose.orientation = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
        elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCE:
            msg = PoseWithCovariance()
            msg.pose.position = Point(x=s.tx, y=s.ty, z=s.tz)
            msg.pose.orientation = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
        elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_POSESTAMPED:
            msg = PoseStamped()
            msg.header = h
            msg.pose.position = Point(x=s.tx, y=s.ty, z=s.tz)
            msg.pose.orientation = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
        elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_POSE:
            msg = Pose()
            msg.position = Point(x=s.tx, y=s.ty, z=s.tz)
            msg.orientation = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
        elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_QUATERNION:
            msg = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
        elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_QUATERNIONSTAMPED:
            msg = QuaternionStamped()
            msg.header = h
            msg.quaternion = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
        elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_TRANSFORM:
            msg = Transform()
            msg.translation = Vector3(x=s.tx, y=s.ty, z=s.tz)
            msg.rotation = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
        elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_TRANSFORMSTAMPED:
            msg = TransformStamped()
            msg.header = h
            msg.transform.translation = Vector3(x=s.tx, y=s.ty, z=s.tz)
            msg.transform.rotation = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
        # else:
        return msg, s.t
Exemple #22
0
    def rot_cb(self, ctx, data):
        temp_data = parse_value(data)

        quat = QuaternionStamped()
        quat.header.stamp = rospy.Time.now()
        quat.header.frame_id = self.frame_id
        quat.quaternion.x = temp_data.x
        quat.quaternion.y = temp_data.y
        quat.quaternion.z = temp_data.z
        quat.quaternion.w = temp_data.w

        self.samples_rot += 1
        self.pub_rotation.publish(quat)
Exemple #23
0
    def publishData(self):
        head = Header()
        head.stamp = rospy.Time.now()
        head.frame_id = 'local'

        # Publish Attitude
        myQuat = self.getQuat()
        outQuat = QuaternionStamped()
        outQuat.header = head
        outQuat.quaternion = myQuat
        self.attitude_pub_.publish(outQuat)

        # Publish IMU
        imuOut = Imu()
        imuOut.header = head
        imuOut.orientation = myQuat
        imuOut.angular_velocity = Vector3(x=0.0, y=0.0, z=self.yaw_rate_)
        imuOut.linear_acceleration = Vector3(x=self.acc_[0],
                                             y=self.acc_[1],
                                             z=self.acc_[2] + 9.81)
        self.imu_pub_.publish(imuOut)

        # Publish Velocity
        velOut = Vector3Stamped()
        velOut.header = head
        velOut.vector = Vector3(x=self.vel_[0], y=self.vel_[1], z=self.vel_[2])
        self.velocity_pub_.publish(velOut)

        # Publish Position
        posStamp = PointStamped()
        posStamp.header = head
        myPoint = Point(x=self.pos_[0], y=self.pos_[1], z=self.pos_[2])
        posStamp.point = myPoint
        self.position_pub_.publish(posStamp)

        # Publish Height Above Ground
        height = Float32(self.pos_[-1])
        # height.data = self.pos_(-1)
        self.height_pub_.publish(height)
Exemple #24
0
 def _link_state_callback(self, msg):
     for i in range(0, len(msg.name)):
         if (msg.name[i] == "robostilt::base_link"):
             orientation_q = msg.pose[i].orientation
             # publish level info to topic
             message = QuaternionStamped()
             message.header.frame_id = "base_link"
             message.header.stamp = rospy.Time.now()
             message.quaternion = orientation_q
             self.publisher.publish(message)
             # Calculate euler angles
             orientation_list = [
                 orientation_q.x, orientation_q.y, orientation_q.z,
                 orientation_q.w
             ]
             (roll_x, pitch_y, yaw_z
              ) = tf.transformations.euler_from_quaternion(orientation_list)
     if (abs(roll_x) > self.max_level_error
             or abs(pitch_y) > self.max_level_error):
         rospy.logerr("Level is out of range. Roll_x= " +
                      str(roll_x * self.rad_to_deg) + " Pitch_y= " +
                      str(pitch_y * self.rad_to_deg))
Exemple #25
0
    def __init__(self):
        rospy.init_node('dji_sdk_ros_python')

        self.attitude = QuaternionStamped()
        self.flight_status = UInt8()
        self.battery_state = BatteryState()
        self.velocity = Vector3Stamped()
        self.gps_health = UInt8()
        self.gps_position = NavSatFix()
        self.local_position = PointStamped()

        self.init_services()
        self.init_subscribers()
        self.init_publishers()
Exemple #26
0
    def test_MESSAGE_TO_TUM(self):
        t = Time()
        t.secs = 0

        self.check_tumline(
            ROSMsg2CSVLine.to_TUM(Point(), t,
                                  ROSMessageTypes.GEOMETRY_MSGS_VECTOR3))
        self.check_tumline(
            ROSMsg2CSVLine.to_TUM(PointStamped(), t,
                                  ROSMessageTypes.GEOMETRY_MSGS_POINTSTAMPED))
        self.check_tumline(
            ROSMsg2CSVLine.to_TUM(Vector3(), t,
                                  ROSMessageTypes.GEOMETRY_MSGS_VECTOR3))
        self.check_tumline(
            ROSMsg2CSVLine.to_TUM(
                Vector3Stamped(), t,
                ROSMessageTypes.GEOMETRY_MSGS_VECTOR3STAMPED))
        self.check_tumline(
            ROSMsg2CSVLine.to_TUM(
                PoseWithCovarianceStamped(), t,
                ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED))
        self.check_tumline(
            ROSMsg2CSVLine.to_TUM(
                PoseWithCovariance(), t,
                ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCE))
        self.check_tumline(
            ROSMsg2CSVLine.to_TUM(PoseStamped(), t,
                                  ROSMessageTypes.GEOMETRY_MSGS_POSESTAMPED))
        self.check_tumline(
            ROSMsg2CSVLine.to_TUM(Pose(), t,
                                  ROSMessageTypes.GEOMETRY_MSGS_POSE))
        self.check_tumline(
            ROSMsg2CSVLine.to_TUM(Quaternion(), t,
                                  ROSMessageTypes.GEOMETRY_MSGS_QUATERNION))
        self.check_tumline(
            ROSMsg2CSVLine.to_TUM(
                QuaternionStamped(), t,
                ROSMessageTypes.GEOMETRY_MSGS_QUATERNIONSTAMPED))
        self.check_tumline(
            ROSMsg2CSVLine.to_TUM(Transform(), t,
                                  ROSMessageTypes.GEOMETRY_MSGS_TRANSFORM))
        self.check_tumline(
            ROSMsg2CSVLine.to_TUM(
                TransformStamped(), t,
                ROSMessageTypes.GEOMETRY_MSGS_TRANSFORMSTAMPED))
Exemple #27
0
 def get_orientation(self, from_frame="wrist_left_ft_link"):
     qs = QuaternionStamped()
     qs.quaternion.w = 1.0
     qs.header.stamp = self.tf_l.getLatestCommonTime("world", from_frame)
     qs.header.frame_id = "/" + from_frame
     transform_ok = False
     while not transform_ok and not rospy.is_shutdown():
         try:
             world_q = self.tf_l.transformQuaternion("/world", qs)
             transform_ok = True
             return world_q
         except ExtrapolationException as e:
             rospy.logwarn(
                 "Exception on transforming pose... trying again \n(" +
                 str(e) + ")")
             rospy.sleep(0.05)
             qs.header.stamp = self.tf_l.getLatestCommonTime(
                 "world", from_frame)
Exemple #28
0
def main():
    global ser, pub
    while 1:
        msg = ser.readline().split(",");
        yaw   = float(msg[1]);
        pitch = float(msg[2]);
        roll  = float(msg[3]);
        #out = quaternion_from_euler(roll,pitch,yaw)#,'rxyz')
        out = quaternion_from_euler(0,0,yaw)#,'rxyz')

	msg = QuaternionStamped()
	msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = 'base_link'
        msg.quaternion.x = out[0]
        msg.quaternion.y = out[1]
        msg.quaternion.z = out[2]
        msg.quaternion.w = out[3]

        pub.publish(msg)
def write_ms25_euler(ms25_euler, i, bag):

    utime = ms25_euler[i, 0]

    r = ms25_euler[i, 1]
    p = ms25_euler[i, 2]
    h = ms25_euler[i, 3]

    timestamp = microseconds_to_ros_timestamp(utime)

    rosquat = QuaternionStamped()
    rosquat.header.stamp = timestamp

    q = getQuaternionFromFromEulerAnglesRollPitchYawRad(r, p, h)

    rosquat.quaternion.x = q.x()
    rosquat.quaternion.y = q.y()
    rosquat.quaternion.z = q.z()
    rosquat.quaternion.w = q.w()

    bag.write('/ms25_orientation', rosquat, t=timestamp)
Exemple #30
0
def transform_rotation(rotation, from_frame, to_frame):
    if from_frame is to_frame:
        return rotation

    global transform_point_listener
    if transform_point_listener is None:
        transform_point_listener = tf.TransformListener()
    rate = rospy.Rate(1.0)

    return_list = True
    if not isinstance(rotation, list):
        rotation = [rotation]
        return_list = False

    print("Looking up transform from " + from_frame + " to " + to_frame +
          " for " + str(len(rotation)) + " rotations.")

    ret = []
    for r in rotation:
        while True:
            try:
                hdr = Header(stamp=rospy.Time(0), frame_id=from_frame)
                msg = QuaternionStamped(header=hdr, quaternion=r)
                retmsg = transform_point_listener.transformQuaternion(
                    to_frame, msg)
            except (tf.LookupException):
                print("lookup excpetion")
                rate.sleep()
                continue
            except (tf.ExtrapolationException):
                print("extrapolation excpetion")
                rate.sleep()
                continue
            break
        ret.append(retmsg.quaternion)
    if return_list:
        return ret
    else:
        return ret[0]
Exemple #31
0
 def test_get_message_type(self):
     self.assertTrue(
         ROSMessageTypes.get_message_type(Point()) ==
         ROSMessageTypes.GEOMETRY_MSGS_VECTOR3)
     self.assertTrue(
         ROSMessageTypes.get_message_type(PointStamped()) ==
         ROSMessageTypes.GEOMETRY_MSGS_POINTSTAMPED)
     self.assertTrue(
         ROSMessageTypes.get_message_type(Vector3()) ==
         ROSMessageTypes.GEOMETRY_MSGS_VECTOR3)
     self.assertTrue(
         ROSMessageTypes.get_message_type(Vector3Stamped()) ==
         ROSMessageTypes.GEOMETRY_MSGS_VECTOR3STAMPED)
     self.assertTrue(
         ROSMessageTypes.get_message_type(PoseWithCovarianceStamped()) ==
         ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED)
     self.assertTrue(
         ROSMessageTypes.get_message_type(PoseWithCovariance()) ==
         ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCE)
     self.assertTrue(
         ROSMessageTypes.get_message_type(PoseStamped()) ==
         ROSMessageTypes.GEOMETRY_MSGS_POSESTAMPED)
     self.assertTrue(
         ROSMessageTypes.get_message_type(Pose()) ==
         ROSMessageTypes.GEOMETRY_MSGS_POSE)
     self.assertTrue(
         ROSMessageTypes.get_message_type(Quaternion()) ==
         ROSMessageTypes.GEOMETRY_MSGS_QUATERNION)
     self.assertTrue(
         ROSMessageTypes.get_message_type(QuaternionStamped()) ==
         ROSMessageTypes.GEOMETRY_MSGS_QUATERNIONSTAMPED)
     self.assertTrue(
         ROSMessageTypes.get_message_type(Transform()) ==
         ROSMessageTypes.GEOMETRY_MSGS_TRANSFORM)
     self.assertTrue(
         ROSMessageTypes.get_message_type(TransformStamped()) ==
         ROSMessageTypes.GEOMETRY_MSGS_TRANSFORMSTAMPED)
def pub_timer_callback(event):
    """Periodic callback function to publish the current orientation of the
    gimbal. This function  will terminate the ROS node if it ecounters a
    SerialException.

    Arg:
        event: ROS event object for the periodic callback.
    """
    # Create new message and header
    msg = QuaternionStamped()
    msg.header.frame_id = frame_id
    msg.header.stamp = rospy.get_rostime()

    try:
        imu1 = (gimbal.get_imu1_angles(), camera_pub, "imu1")
        imu2 = (gimbal.get_imu2_angles(), controller_pub, "imu2")
        for euler, pub, name in (imu1, imu2):
            if euler:
                rospy.logdebug_throttle(
                    1.0,
                    "get_{0}_angles: pitch={1:.2f}, roll={2:.2f}, yaw={3:.2f}".
                    format(name, *euler))
                euler = list(map(np.radians, euler))
                q = tf.transformations.quaternion_from_euler(*euler,
                                                             axes="syxz")
                msg.quaternion.x = q[0]
                msg.quaternion.y = q[1]
                msg.quaternion.z = q[2]
                msg.quaternion.w = q[3]
                pub.publish(msg)

        # diagnostic update is called here because this run at a high rate
        updater.update()
    except serial.serialutil.SerialException as e:
        rospy.logfatal(e)
        rospy.signal_shutdown("SerialError: {0:s}".format(e))
    def inputcb(self, data):
        rospy.logdebug("inputcb triggered")

        # translation from base_link to left string expressed in the
        # base_link frame:
        vb = np.array([[0.0102, 0.0391, 0.086, 0]]).T
        gbs = np.array([[1, 0, 0, vb[0,0]],
                       [0, 0, -1, vb[1,0]],
                       [0, 1, 0, vb[2,0]],
                       [0, 0, 0, 1]])
        gsb = np.linalg.inv(gbs)

        # map to base stuff:
        pbm = np.array([[data.pose.pose.position.x,
                         data.pose.pose.position.y,
                         data.pose.pose.position.z]]).T
        qbm = np.array([[data.pose.pose.orientation.w,
                         data.pose.pose.orientation.x,
                         data.pose.pose.orientation.y,
                         data.pose.pose.orientation.z]]).T
        Rbm = quat_to_rot(qbm)
        gbm = to_se3(Rbm,pbm)
        gmb = np.linalg.inv(gbm)
        vm = np.dot(gmb,np.dot(gbs,np.dot(gsb,vb)))
        vm = np.ravel(vm)[0:3]

        ## so the first thing that we need to do is get the location
        ## of the robot in the "/optimization_frame"
        p = PointStamped()
        p.header = data.pose.header
        p.point = data.pose.pose.position
        p.point.x -= vm[0]
        p.point.y -= vm[1]
        p.point.z -= vm[2]
        quat = QuaternionStamped()
        quat.quaternion = data.pose.pose.orientation
        quat.header = p.header
        try:
            ptrans = self.listener.transformPoint("/optimization_frame", p)
            qtrans = self.listener.transformQuaternion("/optimization_frame", quat)
        except (tf.Exception):
            rospy.loginfo("tf exception caught !")
            return

        ## if we have not initialized the VI, let's do it, otherwise,
        ## let's integrate
        if not self.initialized_flag:
            q = [
                ptrans.point.x,
                ptrans.point.y-h0,
                ptrans.point.z,
                ptrans.point.x,
                ptrans.point.z,
                h0 ]
            self.sys.reset_integration(state=q)
            self.last_time = ptrans.header.stamp
            self.initialized_flag = True
            return

        # get operating_condition:
        if rospy.has_param("/operating_condition"):
            operating = rospy.get_param("/operating_condition")
        else:
            return


        # set string length
        self.len = data.left;

        ## if we are not running, just reset the parameter:
        if operating is not 2:
            self.initialized_flag = False
            return
        else:
            self.initialized_flag = True
            # if we are in the "running mode", let's integrate the VI,
            # and publish the results:
            rho = [ptrans.point.x, ptrans.point.z, self.len]
            dt = (ptrans.header.stamp - self.last_time).to_sec()
            self.last_time = ptrans.header.stamp
            rospy.logdebug("Taking a step! dt = "+str(dt))
            self.sys.take_step(dt, rho)
            ## now we can get the state of the system
            q = self.sys.get_current_configuration()

            ## now add the appropriate amount of noise:
            q += self.noise*np.random.normal(0,1,(self.sys.sys.nQ))
            new_point = PointStamped()
            new_point.point.x = q[0]
            new_point.point.y = q[1]
            new_point.point.z = q[2]
            new_point.header.frame_id = "/optimization_frame"
            new_point.header.stamp = rospy.rostime.get_rostime()
            rospy.logdebug("Publishing mass location")
            self.mass_pub.publish(new_point)

            ## we can also publish the planar results:
            config = PlanarSystemConfig()
            config.header.frame_id = "/optimization_frame"
            config.header.stamp = rospy.get_rostime()
            config.xm = q[0]
            config.ym = q[1]
            config.xr = rho[0]
            config.r = rho[2]
            self.plan_pub.publish(config)

            ## now we can send out the transform
            ns = rospy.get_namespace()
            fr = "mass_location"
            if len(ns) > 1:
                fr = rospy.names.canonicalize_name(ns+fr)

            # here we are going to do a bunch of geometry math to
            # ensure that the orientation of the frame that we are
            # placing at the mass location looks "nice"

            # zvec points from robot to the string
            zvec = np.array([q[0]-ptrans.point.x, q[1]-ptrans.point.y, q[2]-ptrans.point.z])
            zvec = zvec/np.linalg.norm(zvec)
            # get transform from incoming header frame to the optimization frame
            quat = qtrans.quaternion
            qtmp = np.array([quat.x, quat.y, quat.z, quat.w])
            # convert to SO(3), and extract the y-vector for the frame
            R1 = tf.transformations.quaternion_matrix(qtmp)[:3,:3]
            yvec = -R1[:,1]
            yvec[1] = (-yvec[0]*zvec[0]-yvec[2]*zvec[2])/zvec[1]
            # x vector is a cross of y and z
            xvec = np.cross(yvec, zvec)
            # build rotation matrix and send transform
            R = np.column_stack((xvec,yvec,zvec,np.array([0,0,0])))
            R = np.row_stack((R,np.array([0,0,0,1])))
            quat = tuple(tf.transformations.quaternion_from_matrix(R).tolist())
            self.br.sendTransform((new_point.point.x, new_point.point.y, new_point.point.z),
                                  quat,
                                  new_point.header.stamp,
                                  fr,
                                  "/optimization_frame")



        return