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