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 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 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 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)
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 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 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 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 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 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))
qs.quaternion.z = float(argv[4]) qs.quaternion.w = float(argv[5]) from_frame = argv[6] to_frame = argv[7] elif len(argv) == 7: roll = float(argv[2]) pitch = float(argv[3]) yaw = float(argv[4]) if not use_radians: roll = radians(roll) pitch = radians(pitch) yaw = radians(yaw) q = quaternion_from_euler(roll, pitch, yaw) quat = Quaternion(*q) qs.quaternion = quat from_frame = argv[5] to_frame = argv[6] qs.header.frame_id = from_frame rospy.loginfo("Transforming " + str(qs)) tf_qs = tfer.transform_quaternion(qs, from_frame, to_frame) rospy.loginfo("to") rospy.loginfo(str(tf_qs)) o = tf_qs.quaternion r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w]) rospy.loginfo("In frame: " + to_frame) rospy.loginfo("[r, p, y](radians): " + str(([r, p, y]))) rospy.loginfo("[r, p, y](degrees): " + str( ([degrees(r), degrees(p), degrees(y)])))
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
def odom_callback(data): q = QuaternionStamped() q.header = data.header q.quaternion = data.pose.pose.orientation attitude_pub.publish(q)
def measSensorLoopTimerCallback(self, timer_msg): # Get time time_stamp_current = rospy.Time.now() # if (self.flag_robot_pose_set == False): return # Computing the measurement # meas_posi = np.zeros((3, ), dtype=float) meas_atti_quat = ars_lib_helpers.Quaternion.zerosQuat() # Attitude noise_atti_ang = np.zeros((3, ), dtype=float) noise_atti_ang[0] = np.random.normal(loc=0.0, scale=math.sqrt( self.cov_meas_att['x'])) noise_atti_ang[1] = np.random.normal(loc=0.0, scale=math.sqrt( self.cov_meas_att['y'])) noise_atti_ang[2] = np.random.normal(loc=0.0, scale=math.sqrt( self.cov_meas_att['z'])) noise_atti_quat_tf = tf.transformations.quaternion_from_euler( noise_atti_ang[0], noise_atti_ang[1], noise_atti_ang[2], axes='sxyz') noise_atti_quat = np.roll(noise_atti_quat_tf, 1) meas_atti_quat = ars_lib_helpers.Quaternion.quatProd( self.robot_atti_quat, noise_atti_quat) # Covariance #meas_cov_atti = np.diag(self.cov_meas_att['x'], self.cov_meas_att['y'], self.cov_meas_att['z']]) # Filling the message # meas_header_msg = Header() meas_robot_atti_msg = Quaternion() meas_robot_atti_stamp_msg = QuaternionStamped() # meas_header_msg.frame_id = self.robot_frame_id meas_header_msg.stamp = self.robot_pose_timestamp # Attitude meas_robot_atti_msg.w = meas_atti_quat[0] meas_robot_atti_msg.x = meas_atti_quat[1] meas_robot_atti_msg.y = meas_atti_quat[2] meas_robot_atti_msg.z = meas_atti_quat[3] # meas_robot_atti_stamp_msg.header = meas_header_msg meas_robot_atti_stamp_msg.quaternion = meas_robot_atti_msg # self.meas_robot_atti_pub.publish(meas_robot_atti_stamp_msg) # return
def yuv_callback(self, yuv_frame): # Use OpenCV to convert the yuv frame to RGB info = yuv_frame.info( ) # the VideoFrame.info() dictionary contains some useful information such as the video resolution rospy.logdebug_throttle(10, "yuv_frame.info = " + str(info)) cv2_cvt_color_flag = { olympe.PDRAW_YUV_FORMAT_I420: cv2.COLOR_YUV2BGR_I420, olympe.PDRAW_YUV_FORMAT_NV12: cv2.COLOR_YUV2BGR_NV12, }[info["yuv"]["format"]] # convert pdraw YUV flag to OpenCV YUV flag cv2frame = cv2.cvtColor(yuv_frame.as_ndarray(), cv2_cvt_color_flag) # Publish image msg_image = self.bridge.cv2_to_imgmsg(cv2frame, "bgr8") self.pub_image.publish(msg_image) # yuv_frame.vmeta() returns a dictionary that contains additional metadata from the drone (GPS coordinates, battery percentage, ...) metadata = yuv_frame.vmeta() rospy.logdebug_throttle(10, "yuv_frame.vmeta = " + str(metadata)) if metadata[1] != None: header = Header() header.stamp = rospy.Time.now() header.frame_id = '/body' frame_timestamp = metadata[1][ 'frame_timestamp'] # timestamp [millisec] msg_time = Time() msg_time.data = frame_timestamp # secs = int(frame_timestamp//1e6), nsecs = int(frame_timestamp%1e6*1e3) self.pub_time.publish(msg_time) drone_quat = metadata[1]['drone_quat'] # attitude msg_attitude = QuaternionStamped() msg_attitude.header = header msg_attitude.quaternion = Quaternion(drone_quat['x'], -drone_quat['y'], -drone_quat['z'], drone_quat['w']) self.pub_attitude.publish(msg_attitude) location = metadata[1][ 'location'] # GPS location [500.0=not available] (decimal deg) msg_location = PointStamped() if location != {}: msg_location.header = header msg_location.header.frame_id = '/world' msg_location.point.x = location['latitude'] msg_location.point.y = location['longitude'] msg_location.point.z = location['altitude'] self.pub_location.publish(msg_location) ground_distance = metadata[1]['ground_distance'] # barometer (m) self.pub_height.publish(ground_distance) speed = metadata[1]['speed'] # opticalflow speed (m/s) msg_speed = Vector3Stamped() msg_speed.header = header msg_speed.header.frame_id = '/world' msg_speed.vector.x = speed['north'] msg_speed.vector.y = -speed['east'] msg_speed.vector.z = -speed['down'] self.pub_speed.publish(msg_speed) air_speed = metadata[1][ 'air_speed'] # air speed [-1=no data, > 0] (m/s) self.pub_air_speed.publish(air_speed) link_goodput = metadata[1][ 'link_goodput'] # throughput of the connection (b/s) self.pub_link_goodput.publish(link_goodput) link_quality = metadata[1]['link_quality'] # [0=bad, 5=good] self.pub_link_quality.publish(link_quality) wifi_rssi = metadata[1][ 'wifi_rssi'] # signal strength [-100=bad, 0=good] (dBm) self.pub_wifi_rssi.publish(wifi_rssi) battery_percentage = metadata[1][ 'battery_percentage'] # [0=empty, 100=full] self.pub_battery.publish(battery_percentage) state = metadata[1][ 'state'] # ['LANDED', 'MOTOR_RAMPING', 'TAKINGOFF', 'HOWERING', 'FLYING', 'LANDING', 'EMERGENCY'] self.pub_state.publish(state) mode = metadata[1][ 'mode'] # ['MANUAL', 'RETURN_HOME', 'FLIGHT_PLAN', 'TRACKING', 'FOLLOW_ME', 'MOVE_TO'] self.pub_mode.publish(mode) msg_pose = PoseStamped() msg_pose.header = header msg_pose.pose.position = msg_location.point msg_pose.pose.position.z = ground_distance msg_pose.pose.orientation = msg_attitude.quaternion self.pub_pose.publish(msg_pose) Rot = R.from_quat([ drone_quat['x'], -drone_quat['y'], -drone_quat['z'], drone_quat['w'] ]) drone_rpy = Rot.as_euler('xyz') msg_odometry = Odometry() msg_odometry.header = header msg_odometry.child_frame_id = '/body' msg_odometry.pose.pose = msg_pose.pose msg_odometry.twist.twist.linear.x = math.cos( drone_rpy[2]) * msg_speed.vector.x + math.sin( drone_rpy[2]) * msg_speed.vector.y msg_odometry.twist.twist.linear.y = -math.sin( drone_rpy[2]) * msg_speed.vector.x + math.cos( drone_rpy[2]) * msg_speed.vector.y msg_odometry.twist.twist.linear.z = msg_speed.vector.z self.pub_odometry.publish(msg_odometry) # log battery percentage if battery_percentage >= 30: if battery_percentage % 10 == 0: rospy.loginfo_throttle( 100, "Battery level: " + str(battery_percentage) + "%") else: if battery_percentage >= 20: rospy.logwarn_throttle( 10, "Low battery: " + str(battery_percentage) + "%") else: if battery_percentage >= 10: rospy.logerr_throttle( 1, "Critical battery: " + str(battery_percentage) + "%") else: rospy.logfatal_throttle( 0.1, "Empty battery: " + str(battery_percentage) + "%") # log signal strength if wifi_rssi <= -60: if wifi_rssi >= -70: rospy.loginfo_throttle( 100, "Signal strength: " + str(wifi_rssi) + "dBm") else: if wifi_rssi >= -80: rospy.logwarn_throttle( 10, "Weak signal: " + str(wifi_rssi) + "dBm") else: if wifi_rssi >= -90: rospy.logerr_throttle( 1, "Unreliable signal:" + str(wifi_rssi) + "dBm") else: rospy.logfatal_throttle( 0.1, "Unusable signal: " + str(wifi_rssi) + "dBm") else: rospy.logwarn("Packet lost!")